EnsekiTT Blog

EnsekiTTが書くブログです。

【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-6

前回までのえんせき

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

うごいたー!
f:id:ensekitt:20170819212956p:plain
これはひどい

キャリブレーションしてみる

具体的には使うカメラ用の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のコードをよく読んで自分で座標を取得できるようになれば目標達成です。