D455+VINS-Fusion
环境：
Ubuntu20.04 ros:noetic
参考：D455+VINS-Fusion+surfelmapping 稠密建图（一）_全日制一起混的博客-CSDN博客
1. 编译VINS-Fusion
mkdir -p catkin_ws/src
cd catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ..
catkin_make2. 运行数据集
roslaunch vins vins_rviz.launch
rosrun vins vins_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosrun loop_fusion loop_fusion_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
rosbag play /你自己的路径/MH_01_easy.bag3. D455相机实测
相机sdk和相应ros包的安装参考D455启动教程
然后我们开始配置D455相机的参数文件，将config下的realsense_d435i文件夹复制粘贴重命名为realsense_d455，将里面的left.yaml、right.yaml进行修改，然后配置realsense_stereo_imu_config.yaml
left.yaml:
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.007017020579074508
k2: 0.013075992589794715
p1: -0.0037765402744058983
p2: -0.0005132729806830811
projection_parameters:
fx: 437.44398421645786
fy: 437.72233141976125
cx: 430.95314113824475
cy: 231.60352693067642right.yaml:
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 480
distortion_parameters:
k1: 0.018914793505132418
k2: -0.0026985776594766744
p1: -0.0025567868843695152
p2: 0.00645205341789554
projection_parameters:
fx: 432.2163360247922
fy: 431.7881804665646
cx: 433.3636493084969
cy: 232.14028787830168realsense_stereo_imu_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: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "/home/nvidia/Documents/vslam_test/src/VINS-Fusion/config/realsense_d455/output/"
cam0_calib: "left.yaml"
cam1_calib: "right.yaml"
image_width: 640
image_height: 480
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 0 # 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.
#相机到imu的变换矩阵
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [ 0.99981999, 0.00163688 , 0.0189025 , -0.00089318,
-0.00146025 , 0.99995518 ,-0.00935466 ,0.00021664,
-0.01891696, 0.00932537 , 0.99977757 ,0.00065183,
0. , 0. , 0. , 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [0.99977591, -0.0005314 , 0.0211626, 0.0949139 ,
0.00066889 , 0.99997871, -0.00649049, -0.00027429,
-0.0211587 , 0.00650319 , 0.99975498 , 0.0021304 ,
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: 0 # 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: 1.7574789006499388e-02 # accelerometer measurement noise standard deviation. #0.2 0.04
gyr_n: 1.8351398172861977e-03 # gyroscope measurement noise standard deviation. #0.05 0.004
acc_w: 5.3103238396236881e-04 # accelerometer bias random work noise standard deviation. #0.002
gyr_w: 1.3154828587252936e-05 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.78921469 # gravity magnitude
#unsynchronization parameters
estimate_td: 1 # online estimate time offset between camera and imu
td: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: "/home/nvidia/Documents/vslam_test/src/VINS-Fusion/config/realsense_d455/output/pose_graph/" # save and load path
save_image: 0 # save image in pose graph for visualization prupose; you can close this function by setting 03.1 开始实际运行
打开vins-fusion自带显示rviz：
roslaunch vins vins_rviz.launch
打开相机:
roslaunch realsense2_camera stereo-imu.launch
stereo-imu.launch:
/camera/stereo_module/emitter_enabled: 0
/camera/stereo_module/emitter_enabled: 1
开启跟踪节点：
rosrun vins vins_node src/VINS-Fusion/config/realsense_d455/realsense_stereo_confit.yaml
开启闭环
rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d455/realsense_stereo_confit.yaml