20180527 ORB SLAM Code Reading
15 likes13,334 views
20180527 ORB SLAM Code Reading
- 1. 2018/05/27 3D Vision勉強会＠関東 ORB-SLAM Code Reading takmin
- 2. 自己紹介 2 株式会社ビジョン＆ITラボ 代表取締役 皆川 卓也（みながわ たくや） 「コンピュータビジョン勉強会＠関東」主催 博士（工学） 略歴： 1999-2003年 日本HP（後にアジレント・テクノロジーへ分社）にて、ITエンジニアとしてシステム構築、プリ セールス、プロジェクトマネジメント、サポート等の業務に従事 2004-2009年 コンピュータビジョンを用いたシステム/アプリ/サービス開発等に従事 2007-2010年 慶應義塾大学大学院 後期博士課程にて、コンピュータビジョンを専攻 単位取得退学後、博士号取得（2014年） 2009年-現在 フリーランスとして、コンピュータビジョンのコンサル/研究/開発等に従事（2018年法人化） http://visitlab.jp
- 4. 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
- 5. ORB-SLAMコードリーディング 5 ORB SLAM2 https://github.com/raulmur/ORB_SLAM2 ORB_SLAM2はORB_SLAMをステレオカメラと RGBD向けに拡張したもの 元のORB_SLAMのソースコードもあるが、単眼カメ ラの機能もORB_SLAM2に統合しているので、2を 使うのが望ましい ここでは主に単眼カメラ部分についてのみ解説 アルゴリズムについてはsyinari0123さんの解 説があったので割愛
- 6. ORB-SLAMを動かす 6 githubからソースコードを入手 C++11コンパイラが必要 Visual Studio2015では無理だった（2017では？） 必要なライブラリをインストールしてbuild.shと いうシェルを起動すればビルドできる。 KITTI等の既存データセットを動かす場合 リアルタイムで動作させたいときはROSをセット アップした後、build_ros.shでビルド
- 7. 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
- 8. 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 外部接続したカメラ映像にキューブを重畳表示するサンプル
- 9. 外部接続カメラで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を動 かす
- 11. ORB-SLAMコードの特徴 11 実装は論文通り 論文とコードの対応を見つけやすい 論文と実装が違うというケースも割とあるので MonoとStereoとRGB-Dが一緒になっているので、それぞ れを分離して読み解く必要 ほとんど全てのパラメータがメンバ変数になっており、関 数の引数として渡されることがないため、各関数がどの ようなパラメータに依存してるかが非常にわかりにくい。 そのため、カスタマイズの影響範囲を見積もるのがツライ リアルタイム/マルチスレッドのため、デバッグがツライ。。。 例えばLocal Mappingがデバッグ処理で遅れたらTrackingも止 まる
- 15. ORB-SLAMコードリーディング 15 本日お話しする範囲 Systemの使い方 Systemクラスの処理概要 Trackingスレッドの処理概要 LocalMappingスレッドの処理概要 LoopClosingスレッドの処理概要
- 16. ORB-SLAMコードリーディング 16 本日お話しする範囲 Systemの使い方 Systemクラスの処理概要 Trackingスレッドの処理概要 LocalMappingスレッドの処理概要 LoopClosingスレッドの処理概要
- 17. ライブラリ使用方法流れ 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. ライブラリ使用方法流れ 18 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”); using namespace ORB_SLAM2; /*VisualWordファイル、カメラパラメータファ イル、モノクロ/ステレオ/RGBD、画面表示の 順で引数を与える */ System SLAM(voc_file, setting_file, System::MONOCULAR, true);
- 19. ライブラリ使用方法流れ 19 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”); 動画ファイルやカメラ等から画像のキャプ チャ(cv::Mat)およびタイムスタンプ(double)を 取得
- 20. ライブラリ使用方法流れ 20 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”); /*Systemインスタンスに画像とタイムスタンプ を渡す*/ SLAM.TrackMonocular(frame, time_stamp);
- 21. ライブラリ使用方法流れ 21 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”); /*SLAMを修了後、キーフレームの姿勢を ファイルに保存*/ SLAM.Shutdown(); SLAM.SaveKeyFrameTrajectoryTUM(“KeyFra meTrajectory.txt”);
- 22. ORB-SLAMコードリーディング 22 本日お話しする範囲 Systemの使い方 Systemクラスの処理概要 Trackingスレッドの処理概要 LocalMappingスレッドの処理概要 LoopClosingスレッドの処理概要
- 23. 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)
- 24. System::System() 24 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) mpVocabulary = new ORBVocabulary(); bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
- 25. System::System() 25 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) // 引数にVisualVocabularyを与えてDB生成 mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
- 26. System::System() 26 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) mpMap = new Map();
- 27. System::System() 27 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) // カメラからの映像にORBを描画する画面 mpFrameDrawer = new FrameDrawer(mpMap); // 点群とカメラ位置を描画する画面 mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
- 28. System::System() 28 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) /* 描画画面、VisualVocabulary、KeyFrame DB、Map、カメラパラ メータ、などを引数に与える*/ mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
- 29. System::System() 29 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) /* 描画画面、VisualVocabulary、KeyFrame DB、Map、カメラパラ メータ、などを引数に与え、このスレッドで動作させる*/ mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
- 30. System::System() 30 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) /* Mapを渡してLocalMappingインスタンスを生成し、 新規スレッドでRun()を起動*/ mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
- 31. System::System() 31 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) /* MapとVocabularyとDBを渡してLoopClosingインスタンスを生成 し、新規スレッドでRun()を起動*/ mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR); mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
- 32. System::System() 32 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) mpTracker->SetLocalMapper(mpLocalMapper); mpTracker->SetLoopClosing(mpLoopCloser); mpLocalMapper->SetTracker(mpTracker); mpLocalMapper->SetLoopCloser(mpLoopCloser); mpLoopCloser->SetTracker(mpTracker); mpLoopCloser->SetLocalMapper(mpLocalMapper);
- 33. System::TrackMonocular() 33 カメラやビデオから取得した画像 (cv::Mat)を引数として とタイムスタンプ (timestamp)と一緒に入力 内部では、画像とタイムスタンプをそのま まトラッカー（mpTracker）の GrabImageMonocular()メンバ関数へ渡し て、カメラ姿勢や追跡中の点群の情報な どを取得
- 34. ORB-SLAMコードリーディング 34 本日お話しする範囲 Systemの使い方 Systemクラスの処理概要 Trackingスレッドの処理概要 LocalMappingスレッドの処理概要 LoopClosingスレッドの処理概要
- 35. Tracking::GrabImageMonocular() 35 引数として、 System::TrackMonocular()から画像と タイムスタンプが渡される 画像をグレースケールに変換 Frame構築 Tracking::Track()関数呼び出し カメラの姿勢を返す
- 36. Tracking::GrabImageMonocular() 36 引数として、 System::TrackMonocular()から画像と タイムスタンプが渡される 画像をグレースケールに変換 Frame構築 Tracking::Track()関数呼び出し カメラの姿勢を返す mpCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth); グレースケール画像 タイムスタンプ
- 37. Tracking::GrabImageMonocular() 37 引数として、 System::TrackMonocular()から画像と タイムスタンプが渡される 画像をグレースケールに変換 Frame構築 Tracking::Track()関数呼び出し カメラの姿勢を返す mpCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth); ORB検出器 （Frameコンストラクタ内で使用） VisualVocabulary
- 38. Tracking::GrabImageMonocular() 38 引数として、 System::TrackMonocular()から画像と タイムスタンプが渡される 画像をグレースケールに変換 Frame構築 Tracking::Track()関数呼び出し カメラの姿勢を返す mpCurrentFrame = Frame(mImGray, timestamp, mpORBextractorLeft, mpORBVocabulary, mK, mDistCoef, mbf, mThDepth); カメラ行列 歪パラメータ Monocular SLAMでは使用しない
- 40. Tracking::Track() 40 Track() Frame::Frame() MonocularInitializ ation() • 対応点が十分とれた２つのフレー ムからMapとCamera Poseの初期値 算出 • FとHを並行して求めて、良い方の モデルを採用 • 詳細はInitializer::Initialize()参照
- 41. Tracking::Track() 41 Track() Frame::Frame() • Local Map Points の追跡 • カメラ姿勢推定
- 42. Track Local Map TrackWithMotionModel() TrackLocalMap() TrackReferenceKeyFrame() Tracking成功Tracking失敗 OK NG NG OK OKNG 前フレームのキーポイ ントと対応する点を現 フレームから速度一定 と仮定して探索し、対 応点からポーズ推定 BoWで対応キーフレー ムの対応点を探索し、 ポーズ推定 対応するLocal Mapを Refineして、ポーズ推定
- 44. ORB-SLAMコードリーディング 44 本日お話しする範囲 Systemの使い方 Systemクラスの処理概要 Trackingスレッドの処理概要 LocalMappingスレッドの処理概要 LoopClosingスレッドの処理概要
- 47. LoopMapping::Run() 47 Run() • キーフレームを挿入して Covisibility GraphとSpanning Treeを更新 • BoW表現を計算 ProcessNewKeyFrame();
- 49. LoopMapping::Run() 49 Run() Covisibility Graph上の隣接キーフ レームを用いて追跡点の三次元 座標を算出しMap Points生成 CreateNewMapPoints(); もう一つ隣のキーフレームを用い て残った追跡点のMap Points生成 SearchInNeighbors();
- 52. ORB-SLAMコードリーディング 52 本日お話しする範囲 Systemの使い方 Systemクラスの処理概要 Trackingスレッドの処理概要 LocalMappingスレッドの処理概要 LoopClosingスレッドの処理概要
- 56. LoopClosing::Run() 56 Run() • ループ上で隣接するキーフレー ム間の対応点を算出 • キーフレーム間の相似変換算出 （3D to 3D） ComputeSim3();
- 57. LoopClosing::Run() 57 Run() • 相似変換を伝播させてカメラ姿 勢補正 • Essential Graph上でLoop Closingにより各カメラ姿勢を最 適化 • カメラ姿勢最適化後、点群の位 置を最適化 CorrectLoop();
- 58. まとめ 58 実装は論文通りなので、論文と照らし合わせながらソー スコードを読んだ方が理解が早い。 巨大なヒューリスティックの塊 Systemクラスがメインで、Trackingをメインスレッドで動か し、LocalMapping、LoopClosingのスレッドを生成。 各クラス内の処理ではほとんどメンバー関数自体には引 数を渡さず、メンバー変数を直接変更するため、各関数 の影響範囲が見えづらい リアルタイムかつマルチスレッドで処理が複雑、かつクラ ス同士が複雑に絡み合っており、カスタマイズは非常に ツライ。。。