源碼地址

源碼鏈接：https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git

電腦配置

Ubuntu 18.04 + ROS Melodic + GTSAM 4.0.2 + CERES 1.14.0
pcl1.8+vtk8.2.0+opencv3.2.0

環境配置

之前已經配置過LVI-SAM的環境，所以沒有什么額外需要配置的（可參考之前的博客）

編譯

 cd ~/catkin_ws/srcgit clone https://github.com/HKUST-Aerial-Robotics/VINS-Mono.gitcd ..catkin_make -j2

注：直接catkin_make會死機

適配Kitti數據集

在config文件夾下新建kitti文件夾
新建kitti_config.yaml文件
(具體參數設置的方式，可以參考之前LVI-SAM博客)

%YAML:1.0#common parameters
imu_topic: "/imu_raw"   #"/kitti/oxts/imu"
image_topic: "/kitti/camera_gray_left/image_raw"
output_path: "/home/nssc/sbk/outputs/map/vinsmoon/"#camera calibration 
model_type: PINHOLE
camera_name: camera
#10_03
# image_width: 1241
# image_height: 376
# 09_30
image_width: 1226
image_height: 370
distortion_parameters:k1: 0.0k2: 0.0p1: 0.0p2: 0.0
projection_parameters:
# 10_03
#   fx: 7.188560e+02
#   fy: 7.188560e+02
#   cx: 6.071928e+02
#   cy: 1.852157e+02# 09_30fx: 7.070912e+02fy: 7.070912e+02cx: 6.018873e+02cy: 1.831104e+02# 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
extrinsicRotation: !!opencv-matrixrows: 3cols: 3dt: d#  10_03# data: [0.00875116, -0.00479609,  0.99995027, -0.99986428, -0.01400249,  0.00868325, 0.01396015, -0.99989044, -0.00491798]#  09_30data: [0.00781298, -0.0042792,  0.99996033,-0.99985947, -0.01486805,  0.00774856, 0.0148343 , -0.99988023, -0.00439476]     #Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrixrows: 3cols: 1dt: d#  10_03# data: [1.10224312,-0.31907194,  0.74606588]
#09_30 data: [1.14389871,-0.31271847,  0.72654605]#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: 1           # publish tracking image as topic
equalize: 1             # if image is too dark or light, trun on equalize to find enough features
fisheye: 0              # 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.08          # 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.00004         # accelerometer bias random work noise standard deviation.  #0.02
gyr_w: 2.0e-6       # gyroscope bias random work noise standard deviation.     #4.0e-5
g_norm: 9.81007     # gravity magnitude#loop closure parameters
loop_closure: 1                    # start loop closure
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
fast_relocalization: 0             # useful in real-time and large project
pose_graph_save_path: "/home/nssc/sbk/outputs/map/vinsmoon/pose_graph/" # save and load path#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). #visualization parameters
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0 
visualize_imu_forward: 0        # output imu forward propogation to achieve low latency and high frequence results
visualize_camera_size: 0.4      # size of camera marker in RVIZ

在vins_estimator/launch/文件夾下新建文件kitti.launch
（主要修改一下config_path的路徑）

<launch><arg name="config_path" default = "$(find feature_tracker)/../config/kitti/kitti_config.yaml" /><arg name="vins_path" default = "$(find feature_tracker)/../config/../" /><node name="feature_tracker" pkg="feature_tracker" type="feature_tracker" output="log"><param name="config_file" type="string" value="$(arg config_path)" /><param name="vins_folder" type="string" value="$(arg vins_path)" /></node><node name="vins_estimator" pkg="vins_estimator" type="vins_estimator" output="screen"><param name="config_file" type="string" value="$(arg config_path)" /><param name="vins_folder" type="string" value="$(arg vins_path)" /></node><node name="pose_graph" pkg="pose_graph" type="pose_graph" output="screen"><param name="config_file" type="string" value="$(arg config_path)" /><param name="visualization_shift_x" type="int" value="0" /><param name="visualization_shift_y" type="int" value="0" /><param name="skip_cnt" type="int" value="0" /><param name="skip_dis" type="double" value="0" /></node></launch>

運行結果

Euroc數據集

 roslaunch vins_estimator euroc.launch roslaunch vins_estimator vins_rviz.launchrosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag 

同時看到groundtrue:

roslaunch benchmark_publisher publish.launch sequence_name:=MH_01_easy

kitti數據集

有關kitti數據集生成bag包的方式，可參考之前生成LVI-SAM適配數據的博客

 roslaunch vins_estimator kitti.launch roslaunch vins_estimator vins_rviz.launchrosbag play YOUR_PATH_TO_DATASET/ rosbag play kitti_2011_09_30_drive_0027_synced.bag

evo評估（KITTI數據）

輸出軌跡(tum格式)

vins_estimator/src/utility/visualization.cpp
pubOdometry()函數150+行

        // write result to file// ofstream foutC(VINS_RESULT_PATH, ios::app);// foutC.setf(ios::fixed, ios::floatfield);// foutC.precision(0);// foutC << header.stamp.toSec() * 1e9 << ",";// foutC.precision(5);// foutC << estimator.Ps[WINDOW_SIZE].x() << ","//       << estimator.Ps[WINDOW_SIZE].y() << ","//       << estimator.Ps[WINDOW_SIZE].z() << ","//       << tmp_Q.w() << ","//       << tmp_Q.x() << ","//       << tmp_Q.y() << ","//       << tmp_Q.z() << ","//       << estimator.Vs[WINDOW_SIZE].x() << ","//       << estimator.Vs[WINDOW_SIZE].y() << ","//       << estimator.Vs[WINDOW_SIZE].z() << "," << endl;ofstream foutC(VINS_RESULT_PATH, ios::app);foutC.setf(ios::fixed, ios::floatfield);foutC.precision(9);foutC << header.stamp.toSec() << " ";foutC.precision(5);foutC << estimator.Ps[WINDOW_SIZE].x() << " "<< estimator.Ps[WINDOW_SIZE].y() << " "<< estimator.Ps[WINDOW_SIZE].z() << " "<< tmp_Q.x() << " "<< tmp_Q.y() << " "<< tmp_Q.z() << " "<< tmp_Q.w() << endl;foutC.close();

pose_graph/src/pose_graph.cpp
addKeyFrame()函數150+行

    if (SAVE_LOOP_PATH){// ofstream loop_path_file(VINS_RESULT_PATH, ios::app);// loop_path_file.setf(ios::fixed, ios::floatfield);// loop_path_file.precision(0);// loop_path_file << cur_kf->time_stamp * 1e9 << ",";// loop_path_file.precision(5);// loop_path_file  << P.x() << ","//       << P.y() << ","//       << P.z() << ","//       << Q.w() << ","//       << Q.x() << ","//       << Q.y() << ","//       << Q.z() << ","//      << endl;ofstream loop_path_file(VINS_RESULT_PATH, ios::app);loop_path_file.setf(ios::fixed, ios::floatfield);loop_path_file.precision(0);loop_path_file << cur_kf->time_stamp << " ";loop_path_file.precision(5);loop_path_file  << P.x() << " "<< P.y() << " "<< P.z() << " "<< Q.x() << " "<< Q.y() << " "<< Q.z() << " "<< Q.w() << endl;    loop_path_file.close();}

updatePath()函數600+行

        if (SAVE_LOOP_PATH){// ofstream loop_path_file(VINS_RESULT_PATH, ios::app);// loop_path_file.setf(ios::fixed, ios::floatfield);// loop_path_file.precision(0);// loop_path_file << (*it)->time_stamp * 1e9 << ",";// loop_path_file.precision(5);// loop_path_file  << P.x() << ","//       << P.y() << ","//       << P.z() << ","//       << Q.w() << ","//       << Q.x() << ","//       << Q.y() << ","//       << Q.z() << ","//       << endl;ofstream loop_path_file(VINS_RESULT_PATH, ios::app);loop_path_file.setf(ios::fixed, ios::floatfield);loop_path_file.precision(0);loop_path_file << (*it)->time_stamp << " ";loop_path_file.precision(5);loop_path_file  << P.x() << " "<< P.y() << " "<< P.z() << " "<< Q.x() << " "<< Q.y() << " "<< Q.z() << " "<< Q.w() << endl;loop_path_file.close();}

pose_graph_node.cpp中的main()函數

       # VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv";VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";

對輸出的vins_result_loop.txt文件修改時間戳

# 讀取txt文件
with open('vins_result_loop.txt', 'r') as file:lines = file.readlines()# 處理數據
first_line = lines[0].strip().split()
first_num = int(first_line[0])
output_lines = []
for line in lines[0:]:parts = line.split()new_num = float(parts[0]) - first_numnew_line = str(new_num)  +' '+ ' '.join(parts[1:]) + '\n'output_lines.append(new_line)# 寫入txt文件
with open('output.txt', 'w') as file:for line in output_lines:file.write(''.join(line))

結果

evo_traj tum output.txt 07_gt_tum.txt --ref=07_gt_tum.txt -a -p --plot_mode=xyz

參考鏈接：
https://blog.csdn.net/m0_49066914/article/details/131814856
https://blog.csdn.net/Hanghang_/article/details/104535370