前回までのえんせき
Ubuntu16.04上のROS KineticでORB_SLAM2を動かしてiPhoneで撮影した動画を取り込んでみた。
つまりなにしたの?
USBカメラのキャリブレーションをやって、机の周りの主にミクさんをスキャンしてみた。
youtu.be
USBカメラののパッケージをインストール
sudo apt-get install ros-kinetic-usb-cam
本当はキャリブレーションが必要だけど、とりあえず動かしてみる。
# Terminal1 roscore # Terminal2 cd ~/orb_slam2_ws rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml # Terminal3 rosparam set /usb_cam/pixel_format yuyv rosrun usb_cam usb_cam_node /usb_cam/image_raw:=/camera/image_raw
うごいたー!
これはひどい
キャリブレーションしてみる
具体的には使うカメラ用のYamlファイルを用意する
単眼カメラのキャリブレーションは
camera_calibration/Tutorials/MonocularCalibration - ROS Wiki
こちら。
早速インストール
rosdep install camera_calibration
よしやるか!
キャリブレーション用のプレートはこちら
http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration?action=AttachFile&do=view&target=check-108.pdfattachment:check-108.pdf of camera_calibration/Tutorials/MonocularCalibration - ROS Wiki
え、でかい・・・
PTAMのキャリブレーションパターンでもできるらしい。それがこちら。
https://github.com/ethz-asl/ethzasl_ptam/blob/master/ptam/calib_pattern.pdfgithub.com
セブンに印刷しに行く。
# Terminal1 roscore # Terminal2 rosrun camera_calibration cameracalibrator.py --size 11x7 --square 0.02 image:=/camera/image_raw # Terminal3 rosparam set /usb_cam/pixel_format yuyv rosparam set /usb_cam/image_width 1920 rosparam set /usb_cam/image_height 1080 # 僕が持っているカメラにサイズとかを合わせてます rosrun usb_cam usb_cam_node /usb_cam/image_raw:=/camera/image_raw
square のパラメータが0.02になっているのは市松模様の1辺の長さが0.02m→2cmってことなので一応印刷したやつを測っておく。
ぴったりだった。さすがフジゼロックス!
今回使ったカメラはこれ。
http://amzn.to/2w7dlPJamzn.to
市松模様のキャリブレーション板を振り回してキャリブレーションをしていく。
手順
1. Webカメラの画角に全面映るようにしていろんな位置に表示されるように振り回していくとCalibrationボタンが光るのでクリックする。(フリーズしたかと思うけど1分もかからず復活した
2. 斜めにしたり端っこにしたり遠くにしたり近くにしたりしながら適宜SAVEボタンを押していく→キャリブレーション用の画像を保存していく
3. 一通りやって腕が痛くなってきたらCOMMITボタンを押してCtrl+cでキャリブレーションソフトを落とす。(正直終わりどきがわからないので僕はとりあえず腕が痛くなるまでやりました5分位で70回SAVEした。)
4. 拾ってくる
cp /tmp/calibrationdata.tar.gz ~/orb_slam2_ws/dataset cd ~/orb_slam2_ws/dataset tar zvxf calibrationdata.tar.gz cp ./ost.yaml ~/.ros/camera_info/c920r.yaml
c920rは僕が使っているウェブカメラの型番
カメラのlaunchファイルを作る
次からは今回キャリブレーションした結果のyamlファイルを指定する必要があるなどいい加減カメラの設定がめんどくさくなったのでlaunchファイルを作りました。
nano ~/orb_slam2_ws/camera.launch
<launch> <node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" > <param name="video_device" value="/dev/video0" /> <param name="image_width" value="1920" /> <param name="image_height" value="1080" /> <param name="pixel_format" value="yuyv" /> <param name="camera_frame_id" value="usb_cam" /> <param name="io_method" value="mmap"/> <param name="camera_info_url" value="file:///home/myhome/.ros/camera_info/c920r.yaml" /> </node> <node name="image_view" pkg="image_view" type="image_view" respawn="false" output="screen"> <remap from="image" to="/usb_cam/image_raw"/> <param name="autosize" value="true" /> </node> </launch>
こちらはサンプルでcamera_info_urlなどは適宜書き換えて使ってね。
ORB_LSAM2の実行時の設定ファイルも変更する
次に、
~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/TUM1.yaml
についても変えてあげる必要がある。
いきなり書き始めるのも大変なのでまずは
cp ~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/TUM1.yaml ~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/c920r.yaml
でコピーを作る
nano ~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/c920r.yaml
のかなにCamera.*となっているパラメータがあるので、
OpenCV: Camera Calibration
を参考にしながら先ほどのキャリブレーション結果に合わせて編集していきます。
~/.ros/camera_info/c920r.yaml中
camera_matrix: rows: 3 cols: 3 data: [fx , 0, cx, 0, fy, cy, 0, 0, 1]
ここのdataのfx, cx, fy,cyにはそれぞれ値が入っているはず。
この値をCamera.fx,fy,cx,cyに入れていく
~/.ros/camera_info/c920r.yaml中
distortion_coefficients: rows: 1 cols: 5 data: [k1, k2, p1, p2, k3]
ここのdataのk1, k2, p1, p2, k3にはそれぞれ値が入っているはず。
この値をCamera.k1, k2, p1, p2, k3に入れていく
これでキャリブレーション完了
実行してみる
# Terminal1 roscore # Terminal2 cd ~/orb_slam2_ws/ORB_SLAM2 rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/c920r.yaml # Terminal3 roslaunch ~/orb_slam2_ws/camera.launch
できました!1090pにしたら結構重いみたいだけどなんとなくついてきてる。
疎過ぎてトラッキングがはじまりにくいので、検出する特徴量の数を1000から5000に増やしました。
あとはViewerのコードをよく読んで自分で座標を取得できるようになれば目標達成です。