SLAMシリーズ　VINS-MonoとVINS-Fusionの環境構築　relasenseで実行

#環境
ubuntu16.04 / ROS-kinect
ubuntu18.04 / ROS-Melodic

両方で実証済みです。

# VINS-monoのインストール
参考にさせていただきました:bow:
https://qiita.com/PINTO/items/8989f45ad0908184e6e5

githubからインストール

```
sudo apt install libceres-dev libceres1
cd ~/catkin_ws/src
git clone https://github.com/PINTO0309/VINS-Mono.git
cd ../
catkin_make
```

ここでエラーが…

```
CMake Error at /usr/lib/cmake/ceres/CeresConfig.cmake:88 (message):
  Failed to find Ceres - Found Eigen dependency, but the version of Eigen
  found (3.3.4) does not exactly match the version of Eigen Ceres was
  compiled with (3.2.92).  This can cause subtle bugs by triggering
  violations of the One Definition Rule.  See the Wikipedia article
  http://en.wikipedia.org/wiki/One_Definition_Rule for more details
Call Stack (most recent call first):
  /usr/lib/cmake/ceres/CeresConfig.cmake:217 (ceres_report_not_found)
  VINS-Mono/camera_model/CMakeLists.txt:19 (find_package)


CMake Error at VINS-Mono/camera_model/CMakeLists.txt:19 (find_package):
  Found package configuration file:

    /usr/lib/cmake/ceres/CeresConfig.cmake

  but it set Ceres_FOUND to FALSE so package "Ceres" is considered to be NOT
  FOUND.
```
# エラー処理
https://blog.csdn.net/learning_tortosie/article/details/83377780

cmakeファイルをを編集

```
sudo chmod 777 CeresConfig.cmake　                   	
nano /usr/local/lib/cmake/Ceres/CeresConfig.cmake　
```
編集内容

```
set(Ceres_FOUND TRUE)                                #FALSE --> TRUE　70行目あたり
set(EIGEN_INCLUDE_DIR "/usr/local/include/eigen3")   #追加
set(CERES_EIGEN_VERSION 3.3.4)                       #3.3.29 --> 3.3.4  221行目あたり
```

そしてもう一度 

```
catkin_make
```

bashファイルに反映（ターミナルごとに必要）

```
source ~/workspace/catkin_ws/devel/setup.bash
```


#サンプルデータの実行

VINS-Monoの実行

```
roslaunch vins_estimator euroc.launch       
```
rvizで表示

```
roslaunch vins_estimator vins_rviz.launch   
```
grandtruthのダウンロード

```
roslaunch benchmark_publisher publish.launch sequence_name:=MH_05_difficult 
```
bagファイルの再生

```
rosbag play MH_05_difficult.bag             
```
※カメラとIMUをオンライン上でキャリブレーションした上でのVINS-Monoの実行コマンドはこちら
（１つめのコマンド）

```
roslaunch vins_estimator euroc_no_extrinsic_param.launch 
```



# VINS-Fusion
https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

まずはgithubからダウンロード

```
cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/workspace/catkin_ws/devel/setup.bash
※.bashrc に追記してないならターミナル毎に必要
```

#サンプルデータの実行

###サンプルデータのダウンロード

```
cd src/data #dataディレクトリは作ったもの
wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.bag
wget http://robotics.ethz.ch/~asl-datasets/ijrr_euroc_mav_dataset/machine_hall/MH_01_easy/MH_01_easy.zip
unzip MH_01_easy.zip;rm MH_01_easy.zip
```
###VINS-Fusionをサンプルデータを使って実行

まずrvizを起動

```
source workspace/catkin_ws/devel/setup.bash
roslaunch vins vins_rviz.launch
```

・Monocular+IMU の場合のコマンド

```
rosrun vins vins_node ~/workspace/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
(option)rosrun loop_fusion loop_fusion_node ~/workspace/catkin_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml 
```

・Stereo+IMU の場合のコマンド

```
rosrun vins vins_node ~/workspace/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
(option)rosrun loop_fusion loop_fusion_node ~/workspace/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
```

・stereo の場合のコマンド

```
rosrun vins vins_node ~/workspace/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml 
(option)rosrun loop_fusion loop_fusion_node ~/workspace/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_config.yaml
``` 

最後にbagファイルの再生

```
rosbag play ~/workspace/catkin_ws/src/data/MH_01_easy.bag
```

loop_fusionのコマンドを含めると、計４つのターミナルを同時に使います。

#realsenseで実行

realsenseのSDKのインストールはこちら

realsense-rosのディレクトリをシェルの設定に反映

```
source ~/workspace/realsense-ros/devel/setup.bash
```
realsenseのlaunchファイル実行

```
roslaunch realsense2_camera rs_camera.launch
```

※このファイルは下のディレクトリにあります。
```realsense_ros/src/realsense-ros/realsense2_camera/launch```
これでrelasenseがROS上で認識できる状態になります。


###いよいよ、realsens+vins-Fusionの実行
※シェル設定を各ターミナルに反映

```
source ~/workspace/catkin_ws/devel/setup.bash
```
rvizの起動

```
roslaunch vins vins_rviz.launch
```
vins-nodeを```realsense_stereo_imu_config.yaml```を参照しながら実行
※yamlファイルは```config```の中に予め用意されています。

```
rosrun vins vins_node ~/workspace/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
```
loop_fusionを同じく```realsense_stereo_imu_config.yaml```を参照しながら実行

```
rosrun loop_fusion loop_fusion_node ~/workspace/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yam
```

で無事に動くはずです。
こっちも計４つのターミナルが開かれています。



#webカメラで実行（まだ分かってないところ…）

pintgreyのflea3の場合

```
roslaunch pointgrey_camera_driver camera.launch
```

これをcam0[/cam0/image_raw]に送る方法が分からない…

webcameraの場合

```
roscore
rosrun uvc_camera uvc_camera_node
rosrun image_view image_view image:=/image_raw
```

これをcam0[/cam0/image_raw]に送る方法が分からない…