KITTI里程计数据集 | 跑VINS-Fusion计算外参

1、使用KITTI数据集跑VINS，数据集制作参考，本文使用的是2011_09_26_0056。使用双目 +ＩＭＵ，及之后融合激光雷达

• 根据kitti数据集给出的标定参数，计算IMU-to-Cam0，IMU-to-Cam1。
• 本文使用的是未同步的imu数据和同步的相机激光雷达数据。
  

2、计算外参py脚本。也可以直接用MATLAB、Sophus等

$$T_{i{c}_0}=T_{iv}\ast T_{v{c}_0}；T_{i{c}_1}=T_{i{c}_0}\ast T_{c_0{c}_1}$$

from numpy import *
import numpy as np

T_vi = np.mat( [[9.999976e-01, 7.553071e-04, -2.035826e-03, -8.086759e-01],
	       	    [-7.854027e-04, 9.998898e-01, -1.482298e-02, 3.195559e-01],
                [2.024406e-03, 1.482454e-02, 9.998881e-01, -7.997231e-01],
	            [0, 0, 0, 1]] )
T_c0v = np.mat( [[7.967514e-03, -9.999679e-01, -8.462264e-04, -1.377769e-02],
                [-2.771053e-03, 8.241710e-04, -9.999958e-01, -5.542117e-02],
                [9.999644e-01, 7.969825e-03, -2.764397e-03, -2.918589e-01],
		        [0, 0, 0, 1]] )
T_c0c1 = np.mat( [[9.993440e-01, 1.814887e-02, -3.134011e-02, -5.370000e-01],
                  [1.842595e-02, 9.997935e-01, 8.575221e-03, 5.964270e-03],
                  [3.117801e-02,9.147067e-03, 9.994720e-01, -1.274584e-02],
	              [0, 0, 0, 1]] )

T_vc1 = T_c0v.I * T_c0c1.I

print("T_vc1:")
print(T_vc1)
T_ic0 = T_vi.I * T_c0v.I
T_ic1 = T_vi.I  * T_vc1
print("T_ic0 is")
print(mat(T_ic0))
print("T_ic1 is")
print(mat(T_ic1))

• 最后结果IMU to Cam 外参结果：

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 0.00875116, -0.00479609,  0.99995027,  1.10224312,
          -0.99986428, -0.01400249,  0.00868325, -0.31907194,
           0.01396015, -0.99989044, -0.00491798,  0.74606588,
           0.,          0.,          0.,          1.        ]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [-0.02218873, -0.01354233,  0.99989895,  1.1031531,
          -0.99989259,  0.00435299, -0.02270281, -0.85632959,
           0.03227481, -1.00072563,  0.00467743,  0.76942567,
           0.,          0.,          0.,          1.        ]

• bag的相机内参（Kitti中未校正的）：

# Cam00
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1242
image_height: 375
distortion_parameters:
   k1: -3.728755e-01
   k2: 2.037299e-01
   p1: 1.383707e-03
   p2: -7.233722e-02
projection_parameters:
   fx: 9.842439e+02
   fy: 9.808141e+02
   cx: 6.900e+02
   cy: 2.331966e+02


# Cam01
%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1242
image_height: 375
distortion_parameters:
   k1: -3.644661e-01
   k2: 1.790019e-01
   p1: -6.298563e-04
   p2: -5.314062e-02
projection_parameters:
   fx: 9.895267e+02
   fy: 9.878386e+02
   cx: 7.020e+02
   cy: 2.45559e+02

• Fusion自带的相机内参（Kitti中校正的）：

%YAML:1.0
---
model_type: PINHOLE
camera_name: camera
image_width: 1241
image_height: 376
distortion_parameters:
   k1: 0
   k2: 0
   p1: 0
   p2: 0
projection_parameters:
   fx: 7.188560e+02
   fy: 7.188560e+02
   cx: 6.071928e+02
   cy: 1.852157e+02

3、最后跑vins结果：

双目，kitti_gps_test跑syn数据，VINS-Fusion自带参数（kitti中校正的rect）：

单目+IMU，rosbag，bag的内外参：

• 结果
  

双目，rosbag，bag里的内外参数：

双目，rosbag，VINS-Fusion提供的内外参：

双目+IMU，rosbag，bag的内外参，飘的厉害，目前没找到原因（2020.6.17）:

总结：目前bag跑kitti效果不是很好，按理说计算外参没有问题，另外一个可能原因是IMU数据与相机不是很同步