在TUMVI数据集上测试VINS-Fusion算法

VINS_Fusion算法是一个非常优秀的视觉惯性里程计,但原版VINS_Fusion并没有提供与TUM数据集相应的配置文件,因此需要自己进行写yaml文件.

修改配置文件

tum_mono.yaml

%YAML:1.0

imu: 1         
num_of_cam: 1  

#common parameters
imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
output_path: "/home/guoben/output"

cam0_calib: "cam0.yaml"
image_width: 512
image_height: 512

# 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.
                        # 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.
#Rotation from camera frame to imu frame, imu^R_cam
body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ -9.9951465899298464e-01, 7.5842033363785165e-03, -3.0214670573904204e-02, 4.4511917113940799e-02,
            2.9940114644659861e-02, -3.4023430206013172e-02, -9.9897246995704592e-01, -7.3197096234105752e-02,
            -8.6044170750674241e-03, -9.9939225835343004e-01, 3.3779845322755464e-02 ,-4.7972907300764499e-02,
            0,   0,    0,    1]

#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 25            # 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
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 1              # if using fisheye, trun on it. A circle mask will be loaded to remove edge noisy points

#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.04          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.004         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.0004         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.80766     # 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)

#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). 

#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/tony-ws1/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 

cam0.yaml
%YAML:1.0
---
model_type: KANNALA_BRANDT
camera_name: camera
image_width: 512
image_height: 512
mirror_parameters:
   xi: 3.6313355285286337e+00
   gamma1: 2.1387619122017772e+03
projection_parameters:
   k2: 0.0034823894022493434
   k3: 0.0007150348452162257
   k4: -0.0020532361418706202
   k5: 0.00020293673591811182
   mu: 190.97847715128717
   mv: 190.9733070521226
   u0: 254.93170605935475
   v0: 256.8974428996504

测试

需要打开三个Terminal

1. 打开RVIZ

roslaunch vins vins_rviz.launch

2. 打开VINS_Fusion

rosrun vins vins_node /home/guoben/Project/VINS_ws/src/VINS-Fusion/config/tum-vio/tum_mono.yaml 

3. 播放数据集

 rosbag play Dataset/TUM-VIO/dataset-corridor4_512_16.bag