RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_012日目_SLAM_ORB-SLAM2

#<font color="#08088A">◆ 前回記事</font>
[RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい_011日目_SLAM_Karto SLAM\_LiDAR(A1M8)](https://qiita.com/PINTO/items/9aa737c284dc4e8212f1)　の続き
#<font color="#08088A">◆ はじめに</font>
今回は **`ORB-SLAM2`** を試行してみる。

|今回|他記事|アルゴリズム|特徴|
|:-:|:--|:---|:---|
||**[008日目](https://qiita.com/PINTO/items/9aa737c284dc4e8212f1)**|**[Gmapping](https://github.com/ros-perception/slam_gmapping/tree/hydro-devel/gmapping)**|一番有名らしい、ROS対応、loop closure有り|
||**[009日目](https://qiita.com/PINTO/items/4845c438cac05eda4d1e)**|**[Google Cartographer](https://github.com/googlecartographer/cartographer)**|精度が高そう、**[ROS対応](https://github.com/googlecartographer/cartographer_ros)**、loop closure有り|
||**[010日目](https://qiita.com/PINTO/items/b3eb7205baf9c55de81b)**|**[Hector SLAM](https://github.com/tu-darmstadt-ros-pkg/hector_slam)**|精度がいまいちっぽい、loop closure無し|
||**[011日目](https://qiita.com/PINTO/items/7939b1e53e23a9458c1d)**|**[Karto SLAM](https://www.kartorobotics.com/)**|単眼カメラでも動く、**[ROS対応](https://github.com/ros-perception/slam_karto)**、Visual-SLAM|
|★||**[ORB-SLAM2](https://github.com/raulmur/ORB_SLAM2)**|単眼カメラでも動く、Visual-SLAM||
||**[007日目](https://qiita.com/PINTO/items/c5d69c0c0c58d4ded3f8)**|**[mono-vo](https://github.com/PINTO0309/mono-vo)**|単眼カメラでも動く、Visual-SLAM|
||**[013日目](https://qiita.com/PINTO/items/8989f45ad0908184e6e5)**|**[VINS-Mono](https://github.com/PINTO0309/VINS-Mono)**|単眼カメラでも動く、ROS対応、Visual-SLAM、ドローン|
|ー|ー|**[CNN-SLAM](https://github.com/Lishunkai/CNN-SLAM)**|単眼カメラでも動く、3D、アイデアのメモ情報しか見当たらなかったため検証対象外とする|
|ー|ー|**[RGBD-SLAM-V2](http://wiki.ros.org/rgbdslam)**|RGB-Dカメラ専用、検証対象外とする|

では、恒例のとおり、結果から先に公開。
<font color="red">**こいつはスゴイ！**</font> と直感的に感じた。
単眼USBカメラ ＋ ROS の構成にも関わらず、三次元で部屋の様子が綺麗に捉えられているのが分かるだろうか。
自宅の居間が。。。点の集まりでだけ立体的に再現されようとしている。。。

■ ROS ＋ USBカメラ による ORB_SLAM の実行画像
![ezgif.com-optimize (7).gif](https://qiita-image-store.s3.amazonaws.com/0/194769/667add0c-9120-ca3e-5f00-ec49c3ce4030.gif)
#<font color="#08088A">◆ 参考にさせていただいた記事、謝辞</font>
**【公式】
https://github.com/raulmur/ORB_SLAM2**

**【nnn112358さん】
[ROSのORB_SLAM2を動かしてみた。 Qiita](https://qiita.com/nnn112358/items/1087d5b0e4df5367f48b)**

**【EnsekiTTさん】
[【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-1 EnsekiTT Blog](http://ensekitt.hatenablog.com/entry/visualslam1)
[【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-2 EnsekiTT Blog](http://ensekitt.hatenablog.com/entry/visualslam2)
[【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-3 EnsekiTT Blog](http://ensekitt.hatenablog.com/entry/visualslam3)
[【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-4 EnsekiTT Blog](http://ensekitt.hatenablog.com/entry/visualslam4)
[【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-5 EnsekiTT Blog](http://ensekitt.hatenablog.com/entry/visualslam5)
[【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-6 EnsekiTT Blog](http://ensekitt.hatenablog.com/entry/visualslam6)
[【7Days自由研究】Visual SLAMとROSを使えるようになる話 Day-7【最終話】 EnsekiTT Blog](http://ensekitt.hatenablog.com/entry/visualslam7)**

#<font color="#08088A">◆ 作業に着手する前の前提条件</font>
**[Raspbian Stretch+NCS(Neural Compute Stick)+YoloV2+Webカメラ＋ROSによるリアルタイム複数動体検知環境の構築](https://qiita.com/PINTO/items/b084fe3dc716c42e2867)** と **００２日目** から **０１１日目** の記事を参考に、全ての作業を実施済みであり、導入済みパッケージの種類と数の同期がとれていることを前提とする。

#<font color="#08088A">◆ UbuntuPCでのシミュレーション</font>
このフェーズで使用する検証環境は下記。

■ 検証用PC

1. OS：Ubuntu 16.04
2. CPU：第3世代 Intel Core i7-3517U(1.9GHz)
3. MEM：16GB
4. ROS kinetic

##<font color="#0B3B39">● シミュレーション環境構築</font>

１．下記コマンドを実行する。

```shell:事前作業
$ sudo apt-get install libglew-dev 
$ mkdir ~/catkin_wsos
$ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/ros_dev/ORB_SLAM2/Examples/ROS
```

２．下記コマンドを実行する。

```shell:Pangolinのインストール
$ cd ~/catkin_wsos
$ git clone https://github.com/PINTO0309/Pangolin.git
$ cd Pangolin
$ mkdir build;cd build
$ cmake ..
$ make -j4
$ sudo make install
```

３．ORB_SLAM2のCloneとビルドを行う。

```shell:ORB_SLAM2のCloneとビルド
$ cd ~/catkin_wsos
$ git clone https://github.com/PINTO0309/ORB_SLAM2.git ORB_SLAM2
$ cd ORB_SLAM2
$ chmod +x build.sh
$ ./build.sh
```

４．下記コマンドを実行し、CMakeLists.txt を編集する。
**【参考】 https://github.com/raulmur/ORB_SLAM2/issues/494**

```shell:CMakeLists.txt編集
$ nano ~/catkin_wsos/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt
```

```cmake:編集前
set(LIBS 
${OpenCV_LIBS} 
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
)
```

```cmake:編集後
set(LIBS 
${OpenCV_LIBS} 
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM2.so
-lboost_system
)
```

５．ORB_SLAM2のROSノードをコンパイルする。

```shell:ORB_SLAM2のROSノードコンパイル
$ chmod +x build_ros.sh
$ ./build_ros.sh
$ cd ~/catkin_wsos/ORB_SLAM2/Examples
$ rosws init . /opt/ros/kinetic
$ rosws set ~/catkin_wsos/ORB_SLAM2/Examples/ROS -t .
$ cd ..
$ source setup.bash
$ cd ~/catkin_wsos/ORB_SLAM2/Examples/ROS
$ rosmake ORB_SLAM2
```

ORB_SLAM2のビルドは無事成功したようだ。
![masked.png](https://qiita-image-store.s3.amazonaws.com/0/194769/9ddee03d-e87f-6f8e-b1f8-b0dd012a7883.png)
![masked.png](https://qiita-image-store.s3.amazonaws.com/0/194769/7e736afa-8b15-aa03-ecdc-dc2887cf0ca8.png)

##<font color="#0B3B39">● サンプルデータによるシミュレーション</font>

###<font color="#5E610B">＊ ROS無し ＋ シミュレーションデータ</font>
まずは、 **`ROS無し`** でトライしてみる。

```shell:ROS無しでのORB_SLAM実行
$ mkdir ~/catkin_wsos/dataset;cd ~/catkin_wsos/dataset
$ wget https://vision.in.tum.de/rgbd/dataset/freiburg1/rgbd_dataset_freiburg1_xyz.tgz
$ tar -zxf rgbd_dataset_freiburg1_xyz.tgz
$ cd ~/catkin_wsos/ORB_SLAM2
$ ./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml ../dataset/rgbd_dataset_freiburg1_xyz
```
むむっ・・・なんとか動いたぞ！
何やらスゴい匂いはビンビン伝わってくる。
まさか、単眼カメラで姿勢推定までやっているということなのか。 すごいな。
ただ、現時点ではいまいち活用の方法はピンときていない。

■ ROS無しでのORB_SLAMの実行画像
![ezgif.com-optimize (5).gif](https://qiita-image-store.s3.amazonaws.com/0/194769/2f1045d2-692b-5914-22d7-3bae6aaed72a.gif)

###<font color="#5E610B">＊ ROS有り ＋ シミュレーションデータ</font>
では、次に **`ROS有り ＋ シミュレーションデータ`** でトライしてみる。

```shell:サンプルデータのダウンロード_ZIP474MB_解凍後832MB
$ cd ~/catkin_wsos
$ wget http://vmcremers8.informatik.tu-muenchen.de/lsd/LSD_room.bag.zip
$ unzip LSD_room.bag.zip;rm LSD_room.bag.zip
```

```shell:Masterの実行
$ cd ~/catkin_wsos/ORB_SLAM2
$ source Examples/setup.bash
$ roscore
```

```shell:ROSとシミュレーションデータによるORB_SLAMの実行
$ cd ~/catkin_wsos/ORB_SLAM2
$ source Examples/setup.bash
$ rosbag play ~/catkin_wsos/LSD_room.bag /image_raw:=/camera/image_raw
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml
```
こちらも問題無く動いた。
シミュレーションデータでやっていることなので、 **`ROS無し`** の場合と比べてあまり違いを感じない。

■ ROS有りでのORB_SLAMの実行画像
![ezgif.com-optimize (6).gif](https://qiita-image-store.s3.amazonaws.com/0/194769/9b76d84f-b245-87ee-9660-2361e68584c8.gif)

ちなみに余談だが、ココまで、さも簡単に環境構築を進めているように見えるかもしれないが、実際は公式のチュートリアルどおりに環境構築を進めた結果、悪夢の **`Segmentation Fault`** が発生し、５回ほどゼロから環境を作り直し、ようやく動作させることに成功している。
公式チュートリアルどおりではうまくいかなかった。

##<font color="#0B3B39">● USBカメラによるSLAM</font>
では、次に **`ROS有り ＋ USBカメラ`** でリアルのカメラ画像を使用してトライしてみる。
まずは、USBカメラ連携用ROSパッケージのインストールと設定変更を行う。

```shell:USBカメラ連携用ROSパッケージ（OpenCVベース）のインストール
$ cd ~/catkin_wsos
$ mkdir src;cd src
$ git clone https://github.com/PINTO0309/video_stream_opencv.git
$ cd ..
$ catkin_make
$ cp src/video_stream_opencv/launch/webcam.launch src/video_stream_opencv/launch/BK_webcam.launch
$ nano src/video_stream_opencv/launch/webcam.launch
```

下記のとおり編集する。

```xml:編集前
<arg name="camera_name" value="webcam" />
<arg name="visualize" value="true" />
```

```xml:編集後
<arg name="camera_name" value="camera" />
<arg name="visualize" value="false" />
```

では、設定が一通り終わったので **`ROS有り ＋ USBカメラ`** の構成で ORB_SLAM を実行してみる。 

```shell:Masterの起動
$ roscore
```

```shell:USBカメラ連携用ROSパッケージを起動
$ source devel/setup.bash
$ roslaunch video_stream_opencv webcam.launch
```

```shell:ORB_SLAM本体の実行
$ cd ~/catkin_wsos/ORB_SLAM2
$ source Examples/setup.bash
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml
```

**[◆ はじめに](#-はじめに)** へ掲載の画像のとおりの動きになった。
思いのほか部屋の様子がしっかりと再現されていて、ビックリしてしまった。
リソースの制約を度外視すれば、アルゴリズムの性能はすごく良さそう。

やり始めたら面白くなってきて、ついつい全てやりきってしまったが、RaspberryPiで処理させるにはあまりにも重量級の処理だったため、今回もRaspberryPiを使用した検証は見送る。
今回検証に使用した Corei7 クアッドコア機種 でもファンがウァンウァン唸りっぱなしだったので相当キツイと思う。
過去記事の YoloV2、TinyYolo、MobileNet-SSD の検証の際に、画像処理系はARMのCPU単体ではとても負荷が高いことが分かっていたため、ほぼ想定どおりの試行結果だった。
やりたいことに対しては三次元で周囲を捉える必要性は無く、とても面白いのだけれど、今回は採用を見送ることになると思う。

#<font color="#08088A">◆ 本日のまとめ</font>

- ORB-SLAMは、ものすごく目に訴えるものがある。
- 単眼カメラだけで姿勢推定までできてしまうものなの？ ホントに凄すぎる。
- デプスカメラ や ステレオカメラ が無くてもここまでできるのか。
- 画像処理に強いGPUチップを積んだボードなら処理に耐えられるかもしれない。
- 少なくとも RaspberryPi にはほぼ対応不能ではないだろうか。
- ルート探索には不向きな感じがした。
- もっと高精度で高密度な三次元点群として捉えることができれば、かなり使えるものになるのかもしれない。
- 沢山のアルゴリズムを試行してきて感じ始めているのは、 「アルゴリズムは用途に応じて適材適所」 ということ。
- 他の何かと組み合わせて表現できると面白そう。
- 可視化部分だけを切り離せば、意外といける、のかもしれない。

#<font color="#08088A">◆ 次回予告</font>
次は **`VINS-Mono`** にチャレンジしてみようと思う。

#<font color="#08088A">◆ 次回記事</font>
[RaspberryPi3とZumoとROSで半永久自走式充放電ロボを作成したい\_013日目_SLAM_VINS-Mono](https://qiita.com/PINTO/items/8989f45ad0908184e6e5)　へ続く