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

目的

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

ORB-SLAMとは

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

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

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

github/paper

環境

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のインストール

基本はこの通りに。

一応こちらでも示すと…

・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さん本当にありがとうございました。