ステレオ Visual Simultaneous Localization and Mapping

この例では次を使用します。

この例では、ステレオカメラのイメージデータを処理して、屋外環境マップを作成し、カメラの軌跡を推定する方法を説明します。次に、MATLAB® Coder™ を使用して、コード生成をサポートするようにコードを変更する方法を示します。この例では、特徴ベースでステレオカメラをサポートするバージョンの ORB-SLAM2 [1] アルゴリズムを使用します。

Visual simultaneous localization and mapping (vSLAM) とは、環境地図作成を行うのと同時に、周囲に対するカメラの位置と向きを計算する処理を意味します。この処理では、カメラからの視覚的入力のみを使用します。vSLAM の用途には、拡張現実、ロボティクス、自動運転などがあります。

vSLAM は単眼カメラだけでも実行できます。しかし、1 台のカメラでは深度を正確に計算できないため、マップのスケールと推定軌跡が不明となり、時間の経過とともにドリフトが発生します。さらに、最初のフレームから三角形分割することができないため、システムをブートストラップするには、最初のマップ生成用に複数のビューが必要になります。ステレオカメラを使用すると、これらの問題が解決され、より信頼性の高い vSLAM ソリューションが提供されます。

コード生成には MATLAB Coder のライセンスが必要です。

処理パイプラインの概要

ステレオ vSLAM のパイプラインは、単眼の Visual Simultaneous Localization and Mappingの例の単眼 vSLAM のパイプラインと非常によく似ています。主な違いは、"地図の初期化" 段階で、別個のフレームの 2 つのイメージではなく、同じステレオペアに属するステレオイメージのペアから 3 次元マップ点が作成されることです。

地図の初期化: パイプラインはまず、視差マップを使用して、ステレオイメージのペアから 3 次元点のマップを初期化します。左のイメージは最初のキーフレームとして保存されます。
トラッキング: マップが初期化された後は、新しいステレオペアごとに、左のイメージの特徴を最後のキーフレームの特徴とマッチングすることにより、カメラ姿勢が推定されます。推定されたカメラ姿勢は、局所地図のトラッキングにより調整されます。
局所地図作成: 現在の左のイメージがキーフレームとして識別された場合、ステレオペアの視差から新しい 3 次元マップ点が計算されます。この段階で、カメラ姿勢と 3 次元点を調整することにより、バンドル調整を使用して再投影誤差を最小化します。
ループ閉じ込み: bag-of-features アプローチを使用して、キーフレームごとにそれまでのすべてのキーフレームと比較することで、ループを検出します。ループ閉じ込みが検出されると姿勢グラフが最適化され、すべてのキーフレームのカメラ姿勢を調整します。

入力ステレオイメージシーケンスのダウンロードと確認

この例で使用しているデータは、Toronto 大学航空宇宙研究所が提供する UTIAS Long-Term Localization and Mapping Dataset からのものです。Web ブラウザーを使用するか、次のコードを実行することで、データをディレクトリにダウンロードできます。

ftpObj       = ftp('asrl3.utias.utoronto.ca');
tempFolder   = fullfile(tempdir);
dataFolder   = [tempFolder, '2020-vtr-dataset/UTIAS-In-The-Dark/'];
zipFileName  = [dataFolder, 'run_000005.zip'];
folderExists = exist(dataFolder, 'dir');

% Create a folder in a temporary directory to save the downloaded file
if ~folderExists  
    mkdir(dataFolder); 
    disp('Downloading run_000005.zip (818 MB). This download can take a few minutes.') 
    mget(ftpObj,'/2020-vtr-dataset/UTIAS-In-The-Dark/run_000005.zip', tempFolder);

    % Extract contents of the downloaded file
    disp('Extracting run_000005.zip (818 MB) ...') 
    unzip(zipFileName, dataFolder); 
end

2 つのimageDatastoreオブジェクトを使用して、ステレオイメージを保存します。

imgFolderLeft  = [dataFolder,'/images/left/'];
imgFolderRight = [dataFolder,'/images/right/'];
imdsLeft       = imageDatastore(imgFolderLeft);
imdsRight      = imageDatastore(imgFolderRight);

% Inspect the first pair of images
currFrameIdx   = 1;
currILeft      = readimage(imdsLeft, currFrameIdx);
currIRight     = readimage(imdsRight, currFrameIdx);
imshowpair(currILeft, currIRight, 'montage');

Figure contains an axes object. The axes object contains an object of type image.

地図の初期化

ORB-SLAM パイプラインではまず、3 次元ワールドポイントをもつマップを初期化します。このステップは重要であり、SLAM の最終結果の精度に大きく影響します。最初の ORB 特徴点の対応関係は、ステレオペアの 2 つのイメージ間でmatchFeaturesを使用して検出されます。マッチするペアは、次の制約を満たす必要があります。

平行化されたステレオペアイメージ内の対応する 2 つの特徴点間の水平シフトが、最大視差よりも小さい。ステレオペアイメージのステレオアナグリフから、最大視差のおおよその値を決定できます。詳細については、Choosing Range of Disparityを参照してください。
平行化されたステレオペアイメージ内の対応する 2 つの特徴点間の垂直シフトが、しきい値よりも小さい。
マッチした特徴のスケールがほぼ同じである。

マッチした特徴点に対応する 3 次元ワールド位置は、次のように決定されます。

Choosing Range of Disparityを使用し、セミグローバルマッチング (SGM) 法を使用してステレオイメージの各ペアの視差マップを計算する。
reconstructSceneを使用して、視差マップから 3 次元ワールドポイントの座標を計算する。
特徴点とその 3 次元ワールド位置に対応する視差マップ内の位置を求める。

% Set random seed for reproducibility
rng(0);

% Load the initial camera pose. The initial camera pose is derived based 
% on the transformation between the camera and the vehicle:
% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/transform_camera_vehicle.t
initialPoseData = load("initialPose.mat");
initialPose     = initialPoseData.initialPose;

% Construct the reprojection matrix for 3-D reconstruction.
% The intrinsics for the dataset can be found at the following page:
% http://asrl.utias.utoronto.ca/datasets/2020-vtr-dataset/text_files/camera_parameters.txt
focalLength     = [387.777 387.777];     % specified in pixels
principalPoint  = [257.446 197.718];     % specified in pixels [x, y]
baseline        = 0.239965;              % specified in meters
imageSize       = size(currILeft,[1,2]); % in pixels [mrows, ncols]
intrinsics      = cameraIntrinsics(focalLength, principalPoint, imageSize);

reprojectionMatrix = [1, 0, 0, -principalPoint(1); 
    0, 1, 0, -principalPoint(2);
    0, 0, 0, focalLength(1);
    0, 0, 1/baseline, 0];

% In this example, the images are already undistorted and rectified. In a general workflow, 
% uncomment the following code to undistort and rectify the images.
% currILeft     = undistortImage(currILeft, intrinsics);
% currIRight    = undistortImage(currIRight, intrinsics);
% stereoParams  = stereoParameters(intrinsics, intrinsics, eye(3), [-baseline, 0 0]);
% [currILeft, currIRight] = rectifyStereoImages(currILeft, currIRight, stereoParams, OutputView="full");

% Detect and extract ORB features from the rectified stereo images
scaleFactor = 1.2;
numLevels   = 8;
numPoints = 600;
[currFeaturesLeft,  currPointsLeft]   = helperDetectAndExtractFeatures(im2gray(currILeft), scaleFactor, numLevels, numPoints); 
[currFeaturesRight, currPointsRight]  = helperDetectAndExtractFeatures(im2gray(currIRight), scaleFactor, numLevels, numPoints);

% Match feature points between the stereo images and get the 3-D world positions 
disparityRange = [0 48];  % specified in pixels
[xyzPoints, matchedPairs] = helperReconstructFromStereo(im2gray(currILeft), im2gray(currIRight), ...
    currFeaturesLeft, currFeaturesRight, currPointsLeft, currPointsRight, reprojectionMatrix, initialPose, disparityRange);

データの管理と可視化

最初のステレオペアを使用してマップを初期化した後で、imageviewsetとworldpointsetを使用して、最初のキーフレームと対応するマップ点を格納できます。

% Create an empty imageviewset object to store key frames
vSetKeyFrames = imageviewset;

% Create an empty worldpointset object to store 3-D map points
mapPointSet   = worldpointset;

% Add the first key frame
currKeyFrameId = 1;
vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose, Points=currPointsLeft,...
    Features=currFeaturesLeft.Features);

% Add 3-D map points
[mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);

% Add observations of the map points
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, stereoMapPointsIdx, matchedPairs(:, 1));

% Update view direction and depth
mapPointSet = updateLimitsAndDirection(mapPointSet, stereoMapPointsIdx, vSetKeyFrames.Views);

% Update representative view
mapPointSet = updateRepresentativeView(mapPointSet, stereoMapPointsIdx, vSetKeyFrames.Views);

% Visualize matched features in the first key frame
featurePlot = helperVisualizeMatchedFeaturesStereo(currILeft, currIRight, currPointsLeft, ...
    currPointsRight, matchedPairs);

% Visualize initial map points and camera trajectory
mapPlot     = helperVisualizeMotionAndStructureStereo(vSetKeyFrames, mapPointSet);

% Show legend
showLegend(mapPlot);

場所認識データベースの初期化

ループ検出は bags-of-words アプローチを使用して行われます。次の呼び出しを行うことにより、データセットに含まれる大量のイメージセットから抽出された ORB 記述子を使用して、bagOfFeaturesオブジェクトで表されるビジュアルボキャブラリがオフラインで作成されます。

bag = bagOfFeatures(imds,CustomExtractor=@helperORBFeatureExtractorFunction,TreeProperties=[3, 10], StrongestFeatures=1);

ここで、imds は学習イメージを格納するimageDatastoreオブジェクトであり、helperORBFeatureExtractorFunction は ORB 特徴抽出器関数です。詳細については、bag of visual words を用いた画像検索を参照してください。

ループ閉じ込み処理により、データベースがインクリメンタルに作成されます。このデータベースはinvertedImageIndexオブジェクトで表され、bag of ORB features に基づき visual word-to-image mapping を格納します。

% Load the bag of features data created offline
bofData         = load("bagOfFeaturesDataSLAM.mat");

% Initialize the place recognition database
loopDatabase    = invertedImageIndex(bofData.bof,SaveFeatureLocations=false);

% Add features of the first key frame to the database
addImageFeatures(loopDatabase, currFeaturesLeft, currKeyFrameId);

トラッキング

トラッキング処理は全ペアを使用して行われ、新しいキーフレームを挿入するタイミングを決定します。

% ViewId of the last key frame
lastKeyFrameId    = currKeyFrameId;

% Index of the last key frame in the input image sequence
lastKeyFrameIdx   = currFrameIdx; 

% Indices of all the key frames in the input image sequence
addedFramesIdx    = lastKeyFrameIdx;

currFrameIdx      = 2;
isLoopClosed      = false;

各フレームが次のように処理されます。

イメージのステレオペアごとに ORB 特徴が抽出され、既知の対応する 3 次元マップ点をもつ最後のキーフレームの特徴とのマッチングが行われます (matchFeaturesを使用)。
estworldposeを使用し、透視 n 点アルゴリズムでカメラ姿勢を推定します。

matchFeaturesInRadiusを使用して、カメラ姿勢をもとに最後のキーフレームで観測されたマップ点を現在のフレームに投影し、特徴の対応関係を探索します。

bundleAdjustmentMotionを使用して、現在のフレームにおける 3 次元から 2 次元への対応関係をもとに、動きのみのバンドル調整を行うことで、カメラ姿勢を調整します。
matchFeaturesInRadiusを使用し、局所地図点を現在のフレームに投影して、その他の特徴の対応関係を探索します。そして、bundleAdjustmentMotionを使用して、再度カメラ姿勢を調整します。
トラッキングの最後のステップでは、現在のフレームを新しいキーフレームにするかどうかを決定します。次の両方の条件が満たされている場合、フレームはキーフレームとなります。

最後のキーフレームから少なくとも 5 フレームが経過しているか、現在のフレームのトラッキングが 80 マップ点未満である。
現在のフレームで追跡されているマップ点が、参照キーフレームでトラッキングされている点の 90% 未満である。

現在のフレームがキーフレームになる場合、"局所地図作成" 処理に進みます。そうでない場合、次のフレームのトラッキングを開始します。

% Main loop
isLastFrameKeyFrame = true;
while ~isLoopClosed && currFrameIdx <= numel(imdsLeft.Files)

    currILeft  = readimage(imdsLeft, currFrameIdx);
    currIRight = readimage(imdsRight, currFrameIdx);

    currILeftGray = im2gray(currILeft);
    currIRightGray = im2gray(currIRight);

    [currFeaturesLeft, currPointsLeft]    = helperDetectAndExtractFeatures(currILeftGray, scaleFactor, numLevels, numPoints);
    [currFeaturesRight, currPointsRight]  = helperDetectAndExtractFeatures(currIRightGray, scaleFactor, numLevels, numPoints);

    % Track the last key frame
    % trackedMapPointsIdx:  Indices of the map points observed in the current left frame 
    % trackedFeatureIdx:    Indices of the corresponding feature points in the current left frame
    [currPose, trackedMapPointsIdx, trackedFeatureIdx] = helperTrackLastKeyFrame(mapPointSet, ...
        vSetKeyFrames.Views, currFeaturesLeft, currPointsLeft, lastKeyFrameId, intrinsics, scaleFactor);
    
    if isempty(currPose) || numel(trackedMapPointsIdx) < 30
        currFrameIdx = currFrameIdx + 1;
        continue
    end
    
    % Track the local map  and check if the current frame is a key frame.
    % localKeyFrameIds:   ViewId of the connected key frames of the current frame
    numSkipFrames     = 5;
    numPointsKeyFrame = 80;
    [localKeyFrameIds, currPose, trackedMapPointsIdx, trackedFeatureIdx, isKeyFrame] = ...
        helperTrackLocalMap(mapPointSet, vSetKeyFrames, trackedMapPointsIdx, ...
        trackedFeatureIdx, currPose, currFeaturesLeft, currPointsLeft, intrinsics, scaleFactor, ...
        isLastFrameKeyFrame, lastKeyFrameIdx, currFrameIdx, numSkipFrames, numPointsKeyFrame);

    % Match feature points between the stereo images and get the 3-D world positions
    [xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeftGray, currIRightGray, currFeaturesLeft, ...
        currFeaturesRight, currPointsLeft, currPointsRight, reprojectionMatrix, currPose, disparityRange);

    % Visualize matched features in the stereo image
    updatePlot(featurePlot, currILeft, currIRight, currPointsLeft, currPointsRight, trackedFeatureIdx, matchedPairs);
    
    if ~isKeyFrame && currFrameIdx < numel(imdsLeft.Files)
        currFrameIdx = currFrameIdx + 1;
        isLastFrameKeyFrame = false;
        continue
    else
        [untrackedFeatureIdx, ia] = setdiff(matchedPairs(:, 1), trackedFeatureIdx);
        xyzPoints = xyzPoints(ia, :);
        isLastFrameKeyFrame = true;
    end
    
    % Update current key frame ID
    currKeyFrameId  = currKeyFrameId + 1;

局所地図作成

局所地図作成はすべてのキーフレームに対して行われます。新しいキーフレームが決定されると、それをキーフレームに追加し、新しいキーフレームにおいて観測されたマップ点の属性を更新します。mapPointSet に含まれる外れ値をできるだけ少なくするには、有効なマップ点が少なくとも 3 つのキーフレームで観測されなければなりません。

現在のキーフレームにおける ORB 特徴点と、接続されたキーフレームを三角形分割することで、新しいマップ点が作成されます。matchFeatures を使用して、現在のキーフレームにおける非マッチの特徴点ごとに、接続されたキーフレームにおいて、他の非マッチの点との一致を探索します。局所バンドル調整により、現在のキーフレームの姿勢、接続されたキーフレームの姿勢、およびこれらのキーフレームで観測されたすべてのマップ点を調整します。

    % Add the new key frame    
    [mapPointSet, vSetKeyFrames] = helperAddNewKeyFrame(mapPointSet, vSetKeyFrames, ...
        currPose, currFeaturesLeft, currPointsLeft, trackedMapPointsIdx, trackedFeatureIdx, localKeyFrameIds);
        
    % Remove outlier map points that are observed in fewer than 3 key frames
    if currKeyFrameId == 2
        triangulatedMapPointsIdx = [];
    end
    
    mapPointSet = helperCullRecentMapPointsStereo(mapPointSet, ...
        trackedMapPointsIdx, triangulatedMapPointsIdx, stereoMapPointsIdx);
    
    % Add new map points computed from disparity 
    [mapPointSet, stereoMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);
    mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, stereoMapPointsIdx, ...
        untrackedFeatureIdx);
    
    % Create new map points by triangulation
    minNumMatches = 20;
    minParallax   = 0.35;
    [mapPointSet, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = helperCreateNewMapPointsStereo( ...
        mapPointSet, vSetKeyFrames, currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax, ...
        untrackedFeatureIdx, stereoMapPointsIdx);

    % Local bundle adjustment
    [refinedViews, dist] = connectedViews(vSetKeyFrames, currKeyFrameId, MaxDistance=2);
    refinedKeyFrameIds = refinedViews.ViewId;

    % Always fix the first two key frames
    fixedViewIds = refinedKeyFrameIds(dist==2);
    fixedViewIds = fixedViewIds(1:min(10, numel(fixedViewIds)));

    % Refine local key frames and map points
    [mapPointSet, vSetKeyFrames, mapPointIdx] = bundleAdjustment(...
        mapPointSet, vSetKeyFrames, [refinedKeyFrameIds; currKeyFrameId], intrinsics, ...
        FixedViewIDs=fixedViewIds, PointsUndistorted=true, AbsoluteTolerance=1e-7,...
        RelativeTolerance=1e-16, Solver='preconditioned-conjugate-gradient', MaxIteration=10);

    % Update view direction and depth
    mapPointSet = updateLimitsAndDirection(mapPointSet, mapPointIdx, ...
        vSetKeyFrames.Views);

    % Update representative view
    mapPointSet = updateRepresentativeView(mapPointSet, mapPointIdx, ...
        vSetKeyFrames.Views);

    % Visualize 3-D world points and camera trajectory
    updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

ループ閉じ込み

ループ閉じ込みのステップでは、局所地図作成処理で処理された現在のキーフレームを受け取り、ループを検出して閉じようと試みます。evaluateImageRetrieval を使用し、データベース内のイメージをクエリすることで、現在のキーフレームと視覚的に似ているループ候補が特定されます。候補キーフレームが最後のキーフレームと接続されておらず、隣接するキーフレームの 3 つがループ候補であれば、その候補キーフレームは有効です。

有効なループ閉じ込みの候補が見つかったら、estgeotform3dを使用して、ループ閉じ込みの候補フレームと現在のキーフレームの間の相対姿勢を計算します。そして、ループの接続を相対姿勢と共に追加し、mapPointSet と vSetKeyFrames を更新します。

    % Check loop closure after some key frames have been created    
    if currKeyFrameId > 50
        
        % Minimum number of feature matches of loop edges
        loopEdgeNumMatches = 50;
        
        % Detect possible loop closure key frame candidates
        [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ...
            loopDatabase, currILeft, loopEdgeNumMatches);
        
        isTooCloseView = currKeyFrameId - max(validLoopCandidates) < 100;
        if isDetected && ~isTooCloseView
            % Add loop closure connections
            [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo(...
                mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ...
                currFeaturesLeft, currPointsLeft, loopEdgeNumMatches);
        end
    end
    
    % If no loop closure is detected, add current features into the database
    if ~isLoopClosed
        addImageFeatures(loopDatabase,  currFeaturesLeft, currKeyFrameId);
    end
    
    % Update IDs and indices
    lastKeyFrameId  = currKeyFrameId;
    lastKeyFrameIdx = currFrameIdx;
    addedFramesIdx  = [addedFramesIdx; currFrameIdx]; 
    currFrameIdx    = currFrameIdx + 1;
end % End of main loop

Figure contains an axes object. The axes object with title Matched Features in Current Frame contains 4 objects of type image, line. One or more of the lines displays its values using only markers

Figure Point Cloud Player contains an axes object. The axes object with xlabel X, ylabel Y contains 12 objects of type line, text, patch, scatter. These objects represent Map points, Estimated trajectory.

最後に、vSetKeyFrames の Essential グラフ全体に対して姿勢グラフの最適化を適用し、ドリフトを修正します。Essential グラフは、Covisibility グラフの接続のうち、一致数が minNumMatches よりも少ない接続を削除することで、内部的に作成されます。姿勢グラフの最適化後、最適化された姿勢を使用してマップ点の 3 次元での位置を更新します。

% Optimize the poses
vSetKeyFramesOptim = optimizePoses(vSetKeyFrames, minNumMatches, Tolerance=1e-16);

% Update map points after optimizing the poses
mapPointSet = helperUpdateGlobalMap(mapPointSet, vSetKeyFrames, vSetKeyFramesOptim);

updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

% Plot the optimized camera trajectory
optimizedPoses  = poses(vSetKeyFramesOptim);
plotOptimizedTrajectory(mapPlot, optimizedPoses)

% Update legend
showLegend(mapPlot);

グラウンドトゥルースとの比較

最適化されたカメラの軌跡をグラウンドトゥルースと比較して、解の精度を評価できます。ダウンロードしたデータには、各フレームの GPS 位置が格納された gps.txt ファイルが含まれています。関数 helperReadGroundTruth を使用して、GPS 位置を地理座標からローカル直交座標に変換できます。

gpsLocations = helperReadGroundTruth(dataFolder)

この例では、変換済みの GPS データを M ファイルから読み込むこともできます。

% Load GPS data
gpsData     = load("gpsLocation.mat");
gpsLocation = gpsData.gpsLocation;

% Transform GPS locations to the reference coordinate system
gTruth = helperTransformGPSLocations(gpsLocation, optimizedPoses);

% Plot the GPS locations
plotActualTrajectory(mapPlot, gTruth(addedFramesIdx, :));

% Show legend
showLegend(mapPlot);

Figure Point Cloud Player contains an axes object. The axes object with xlabel X, ylabel Y contains 4 objects of type scatter, line. These objects represent Map points, Estimated trajectory, Optimized trajectory, GPS trajectory.

ステレオイメージからの緻密な再構成

調整されたカメラの姿勢をもとに、キーフレームに対応するステレオイメージからの緻密な再構成を実行できます。

% Create an array of pointCloud objects to store the 3-D world points
% associated with the key frames
ptClouds =  repmat(pointCloud(zeros(1, 3)), numel(addedFramesIdx), 1);

for i = 1: numel(addedFramesIdx)
    ILeft  = readimage(imdsLeft, addedFramesIdx(i));
    IRight = readimage(imdsRight, addedFramesIdx(i));

    % Reconstruct scene from disparity
    disparityMap = disparitySGM(im2gray(ILeft), im2gray(IRight), DisparityRange=disparityRange);
    xyzPoints    = reconstructScene(disparityMap, reprojectionMatrix);

    % Ignore the upper half of the images which mainly show the sky
    xyzPoints(1:floor(imageSize(1)/2), :, :) = NaN;

    % Ignore the lower part of the images which show the vehicle
    xyzPoints(imageSize(1)-50:end, :, :) = NaN;

    xyzPoints  = reshape(xyzPoints, [imageSize(1)*imageSize(2), 3]);

    % Get color from the color image
    colors = reshape(ILeft, [imageSize(1)*imageSize(2), 3]);
    
    % Remove world points that are too far away from the camera
    validIndex = xyzPoints(:, 3) > 0 & xyzPoints(:, 3) < 100/reprojectionMatrix(4, 3);  
    xyzPoints  = xyzPoints(validIndex, :);
    colors     = colors(validIndex, :);
    
    % Transform world points to the world coordinates
    currPose   = optimizedPoses.AbsolutePose(i);
    xyzPoints  = transformPointsForward(currPose, xyzPoints);
    ptCloud    = pointCloud(xyzPoints, Color=colors);

    % Downsample the point cloud
    ptClouds(i) = pcdownsample(ptCloud, random=0.5); 
end

% Concatenate the point clouds
pointCloudsAll = pccat(ptClouds);

% Visualize the point cloud
figure
ax = pcshow(pointCloudsAll,VerticalAxis="y", VerticalAxisDir="down");
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Dense Reconstruction')

% Display the bird's eye view of the scene
view(ax, [0 0 1]);
camroll(ax, 90);

Figure contains an axes object. The axes object with title Dense Reconstruction, xlabel X, ylabel Y contains an object of type scatter.

コード生成

コード生成では ImageDatastore オブジェクトがサポートされないため、入力イメージを読み取り、グレースケールに変換して、cell 配列に保存します。

for i=1:numel(imdsLeft.Files)
    imagesLeft{i} = im2gray(readimage(imdsLeft,i));
end

for i=1:numel(imdsRight.Files)
    imagesRight{i} = im2gray(readimage(imdsRight,i));
end

初期カメラ姿勢をワークスペースに読み込みます。初期カメラ姿勢は、カメラと車両の間の変換に基づいて導出されます。初期姿勢データからカメラの 3 次元フォワード剛体変換を取得します。

initialPoseData = load("initialPose.mat");
initialPose = initialPoseData.initialPose;
% Get the forward 3-D rigid transformation of the camera
forwardTransformation = initialPose.A;

関数 helperStereoVisualSLAMCodegen を使用して、ホストコンピューターに展開するための C++ コードを生成します。MATLAB Coder の要件を満たすには、アルゴリズムを可視化のためのコードから分離するよう、コードを再構成する必要があります。関数 helperStereoVisualSLAMCodegen は、地図の初期化、追跡、局所地図作成、およびループ閉じ込みのアルゴリズムプロセスをカプセル化します。この関数は、イメージの 2 つの cell 配列 (imagesLeft と imagesRight,) と初期カメラ姿勢を入力引数として受け取り、3 次元worldpointset、推定カメラ姿勢、およびすべてのキーフレームのインデックスを出力します。

コンパイル命令 %#codegen (MATLAB Coder)コマンドを使用して、関数 helperStereoVisualSLAMCodegen を MEX ファイルにコンパイルします。既定では、生成された MEX ファイルの名前は、元の MATLAB 関数に接尾辞 "_mex" を付加した helperStereoVisualSLAMCodegen_mex となります。MEX ファイルをカスタムファイル名で保存するには、-o オプションを使用します。-report オプションを指定して、コンパイルレポートを生成できます。このレポートには元の MATLAB コードと、C コードの生成時に作成された関連ファイルが含まれています。MATLAB Coder が生成ファイルを保存できるように、一時ディレクトリを作成することもできます。MEX ファイル生成に関連するその他のオプションについては、codegen ページのoptions (MATLAB Coder)を参照してください。

cpuConfig = coder.config("mex");
cpuConfig.TargetLang = "C++";
% Generate C++ code
codegen -config cpuConfig helperStereoVisualSLAMCodegen -args {imagesLeft,imagesRight,forwardTransformation}

Code generation successful.

関数 helperStereoVisualSLAMCodegen_mex を使用して、imagesLeft、imagesRight、および forwardTransformation に基づいて推定され最適化されたカメラ姿勢を取得します。

% Set the random number generation seed to default, for reproducibility
rng("default");
% Run the generated MEX file
stereoSlamOut = helperStereoVisualSLAMCodegen_mex(imagesLeft,imagesRight,forwardTransformation);

補助関数 helperVisualizeStereoVisualSlam への入力引数として stereoSlamOut を指定して、カメラの推定軌跡と実際の軌跡をプロットします。

% Visualize the results
helperVisualizeStereoVisualSlam(stereoSlamOut);

サポート関数

この例には、以下の補助関数が個別のファイルとして含まれています。

helperDetectAndExtractFeatures — イメージから ORB 特徴を検出して抽出します。

helperReconstructFromStereo — 視差マップを使用して、ステレオイメージからシーンを再構成します。

helperFindValidFeaturePairs — ステレオイメージのペア間で特徴をマッチングします。

helperCullRecentMapPointsStereo — 最近追加されたマップ点をカリングします。

helperAddLoopConnectionsStereo — 現在のキーフレームと有効なループ候補キーフレームの間の接続を追加します。

helperAddNewKeyFrame — キーフレームセットにキーフレームを追加します。

helperCheckLoopClosure — 特徴データベースから視覚的に似たイメージを取得して、ループ候補となるキーフレームを検出します。

helperCreateNewMapPointsStereo — 現在のキーフレームと、接続されたキーフレームの間の一致した特徴点を三角形分割して、新しいマップ点を作成します。

helperStereoVisualSLAMCodegen — コード生成ワークフロー用のステレオ SLAM アルゴリズムが含まれます。

helperVisualizeStereoVisualSlam — コード生成ワークフローの出力を可視化します。

helperVisualizeMotionAndStructureStereo — マップ点とカメラの軌跡を表示します。

helperVisualizeMatchedFeaturesStereo — フレーム内のマッチした特徴を表示します。

helperTrackLocalMap — 局所地図を追跡して、カメラ姿勢を調整します。

helperTrackLastKeyFrame — 最後のキーフレームを追跡して、カメラ姿勢を推定します。

この例では、以下の補助関数も使用します。

helperUpdateGlobalMap — 姿勢グラフの最適化後にマップ点の 3 次元位置を更新します。

function mapPointSet = helperUpdateGlobalMap(mapPointSet,vSetKeyFrames,vSetKeyFramesOptim)

posesOld = vSetKeyFrames.Views.AbsolutePose;
posesNew = vSetKeyFramesOptim.Views.AbsolutePose;
positionsOld = mapPointSet.WorldPoints;
positionsNew = positionsOld;
indices = 1:mapPointSet.Count;

% Update world location of each map point based on the new absolute pose of 
% the corresponding major view
for i = 1:mapPointSet.Count
    majorViewIds = mapPointSet.RepresentativeViewId(i);
    tform = rigidtform3d(posesNew(majorViewIds).A/posesOld(majorViewIds).A);
    positionsNew(i,:) = transformPointsForward(tform,positionsOld(i,:));
end
mapPointSet = updateWorldPoints(mapPointSet,indices,positionsNew);
end

helperReadGroundTruth — グラウンドトゥルースデータを読み取り、それを直交座標に変換します。

function gTruth = helperReadGroundTruth(dataFolder) %#ok<DEFNU>
opts = delimitedTextImportOptions("NumVariables",5);

% Specify range and delimiter
opts.DataLines = [1 Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["Var1","Var2","VarName3","VarName4","VarName5"];
opts.SelectedVariableNames = ["VarName3","VarName4","VarName5"];
opts.VariableTypes = ["string","string","double","double","double"];

% Specify file level properties
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Specify variable properties
opts = setvaropts(opts, ["Var1" "Var2"],"WhitespaceRule","preserve");
opts = setvaropts(opts, ["Var1" "Var2"],"EmptyFieldRule","auto");

% Import the data
gps = readtable([dataFolder "/gps.txt"],opts);
gps = table2array(gps);

index = helperImportTimestampFile(dataFolder);

origin = [gps(1,1) gps(1,2) gps(1,3)];
[x,y,z] = latlon2local(gps(index,1),gps(index,2),gps(index,3),origin);
gTruth = [x y z];
end

helperImportTimestampFile — GPS データとイメージのタイムスタンプをインポートします。

function index = helperImportTimestampFile(dataFolder)
% Read GPS and image timestamp files
% Set up the import Options and import the data
opts = delimitedTextImportOptions("NumVariables",2);

% Specify range and delimiter
opts.DataLines = [1 Inf];
opts.Delimiter = ",";

% Specify column names and types
opts.VariableNames = ["VarName1","VarName2"];
opts.VariableTypes = ["double","double"];

% Specify file-level properties
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";

% Import the GPS and image data
timestampsgps = readtable([dataFolder "/timestamps_gps.txt"], opts);
timestampsimg = readtable([dataFolder "timestamps_images.txt"], opts);

% Convert to output type
timestampsgps = table2array(timestampsgps);
timestampsimg = table2array(timestampsimg);

% Synchronize the timestamps of the GPS data and images
index = interp1(timestampsgps(:,2),1:size(timestampsgps,1),timestampsimg(:,2),"nearest","extrap");
end

helperTransformGPSLocations — GPS 位置を参照座標系に変換します。

function gTruth = helperTransformGPSLocations(gpsLocations,optimizedPoses)

initialYawGPS  = atan((gpsLocations(100,2) - gpsLocations(1,2))/ ...
    (gpsLocations(100,1) - gpsLocations(1,1)));
initialYawSLAM = atan((optimizedPoses.AbsolutePose(50).Translation(2) - ...
    optimizedPoses.AbsolutePose(1).Translation(2))/ ...
    (optimizedPoses.AbsolutePose(59).Translation(1) - ...
    optimizedPoses.AbsolutePose(1).Translation(1)));

relYaw = initialYawGPS - initialYawSLAM;
relTranslation = optimizedPoses.AbsolutePose(1).Translation;

initialTform = rotationVectorToMatrix([0 0 relYaw]);
for i = 1:size(gpsLocations, 1)
    gTruth(i, :) =  initialTform * gpsLocations(i, :)' + relTranslation';
end
end

参考文献

[1] Mur-Artal, Raul, and Juan D. Tardós. “ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras.” IEEE Transactions on Robotics 33, no. 5 (October 2017): 1255–62. https://doi.org/10.1109/TRO.2017.2705103.