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

最新推荐文章于 2025-06-15 09:25:42 发布

原创最新推荐文章于 2025-06-15 09:25:42 发布 · 3.1k 阅读

21 ·

CC 4.0 BY-SA版权

文章标签：

#slam

视觉激光雷达融合专栏收录该内容

5 篇文章

订阅专栏

本文详细介绍了如何使用KITTI数据集为VINS-Fusion视觉惯性里程计算法准备数据，包括计算IMU到相机的外参，以及对比不同内外参设置下VINS-Fusion的表现。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

在这里插入图片描述

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

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

$T_{ic_0} = T_{iv} * T_{vc_0}； T_{ic_1} = T_{ic_0} *T_{c_0c_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