Qoncept(http://qoncept.jp/)でのInternship成果の一部として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
関連研究１
• Place Recognition
– 場所認識には画像同士の比較手法が大規模な環境下で最も優れ
ているらしい(Williams+(2009))
– 我々はORB+(DBoW2上での)BoW場所認識器を利用
• Map initialization
– Engel+は画素の逆深度を分散の大きいランダム値で初期化し,実
際の位置に収束させていた(Engel+, LSD-SLAM(2014))
– 2視点からの初期化方法として一般には以下が存在
• Homography
• 5点アルゴリズムからの基本行列(平面と一般シーンに対して)
• 8点アルゴリズムによる基礎行列(非平面シーンに対して)
– 我々はHomographyと基礎行列との間のモデル選択を自動的に
行うアルゴリズムを提案
(Torr+(1999)の改良版で, 縮退のケースを考慮に入れている)

6. 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)

7. 6
関連研究３
• 単眼SLAM
– KFの選択方法
• 全ての点とフレームに対するBAを行うことは一般的に不可能
• Strasdat+は「重複しないKFだけを保持しながら多くの点を
保持することがコスト効率が最も高い」としている
• PTAMはKFの挿入がかなり慎重なのでrelocalizeが困難
• 以上から「できるだけKFを早く挿入し, 後で冗長なものを削
除するのが最良」であり, 今回はこれを利用することで高い
ロバスト性能を獲得することに成功

8. • Tracking, Local mapping, Loop closingを３スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
7
システムの大まかな概要

9. • Tracking, Local mapping, Loop closingを３スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
8
システムの大まかな概要
スレッド１
全てのFrameでカメラの位置を推定し,
新しいKFを挿入するタイミングを決定

10. • Tracking, Local mapping, Loop closingを３スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
9
システムの大まかな概要
スレッド２
新しいKFを処理し, local BAでカメラ周
りのマップの最適化

11. • Tracking, Local mapping, Loop closingを３スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
10
システムの大まかな概要
スレッド３
新しいKF毎にループの検索を行い, ルー
プを検出した場合にglobal BAで最適化

12. • Tracking, Local mapping, Loop closingを３スレッドで並列実行
• 同時に推定した点のMapを作成
• Relocalization, Loop Closingのために場所認識用の辞書作成
11
システムの大まかな概要
Mapの作成
Trackingの結果を得られた
点やKFの情報を格納

13. • Tracking, Local mapping, Loop closingを３スレッドで並列実行
• 同時に推定した点の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)

39. 38
参考文献
• ORB-SLAM
– http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO1
5.pdf