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

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

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/506640/a19afddc-5f34-1966-6bf7-b6e1f9b1d5a0.png)

[github](https://github.com/tum-vision/lsd_slam)/[paper](https://vision.in.tum.de/research/vslam/lsdslam)

# 環境
ubuntu14.04
ROS indigo

# ROS indigoのインストール
[HP](http://wiki.ros.org/ja/indigo/Installation/Ubuntu)の通りにする

# LSD-SLAMのインストール

[github](https://github.com/tum-vision/lsd_slam)の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 :point_right_tone1: 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 :point_right_tone1: 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 :point_right_tone1: 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のキャリブレーションボード](https://github.com/ethz-asl/ethzasl_ptam/blob/master/ptam/calib_pattern.pdf)をディスプレイに表示 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 :point_right_tone1:  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のキャリブレーションボード](https://github.com/ethz-asl/ethzasl_ptam/blob/master/ptam/calib_pattern.pdf)をディスプレイに表示 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は下の値に対応します。
![ProjectionMatrix (2).jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/506640/a44678c6-5741-e15d-75b0-95d5f740e3b5.jpeg)



##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で使うとかなり重たかったです。