Main Content

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

単眼の Visual Simultaneous Localization and Mapping

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

この例では、単眼カメラのイメージ データを処理して、屋内環境マップをビルドし、カメラの軌跡を推定する方法を説明します。例では、特徴ベースの vSLAM アルゴリズムである ORB-SLAM [1] を使用します。

計算を高速化するために、Computer Vision Toolbox の基本設定ダイアログ ボックスで並列計算を有効にできます。Computer Vision Toolbox™ の基本設定を開くには、[ホーム] タブの [環境] セクションで [基本設定] をクリックします。そして [Computer Vision Toolbox] を選択します。

用語

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

  • キー フレーム: 位置推定とトラッキングのキューを含む、ビデオ フレームのサブセット。通常、2 つの連続するキー フレームにより、十分な視覚的変化が生じます。

  • マップ点: キー フレームから再構成した環境のマップを表す 3 次元点のリスト。

  • Covisibility グラフ: ノードとしてのキー フレームで構成されるグラフ。2 つのキー フレームが共通のマップ点を共有している場合、エッジにより接続されます。エッジの重みは、共有されているマップ点の数です。

  • Essential グラフ: 重みが大きい、つまり共有されているマップ点が多いエッジのみを含む、Covisibility グラフの部分グラフ。

  • 認識データベース: ある場所を過去に訪れたことがあるかどうかを認識するために使用されるデータベース。このデータベースは、入力された bag of features に基づく、ビジュアル ワードからイメージへのマッピングを格納します。クエリ イメージと視覚的に類似したイメージを探索するために使用されます。

ORB-SLAM の概要

ORB-SLAM のパイプラインには次が含まれます。

  • マップの初期化: ORB-SLAM はまず、2 つのビデオ フレームから 3 次元点のマップを初期化します。2 次元 ORB 特徴の対応関係に基づく三角形分割を使用して、3 次元点と相対カメラ姿勢が計算されます。

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

  • 局所マッピング: 現在のフレームがキー フレームとして識別された場合に、そのフレームを使用して、新しい 3 次元マップ点を作成します。この段階で、カメラ姿勢と 3 次元点を調整することにより、バンドル調整を使用して再投影誤差を最小化します。

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

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

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

baseDownloadURL = 'https://vision.in.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

imageDatastore オブジェクトを作成して、RGB イメージを検査します。

imageFolder   = [dataFolder,'rgbd_dataset_freiburg3_long_office_household/rgb/'];
imds          = imageDatastore(imageFolder);

% Inspect the first image
currFrameIdx = 1;
currI = readimage(imds, currFrameIdx);
himage = imshow(currI);

マップの初期化

ORB-SLAM パイプラインではまず、3 次元ワールド座標点をもつマップを初期化します。このステップは重要であり、SLAM の最終結果の精度に大きく影響します。2 つのイメージ間の matchFeatures を使用して、初期の ORB 特徴点の対応関係を求めます。対応関係を求めた後、2 つの幾何学的変換モデルを使用して、マップの初期化を設定します。

  • ホモグラフィ: シーンが平面の場合、特徴点の対応関係を表すには、ホモグラフィ射影変換の方が適しています。

  • 基礎行列: シーンが平面ではない場合、代わりに基礎行列を使用しなければなりません。

ホモグラフィと基礎行列はそれぞれ、estimateGeometricTransform2D およびestimateFundamentalMatrixを使用して計算できます。再投影誤差がより少なかったモデルを選択し、relativeCameraPose を使用して、2 つのフレーム間の相対的な回転と並進を推定します。深さの情報を提供しない単眼カメラから RGB イメージを取得しているため、相対並進はある程度のスケール係数までしか復元できません。

2 つのイメージの相対カメラ姿勢および一致した特徴点をもとに、関数triangulate を使用して、特徴点の 3 次元での位置を決定します。三角形分割されたマップ点は、それが両方のカメラの正面にある場合、再投影誤差が小さい場合、およびその点の 2 つのビューのパララックスが十分に大きい場合には有効です。

% 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
% Note that the images in the dataset are already undistorted, hence there
% is no need to specify the distortion coefficients.
focalLength    = [535.4, 539.2];    % in units of pixels
principalPoint = [320.1, 247.6];    % in units of pixels
imageSize      = size(currI,[1 2]);  % in units of pixels
intrinsics     = cameraIntrinsics(focalLength, principalPoint, imageSize);

% Detect and extract ORB features
scaleFactor = 1.2;
numLevels   = 8;
[preFeatures, prePoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels); 

currFrameIdx = currFrameIdx + 1;
firstI       = currI; % Preserve the first frame 

isMapInitialized  = false;

% Map initialization loop
while ~isMapInitialized && currFrameIdx < numel(imds.Files)
    currI = readimage(imds, currFrameIdx);

    [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels); 
    
    currFrameIdx = currFrameIdx + 1;
    
    % Find putative feature matches
    indexPairs = matchFeatures(preFeatures, currFeatures, 'Unique', true, ...
        'MaxRatio', 0.9, 'MatchThreshold', 40);
    
    preMatchedPoints  = prePoints(indexPairs(:,1),:);
    currMatchedPoints = currPoints(indexPairs(:,2),:);

    % If not enough matches are found, check the next frame
    minMatches = 100;
    if size(indexPairs, 1) < minMatches
        continue
    end
    
    preMatchedPoints  = prePoints(indexPairs(:,1),:);
    currMatchedPoints = currPoints(indexPairs(:,2),:);
    
    % Compute homography and evaluate reconstruction
    [tformH, scoreH, inliersIdxH] = helperComputeHomography(preMatchedPoints, currMatchedPoints);

    % Compute fundamental matrix and evaluate reconstruction
    [tformF, scoreF, inliersIdxF] = helperComputeFundamentalMatrix(preMatchedPoints, currMatchedPoints);
    
    % Select the model based on a heuristic
    ratio = scoreH/(scoreH + scoreF);
    ratioThreshold = 0.45;
    if ratio > ratioThreshold
        inlierTformIdx = inliersIdxH;
        tform          = tformH;
    else
        inlierTformIdx = inliersIdxF;
        tform          = tformF;
    end

    % Computes the camera location up to scale. Use half of the 
    % points to reduce computation
    inlierPrePoints  = preMatchedPoints(inlierTformIdx);
    inlierCurrPoints = currMatchedPoints(inlierTformIdx);
    [relOrient, relLoc, validFraction] = relativeCameraPose(tform, intrinsics, ...
        inlierPrePoints(1:2:end), inlierCurrPoints(1:2:end));
    
    % If not enough inliers are found, move to the next frame
    if validFraction < 0.9 || numel(size(relOrient))==3
        continue
    end
    
    % Triangulate two views to obtain 3-D map points
    relPose = rigid3d(relOrient, relLoc);
    minParallax = 3; % In degrees
    [isValid, xyzWorldPoints, inlierTriangulationIdx] = helperTriangulateTwoFrames(...
        rigid3d, relPose, inlierPrePoints, inlierCurrPoints, intrinsics, minParallax);
    
    if ~isValid
        continue
    end
    
    % Get the original index of features in the two key frames
    indexPairs = indexPairs(inlierTformIdx(inlierTriangulationIdx),:);
    
    isMapInitialized = true;
    
    disp(['Map initialized with frame 1 and frame ', num2str(currFrameIdx-1)])
end % End of map initialization loop
Map initialized with frame 1 and frame 44
if isMapInitialized
    close(himage.Parent.Parent); % Close the previous figure
    % Show matched features
    hfeature = showMatchedFeatures(firstI, currI, prePoints(indexPairs(:,1)), ...
        currPoints(indexPairs(:, 2)), 'Montage');
else
    error('Unable to initialize map.')
end

初期キー フレームとマップ点の格納

2 つのフレームを使用してマップを初期化した後で、imageviewset,worldpointsethelperViewDirectionAndDepth を使用して、この 2 つのキー フレームとこれらに対応するマップ点を格納できます。

  • imageviewset は、キー フレームとその属性 (ORB 記述子など)、特徴点とカメラ姿勢、およびキー フレーム間の関係 (特徴点の一致、相対カメラ姿勢など) を格納します。カメラの姿勢は rigid3d オブジェクトとして格納されます。

  • worldpointset は、マップ点の 3 次元の位置と、3 次元から 2 次元への投影の対応関係、つまりどのマップ点がキー フレームで観測され、どのキー フレームでマップ点を観測するかという関係も格納します。

  • helperViewDirectionAndDepth は、マップ点の他の属性 (平均的なビューの方向、代表的な ORB 記述子、およびマップ点を観測可能な距離範囲など) を格納します。

% 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(xyzWorldPoints, 1));

% Add the first key frame. Place the camera associated with the first 
% key frame at the origin, oriented along the Z-axis
preViewId     = 1;
vSetKeyFrames = addView(vSetKeyFrames, preViewId, rigid3d, 'Points', prePoints,...
    'Features', preFeatures.Features);

% Add the second key frame
currViewId    = 2;
vSetKeyFrames = addView(vSetKeyFrames, currViewId, relPose, 'Points', currPoints,...
    'Features', currFeatures.Features);

% Add connection between the first and the second key frame
vSetKeyFrames = addConnection(vSetKeyFrames, preViewId, currViewId, relPose, 'Matches', indexPairs);

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

% Add observations of the map points
preLocations   = prePoints.Location;
currLocations  = currPoints.Location;
preScales      = prePoints.Scale;
currScales     = currPoints.Scale;

% Add image points corresponding to the map points in the first key frame
mapPointSet   = addCorrespondences(mapPointSet, preViewId, newPointIdx, indexPairs(:,1));

% Add image points corresponding to the map points in the second key frame
mapPointSet   = addCorrespondences(mapPointSet, currViewId, newPointIdx, indexPairs(:,2));

初期再構成の調整と可視化

bundleAdjustment を使用して、初期再構成を調整します。これにより、カメラ姿勢とワールド座標点の両方を最適化し、全体の再投影誤差を最小化します。調整後、マップ点の 3 次元での位置、ビューの方向、深さの範囲などの属性が更新されます。helperVisualizeMotionAndStructure を使用して、マップ点とカメラの位置を可視化できます。

% Run full bundle adjustment on the first two key frames
tracks       = findTracks(vSetKeyFrames);
cameraPoses  = poses(vSetKeyFrames);

[refinedPoints, refinedAbsPoses] = bundleAdjustment(xyzWorldPoints, tracks, ...
    cameraPoses, intrinsics, 'FixedViewIDs', 1, ...
    'PointsUndistorted', true, 'AbsoluteTolerance', 1e-7,...
    'RelativeTolerance', 1e-15, 'MaxIteration', 50);

% Scale the map and the camera pose using the median depth of map points
medianDepth   = median(vecnorm(refinedPoints.'));
refinedPoints = refinedPoints / medianDepth;

refinedAbsPoses.AbsolutePose(currViewId).Translation = ...
    refinedAbsPoses.AbsolutePose(currViewId).Translation / medianDepth;
relPose.Translation = relPose.Translation/medianDepth;

% Update key frames with the refined poses
vSetKeyFrames = updateView(vSetKeyFrames, refinedAbsPoses);
vSetKeyFrames = updateConnection(vSetKeyFrames, preViewId, currViewId, relPose);

% Update map points with the refined positions
mapPointSet   = updateWorldPoints(mapPointSet, newPointIdx, refinedPoints);

% Update view direction and depth 
directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, newPointIdx, true);

% Visualize matched features in the current frame
close(hfeature.Parent.Parent);
featurePlot   = helperVisualizeMatchedFeatures(currI, currPoints(indexPairs(:,2)));

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

% Show legend
showLegend(mapPlot);

トラッキング

トラッキング処理は全フレームを使用して行われ、新しいキー フレームを挿入するタイミングを決定します。この例を簡略化するため、ループ クロージャが見つかったところでトラッキング処理を終了します。

% ViewId of the current key frame
currKeyFrameId    = currViewId;

% ViewId of the last key frame
lastKeyFrameId    = currViewId;

% ViewId of the reference key frame that has the most co-visible 
% map points with the current key frame
refKeyFrameId     = currViewId;

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

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

isLoopClosed      = false;

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

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

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

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

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

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

  6. トラッキングの最後のステップでは、現在のフレームが新しいキー フレームかどうかを決定します。現在のフレームがキー フレームの場合、局所マッピング処理に進みます。そうでない場合、次のフレームのトラッキングを開始します。

% Main loop
while ~isLoopClosed && currFrameIdx < numel(imds.Files)  
    currI = readimage(imds, currFrameIdx);

    [currFeatures, currPoints] = helperDetectAndExtractFeatures(currI, scaleFactor, numLevels);

    % Track the last key frame
    % mapPointsIdx:   Indices of the map points observed in the current frame
    % featureIdx:     Indices of the corresponding feature points in the 
    %                 current frame
    [currPose, mapPointsIdx, featureIdx] = helperTrackLastKeyFrame(mapPointSet, ...
        vSetKeyFrames.Views, currFeatures, currPoints, lastKeyFrameId, intrinsics, scaleFactor);
    
    % Track the local map
    % refKeyFrameId:      ViewId of the reference key frame that has the most 
    %                     co-visible map points with the current frame
    % localKeyFrameIds:   ViewId of the connected key frames of the current frame
    [refKeyFrameId, localKeyFrameIds, currPose, mapPointsIdx, featureIdx] = ...
        helperTrackLocalMap(mapPointSet, directionAndDepth, vSetKeyFrames, mapPointsIdx, ...
        featureIdx, currPose, currFeatures, currPoints, intrinsics, scaleFactor, numLevels);
    
    % 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 80 map points
    % 2. The map points tracked by the current frame are fewer than 90% of 
    %    points tracked by the reference key frame
    isKeyFrame = helperIsKeyFrame(mapPointSet, refKeyFrameId, lastKeyFrameIdx, ...
        currFrameIdx, mapPointsIdx);
    
    % Visualize matched features
    updatePlot(featurePlot, currI, currPoints(featureIdx));
    
    if ~isKeyFrame
        currFrameIdx = currFrameIdx + 1;
        continue
    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, mapPointsIdx, featureIdx, localKeyFrameIds);
    
    % Remove outlier map points that are observed in fewer than 3 key frames
    [mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx);
    
    % Create new map points by triangulation
    minNumMatches = 20;
    minParallax = 3;
    [mapPointSet, vSetKeyFrames, newPointIdx] = helperCreateNewMapPoints(mapPointSet, vSetKeyFrames, ...
        currKeyFrameId, intrinsics, scaleFactor, minNumMatches, minParallax);
    
    % Update view direction and depth
    directionAndDepth = update(directionAndDepth, mapPointSet, vSetKeyFrames.Views, [mapPointsIdx; newPointIdx], true);
    
    % Local bundle adjustment
    [mapPointSet, directionAndDepth, vSetKeyFrames, newPointIdx] = helperLocalBundleAdjustment(mapPointSet, directionAndDepth, vSetKeyFrames, ...
        currKeyFrameId, intrinsics, newPointIdx); 
    
    % Visualize 3D world points and camera trajectory
    updatePlot(mapPlot, vSetKeyFrames, mapPointSet);

ループ クロージャ

ループ クロージャのステップでは、局所マッピング処理で処理された現在のキー フレームを受け取り、ループを検出して閉じようと試みます。ループ検出は bags-of-words アプローチを使用して行われます。次の呼び出しを行うことにより、データセットに含まれる大量のイメージ セットから抽出された SURF 記述子を使用して、bagOfFeatures オブジェクトで表されるビジュアル ボキャブラリがオフラインで作成されます。

bag = bagOfFeatures(imds,'CustomExtractor', @helperSURFFeatureExtractorFunction);

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

ループ クロージャ処理により、データベースがインクリメンタルにビルドされます。このデータベースは invertedImageIndex オブジェクトで表され、bag of SURF features によるビジュアル ワードからイメージへのマッピングを格納します。evaluateImageRetrieval を使用し、データベース内のイメージをクエリすることで、現在のキー フレームと視覚的に似ているループ候補が特定されます。候補キー フレームが最後のキー フレームと接続されておらず、隣接するキー フレームの 3 つがループ候補であれば、その候補キー フレームは有効です。

有効なループ候補が見つかると、トラッキング処理で使用したのと同じ方法で、ループ候補フレームと現在のキー フレームの間の相対姿勢を計算します。そして、ループの接続を相対姿勢と共に追加し、mapPointSet vSetKeyFrames を更新します。

    % Initialize the loop closure database
    if currKeyFrameId == 3
        % Load the bag of features data created offline
        bofData         = load('bagOfFeaturesData.mat');
        loopDatabase    = invertedImageIndex(bofData.bof);
        loopCandidates  = [1; 2];
        
    % Check loop closure after some key frames have been created    
    elseif currKeyFrameId > 20
        
        % Minimum number of feature matches of loop edges
        loopEdgeNumMatches = 50;
        
        % Detect possible loop closure key frame candidates
        [isDetected, validLoopCandidates] = helperCheckLoopClosure(vSetKeyFrames, currKeyFrameId, ...
            loopDatabase, currI, loopCandidates, loopEdgeNumMatches);
        
        if isDetected 
            % Add loop closure connections
            [isLoopClosed, mapPointSet, vSetKeyFrames] = helperAddLoopConnections(...
                mapPointSet, vSetKeyFrames, validLoopCandidates, currKeyFrameId, ...
                currFeatures, currPoints, intrinsics, scaleFactor, loopEdgeNumMatches);
        end
    end
    
    % If no loop closure is detected, add the image into the database
    if ~isLoopClosed
        currds = imageDatastore(imds.Files{currFrameIdx});
        addImages(loopDatabase, currds, 'Verbose', false);
        loopCandidates= [loopCandidates; currKeyFrameId]; %#ok<AGROW>
    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: 1 and 123

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

% Optimize the poses
vSetKeyFramesOptim = optimizePoses(vSetKeyFrames, minNumMatches, 'Tolerance', 1e-16, 'Verbose', true);
Iteration 1, residual error 0.047601
Iteration 2, residual error 0.046330
Iteration 3, residual error 0.046329
Iteration 4, residual error 0.046329
Iteration 5, residual error 0.046329
Iteration 6, residual error 0.046329
Iteration 7, residual error 0.046329
Iteration 8, residual error 0.046329
Iteration 9, residual error 0.046329
Iteration 10, residual error 0.046329
Solver stopped because change in function value was less than specified function tolerance.
% 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);

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

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

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

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

% Show legend
showLegend(mapPlot);

% Evaluate tracking accuracy
helperEstimateTrajectoryError(gTruth(addedFramesIdx), optimizedPoses);
Absolute RMSE for key frame trajectory (m): 0.17784

ORB-SLAM を使用して、屋内環境マップをビルドし、カメラの軌跡を推定する方法の概要は、以上になります。

サポート関数

短い補助関数は以下のとおりです。大きな関数は別ファイルにあります。

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

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

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

helperCreateNewMapPoints は、三角形分割により、新しいマップ点を作成します。

helperFindProjectedPointsInImage は、投影されたワールド座標点がイメージ内にあるかどうかをチェックします。

helperHammingDistance は、2 つのバイナリ特徴ベクトル グループ間のハミング距離を計算します。

helperLocalBundleAdjustment は、現在のキー フレームの姿勢と周囲のシーンのマップを調整します。

helperMatchFeaturesInRadius は、半径内の特徴のマッチングを行います。

helperSelectStrongConnections は、指定した数よりも多く一致をもつ強力な接続を選択します。

helperSURFFeatureExtractorFunction は、bagOfFeatures で使用される SURF 機能抽出器を実装します。

helperTrackLastKeyFrame は、最後のキー フレームのトラッキングにより、現在のカメラ姿勢を推定します。

helperTrackLocalMap は、局所マップのトラッキングにより、現在のカメラ姿勢を調整します。

helperViewDirectionAndDepth は、マップ点の平均的なビューの方向と予測される深度を保存します。

helperVisualizeMatchedFeatures は、フレーム内の一致した特徴を表示します。

helperVisualizeMotionAndStructure は、マップ点とカメラの軌跡を表示します。

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

function [features, validPoints] = helperDetectAndExtractFeatures(Irgb, ...
    scaleFactor, numLevels, varargin)

numPoints   = 1000;

% In this example, the images are already undistorted. In a general
% workflow, uncomment the following code to undistort the images.
%
% if nargin > 3
%     intrinsics = varargin{1};
% end
% Irgb  = undistortImage(Irgb, intrinsics);

% 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

helperHomographyScore は、ホモグラフィを計算し、再構成を評価します。

function [H, score, inliersIndex] = helperComputeHomography(matchedPoints1, matchedPoints2)

[H, inliersLogicalIndex] = estimateGeometricTransform2D( ...
    matchedPoints1, matchedPoints2, 'projective', ...
    'MaxNumTrials', 1e3, 'MaxDistance', 4, 'Confidence', 90);

inlierPoints1 = matchedPoints1(inliersLogicalIndex);
inlierPoints2 = matchedPoints2(inliersLogicalIndex);

inliersIndex  = find(inliersLogicalIndex);

locations1 = inlierPoints1.Location;
locations2 = inlierPoints2.Location;
xy1In2     = transformPointsForward(H, locations1);
xy2In1     = transformPointsInverse(H, locations2);
error1in2  = sum((locations2 - xy1In2).^2, 2);
error2in1  = sum((locations1 - xy2In1).^2, 2);

outlierThreshold = 6;

score = sum(max(outlierThreshold-error1in2, 0)) + ...
    sum(max(outlierThreshold-error2in1, 0));
end

helperFundamentalMatrixScore は、基礎行列を計算し、再構成を評価します。

function [F, score, inliersIndex] = helperComputeFundamentalMatrix(matchedPoints1, matchedPoints2)

[F, inliersLogicalIndex]   = estimateFundamentalMatrix( ...
    matchedPoints1, matchedPoints2, 'Method','RANSAC',...
    'NumTrials', 1e3, 'DistanceThreshold', 0.01);

inlierPoints1 = matchedPoints1(inliersLogicalIndex);
inlierPoints2 = matchedPoints2(inliersLogicalIndex);

inliersIndex  = find(inliersLogicalIndex);

locations1    = inlierPoints1.Location;
locations2    = inlierPoints2.Location;

% Distance from points to epipolar line
lineIn1   = epipolarLine(F', locations2);
error2in1 = (sum([locations1, ones(size(locations1, 1),1)].* lineIn1, 2)).^2 ...
    ./ sum(lineIn1(:,1:2).^2, 2);
lineIn2   = epipolarLine(F, locations1);
error1in2 = (sum([locations2, ones(size(locations2, 1),1)].* lineIn2, 2)).^2 ...
    ./ sum(lineIn2(:,1:2).^2, 2);

outlierThreshold = 4;

score = sum(max(outlierThreshold-error1in2, 0)) + ...
    sum(max(outlierThreshold-error2in1, 0));

end

helperTriangulateTwoFrames は、2 つのフレームを三角形分割して、マップを初期化します。

function [isValid, xyzPoints, inlierIdx] = helperTriangulateTwoFrames(...
    pose1, pose2, matchedPoints1, matchedPoints2, intrinsics, minParallax)

[R1, t1]   = cameraPoseToExtrinsics(pose1.Rotation, pose1.Translation);
camMatrix1 = cameraMatrix(intrinsics, R1, t1);

[R2, t2]   = cameraPoseToExtrinsics(pose2.Rotation, pose2.Translation);
camMatrix2 = cameraMatrix(intrinsics, R2, t2);

[xyzPoints, reprojectionErrors, isInFront] = triangulate(matchedPoints1, ...
    matchedPoints2, camMatrix1, camMatrix2);

% Filter points by view direction and reprojection error
minReprojError = 1;
inlierIdx  = isInFront & reprojectionErrors < minReprojError;
xyzPoints  = xyzPoints(inlierIdx ,:);

% A good two-view with significant parallax
ray1       = xyzPoints - pose1.Translation;
ray2       = xyzPoints - pose2.Translation;
cosAngle   = sum(ray1 .* ray2, 2) ./ (vecnorm(ray1, 2, 2) .* vecnorm(ray2, 2, 2));

% Check parallax
isValid = all(cosAngle < cosd(minParallax) & cosAngle>0);
end

helperIsKeyFrame は、あるフレームがキー フレームかどうかをチェックします。

function isKeyFrame = helperIsKeyFrame(mapPoints, ...
    refKeyFrameId, lastKeyFrameIndex, currFrameIndex, mapPointsIndices)

numPointsRefKeyFrame = numel(findWorldPointsInView(mapPoints, refKeyFrameId));

% More than 20 frames have passed from last key frame insertion
tooManyNonKeyFrames = currFrameIndex >= lastKeyFrameIndex + 20;

% Track less than 90 map points
tooFewMapPoints     = numel(mapPointsIndices) < 90;

% Tracked map points are fewer than 90% of points tracked by
% the reference key frame
tooFewTrackedPoints = numel(mapPointsIndices) < 0.9 * numPointsRefKeyFrame;

isKeyFrame = (tooManyNonKeyFrames || tooFewMapPoints) && tooFewTrackedPoints;
end

helperCullRecentMapPoints は、最近追加されたマップ点を選抜します。

function [mapPointSet, directionAndDepth, mapPointsIdx] = helperCullRecentMapPoints(mapPointSet, directionAndDepth, mapPointsIdx, newPointIdx)
outlierIdx    = setdiff(newPointIdx, mapPointsIdx);
if ~isempty(outlierIdx)
    mapPointSet   = removeWorldPoints(mapPointSet, outlierIdx);
    directionAndDepth = remove(directionAndDepth, 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, directionAndDepth] = helperUpdateGlobalMap(...
    mapPointSet, directionAndDepth, vSetKeyFrames, vSetKeyFramesOptim)
%helperUpdateGlobalMap update map points after pose graph optimization
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

参考文献

[1] Mur-Artal, Raul, Jose Maria Martinez Montiel, and Juan D. Tardos. "ORB-SLAM: a versatile and accurate monocular SLAM system." IEEE Transactions on Robotics 31, no. 5, pp 1147-116, 2015.

[2] Sturm, Jürgen, Nikolas Engelhard, Felix Endres, Wolfram Burgard, and Daniel Cremers. "A benchmark for the evaluation of RGB-D SLAM systems". In Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 573-580, 2012.