VINS-Fusion运行Kaist数据集

Kaist数据集转rosbag

使用Kaist序列38，转换rosbag参考GithubKaist2bag，这里主要使用IMU,双目相机以及GPS数据。
附：其他的合并rosbag方法数据参考bag_merge。

#!/usr/bin/env pythonimport sys
import argparse
from fnmatch import fnmatchcasefrom rosbag import Bagdef main():parser = argparse.ArgumentParser(description='Merge one or more bag files with the possibilities of filtering topics.')parser.add_argument('outputbag',help='output bag file with topics merged')parser.add_argument('inputbag', nargs='+',help='input bag files')parser.add_argument('-v', '--verbose', action="store_true", default=False,help='verbose output')parser.add_argument('-t', '--topics', default="*",help='string interpreted as a list of topics (wildcards \'*\' and \'?\' allowed) to include in the merged bag file')args = parser.parse_args()topics = args.topics.split(' ')total_included_count = 0total_skipped_count = 0if (args.verbose):print("Writing bag file: " + args.outputbag)print("Matching topics against patters: '%s'" % ' '.join(topics))with Bag(args.outputbag, 'w') as o: for ifile in args.inputbag:matchedtopics = []included_count = 0skipped_count = 0if (args.verbose):print("> Reading bag file: " + ifile)with Bag(ifile, 'r') as ib:for topic, msg, t in ib:if any(fnmatchcase(topic, pattern) for pattern in topics):if not topic in matchedtopics:matchedtopics.append(topic)if (args.verbose):print("Including matched topic '%s'" % topic)o.write(topic, msg, t)included_count += 1else:skipped_count += 1total_included_count += included_counttotal_skipped_count += skipped_countif (args.verbose):print("< Included %d messages and skipped %d" % (included_count, skipped_count))if (args.verbose):print("Total: Included %d messages and skipped %d" % (total_included_count, total_skipped_count))if __name__ == "__main__":main()

运行Kaist rosbag数据集

Kaist相机配置文件：

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1280
image_height: 560
distortion_parameters:k1: -5.6143027800000002e-02k2: 1.3952563200000001e-01p1: -1.2155906999999999e-03p2: -9.7281389999999998e-04
projection_parameters:fx: 816.90378992770002fy: 811.56803828490001cx: 608.50726281690004cy: 263.47599764440002

总的配置文件：

%YAML:1.0#common parameters
imu: 1
num_of_cam: 1  imu_topic: "/imu0"
image0_topic: "/cam0/image_raw"
image1_topic: "/cam1/image_raw"
output_path: "/home/Workspace/RosWorkspace/catkin_ws_vins_fusion/src/VINS-Fusion/output/"cam0_calib: "kaist_cam.yaml"
cam1_calib: "kaist_cam.yaml"
image_width: 1280
image_height: 560# 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.body_T_cam0: !!opencv-matrixrows: 4cols: 4dt: ddata: [-6.80499000e-03, -1.53215000e-02, 9.99850000e-01, 1.71239,-9.99977000e-01,  3.34627000e-04, -6.80066000e-03, 0.247401,-2.30383000e-04, -9.99883000e-01, -1.53234000e-02, -0.11589,0, 0, 0, 1]body_T_cam1: !!opencv-matrixrows: 4cols: 4dt: ddata: [-6.80499000e-03, -1.53215000e-02, 9.99850000e-01, 1.71239,-9.99977000e-01,  3.34627000e-04, -6.80066000e-03, 0.247401,-2.30383000e-04, -9.99883000e-01, -1.53234000e-02, -0.11589,0, 0, 0, 1]#Multiple thread support
multiple_thread: 1#feature traker paprameters
max_cnt: 200            # 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.08  # max solver itration time (s), to guarantee real time
max_num_iterations: 10   # max solver itrations, to guarantee real time
keyframe_parallax: 15 # keyframe selection threshold (pixel)#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.02          # accelerometer measurement noise standard deviation. #0.2   0.04
gyr_n: 0.003         # gyroscope measurement noise standard deviation.     #0.05  0.004
acc_w: 0.005         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 3.0e-5       # gyroscope bias random work noise standard deviation.     #4.0e-5
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: "/home/Workspace/RosWorkspace/catkin_ws_vins_fusion/src/VINS-Fusion/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

运行指令：

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/kaist/kaist_config.yaml 
rosrun global_fusion global_fusion_node
rosbag play Kaist38_gps.bag

运行结果：
前期由于ENU坐标系与VIO坐标系没有对齐，误差稍大，收敛之后整体效果尚可，但是在个别路段由于GPS的定位偏差较大，优化出的位姿效果没有预期的好。