ステレオビデオからの深度推定

この例では、キャリブレーション済みのステレオカメラで撮影したビデオから人物および人物とカメラの距離を検出する方法を示します。

ステレオカメラのパラメーターの読み込み

stereoParameters オブジェクトを読み込みます。このオブジェクトは stereoCameraCalibrator アプリまたは関数 estimateCameraParameters を使用してカメラのキャリブレーションを行った結果です。

% Load the stereoParameters object.
load("handshakeStereoParams.mat");

% Visualize camera extrinsics.
showExtrinsics(stereoParams);

ビデオファイルリーダーとビデオプレーヤーの作成

ビデオの読み取りと表示を行う System object を作成します。

videoFileLeft = "handshake_left.avi";
videoFileRight = "handshake_right.avi";

readerLeft = VideoReader(videoFileLeft);
readerRight = VideoReader(videoFileRight);
player = vision.VideoPlayer(Position=[20,200,740 560]);

ビデオフレームの読み取りと平行化

視差を計算して 3 次元のシーンを再構成するには、左右の各カメラからのフレームを平行化しなければなりません。平行化されたイメージには水平のエピポーラ線があり、行が揃えられています。これにより、ポイントをマッチングする探索空間が 1 次元に減り、視差の計算が簡略化されます。また、平行化されたイメージはアナグリフに組み合わせて、赤とシアンの立体眼鏡で 3 次元効果を見ることもできます。

frameLeft = readFrame(readerLeft);
frameRight = readFrame(readerRight);

[frameLeftRect, frameRightRect, reprojectionMatrix] = ...
    rectifyStereoImages(frameLeft, frameRight, stereoParams);

figure;
imshow(stereoAnaglyph(frameLeftRect, frameRightRect));
title("Rectified Video Frames");

視差の計算

平行化されたステレオイメージでは、対応するどの点のペアも同じピクセル行に配置されています。左のイメージの各ピクセルにつき、右のイメージで対応するピクセルまでの距離を計算します。この距離は「視差」と呼ばれ、対応するワールドポイントのカメラからの距離に比例しています。

frameLeftGray  = im2gray(frameLeftRect);
frameRightGray = im2gray(frameRightRect);
    
disparityMap = disparitySGM(frameLeftGray, frameRightGray);
figure;
imshow(disparityMap, [0, 64]);
title("Disparity Map");
colormap jet
colorbar

3 次元シーンの再構成

視差マップからの各ピクセルに対応する点の 3 次元ワールド座標を再構成します。

points3D = reconstructScene(disparityMap, reprojectionMatrix);

% Convert to meters and create a pointCloud object
points3D = points3D ./ 1000;
ptCloud = pointCloud(points3D, Color=frameLeftRect);

% Create a streaming point cloud viewer
player3D = pcplayer([-3, 3], [-3, 3], [0, 8], VerticalAxis="y", ...
    VerticalAxisDir="down");

% Visualize the point cloud
view(player3D, ptCloud);

左のイメージ内での人物の検出

事前学習済みの人物検出器を読み込み、左のカメラフレーム内の人物を検出します。

peopleDetector = peopleDetectorACF();
bboxes = detect(peopleDetector,frameLeftGray);

各人物からカメラまでの距離の決定

検出された各人物の重心の 3 次元ワールド座標を求め、その重心からカメラまでの距離をメートル単位で計算します。

% Find the centroids of detected people.
centroids = [round(bboxes(:, 1) + bboxes(:, 3) / 2), ...
    round(bboxes(:, 2) + bboxes(:, 4) / 2)];

% Find the 3-D world coordinates of the centroids.
centroidsIdx = sub2ind(size(disparityMap), centroids(:, 2), centroids(:, 1));
X = points3D(:, :, 1);
Y = points3D(:, :, 2);
Z = points3D(:, :, 3);
centroids3D = [X(centroidsIdx)'; Y(centroidsIdx)'; Z(centroidsIdx)'];

% Find the distances from the camera in meters.
dists = sqrt(sum(centroids3D .^ 2));
    
% Display the detected people and their distances.
labels = dists+" meters";
figure
imshow(insertObjectAnnotation(frameLeftRect, "rectangle", bboxes, labels));
title("Detected People");

残りのビデオの処理

上記で説明した、人物を検出してビデオの各フレームにおけるカメラまでの距離を測定する手順を適用します。

while hasFrame(readerLeft) && hasFrame(readerRight)
    % Read the frames.
    frameLeft = readFrame(readerLeft);
    frameRight = readFrame(readerRight);
    
    % Rectify the frames.
    [frameLeftRect, frameRightRect] = ...
        rectifyStereoImages(frameLeft, frameRight, stereoParams);
    
    % Convert to grayscale.
    frameLeftGray  = im2gray(frameLeftRect);
    frameRightGray = im2gray(frameRightRect);
    
    % Compute disparity. 
    disparityMap = disparitySGM(frameLeftGray, frameRightGray);
    
    % Reconstruct 3-D scene.
    points3D = reconstructScene(disparityMap, reprojectionMatrix);
    points3D = points3D ./ 1000;
    ptCloud = pointCloud(points3D, Color=frameLeftRect);
    view(player3D, ptCloud);
    
    % Detect people.
    bboxes = detect(peopleDetector,frameLeftGray);
    
    if ~isempty(bboxes)
        % Find the centroids of detected people.
        centroids = [round(bboxes(:, 1) + bboxes(:, 3) / 2), ...
            round(bboxes(:, 2) + bboxes(:, 4) / 2)];
        
        % Find the 3-D world coordinates of the centroids.
        centroidsIdx = sub2ind(size(disparityMap), centroids(:, 2), centroids(:, 1));
        X = points3D(:, :, 1);
        Y = points3D(:, :, 2);
        Z = points3D(:, :, 3);
        centroids3D = [X(centroidsIdx), Y(centroidsIdx), Z(centroidsIdx)];
        
        % Find the distances from the camera in meters.
        dists = sqrt(sum(centroids3D .^ 2, 2));
        
        % Display the detect people and their distances.
        labels = dists+" meters";
        dispFrame = insertObjectAnnotation(frameLeftRect, "rectangle", bboxes,...
            labels);
    else
        dispFrame = frameLeftRect;
    end
    
    % Display the frame.
    step(player, dispFrame);
end