如何将相机与激光雷达标定文件转换为KITTI数据集格式？

KITTI数据集的标定文件（通常命名为calib.txt）包含相机内参矩阵(P0)、**相机校正旋转矩阵(R0_rect)和激光雷达到相机的变换矩阵(Tr_velo_to_cam)**三个核心部分，以下是基于你提供的参数的转换步骤：

1. 生成相机内参矩阵 P0 #

KITTI的P0是3×4的投影矩阵，结构固定为：

fx  0   cx  0
0   fy  cy  0
0   0   1   0

直接代入你的相机参数：

• fx=1881.648438，fy=1941.380737，cx=960.0，cy=540.0

最终P0的KITTI格式行：

P0: 1881.648438 0 960.0 0 0 1941.380737 540.0 0 0 0 1 0

2. 生成相机校正旋转矩阵 R0_rect #

KITTI中R0_rect用于将相机坐标系校正到立体视觉标准坐标系。如果你的相机输出图像已经完成畸变校正，直接使用单位矩阵即可：

R0_rect: 1 0 0 0 1 0 0 0 1

若需要基于相机的roll/pitch/yaw做校正，可将对应的旋转矩阵作为R0_rect，但通常无额外校正需求时用单位矩阵。

3. 生成激光雷达到相机的变换矩阵 Tr_velo_to_cam #

这是转换的核心，需要先分别构建相机和激光雷达相对于世界坐标系的位姿矩阵，再通过矩阵运算得到两者的变换关系：

步骤3.1 欧拉角转旋转矩阵 #

采用**roll(绕X轴)→pitch(绕Y轴)→yaw(绕Z轴)**的右手系旋转顺序，将传感器的roll/pitch/yaw转换为3×3旋转矩阵：

import numpy as np

def euler_to_rotation(roll, pitch, yaw):
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])
    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])
    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])
    return R_z @ R_y @ R_x

步骤3.2 构建传感器位姿矩阵 #

位姿矩阵是4×4的齐次变换矩阵，结构为：

[R[0][0] R[0][1] R[0][2] tx]
[R[1][0] R[1][1] R[1][2] ty]
[R[2][0] R[2][1] R[2][2] tz]
[0       0       0       1 ]

其中tx/ty/tz是传感器在世界坐标系中的平移量，R是步骤3.1得到的旋转矩阵。

步骤3.3 计算变换矩阵Tr_velo_to_cam #

激光雷达到相机的变换矩阵等于相机位姿矩阵 × 激光雷达位姿矩阵的逆矩阵，因为：
Tr_velo_to_cam = T_cam_world × T_velo_world⁻¹

完整计算代码：

import numpy as np

def euler_to_rotation(roll, pitch, yaw):
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(roll), -np.sin(roll)],
        [0, np.sin(roll), np.cos(roll)]
    ])
    R_y = np.array([
        [np.cos(pitch), 0, np.sin(pitch)],
        [0, 1, 0],
        [-np.sin(pitch), 0, np.cos(pitch)]
    ])
    R_z = np.array([
        [np.cos(yaw), -np.sin(yaw), 0],
        [np.sin(yaw), np.cos(yaw), 0],
        [0, 0, 1]
    ])
    return R_z @ R_y @ R_x

def build_transform_matrix(R, t):
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t
    return T

# 相机位姿参数
cam_trans = np.array([0.8321346282958983, -0.011121243238449097, -0.5207197332382203])
cam_rpy = np.array([0.5077840027360132, 0.7446574568748474, 0.3340256132006706])
R_cam = euler_to_rotation(*cam_rpy)
T_cam = build_transform_matrix(R_cam, cam_trans)

# 激光雷达位姿参数
velo_trans = np.array([1.1, 0.0, 1.84])
velo_rpy = np.array([0.0, 0.0, -0.6])
R_velo = euler_to_rotation(*velo_rpy)
T_velo = build_transform_matrix(R_velo, velo_trans)

# 计算激光雷达位姿的逆矩阵
T_velo_inv = np.linalg.inv(T_velo)

# 生成Tr_velo_to_cam
Tr_velo_to_cam = T_cam @ T_velo_inv

# 输出KITTI格式的Tr_velo_to_cam
print("Tr_velo_to_cam: " + " ".join([f"{val:.6f}" for val in Tr_velo_to_cam.flatten()[:12]]))

运行代码后，会输出Tr_velo_to_cam的12个元素（KITTI格式只保留前3行，共12个值）。

最终KITTI标定文件内容 #

将上述三个矩阵按以下格式保存为calib.txt：

P0: 1881.648438 0 960.0 0 0 1941.380737 540.0 0 0 0 1 0
R0_rect: 1 0 0 0 1 0 0 0 1
Tr_velo_to_cam: [代码输出的12个数值]

内容的提问来源于stack exchange，提问作者Dang Vo