単眼ビジュアル オドメトリ

ビジュアル オドメトリは、イメージのシーケンスを解析してカメラの位置と向きを決定するプロセスです。ビジュアル オドメトリは、移動ロボット、自動運転車、無人航空機など、さまざまな用途に使用されます。この例では、イメージのシーケンスからキャリブレーションされた単一カメラの軌跡を推定する方法を説明します。

概要

この例では、2 次元ビューのシーケンスからキャリブレーションされたカメラの軌跡を推定する方法を説明します。この例では、筑波大学のコンピューター ビジョン研究室 (CVLAB) で作成された New Tsukuba Stereo Dataset に含まれるイメージを使用します (https://cvlab.cs.tsukuba.ac.jp)。このデータセットはコンピューター グラフィックスを使用して生成された合成イメージで構成されており、カメラの姿勢のグラウンド トゥルースが含まれています。

追加情報がない場合、単眼カメラの軌跡は未知のスケール係数までしか復元できません。移動ロボットや自律型車両に使用される単眼ビジュアル オドメトリ システムは、通常、別のセンサー (ホイール オドメーター、GPS など) から、またはシーン内のサイズが既知のオブジェクトから、スケール係数を求めます。この例では、グラウンド トゥルースからスケール係数を計算します。

この例は次の 3 つの部分に分かれています。

1. 最初のビューに対する 2 番目のビューの姿勢の推定。基本行列を推定してカメラの位置と向きに分解することによって、2 番目のビューの姿勢を推定します。

2. グローバル バンドル調整を使用した、カメラの軌跡のブートストラッピングによる推定。エピポーラ制約を使用して外れ値を排除します。前の 2 つのビューと現在のビューから三角形分割された点間の 3 次元から 2 次元の対応関係を求めます。透視 n 点 (PnP) 問題を解くことによって、現在のビューのワールド カメラの姿勢を計算します。カメラの姿勢の推定では誤差の発生を避けることができず、時間の経過に伴って累積します。このような影響を "ドリフト" といいます。ドリフトを減らすため、この例では、バンドル調整を使用してこれまでに推定されたすべての姿勢を調整します。

3. ウィンドウを適用したバンドル調整を使用した、残りのカメラの軌跡の推定。新しいビューが追加されるたびに、すべての姿勢の調整にかかる時間が長くなります。ウィンドウを適用したバンドル調整は、軌跡全体ではなく最後の n 個のビューのみを最適化することで計算時間を短縮する方法です。ビューごとにバンドル調整を呼び出さないことで、計算時間がさらに短縮されます。

入力イメージ シーケンスとグラウンド トゥルースの読み取り

この例では、筑波大学のコンピューター ビジョン研究室 (CVLAB) で作成された New Tsukuba Stereo Dataset に含まれるイメージを使用します。実際の作業や出版物にこれらのイメージを使用する場合、次の論文を引用元としてください。

[1] Martin Peris Martorell, Atsuto Maki, Sarah Martull, Yasuhiro Ohkawa, Kazuhiro Fukui, "Towards a Simulation Driven Stereo Vision System".Proceedings of ICPR, pp.1038-1042, 2012.

[2] Sarah Martull, Martin Peris Martorell, Kazuhiro Fukui, "Realistic CG Stereo Image Dataset with Ground Truth Disparity Maps", Proceedings of ICPR workshop TrakMark2012, pp.40-42, 2012.

images = imageDatastore(fullfile(toolboxdir('vision'), 'visiondata', 'NewTsukuba'));

% Load ground truth camera poses.
load("visualOdometryGroundTruth.mat")

シーケンスの最初のビューを含むビュー セットの作成

imageviewset オブジェクトを使用して、各ビューに関連付けられたイメージ ポイントとカメラの姿勢を、ビューのペア間における点のマッチと共に保存し、管理します。imageviewsetオブジェクトに値を入れた後、これを使用して複数のビューにわたる点のトラックを見つけ、関数triangulateMultiviewおよびbundleAdjustmentで使用されるカメラの姿勢を取得できます。

% Create an empty imageviewset object to manage the data associated with each view.
vSet = imageviewset;

% Read and display the first image.
Irgb = readimage(images, 1);
player = vision.VideoPlayer(Position=[20, 400, 650, 510]);
step(player, Irgb);

% Create the camera intrinsics object using camera intrinsics from the 
% New Tsukuba dataset.
focalLength    = [615 615];        % specified in units of pixels
principalPoint = [320 240];        % in pixels [x, y]
imageSize      = size(Irgb,[1,2]); % in pixels [mrows, ncols]
intrinsics = cameraIntrinsics(focalLength, principalPoint, imageSize);

グレー スケールに変換して歪みを補正します。この例では、イメージがレンズ歪みのない合成イメージであるため、歪み補正の効果はありません。ただし、実際のイメージでは歪み補正が必要です。

prevI = undistortImage(im2gray(Irgb), intrinsics); 

% Detect features. 
prevPoints = detectSURFFeatures(prevI, MetricThreshold=500);

% Select a subset of features, uniformly distributed throughout the image.
numPoints = 200;
prevPoints = selectUniform(prevPoints, numPoints, size(prevI));

% Extract features. Using 'Upright' features improves matching quality if 
% the camera motion involves little or no in-plane rotation.
prevFeatures = extractFeatures(prevI, prevPoints, Upright=true);

% Add the first view. Place the camera associated with the first view
% at the origin, oriented along the Z-axis.
viewId = 1;
vSet = addView(vSet, viewId, rigidtform3d(eye(3), [0 0 0]), Points=prevPoints);

カメラの最初の姿勢のプロット

New Tsukuba Dataset のグラウンド トゥルース データに基づいて、カメラの推定姿勢と実際の姿勢を表す 2 つのグラフィカル カメラ オブジェクトを作成します。

% Setup axes.
figure
axis([-220, 50, -140, 20, -50, 300]);

% Set Y-axis to be vertical pointing down.
view(gca, 3);
set(gca, CameraUpVector=[0, -1, 0]);
camorbit(gca, -120, 0, data=[0, 1, 0]);

grid on
xlabel('X (cm)');
ylabel('Y (cm)');
zlabel('Z (cm)');
hold on

% Plot estimated camera pose. 
cameraSize = 7;
camPose = poses(vSet);
camEstimated = plotCamera(camPose, Size=cameraSize,...
    Color="g", Opacity=0);

% Plot actual camera pose.
camActual = plotCamera(Size=cameraSize, ...
    AbsolutePose=rigidtform3d(groundTruthPoses.Orientation{1}', ...
    groundTruthPoses.Location{1}), ...
    Color="b", Opacity=0);

% Initialize camera trajectories.
trajectoryEstimated = plot3(0, 0, 0, "g-");
trajectoryActual    = plot3(0, 0, 0, "b-");

legend('Estimated Trajectory', 'Actual Trajectory');
title('Camera Trajectory');

2 番目のビューの姿勢の推定

2 番目のビューから特徴を検出して抽出し、helperDetectAndMatchFeatures を使用して最初のビューにマッチングします。helperEstimateRelativePose を使用して最初のビューに対する 2 番目のビューの姿勢を推定し、imageviewset に追加します。

% Read and display the image.
viewId = 2;
Irgb = readimage(images, viewId);
step(player, Irgb);

% Convert to gray scale and undistort.
I = undistortImage(im2gray(Irgb), intrinsics);

% Match features between the previous and the current image.
[currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(...
    prevFeatures, I);

% Estimate the pose of the current view relative to the previous view.
[relPose, inlierIdx] = helperEstimateRelativePose(...
    prevPoints(indexPairs(:,1)), currPoints(indexPairs(:,2)), intrinsics);

% Exclude epipolar outliers.
indexPairs = indexPairs(inlierIdx, :);
    
% Add the current view to the view set.
vSet = addView(vSet, viewId, relPose, Points=currPoints);

% Store the point matches between the previous and the current views.
vSet = addConnection(vSet, viewId-1, viewId, Matches=indexPairs);

最初のビューに対する 2 番目のビューの位置は、未知のスケール係数までしか復元できません。helperNormalizeViewSet を使用し、一般的な単眼ビジュアル オドメトリ システムで使用される外部センサーのシミュレーションを行って、グラウンド トゥルースからスケール係数を計算します。

vSet = helperNormalizeViewSet(vSet, groundTruthPoses);

helperUpdateCameraPlots と helperUpdateCameraTrajectories を使用して、カメラの軌跡のプロットを更新します。

helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ...
    groundTruthPoses);
helperUpdateCameraTrajectories(viewId, trajectoryEstimated, trajectoryActual,...
    poses(vSet), groundTruthPoses);

prevI = I;
prevFeatures = currFeatures;
prevPoints   = currPoints;

グローバル バンドル調整を使用した、カメラの軌跡のブートストラッピングによる推定

前の 2 つのビューから三角形分割された点と現在のビューのイメージ ポイントの間の 3 次元から 2 次元の対応関係を求めます。helperFindEpipolarInliers を使用してエピポーラ制約を満たすマッチを求めます。次に、helperFind3Dto2DCorrespondences を使用して前の 2 つのビューの 3 次元の点を三角形分割し、現在のビューで対応する 2 次元の点を求めます。

estworldposeを使用して透視 n 点 (PnP) 問題を解くことによって、現在のビューのワールド カメラの姿勢を計算します。最初の 15 個のビューにグローバル バンドル調整を使用して、軌跡全体を調整します。限られた数のビューにグローバル バンドル調整を使用して、カメラの残りの軌跡をブートストラッピングによって推定します。この方法の計算量はそれほど多くありません。

for viewId = 3:15
    % Read and display the next image
    Irgb = readimage(images, viewId);
    step(player, Irgb);
    
    % Convert to gray scale and undistort.
    I = undistortImage(im2gray(Irgb), intrinsics);
    
    % Match points between the previous and the current image.
    [currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(...
        prevFeatures, I);
      
    % Eliminate outliers from feature matches.
    [~, inlierIdx] = helperEstimateRelativePose(prevPoints(indexPairs(:,1)),...
        currPoints(indexPairs(:, 2)), intrinsics);
    indexPairs = indexPairs(inlierIdx, :);
    
    % Triangulate points from the previous two views, and find the 
    % corresponding points in the current view.
    [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(vSet,...
        intrinsics, indexPairs, currPoints);
    
    % Since RANSAC involves a stochastic process, it may sometimes not
    % reach the desired confidence level and exceed maximum number of
    % trials. Disable the warning when that happens since the outcomes are
    % still valid.
    warningstate = warning('off','vision:ransac:maxTrialsReached');
    
    % Estimate the world camera pose for the current view.
    absPose = estworldpose(imagePoints, worldPoints, intrinsics);
    
    % Restore the original warning state
    warning(warningstate)
    
    % Add the current view to the view set.
    vSet = addView(vSet, viewId, absPose, Points=currPoints);
    
    % Store the point matches between the previous and the current views.
    vSet = addConnection(vSet, viewId-1, viewId, Matches=indexPairs);    
    
    tracks = findTracks(vSet); % Find point tracks spanning multiple views.
        
    camPoses = poses(vSet);    % Get camera poses for all views.
    
    % Triangulate initial locations for the 3-D world points.
    xyzPoints = triangulateMultiview(tracks, camPoses, intrinsics);
    
    % Refine camera poses using bundle adjustment.
    [~, camPoses] = bundleAdjustment(xyzPoints, tracks, camPoses, ...
        intrinsics, PointsUndistorted=true, AbsoluteTolerance=1e-12,...
        RelativeTolerance=1e-12, MaxIterations=200, FixedViewID=1);
        
    vSet = updateView(vSet, camPoses); % Update view set.
    
    % Bundle adjustment can move the entire set of cameras. Normalize the
    % view set to place the first camera at the origin looking along the
    % Z-axes and adjust the scale to match that of the ground truth.
    vSet = helperNormalizeViewSet(vSet, groundTruthPoses);
    
    % Update camera trajectory plot.
    helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ...
        groundTruthPoses);
    helperUpdateCameraTrajectories(viewId, trajectoryEstimated, ...
        trajectoryActual, poses(vSet), groundTruthPoses);
    
    prevI = I;
    prevFeatures = currFeatures;
    prevPoints   = currPoints;  
end

ウィンドウを適用したバンドル調整を使用した、残りのカメラの軌跡の推定

計算量を限定するために、ウィンドウを適用したバンドル調整を最後の 15 個のビューの調整にのみ使用して、残りのカメラの軌跡を推定します。さらに、estworldposeによって姿勢が 3 次元の点として同じ単位で計算されるため、ビューごとにバンドル調整を呼び出す必要がありません。このセクションでは、7 つのビューごとにバンドル調整が呼び出されます。バンドル調整の呼び出しのウィンドウ サイズと頻度は、実験に基づいて選択されています。

for viewId = 16:numel(images.Files)
    % Read and display the next image
    Irgb = readimage(images, viewId);
    step(player, Irgb);
    
    % Convert to gray scale and undistort.
    I = undistortImage(im2gray(Irgb), intrinsics);

    % Match points between the previous and the current image.
    [currPoints, currFeatures, indexPairs] = helperDetectAndMatchFeatures(...
        prevFeatures, I);    
          
    % Triangulate points from the previous two views, and find the 
    % corresponding points in the current view.
    [worldPoints, imagePoints] = helperFind3Dto2DCorrespondences(vSet, ...
        intrinsics, indexPairs, currPoints);

    % Since RANSAC involves a stochastic process, it may sometimes not
    % reach the desired confidence level and exceed maximum number of
    % trials. Disable the warning when that happens since the outcomes are
    % still valid.
    warningstate = warning('off','vision:ransac:maxTrialsReached');
    
    % Estimate the world camera pose for the current view.
    absPose = estworldpose(imagePoints, worldPoints, intrinsics);
    
    % Restore the original warning state
    warning(warningstate)
    
    % Add the current view and connection to the view set.
    vSet = addView(vSet, viewId, absPose, Points=currPoints);
    vSet = addConnection(vSet, viewId-1, viewId, Matches=indexPairs);
        
    % Refine estimated camera poses using windowed bundle adjustment. Run 
    % the optimization every 7th view.
    if mod(viewId, 7) == 0        
        % Find point tracks in the last 15 views and triangulate.
        windowSize = 15;
        startFrame = max(1, viewId - windowSize);
        tracks = findTracks(vSet, startFrame:viewId);
        camPoses = poses(vSet, startFrame:viewId);
        [xyzPoints, reprojErrors] = triangulateMultiview(tracks, camPoses, intrinsics);
                                
        % Hold the first two poses fixed, to keep the same scale. 
        fixedIds = [startFrame, startFrame+1];
        
        % Exclude points and tracks with high reprojection errors.
        idx = reprojErrors < 2;
        
        [~, camPoses] = bundleAdjustment(xyzPoints(idx, :), tracks(idx), ...
            camPoses, intrinsics, FixedViewIDs=fixedIds, ...
            PointsUndistorted=true, AbsoluteTolerance=1e-12,...
            RelativeTolerance=1e-12, MaxIterations=200);
        
        vSet = updateView(vSet, camPoses); % Update view set.
    end
    
    % Update camera trajectory plot.
    helperUpdateCameraPlots(viewId, camEstimated, camActual, poses(vSet), ...
        groundTruthPoses);    
    helperUpdateCameraTrajectories(viewId, trajectoryEstimated, ...
        trajectoryActual, poses(vSet), groundTruthPoses);    
    
    prevI = I;
    prevFeatures = currFeatures;
    prevPoints   = currPoints;  
end

hold off

まとめ

この例では、ビューのシーケンスからキャリブレーションされた単眼カメラの軌跡を推定する方法を説明しました。推定された軌跡はグラウンド トゥルースと完全には一致しないことに注意してください。カメラの姿勢の非線形調整を行っても、カメラの姿勢の推定の誤差が累積し、ドリフトが発生するためです。ビジュアル オドメトリ システムでは通常、複数のセンサーからの情報を結合してループ閉じ込みを実行することによって、この問題に対処します。

参考文献

[1] Martin Peris Martorell, Atsuto Maki, Sarah Martull, Yasuhiro Ohkawa, Kazuhiro Fukui, "Towards a Simulation Driven Stereo Vision System".Proceedings of ICPR, pp.1038-1042, 2012.

[2] Sarah Martull, Martin Peris Martorell, Kazuhiro Fukui, "Realistic CG Stereo Image Dataset with Ground Truth Disparity Maps", Proceedings of ICPR workshop TrakMark2012, pp.40-42, 2012.

[3] M.I.A. Lourakis and A.A. Argyros (2009). "SBA: A Software Package for Generic Sparse Bundle Adjustment". ACM Transactions on Mathematical Software (ACM) 36 (1): 1-30.

[4] R. Hartley, A. Zisserman, "Multiple View Geometry in Computer Vision," Cambridge University Press, 2003.

[5] B. Triggs; P. McLauchlan; R. Hartley; A. Fitzgibbon (1999). "Bundle Adjustment: A Modern Synthesis". Proceedings of the International Workshop on Vision Algorithms.Springer-Verlag. pp. 298-372.

[6] X.-S. Gao, X.-R. Hou, J. Tang, and H.-F. Cheng, "Complete Solution Classification for the Perspective-Three-Point Problem," IEEE Trans. Pattern Analysis and Machine Intelligence, vol. 25, no. 8, pp. 930-943, 2003.