前回に続いて今回はExamplesフォルダを見ていきたいと思います。
ORB-SLAM3は単眼、RGB-D、ステレオカメラの3タイプの入力とIMU[加速度]有り無しの計6パターンに対応しているのでExamplesフォルダも全てのパターンについて作成されています。
今回はその中でも需要の高そうなピンホール単眼+IMU(Monocular-Inertial)サンプルについて見ていきます。
Monocular-Inertialサンプルの中身
フォルダの中身は下記の通り。
ORB-SLAM3
├ Examples
├ Monocular-Inertial/
├ mono_inertial_euroc.cc | オフライン/ピンホール
├ EuRoC.yaml
├ mono_inertial_realsense_D435i.cc | オンライン/ピンホール
├ RealSense_D435i.yaml
├ mono_inertial_realsense_t265.cc | オンライン/魚眼
├ RealSense_T265.yaml
├ mono_inertial_tum_vi.cc | オフライン/魚眼
├ TUM-VI.yaml | 通常設定値
├ TUM-VI_far.yaml | System.thFarPoints設定
├ EuRoC_IMU
├ EuRoC_TimeStamps
├ TUM_IMU
└ TUM_TimeStamps
用語は下記。
- EuRoC : チューリッヒ工科大学公開データセット
- realsense D435i : intelのステカメ
- realsense t265 : intelの魚眼レンズのステカメ
- TUM : ミュンヘン工科大学公開データセット
カメラとIMUの設定値はyaml形式でそれぞれ準備されていて、カメラやIMUのキャリブレーションデータが記載されています。
設定値の意味や設定の仕方についてはプロジェクトのルートディレクトリにあるCalibration_Tutorial.pdfに詳細が載っています。
忙しい人向けに目次と概要をメモしておきます。
- 導入
- カメラ-IMUの座標モデルと変数の定義
- IMUパラメータの設定値について
- Realsense D435iの設定例
- 新規設定の例 : 設定値変数の説明
mono_inertial_euroc.ccのコード
slamの実行には下記の４つの引数が必須で、オプションでデータ保存用のファイル名が入れられます。
- vocabularyファイルのパス [ORBvoc.txt]
- 設定値ファイルのパス [EuRoC.yaml]
- シーケンスフォルダのパス [データセットのルート]
- タイムスタンプデータのパス [MH01.txt]
- カメラとキーフレームの軌跡データの保存ファイル名 [オプション]
コマンドライン引数のチェック
ざっくりと2個の処理をしています。
- 引数の個数が必須引数の個数以下であればエラーで返す。
- 必須引数とシーケンスフォルダパスのペア(imageとtimes)が存在する時、引数は奇数でオプションのデータ保存ファイル名が存在する時に引数は偶数となるので、偶数の場合は”file_name”引数にファイル名を設定しています。[トリッキーな気がしますが。。。]
- オプション引数が無い場合は”CameraTrajectory.txt”と”KeyFrameTrajectory.txt”というファイル名で保存されます。
// データ保存ファイル名のオプション設定 51 - 57行目
bool bFileName= (((argc-3) % 2) == 1);
string file_name;
if (bFileName)
{
file_name = string(argv[argc-1]);
cout << "file name: " << file_name << endl;
}
// データ保存ファイル名の設定 236 - 247行目
if (bFileName)
{
const string kf_file = "kf_" + string(argv[argc-1]) + ".txt";
const string f_file = "f_" + string(argv[argc-1]) + ".txt";
SLAM.SaveTrajectoryEuRoC(f_file);
SLAM.SaveKeyFrameTrajectoryEuRoC(kf_file);
}
else
{
SLAM.SaveTrajectoryEuRoC("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryEuRoC("KeyFrameTrajectory.txt");
}
入力データの読み込み
シーケンス個数分、カメラ画像とIMUデータ、それぞれに対応するタイムスタンプを読み込んでいきます。
string pathSeq(argv[(2*seq) + 3]);
string pathTimeStamps(argv[(2*seq) + 4]);
string pathCam0 = pathSeq + "/mav0/cam0/data";
string pathImu = pathSeq + "/mav0/imu0/data.csv";
LoadImages(pathCam0, pathTimeStamps, vstrImageFilenames[seq], vTimestampsCam[seq]);
cout << "LOADED!" << endl;
cout << "Loading IMU for sequence " << seq << "...";
LoadIMU(pathImu, vTimestampsImu[seq], vAcc[seq], vGyro[seq]);
cout << "LOADED!" << endl;
LoadImages関数の処理概要 [252-274行目]
- Examples/Monocular-Inertial/EuRoC_TimeStamps/MH01.txtのタイムスタンプを１行読み取ります。
- 読み取った値[=時刻]はナノ秒なので秒に換算して”vTimeStamps”に入れます。
- タイムスタンプはEuRoc/MH01/mav0/cam0/data以下に保存されているカメラ画像のファイル名[=ナノ秒時刻.png]の指定にも利用され、画像パスを”vstrImageFilenames”に格納されます。
LoadIMU関数の処理概要 [276-310行目]
- EuRoc/MH01/mav0/imu0/data.csvファイルを読み込みます。
- data.csvの中身を１行づつ取り出してパースします[タイムスタンプ、gyro x, y, z, inertial x, y, z]
- パースしたデータを”vTimeStamps”,”vAcc”,”vGyro”に格納していきます。
IMUタイムスタンプの開始位置の設定
カメラ画像のタイムスタンプとIMUのタイムスタンプはそれぞれ別々なので、カメラフレームの最初のタイムスタンプよりも早いIMUデータをスキップし、対応する最初のIMUデータのインデックスを設定します。
while(vTimestampsImu[seq][first_imu[seq]] <= vTimestampsCam[seq][0])
first_imu[seq]++;
first_imu[seq]--; // first imu measurement to be considered
SLAMシステムの初期化
下記のコードで初期化しています。最後の引数はviewerの使用/不使用を切り替える変数です。
// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_MONOCULAR, true);
メインループ
SLAMのメインの処理です。
と言ってもライブラリのユーザーとしては画像とIMUデータを渡すだけですが。
- vstrImageFilenamesからOpenCVを使って一つ画像を読込 [137行目]
- imageScale [SystemクラスのGetImageScale()で得られる値]が1以上の場合はリサイズを実行 [148-169行目]
- REGISTER_TIMESオプションが設定されている場合はリサイズ処理時間をInsertResizeTime()で保存していきます
- IMUデータの検索 [174-185行目]
- IMUデータのタイムスタンプがカメラのタイムスタンプ時刻以下である場合を確認
- トラッキングアルゴリズムの実行 195行目
- 画像、(画像の)タイムスタンプ、IMUデータを引数にTrackMonocular関数へ渡します
- REGISTER_TIMESオプションが設定されている場合はInsertTrackTime()で処理時間を保存します。[画像のリサイズとトラッキングに掛かった時間の和]
- 待機時間の調整 [208-223行目]
- TrackMonocular関数の処理時間を計算
- タイムスタンプデータからの計算上の経過時間を算出
- “ni<nImages[seq]-1″は現在のフレームが最後ではない場合、 “ni > 0″は現在のフレームが最初のフレームではない場合
- オフラインの処理が速すぎる場合は、タイムスタンプデータに合わせてスリープをかける
データセットの変更
起動時のオプション引数で複数のシーケンスデータを渡している場合は次のシーケンス処理に移ります。
if(seq < num_seq - 1)
{
cout << "Changing the dataset" << endl;
SLAM.ChangeDataset();
}
終了処理
終了は”SLAM.Shutdown();”で行います。
// Stop all threads
SLAM.Shutdown();
