ZED2双目相机运行VINS-FUSION初试

基于ZED2运行VINS-MONO的经验，在运行VINS-FUSION前有几点tips想跟大家分享，应该能在运行的过程中避免一些踩坑吧。

TIPs

1）关于zed2标定
开始使用Kalibr进行相机单目/双目及联合标定，虽然重投影误差最高也就5个像素，但实际使用效果却并不好，故建议大家相机内参还是从ZED2相机发布出的topic获取或可尝试/usr/local/zed/settings文件下的参数文档
另，启动ZED2.launch的时候，会有一个相机(一般是左目)和IMU的轴系转换关系Camera-IMU Transform，这个是系统值，最好还是自行标定。

2）关于运行频率
这个基于自己电脑（实验设备）性能可进行实际调试，可适当降低分辨率和帧率进行测试，尤其运行双目时，否则系统可能会卡掉
降帧命令：

rosrun topic_tools throttle messages old_topic hz new_topic//hz替换成测试帧率

3）关于实验
实验过程中尽量保持相机平稳，否则很容易造成位置较大漂移

运行VINS-Fusion

1）按照标定结果更改配置文件中的参数，包括cam0.yaml、cam1.yaml、zed2_stereo_config.yaml

%YAML:1.0

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

imu_topic: "/zed2/zed_node/imu/data2"
image0_topic: "/zed2/zed_node/left/image_rect_gray"
image1_topic: "/zed2/zed_node/right/image_rect_gray"
output_path: "~"

cam0_calib: "cam0.yaml"
cam1_calib: "cam1.yaml"
image_width: 672
image_height: 376
   

# 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: 填写自己的旋转矩阵参数
   
body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: 填写自己的旋转矩阵参数
   
#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. 
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     
acc_w: 0.001        # accelerometer bias random work noise standard deviation.  
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     
g_norm: 9.81007     # gravity magnitude

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

#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/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 

另外，需修改IMU参数：
imu_tils官网给的IMU单位：
根据IMU的单位，这里需要相应的乘以或除以根号频率
acc_n=acc_n/(sqrt(1/imu frequency (hz)))
gyr_n=gyr_n/(sqrt(1/imu frequency (hz)))
acc_w=acc_w*(sqrt(1/imu frequency (hz)))
gyr_w=gyr_w*(sqrt(1/imu frequency (hz)))

终端运行命令：

roscore
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/vins-fusion/src/VINS-Fusion/config/zed/zed2_stereo_config.yaml

启动ZED2的ROS节点：

roscore
roslaunch zed_wrapper zed2.launch

加上回环检测部分：

rosrun loop_fusion loop_fusion_node ~/vins-fusion/src/VINS-Fusion/config/zed/zed2_stereo_config.yaml