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
- 打开RVIZ
roslaunch vins vins_rviz.launch
- 打开VINS_Fusion
rosrun vins vins_node /home/guoben/Project/VINS_ws/src/VINS-Fusion/config/tum-vio/tum_mono.yaml
- 播放数据集
rosbag play Dataset/TUM-VIO/dataset-corridor4_512_16.bag