录制室内数据集跑 VINS-Fusion

录制室内数据集跑 VINS-Fusion

VINS 配置文件修改

• 左右相机配置文件

这里订阅的是畸变纠正后的话题，所以畸变系数设置为 0

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 640
image_height: 360
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 261.15583086682227
   fy: 262.8592818059595
   cx: 319.313424543224
   cy: 184.9957507446628

• zed 相机配置文件

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  

# 录制bag包
#imu_topic: "/zedm/zed_node/imu/data_raw"
#image0_topic: "/zedm/zed_node/left/image_rect_color"
#image1_topic: "/zedm/zed_node/right/image_rect_color"
output_path: "~"

cam0_calib: "cam0.yaml"
cam1_calib: "cam1.yaml"
image_width: 640
image_height: 360
   

# 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.0178199, 0.00393081, 0.999833, 0.00227199,
          -0.999839, -0.00199068, 0.0178278, 0.0173767,
           0.00206042, -0.99999, 0.0038947, -0.00345778,
           0, 0, 0, 1]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [0.0200614, 0.00823906, 0.999765, 0.00436638,
          -0.999797, -0.00160234, 0.0200752, -0.101031,
          0.00176737, -0.999965, 0.00820524, -0.00311081,
          0, 0, 0, 1]

#Multiple thread support
multiple_thread: 0

#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
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. 
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     
acc_w: 0.001        # accelerometer bias random work noise standard deviation.  
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     
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: "~/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

💡 配置文件中的body_T_cam0: !!opencv-matrix和body_T_cam1: !!opencv-matrix是相机到IMU的变换矩阵

bag 包录制

选取的是实验室外部右侧的走廊，比较小的一段距离

• 相机频率 15 Hz，分辨率 HD，640*360
• IMU 频率 200 Hz

rosbag record -O vins_indoor.bag /zedm/zed_node/imu/data_raw /zedm/zed_node/left/image_rect_color /zedm/zed_node/right/image_rect_color

这次实验的时候请闫宇腾录制了实际的视频，对于结果的评估还是有帮助的

结果显示及输出

在终端中输入如下指令

# Start RViz
roslaunch vins vins_rviz.launch

# Start VINS-Fusion node
rosrun vins vins_node src/config/zed/zedm_stereo_config.yaml

# Play rosbag
rosbag play /home/jia/Desktop/data/EuRoc/MH_01_easy.bag

#回环检测
rosrun loop_fusion loop_fusion_node src/config/zed/zedm_stereo_config.yaml

Rviz 中显示如下

输出如下 3 个文件

redwall@redwall-G3-3500:~/slam_ws/src/VINS-Fusion/output$ tree
.
├── extrinsic_parameter.csv
├── vio.csv
└── vio_loop.csv

0 directories, 3 files

• extrinsic_parameter 是相机-IMU的外参矩阵
• vio.csv 的输出代码在 visualization.cpp 中

      	// 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;
        foutC.close();
        Eigen::Vector3d tmp_T = estimator.Ps[WINDOW_SIZE];
        printf("time: %f, t: %f %f %f q: %f %f %f %f \n", header.stamp.toSec(), tmp_T.x(), tmp_T.y(), tmp_T.z(),
                                                          tmp_Q.w(), tmp_Q.x(), tmp_Q.y(), tmp_Q.z());

• vio_loop.csv 的输出代码在 pose_graph.cpp 中

    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;
        loop_path_file.close();
    }

💡 输出的格式是无法直接用于 evo 评估的，需要进行调整

问题

1、IMU 传感器数据用 data_raw 还是 data，data 多了滤波，博客中说没有什么区别，算法也没有明确说明哪种输入

2、采集到的图像的分辨率与官方的分辨率不同，同时标定后的相机内参也与官方的有差别

resolution = 2 对应 HD，应该是 1280 x 720，但是实际上是 640 x 360

[LEFT_CAM_HD]
fx=700.455
fy=700.455
cx=654.62
cy=381.196
k1=-0.172408
k2=0.0261692
p1=0.000139195
p2=-0.00020522
k3=0

[RIGHT_CAM_HD]
fx=699.895
fy=699.895
cx=625.97
cy=338.899
k1=-0.17124
k2=0.0254432
p1=0.000633285
p2=-0.000151551
k3=0

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.0028510733990104497, -0.011274881607720331, 0.0019075250446384253, 0.0010167677344458377]
  distortion_model: radtan
  intrinsics: [354.38533994265794, 355.65442412870925, 316.9161407913751, 186.00204948797065]
  resolution: [640, 360]
  rostopic: /zedm/zed_node/left/image_rect_color
cam1:
  T_cn_cnm1:
  - [0.9999997061752333, -0.0005432042046784754, -0.0005409053907860638, -0.062055301041486335]
  - [0.0005424161718011826, 0.9999987929819073, -0.0014559599676946995, -0.00013697227071209254]
  - [0.0005416956214797652, 0.0014556661440662227, 0.9999987938002391, -0.000284854301520163]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.006581396295374314, -0.01629833767651498, 0.0027074947225659233, 0.0003292930377570186]
  distortion_model: radtan
  intrinsics: [353.9649677842561, 355.3102829403972, 317.26985718211967, 186.53029793899026]
  resolution: [640, 360]
  rostopic: /zedm/zed_node/right/image_rect_color

3、实际测试时，在走廊尽头原地掉头后轨迹就会跑飞，还需要继续调试