ORB_SLAM2のソースコードを読む
ORB_SLAM2のソースコードを少し読んでみました。
まずはORB_SlAMについてもう少し詳しく知った方が良いと思い調べてました。以下の論文ではORB_SLAMについて日本語で詳しくまとめられているのでおすすめです。
library.naist.jp
以下のサイトにもお世話になりました。
jp.mathworks.com
「orb_slam slideShare」などで検索しても詳しく解説されているページが見つかります。
そのままコードを読むよりもソースコード解析ツールなどを使った方が読みやすいと思います。今回は「SourceTrail」というオープンソースの解析ツールを使ってみました。SourceTrailの対応言語はC, C++, Java, Pythonです。ORB_SLAMはROS上で動いていてC++で書かれています。そのためROSについてもある程度知る必要があります。解析ツールというと大層なものに聞こえますが、自分の知識も弱いため、ちょっとしたコードビューワー程度に使っています。ソースコードの可視化や、型を表示してくれたり、参照下をすぐに表示できたりして便利です。
SourceTrailの導入
SourceTrailはLinux,Windows,Macに対応しています。Ubuntuでは、AppImageファイルを使えばインストール不要で起動できます。AppImageファイルはWindowsでいうところのexeファイルのようなものです。
Releases · CoatiSoftware/Sourcetrail · GitHub
そのリンクから現在最新のリリースバージョンであるSourcetrail_2020_2_43_Linux_64bit.AppImage
をダウンロードします。以下のコマンドで実行権限を付与すれば、AppImageをクリックして起動できるようになります。
cd ~/ダウンロード/ chmod a+x Sourcetrail_2020_2_43_Linux_64bit.AppImage
以下のサイトを参考にすればLauncherや検索画面からも起動できるようになります。
moebuntu.blog48.fc2.com
プロジェクトの読み込み
SourceTrailでプロジェクトを読み込むためにcompile_commands.jsonというファイルを生成します。ORB_SLAM2/build.shの30行目を以下のように書き換えます。
cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON .
同様に,ORB_SLAM2/build_ros.shの6行目を以下のように書き換えます。
cmake .. -DROS_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON .
もう一度ビルドしなおします。
./build.sh ./build_ros.sh
これで以下の二つのファイルが生成されます。
ORB_SLAM2/build/compile_commands.json
ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/compile_commands.json
次にSourceTrailを起動します。ORB_SLAM2はスタンドアローン版とROS版でそれぞれ別にビルドしたので、SourceTrail上でも別のプロジェクトとして二つに分けます。
www.s-style.co.jp
上記のサイトを参考に新しくプロジェクトを作成します。
スタンドアローン版ではProjectNameを「ORB_SLAM2」,ProjectLocationを「~/ORB_SLAM2」として設定して作成しました。同様にROS版では、ProjectNameを「ORB_SLAM2_ROS」,ProjectLocationを「~/ORB_SLAM2」として作成しました。それ以外の設定は以下のようにしました。
コードリーディング
今回は単眼カメラを使った処理を軽く追っていきました。ROS上で単眼カメラを使ってORB_SLAMを実行すると以下のファイルが実行されます。
ORB_SLAM2/Examples/ROS/src/ros_mono.cc
59行目からmain関数が始まります。引数にはterminalで入力した文字列が渡されます。
int main(int argc, char **argv)
ROSシステムの初期化や72行目ではSLAMに必要なスレッドの初期化を行っています(~/ORB_SLAM2/src/System.ccの33行目を呼び出す)。
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
76行目で画像を受け取ったときのコールバック関数が設定されています。
ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);画像データを引数に91行目以降のGrabImageが呼び出されます。
void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg)
画像をOpenCVでも扱える形式に変換して105行目のTrackMonocular(ORB_SLAM2/src/System.ccの219行目)を実行します。
TrackMonocular関数ではローカリゼーションモードやリセットボタンが押された際の処理があります。262行目で画像データとタイムスタンプを引数にGrabImageMonocular(/ORB_SLAM2/src/Tracking.ccの239行目)を実行しています。変数Tcwにはカメラの位置、姿勢の情報が含まれています。
cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp)
GrabImageMonocularでは画像をグレースケールに変換しています。変換した画像とタイムスタンプ、キャリブレーションファイルから読み込んだデータなどを引数としてFrame(ORB_SLAM2/src/System.ccの174行目)を呼び出します。引数の一つであるmpIniORBextractorなどは一番最初のmain関数のあるファイルの72行目で呼び出されるプログラムを追っていけば分かります。
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
キリがなく、時間もかかるので書くのはこれだけにします。関数名を検索して処理内容の確認と、呼び出し元を探しての繰り返しで画像の処理を中心に、もう少し途中まで読んでみました。
以下のサイトでは分かりやすくスライドにまとめられています。ある程度読んでから以下のサイトを見ると、読んできた内容がしっかり書いてあり、おお合ってたなあってなります。
20180527 ORB SLAM Code Reading
