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

8627 ワード
ROS SLAM RealSense Ubuntu RealSenseD435i Ubuntu テキストリンク

環境

ubuntu16.04 / ROS-kinect
ubuntu18.04 / ROS-Melodic

両方で実証済みです。

VINS-monoのインストール

参考にさせていただきました
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.

エラー処理

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

まずは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]に送る方法が分からない…