Main Content

MATLAB での Visual SLAM の実装

Visual simultaneous localization and mapping (vSLAM) とは、環境のマッピングを行うのと同時に、周囲に対するカメラの位置と向きを計算する処理を意味します。この処理では、カメラからの視覚的入力のみを使用します。Visual SLAM の用途には、拡張現実、ロボット工学、自動運転などがあります。SLAM が重要である理由や、さまざまな用途での SLAM の応用例の全般的な説明については、SLAM とはを参照してください。

Visual SLAM アルゴリズムは、カメラの動きを推定する方法に応じて、大きく 2 つのカテゴリに分類されます。特徴ベースの間接法では、イメージの特徴点を使用して再投影誤差を最小限に抑えます。直接法では、イメージの全体的な明るさを使用して、光度測定誤差を最小限に抑えます。Computer Vision Toolbox™ アルゴリズムは、特徴ベースの Visual SLAM を実行するための関数を提供します。ワークフローは、マップの初期化、追跡、局所マッピング、ループ検出、およびドリフト補正で構成されます。

メモ

この概要で説明するワークフローは、ピンホール カメラで撮影したイメージに適用されます。魚眼カメラで撮影したイメージで Visual SLAM ワークフローを使用するには、関数 undistortFisheyeImage を使用して魚眼カメラを仮想ピンホール カメラに変換します。

Visual SLAM で使用される用語

Visual SLAM の文献では、次の一般的な用語が使用されます。

  • キー フレーム — 位置推定とトラッキングのキューを含む、ビデオ フレームのサブセット。2 つの連続するキー フレームは通常、カメラの動きによって引き起こされる大きな視覚的変化を示します。

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

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

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

特徴ベース Visual SLAM の典型的なワークフロー

イメージのシーケンス上に特徴ベースの Visual SLAM パイプラインを構築するには、次の手順に従います。

  1. マップの初期化 — 2 つのイメージ フレームから 3 次元点のマップを初期化します。2 次元特徴の対応関係に基づく三角形分割を使用して、3 次元点と相対カメラ姿勢を計算します。

  2. 特徴の追跡 — 新しいフレームごとに、現在のフレームの特徴を最後のキー フレームの特徴とマッチングしてカメラ姿勢を推定します。

  3. 局所マップの作成 — 現在のフレームをキー フレームとして識別する場合は、新しい 3 次元マップ点を作成します。バンドル調整を使用して、カメラの姿勢と 3 次元点を調整します。

  4. ループの検出 — bag-of-features アプローチを使用して、キー フレームごとに現在のフレームとそれまでのすべてのキー フレームを比較することで、ループを検出します。

  5. ドリフトの修正 — 姿勢グラフを最適化して、すべてのキー フレームのカメラ姿勢のドリフトを修正します。

この図は、典型的な特徴ベースの Visual SLAM ワークフローを示しています。また、データを管理しているオブジェクトからデータが保存または取得される時点も示しています。

Flow chart diagram showing map initialization, tracking, local mapping, loop detection, and drift correction.

キー フレームとマップ データの管理

ビュー セット、点セット、および変換オブジェクトを使用して、キー フレームとマップ データを管理します。

  • imageviewset オブジェクトを使用して、オドメトリとマッピングの処理に関連するデータを管理します。オブジェクトには、一連のビューおよびビュー間のペアワイズ接続としてデータが含まれます。このオブジェクトを使用して、姿勢グラフを作成および更新することもできます。

    • "ビュー" は、カメラの絶対姿勢とイメージから抽出された特徴点で構成されます。ビュー セット内で一意の識別子 (ビュー ID) をもつ各ビューは、姿勢グラフのノードを形成します。

    • "接続" には、あるビューを別のビューに結び付ける情報が格納されます。接続には、ビュー間でマッチした特徴のインデックス、接続されたビュー間の相対的な変換、および測定値の計算における不確実性が含まれます。各接続は、姿勢グラフのエッジを形成します。

    • imageviewsetrigid3d オブジェクト入力を使用して、オドメトリ エッジの絶対カメラ姿勢と相対カメラ姿勢を保存します。imageviewsetaffine3d オブジェクト入力を使用して、ループクロージャ エッジの相対カメラ姿勢を保存します。

  • worldpointset オブジェクトを使用して、カメラ ビュー全体における 3 次元マップ点と 2 次元イメージ点の間の対応関係を保存します。

    • worldpointsetWorldPoints プロパティは、マップ点の 3 次元位置を保存します。

    • worldpointsetCorrespondence プロパティは、マップ点を監視するキー フレームのビュー ID を保存します。

マップの初期化

マッピングを初期化するには、2 つのイメージ間の特徴をマッチングし、相対的なカメラ姿勢を推定し、最初の 3 次元ワールド座標点を三角形分割しなければなりません。このワークフローでは通常、Speeded-Up Robust Features (SURF) 特徴や Oriented FAST and Rotated BRIEF (ORB) 特徴などの特徴点を使用します。マップ初期化ワークフローは、特徴の検出、抽出、マッチング、相対カメラ姿勢の推定値の検索、マッチした特徴の 3 次元位置の検索、および初期マップの調整で構成されます。最後に、結果のキー フレームとマッピングされた点をそれぞれイメージ ビュー セットとワールド座標点セットに保存します。

ワークフロー関数説明
1.検出detectSURFFeaturesSURF 特徴を検出して SURFPoints オブジェクトを返します。
detectORBFeaturesORB 特徴を検出して、ORBPoints オブジェクトを返します。
detectSIFTFeaturesSIFT 特徴を検出して、SIFTPoints オブジェクトを返します。
2.抽出extractFeaturesバイナリ イメージまたは強度イメージ内の特徴ベクトルとそれに対応する位置を抽出します。
3.マッチングmatchFeatures 2 つの特徴セット間でマッチする特徴のインデックスを取得します。
4.マッチした特徴点からの相対カメラ姿勢の推定estimateGeometricTransform2D マッチする点のペアからホモグラフィを計算します。
estimateFundamentalMatrix マッチする点のペアから基礎行列を推定します。
relativeCameraPose ホモグラフィまたは基礎行列に基づいて、rigid3d オブジェクトとして表される相対カメラ姿勢を計算します。スケールを除いた位置のみが計算できるため、2 つのカメラ間の距離は 1 に設定されます。
5.マッチした特徴点の 3 次元位置の検索triangulate 歪み補正後のイメージ点のマッチするペアの 3 次元位置を求めます。
6.最初のマップの調整bundleAdjustment 再投影誤差が最小になるように 3 次元マップ点とカメラ姿勢を調整します。
7.最初のマップとキー フレームのデータの管理addView 特徴点とそれらの絶対姿勢によって形成される 2 つのビューを imageviewset オブジェクトに追加します。
addConnection 連続するキー ビュー間の接続によって定義され、カメラ間の相対姿勢変換によって形成されたオドメトリ エッジを imageviewset オブジェクトに追加します。
addWorldPoints 最初のマップ点を worldpointset オブジェクトに追加します。
addCorrespondences キー フレームとマップ点の間での 3 次元から 2 次元への投影の対応関係を worldpointset オブジェクトに追加します。

トラッキング

トラッキング ワークフローは、新しいキー フレームを挿入するタイミングを決定するために全フレームを使用します。以下の手順と関数をトラッキング ワークフローで使用します。

ワークフロー関数説明
抽出した特徴のマッチングmatchFeatures現在のフレームから抽出された特徴を、既知の 3 次元位置をもつ最後のキー フレームの特徴とマッチングします。
カメラの姿勢の推定estimateWorldCameraPose現在のカメラの姿勢を推定します。
マップ点の投影worldToImage最後のキー フレームで観測されたマップ点を現在のフレームに投影します。
特徴の対応関係の検索matchFeaturesInRadius空間的制約内の特徴の対応関係を検索します。
カメラの姿勢の調整bundleAdjustmentMotion動きのみのバンドル調整を実行し、3 次元から 2 次元の対応関係をもとにカメラの姿勢を調整します。
局所マップ点の特定

findWorldPointsInView

findWorldPointsInTracks

ビュー内の点と、点のトラックに対応する点を特定します。
その他の特徴の対応関係の検索matchFeaturesInRadius現在のフレームで、投影された局所マップ点を含む、その他の特徴の対応関係を検索します。
カメラの姿勢の調整 bundleAdjustmentMotion動きのみのバンドル調整を実行し、3 次元から 2 次元の対応関係をもとにカメラの姿勢を調整します。
新しいキー フレームの保存

addView

addConnection

現在のフレームが新しいキー フレームであると判断した場合、そのフレーム、およびコビジブルなキー フレームとの接続を imageviewset に追加します。

トラッキング ワークフローでは、特徴のマッチングが重要です。関数 matchFeaturesInRadius を使用して、マッチした特徴点の位置の推定が利用可能な場合に、より多くの推定マッチを返します。ワークフローでは、次の 2 つの特徴マッチング関数が使用されます。

  • matchFeatures — マッチする特徴のインデックスを 2 つの入力特徴セットで返します。

  • matchFeaturesInRadius — 2 つの入力特徴セットで空間的制約を満たす、マッチする特徴のインデックスを返します。

より多くのマッチした特徴のペアを得るには、関数 matchFeatures と関数 matchFeaturesInRadius の名前と値の引数 MatchThreshhold および MaxRatio の値を増やします。外れ値のペアは、局所マッピングの手順でバンドル調整を実行した後に破棄できます。

局所マッピング

すべてのキー フレームに対して局所マッピングを実行します。次の手順に従って、新しいマップ点を作成します。

ワークフロー関数説明
キー フレームの接続 connectedViews現在のキー フレームのコビジブルなキー フレームを見つけます。
接続されたキー フレーム内でのマッチングの検索matchFeatures現在のキー フレーム内のマッチしない特徴点ごとに、関数 matchFeatures を使用して、コビジブルなキー フレームにおける他のマッチしない点とのマッチングを検索します。
新しいマッチングの位置の計算triangulateマッチした特徴点の 3 次元位置を計算します。
新しいマップ点の保存addWorldPoints新しいマップ点を worldpointset オブジェクトに追加します。
3 次元から 2 次元への対応関係の保存addCorrespondencesworldpointset オブジェクトに 3 次元から 2 次元への新しい対応関係を追加します。
オドメトリ接続の更新updateConnection現在のキー フレームとそのコビジブルなフレームの間の接続を、より多くの特徴マッチで更新します。
姿勢の調整bundleAdjustment

現在のキー フレームの姿勢、コビジブルなキー フレームの姿勢、およびこれらのキー フレームで観測されたすべてのマップ点を調整します。パフォーマンスを向上させるには、強力に接続されたコビジブルなキー フレームのみを調整処理に含めます。

強力に接続されたコビジブルなキー フレームを選択するには、関数 connectedViewsminNumMatches 引数を使用します。

外れ値の削除removeWorldPointsworldpointset オブジェクトから、再投影誤差の大きい外れ値のマップ点を削除します。関連付けられている 3 次元から 2 次元への対応関係は自動的に削除されます。

次の表では、3 次元再構成で使用されるバンドル調整関数ごとに、カメラの姿勢、マップ点、およびカメラの数を比較しています。

関数カメラの姿勢マップ点カメラの数
bundleAdjustment最適化済み最適化済み複数
bundleAdjustmentMotion最適化済み固定1
bundleAdjustmentStructure固定最適化済み複数

ループ検出

ビジュアル オドメトリを単独で使用すると、誤差の累積によるドリフトが発生する可能性があります。これらの誤差は、長距離において深刻な不正確さをもたらす可能性があります。グラフベースの SLAM を使用すると、ドリフトを修正するのに役立ちます。これを行うには、過去にアクセスしたことのある位置を見つけてループ クロージャを検出します。一般的なアプローチは、次の bag-of-features ワークフローを使用することです。

ワークフロー関数説明
bag of visual words の作成bagOfFeatures場所認識用の bag of visual words を作成します。
認識データベースの作成indexImagesビジュアル ワードをイメージにマッピングするための認識データベース invertedImageIndex を作成します。
ループ クロージャ候補の特定retrieveImages現在のキー フレームに類似しているイメージを検索します。連続するイメージが現在のフレームに類似している場合は、ループ クロージャの候補として特定します。そうでない場合は、現在のキー フレームを認識データベースに追加します。
ループ クロージャ候補の相対カメラ姿勢の計算estimateGeometricTransform3Dループ クロージャ候補ごとに、候補キー フレームと現在のキー フレームの間の相対カメラ姿勢を計算します。
ループのクローズaddConnection相対カメラ姿勢のループ クロージャ エッジを imageviewset オブジェクトに追加して、ループを閉じます。

ドリフト補正

imageviewset オブジェクトは、ビューと接続が追加されると、姿勢グラフを内部的に更新します。ドリフトを最小限に抑えるために、十分なループ クロージャが追加されたら、関数 optimizePoses を使用して姿勢グラフの最適化を実行します。関数 optimizePoses は、ビューごとに最適化された絶対姿勢変換をもつ imageviewset オブジェクトを返します。

関数 createPoseGraph を使用して、姿勢グラフを MATLAB® digraph オブジェクトとして返すことができます。MATLAB のグラフ アルゴリズムを使用して、姿勢グラフを検査、表示、または変更できます。Navigation Toolbox™ の関数 optimizePoseGraph (Navigation Toolbox) を使用して変更された姿勢グラフを最適化し、関数 updateView を使用してビュー セット内のカメラの姿勢を更新します。

可視化

Visual SLAM システムを開発するために、次の可視化関数を使用できます。

関数説明
imshowイメージの表示
showMatchedFeatures2 つのイメージ内のマッチした特徴点の表示
plotイメージ ビュー セットのビューと接続のプロット
plotCamera3 次元座標でのカメラのプロット
pcshow3 次元点群のプロット
pcplayerストリーミング 3 次元点群データの可視化

参照

[1] Hartley, Richard, and Andrew Zisserman. Multiple View Geometry in Computer Vision. 2nd ed. Cambridge: Cambridge University Press, 2003.

[2] Fraundorfer, Friedrich, and Davide Scaramuzza. “Visual Odometry: Part II: Matching, Robustness, Optimization, and Applications.” IEEE Robotics & Automation Magazine 19, no. 2 (June 2012): 78–90. https://doi.org/10.1109/MRA.2012.2182810.

[3] Mur-Artal, Raul, J. M. M. Montiel, and Juan D. Tardos. “ORB-SLAM: A Versatile and Accurate Monocular SLAM System.” IEEE Transactions on Robotics 31, no. 5 (October 2015): 1147–63. https://doi.org/10.1109/TRO.2015.2463671.

[4] Kümmerle, Rainer, Giorgio Grisetti, Hauke Strasdat, Kurt Konolige, and Wolfram Burgard. "G2o: A General Framework for Graph Optimization." In 2011 IEEE International Conference on Robotics and Automation (ICRA 2011), Shanghai, 9–13 May 2011, 3607–13. New York: Institute of Electrical and Electronics Engineers. https://doi.org//10.1109/ICRA.2011.5979949.

参考

関数

オブジェクト

関連するトピック