前回はMonocular-Inertialのサンプルコード[mono_inertial_euroc.cc]を見てみたので、今回は本体のSystem[.cc/.h]について処理内容のメモを書いてみます。
ORB-SLAM3の本体はinclude、srcフォルダ内にあり２つのヘッダファイルを除いて一対一対応するように作成されています。
それぞれのファイルの処理を何となく分類してみると下図のような感じになると思います。
ようするに今回調べるSystemクラスはORB-SLAM3を動かすエントリーポイント的な役割を担っています。
System::System
System.ccファイルの41行目から始まるSystem関数がシステムの全体の初期化を担当しています。
ちなみに変数名の接頭辞は恐らく下記のような意味
- m～：member
- b～：boolean
- mb～：member boolean
- mp～：member pointer
メッセージ&動作モード
SLAM起動時のメッセージとセンサタイプの確認メッセージが表示されます。
設定値ファイル[.yaml]の展開
引数のstrSettingsFileから設定値ファイル.yamlを開いて下記の処理をしています。
//Check settings file ← コメント以下の処理- atlasファイルの設定
- loopClosingの起動フラグ | 設定値ファイルに記載が無い場合はアクティブ状態になります
- vocabrary file pathの保持
Atlas関連インスタンス生成
112行目付近からORB-SLAMに必要な主要インスタンスを生成していきます。
- ORBVocabulary
- KeyFrameDatabase
- Atlas / atlasファイルが存在する場合、System::LoadAtlas関数から生成
IMUセンサありの場合、Atlasクラスに設定
new部分を纏めると下記
mpVocabulary = new ORBVocabulary();
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
mpAtlas = new Atlas(0); 又は bool isRead = LoadAtlas(FileType::BINARY_FILE);
//IMUセンサありの場合
if (mSensor==IMU_STEREO || mSensor==IMU_MONOCULAR || mSensor==IMU_RGBD)
mpAtlas->SetInertialSensor();ビュワー用変数のインスタンス生成
- FrameDrawer
- MapDrawer
主要スレッドのインスタンス生成
- Tracking
- LocalMapping
- LoopClosing
Atlasクラスのインスタンスは生成済みなので、これで主要なアルゴリズムは全て起動完了です。
スレッド間のアクセスポイント設定
コードそのままですが、下記のコードで各スレッド間で共有ポインタを設定しています。
//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);Viewerスレッド開始
ビュワー用変数のインスタンス生成で生成した変数をVierwerクラスにセットします。
(bUseViewerがtrueの場合)
以上がざっくりとしたシステム全体の初期化の流れになります。
System::TrackMonocular
前回はMonocular-Inertialのサンプルを読んだのでその続きとして今回はTrackMonocularを読んでいきます。
この処理はmono_inertial_euroc.ccファイル内でデータセットのフレームが存在する限りfor文で繰り返し処理をされます。
モノカラー以外にカメラタイプ別に下記の２つの関数が用意されています。
- TrackStereo | ステレオカメラ
- TrackRGBD | カラー&デプスカメラ
リセット処理
システムがシャットダウン中であればここで処理終了
センサタイプの確認
System::Systemで渡されたセンサタイプと違う場合処理終了
if(mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR)画像のリサイズ（必要な場合）
設定値ファイルでリサイズ指定した場合処理が実行されます。
ちなみにmono_inertial_euroc.cc側でもリサイズ設定があるので２重リサイズされないように注意してください。
ローカライゼーションモード変更の確認
ローカライゼーションモードの有効無効をチェックし、LocalMapper[LocalMappingクラス]の処理指令を出します。
リセットの確認
リセット指令が来た場合にリセット処理を行います。
IMUデータの取得
IMUセンサ有の場合、IMUからデータ取得しトラッカーへデータを渡します。
mpTracker->GrabImuData(vImuMeas[i_imu]);画像トラッキングの実行
トラッカーへ画像データを渡しカメラの位置と姿勢を取得します。
Sophus::SE3f Tcw = mpTracker->GrabImageMonocular(imToFeed, timestamp, filename);トラッキング状態の更新
トラッキング状態、トラッキングされたマップポイント、キー点の位置を更新します。
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
以上がトラッキング処理概要になります。
