MATLAB での Visual SLAM の実装

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

Visual SLAM アルゴリズムは、カメラの動きを推定する方法に応じて、大きく 2 つのカテゴリに分類されます。特徴ベースの間接法では、イメージの特徴点を使用して再投影誤差を最小限に抑えます。直接法では、イメージの全体的な明るさを使用して、光度測定誤差を最小限に抑えます。Computer Vision Toolbox™ のアルゴリズムは、特徴ベースの Visual SLAM ワークフローのステップを実行するための関数を提供します。また、完全なワークフローを含むオブジェクト monovslam も提供します。この概要では、地図の初期化、トラッキング、局所地図作成、ループ検出、ドリフト補正という構成で、ワークフローとそれに対応する関数を説明します。

メモ

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

Visual SLAM で使用される用語

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

キーフレーム — 位置推定とトラッキングのキューを含む、ビデオフレームのサブセット。2 つの連続するキーフレームは通常、カメラの動きによって引き起こされる大きな視覚的変化を示します。
マップ点 — キーフレームから再構成した環境のマップを表す 3 次元ワールドポイントのリスト。
Covisibility グラフ — キーフレームをノードとするグラフ。2 つのキーフレームが共通のマップ点を共有している場合、エッジにより接続されます。エッジの重みは、共有されているマップ点の数です。
認識データベース — 入力された bag of features に基づく、visual word-to-image mapping を格納するデータベース。クエリイメージと視覚的に類似したイメージをデータベースから検索して、ある場所を過去に訪れたことがあるかどうかを判断します。

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

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

マップの初期化 — 2 つのイメージフレームから 3 次元の点のマップを初期化します。2 次元特徴の対応関係に基づく三角形分割を使用して、3 次元の点と相対カメラ姿勢を計算します。
特徴の追跡 — 新しいフレームごとに、現在のフレームの特徴を最後のキーフレームの特徴とマッチングしてカメラ姿勢を推定します。
局所地図の作成 — 現在のフレームをキーフレームとして識別する場合は、新しい 3 次元マップ点を作成します。バンドル調整を使用して、カメラの姿勢と 3 次元の点を調整します。
ループの検出 — bag-of-features アプローチを使用して、キーフレームごとに現在のフレームとそれまでのすべてのキーフレームを比較することで、ループを検出します。
ドリフトの修正 — 姿勢グラフを最適化して、すべてのキーフレームのカメラ姿勢のドリフトを修正します。

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

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

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

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

imageviewset オブジェクトを使用して、オドメトリとマッピングの処理に関連するデータを管理します。オブジェクトには、一連のビューおよびビュー間のペアワイズ接続としてデータが含まれます。このオブジェクトを使用して、姿勢グラフを作成および更新することもできます。
- 各 "ビュー" は、カメラの絶対姿勢とイメージから抽出された特徴点で構成されます。ビューセット内で一意の識別子 (ビュー ID) をもつ各ビューは、姿勢グラフのノードを形成します。
- 各 "接続" には、あるビューを別のビューに結び付ける情報が格納されます。接続には、ビュー間でマッチした特徴のインデックス、接続されたビュー間の相対的な変換、および測定値の計算における不確実性が含まれます。各接続は、姿勢グラフのエッジを形成します。
- imageviewset で rigidtform3d オブジェクト入力を使用して、オドメトリエッジの絶対カメラ姿勢と相対カメラ姿勢を保存します。imageviewset で simtform3d オブジェクト入力を使用して、ループ閉じ込みエッジの相対カメラ姿勢を保存します。
worldpointset オブジェクトを使用して、カメラビュー全体における 3 次元マップ点と 2 次元イメージポイントの間の対応関係を保存します。
- worldpointset の WorldPoints プロパティは、マップ点の 3 次元位置を保存します。
- worldpointset の Correspondence プロパティは、マップ点を監視するキーフレームのビュー ID を保存します。

地図の初期化

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

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

トラッキング

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

ワークフロー	関数	説明
抽出した特徴のマッチング	`matchFeatures`	現在のフレームから抽出された特徴を、既知の 3 次元位置をもつ最後のキーフレームの特徴とマッチングします。
カメラの姿勢の推定	`estworldpose`	現在のカメラの姿勢を推定します。
マップ点の投影	`world2img`	最後のキーフレームで観測されたマップ点を現在のフレームに投影します。
特徴の対応関係の検索	`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 次元への対応関係の保存	`addCorrespondences`	`worldpointset` オブジェクトに 3 次元から 2 次元への新しい対応関係を追加します。
オドメトリ接続の更新	`updateConnection`	現在のキーフレームとそのコビジブルなフレームの間の接続を、より多くの特徴マッチで更新します。
3 次元点の代表的なビューの保存	`updateRepresentativeView`	代表的なビュー ID と対応する特徴インデックスを更新します。
3 次元点の距離範囲とビュー方向の保存	`updateLimitsAndDirection`	距離の範囲とビューの平均的な方向を更新します。
姿勢の調整	`bundleAdjustment`	現在のキーフレームの姿勢、コビジブルなキーフレームの姿勢、およびこれらのキーフレームで観測されたすべてのマップ点を調整します。パフォーマンスを向上させるには、強力に接続されたコビジブルなキーフレームのみを調整処理に含めます。強力に接続されたコビジブルなキーフレームを選択するには、関数 `connectedViews` の `minNumMatches` 引数を使用します。
外れ値の削除	`removeWorldPoints`	`worldpointset` オブジェクトから、再投影誤差の大きい外れ値のマップ点を削除します。関連付けられている 3 次元から 2 次元への対応関係は自動的に削除されます。

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

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

ループ検出

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

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

ドリフト補正

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

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

可視化

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

関数	説明
`imshow`	イメージの表示
`showMatchedFeatures`	2 つのイメージ内のマッチした特徴点の表示
`plot`	イメージビューセットのビューと接続のプロット
`plotCamera`	3 次元座標でのカメラのプロット
`pcshow`	3 次元点群のプロット
`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. "G²o: 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.