SLAMシリーズ LSD-SLAMとpointgreyとwebカメラで環境の3次元復元を行う

目的

LSD-SLAMとpointgreyのカメラを使って3Dスキャンを行う

LSD-SLAMとは

Large-Scale Direct Monocular SLAMの略で、特徴点を用いず、直接的に勾配情報を用いることを特徴としたSLAM

github/paper

環境

ubuntu14.04
ROS indigo

ROS indigoのインストール

HPの通りにする

LSD-SLAMのインストール

githubの2.2の通りにする

まずは、ワークスペースを作成

sudo apt-get install python-rosinstall
mkdir ~/rosbuild_ws
cd ~/rosbuild_ws
rosws init . /opt/ros/indigo
mkdir package_dir
rosws set ~/rosbuild_ws/package_dir -t .
echo "source ~/rosbuild_ws/setup.bash" >> ~/.bashrc
bash
cd package_dir

依存関係のあるシステムをインストール

sudo apt-get install ros-indigo-libg2o ros-indigo-cv-bridge liblapack-dev libblas-dev freeglut3-dev libqglviewer-dev libsuitesparse-dev libx11-dev

githubからcloneしてmake

git clone https://github.com/tum-vision/lsd_slam.git lsd_slam
rosmake lsd_slam

2.3はスキップしました。

OpenCV nonfree moduleのインストール

ROS indigoにはOpneCVのnonfree module が含まれていないので手動でインストールしておきます。
CHECK http://coffeegkgk.hatenablog.com/entry/2015/05/25/112307

sudo apt-get install build-essential cmake 
sudo apt-get install libjpeg-dev libtiff4-dev libjasper-dev 
sudo apt-get install libgtk2.0-dev 
sudo apt-get install libavcodec-dev libavformat-dev libswscale-dev libv4l-dev

cd ~ 
wget https://github.com/Itseez/opencv/archive/2.4.8.zip
unzip 2.4.8.zip 
cd opencv-2.4.8 
mkdir build
cd build

cmake -D CMAKE_BUILD_TYPE=RELEASE -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_OPENGL=ON ..
cmake -D WITH_TBB=ON -D BUILD_NEW_PYTHON_SUPPORT=ON -D WITH_V4L=ON -D INSTALL_C_EXAMPLES=ON -D INSTALL_PYTHON_EXAMPLES=ON -D BUILD_EXAMPLES=ON -D WITH_OPENGL=ON -D MAKE_INSTALL_PREFIX=/usr/local ..

make
sudo make install

echo "/usr/local/lib" | sudo tee /etc/ld.so.conf.d/opencv.conf
sudo ldconfig

export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:$PKG_CONFIG_PATH
export LD_LIBRARY_PATH=/usr/local/lib:$LD_LIBRARY_PATH
export PKG_CONFIG_PATH=/usr/local/lib/pkgconfig:$PKG_CONFIG_PATH

opneCVインストール後データセット起動

CHECK https://noshumi.blogspot.com/2016/12/lsd-slamros1.html

その前にlsd_slam_coreの中にあるCMakeLists.txtを少し変えます。
下のコメントアウトされている部分の#を消して、

# add_subdirectory(${PROJECT_SOURCE_DIR}/thirdparty/openFabMap)
# include_directories(${PROJECT_SOURCE_DIR}/thirdparty/openFabMap/include)
# add_definitions("-DHAVE_FABMAP")
# set(FABMAP_LIB openFABMAP )

もう一度rosmakeを実行

rosmake lsd_slam

データセットを↓から好きな場所にダウンロード
http://vmcremers8.informatik.tu-muenchen.de/lsd/LSD_room.bag.zip

そしてterminalを4つ開いて下のコマンドをそれぞれ実行します。

roscore
rosrun lsd_slam_viewer viewer
rosrun lsd_slam_core live_slam image:=/image_raw camera_info:=/camera_info
rosbag play ~/LSD_room.bag #データセットをDLした場所を指定

これでデモが再生されれば成功です。

rosmake lsd_slamでCmakeのエラーが出てたとき

python2.7のファイルを編集

sudo nano /opt/ros/indigo/lib/python2.7/dist-packages/dynamic_reconfigure/parameter_generator.py

check_name(name)
check_description(description)  <--消す
self.gen.fill_type(newparam)
self.gen.check_type_fill_default(newparam, 'default', self.gen.defval[paramtype])
self.gen.check_type_fill_default(newparam, 'max', self.gen.maxval[paramtype])

これでもう一度rosmake lsd_slam

キャリブレーションツールのインストール

キャリブレーションでは、個々のカメラの内部パラメータ、外部パラメータを計算します。
正確なSLAMを実行する上で欠かせない工程です。
ROSにもキャリブレーションツールが用意されているので、今回はそちらをインストールします。

rosdep install camera_calibration
rosmake camera_calibration

まずは普通のwebカメラでLSD-SLAM

CHECK https://noshumi.blogspot.com/2016/12/lsd-slamros2.html

ではいよいよLSD-SLAMを実行します。
まずは普通のwebカメラ、今回はlogicoolのwebカメラ使用しました。

ドライバusb-camをインストール&実行

下3つのコマンドは、それぞれ別のターミナルで実行します。

sudo apt-get install ros-indigo-usb-cam
roscore
rosrun usb_cam usb_cam_node
rosrun image_view image_view image:=/usb_cam/image_raw

カメラの映像が表示されればおｋです。

キャリブレーション

PTAMのキャリブレーションボードをディスプレイに表示 or 印刷したら実行
これもそれぞれ別のターミナルで

roscore
rosrun usb_cam usb_cam_node
rosrun camera_calibration cameracalibrator.py --size 11x7 --square 0.02 image:=/usb_cam/image_raw

※--sizeは横縦の交点の数, --squareはマス目の大きさで今回は20mm
　
しばらく色々な角度からキャリブレーションボードを見せていると、calibrateボタンが押せるようになります。そこからまた色々な角度で何回かsaveボタンを押して保存したのち、commitボタンを押してキャリブレーションは完了です。
calibrateを押すとフリーズした状態になることがありますが、しばらく待っていれば復活してきます。

ファイルは/tmpの中のcalibrationdata.tar.gzの中にost.txtとost.yamlとして出力されます。
usb_camを実行すると、unable to open calibration・・・・~/home/username/.ros/camera_info　とwarningが出るはずので、この時探している場所に今回つくったost.yamlのファイルの名前をhead_camera.yamlに変えて入れます。

cd ..
cd ..
cd tmp
tar -zxvf calibrationdata.tar.gz 
cd calibrationdata.tar.gz 
mv ost.yaml head_camera.yaml
mv head_camera.yaml /home/username/.ros/camera_info

前回のyamlファイルがあるのなら古い方の名前を変えておきましょう。末尾に.oldとか付けておくと便利です。

cd /home/okubo14/.ros/camera_info
mv head_camera.yaml head_camera.yaml.old

あと一応最後にキャリブレーション結果の確認しておきましょう。
パラメータが０でなければ成功

rosrun usb_cam usb_cam_node
rostopic echo /usb_cam/camera_info

キャリブファイルを使ってカメラ起動

では、このファイルを使ってカメラを起動します。
もうあるべき場所にキャリブレーションのファイルは移動させたので、ターミナルを2つ開いて以下のコマンドを順番に実行します。
※ここで、[head_camera] does not match name narrow_stereo in file ~~~的なコメントが出ても無視してとりあえずは大丈夫です。

roscore
rosrun usb_cam usb_cam_node

この状態でrostopic listを実行し、/image_rawと/camera_infoのトピック名を調べます。

rostopic list

LSD-SLAMの実行

下の2つ目のコマンドのusb_camとcamera_infoのトピック名を書き換えて、2つのコマンドをさらに実行します。
※usb_cam/image_raw usb_cam/camera_info のところがカメラによって変わります。

rosrun lsd_slam_viewer viewer
rosrun lsd_slam_core live_slam /image:=/usb_cam/image_raw /camera_info:=/usb_cam/camera_info

CHECK https://noshumi.blogspot.com/2016/12/lsd-slam3.html

コマンドまとめ

roscore
rosrun usb_cam usb_cam_node
rosrun lsd_slam_viewer viewer
rosrun lsd_slam_core live_slam /image:=/usb_cam/image_raw /camera_info:=/usb_cam/camera_info

pointgreyのカメラでLSD-SLAM

pointgreyのドライバインストール

まず、ROSが用意してくれているROSindigoのpointgreyのドライバをインストール
http://wiki.ros.org/pointgrey_camera_driver

sudo apt-get install ros-indigo-pointgrey-camera-driver
rosrun pointgrey_camera_driver list_cameras

カメラ起動

カメラ起動

rescore
roslaunch pointgrey_camera_driver camera.launch

この状態でrostopicを確認

rostopic list  (image:とcamera_info:を調べる)
rostopic type /camera/iamge  (カメラのタイプを見る．）

キャリブレーション

PTAMのキャリブレーションボードをディスプレイに表示 or 印刷したら実行

rosrun camera_calibration cameracalibrator.py --size 11x7 --square 0.02 image:=/camera/image_raw

image:=/camera/image_rawのところ、rostopicでimage:を確認したところ、さっきはusb_cam/image_rawでしたが、pointgreyは/camera/image_rawになってました。

あとはさっきと同じようにキャリブレーションすれば、/tmpの中のcalibrationdata.tar.gzの中にost.txtとost.yamlが出力されます。

キャリブレーションファイルの編集

ここから少しwebカメラの方法と変わって編集が必要になってきます。

以下のフォルダにcfgファイルを作成します。
場所：~/rosbuild_ws/package_dir/lsd_slam/lsd_slam_core/calib

nano ~/rosbuild_ws/package_dir/lsd_slam/lsd_slam_core/calib/FOV_flea3_calib.cfg 

ファイルの中身は👇

fx fy cx cy k1 k2 p1 p2
inputWidth inputHeight
"crop" / "full" / "none" / "e1 e2 e3 e4 0"
outputWidth outputHeight

1行目：ptamでcalibした結果（ost.yaml）からの値を入力
2行目：入力画像の大きさ
3行目：crop or １行目と同じ
4行目：推奨　640 480

camera matrix は3*3の行列で、fx,fy,cx,cyは下の値に対応します。

LSD-SLAM実行

準備が整ったらLSD-SLAM実行します。
それぞれ別のターミナルでコマンドを打ちます。

roscore
roslaunch pointgrey_camera_driver camera.launch
rosrun lsd_slam_viewer viewer
rosrun lsd_slam_core live_slam /image:=/camera/image_raw _calib:=FOV_flea3_calib.cfg 

これでうまいことviewerが動いて、点群が見えるようになったら成功です。

便利コマンド

ノードのやり取りが見える便利なコマンドです。
ノード同士がどのようにやり取りしているか見えます。
うまくいかないときも、どのノードが動いてないとか繋がってないとか確認できるのでかなり役に立ちます。

rosrun rqt_graph rqt_graph 

おわり

試した結果としてはpoitgreyのカメラは性能が良いあまり、LSD-SLAMで使うとかなり重たかったです。