RGB-D カメラでの Visual SLAM

Visual simultaneous localization and mapping (vSLAM) とは、環境地図作成を行うのと同時に、周囲に対するカメラの位置と向きを計算する処理を意味します。

vSLAM は単眼カメラを使用して実行できます。しかし、深度を正確に計算できず、推定軌跡が不明で、時間の経過とともにドリフトが発生します。最初のフレームから三角形分割できないように初期マップを作成するためには、単眼カメラの複数のビューを使用しなければなりません。より適切で信頼性の高い解決策は、1 つの RGB カラーイメージと 1 つの深度イメージで構成される RGB-D カメラを使用することです。

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

処理パイプラインの概要

RGB-D vSLAM のパイプラインは、単眼の Visual Simultaneous Localization and Mappingの例にある単眼 vSLAM のパイプラインと非常によく似ています。主な違いは、"地図の初期化" 段階で、2 フレームのカラーイメージではなく、1 つのカラーイメージと 1 つの深度イメージで構成されるイメージのペアから 3 次元マップ点が作成されることです。

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

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

この例で使用するデータは TUM RGB-D ベンチマーク [2] のものです。Web ブラウザーを使用するか、次のコードを実行することで、データを一時フォルダーにダウンロードできます。

baseDownloadURL = "https://cvg.cit.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_long_office_household.tgz"; 
dataFolder      = fullfile(tempdir, 'tum_rgbd_dataset', filesep); 
options         = weboptions('Timeout', Inf);
tgzFileName     = [dataFolder, 'fr3_office.tgz'];
folderExists    = exist(dataFolder, 'dir');

% Create a folder in a temporary directory to save the downloaded file
if ~folderExists  
    mkdir(dataFolder); 
    disp('Downloading fr3_office.tgz (1.38 GB). This download can take a few minutes.') 
    websave(tgzFileName, baseDownloadURL, options); 
    
    % Extract contents of the downloaded file
    disp('Extracting fr3_office.tgz (1.38 GB) ...') 
    untar(tgzFileName, dataFolder); 
end

Downloading fr3_office.tgz (1.38 GB). This download can take a few minutes.

Extracting fr3_office.tgz (1.38 GB) ...

imageFolder = [dataFolder, 'rgbd_dataset_freiburg3_long_office_household/'];

2 つのimageDatastoreオブジェクトを作成して、カラーイメージと深度イメージをそれぞれ保存します。

imgFolderColor = [imageFolder,'rgb/'];
imgFolderDepth = [imageFolder,'depth/'];
imdsColor      = imageDatastore(imgFolderColor);
imdsDepth      = imageDatastore(imgFolderDepth);

カラーイメージと深度イメージが、データセット内で非同期的に生成されることに注意してください。したがって、タイムスタンプに基づいてカラーイメージを深度イメージに関連付ける必要があります。

% Load time stamp data of color images
timeColor = helperImportTimestampFile([imageFolder, 'rgb.txt']);

% Load time stamp data of depth images
timeDepth = helperImportTimestampFile([imageFolder, 'depth.txt']);

% Align the time stamp
indexPairs = helperAlignTimestamp(timeColor, timeDepth);

% Select the synchronized image data
imdsColor     = subset(imdsColor, indexPairs(:, 1));
imdsDepth     = subset(imdsDepth, indexPairs(:, 2));

% Inspect the first RGB-D image
currFrameIdx  = 1;
currIcolor    = readimage(imdsColor, currFrameIdx);
currIdepth    = readimage(imdsDepth, currFrameIdx);
imshowpair(currIcolor, currIdepth, "montage");

地図の初期化

パイプラインではまず、3 次元ワールドポイントをもつマップを初期化します。このステップは重要であり、SLAM の最終結果の精度に大きく影響します。初期 ORB 特徴点は、helperDetectAndExtractFeatures を使用して最初のカラーイメージから抽出されます。対応する 3 次元ワールド位置は、helperReconstructFromRGBD を使用して、特徴点のピクセル座標と深度値から計算できます。

% Set random seed for reproducibility
rng(0);

% Create a cameraIntrinsics object to store the camera intrinsic parameters.
% The intrinsics for the dataset can be found at the following page:
% https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats
focalLength    = [535.4, 539.2];    % in units of pixels
principalPoint = [320.1, 247.6];    % in units of pixels
imageSize      = size(currIcolor,[1,2]); % in pixels [mrows, ncols]
depthFactor    = 5e3;
intrinsics     = cameraIntrinsics(focalLength,principalPoint,imageSize);

% Detect and extract ORB features from the color image
scaleFactor = 1.2;
numLevels   = 8;
[currFeatures, currPoints] = helperDetectAndExtractFeatures(currIcolor, scaleFactor, numLevels); 

initialPose = rigidtform3d();
[xyzPoints, validIndex] = helperReconstructFromRGBD(currPoints, currIdepth, intrinsics, initialPose, depthFactor);

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

ループ検出は 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
currKeyFrameId = 1;
addImageFeatures(loopDatabase, currFeatures, currKeyFrameId);

データの管理と可視化

カラーイメージと深度イメージの最初のペアを使用してマップを初期化した後で、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
vSetKeyFrames = addView(vSetKeyFrames, currKeyFrameId, initialPose, Points=currPoints,...
    Features=currFeatures.Features);

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

% Add observations of the map points
mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, rgbdMapPointsIdx, validIndex);

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

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

% Visualize matched features in the first key frame
featurePlot = helperVisualizeMatchedFeaturesRGBD(currIcolor, currIdepth, currPoints(validIndex));

% Visualize initial map points and camera trajectory
xLim = [-4 4];
yLim = [-3 1];
zLim = [-1 6];
mapPlot  = helperVisualizeMotionAndStructure(vSetKeyFrames, mapPointSet, xLim, yLim, zLim);

% Show legend
showLegend(mapPlot);

トラッキング

トラッキング処理はすべての RGB-D イメージを使用して行われ、新しいキーフレームを挿入するタイミングを決定します。

% 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を使用)。
"透視 n 点" アルゴリズムを使用してカメラの姿勢を推定します。このアルゴリズムでは、estworldposeを使用して 3 次元点のセットとそれに対応する 2 次元投影を指定し、キャリブレートされたカメラの姿勢を推定します。
matchFeaturesInRadiusを使用して、カメラ姿勢をもとに最後のキーフレームで観測されたマップ点を現在のフレームに投影し、特徴の対応関係を探索します。
bundleAdjustmentMotionを使用して、現在のフレームにおける 3 次元から 2 次元への対応関係をもとに、動きのみのバンドル調整を行うことで、カメラ姿勢を調整します。
matchFeaturesInRadiusを使用し、局所地図点を現在のフレームに投影して、その他の特徴の対応関係を探索します。そして、bundleAdjustmentMotionを使用して、再度カメラ姿勢を調整します。
トラッキングの最後のステップでは、現在のフレームを新しいキーフレームにするかどうかを決定します。次の両方の条件が満たされている場合、フレームはキーフレームとなります。

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

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

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

    currIcolor = readimage(imdsColor, currFrameIdx);
    currIdepth = readimage(imdsDepth, currFrameIdx);

    [currFeatures, currPoints]    = helperDetectAndExtractFeatures(currIcolor, 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, currFeatures, currPoints, 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.
    % A frame is a key frame if both of the following conditions are satisfied:
    %
    % 1. At least 20 frames have passed since the last key frame or the 
    %    current frame tracks fewer than 100 map points. 
    % 2. The map points tracked by the current frame are fewer than 90% of 
    %    points tracked by the reference key frame.
    %
    % localKeyFrameIds:   ViewId of the connected key frames of the current frame
    numSkipFrames     = 20;
    numPointsKeyFrame = 100;
    [localKeyFrameIds, currPose, trackedMapPointsIdx, trackedFeatureIdx, isKeyFrame] = ...
        helperTrackLocalMap(mapPointSet, vSetKeyFrames, trackedMapPointsIdx, ...
        trackedFeatureIdx, currPose, currFeatures, currPoints, intrinsics, scaleFactor, numLevels, ...
        isLastFrameKeyFrame, lastKeyFrameIdx, currFrameIdx, numSkipFrames, numPointsKeyFrame);

    % Visualize matched features
    updatePlot(featurePlot, currIcolor, currIdepth, currPoints(trackedFeatureIdx));
    
    if ~isKeyFrame
        currFrameIdx = currFrameIdx + 1;
        isLastFrameKeyFrame = false;
        continue
    else
        % Match feature points between the stereo images and get the 3-D world positions
        [xyzPoints, validIndex] = helperReconstructFromRGBD(currPoints, currIdepth, ...
            intrinsics, currPose, depthFactor);

        [untrackedFeatureIdx, ia] = setdiff(validIndex, 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, currFeatures, currPoints, trackedMapPointsIdx, trackedFeatureIdx, localKeyFrameIds);
        
    % Remove outlier map points that are observed in fewer than 3 key frames
    if currKeyFrameId == 2
        triangulatedMapPointsIdx = [];
    end
    
    [mapPointSet, trackedMapPointsIdx] = ...
        helperCullRecentMapPoints(mapPointSet, trackedMapPointsIdx, triangulatedMapPointsIdx, ...
        rgbdMapPointsIdx);
    
    % Add new map points computed from disparity 
    [mapPointSet, rgbdMapPointsIdx] = addWorldPoints(mapPointSet, xyzPoints);
    mapPointSet = addCorrespondences(mapPointSet, currKeyFrameId, rgbdMapPointsIdx, ...
        untrackedFeatureIdx);
    
    % Create new map points by triangulation
    minNumMatches = 10;
    minParallax   = 0.35;
    [mapPointSet, vSetKeyFrames, triangulatedMapPointsIdx, rgbdMapPointsIdx] = helperCreateNewMapPointsStereo( ...
        mapPointSet, vSetKeyFrames, currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax, ...
        untrackedFeatureIdx, rgbdMapPointsIdx);

    % Update view direction and depth
    mapPointSet = updateLimitsAndDirection(mapPointSet, [triangulatedMapPointsIdx; rgbdMapPointsIdx], ...
        vSetKeyFrames.Views);

    % Update representative view
    mapPointSet = updateRepresentativeView(mapPointSet, [triangulatedMapPointsIdx; rgbdMapPointsIdx], ...
        vSetKeyFrames.Views);

    % Local bundle adjustment
    [mapPointSet, vSetKeyFrames, triangulatedMapPointsIdx, rgbdMapPointsIdx] = ...
        helperLocalBundleAdjustmentStereo(mapPointSet, vSetKeyFrames, ...
        currKeyFrameId, intrinsics, triangulatedMapPointsIdx, rgbdMapPointsIdx);

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

ループ閉じ込み

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

有効なループ候補が見つかると、estgeotform3dを使用して、ループ候補フレームと現在のキーフレームの間の相対姿勢を計算します。相対姿勢はrigidtform3dオブジェクトに保存されている 3 次元剛体変換を表します。そして、ループの接続を相対姿勢と共に追加し、mapPointSet と vSetKeyFrames を更新します。

    % Check loop closure after some key frames have been created    
    if currKeyFrameId > 20
        
        % Minimum number of feature matches of loop edges
        loopEdgeNumMatches = 120;
        
        % Detect possible loop closure key frame candidates
        [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ...
            loopDatabase, currIcolor, loopEdgeNumMatches);
        
        if isDetected 
            % Add loop closure connections
            maxDistance = 0.1;
            [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnectionsStereo(...
                mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ...
                currFeatures, currPoints, loopEdgeNumMatches, maxDistance);
        end
    end
    
    % If no loop closure is detected, add current features into the database
    if ~isLoopClosed
        addImageFeatures(loopDatabase,  currFeatures, currKeyFrameId);
    end
    
    % Update IDs and indices
    lastKeyFrameId  = currKeyFrameId;
    lastKeyFrameIdx = currFrameIdx;
    addedFramesIdx  = [addedFramesIdx; currFrameIdx]; %#ok<AGROW>
    currFrameIdx    = currFrameIdx + 1;
end % End of main loop

Loop edge added between keyframe: 4 and 87
Loop edge added between keyframe: 3 and 87

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

% Optimize the poses
minNumMatches      = 50;
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);

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

最適化されたカメラの軌跡をグラウンドトゥルースと比較して、精度を評価できます。ダウンロードしたデータには、各フレームのカメラ姿勢のグラウンドトゥルースが格納された groundtruth.txt ファイルが含まれています。データは MAT ファイル形式で保存されています。軌跡の平方根平均二乗誤差 (RMSE) の推定を計算することもできます。

% Load ground truth 
gTruthData = load("orbslamGroundTruth.mat");
gTruth     = gTruthData.gTruth;

% Plot the actual camera trajectory 
plotActualTrajectory(mapPlot, gTruth(indexPairs(addedFramesIdx, 1)), optimizedPoses);

% Show legend
showLegend(mapPlot);

% Evaluate tracking accuracy
helperEstimateTrajectoryError(gTruth(indexPairs(addedFramesIdx, 1)), optimizedPoses);

Absolute RMSE for key frame trajectory (m): 0.15358

深度イメージからの緻密な再構成

調整されたカメラの姿勢をもとに、関連する深度イメージ内のすべての有効なイメージポイントを 3 次元空間に再投影して、緻密な再構成を実行できます。

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

% Ignore image points at the boundary 
offset = 40;
[X, Y] = meshgrid(offset:2:imageSize(2)-offset, offset:2:imageSize(1)-offset);

for i = 1: numel(addedFramesIdx)
    Icolor = readimage(imdsColor, addedFramesIdx(i));
    Idepth = readimage(imdsDepth, addedFramesIdx(i));

    [xyzPoints, validIndex] = helperReconstructFromRGBD([X(:), Y(:)], ...
        Idepth, intrinsics, optimizedPoses.AbsolutePose(i), depthFactor);

    colors = zeros(numel(X), 1, 'like', Icolor);
    for j = 1:numel(X)
        colors(j, 1:3) = Icolor(Y(j), X(j), :);
    end
    ptClouds(i) = pointCloud(xyzPoints, Color=colors(validIndex, :));
end

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

figure
pcshow(pointCloudsAll,VerticalAxis="y", VerticalAxisDir="down");
xlabel('X')
ylabel('Y')
zlabel('Z')

サポート関数

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

"helperImportTimestampFile" は、タイムスタンプファイルをインポートします。

function timestamp = helperImportTimestampFile(filename)

% Input handling
dataLines = [4, Inf];

%% Set up the Import Options and import the data
opts = delimitedTextImportOptions("NumVariables", 2);

% Specify range and delimiter
opts.DataLines = dataLines;
opts.Delimiter = " ";

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

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

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

% Import the data
data = readtable(filename, opts);

% Convert to output type
timestamp = table2array(data);
end

"helperAlignTimestamp" は、カラーイメージと深度イメージのタイムスタンプを揃えます。

function indexPairs = helperAlignTimestamp(timeColor, timeDepth)
idxDepth = 1;
indexPairs = zeros(numel(timeColor), 2);
for i = 1:numel(timeColor)
    for j = idxDepth : numel(timeDepth)
        if abs(timeColor(i) - timeDepth(j)) < 1e-4
            idxDepth = j;
            indexPairs(i, :) = [i, j];
            break
        elseif timeDepth(j) - timeColor(i) > 1e-3
            break
        end
    end
end
indexPairs = indexPairs(indexPairs(:,1)>0, :);
end

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

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

% Detect ORB features
Igray  = rgb2gray(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

"helperReconstructFromRGBD " は、カラーイメージと深度イメージからシーンを再構成します。

function [xyzPoints, validIndex] = helperReconstructFromRGBD(points, ...
    depthMap, intrinsics, currPose, depthFactor)

ptcloud = pcfromdepth(depthMap,depthFactor,intrinsics,ImagePoints=points, DepthRange=[0.1, 5]);

isPointValid = ~isnan(ptcloud.Location(:, 1));
xyzPoints    = ptcloud.Location(isPointValid, :);
xyzPoints    = transformPointsForward(currPose, xyzPoints);
validIndex   = find(isPointValid);
end

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

function [mapPointSet, mapPointsIdx] = ...
    helperCullRecentMapPoints(mapPointSet, mapPointsIdx, newPointIdx, rgbdMapPointsIndices)
outlierIdx = setdiff([newPointIdx; rgbdMapPointsIndices], mapPointsIdx);
if ~isempty(outlierIdx)
    mapPointSet   = removeWorldPoints(mapPointSet, outlierIdx);
    mapPointsIdx  = mapPointsIdx - arrayfun(@(x) nnz(x>outlierIdx), mapPointsIdx);
end
end

"helperEstimateTrajectoryError" は、追跡エラーを計算します。

function rmse = helperEstimateTrajectoryError(gTruth, cameraPoses)
locations       = vertcat(cameraPoses.AbsolutePose.Translation);
gLocations      = vertcat(gTruth.Translation);
scale           = median(vecnorm(gLocations, 2, 2))/ median(vecnorm(locations, 2, 2));
scaledLocations = locations * scale;

rmse = sqrt(mean( sum((scaledLocations - gLocations).^2, 2) ));
disp(['Absolute RMSE for key frame trajectory (m): ', num2str(rmse)]);
end

"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

参考文献

[1] Mur-Artal, Raul, and Juan D. Tardos. “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.

[2] Sturm, Jürgen, Nikolas Engelhard, Felix Endres, Wolfram Burgard, and Daniel Cremers. “A Benchmark for the Evaluation of RGB-D SLAM Systems.” In 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems, 573–80, 2012.