ORB_SLAM2をUbuntu18.04で動かしてみた

SLAMとは自己位置推定と地図生成を同時に行う技術のことであり、ロボット掃除機のルンバや、自動運転にも使われています。
ORB_SLAMはVisual SLAMの１つであり、カメラで取得した画像から特徴点(色の変化が激しい点、際立っている点)を用いて自己位置推定と地図の生成をします。
[ORB_SLAM2](https://github.com/raulmur/ORB_SLAM2)はUbuntu12.04, 14.04, 16.04のみでしか動作テストしてないらしいので、ORB_SLAM2の導入方法を紹介しているサイトでもUbuntu16.04以下で動作しているものがほとんどです。
そこで、今回はORB_SLAM2をUbuntu18.04で動かしてみました。
![Screenshot from 2019-07-24 14-54-02.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/237700/c7de426f-01b8-0abe-8ce0-ea4a3cc634c4.png)

# 環境
- Ubuntu18.04
- ROS melodic

#ROSのインストール
ROSのmelodicをインストールします。

aptリポジトリのソースリストに追加

```
$ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
```
aptのキーを設定

```
$ sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
```
リポジトリ更新

```
$ sudo apt-get update
```
ここで`error: http://security.debian.org stable/updates Release: 公開鍵を利用で きないため、以下の署名は検証できませんでした: NO_PUBKEY 9D6D8F6BC857C906 `というエラーが出た人は下のコマンドを入れます

```
$ sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys 9D6D8F6BC857C906
```
今回の場合は`NO_PUBKEY 9D6D8F6BC857C906`と出てきたので、コマンドの最後を9D6D8F6BC857C906にしました。
出てきた文字列に合わせてコマンドを変えてください。
そして再度`sudo apt-get update`を試してください。

ROSのインストール

```
$ sudo apt install ros-melodic-desktop-full
```
rosdepの初期化

```
$ sudo rosdep init
```

rosdep updateしてと言われるので実行。

```
$ rosdep update
```
これで必要なパッケージがダウンロードされます。
ログイン時、「/opt/ros/melodic/setup.bash」が実行されるようにします。

```
$ echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
$ source ~/.bashrc
```
依存関係のあるパッケージをダウンロード

```
$ sudo apt-get install python-rosinstall python-rosinstall-generator python-wstool build-essential
```

これでROSのインストールと設定が完了しました。

# 必要なパッケージのインストール
```
$ sudo apt-get install python-rosinstall
$ sudo apt-get install libglew-dev
$ sudo apt-get install cmake
$ sudo apt-get install libpython2.7-dev
$ sudo apt-get install libjpeg-dev libpng12-dev libtiff5-dev libopenexr-dev
```

# 作業フォルダの作成
```
$ mkdir ~/ros_dev
```
.bashrcにORSへのパスを追加。
 
```
$ vi .bashrc
```
```
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/ros_dev/ORB_SLAM2/Examples/ROS
```

#Pangolinのインストール
描画ライブラリである[Pangolin](https://github.com/stevenlovegrove/Pangolin)をインストールします。

```
$ cd ~/ros_dev
$ git clone https://github.com/stevenlovegrove/Pangolin.git
$ cd Pangolin
$ mkdir build
$ cd build
$ cmake ..
```
ここで`No package 'xkbcommon' found`など、エラーが出てきた場合は[Pangolin](https://github.com/stevenlovegrove/Pangolin)のGithubに飛び、足りないものをインストールしてください。
今回は`xkbcommon`が必要なので

```
$ sudo apt install libegl1-mesa-dev libwayland-dev libxkbcommon-dev wayland-protocols
```
これで解決です。
再度`cmake ..`を実行。
うまくいったら以下のコマンドを実行。

```
$ make -j
$ sudo make install
```
これでPangolinのインストールは終わりです。

### make -jでエラーが起きる場合
`make -j`がうまく行かない場合はメモリ不足が原因だと思います(make -jは可能な限り並行処理の数を増やすため)。
`cat /proc/cpuinfo`を実行してプロセッサの数を確認します。

```
$ cat /proc/cpuinfo | grep processor
processor       : 0
processor       : 1
processor       : 2
processor       : 3
```
今回の場合はプロセッサが4つあるので、

```
$ make -j4
```
と、並行処理の数を指定することでうまくいきます。
そして下のコマンドを実行することでPangolinのインストールは終わりです。

```
$ sudo make install
```


# ORB_SLAM2のインストール
```
$ cd ~/ros_dev
$ git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
```
### ORB_SLAM2をコンパイルします。

```
$ cd ORB_SLAM2
$ chmod +x build.sh
$ ./build.sh
```
おそらくエラーが起きます。
「usleepってなに？？？知らんわ」って言われます
以下の原因の.ccファイル全てに`#include "unistd.h"`を追加します。

`~/ros_dev/ORB_SLAM2/src/LocalMapping.cc
~/ros_dev/ORB_SLAM2/src/LoopClosing.cc
~/ros_dev/ORB_SLAM2/src/System.cc
~/ros_dev/ORB_SLAM2/src/Tracking.cc
~/ros_dev/ORB_SLAM2/src/Viewer.cc
~/ros_dev/ORB_SLAM2/Examples/Monocular/mono_euroc.cc
~/ros_dev/ORB_SLAM2/Examples/Monocular/mono_kitti.cc
~/ros_dev/ORB_SLAM2/Examples/Monocular/mono_tum.cc
~/ros_dev/ORB_SLAM2/Examples/RGB-D/rgbd-tum.cc
~/ros_dev/ORB_SLAM2/Examples/Stereo/stereo_euroc.cc
~/ros_dev/ORB_SLAM2/Examples/Stereo/stereo_kitti.cc`

これで大丈夫なはずです。
修正し終わったらもう一度実行。

```
$ ./build.sh
```

### ORB_SLAM2のROSノードをコンパイル
```
$ chmod +x build_ros.sh
$ ./build_ros.sh
```
はい、またエラーがでます。
今回のエラーはパスが通っていないのが原因です。
`~/ros_dev/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt`の一部を修正します。
修正前

```
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
)
```
修正後

```
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
)
```
`-lboost_system`を追加するだけです。
そして再度`./build_ros.sh`を実行。

まだ、エラーが出ます
以下のコマンドを実行。

```
$ sudo ln -s /home/(自身のhomeの名前)/ros_dev/ORB_SLAM2/Examples/ROS/ORB_SLAM2 /opt/ros/melodic/share/ORB_SLAM2
```
そして再々度`./build_ros.sh`を実行。
また「usleepってなに？？？知らんわ」って言われます。

以下の.ccファイルに`#include "unistd.h`を追加します。
`~/ros_dev/ORB_SLAM2/Example/ROS/ORB_SLAM2/src/AR/ViewerAR.cc`
これができたら再々度

```
$ ./build_ros.sh
```
を実行。
これでORB_SLAM2のROSノードをコンパイルは終わりです。

#テストデータで動作確認する
![Screenshot from 2019-07-24 14-54-02.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/237700/10d1e060-88d9-5a82-2ad5-06b3d4f23ca9.png)
これができるとこんなのが出てくるはずです。
LSD_SLAMのテストデータで動作確認します。

```
$ cd ~/ros_dev
$ wget http://vmcremers8.informatik.tu-muenchen.de/lsd/LSD_room.bag.zip
$ unzip LSD_room.bag.zip 
```
コンソールを３つ立ち上げてそれぞれ実行します。
1つめ

```
$ roscore
```
2つめ

```
$ cd ~/ros_dev/ORB_SLAM2/
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml
```
3つめ

```
$ cd ~/ros_dev
$ rosbag play LSD_room.bag /image_raw:=/camera/image_raw
```
これでテストデータが動くはずです。

# Webカメラで動かす
まずは以下のコマンドでカメラを使えるようにします

```
$ sudo apt install ros-melodic-usb-cam
```
コンソールを３つ立ち上げます。
1つめ

```
$ roscore
```
2つめ

```
$ cd ~/ros_dev/ORB_SLAM2/
$ rosrun ORB_SLAM2 Mono Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml
```
3つめ

```
$ cd ~/ros_dev
$ rosrun usb_cam usb_cam_node /usb_cam/image_raw:=/camera/image_raw
```
これで動きます。

#カメラを切り替える
本当はUSBで接続しているWebカメラでやりたいのにPCについてる標準のカメラがついてしまったよーーーー！！！！って人向けです。

```
$ ls /dev/video*
```
これで使用可能なカメラ一覧を表示できます。
/dev/video1を使用したい場合は

```
$ rosparam set /usb_cam/video_device /dev/video1
```
で切り替えることができます。

# 実際に動いている時の動画
こんな感じに動作すると思います(クリックすると動画が見れます。)

[ORB_SLAM2のテスト](https://www.youtube.com/watch?v=TF7TDVXp_9k&feature=youtu.be)
[![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/TF7TDVXp_9k/0.jpg)](https://www.youtube.com/watch?v=TF7TDVXp_9k&feature=youtu.be)

[ORB_SLAM2のテスト2](https://www.youtube.com/watch?v=j1FEBHdv0HM&feature=youtu.be)
[![IMAGE ALT TEXT HERE](http://img.youtube.com/vi/j1FEBHdv0HM/0.jpg)](https://www.youtube.com/watch?v=j1FEBHdv0HM&feature=youtu.be)

カメラを早く動かすと特徴点を検出できなくなるようです。

# 終わりに
ORB_SLAM2をubuntu18.04で動かしてみました。ubuntu16.04でやっている人がほとんどで、僕が導入するのにかなり手こずったので今回まとめてみました。
僕がやったときに遭遇したエラーや、VM上のUbuntuでも試した時のエラーの解決策を書いた結果長くなってしまいました。
ちなみにVM上でやったときは3FPSくらいしか出なかったので、カックカクでした。
誰かの役に立てれたら幸いです。間違っているところがあったら指摘ください。

#参考サイト
- [ROSのORB_SLAM2を動かしてみた。](https://qiita.com/nnn112358/items/1087d5b0e4df5367f48b)
- [ORB-SLAM導入](http://ei0124.blog.fc2.com/?no=21)
- [ROSをUbuntu 18.04.1にインストールする](https://symfoware.blog.fc2.com/blog-entry-2265.html)
- [LSD-SLAMをROSで試す2-カメラの設定とキャリブレーション](https://noshumi.blogspot.com/2016/12/lsd-slamros2.html)
- [ORB_SLAM2](https://github.com/raulmur/ORB_SLAM2)