2 用D435i运行VINS-fusion

环境：Ubuntu20.04 + ROS-Noetic

1 VINS-fusion的安装

1.1 环境和依赖的安装

编译VINS需要ROS，Ceres和Eigen，Ceres需要依赖Eigen，所以先安Eigen才能安装Ceres，但是由于安装ROS-noetic也自动通过apt安装了Eigen3.3.7版（这个版本还行，如果想安装其他版本可以通过源码方式安装），所以安装完ROS可以直接安装Ceres，Ceres安装1.14版即可，安装最新2.x版本会报错。

1. 安装ros-noetic

可以搜索鱼香ROS一键安装，很方便。

2. 安装Ceres1.14

开头说了Ceres的依赖Eigen已经在安装ros时通过apt安装了，但是我是通过源码安装了Eigen，因为上过SLAM十四讲的都会把一堆第三方库通过源码安装，能更好滴管理库的版本，用apt安装的大都是很新的版本，但是新的不一定稳定，可能会报一堆错，所以会选择源码的方式安装稳定版本的第三方库，那既然说到这了，就写一下用源码安装Eigen的过程，做不做这步你们随意。
安装Eigen3.3.9：
进入官网下载界面下载Eigen3.3.9的Source code(zip)，并解压，可以保存在一个文件夹里便于查看以及误删后的再次安装。
安装命令：

1 右键点击文件夹，选在终端打开
mkdir build && cd build
2 在build路径下输入
cmake ..
3 安装
sudo make install

简简单单就安装成功了，由于Eigen是由纯头文件组成的库，所以可以在/usr/local/include/eigen3/目录下看到。
由于有多个版本(多版本管理请自行查找)，可以用命令行查看当前使用的Eigen版本，
pkg-config --modversion eigen3，结果如下，

安装Ceres1.14：
进入官网下载界面下载Ceres1.14的源码并解压，如果不能进入github可以上gittee上搜一搜。
首先需要安装一些依赖：

# google-glog + gflags
sudo apt install libgoogle-glog-dev libgflags-dev

# Use ATLAS for BLAS & LAPACK
sudo apt install libatlas-base-dev

# SuiteSparse and CXSparse (optional)
sudo apt install libsuitesparse-dev

安装命令：

1 右键点击文件夹，选在终端打开
mkdir build && cd build
2 在build路径下输入
cmake ..
3 编译一下
make -j4
4 测试
make test
5 安装
sudo make install

至此Ceres也安装完成了，VINS-fusion的环境准备工作结束了。

1.2 编译VINS-Fusion

首先创建一个ros工作空间，可以和上篇文章Ubuntu20.04安装D435i相机驱动中的realsense-ros工作空间catkin_ws放在一起，这样后面用相机运行VINS就不用source两遍了。

cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ..
catkin_make

此时就会报一堆由OpenCV版本引起的错误。

1.3 编译错误解决方法

由于ROS-noetic安装时自动安装了OpenCV4.2，而VINS运行需要的是OpenCV3，主要在文件夹camera_model中两个文件添加缺失的头文件即可。
在下图两个文件中分别添加78两行头文件，

代码如下,可直接复制

#include <opencv2/imgproc/types_c.h>
#include <opencv2/imgproc/imgproc_c.h>

#include <opencv2/imgproc/types_c.h>
#include <opencv2/calib3d/calib3d_c.h>

接下来还会遇到几个not declared的错误，按下面方法对报错文件逐一进行修改即可。

1. error: ‘CV_FONT_HERSHEY_SIMPLEX’was not declared in this scope

将所有报这个错的文件中CV_FONT_HERSHEY_SIMPLEX 参数改为 cv::FONT_HERSHEY_SIMPLEX

2. error: ‘CV_LOAD_IMAGE_GRAYSCALE’ was not declared in this scope

将所有报这个错的文件中CV_LOAD_IMAGE_GRAYSCALE 参数改为 cv::IMREAD_GRAYSCALE

3. error: ‘CV_RGB2GRAY’ was not declared in this scope

将所有报这个错的文件中添加#include <opencv2/imgproc/types_c.h>

都修改完成之后就可以编译成功了。

2 VINS-Fusion跑数据集

以EuRoC数据集为例，这个数据集是ETH大学用无人机录制的，数据集可去官网下载（如果你现在还不能科学上网，你应该知道科学上网的重要性了），下载MH_01_easy.bag文件就行。
输入下列命令运行双目+IMU：

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml  
rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag

不带回环检测的命令及结果如下所示，

下面就可以用相机运行VINS-Fusion了，和跑数据集唯一的不同就是用相机的发布到话题的数据代替了ros bag记录的数据的播放。

3 用相机运行VINS-Fusion

修改文件realsense-ros中的rs_camera.launch文件，可以复制一个命名为rs_camera_vins.launch能保留原文件。直接复制下面代码就行，

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>

  <arg name="fisheye_width"       default="640"/>
  <arg name="fisheye_height"      default="480"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="640"/>
  <arg name="depth_height"        default="480"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="infra_width"        default="640"/>
  <arg name="infra_height"       default="480"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>

  <arg name="color_width"         default="640"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="30"/>
  <arg name="depth_fps"           default="30"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="30"/>
  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="250"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>

  <arg name="enable_sync"               default="true"/>
  <arg name="align_depth"               default="true"/>

  <arg name="publish_tf"                default="true"/>
  <arg name="tf_publish_rate"           default="0"/>

  <arg name="filters"                   default=""/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="linear_accel_cov"          default="0.01"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="unite_imu_method"          default="linear_interpolation"/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="publish_odom_tf"           default="true"/>
  <arg name="allow_no_texture_points"   default="false"/>
  <arg name="emitter_enable"   		default="false"/>

<!-- rosparam set /camera/stereo_module/emitter_enabled false -->
<rosparam>
  /camera/stereo_module/emitter_enabled: 0
</rosparam>

<rosparam if="$(arg emitter_enable)">
  /camera/stereo_module/emitter_enabled: 1
</rosparam>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
    </include>
  </group>
</launch>

主要改变就是把角速度和加速度话题合并成IMU话题，这样vins能接收到，还有一些改变可查看realsense-ros的readme查看具体意思。
接下来可以运行了，输入下列命令，

roslaunch vins vins_rviz.launch 
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
roslaunch realsense2_camera rs_camera_vins.launch 

这三条命令需要特别注意第二条命令，我们可以看到yaml存在的路径很长，所以自然想到可以先进入到~/catkin_ws/src/VINS-Fusion/config/realsense_d435i目录下运行rosrun vins vins_node realsense_stereo_imu_config.yaml,但是这会报一个惊天大错，Assertion “!px=0” failed，如下图所示，
这是一个智能指针错误，我一直以为和库的环境有关系，主要是被issue里一个大哥误导了，他说他重新安装了一个稳定的Eigen和Ceres就好了，但是我换了好几次有时候好使有时候不好使，卡了我两三天最后终于找到了这篇博客，大家可以点进去看看，在parameter.cpp中它规定的路径格式就是得带一个"/"，所以前边至少带一个上级目录，这样就不会报错了，当然像这篇博客中把configpath和/删掉之后就可以直接运行rosrun vins vins_node realsense_stereo_imu_config.yaml了。
我们可以看到图中定位已经飞了，所以需要对相机进行标定，下篇进入相机标定。