ORB-SLAMの手法解説

ORB-SLAMの手法解説
ORB-SLAM a Versatile and Accurate Monocular SLAM System 東京大学相澤山崎研究室 Qoncept Internship 2017/12/24 B4 金子真也
1 What is this? • 現在, 株式会社Qoncept(http://qoncept.jp/)でのInternshipに参加しており, その成果の一つとして作成した資料です. • 近年SLAMとして最も一般的に用いられている, ORB-SLAM (http:/ /webdiis.unizar.es/~raulmur/orbslam/)の手法部分を主にまとめました. • ORB-SLAMに関する日本語の文献は現在かなり少ないので, 参考になると思い公開しました. • 何か質問やコメント等あれば気軽にお願いします.
2 概要 • 大小関係なく室内, 屋外環境など様々な環境下でリアルタイムに動作する特徴点ベースの単眼SLAM • 重度の動きの乱れに対して頑強であり, Loop Closing, Relocalizati on, full automatic initialization等で優れた性能を持つ • PTAMに次ぐ特徴点ベースのSLAM手法で, 2014年に発表された論文・実装だが未だにSOTAの精度を持ち, SLAMの性能比較の際には必ず挙げられる (アカデミックな現場でSLAMを0から作るのは中々大変だから？) • 特徴点ベースのSLAMとしてはソースコード的に最も使いやすく, コミュニティ的にもこのSLAMが一般的に用いられる.
3 Contribution • Tracking, Mapping, Relocalization, Loop closing等, 全てのタスクで同じ特徴量(ORB特徴量)を使用できる – より効率的, 単純, 信頼度の高いシステム – GPUなしでリアルタイム性能が可能. 視点照明の変化に不変 • 大規模な環境でもリアル操作が可能 – Covisibilityグラフを用い, 可視領域を絞りながらの最適化 • Essentialグラフを最適化することでのリアルタイムLoop closing • リアルタイムでのRelocalization • 自動かつロバストな新しい初期化手法（平面と非平面シーン） • Map pointとKeyframeの内冗長なものを厳しく間引く作業 – survival of the fittest approach
4 関連研究１ • Place Recognition – 場所認識には画像同士の比較手法が大規模な環境下で最も優れているらしい(Williams+(2009)) – 我々はORB+(DBoW2上での)BoW場所認識器を利用 • Map initialization – Engel+は画素の逆深度を分散の大きいランダム値で初期化し,実際の位置に収束させていた(Engel+, LSD-SLAM(2014)) – 2視点からの初期化方法として一般には以下が存在 • Homography • 5点アルゴリズムからの基本行列(平面と一般シーンに対して) • 8点アルゴリズムによる基礎行列(非平面シーンに対して) – 我々はHomographyと基礎行列との間のモデル選択を自動的に行うアルゴリズムを提案 (Torr+(1999)の改良版で, 縮退のケースを考慮に入れている)
5 関連研究２ • 単眼SLAM – 初期はfiltering手法によって解決 • 似ている連続frameの計算が無駄+線形化エラーの累積 – keyframe手法はBAの最適化で, より正確(Strasdat+(2012)) • PTAMが最も有名(relocarizationがザコ) • Strasdat+はLoop closingで7DoFのPose graph最適化を行うSLAMを提案(2010) • Strasdat+はlocal+globalの2つで最適化を行う手法を提案 (2011)(Covisibilityの考えを利用) • Pirkerは実装がないが, CD-SLAMを提案(2011) • Song+はTrackingにのみORB特徴量を利用(2013) • Lim+はこのSLAMの暫定版を提案(2014) • Engel+はdirectな手法としてLSD-SLAMを提案(2014) • Foster+はSVOを提案(中間的な手法)(2014)
6 関連研究３ • 単眼SLAM – KFの選択方法 • 全ての点とフレームに対するBAを行うことは一般的に不可能 • Strasdat+は「重複しないKFだけを保持しながら多くの点を保持することがコスト効率が最も高い」としている • PTAMはKFの挿入がかなり慎重なのでrelocalizeが困難 • 以上から「できるだけKFを早く挿入し, 後で冗長なものを削除するのが最良」であり, 今回はこれを利用することで高いロバスト性能を獲得することに成功
• Tracking, Local mapping, Loop closingを３スレッドで並列実行 • 同時に推定した点のMapを作成 • Relocalization, Loop Closingのために場所認識用の辞書作成 7 システムの大まかな概要
• Tracking, Local mapping, Loop closingを３スレッドで並列実行 • 同時に推定した点のMapを作成 • Relocalization, Loop Closingのために場所認識用の辞書作成 8 システムの大まかな概要スレッド１全てのFrameでカメラの位置を推定し, 新しいKFを挿入するタイミングを決定
• Tracking, Local mapping, Loop closingを３スレッドで並列実行 • 同時に推定した点のMapを作成 • Relocalization, Loop Closingのために場所認識用の辞書作成 9 システムの大まかな概要スレッド２新しいKFを処理し, local BAでカメラ周りのマップの最適化
• Tracking, Local mapping, Loop closingを３スレッドで並列実行 • 同時に推定した点のMapを作成 • Relocalization, Loop Closingのために場所認識用の辞書作成 10 システムの大まかな概要スレッド３新しいKF毎にループの検索を行い, ループを検出した場合にglobal BAで最適化
• Tracking, Local mapping, Loop closingを３スレッドで並列実行 • 同時に推定した点のMapを作成 • Relocalization, Loop Closingのために場所認識用の辞書作成 11 システムの大まかな概要 Mapの作成 Trackingの結果を得られた点やKFの情報を格納
• Tracking, Local mapping, Loop closingを３スレッドで並列実行 • 同時に推定した点のMapを作成 • Relocalization, Loop Closingのために場所認識用の辞書作成 12 システムの大まかな概要場所認識用データベース Frameに映る場所が同じかどうかを判定するためにBoWを利用する
• MapPoint 𝑝𝑖の定義 – ワールド座標系の点の位置𝐗 𝑤,𝑖 – 視線方向単位ベクトル𝐧𝑖 : 複数のKF間での平均 – ORB特徴記述子𝐃𝑖 : 特徴点が載っているKF間でハミング距離を小さくするFrameのORB特徴量を代表定な記述子とする – 点が観測できる距離の範囲𝑑 𝑚𝑎𝑥, 𝑑 𝑚𝑖𝑛 : ORBのスケール不変性の限界に基づく 13 マップの定義 𝐗 𝑤,𝑖 𝐧𝑖 KF1 KF2 KF3 𝑑 𝑚𝑖𝑛 𝑑 𝑚𝑎𝑥 world座標
• MapPoint 𝑝𝑖の定義 – ワールド座標系の点の位置𝐗 𝑤,𝑖 – 視線方向単位ベクトル𝐧𝑖 : 複数のKF間での平均 – ORB特徴記述子𝐃𝑖 : 特徴点が載っているKF間でハミング距離を小さくするFrameのORB特徴量を代表定な記述子とする – 点が観測できる距離の範囲𝑑 𝑚𝑎𝑥, 𝑑 𝑚𝑖𝑛 : ORBのスケール不変性の限界に基づく 14 マップの定義 𝐗 𝑤,𝑖 𝐧𝑖 KF1 KF2 KF3 1001 1010 1011 world座標 1 1 2 𝑑 𝑚𝑖𝑛 𝑑 𝑚𝑎𝑥
• MapPoint 𝑝𝑖の定義 – ワールド座標系の点の位置𝐗 𝑤,𝑖 – 視線方向単位ベクトル𝐧𝑖 : 複数のKF間での平均 – ORB特徴記述子𝐃𝑖 : 特徴点が載っているKF間でハミング距離を小さくするFrameのORB特徴量を代表定な記述子とする – 点が観測できる距離の範囲𝑑 𝑚𝑎𝑥, 𝑑 𝑚𝑖𝑛 : ORBのスケール不変性の限界に基づく 15 マップの定義 𝐗 𝑤,𝑖 𝐧𝑖 KF1 KF2 KF3 1001 1010 1011 𝐃𝑖 1011 world座標 𝑑 𝑚𝑖𝑛 𝑑 𝑚𝑎𝑥
16 マップの定義 • KeyFrame 𝐾𝑖の定義 – ワールド座標系でのカメラ姿勢𝐓𝑖𝑤 – カメラの内部パラメータ(焦点距離, 中心点) – 𝐾𝑖上で抽出された全てのORB特徴点 : Map pointと結びついてなくてもよい + 歪み補正できればやる • これらは寛容に作成され, 除去メカニズムにより, 重複するKFや誤った対応関係の点, 追跡不能な点を検出し厳密に除去 – 柔軟な地図展開が可能になり困難な環境でも対応可能
17 マップの定義 • CovisibilityグラフとEssentialグラフの定義 – Covisibilityグラフ : nodeがKFであり, edge間の重み𝜃は2KF間で共有するMap pointの数(15個以上) KFの位置現在のカメラの位置現在のLocal MapPoints (見えている範囲) MapPoints Covisibilityグラフ
18 マップの定義 – Essentialグラフ : 高密度なCovisibilityグラフに含まれる, 全てのnodeを繋ぐ全域木をKFの挿入の度に構築 (全域木で得られたedgeで, 𝜃 𝑚𝑖𝑛より大きい重みのedgeを採用) Essentialグラフある時点での全域木 (Spanning Tree) Loop Closure
19 BoW場所認識器 • DBoW2[1](Garez+2012)をベースとしたBoWによる場所認識 – Matching, Loop closingとRelocalizationに用いる • 大量の画像群から抽出されたORB記述子を使用しvocabを作成 – 画像が十分に一般的であれば同じVocabで異なる環境に対して良好な結果が得られる • Vocabへの照会の効率化のために, 逆インデックスを格納する2つ目のデータベースを用意 – KFが除去されると同時にこのデータベースが更新される • KF同士はどれも似ているため, Covisibilityグラフでの接続状況によるグループ化を行いスコアに加える – スコアが最高スコアの75%を超えるKFを返す • ORB特徴量セット間の対応を計算する時に, Vocab treeを参照して検索対象を絞る [1] https://github.com/dorian3d/DBoW2
20 マップ初期化 • 2つのframe間の相対的なポーズを計算してMapPointsの初期セットの三角測量を行う – 平面を想定したHomographyと非平面を仮定した基礎行列の2 つの幾何学モデルを並列に計算し, 適切な方を自動で選択する • アルゴリズムとしては以下 1. 現在のFrame 𝐹𝑐と参照Frame 𝐹𝑟内のORB特徴量の対応𝐱 𝑐 ↔ 𝐱 𝑟 を検索, 見つからない場合には𝐹𝑟をリセットする 2. 2つのモデルで並列計算それぞれDLTアルゴリズムと8点アルゴリズムを使用し, 反復最適化を行う(回数は4回と8回) 各反復で以下のスコア𝑆 𝑀を計算して最適化
21 マップ初期化 • 𝑑 𝑟𝑐 2 , 𝑑 𝑐𝑟 2 はフレーム間での対称的な変換誤差 • 𝑇 𝑀は1ピクセルの標準偏差を𝑇 𝐻 = 5.99, 𝑇𝐹 = 3.84と仮定した時の𝜒2 検定の95%閾値 • Γ = 𝑇 𝑀として定義され, inlier領域内の同じ𝑑に等しいスコア • モデルが見つからない場合, 手順1からやり直す 3. モデル選択：場面が平面か非平面か視差が十分でないかを判定するのに以下の式を用いる(𝑅 𝐻 > 0.45の場合, Homography) 4. モデルが選択されたときに生じる仮説群の内正しいものを選択 • Homographyの場合, 8つの仮説(Faugeras+(1988)) • 基礎行列の場合, 基本行列に変換しSVDで4つの仮説[2] 全ての解を三角測量し, 最も再投影誤差が小さい仮説を選択 [2] Multiple View Geometry
22 マップ初期化 5. Bundle Adjustment 対応関係のKeypoint 𝐱 𝑖,𝑗 ∈ ℝ2に対する再投影誤差の最小化で, MapPointの三次元位置𝐗 𝑤,𝑗 ∈ ℝ3とKFのカメラ姿勢𝐓𝑖,𝑤 ∈ 𝑆𝐸(3) を最適化する誤差項はコスト関数は – 𝜌ℎはHuber robust関数 – 𝛀𝑖,𝑗 = 𝜎𝑖𝑗 2 𝐈2×2 はscaleに関する共分散行列初期化では点の数が少ないためfull BAを行う
23 マップ初期化 • 他の手法との比較 – PTAM, LSD-SLAMは平面として推定し正確に初期化できていないが, ORB-SLAMでは基礎行列を用いてうまく推定できている ORB-SLAM PTAM LSD-SLAM
24 Tracking 1. ORB特徴点の抽出 – Scale factor 1.2で8段階のscaleでFAST corner検出 – 512 × 384~752 × 480画素の画像には1000 corner, 1241 × 376 (KITTI dataset)では2000 cornerが実験的に適切だった – Homogeneousな分布のために, 画像を各scale毎にgridに分けて最低5個ずつ検出, コーナーが見つからない場合は検出器の閾値を調整 – ORB特徴量は検出されたFAST cornerで計算
25 Tracking 2. 前のFrameとの比較による初期姿勢推定 – 最後のFrameでTrackingに成功した場合, 定速度モーションモデルを仮定してカメラ姿勢を推定 – 最後のFrameで観測されたMapPointsを, 推定された姿勢に基づいて現在のFrameに投影し, その付近で対応点の検索 – 十分な対応点が見つからない場合, 最後のFrameでの位置の周りのMapPointsをより広範囲で検索 – 姿勢は対応点に基づいてmotion-only BAで最適化される(点を固定した状態でカメラ姿勢のみを最適化)
26 Tracking 3. Global Relocalizationによる初期姿勢推定 – Trackingに失敗した場合, FrameはBoWに変換され, global relocalizationのためのFrameの候補を探索 – ORB特徴点の対応関係を比較し, RANSACによる反復を行い, PnPアルゴリズム(Lepetit+(2012))によりカメラ姿勢を求める – 十分な数のinlierを得た状態でカメラ姿勢を推定できたら候補 Frameからより多くの対応点を探索し最適化 – 最終的にカメラ姿勢が得られたらTracking手順を再開する
27 Tracking 4. Local Mapの探索 – より多くの対応点の探索+カメラ姿勢の最適化 – Local MapのMapPointを投影して探索. その範囲が以下. • 現在のFrameとMapPointを共有するKFのセット𝒦1 • Covisibilityグラフで𝒦1に隣接するKFのセット𝒦2 – 上のセットのMapPointを以下の手順で現在のFrame内で探索 1. 現在のFrameへの投影点𝐱を計算 2. 現在の視線𝐯とMapPointの平均姿勢方向𝐧との角度を計算. 𝐯 ∙ 𝐧 < cos(60°)の場合は棄却する 3. カメラとMapPointの間の距離𝑑を計算し, 𝑑 ∉ [𝑑 𝑚𝑖𝑛, 𝑑 𝑚𝑎𝑥]の場合は棄却 4. Frame内のスケールを𝑑/𝑑 𝑚𝑖𝑛で計算 5. MapPointのORB記述子𝑫をFrame内の対応付けされていない点を探索し関連付け
28 Tracking 5. 新しくKFを挿入するかの決定 – 後で厳密に比較し削除するのでできるだけ手早くKFを挿入する – 以下の条件が全て満たされている場合にKFを挿入 1. 最後にGlobal Relocalizationを行って20 frameが経過 2. Local Mappingが止まっている又は, 最後のKF挿入から20 frame以上経過 3. 現在のFrameが少なくとも50点以上のMapPointを追跡 4. セット𝒦1の中で最も多くのMapPointを共有するFrameを 𝐾𝑟𝑒𝑓とし,現在のFrameがその𝐾𝑟𝑒𝑓 のMapPointの90%以下の点しか追跡できていない(重複しないようにするため)
29 Local Mapping 1. KFの挿入 – 新しくKFが挿入された場合に行う – 新しいnode 𝐾𝑖の追加+他のKFと共有しているMapPointの数によるedgeを追加することでCovisibilityグラフの更新 – 𝐾𝑖と一番MapPointを共有するKFを, 全域木(Spanning Tree)として繋ぎ, 更新を行う – KFのBoWを計算する
30 Local Mapping 2. 最近追加されたMapPointの削除 – MapPointがMap上に保持されるかどうか？のテストを行う • 見えていると期待されたFrame群の中で,全体の25%以上の Frameで観測されている • MapPointが作成されてKFが1枚以上追加された場合に, 少なくとも3枚上のKFで観測されていなければならない – 以上が満たされない場合, MapPointを除去する – 除去されるタイミングとしてはKFが除去される場合, Local BA でoutlierを除去するタイミングで行われる
31 Local Mapping 3. MapPointの新規追加 – Covisibilityグラフで接続されたKFセット𝒦𝑐でORBを三角測量することで新しいMapPointを追加 – Frame 𝐾𝑖の中で対応付けられていないORB特徴点それぞれに対して, 他のFrameとの対応を探索 – BoWを元に作られたVocab Treeで対応候補を絞って探索し(Bo W場所認識器), エピポーラ制約を満たさない対応を破棄 – 各ペアに対してカメラの深さ, 視差, 再投影誤差, スケールがチェックされる – 最初は2つのKFから探索されるが, 他のKFとの対応も確認するため, Tracking(4)の手順を用いて探索される
32 Local Mapping 4. Local Bundle Adjustment – 現在処理されているKF 𝑲𝒊とCovisibilityグラフで接続されているセット𝓚 𝒄, さらにその上のMapPointを最適化 – 今回は上にあげられていないKFを固定して利用する – 異常値として観測された特徴点は除去(Local Mapping(2)) – 細かいアルゴリズムはマップ初期化(5)を参照
33 Local Mapping 5. LocalなKFの削除 – 簡素な復元結果の表現を保持するために, 重複するKFを削除 • BAの複雑さを抑える • 同じ環境で長時間の操作を可能にする – MapPointの90%が他の少なくとも3枚のKFで, 同じ又はより細かいスケールで表示されているようなセット𝒦𝑐の中のKFを削除 • スケールによってKFを正確に測定して維持することを保証 • Tan+(2013)らの研究に触発されている
34 Loop Closing 1. Loop候補の推定 – Local Mappingで処理された最後のKF 𝑲𝒊のLoop候補推定 – 𝐾𝑖と, そのCovisibilityグラフ(𝜃 𝑚𝑖𝑛 = 30)内での隣接KFとの間でのBoWの類似度を計算しその最小スコア𝑠 𝑚𝑖𝑛を保持 – 次にBoWのデータベースを照会 • スコアが𝑠 𝑚𝑖𝑛よりも小さいものは候補から外す • 𝐾𝑖に直接接続しているKFも候補から外す • 最終的に一貫性のある3つのLoop候補を連続して検出できた時に, Loop候補として認定
35 Loop Closing 2. 類似変換の計算 – Loopを閉じるには現在のKF 𝑲𝒊と, Loopを閉じる先𝑲𝒍との間で7自由度を考慮に入れて類似変換を計算する必要あり – 現在のKF 𝐾𝑖とLoop候補との間のORB対応をBoW場所認識器に従って計算(各Loop候補に対して対応が得られた) – Horn+(1987) の代わりにRANSACを用いて類似変換を計算 • 類似変換𝐒𝑖𝑙の仮説を求め, それを満たすinlierの数を数え, 十分な数inlierが存在すればLoop候補として認定 – KF1とKF2との間での類似変換は以下の式を最適化することで求める(𝑖 ⇒ 𝑗へのn個の対応) 𝛀1𝑖, 𝛀2𝑖はKF1とKF2の画像のスケールに対応する共分散行列
36 Loop Closing 3. Loopの融合 – Loop補正ではまずCovisibilityグラフに新しいedgeを挿入し更新 – まず現在のKF 𝐾𝑖の姿勢𝐓𝑖𝑤を類似変換𝐒𝑖𝑙で補正 • 𝐾𝑖の全ての近傍に伝播, 変換を連結しLoopの両端を揃える • Loopで動いたKFのMapPointは𝐾𝑖とその近傍に投影され一致点の検索が行われる(Tracking(4)) • 一致した全てのMapPointと𝐒𝑖𝑙の計算でinlierになったMapPoi ntが融合される • Covisibilityグラフのedgeの値を更新(共有するMapPoint数)
37 Loop Closing 4. Essentialグラフの最適化 – Essentialグラフに関してポーズグラフの最適化 – Loop Closingのエラーをグラフに沿って分散させる • 類似変換を介して行い, スケールドリフトの修正 – ポーズグラフの姿勢の最適化は以下の式の最適化で行う 𝐒𝑖𝑗はポーズグラフ最適化の直前の𝑆𝐸(3)ポーズから計算された両方のK F間の相対的な𝑆𝑖𝑚(3)変換 Λ 𝑖𝑗はedgeの情報行列であり, 単位行列に設定 7自由度の固定のためLoop ClosureのKFは固定 𝐞𝑖𝑗 = log 𝑠𝑖𝑚(3)(𝐒𝑖𝑗 𝐒𝑗𝑤 𝐒𝑖𝑤 −1 ) 𝐶 = 𝑖,𝑗 (𝐞𝑖𝑗 𝑇 𝚲ij 𝐞ij)
38 参考文献 • ORB-SLAM – http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO1 5.pdf

Check these out next

ORB-SLAMの手法解説

Recomendados

Mais conteúdo relacionado

Apresentações para você(20)

Mais de Masaya Kaneko(7)

Último(20)

ORB-SLAMの手法解説