ORB_SLAM3 データ出力に関するプログラム変更
ORB_SLAM3のデータ出力に関して、いくつか不便な点がありプログラムを書き換えたので紹介します。プログラムを書き換えた後、変更を適用するには再度ORB_SLAMをビルドする必要があります。
CSV出力
まずプログラムを書き換えて、最初からCSVファイルとして出力を行うようにしました。データ出力を行なっているプログラムが書かれているのは~/ORB_SLAM3/src/System.ccというファイルです。この中のSaveKeyFrameTrajectoryTUMという関数(540行目)でデータ出力をしています。空白で区切られているので、それをカンマで区切るように変更します。
// 565行目を以下のように変更 f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << "," << t.at<float>(0) << "," << t.at<float>(1) << "," << ...
記録される時間をUnixTimeから経過秒数に
通常だと動画入力時のUnixTimeが記録されますが、動画入力開始からの経過時間を記録するように変更しました。再びSaveKeyFrameTrajectoryTUM(540行目)を書き換えます。
// 540行目のSaveKeyFrameTrajectoryTUM関数の中に以下を追加 double firstTime=0.0; // 553行目のfor(size_t i=0; i<vpKFs.size(); i++)の中に以下を追加 if(i==0.0){ firstTime=pKF->mTimeStamp; } // 565行目の出力部を以下のように変更 f << setprecision(6) << (pKF->mTimeStamp - firstTime) << setprecision(7) << "," << t.at<float>(0) << ...
スロー動画入力時の時間軸を実時間に
次に、時間を記録する箇所にも多少手を加えました。研究ではスロー撮影された動画を入力しています。具体的には240fpsで動画撮影を行い、30fpsで入力しています。そのため出力ファイルの時間も実時間の8倍ゆっくり進んでいます。以下のように書き換えることで、スローで動画入力しても記録される時間は実時間と合うようにしました。
// 565行目の出力部を以下のように変更 f << setprecision(6) << (pKF->mTimeStamp - firstTime)*30/240 << setprecision(7) << ...
全てのマップ上の軌跡を出力
さらに軌跡の出力を行う箇所にもう少し手を加えています。ORB_SLAMでは自己位置を失うたびに新しいマップが作成されます。ですが軌跡の出力が行われるのは、ORB_SLAMが終了する直前に読み込んでいた最後のマップ中にある軌跡のみです。つまり入力動画の終了直前で自己位置を失うと、KeyFrameTrajectory.txtは空っぽのファイルになります。複数のマップが作成されても、それらのマップが結合され一つのマップになれば問題ありません。ですが、最後までマップの統合が行われず、複数のマップが存在した場合、軌跡の出力が行われるのは、より後に作成された最後のマップ中の軌跡のみです。
これでは不便なので全てのマップの軌跡が出力されるように手を加えます。最終的にSaveKeyFrameTrajectoryTUMは以下のようになりました。
void System::SaveKeyFrameTrajectoryTUM(const string &filename) { cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl; double firstTime=0.0; vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames(); vector<Map*> allMaps=mpAtlas->GetAllMaps(); sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId); ofstream f; f.open(filename.c_str()); f << fixed; cout << endl << "allMaps Size : " << allMaps.size() << endl; for(size_t i=0; i<allMaps.size(); i++) { Map* map = allMaps[i]; vector<KeyFrame*> KFs=map->GetAllKeyFrames(); cout << endl << "KF Size : " << KFs.size() << endl; sort(KFs.begin(),KFs.end(),KeyFrame::lId); for(size_t i=0; i<KFs.size(); i++) { KeyFrame* pKF = KFs[i]; // pKF->SetPose(pKF->GetPose()*Two); // if(pKF->isBad()) // continue; if(i==0.0){ firstTime=pKF->mTimeStamp; } cv::Mat R = pKF->GetRotation().t(); vector<float> q = Converter::toQuaternion(R); cv::Mat t = pKF->GetCameraCenter(); f << setprecision(6) << (pKF->mTimeStamp - firstTime)*30/240 << setprecision(7) << "," << t.at<float>(0) << "," << t.at<float>(1) << "," << t.at<float>(2) << "," << q[0] << "," << q[1] << "," << q[2] << "," << q[3] << endl; } f << "-----------------------" << endl; } f.close(); }
リアルタイムでの自己位置の取得
リアルタイムでの自己位置の取得についてです。これをうまく利用すれば、ORB_SLAMを使った経路追従のようなことができるかもです。やろうとしてたのがだいぶ前でうまくいったかいってないかも覚えていませんが、以下のページを参考にしました。この書き換えを行なったのはORB_SLAM2の方ですが、多分ORB_SLAM3でもいけると思います。
github.com
~/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_mono.ccの79行目のImageGrabber::GrabImage関数です。この関数はフレーム画像が一枚入力されるたびに呼び出されます。この関数内の93行目でTrackMonocularが呼ばれています。TrackMonocularの戻り値はカメラの位置と姿勢が含まれた変数になります。この変数をデフォルトでは受けとっていませんが、書き換えればカメラの位置と姿勢を取り出すことができます。試したのがだいぶ前で覚えていませんが、カメラの現在地をリアルタイムでROSトピックに配信するような仕様に変更しようとしてました。以下のコードに書き換えました。これで確か自己位置をトピックに配信することはできてたと思います。例えばRaspberry piにwebカメラを取り付けてネットワーク越しに映像をROSトピックに配信し、PCから自己位置を取得すれば経路の作成と追従ができると思います。
//ros_mono.ccの先頭に以下を追加 #include <string> #include <fstream> #include <geometry_msgs/PoseStamped.h> #include <tf/tf.h> #include <tf/transform_datatypes.h> #include"../../../include/Converter.h" using namespace std; // imageGrabberクラスを以下のように変更 class ImageGrabber { public: ImageGrabber(ORB_SLAM2::System* pSLAM,ros::NodeHandle nodeHandler):mpSLAM(pSLAM){ pose_pub = nodeHandler.advertise<geometry_msgs::PoseStamped>("orb_pose", 100); writing_file.open("Trajectory_log.txt", std::ios::trunc); } void GrabImage(const sensor_msgs::ImageConstPtr& msg); ORB_SLAM2::System* mpSLAM; ros::Publisher pose_pub; std::ofstream writing_file; }; ... void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) { ... cv::Mat Tcw = ImageGrabber::mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); geometry_msgs::PoseStamped pose; pose.header.stamp = ros::Time::now(); pose.header.frame_id ="map"; if(!Tcw.empty()){ ImageGrabber::writing_file << Tcw << std::endl; cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t(); // Rotation information cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3); // translation information vector<float> q = ORB_SLAM2::Converter::toQuaternion(Rwc); tf::Transform new_transform; new_transform.setOrigin(tf::Vector3(twc.at<float>(0, 0), twc.at<float>(0, 1), twc.at<float>(0, 2))); tf::Quaternion quaternion(q[0], q[1], q[2], q[3]); new_transform.setRotation(quaternion); tf::poseTFToMsg(new_transform, pose.pose); ImageGrabber::pose_pub.publish(pose); } }
以下の論文ではORB_SLAMを使った経路追従をやっています。
docsplayer.net
以下のサイトではマップポイントをトピックに配信し、経路計画を立てています。
qiita.com
一度でも自己位置を失うとマップの座標が変わってしまうため、マップの保存と読み込みとか出来れば事前に作成した経路を追従できるので出来ることは増えそうですね。試していませんが海外の方がマップの保存と読み込み機能を追加したものをgithubで公開してたと思います。時間があったらやってみたかったなあ。
- 関数探し
以下は上記の変更を行うために関数を探したときのやり方です。まずは軌跡データの保存を行っている関数を探しました。SLAM終了時にterminalに以下の文字が表示されます。
Saving keyframe trajectory to KeyFrameTrajectory.txt ...
この文字列が含まれたファイルを文字列検索します。
$ fgrep -rn "Saving keyframe trajectory to" ./*
ORB_SLAM3/src/System.ccの542行目と698行目に含まれていることが確認できました。このようにterminalに表示されるログを文字列検索して関数を探せば見つかります。またROS上で単眼カメラを使ってORB_SLAMを実行すると以下のファイルが実行されます。
ORB_SLAM3/Examples/ROS/src/ros_mono.cc
ros_mono.ccを開いてmain関数からプログラムを順に読んでいき、それっぽい関数名を探しても目的の関数が見つかったりします。そのファイルを読むとSLAM終了時に、70行目の以下の関数が実行されていることがわかります。
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");呼び出し先を辿って、ORB_SLAM3/src/System.ccの542行目のSystem::SaveKeyFrameTrajectoryTUM関数により軌跡データの保存が行われていることがわかりました。その関数の中身を読んでみると544行目に
vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();
とあり、キーフレームを取得しているっぽい関数を見つけました。呼び出し先の関数はORB_SLAM3/src/Atlas.ccの151行目Atlas::GetAllKeyFrames関数です。この関数の戻り値は
return mpCurrentMap->GetAllKeyFrames();
となっておりORB_SLAM3/src/Map.ccの147行目のMap::GetAllKeyFrames関数が呼ばれます。今度はこの関数の戻り値をみると
return vector<KeyFrame*>(mspKeyFrames.begin(),mspKeyFrames.end());
となっています。戻り値の型であるstd::vectorはシーケンスコンテナ？みたいです。自分はC++は知らないのでコンテナがなんなのか知りませんが配列やリストのようなものですかね。vectorのコンストラクタに渡されている変数mspKeyFramesにキーフレームのデータが格納されているみたいです。この変数の型はstd::set
mspKeyFramesにどのようなことが行われているのか調べるため
fgrep -rn "mspKeyFrames." ./*
で文字列検索をかけると./src/Map.ccの69行目に
mspKeyFrames.insert(pKF);
とあることがわかりました。どうやらこのコードでキーフレームの挿入が行われているみたいです。このコードが含まれているMap::AddKeyFrame関数を以下のように書き換えました。src/Map.ccとinclude/Map.hを書き換えてます。キーフレームの追加が行われたときにキーフレームの合計数を表示しています。keyframe cullingが行われてキーフレーム数が増減している様子をリアルタイムで確認できます。
void Map::AddKeyFrame(KeyFrame *pKF)
{
kfSize = mspKeyFrames.size();
cout << "KeyFrame inserted:" << pKF->mnId << ":size" << kfSize << endl;
unique_lock<mutex> lock(mMutexMap);
....
}
