1. 简介

上一篇跑了一下VINS_Fusion自带的demo，用的是几个常用的开源数据集，这篇文章主要是将VINS_Fusion用在自己的实验室设备上，在进行前期参数标定、config文件修改、精度验证过程中对算法有更深次理解，也方便后期开展代码阅读。

2.相机参数标定
2.1相机型号
realsense_d435i是一个很常用的相机，主要包含话题如下

1.imu话题          名称：/camera/imu               类型：sensor_msgs/Imu
2.相机原始数据     名称:/camera/image_raw     类型：sensor_msgs/Image
3.相机深度信息     名称：/camera/image_rect   类型：sensor_msgs/Image

2.2 相机参数标定

一篇写的很好的文章

3.cofig文件配置
在跑VINS_Fusion自带demo时可以看出来，基本上是不同算法不同数据集只需要换一个config文件，所以我们在用Realense_D435i跑算法的重点就是配置这个yaml文件.本小节先给出最终的config文件，然后解释其中每一部分参数的作用

3.1单目+IMU的config文件
在3.2-3.5中出现和3.1参数设置不一致情况，以3.1为准

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 1 

imu_topic: "/camera/imu"
image0_topic: "/camera/color/image_raw"
output_path: "/home/dji/output/"

cam0_calib: "left.yaml"
image_width: 640
image_height: 480

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 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.
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [0.9992453, -0.01127283, -0.00488404, -0.0087336,
               0.0112394,   0.9999137,   -0.00679953, 0.0068368,
               0.0049602,  0.00674412,   0.9999649, 0.01134131,
          0, 0, 0, 1]
          
#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.001         # accelerometer bias random work noise standard deviation.  #0.002
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.805         # gravity magnitude

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

#loop closure parameters
load_previous_pose_graph: 1        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/dji/output/pose_graph/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 

3.2 基本参数

#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam;  分别表示的是单目+IMU，双目+IMU，双目 在这里设置选用的话题名 以及选用相机的标定文件
imu: 1         
num_of_cam: 1 
imu_topic: "/camera/imu" #imu话题名
image0_topic: "/camera/color/image_raw" #相机原始图像话题名
output_path: "/home/dji/output/"

3.3传感器参数

①相机内参

cam0_calib: "left.yaml" #左相机内参
image_width: 640 #图像宽度
image_height: 480 #图像高度

left.yaml

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
   k1: 0.10866995537152147
   k2: -0.23049837670803422
   p1: 0.002195369
   p2: -0.000499223
projection_parameters:
   fx: 588.2104060261963
   fy: 589.1738695825263
   cx: 338.16491234179625
   cy: 252.1523457447961

right.yaml
和left.yaml格式一样 具体数据是多少取决于你的标定结果
②imu标定参数
imu标定包含可标定噪声（比如零偏，安装误差）和不可标定参数（噪声协方差）

#imu parameters       T
acc_n: 0.1          # 加速度计噪声标准差
gyr_n: 0.01         # 陀螺仪噪声标准差
acc_w: 0.001         # 加速度计随机游走标准差
gyr_w: 0.0001       # 陀螺仪随机游走标准差
g_norm: 9.805         # 重力加速度

③IMU和相机外参
这里说一下：一般相机标定，内参是固定的，外参是随着每次实验安装不同，但是我们用的D435i imu和相机是集成的，所以基本上外参也不太变化。

#这是一个标志位 ，是（1）否（0）在数据融合过程中优化相机外参。
#在imu和camera深耦合过程中可以优化下边的ody_T_cam0 和body_T_cam1 ，这里选择是否优化
estimate_extrinsic: 1
#左相机和imu的外参 核心就是4*4的位姿矩阵，多了三个参数，分别描述行数，列数，单位
body_T_cam0: !!opencv-matrix # Timu2c_1 Tu2c
   rows: 4
   cols: 4
   dt: d
   data: [0.9992453, -0.01127283, -0.00488404, -0.0087336,
          0.0112394,   0.9999137,   -0.00679953, 0.0068368,
          0.0049602,  0.00674412,   0.9999649, 0.01134131,
          0, 0, 0, 1]

3.4数据融合参数

其实在更换相机时，这些参数默认即可
①多线程参数
在回调函数读取数据过程中，避免线程冲突，会加锁，所以这里设置允许的线程数，默认1即可

multiple_thread: 1

②特征点提取参数

max_cnt: 150            # 特征点追踪中最大的特征个数
min_dist: 30            # 两个特征点之间最小的距离
freq: 10                # 发布融合结果的频率，一般不小于10，当设置为0，那么发布的频率==image的频率
F_threshold: 1.0       
show_track: 0        #标志位参数：是(1)否(0)  把特征点作为话题发布出来
flow_back: 1         #标志位参数：是(1)否(0)  执行前向和反向光流来提高特征点提取精度

③优化相关参数

max_solver_time: 0.04  # 最大求解器迭代时间（毫秒），保证实时性
max_num_iterations: 8   # 最大求解器迭代次数，以保证实时性
keyframe_parallax: 10.0 # 关键帧选择阈值（像素）

④时间同步相关参数

estimate_td: 0                      # 标志位参数    在线估计相机和 IMU 之间的时间偏移；如果你的相机和IMU时间是对齐的，就关闭即可
td: 0.00                             # 时间偏移的初始值。单位：s. 读取的图像时钟 + td = 真实图像时钟（IMU 时钟）
# 如果你的相机和IMU初始时间没有对准，那么需要设置td来做对齐

⑤闭环检测相关参数

load_previous_pose_graph: 0        # 标志位参数： 是(1)否(0)从“pose_graph_save_path”加载和重用以前的pose 图
pose_graph_save_path: "/home/dji/output/pose_graph/" # 保存和加载路径
save_image: 1                  # 将图像保存在姿势图中以进行可视化;您可以通过设置 0 关闭此功能

4.代码运行

4.1代码环境

注意config文件的目录，使用红框标记的两个yaml

4.2运行代码

1.运行rviz
roslaunch vins vins_rviz.launch
2.数据融合代码
rosrun vins vins_node /home/passoni/pedestrian/src/VINS-Fusion/config/realsense_d435i/realsense_mono_imu_config.yaml
3.回环（可选）
rosrun loop_fusion loop_fusion_node  /home/passoni/pedestrian/src/VINS-Fusion/config/realsense_d435i/realsense_mono_imu_config.yaml
4.播放rosbag包
rosbag play  data3.bag

4.3运行结果