0-VINS在自己的相机上运行

1.环境

ubuntu18.04;ros1;opencv 的OAK-D-W

2.相机驱动以及标定参考

https://www.cnblogs.com/gary-guo/p/16567389.html

3.编译vins-fusion

下载https://github.com/HKUST-Aerial-Robotics/VINS-Fusion

直接编译即可；

配置yaml文件

oak-d-w.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: "/stereo_inertial_publisher/imu"
image0_topic: "/stereo_inertial_publisher/left/image_rect"
image1_topic: "/stereo_inertial_publisher/right/image_rect"
output_path: "/home/gary/VINS/vins_fusion_ws/output/data"

cam0_calib: "oak-left.yaml"
cam1_calib: "oak-right.yaml"
image_width: 1280
image_height: 720
   

# 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.00635382, 0.99990027,
       0.01261288, -0.00024044,
       0.99983888, 0.00656418,
       -0.01670701, 0.0001788,
       -0.01678814, 0.01250469,
       -0.99978087, 0.00007448, 0., 0., 0., 1. ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -0.00871704, 0.99988493,
       0.01241492, -0.07392172,
       0.99983161, 0.00891572,
       -0.01603926, -0.00081612,
       -0.0161481, 0.01227302,
       -0.99979428, -0.00052702, 0., 0., 0., 1. ]

#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 10            # min distance between two features 30
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: 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: 1                      # 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: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "/home/gary/VINS/vins_fusion_ws/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 

oak-left.yaml

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1280
image_height: 720
distortion_parameters:
   k1: 0.0034134236898324694
   k2: -0.005784037819853257
   p1: 0.006612629714095522
   p2: -6.620438974374915e-05
projection_parameters:
   fx: 549.8435891287721
   fy: 548.7383777426921
   cx: 614.0003853040437
   cy: 333.32127862399466

oak-right.yaml

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1280
image_height: 720
distortion_parameters:
   k1: 0.0019263641724087306
   k2: -0.007969836175443666
   p1: 0.007617163141992544
   p2: -3.010542501917051e-05
projection_parameters:
   fx: 548.1775690210155
   fy: 547.319988333231
   cx: 616.4979955625082
   cy: 335.0018773143142

具体数据参考标定内容

说明一下，在这里imu标定的内容放进去好像效果没那么好了，不知道为什么

4.运行

相机驱动：位于：/home/gary/oak_d_wide_ws
roslaunch depthai_examples stereo_inertial_node.launch

运行vins-fusion:

位于：/home/gary/VINS/vins_fusion_ws

可视化：roslaunch vins vins_rviz.launch

运行vio：rosrun vins vins_node /home/gary/VINS/vins_fusion_ws/src/VINS-Fusion/config/realsense_d435i/oak-d-w.yaml

运行回环：rosrun loop_fusion loop_fusion_node /home/gary/VINS/vins_fusion_ws/src/VINS-Fusion/config/realsense_d435i/oak-d-w.yaml

完成！

5.运行结果展示