SlideShare a Scribd company logo
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
関連研究1
• Place Recognition
– 場所認識には画像同士の比較手法が大規模な環境下で最も優れ
ているらしい(Williams+(2009))
– 我々はORB+(DBoW2上での)BoW場所認識器を利用
• Map initialization
– Engel+は画素の逆深度を分散の大きいランダム値で初期化し,実
際の位置に収束させていた(Engel+, LSD-SLAM(2014))
– 2視点からの初期化方法として一般には以下が存在
• Homography
• 5点アルゴリズムからの基本行列(平面と一般シーンに対して)
• 8点アルゴリズムによる基礎行列(非平面シーンに対して)
– 我々はHomographyと基礎行列との間のモデル選択を自動的に
行うアルゴリズムを提案
(Torr+(1999)の改良版で, 縮退のケースを考慮に入れている)
5
関連研究2
• 単眼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
関連研究3
• 単眼SLAM
– KFの選択方法
• 全ての点とフレームに対するBAを行うことは一般的に不可能
• Strasdat+は「重複しないKFだけを保持しながら多くの点を
保持することがコスト効率が最も高い」としている
• PTAMはKFの挿入がかなり慎重なのでrelocalizeが困難
• 以上から「できるだけKFを早く挿入し, 後で冗長なものを削
除するのが最良」であり, 今回はこれを利用することで高い
ロバスト性能を獲得することに成功
• Tracking, Local mapping, Loop closingを3スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
7
システムの大まかな概要
• Tracking, Local mapping, Loop closingを3スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
8
システムの大まかな概要
スレッド1
全てのFrameでカメラの位置を推定し,
新しいKFを挿入するタイミングを決定
• Tracking, Local mapping, Loop closingを3スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
9
システムの大まかな概要
スレッド2
新しいKFを処理し, local BAでカメラ周
りのマップの最適化
• Tracking, Local mapping, Loop closingを3スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
10
システムの大まかな概要
スレッド3
新しいKF毎にループの検索を行い, ルー
プを検出した場合にglobal BAで最適化
• Tracking, Local mapping, Loop closingを3スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
11
システムの大まかな概要
Mapの作成
Trackingの結果を得られた
点やKFの情報を格納
• Tracking, Local mapping, Loop closingを3スレッドで並列実行
• 同時に推定した点の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

More Related Content

What's hot

Visual slam
Visual slamVisual slam
Visual slam
Takuya Minagawa
 
SLAM勉強会(3) LSD-SLAM
SLAM勉強会(3) LSD-SLAMSLAM勉強会(3) LSD-SLAM
SLAM勉強会(3) LSD-SLAM
Iwami Kazuya
 
SLAM勉強会(PTAM)
SLAM勉強会(PTAM)SLAM勉強会(PTAM)
SLAM勉強会(PTAM)
Masaya Kaneko
 
20190825 vins mono
20190825 vins mono20190825 vins mono
20190825 vins mono
Takuya Minagawa
 
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII
 
SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~
SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~
SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~
SSII
 
SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向
SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向
SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向
SSII
 
LiDAR点群とSfM点群との位置合わせ
LiDAR点群とSfM点群との位置合わせLiDAR点群とSfM点群との位置合わせ
LiDAR点群とSfM点群との位置合わせ
Takuya Minagawa
 
ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発
ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発
ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発
Yoshitaka HARA
 
SLAM開発における課題と対策の一例の紹介
SLAM開発における課題と対策の一例の紹介SLAM開発における課題と対策の一例の紹介
SLAM開発における課題と対策の一例の紹介
miyanegi
 
G2o
G2oG2o
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII
 
SLAM入門 第2章 SLAMの基礎
SLAM入門 第2章 SLAMの基礎SLAM入門 第2章 SLAMの基礎
SLAM入門 第2章 SLAMの基礎
yohei okawa
 
LiDAR点群と画像とのマッピング
LiDAR点群と画像とのマッピングLiDAR点群と画像とのマッピング
LiDAR点群と画像とのマッピング
Takuya Minagawa
 
Structure from Motion
Structure from MotionStructure from Motion
Structure from Motion
Ryutaro Yamauchi
 
Cartographer を用いた 3D SLAM
Cartographer を用いた 3D SLAMCartographer を用いた 3D SLAM
Cartographer を用いた 3D SLAM
Yoshitaka HARA
 
20090924 姿勢推定と回転行列
20090924 姿勢推定と回転行列20090924 姿勢推定と回転行列
20090924 姿勢推定と回転行列
Toru Tamaki
 
[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis
[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis
[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis
Deep Learning JP
 
三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介
三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介
三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介
Ryohei Ueda
 
SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​
SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​
SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​
SSII
 

What's hot (20)

Visual slam
Visual slamVisual slam
Visual slam
 
SLAM勉強会(3) LSD-SLAM
SLAM勉強会(3) LSD-SLAMSLAM勉強会(3) LSD-SLAM
SLAM勉強会(3) LSD-SLAM
 
SLAM勉強会(PTAM)
SLAM勉強会(PTAM)SLAM勉強会(PTAM)
SLAM勉強会(PTAM)
 
20190825 vins mono
20190825 vins mono20190825 vins mono
20190825 vins mono
 
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
 
SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~
SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~
SSII2021 [TS1] Visual SLAM ~カメラ幾何の基礎から最近の技術動向まで~
 
SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向
SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向
SSII2019企画: 画像および LiDAR を用いた自動走行に関する動向
 
LiDAR点群とSfM点群との位置合わせ
LiDAR点群とSfM点群との位置合わせLiDAR点群とSfM点群との位置合わせ
LiDAR点群とSfM点群との位置合わせ
 
ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発
ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発
ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発
 
SLAM開発における課題と対策の一例の紹介
SLAM開発における課題と対策の一例の紹介SLAM開発における課題と対策の一例の紹介
SLAM開発における課題と対策の一例の紹介
 
G2o
G2oG2o
G2o
 
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
SSII2019TS: 実践カメラキャリブレーション ~カメラを用いた実世界計測の基礎と応用~
 
SLAM入門 第2章 SLAMの基礎
SLAM入門 第2章 SLAMの基礎SLAM入門 第2章 SLAMの基礎
SLAM入門 第2章 SLAMの基礎
 
LiDAR点群と画像とのマッピング
LiDAR点群と画像とのマッピングLiDAR点群と画像とのマッピング
LiDAR点群と画像とのマッピング
 
Structure from Motion
Structure from MotionStructure from Motion
Structure from Motion
 
Cartographer を用いた 3D SLAM
Cartographer を用いた 3D SLAMCartographer を用いた 3D SLAM
Cartographer を用いた 3D SLAM
 
20090924 姿勢推定と回転行列
20090924 姿勢推定と回転行列20090924 姿勢推定と回転行列
20090924 姿勢推定と回転行列
 
[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis
[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis
[DL輪読会]NeRF: Representing Scenes as Neural Radiance Fields for View Synthesis
 
三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介
三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介
三次元点群処理ライブラリPCLと 統合ロボットシステム研究での 利用例の紹介
 
SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​
SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​
SSII2022 [SS1] ニューラル3D表現の最新動向〜 ニューラルネットでなんでも表せる?? 〜​
 

More from Masaya Kaneko

Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...
Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...
Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...
Masaya Kaneko
 
GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説
GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説
GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説
Masaya Kaneko
 
論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])
論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])
論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])
Masaya Kaneko
 
Neural scene representation and rendering の解説(第3回3D勉強会@関東)
Neural scene representation and rendering の解説(第3回3D勉強会@関東)Neural scene representation and rendering の解説(第3回3D勉強会@関東)
Neural scene representation and rendering の解説(第3回3D勉強会@関東)
Masaya Kaneko
 
論文読み会2018 (CodeSLAM)
論文読み会2018 (CodeSLAM)論文読み会2018 (CodeSLAM)
論文読み会2018 (CodeSLAM)
Masaya Kaneko
 
Dynamic Routing Between Capsules
Dynamic Routing Between CapsulesDynamic Routing Between Capsules
Dynamic Routing Between Capsules
Masaya Kaneko
 
論文読み会(DeMoN;CVPR2017)
論文読み会(DeMoN;CVPR2017)論文読み会(DeMoN;CVPR2017)
論文読み会(DeMoN;CVPR2017)
Masaya Kaneko
 
コンピュータ先端ガイド2巻3章勉強会(SVM)
コンピュータ先端ガイド2巻3章勉強会(SVM)コンピュータ先端ガイド2巻3章勉強会(SVM)
コンピュータ先端ガイド2巻3章勉強会(SVM)
Masaya Kaneko
 

More from Masaya Kaneko (8)

Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...
Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...
Unsupervised Collaborative Learning of Keyframe Detection and Visual Odometry...
 
GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説
GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説
GN-Net: The Gauss-Newton Loss for Deep Direct SLAMの解説
 
論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])
論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])
論文読み会@AIST (Deep Virtual Stereo Odometry [ECCV2018])
 
Neural scene representation and rendering の解説(第3回3D勉強会@関東)
Neural scene representation and rendering の解説(第3回3D勉強会@関東)Neural scene representation and rendering の解説(第3回3D勉強会@関東)
Neural scene representation and rendering の解説(第3回3D勉強会@関東)
 
論文読み会2018 (CodeSLAM)
論文読み会2018 (CodeSLAM)論文読み会2018 (CodeSLAM)
論文読み会2018 (CodeSLAM)
 
Dynamic Routing Between Capsules
Dynamic Routing Between CapsulesDynamic Routing Between Capsules
Dynamic Routing Between Capsules
 
論文読み会(DeMoN;CVPR2017)
論文読み会(DeMoN;CVPR2017)論文読み会(DeMoN;CVPR2017)
論文読み会(DeMoN;CVPR2017)
 
コンピュータ先端ガイド2巻3章勉強会(SVM)
コンピュータ先端ガイド2巻3章勉強会(SVM)コンピュータ先端ガイド2巻3章勉強会(SVM)
コンピュータ先端ガイド2巻3章勉強会(SVM)
 

ORB-SLAMの手法解説

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