20180527 ORB SLAM Code Reading

2018/05/27 3D Vision勉強会＠関東
ORB-SLAM Code Reading
takmin

自己紹介
2
株式会社ビジョン＆ITラボ代表取締役
皆川卓也（みながわたくや）
「コンピュータビジョン勉強会＠関東」主催
博士（工学）
略歴：
1999-2003年
日本HP（後にアジレント・テクノロジーへ分社）にて、ITエンジニアとしてシステム構築、プリ
セールス、プロジェクトマネジメント、サポート等の業務に従事
2004-2009年
コンピュータビジョンを用いたシステム/アプリ/サービス開発等に従事
2007-2010年
慶應義塾大学大学院後期博士課程にて、コンピュータビジョンを専攻
単位取得退学後、博士号取得（2014年）
2009年-現在
フリーランスとして、コンピュータビジョンのコンサル/研究/開発等に従事（2018年法人化）
http://visitlab.jp

コンピュータビジョン勉強会＠関東
 こんなテーマでやってきました！
「コンピュータビジョン最
先端ガイド」の輪読会
ECCV読み会
強化学習縛り
論文読み会
AR/VRを支える
技術
ICCV読み会
CVPR読み会
CVでこんなもの作
りました大LT大会
CVの便利なツール/
ライブラリ
CVのお仕事
ハッカソン
OpenCV祭り
有名論文読み会
三次元形状計測・
認識
「CV勉強会」で検索！
https://sites.google.com/site/cvsaisentan/

ORB-SLAMとは？
4
 単眼カメラのVisual SLAM
 Mur-Artal, R., Montiel, J. M. M., & Tardos, J. D. (2015).
ORB-SLAM:AVersatile and Accurate Monocular
SLAM System. IEEETransactions on Robotics,
 ソースコード:
https://github.com/raulmur/ORB_SLAM2
 動画：
https://www.youtube.com/watch?v=ufvPS5wJAx0

ORB-SLAMコードリーディング
5
 ORB SLAM2
 https://github.com/raulmur/ORB_SLAM2
 ORB_SLAM2はORB_SLAMをステレオカメラと
RGBD向けに拡張したもの
 元のORB_SLAMのソースコードもあるが、単眼カメ
ラの機能もORB_SLAM2に統合しているので、2を
使うのが望ましい
 ここでは主に単眼カメラ部分についてのみ解説
 アルゴリズムについてはsyinari0123さんの解
説があったので割愛

ORB-SLAMを動かす
6
 githubからソースコードを入手
 C++11コンパイラが必要
 Visual Studio2015では無理だった（2017では？）
 必要なライブラリをインストールしてbuild.shと
いうシェルを起動すればビルドできる。
 KITTI等の既存データセットを動かす場合
 リアルタイムで動作させたいときはROSをセット
アップした後、build_ros.shでビルド

ORB-SLAMの依存ライブラリ
7
 Pangolin
 ビジュアライゼーションに必要
 https://github.com/stevenlovegrove/Pangolin
 OpenCV (2.4.11と3.2でテスト)
 画像の操作と特徴量の抽出
 http://opencv.org
 Eigen3
 g2oを動かすのに必要
 http://eigen.tuxfamily.org
 DBoW2
 Place Recognitionに使用。ORB-SLAM2に同梱
 g2o
 幾何学的な最適化に使用。ORB-SLAM2に同梱
 ROS (Hydro or Newer)
 リアルタイムで動かす時、カメラ出力をROSのトピックとして動かす必要
 http://ros.org

ORB-SLAMのサンプル
8
 あらかじめ、以下のサンプルコードが用意されており、コ
ンパイルすれば使える。
 Examples/Monocular/mono_euroc.cc
 EuRoCデータセット用サンプル
 Examples/Monocular/mono_kitti.cc
 KITTIデータセット用サンプル
 Examples/Monocular/mono_tum.cc
 TUMデータセット用サンプル
 Examples/ROS/ORB_SLAM2/src/ros_mono.cc
 外部接続したカメラで動かすためのサンプル
 Examples/ROS/ORB_SLAM2/src/AR/ros_mono_ar.cc
 外部接続したカメラ映像にキューブを重畳表示するサンプル

外部接続カメラでORB-SLAMを動かす
9
 外部接続カメラを使用するのはROS (Robot
Operating System)とROS上でカメラを動かす
ためのモジュールをインストールする必要が
ある。
 例えば、 https://github.com/bosch-ros-pkg/usb_cam
 カメラから映像を取得するトピック名を
/camera/image_rawとして実行した状態で、
ORB-SLAMのros_monoやros_mono_arを動
かす

ORB-SLAMのコード構成
10
メインのコード群
ビルドのスクリプト
リアルタイムカメラ入力を扱うためのビルドス
クリプト
各データセットのための試験コードやAR等の実
行ファイル
DBoW2とg2o
Place Recognitionを行うための学習済み辞書
データ

ORB-SLAMコードの特徴
11
 実装は論文通り
 論文とコードの対応を見つけやすい
 論文と実装が違うというケースも割とあるので
 MonoとStereoとRGB-Dが一緒になっているので、それぞ
れを分離して読み解く必要
 ほとんど全てのパラメータがメンバ変数になっており、関
数の引数として渡されることがないため、各関数がどの
ようなパラメータに依存してるかが非常にわかりにくい。
 そのため、カスタマイズの影響範囲を見積もるのがツライ
 リアルタイム/マルチスレッドのため、デバッグがツライ。。。
 例えばLocal Mappingがデバッグ処理で遅れたらTrackingも止
まる

ORB-SLAMのコード
12
System.h
main()からcv::Mat形式の画像
を一枚一枚
System::TrackMonocular()で
Systemに渡す
Examplesフォルダにサンプル

13
System.h
Tracking.h
LocalMapping.h
LoopClosing.h
Map.hORBVocabulary.h
KeyFrameDatabase.h
Frame.h
KeyFrame.h

14
Read
Read/
Write
Read/Write
共有メモリ
Read/Write
Read

15
本日お話しする範囲
 Systemの使い方
 Systemクラスの処理概要
 Trackingスレッドの処理概要
 LocalMappingスレッドの処理概要
 LoopClosingスレッドの処理概要

16

ライブラリ使用方法流れ
17
using namespace ORB_SLAM2;
System SLAM(voc_file, setting_file, System::MONOCULAR, true);
cv::VideoCapture cap(0);
cv::Mat frame;
while(true){
cap >> frame;
double time_stamp = (double)clock();
if(frame.empty())
break;
SLAM.TrackMonocular(frame, time_stamp);
}
SLAM.Shutdown();
SLAM.SaveKeyFrameTrajectoryTUM(“KeyFrameTrajectory.txt”);

18
cv::Mat frame;
while(true){
cap >> frame;
if(frame.empty())
break;
}
SLAM.Shutdown();
/*VisualWordファイル、カメラパラメータファ
イル、モノクロ/ステレオ/RGBD、画面表示の
順で引数を与える */
System SLAM(voc_file, setting_file,
System::MONOCULAR, true);

19
cv::Mat frame;
while(true){
cap >> frame;
if(frame.empty())
break;
}
SLAM.Shutdown();
動画ファイルやカメラ等から画像のキャプ
チャ(cv::Mat)およびタイムスタンプ(double)を
取得

20
cv::Mat frame;
while(true){
cap >> frame;
if(frame.empty())
break;
}
SLAM.Shutdown();
/*Systemインスタンスに画像とタイムスタンプ
を渡す*/

21
cv::Mat frame;
while(true){
cap >> frame;
if(frame.empty())
break;
}
SLAM.Shutdown();
/*SLAMを修了後、キーフレームの姿勢を
ファイルに保存*/
SLAM.Shutdown();
SLAM.SaveKeyFrameTrajectoryTUM(“KeyFra
meTrajectory.txt”);

22

System::System()
23
Systemコンストラクタ（System.cc: 32 - 114）内の
主な処理
1. VisualVocabulary読み込み(l.64)
2. Recognition Database生成(l.75)
3. Map生成(l.78)
4. 結果表示画面生成(l.81)
5. Tracking生成(l.86)
6. Local Mapping生成およびスレッド起動(l.90)
7. Loop Closing生成およびスレッド起動(l.94)
8. スレッド間相互参照設定(l.106)

System::System()
24
主な処理
3. Map生成(l.78)
mpVocabulary = new ORBVocabulary();
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);

System::System()
25
主な処理
3. Map生成(l.78)
// 引数にVisualVocabularyを与えてDB生成
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

System::System()
26
主な処理
3. Map生成(l.78)
mpMap = new Map();

System::System()
27
主な処理
3. Map生成(l.78)
// カメラからの映像にORBを描画する画面
mpFrameDrawer = new FrameDrawer(mpMap);
// 点群とカメラ位置を描画する画面
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

System::System()
28
主な処理
3. Map生成(l.78)
/* 描画画面、VisualVocabulary、KeyFrame DB、Map、カメラパラ
メータ、などを引数に与える*/
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer,
mpMapDrawer, mpMap, mpKeyFrameDatabase,
strSettingsFile, mSensor);

System::System()
29
主な処理
3. Map生成(l.78)
/* 描画画面、VisualVocabulary、KeyFrame DB、Map、カメラパラ
メータ、などを引数に与え、このスレッドで動作させる*/
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer,
mpMapDrawer, mpMap, mpKeyFrameDatabase,
strSettingsFile, mSensor);

System::System()
30
主な処理
3. Map生成(l.78)
/* Mapを渡してLocalMappingインスタンスを生成し、
新規スレッドでRun()を起動*/
mpLocalMapper = new
LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new
thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);

System::System()
31
主な処理
3. Map生成(l.78)
/* MapとVocabularyとDBを渡してLoopClosingインスタンスを生成
し、新規スレッドでRun()を起動*/
mpLoopCloser = new LoopClosing(mpMap,
mpKeyFrameDatabase, mpVocabulary,
mSensor!=MONOCULAR);
mptLoopClosing = new
thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

System::System()
32
主な処理
3. Map生成(l.78)
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);

System::TrackMonocular()
33
 カメラやビデオから取得した画像
(cv::Mat)を引数としてとタイムスタンプ
(timestamp)と一緒に入力
 内部では、画像とタイムスタンプをそのま
まトラッカー（mpTracker）の
GrabImageMonocular()メンバ関数へ渡し
て、カメラ姿勢や追跡中の点群の情報な
どを取得

34

Tracking::GrabImageMonocular()
35
 引数として、
System::TrackMonocular()から画像と
タイムスタンプが渡される
 画像をグレースケールに変換
 Frame構築
 Tracking::Track()関数呼び出し
 カメラの姿勢を返す

36
 Frame構築
mpCurrentFrame = Frame(mImGray, timestamp,
mpORBextractorLeft, mpORBVocabulary,
mK, mDistCoef, mbf, mThDepth);
グレースケール画像タイムスタンプ

37
 Frame構築
ORB検出器
（Frameコンストラクタ内で使用）
VisualVocabulary

38
 Frame構築
カメラ行列歪パラメータ Monocular SLAMでは使用しない

Tracking::Track()
39
Track()
Frame::Frame()

Tracking::Track()
40
Track()
Frame::Frame()
MonocularInitializ
ation()
• 対応点が十分とれた２つのフレー
ムからMapとCamera Poseの初期値
算出
• FとHを並行して求めて、良い方の
モデルを採用
• 詳細はInitializer::Initialize()参照

Tracking::Track()
41
Track()
Frame::Frame()
• Local Map Points
の追跡
• カメラ姿勢推定

Track Local Map
TrackWithMotionModel()
TrackLocalMap()
TrackReferenceKeyFrame()
Tracking成功Tracking失敗
OK
NG
NG
OK
OKNG
前フレームのキーポイ
ントと対応する点を現
フレームから速度一定
と仮定して探索し、対
応点からポーズ推定
BoWで対応キーフレー
ムの対応点を探索し、
ポーズ推定
対応するLocal Mapを
Refineして、ポーズ推定

Tracking::Track()
43
Track()
Frame::Frame()
KeyFrameが条件を満たしているか
の判定
if(NeedNewKeyFrame())
CreateNewKeyFrame();

44

LoopMapping::Run()
46
Run()
新しいKeyFrameが来たかの判定
CheckNewKeyFrames();

LoopMapping::Run()
47
Run()
• キーフレームを挿入して
Covisibility GraphとSpanning
Treeを更新
• BoW表現を計算
ProcessNewKeyFrame();

LoopMapping::Run()
48
Run()
キーフレームの点のうち、ロバスト
でないものを除去
MapPointCulling();

LoopMapping::Run()
49
Run()
Covisibility Graph上の隣接キーフ
レームを用いて追跡点の三次元
座標を算出しMap Points生成
CreateNewMapPoints();
もう一つ隣のキーフレームを用い
て残った追跡点のMap Points生成
SearchInNeighbors();

LoopMapping::Run()
50
Run()
バンドル調整で現キーフレームと
隣接キーフレームの姿勢、および
それらから観測できる点群を最適
化
Optimizer::LocalBundleAdjustmen
t(mpCurrentKeyFrame,&mbAbort
BA, mpMap);

LoopMapping::Run()
51
Run()Covisibility Graph上の隣接キーフ
レームのうち、他と重複の大きいも
のを除去
KeyFrameCulling();

52

LoopClosing::Run()
54
Run()
新規キーフレームの取得
CheckNewKeyFrames();

LoopClosing::Run()
55
Run()
Covisibility Graph上のループ候補
を取得
DetectLoop();

LoopClosing::Run()
56
Run()
• ループ上で隣接するキーフレー
ム間の対応点を算出
• キーフレーム間の相似変換算出
（3D to 3D）
ComputeSim3();

LoopClosing::Run()
57
Run()
• 相似変換を伝播させてカメラ姿
勢補正
• Essential Graph上でLoop
Closingにより各カメラ姿勢を最
適化
• カメラ姿勢最適化後、点群の位
置を最適化
CorrectLoop();

まとめ
58
 実装は論文通りなので、論文と照らし合わせながらソー
スコードを読んだ方が理解が早い。
 巨大なヒューリスティックの塊
 Systemクラスがメインで、Trackingをメインスレッドで動か
し、LocalMapping、LoopClosingのスレッドを生成。
 各クラス内の処理ではほとんどメンバー関数自体には引
数を渡さず、メンバー変数を直接変更するため、各関数
の影響範囲が見えづらい
 リアルタイムかつマルチスレッドで処理が複雑、かつクラ
ス同士が複雑に絡み合っており、カスタマイズは非常に
ツライ。。。

20180527 ORB SLAM Code Reading

Recommended

More Related Content

What's hot (20)

Similar to 20180527 ORB SLAM Code Reading (20)

More from Takuya Minagawa (20)

20180527 ORB SLAM Code Reading