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);
....
}
リモートでPCの電源をオン
研究室のPCを自宅から操作したい場合、リモートデスクトップソフトを使います。UbuntuではAnyDeskを使っていますが、PCの電源が切れているならまだしも、スリープの状態からでもAnyDeskが利用できません。これはTeamViewerなど他のリモートデスクトップソフトでも同じようです。そこでWOL(wake on lan)という機能を利用すれば完全にPCの電源が切れた状態からでも遠隔でPCを起動させることができます。おそらく有線接続のPCしか不可だと思います。
BIOSの設定
まずはPCのBIOS(ASRock)の設定画面を開いて以下の画像のように設定しました。PCによってはWOLに非対応のものもあります。起動>内蔵LANから起動、アドバンス>ACPI設定>PCIEデバイス電源オン、をそれぞれ有効にして設定を保存します。
管理者ユーザでtelnet接続
研究室には有線のルーター(Yamaha RTX830)があります。ルーターの設定画面(http://192.168.0.1)にブラウザでアクセスします。ログインを求められ管理ユーザーとしてログインします。管理ユーザのパスワードが設定されていない場合は以下のページを参考に設定します。
network.yamaha.com
ログインできることが確認できたらtelnetで接続します。terminalを開いて以下のコマンドを実行します。文字化けを避けるためnkfコマンドにパイプします。
telnet 192.168.0.1 | nkf -uw
ログインできたら
administrator
と入力して管理ユーザでログインします。あとは公式のサイトにしたがってブラウザからwolが実行できるようにします。
network.yamaha.com
WAN越しに接続
ここまでブラウザでルータのアドレスを開いてユーザ名をwolでログインすると、LAN内であればWOL機能が使えます。しかし、WAN越しには接続できません。いろんなサイトを見て試行錯誤しましたが、このサイトを参考にルーターの設定を行うとうまく行きました。natやip filterなど難しかったです。
qiita.com
これで自宅からでも接続できると思いブラウザでそのままアクセス(http://グローバルアドレス:44444)を試みましたが、なぜかできませんでした。しかし大学のVPNに接続した状態であれば、開放したポートへのアクセスが可能になっていました。大学のVPNに接続後、http://グローバルアドレス:44444を開いて、ユーザ名wolでログインすることで自宅からでも研究室のPCの電源を入れられるようになりました。
複数カメラを使った人体運動計測
卒業論文でORB_SLAMを使った論文を書くことにしました。
卒論の内容
ORB_SLAMではカメラ映像からカメラの軌跡を求めることができます。それを使ってモーションキャプチャの代用にしようという内容で書きます。論文の内容を考えていたら、偶然同じような内容で論文を書いている人を発見してしまいました。
http://www.hcilab.jp/~kono/Paper/2016/int2017tai.pdf
卒業研究なので全く同じ研究にしてはいけませんが，まずは複数カメラを使ってSLAMができるか試してみます。
実験内容についてです。人体にスマホを複数台取り付けます。各スマホで同時に動画を撮影して、PCに全ての動画を入力してSLAMを行います。SLAMでは各カメラの姿勢と軌跡を計算して、一つの座標上に表示します。データの出力も行い、pythonを使ってグラフ化も行います。
カメラのキャリブレーション
まず撮影済みの動画で、キャリブレーションファイルを作成します。一分間ほどキャリブレーション用の画像を撮影した動画を用意します。次に撮影済みの動画をROS上でトピックに配信するために以下のパッケージを入れます。
$ sudo apt install ros-kinetic-video-stream-opencv
video-stream-opencvを使って動画をトピックに配信するlaunchファイルを作成します。以下のようなlaunchファイル(videoFile_publish.launch)を作成しました。
<?xml version="1.0"?> <launch> <!-- launch video stream --> <include file="$(find video_stream_opencv)/launch/camera.launch" > <arg name="camera_name" value="camera" /> <arg name="video_stream_provider" value="/home/robolab/ビデオ/iphone/videoName.MOV" /> <arg name="set_camera_fps" value="30"/> <!-- set buffer queue size of frame capturing to --> <arg name="buffer_queue_size" value="1000" /> <!-- throttling the querying of frames to --> <arg name="fps" value="30" /> <arg name="frame_id" value="videofile_frame" /> <arg name="flip_horizontal" value="false" /> <arg name="flip_vertical" value="false" /> <arg name="loop_videofile" value="false" /> <arg name="visualize" value="false" /> </include> </launch>
以下のコマンドで撮影済みの動画をROSトピックに流すことができます。
roslaunch videoFile_publish.launch
あとは自分が過去に書いた以下の記事を参考にしてキャリブレーションを行ってください。以下の記事ではストリーム配信をROSトピックに流して行っていますが、そこが撮影済みの動画に置き換わっただけです。
sh090.hatenablog.com
ROSのcamera_calibrationから出力されたファイルを開いて、元からあるキャリブレーションファイル(~/ORB_SLAM3/Examples/Monocular/TUM1.yaml)を元に新しいキャリブレーションファイルを作成します。
以下がiPhoneXで作成したキャリブレーションファイル(iphone_recordVideo_calib.yaml)の前半部を抜粋した内容です。
%YAML:1.0 #------------------------------------------- # Camera Parameters. Adjust them! #------------------------------------------ Camera.type: "PinHole" # Camera calibration and distortion parameters (OpenCV) Camera.fx: 1828.896286 Camera.fy: 1828.370849 Camera.cx: 1002.453202 Camera.cy: 502.978252 Camera.k1: 0.192628 Camera.k2: -0.418260 Camera.p1: -0.004371 Camera.p2: 0.014453 Camera.k3: 0.000000 # Camera frames per second Camera.fps: 30.0 # Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) Camera.RGB: 1 # Camera resolution Camera.width: 1920 Camera.height: 1080
映像の結合
ORB_SLAM2では、1つ目の映像を流し終わった後、２つ目の映像を流す操作を行えば再び処理してくれました。しかしORB_SLAM3の方では1つ目を流し終わり、その後２つ目の映像を流す処理を行ってもフリーズしたままです。ORB_SLAM3の方が精度が高いので、解決策を考え、二つの映像を結合して一つの映像とすれば処理してくれました。映像の切り替わりの際に自己位置はロストしますが、しばらくして復帰してくれます。
以下の内容でvideo_mergeList.txtというファイルを作り、
file 'video1.AVI' file 'video2.AVI'
このコマンドで二つの映像を結合しました。
$ ffmpeg -f concat -i video_mergeList.txt -c copy output.AVI
マップの統合について
ORB_SLAMでは、カメラの軌跡とマップポイント(特徴点を三次元上に配置したもの)などを一つの座表上に表し、それをマップといいます。以下の動画は研究室内で映像の撮影をしてSLAMを行ったものです。同時刻に撮影したものではありませんが、別で撮影した二つの映像をORB_SLAMに入力しています。自己位置を失うたびに新しいマップが作成されます。各マップの中に共通の特徴点が見つかれば、マップの統合が行われます。共通の特徴点を作るために、入力した映像ではどちらも最後に自分の机の上を細かく撮影しました。
youtu.be
実験では撮影済みの動画を使ってSLAMを行います。その代わり動画をスロー再生で入力でき、ゆっくり処理を行うため自己位置をロストしにくくなります。
マップ(座標)統合前と統合後の画像です。二つの軌跡が同じ座標で同じスケールに調整されます。
ちなみにVB5500Proという中国製の格安スマホで撮影した映像だとすぐに自己位置をロストしてしまいました。屋外でも屋内でも撮影して試しましたが、うまくいきません。フレームレートが低いのと解像度が低いのが原因かと思います。フレームレートは機器に依存するので上げられませんが、VB5500Proの場合25fpsとなっていました。iPhone Xでは60fpsの動画が撮影できます。あと、2013年発売のiPhone5s以降ではスローモーション撮影が可能で240fpsで撮影が行えます。
二つのiPhoneでSLAM(世代の異なるiPhone)
上記の検証は、同じ一台のiPhoneXを用いて入力動画を撮影しました。今度は二台のiPhone(iPhoneX,iPhone8)を使って検証を行います。同じ世代のiPhoneを用意したかったのですが、この時はありませんでした。iPhoneXとiPhone8を使って２つの入力映像を用意します。そのまま２つの映像を結合しようとすると、うまく行きませんでした。どうやら映像ファイルの音声が原因のようで、以下のコマンドで音声を削除しました。
ffmpeg -i iphoneX_video.MOV -vcodec copy -an iphoneX_video_silent.MOV ffmpeg -i iphone8_video.MOV -vcodec copy -an iphone8_video_silent.MOV
その後ffmpegを使えば２つの映像を結合できました。結合された映像を使ってSLAMを試します。左が通常撮影、右がスロー撮影したときの結果です。通常撮影もスロー撮影も、撮影時には大体同じ内容のカメラ映像となるようにしました。
あとは論文に書きます。
ORB_SLAM3 軌跡データの取得とグラフ化
ORB_SLAM3で軌跡データの取得とグラフ化を行います。
gedit KeyFrameTrajectory.txt
を実行します。geditで「検索」>「置換」を選択します。置換する文字に半角空白「 」を入力し、置換後の文字にカンマ「,」を入力して全て置換します。さらにファイル名をKeyFrameTrajectory.csvに変更します。これでエクセルで読み込めるようになりました。次にこのデータを、pythonで三次元グラフ化していきます。エクセルでは三次元の散布図グラフは作れません。pythonで三次元のグラフを作成するためmatplotlibというpythonライブラリを利用します。matplotlibの導入方法については以下のページを参考にしました。
ai-gaminglife.hatenablog.com
pythonで書いたプログラム(orb_slam_3d_graph.py)は以下のようなものです。
csvを横１行ずつ読み込んで各データごとの配列を用意し、グラフにプロットします。自分の卒業研究についての話ですが、csvファイル内では複数のスマホの軌跡のデータも、一つの時間軸として統合されています。どこで2つ目の軌跡に切り替わったか分かりづらいですが、別々の軌跡としてグラフ化する必要があるため工夫が必要です。三次元の点を線でつないで軌跡のグラフ化が行われますが、プログラム中では時間の変化量が一秒以上あった場合には線で繋がないようにしています。pythonで実行する際には日本語のコメントを削除してから実行してください。
import csv import pprint import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import math plotData_x=[]; plotData_y=[]; plotData_z=[]; plotData_time=[]; fig = plt.figure() ax = Axes3D(fig) #Xの回転行列 def rotate_x(deg): r = np.radians(deg) R_x = np.array([[1, 0, 0], [0, np.cos(r), np.sin(r)], [0, -np.sin(r), np.cos(r)]]) return R_x #Yの回転行列 def rotate_y(deg): r = np.radians(deg) R_y = np.array([[np.cos(r), 0, -np.sin(r)], [0, 1, 0], [np.sin(r), 0, np.cos(r)]]) return R_y #Zの回転行列 def rotate_z(deg): r = np.radians(deg) R_z = np.array([[np.cos(r), np.sin(r), 0], [-np.sin(r), np.cos(r), 0], [0, 0, 1]]) return R_z def plot_Data(): global ax #表示するx,y,zの範囲 ax.set_xlim3d(0, 1400.0) ax.set_ylim3d(0, 1400.0) ax.set_zlim3d(0, 1400.0) #x,y,zのラベル ax.set_xlabel('x(m)') ax.set_ylabel('y(m)') ax.set_zlabel('z(m)') #時間の変化量を計算する準備 plotData_time.append(0.00) with open('KeyFrameTrajectory.csv') as f: reader = csv.reader(f) #csvファイルを横１行ずつ読み込むforループ # 読み込んだ横１行の各データ(timestamp,x,y,z,...)を配列(row)にする for row in reader: #row[0]にtimestamp、row[4~7]にクウォータニオンが入っている #配列(rawArray)にx,y,zの値を入れる際には文字列から数値化(float)する rawArray = np.array((float(row[1]),float(row[2]),float(row[3]))) #前の１行との時間の変化量を計算 timeDiff=np.abs(float(row[0])-plotData_time[-1]) #時間の変化量が一秒以上ある場合、もしくは二つ目の軌跡に切り替わった指定秒数の場合に実行 if timeDiff>1 or float(row[0])==78.786749: #配列に入っているデータをグラフ上に一度プロット ax.plot(plotData_x, plotData_y,plotData_z,color = "b",linestyle = "solid") #配列を一度空にする plotData_x.clear() plotData_y.clear() plotData_z.clear() #回転行列を計算 rotX = rotate_x(10) rotY = rotate_y(10) rotZ = rotate_z(10) rotatedArray = np.dot(rotX,rawArray) rotatedArray = np.dot(rotY,rotatedArray) rotatedArray = np.dot(rotZ,rotatedArray) #x,y,zの各値はスケールと平行移動を調整して配列に代入。 plotData_time.append(float(row[0])) plotData_x.append(rotatedArray[0]*630+30) plotData_y.append(rotatedArray[1]*630) plotData_z.append(rotatedArray[2]*630+200) #ループ終了後配列に残っている値をグラフにプロット ax.plot(plotData_x, plotData_y, plotData_z,color='r', linestyle = "solid") #m単位の目盛り文字列を指定。以下のコードを書かかなくてもcm単位の目盛りが表示される。 ax.set_xticklabels(["0", "2", "4", "6", "8", "10", "12", "14"]) ax.set_yticklabels(["0", "2", "4", "6", "8", "10", "12", "14"]) ax.set_zticklabels(["0", "2", "4", "6", "8", "10", "12", "14"]) #最初に呼び出される関数 def main(): plot_Data() #グラフの凡例を表示 plt.legend(loc="lower left", fontsize=15) plt.show() if __name__ == '__main__': main()
自分はORB_SLAMのデータ出力に関するプログラムを若干変更しています。
sh090.hatenablog.com
動画入力時のUnixTimeでなく、動画入力開始からの経過時間が記録されるようにしているため、上記のプログラムはそのままでは使えないかもです。if文の箇所だけ抜けばプログラムの変更をしてなくても使えると思います。
プログラムのファイル(orb_slam_3d_graph.py)とKeyFrameTrajectory.csvを同じディレクトリに置いてTerminalで以下を実行することでグラフ化できます。
python orb_slam_3d_graph.py
上記のコードだと一部の関数(list.clear)がpython3.3以降でしか実行できなくて別の環境だとエラーが発生したので、一部変更したものを載せておきます。(3d_graph_for_elapsedTime.py)
# coding: UTF-8 import csv import pprint import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import math plotData_x=[]; plotData_y=[]; plotData_z=[]; plotData_time=[]; fig = plt.figure() ax = Axes3D(fig) def rotate_x(deg): r = np.radians(deg) R_x = np.array([[1, 0, 0], [0, np.cos(r), np.sin(r)], [0, -np.sin(r), np.cos(r)]]) return R_x def rotate_y(deg): r = np.radians(deg) R_y = np.array([[np.cos(r), 0, -np.sin(r)], [0, 1, 0], [np.sin(r), 0, np.cos(r)]]) return R_y def rotate_z(deg): r = np.radians(deg) R_z = np.array([[np.cos(r), np.sin(r), 0], [-np.sin(r), np.cos(r), 0], [0, 0, 1]]) return R_z def plot_plarail_type1(): global ax x1 = np.linspace( -21.4, 21.4, 70) y1 = [21.4] * 70 rad = np.linspace( math.pi/2, math.pi*3/2, 40) x2 = -21.4*np.cos(rad)+21.4 y2 = 21.4*np.sin(rad) x3 = np.linspace( -21.4, 21.4, 70)*-1 y3 = [-21.4] * 70 rad = np.linspace( math.pi*3/2, math.pi*5/2, 40) x4 = -21.4*np.cos(rad)-21.4 y4 = 21.4*np.sin(rad) x=np.hstack([x1,x2,x3,x4]) y=np.hstack([y1,y2,y3,y4]) ax.plot(x, y, np.zeros(len(x)),color = "r", label = "Plarail") def plot_Data(): global ax global plotData_x global plotData_y global plotData_z ax.set_xlim3d(0, 1400.0) ax.set_ylim3d(0, 1400.0) ax.set_zlim3d(0, 1400.0) ax.set_xlabel('x(m)') ax.set_ylabel('y(m)') ax.set_zlabel('z(m)') plotData_time.append(0.00) with open('KeyFrameTrajectory.csv') as f: reader = csv.reader(f) for row in reader: if row[0] == "-----------------------": print("\nKeyFrameTrajectory.csvを編集してください。マップの区切り文字(-----------------------)が含まれています。\n") break rawArray = np.array((float(row[1]),float(row[2]),float(row[3]))) timeDiff=np.abs(float(row[0])-plotData_time[-1]) rotX = rotate_x(10) rotY = rotate_y(10) rotZ = rotate_z(10) rotatedArray = np.dot(rotX,rawArray) rotatedArray = np.dot(rotY,rotatedArray) rotatedArray = np.dot(rotZ,rotatedArray) if timeDiff>2 or float(row[0])==78.786749: ax.plot(plotData_x, plotData_y,plotData_z,color = "b",linestyle = "solid") plotData_x=[] plotData_y=[] plotData_z=[] plotData_time.append(float(row[0])) plotData_x.append(rotatedArray[0]*630+30) plotData_y.append(rotatedArray[1]*630) plotData_z.append(rotatedArray[2]*630+200) ax.plot(plotData_x, plotData_y, plotData_z,color='r', linestyle = "solid") ax.set_xticklabels(["0", "2", "4", "6", "8", "10", "12", "14"]) ax.set_yticklabels(["0", "2", "4", "6", "8", "10", "12", "14"]) ax.set_zticklabels(["0", "2", "4", "6", "8", "10", "12", "14"]) def main(): plot_Data() plt.legend(loc="lower left", fontsize=15) plt.show() if __name__ == '__main__': main()
あとORB_SLAMのプログラム変更前の出力データ(時間軸がUnixTime)に対応したソースも載せておきます。(3d_graph_for_unixTime.py)
import csv import pprint import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D import math plotData_x=[]; plotData_y=[]; plotData_z=[]; plotData_time=[]; fig = plt.figure() ax = Axes3D(fig) def rotate_x(deg): r = np.radians(deg) R_x = np.array([[1, 0, 0], [0, np.cos(r), np.sin(r)], [0, -np.sin(r), np.cos(r)]]) return R_x def rotate_y(deg): r = np.radians(deg) R_y = np.array([[np.cos(r), 0, -np.sin(r)], [0, 1, 0], [np.sin(r), 0, np.cos(r)]]) return R_y def rotate_z(deg): r = np.radians(deg) R_z = np.array([[np.cos(r), np.sin(r), 0], [-np.sin(r), np.cos(r), 0], [0, 0, 1]]) return R_z def plot_Data(): global ax global plotData_x global plotData_y global plotData_z ax.set_xlim3d(0, 1200.0) ax.set_ylim3d(0, 1200.0) ax.set_zlim3d(0, 1200.0) ax.set_xlabel('x(m)') ax.set_ylabel('y(m)') ax.set_zlabel('z(m)') plotData_time.append(0.00) with open('KeyFrameTrajectory_unixTime.csv') as f: reader = csv.reader(f) for row in reader: rawArray = np.array((float(row[1]),float(row[2]),float(row[3]))) timeDiff=np.abs(float(row[0])-plotData_time[-1]) if timeDiff>5 and len(plotData_time)>1: print(len(plotData_x)) ax.plot(plotData_x, plotData_y,plotData_z,color = "b",linestyle = "solid") plotData_x=[] plotData_y=[] plotData_z=[] rotX = rotate_x(90) rotY = rotate_y(-3) rotZ = rotate_z(10) rotatedArray = np.dot(rotX,rawArray) rotatedArray = np.dot(rotY,rotatedArray) rotatedArray = np.dot(rotZ,rotatedArray) plotData_time.append(float(row[0])) plotData_x.append(rotatedArray[0]*500+30) plotData_y.append(rotatedArray[1]*700+600) plotData_z.append(rotatedArray[2]*700+200) ax.plot(plotData_x, plotData_y, plotData_z,color='r', linestyle = "solid") ax.set_xticklabels(["0", "2", "4", "6", "8", "10", "12"]) ax.set_yticklabels(["0", "2", "4", "6", "8", "10", "12"]) ax.set_zticklabels(["0", "2", "4", "6", "8", "10", "12"]) def main(): plot_Data() plt.legend(loc="lower left", fontsize=15) plt.show() if __name__ == '__main__': main()
MacBook Ubuntuでwifiを認識しない時の対処法
自宅にある古いMacBookでUbuntuを起動するとwifiを認識しませんでした。検索すると多くのサイトがヒットしたので、ubuntuを入れたときにありがちな問題のようです。しかし同じ症状でもPCによって解決策が異なるので少々手間取りました。
qiita.com
最終的に上記のサイトを参考にするとうまくいきました。
まずwifiモジュールの種類を何かしらで確認します。自分の場合はMacOSとUbuntuのデュアルブートのため一度MacOSを起動させて確認しました。
MacOSでのWifiモジュールの確認の仕方ですが、デスクトップでOptionをおしながら画面左上のりんごのマークをクリックして「システム情報」、ネットワークタブの「Wifi」をクリックすれば確認できます。
Broadcom BCM43xx 1.0であれば、以下の５つのdebファイルをインストールします。
libfakeroot_1.20-3
fakeroot_1.20-3
module-init-tools_15-0
dkms_2.2.0.3-1.1
bcmwl-kernel-source_6.30.223.248
参考にしたサイト内にあるURLはリンクが切れていて使えませんでした。今回はルーターからLANケーブルを一度抜いて、有線接続してapt-getを使って入れました。
sudo apt-get update sudo apt-get install module-init-tools sudo apt-get install dkms //同時にlibfakeroot,fakerootも入った sudo apt-get install bcmwl-kernel-source
これでWifiを認識するようになりました。
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
スマホ二台を使ったステレオカメラでRTAB-MAPしようとして失敗
androidスマホ(BV5500)を二台横に並べてステレオカメラをつくりRTAB-MAPを行おうとしました。
3Dプリンターを使ってスマホを横に２つ並べる枠を作りました。
androidからストリーム配信した動画をPCのROS上で受信していきます。画像の受信は「video_stream_opencv」というパッケージを使って行います。二台同時に受信するため、launchファイルを作成します。
$ cd /opt/ros/kinetic/share/video_stream_opencv/launch/ $ sudo cp camera.launch stereo_camera.launch $ sudo gedit stereo_camera.launch
テキストエディタで32行目を以下に書き換えます。
<group ns="/stereo/$(arg camera_name)">
$ mkdir ~/ros_launch_files && cd ~/ros_launch_files $ touch stereo_rtsp_VB5500.launch $ gedit stereo_rtsp_VB5500.launch
テキストエディタで以下の内容をコピペして保存します。
<?xml version="1.0"?>
<launch>
<include file="$(find video_stream_opencv)/launch/stereo_camera.launch" >
<!-- node name and ros graph name -->
<arg name="camera_name" value="right" />
<arg name="video_stream_provider" value="rtsp://192.168.0.22:5540/ch0" />
<arg name="buffer_queue_size" value="1000" />
<arg name="fps" value="30" />
<arg name="frame_id" value="rtsp_frame_right" />
<arg name="camera_info_url" value="file:///home/user_name/calibrate_file/VB5500_stereo_streamVideo_right.yaml" />
<arg name="flip_horizontal" value="false" />
<arg name="flip_vertical" value="false" />
<arg name="visualize" value="true" />
</include>
<include file="$(find video_stream_opencv)/launch/stereo_camera.launch" >
<!-- node name and ros graph name -->
<arg name="camera_name" value="left" />
<arg name="video_stream_provider" value="rtsp://192.168.0.28:5540/ch0" />
<arg name="buffer_queue_size" value="1000" />
<arg name="fps" value="30" />
<arg name="frame_id" value="rtsp_frame_left" />
<arg name="camera_info_url" value="file:///home/user_name/calibrate_file/VB5500_stereo_streamVideo_left.yaml" />
<arg name="flip_horizontal" value="false" />
<arg name="flip_vertical" value="false" />
<arg name="visualize" value="true" />
</include>
</launch>受信に成功したらキャリブレーションファイルを作成します。
sh090.hatenablog.com
以前書いたページを参考に、以下のコマンドを実行してキャリブレーションを行います。yamlファイルが２つ作成されます。さっき作ったlaunchファイルのcamera_info_urlにファイルの保存先を入力します。
$ rosrun camera_calibration cameracalibrator.py --approximate 0.1 --size 8x6 --square 0.025 right:=/right/image_raw left:=/left/image_raw right_camera:=/right left_camera:=/left
<arg name="camera_info_url" value="file:///home/user_name/calibrate_file/VB5500_stereo_streamVideo_left.yaml" />
ビジュアルオドメトリのライブラリであるviso2(https://github.com/srv/viso2)を導入します。
$ cd ~/ros_ws/src $ git clone https://github.com/srv/viso2.git $ cd .. $ catkin_make
RTAB-MAPを実行するlaunchファイルを作成します。
cd ~/ros_launch_files touch stereo_RTAB.launch gedit stereo_RTAB.launch
テキストエディタで以下の内容をコピペして保存します。
<launch>
<!-- Run the ROS package stereo_image_proc for image rectification and disparity computation -->
<group ns="stereo">
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<param name="approximate_sync" value="true"/>
</node>
<!-- Disparity to depth -->
<node pkg="nodelet" type="nodelet" name="disparity2depth" args="standalone rtabmap_ros/disparity_to_depth"/>
</group>
<!-- Odometry: Run the viso2_ros package -->
<node pkg="viso2_ros" type="stereo_odometer" name="stereo_odometer" output="screen">
<remap from="image" to="image_rect"/>
<param name="base_link_frame_id" value="/base_link"/>
<param name="odom_frame_id" value="/odom"/>
<param name="ref_frame_change_method" value="1"/>
<param name="queue_size" type="int" value="20"/>
<param name="approximate_sync" type="bool" value="true"/>
</node>
<group ns="rtabmap">
<!-- Visual SLAM -->
<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
<param name="subscribe_depth" type="bool" value="true"/>
<param name="subscribe_laserScan" type="bool" value="false"/>
<remap from="rgb/image" to="/stereo/left/image_rect"/>
<remap from="rgb/camera_info" to="/stereo/left/camera_info"/>
<remap from="depth/image" to="/stereo/depth"/>
<remap from="odom" to="/stereo_odometer/odometry"/>
<param name="frame_id" type="string" value="/base_link"/>
<param name="queue_size" type="int" value="40"/>
<param name="approx_sync" type="bool" value="true"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="wait_for_transform_duration" type="double" value="5.3"/>
<param name="Vis/MinInliers" type="string" value="12"/>
</node>
</group>
</launch>二台のandroidからストリーム配信を開始して実行します。
$ cd ~/ros_launch_files $ roslaunch stereo_rtsp_VB5500.launch $ source ~/ros_ws/devel/setup.bash $ roslaunch stereo_RTAB.launch
ここまでも試行錯誤しましたがエラーが発生しました。
[ WARN] [1599296766.785042898]: Could not get transform from /base_link to rtsp_frame after 5.300000 seconds (for stamp=1599296761.330938)! Error="canTransform: source_frame rtsp_frame does not exist.. canTransform returned after 5.30874 timeout was 5.3.". [ERROR] [1599296766.785148236]: TF of received image 0 at time 1599296761.330938s is not set! [ERROR] [1599296766.785192122]: Could not convert rgb/depth msgs! Aborting rtabmap update...
TF関連の問題っぽいです。とりあえずTFのチュートリアルをやりながら原因を探りたいと思います。
