2、 D435i ----- Ubuntu18.04使用D435i运行VINS-Mono

---

运行环境

• Linux：Ubuntu18.04
• ros：Melodic
• Ceres优化库（Ubuntu18.04安装Ceres教程）

---

1. 新建ROS工程目录文件夹

mkdir catkin_ws
cd catkin_ws
mkdir src
catkin_make

---

2. 在src文件夹下，下载安装VINS-Mono相关源码功能包并进行编译

https://github.com/intel-ros/realsense.git ：D435i 深度相机的相关SDK 2.0 开发包
https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git ： VINS-Mono相关源码

cd src
git clone https://github.com/intel-ros/realsense.git
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git
cd ..
catkin_make

---

注意：出现相关的错误
原因：ROS未安装相关依赖包

解决办法：

sudo apt-get install ros-melodic-ddynamic-reconfigure

---

测试相机开启

roslaunch realsense2_camera rs_camera.launch filters:=pointcloud

打开rviz，add -> by Topic
添加PointCloud2的mudule就可以看到点云，如下

---

3. 下载相关数据集，并用数据集的进行测试

3.1 下载数据集，并用数据集的进行测试

数据集的下载网站链接： 数据集下载链接
https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets

该测试下载的是数据集包为MH_01_easy.bag（大小为2.5G）

3.2 用数据集的进行测试

source ~/catkin_ws/devel/setup.bash
roslaunch vins_estimator euroc.launch
roslaunch vins_estimator vins_rviz.launch

运用rosbag play运行数据集包，其中下载的数据集包为MH_01_easy.bag（大小为2.5G）

rosbag play MH_01_easy.bag 

3.3 测试结果

• 数据集测试得到的相关窗口
  

---

4. 利用 D435i 深度相机运行 VINS-Mono

4.1 修改相机启动参数文件

1）.修改realsense功能包里的rs_camera.launch文件

• 第一处，修改unite_imu_method为copy
• 并将enable_gyro和enable_acce参数修改为true
• 这样才能让IMU的角速度和加速度作为一个topic输出，输出的话题为/camera/imu

<arg name="unite_imu_method"      default="copy"/>

<arg name="enable_gyro"         default="true"/>
<arg name="enable_accel"        default="true"/>

• 第二处，修改enable_sync参数为true，这里是开机相机和IMU的同步

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

2）.检验输出话题是否含有/camera/imu

roslaunch realsense2_camera rs_camera.launch
rostopic list

话题如下：

/camera/accel/imu_info
/camera/color/camera_info
/camera/color/image_raw
/camera/depth/camera_info
/camera/depth/image_rect_raw
/camera/extrinsics/depth_to_color
/camera/gyro/imu_info
/camera/imu
/camera/motion_module/parameter_descriptions
/camera/motion_module/parameter_updates
/camera/realsense2_camera_manager/bond
/camera/rgb_camera/auto_exposure_roi/parameter_descriptions
/camera/rgb_camera/auto_exposure_roi/parameter_updates
/camera/rgb_camera/parameter_descriptions
/camera/rgb_camera/parameter_updates
/camera/stereo_module/auto_exposure_roi/parameter_descriptions
/camera/stereo_module/auto_exposure_roi/parameter_updates
/camera/stereo_module/parameter_descriptions
/camera/stereo_module/parameter_updates
/diagnostics
/rosout
/rosout_agg
/tf
/tf_static

---

4.2 修改VINS-Mono包里的相关配置文件

修改VINS-Mono包里config文件夹中的realsense_color_config.yaml文件

• 第一处，修改订阅的topic

imu_topic: "/camera/imu"
image_topic: "/camera/color/image_raw"

• 第二处，修改相机内参，这里先再次打开运行realsesne包，然后可以通过如下命令获取相机内参

rostopic echo /camera/color/camera_info

相关/camera/color/camera_info的参数解释可以参考该链接

其中主要修改相机的图片大小相关参数，以及根据相机的内参矩阵K对相机的内参进行修改（文章下面章节会对其进行相关描述）

image_width: 1280
image_height: 720

• 第三处，IMU到相机的变换矩阵，这里根据注释的提示修改成2

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2   # 0  Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
                        # 1  Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
                        # 2  Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.                        
#If you choose 0 or 1, you should write down the following matrix.

• 第四处，IMU参数，这里全部修改注释给的参数

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.2          # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05         # gyroscope measurement noise standard deviation.     #0.05
acc_w: 0.02         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 4.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80       # gravity magnitude

• 第五处，是否需要在线估计同步时差，根据其它博主的建议这里选择不需要

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.000                           # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

• 第六处，相机曝光改成全局曝光

#rolling shutter parameters
rolling_shutter: 0                      # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0               # unit: s. rolling shutter read out time per frame (from data sheet). 

---

4.3 打开摄像头，运行VINS-Mono

roslaunch realsense2_camera rs_camera.launch 
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

• 运行VINS-Mono得到的相关窗口

下图为没有更改相机内参矩阵K的相关参数，可以看到相机的位姿轨迹漂移很严重

---

修改相机内参，解决相关的漂移问题

修改VINS-Mono包里config文件夹中的realsense_color_config.yaml文件，修改相关的相机内参配置文件

• 利用以下命令查看相机内参

rostopic echo /camera/color/camera_info

下面为作者相关的相机内参情况如下

---
header: 
  seq: 247
  stamp: 
    secs: 1637989805
    nsecs: 441106319
  frame_id: "camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [908.592529296875, 0.0, 649.6251220703125, 0.0, 906.947021484375, 374.77374267578125, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [908.592529296875, 0.0, 649.6251220703125, 0.0, 0.0, 906.947021484375, 374.77374267578125, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

相机的内参矩阵K的定义

$$D=\left[\begin{array}{ccccc}k1& k2& t1& t2& k3\end{array}\right]$$

$$K=\left[\begin{array}{ccc}fx& 0& cx\\ 0& fy& cy\\ 0& 0& 0\end{array}\right]$$

根据相机中的相关参数进行realsense_color_config.yaml文件的参数修改，下面为作者的相机参数修改情况

#camera calibration 
model_type: PINHOLE
camera_name: camera
image_width: 1280
image_height: 720
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 908.592529296875
   fy: 906.947021484375
   cx: 649.6251220703125
   cy: 374.77374267578125

• 修改完相关参数，重新运行VINS-Mono算法

roslaunch realsense2_camera rs_camera.launch 
roslaunch vins_estimator realsense_color.launch 
roslaunch vins_estimator vins_rviz.launch

修改完相机配置相关参数后的运行情况，可以看出相关的漂移情况得到了改善
注意：在相机开始时，让相机左右进行平移运动，比较容易让相机进行初始化

---

相关参考链接：

• Ubuntu 18.04 测试安装D435i
• 从零开始使用Realsense D435i运行VINS-Mono
• 如何用Realsense D435i运行VINS-Mono等VIO算法 获取IMU同步数据

---

相关VINS-Mono参考链接：

• 【SLAM】VINS-MONO解析——综述
• SLAM算法总结——经典SLAM算法框架总结

---