ORB-SLAM3をROS1で使うためのセットアップまで

ORB-SLAM3がなんか動きそうなのでメモがてら．
現段階では制約あり

# 環境
- Ubuntu 20.04
- ROS noetic

上記は公式の標準的なインストール方法を踏んでます．

- OpenCV 4.2
- Eigen 3.3.7

気が付いたら入っていた．多分どこかで`apt install libopencv`や`apt install libeigen-dev`
をしたのだろう

<details>
　<summary>aptでインストールしたのかの確認方法(展開)</summary>

``` bash
apt list --installed | grep libeigen

libeigen3-dev/focal,focal,now 3.3.7-2 all [インストール済み]
```

``` bash
apt list --installed | grep libopencv

libopencv-calib3d-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-calib3d4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-contrib-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-contrib4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-core-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-core4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-dnn-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-dnn4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-features2d-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-features2d4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-flann-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-flann4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-highgui-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-highgui4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-imgcodecs-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-imgcodecs4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-imgproc-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-imgproc4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-ml-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-ml4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-objdetect-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-objdetect4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-photo-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-photo4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-shape-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-shape4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-stitching-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-stitching4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-superres-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-superres4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-ts-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-video-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-video4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-videoio-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-videoio4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-videostab-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-videostab4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-viz-dev/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv-viz4.2/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
libopencv4.2-java/focal,focal,now 4.2.0+dfsg-5 all [インストール済み、自動]
libopencv4.2-jni/focal,now 4.2.0+dfsg-5 amd64 [インストール済み、自動]
```

別のなにか(PCL?)を入れたときに依存関係として入ったっぽい
</details>

### 補足：EigenのVersion確認方法
``` bash
 grep "#define EIGEN_[^_]*_VERSION" /usr/include/eigen3/Eigen/src/Core/util/Macros.h
#define EIGEN_WORLD_VERSION 3
#define EIGEN_MAJOR_VERSION 3
#define EIGEN_MINOR_VERSION 7
```

3.3.7ですね

### 補足：OpenCVのVersion確認方法
python-opencvではなくてc++のほうなので

```bash
grep "CV_VERSION" /usr/include/opencv4/opencv2/core/version.hpp 
#ifndef OPENCV_VERSION_HPP
#define OPENCV_VERSION_HPP
#define CV_VERSION_MAJOR    4
#define CV_VERSION_MINOR    2
#define CV_VERSION_REVISION 0
#define CV_VERSION_STATUS   ""
#define CV_VERSION          CVAUX_STR(CV_VERSION_MAJOR) "." CVAUX_STR(CV_VERSION_MINOR) "." CVAUX_STR(CV_VERSION_REVISION) CV_VERSION_STATUS
#define CV_MAJOR_VERSION    CV_VERSION_MAJOR
#define CV_MINOR_VERSION    CV_VERSION_MINOR
#define CV_SUBMINOR_VERSION CV_VERSION_REVISION
#endif // OPENCV_VERSION_HPP
```
4.2.0っぽいことがわかる

# セットアップ

## まえがき
それぞれのGitリポジトリのReadmeを一読してから以下を進めると良いかも
大まかには以下の二つを入れる

https://github.com/UZ-SLAMLab/ORB_SLAM3

https://github.com/thien94/orb_slam3_ros_wrapper

後者はラッパーとして普通のROSパッケージになってるので使いやすそう

## ディレクトリ作成

``` bash
 mkdir ~/ORB_SLAM
```
ROSと関係ない部分を1フォルダに押し込めようとしているだけ

# Build Pangolin

https://github.com/stevenlovegrove/Pangolin

``` bash
 cd ~/ORB_SLAM
 git clone --recursive https://github.com/stevenlovegrove/Pangolin.git
 cd Pangolin/
 ./scripts/install_prerequisites.sh recommended
 cmake -B build -GNinja
 cmake --build build
```

recommendedで依存パッケージのインストールを行うとNinjaが入るので，オプション指定してビルドを高速化する

# Modify and Build ORB_SLAM3

https://github.com/UZ-SLAMLab/ORB_SLAM3

``` bash
 cd ~/ORB_SLAM
 git clone https://github.com/UZ-SLAMLab/ORB_SLAM3.git
 cd ORB_SLAM3/
```
ここで`CMakeLists.txt`を編集します
お好きなエディタで開いてください

L15~29のうち，`++11`を`++14`に置き換えます．
以下置き換えた結果
``` cmake
# Check C++14 or C++0x support
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
   add_definitions(-DCOMPILEDWITHC11)
   message(STATUS "Using flag -std=c++14.")
elseif(COMPILER_SUPPORTS_CXX0X)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
   add_definitions(-DCOMPILEDWITHC0X)
   message(STATUS "Using flag -std=c++0x.")
else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
```

つぎにL33~36で`4.4`を`4.2`にします．
最初に示したOpenCVのバージョンです．
人によっては違うのかもしれない．
ORB-SLAM3のReadme曰く
`Required at leat 3.0. Tested with OpenCV 3.2.0 and 4.4.0.`
とのこと

``` cmake
find_package(OpenCV 4.2)
   if(NOT OpenCV_FOUND)
      message(FATAL_ERROR "OpenCV > 4.2 not found.")
   endif()
```
find_packageでバージョンを範囲指定できるのでしょうか

編集した内容を保存してからビルドします．

``` bash
 chmod +x build.sh
 ./build.sh
```
warningが出るものの一応ビルドが通ればOKとしました．

## orb_slam3_ros_wrapper

ここでORB_SLAM3本家のセットアップを離れます．

https://github.com/thien94/orb_slam3_ros_wrapper

READMEを参考に以下
- 普通のROSのワークスペース(catkin_wsなど)のsrc内にcloneする
- CMakeLists.txtのL6修正(上記のディレクトリでインストールしてたら`$ENV{HOME}/ORB_SLAM/ORB_SLAM3`)
- __CMakeLists.txtのL70-76, L90-96をコメントアウトしてStereoシリーズを除外__\
(本家側のAPIが変わってしまったようでビルドが通りませんでした．ステレオカメラ使いたかったら調べて修正する必要がありそうです)
- ワークスペースに戻って`catkin_make`なり`catkin build`なり
- `cp ORB_SLAM/ORB_SLAM3/Vocabulary/ORBvoc.txt catkin_ws/src/orb_slam3_ros_wrapper/config/`
- `sudo apt install ros-noetic-hector-trajectory-server`

## usb_cam
取り敢えず普通のwebカメラでテストするためにいれます．他のパッケージ(libuvc-camera)とかでも問題ないかと

### Install

``` bash
sudo apt install ros-noetic-usb-cam
```

### usb_camに関連した修正
orb_slam3_ros_wrapperのeuroc_mono.launchでL4を
``` xml
<remap from="/camera/image_raw" to="/usb_cam/image_raw"/>
```
に変更

# 動かしてみる
``` bash
roslaunch orb_slam3_ros_wrapper euroc_mono.launch
```
別ターミナルで
``` bash
rosrun usb_cam usb_cam_node _video_device:=/dev/video0 _image_width:=1280 _image_height:=720
```

(カメラの種類によってはオプション変える必要があるかも)

なんか動いてなくはないけど…
euroc_monoimu.launchとの違いは何だろう．

# 参考先

https://github.com/UZ-SLAMLab/ORB_SLAM3

https://github.com/stevenlovegrove/Pangolin

https://learnopencv.com/how-to-find-opencv-version-python-cpp/

http://ithat.me/2015/10/11/opencv-version-confirm

https://atmarkit.itmedia.co.jp/ait/articles/1810/05/news019.html#:~:text=%E3%80%8Capt%2Dshow%2Dversions%E3%80%8D%E3%81%AF%E3%80%81Ubuntu%E3%81%AA%E3%81%A9Debian,%E3%83%90%E3%83%BC%E3%82%B8%E3%83%A7%E3%83%B3%E3%82%92%E7%A2%BA%E8%AA%8D%E3%81%A7%E3%81%8D%E3%81%BE%E3%81%99%E3%80%82

https://handaru.net/blog/2022/09/15/376/

https://ma38su.hatenablog.com/entry/2021/06/21/002240