Main Content

このページの翻訳は最新ではありません。ここをクリックして、英語の最新版を参照してください。

ステレオ Visual Simultaneous Localization and Mapping

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

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

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

処理パイプラインの概要

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

  • 地図の初期化: パイプラインはまず、視差マップを使用して、ステレオ イメージのペアから 3 次元点のマップを初期化します。左のイメージは最初のキー フレームとして保存されます。

  • トラッキング: マップが初期化された後は、新しいステレオ ペアごとに、左のイメージの特徴を最後のキー フレームの特徴とマッチングすることにより、カメラ姿勢が推定されます。推定されたカメラ姿勢は、局所地図のトラッキングにより調整されます。

  • 局所地図作成: 現在の左のイメージがキー フレームとして識別された場合、ステレオ ペアの視差から新しい 3 次元マップ点が計算されます。この段階で、カメラ姿勢と 3 次元点を調整することにより、バンドル調整を使用して再投影誤差を最小化します。

  • ループ閉じ込み: bag-of-features アプローチを使用して、キー フレームごとにそれまでのすべてのキー フレームと比較することで、ループを検出します。ループ閉じ込みが検出されると姿勢グラフが最適化され、すべてのキー フレームのカメラ姿勢を調整します。

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

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

dataFolder   = [fullfile(pwd), filesep, 'stereoImageData'];
zipFileName  = [dataFolder, filesep, 'run_000005.zip'];
folderExists = exist(dataFolder, 'dir');

% Create a folder in the current directory to save the downloaded file
if ~folderExists  
    mkdir(dataFolder); 
    disp('Downloading run_000005.zip (818 MB). This download can take a few minutes.') 
    !wget ftp://asrl3.utias.utoronto.ca/2020-vtr-dataset/UTIAS-In-The-Dark/run_000005.zip -P ./stereoImageData -nv

    % 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');

地図の初期化

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

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

  • 平行化されたステレオ ペア イメージ内の対応する 2 つの特徴点間の垂直シフトが、しきい値よりも小さい。

  • マッチした特徴のスケールがほぼ同じである。

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

  • disparitySGM を使用して、セミグローバル マッチング (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.tx 
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;
[currFeaturesLeft,  currPointsLeft]   = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels); 
[currFeaturesRight, currPointsRight]  = helperDetectAndExtractFeatures(currIRight, scaleFactor, numLevels);

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

データの管理と可視化

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

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

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

% Create a helperViewDirectionAndDepth object to store view direction and depth 
directionAndDepth = helperViewDirectionAndDepth(size(xyzPoints, 1));

% 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));

% 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',[5, 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;

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

  1. イメージのステレオ ペアごとに ORB 特徴が抽出され、既知の対応する 3 次元マップ点をもつ最後のキー フレームの特徴とのマッチングが行われます (matchFeaturesを使用)。

  2. estimateWorldCameraPose を使用し、透視 n 点アルゴリズムでカメラ姿勢を推定します。

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

  1. bundleAdjustmentMotionを使用して、現在のフレームにおける 3 次元から 2 次元への対応関係をもとに、動きのみのバンドル調整を行うことで、カメラ姿勢を調整します。

  2. matchFeaturesInRadiusを使用し、局所地図点を現在のフレームに投影して、その他の特徴の対応関係を探索します。そして、bundleAdjustmentMotionを使用して、再度カメラ姿勢を調整します。

  3. トラッキングの最後のステップでは、現在のフレームを新しいキー フレームにするかどうかを決定します。次の両方の条件が満たされている場合、フレームはキー フレームとなります。

  • 最後のキー フレームから少なくとも 5 フレームが経過しているか、現在のフレームのトラッキングが 100 マップ点未満である。

  • 現在のフレームで追跡されているマップ点が、参照キー フレームでトラッキングされている点の 90% 未満である。

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

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

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

    [currFeaturesLeft, currPointsLeft]    = helperDetectAndExtractFeatures(currILeft, scaleFactor, numLevels);
    [currFeaturesRight, currPointsRight]  = helperDetectAndExtractFeatures(currIRight, scaleFactor, numLevels);

    % 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 = 100;
    [localKeyFrameIds, currPose, trackedMapPointsIdx, trackedFeatureIdx, isKeyFrame] = ...
        helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, trackedMapPointsIdx, ...
        trackedFeatureIdx, currPose, currFeaturesLeft, currPointsLeft, intrinsics, scaleFactor, numLevels, ...
        isLastFrameKeyFrame, lastKeyFrameIdx, currFrameIdx, numSkipFrames, numPointsKeyFrame);

    % Match feature points between the stereo images and get the 3-D world positions
    [xyzPoints, matchedPairs] = helperReconstructFromStereo(currILeft, currIRight, 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, directionAndDepth, trackedMapPointsIdx] = ...
        helperCullRecentMapPoints(mapPointSet, directionAndDepth, 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);
    
    % Update view direction and depth
    directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, ...
        [trackedMapPointsIdx; triangulatedMapPointsIdx; stereoMapPointsIdx], true);
    
    % Local bundle adjustment
    [mapPointSet, directionAndDepth, vSetKeyFrames, triangulatedMapPointsIdx, stereoMapPointsIdx] = ...
        helperLocalBundleAdjustmentStereo(mapPointSet, directionAndDepth, vSetKeyFrames, ...
        currKeyFrameId, intrinsics, triangulatedMapPointsIdx, stereoMapPointsIdx); 
    
    % Visualize 3-D world points and camera trajectory
    updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

ループ閉じ込み

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

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

    % 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, 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

Loop edge added between keyframe: 4 and 180
Loop edge added between keyframe: 3 and 180
Loop edge added between keyframe: 6 and 180

最後に、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, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim);

updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

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

% Update legend
showLegend(mapPlot);

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

最適化されたカメラの軌跡をグラウンド トゥルースと比較して、解の精度を評価できます。ダウンロードしたデータには、各フレームの GPS 位置が格納された gps.txt ファイルが含まれています。Automated Driving Toolbox のlatlon2local (Automated Driving Toolbox)または Mapping Toolbox のgeodetic2enu (Mapping Toolbox)を使用して、GPS 位置を地理座標からローカル直交座標に変換できます。この例では、変換済みの 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);

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

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

% 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  = xyzPoints * currPose.Rotation + currPose.Translation;
    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);

サポート関数

短い補助関数を以下に示します。大きな関数は別ファイルにあります。

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

function [features, validPoints] = helperDetectAndExtractFeatures(Irgb, scaleFactor, numLevels)
 
numPoints = 1500;

% Detect ORB features
Igray  = im2gray(Irgb);

points = detectORBFeatures(Igray, 'ScaleFactor', scaleFactor, 'NumLevels', numLevels);

% Select a subset of features, uniformly distributed throughout the image
points = selectUniform(points, numPoints, size(Igray, 1:2));

% Extract features
[features, validPoints] = extractFeatures(Igray, points);
end

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

function [xyzPoints, indexPairs] = helperReconstructFromStereo(I1, I2, ...
    features1, features2, points1, points2, reprojectionMatrix, currPose, disparityRange)

indexPairs     = helperFindValidFeaturePairs(features1, features2, points1, points2, disparityRange);

% Compute disparity for all pixels in the left image. In practice, it is more 
% common to compute disparity just for the matched feature points.
disparityMap   = disparitySGM(rgb2gray(I1), rgb2gray(I2), 'DisparityRange', disparityRange);
xyzPointsAll   = reconstructScene(disparityMap, reprojectionMatrix);

% Find the corresponding world point of the matched feature points 
locations      = floor(points1.Location(indexPairs(:, 1), [2 1]));
xyzPoints      = [];
isPointFound   = false(size(points1));

for i = 1:size(locations, 1)
    point3d = squeeze(xyzPointsAll(locations(i,1), locations(i, 2), :))';
    isPointValid   = point3d(3) > 0 & point3d(3) < 200/reprojectionMatrix(4, 3);
    if isPointValid  
        xyzPoints       = [xyzPoints; point3d]; %#ok<*AGROW> 
        isPointFound(i) = true;
    end
end
indexPairs = indexPairs(isPointFound, :);
xyzPoints  = xyzPoints * currPose.Rotation + currPose.Translation;
end

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

function indexPairs = helperFindValidFeaturePairs(features1, features2, points1, points2, disparityRange)
indexPairs  = matchFeatures(features1, features2,...
    'Unique', true, 'MaxRatio', 1, 'MatchThreshold', 40);

matchedPoints1 = points1.Location(indexPairs(:,1), :);
matchedPoints2 = points2.Location(indexPairs(:,2), :);
scales1        = points1.Scale(indexPairs(:,1), :);
scales2        = points2.Scale(indexPairs(:,2), :);

dist2EpipolarLine = abs(matchedPoints2(:, 2) - matchedPoints1(:, 2));
shiftDist = matchedPoints1(:, 1) - matchedPoints2(:, 1);

isCloseToEpipolarline = dist2EpipolarLine < 2*scales2;
isDisparityValid      = shiftDist > 0 & shiftDist < disparityRange(2);
isScaleIdentical      = scales1 == scales2;
indexPairs = indexPairs(isCloseToEpipolarline & isDisparityValid & isScaleIdentical, :);
end

"helperCullRecentMapPoints" は、最近追加されたマップ点をカリングします。

function [mapPointSet, directionAndDepth, mapPointsIdx] = ...
    helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx, stereoMapPointsIndices)

outlierIdx = setdiff([newPointIdx; stereoMapPointsIndices], mapPointsIdx);

if ~isempty(outlierIdx)
    mapPointSet   = removeWorldPoints(mapPointSet, outlierIdx);
    directionAndDepth = remove(directionAndDepth, outlierIdx);
    mapPointsIdx  = mapPointsIdx - arrayfun(@(x) nnz(x>outlierIdx), mapPointsIdx);
end
end

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

function [mapPointSet, directionAndDepth] = helperUpdateGlobalMap(...
    mapPointSet, directionAndDepth, 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 = directionAndDepth.MajorViewId(i);
    tform = posesOld(majorViewIds).T \ posesNew(majorViewIds).T ;
    positionsNew(i, :) = positionsOld(i, :) * tform(1:3,1:3) + tform(4, 1:3);
end
mapPointSet = updateWorldPoints(mapPointSet, indices, positionsNew);
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 (2017): 1255-1262.