SLAMシリーズ　ORB-SLAM2のインストールとテスト

# 目的
画素値直接利用のLSD-SLAMと特徴点抽出のORB-SLAM2を使用して比較する。
（なかなか長い道のりです。）


#ORB-SLAMとは 
PTAMの流れを汲んでいるリアルタイムに動作する特徴点ベースの単眼visual SLAM
2014年に発表された論文・実装 

以下のような特徴がある。

・（特徴量を用いるので）回転や光の変化・動的物体などに対してロバスト
・BA(Bundle Adjustment)を使うことで誤差を吸収
・フレームを多めに取ることで局所的に精度の良い自己位置推定を行い、あとで余計なキーフレームを削除（敵者生存戦略）
・初期化を自動で行う

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/506640/3c9ce62d-329a-6a4f-705c-b1e64b15f29a.png)


[github](https://github.com/raulmur/ORB_SLAM2)/[paper](https://128.84.21.199/pdf/1610.06475.pdf)


# 環境
ubuntu16.04
ROS kinetic


#pangolinのインストール

依存関係のあるパッケージをインストール

```
sudo apt-get install libglew-dev
sudo apt-get install cmake
sudo apt-get install libpython2.7-dev
sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev
sudo apt-get install libdc1394-22-dev libraw1394-dev
sudo apt-get install libjpeg-dev libpng12-dev libtiff5-dev libopenexr-dev
```

githubからclone＆make＆install

```
mkdir ~/pangolin_ws
cd ~/pangolin_ws
git clone https://github.com/stevenlovegrove/Pangolin.git
cd Pangolin
mkdir build
cd build
cmake ..
make -j
sudo make install
```

#opencvのインストール

依存関係のあるパッケージをインストール

```
sudo apt-get install build-essential
sudo apt-get install cmake git libgtk2.0-dev pkg-config libavcodec-dev libavformat-dev libswscale-dev
sudo apt-get install python-dev python-numpy libtbb2 libtbb-dev libjpeg-dev libpng-dev libtiff-dev libjasper-dev libdc1394-22-dev
```

githubからclone＆make＆install

```
mkdir ~/opencv_ws
cd ~/opencv_ws
git clone https://github.com/opencv/opencv.git
cd opencv
mkdir build
cd build
cmake -D CMAKE_BUILD_TYPE=Release -D CMAKE_INSTALL_PREFIX=/usr/local ..
make -j7
sudo make install
```

#Eigen3のインストール

```
mkdir ~/eigen_ws
cd ~/eigen_ws
wget http://bitbucket.org/eigen/eigen/get/3.3.4.tar.gz
tar -zxf 3.3.4.tar.gz
cd eigen-eigen-5a0156e40feb
mkdir build
cd build
cmake ..
make check
sudo make install
```

#LIBUVCのインストール

```

mkdir ~/libuvc_ws
cd ~/libuvc_ws
git clone git://github.com/ktossell/libuvc.git
cd libuvc
mkdir build
cd build
cmake ..
make
```

※ここでエラーがでました。
どうやら`libusb.h`を見つけられてなかったみたいです。
cmakeをしたときのログチェックの例
http://kawauso-festival.blogspot.com/2016/08/pclcpython.html

解決策は、バージョンを指定してlibusbをインストール
https://askubuntu.com/questions/225382/install-usblib-package-ubuntu

```
sudo apt-get install libusb-1.0-0-dev
```

そしてもう一度

```
sudo make install
```

#g2oのインストール

g2oはeigenがないとインストールできないので注意です。

依存関係のあるパッケージをインストール

```
sudo apt install libsuitesparse-dev - qtdeclarative5-dev - qt5-qmake - libqglviewer-dev
```

gituhubをcloneしてインストール

```
mkdir ~/g2o_ws
cd ~/g2o_ws
git clone https://github.com/RainerKuemmerle/g2o.git
cd g2o
mkdir build
cd build
cmake ..
make
sudo make install
```

※もし、cmake　.. でエラーが出たら、
https://github.com/raulmur/ORB_SLAM2/issues/647

```
sudo apt-get install libeigen3-dev instead.
```
eigen3を先にインストールしていれば、エラーは出ないはず、です。


#DBoW2

依存関係のあるパッケージをインストール

```
sudo apt-get install libboost-dev
```

githubからcloneしてきてインストール

```
mkdir ~/DBoW2_ws
cd ~/DBoW2_ws
git clone https://github.com/dorian3d/DBoW2.git
cd DBoW2
mkdir build
cd build
cmake ..
```

※ここでエラーが出ました。

```
error "OpenCV 4.x+ requires enabled C++11 support"
```

解決策は、以下の方法でflagをつけてcmakeを実行。
https://github.com/BVLC/caffe/issues/6358

```
cmake .. -DCMAKE_CXX_FLAGS="-std=c++11"
```
C++11じゃないと使えないよ？ということかな。。。


上が通れば

```
make
sudo make install
```

#ROS kineticのインストール

基本は[この](http://www2.meijo-u.ac.jp/~kohara/cms/technicalreport/ubuntu16-04_ros_install)通りに。

一応こちらでも示すと…

・aptへのROSのダウンロード先の登録＆公開鍵の取得

```
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
```

・ROSのインストール＆初期設定

```
sudo apt-get update
sudo apt-get install ros-kinetic-desktop-full
sudo rosdep init
rosdep update
```

・環境変数の設定

```
cd
echo "source /opt/ros/kinetic/setup.bash" >> ~/.bashrc
source ~/.bashrc
```

・ROSのパッケージインストーラのインストール

```
sudo apt-get install python-rosinstall
```

・ワークスペースのパスを環境変数に追加

```
cd
source /opt/ros/kinetic/setup.bash
```

・ROSのワークスペース

```
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
```

・ワークスペースの中をビルドできるかテスト

```
cd catkin_ws
catkin_make
```

・自分のワークスペースをインストール環境に配置＆確認

```
source devel/setup.bash
echo $ROS_PACKAGE_PATH
```


#ORB-SLAM2 のインストール

いよいよです。

```
mkdir ~/orb_slam2_ws
cd orb_slam2_ws
git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
cd ORB_SLAM2
chmod +x build.sh
./build.sh
```

※ここでエラー
解決策は、.ccのファイルに片っ端から#include "unistd.h"を付けます。
http://ei0124.blog.fc2.com/blog-entry-21.html

>おそらくは./build.shでエラーすると思います
「usleepとか知らねえ」とか言われます
よって原因である.ccのファイルに片っ端から#include "unistd.h"を付けていきます
（10個くらいのファイルに付けた気がする）
やりまくればいつかは通ります
>
追記: 2018/7/31
修正するべき.ccは11もありました
~/orb_slam2_ws/ORB_SLAM2/src/LocalMapping.cc
~/orb_slam2_ws/ORB_SLAM2/src/LoopClosing.cc
~/orb_slam2_ws/ORB_SLAM2/src/System.cc
~/orb_slam2_ws/ORB_SLAM2/src/Tracking.cc
~/orb_slam2_ws/ORB_SLAM2/src/Viewer.cc
~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/mono_euroc.cc
~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/mono_kitti.cc
~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/mono_tum.cc
~/orb_slam2_ws/ORB_SLAM2/Examples/RGB-D/rgbd-tum.cc
~/orb_slam2_ws/ORB_SLAM2/Examples/Stereo/stereo_euroc.cc
~/orb_slam2_ws/ORB_SLAM2/Examples/Stereo/stereo_kitti.cc
以上、追記終わり


#ORB-SLAM2のテスト

適当にデータをダウンロードしてきて動かしてみます。

```
mkdir ~/orb_slam2_ws/dataset
cd ~/orb_slam2_ws/dataset
wget https://vision.in.tum.de/rgbd/dataset/freiburg1/rgbd_dataset_freiburg1_xyz.tgz
tar -zxf rgbd_dataset_freiburg1_xyz.tgz
cd .. 
cd ORB_SLAM2 (※この場所じゃないとできないです）
./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml ../dataset/rgbd_dataset_freiburg1_xyz
```


#ROSで動かす

テストで何やらうまくいったみたいなので、ROS上でも動かせるか確認します。

```
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$HOME/orb-slam2_ws/ORB_SLAM2/Examples/ROS
cd ~/orb-slam2_ws/ORB_SLAM2
chmod +x build_ros.sh
./build_ros.sh   
```

ここで`./build_ros.sh`のエラーが出ました。
先ほどのブログを参考に解決していきます。

>
今回のエラーはパスが通っていないことでした
と、いうわけでCMakeLists.txtをいじります
>
いじるべきCMakeLists.txtは
~/orb_slam2_ws/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を追加するんですね
おそらくはこれで
cd ~/orb_slam2_ws/ORB_SLAM2/
./build_ros.sh
で通ります
>
追記: 2018/7/23
ここでまたエラーしました
対処方法はパスを通してやることです
例えば
sudo ln -s /home/(自身のhomeの名前)/orb_slam2_ws/ORB_SLAM2/Examples/ROS/ORB_SLAM2 /opt/ros/kinetic/share/ORB_SLAM2
としてやります
僕はこれで./build_ros.shを突破しました
-lboost systemがいるかどうかはわかりませんでした・・・
とりあえず付けましたが・・・
以上、追記終わり


私の場合、以上をしても下のエラーが消えなかったのですが、

```
fatal error: Eigen/Core: No such file or directory
```

以下のパスを通したら解決しました。
https://github.com/raulmur/ORB_SLAM2/issues/403

```
sudo ln -s /usr/include/eigen3/Eigen /usr/local/include/Eigen
```

サンプルを試します。

```
cd ~/orb-slam2_ws/ORB_SLAM2/Examples
rosws init . /opt/ros/kinetic   
rosws set ~/orb-slam2_ws/ORB_SLAM2/Examples/ROS -t .  
echo "source ~/workspace/orb_slam2_ws/ORB_SLAM2/Examples/setup.bash" >> ~/.bashrc
bash
```

ここで最初ディレクトリが無いというエラーが出ましたが、home直下のディレクトリ（私の場合workspace）からsourceするのを忘れていただけでした…（余談）

そしてrosmake

```
cd ~/orb-slam2_ws/ORB_SLAM2/Examples/ROS
rosmake ORB_SLAM2
```


#webカメラで実行準備

・キャリブレーション用アプリインストール

```
rosdep install camera_calibration
rosmake camera_calibration
```

・キャリブレーションの実行
※PTAMでする場合専用のキャリブレーションボード使わないとできない

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

＊0.02=キャリブパターンの１マスの大きさ（2cm）
＊しばらくいろんな角度からキャリブレーションボードを見せているとcalibrateボタンが押せる
＊そこからまたいろんな角度からsaveボタンを押して保存したのちcommit
/tmpの中のcalibrationdata.tar.gzの中にost.txtとost.yamlが出力される


tmpフォルダに行ってost.yamlの名前を変更＆移動

```
cd ..
cd ..
cd tmp
cd calibrationdata.tar.gz 
mv ost.yaml webcam.yaml
mv webcam.yaml /home/okubo14/.ros/camera_info
```


ORBALSAM2の方のcalibファイルを書き換え（TUMI1.yamlのファイルを参考に）

```
cp ~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/TUM1.yaml ~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/webcam.yaml
nano ~/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/webcam.yaml
```

fx,fy,cx,cy,k1, k2, p1, p2, k3 
の値をost.yaml(webcam.yaml)から抽出して修正



#webカメラで実行

・Terminal1

```
roscore
```

・Terminal2

```
rosrun ORB_SLAM2 Mono workspace/orb_slam2_ws/ORB_SLAM2/Vocabulary/ORBvoc.txt workspace/orb_slam2_ws/ORB_SLAM2/Examples/Monocular/webcam.yaml
```


・Terminal3

```
rosparam set /usb_cam/pixel_format yuyv
rosparam set /usb_cam/image_height 480
rosparam set /usb_cam/image_width 640
rosparam set /usb_cam/camera_info_url file:///home/okubo/.ros/camera_info/webcam.yaml
rosrun usb_cam usb_cam_node /usb_cam/image_raw:=/camera/image_raw
```

・ROSのノード見える化と画面録画

```
rosrun rqt_graph rqt_graph
simplescreenrecorder
```

#stereoカメラでやってみる

sampledataを試してみました。


・Terminal1

```
roscore
```

・Terminal2

```
rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true
```

・Terminal3

```
rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw
```

bagファイルは👇からdownloadsしたものに変更してます。
http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets



#おしまい

[ei](http://ei0124.blog.fc2.com/blog-entry-21.html)さん本当にありがとうございました。:bow_tone2: