このページの内容は最新ではありません。最新版の英語を参照するには、ここをクリックします。
ステレオ 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');
地図の初期化
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
最後に、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);
ステレオ イメージからの緻密な再構成
調整されたカメラの姿勢をもとに、キー フレームに対応するステレオ イメージからの緻密な再構成を実行できます。
% 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);
コード生成
コード生成では 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.