用Python代码拆解KITTI calib文件：从P0到Tr，手把手教你坐标转换
在自动驾驶和机器人感知领域，KITTI数据集堪称黄金标准。但当你第一次打开那个神秘的calib.txt文件，面对P0、P1、P2、P3和Tr这些矩阵时，是否感到一头雾水？本文将带你用Python代码"解剖"这些参数，不仅理解它们的数学含义，更要通过实际编程验证它们的应用场景。我们将从文件读取开始，逐步实现内参提取、外参计算，最终完成点云到图像的投影验证——整个过程就像在实验室里拆解一台精密仪器，每个螺丝的用途都了然于胸。
1. 环境准备与数据加载
在开始编码之前，我们需要搭建一个适合科学计算和计算机视觉开发的Python环境。推荐使用Anaconda创建独立环境：
conda create -n kitti_calib python=3.8 conda activate kitti_calib pip install numpy opencv-python matplotlibKITTI Odometry数据集中的calib文件通常位于每个序列的根目录下。让我们先定义一个函数来加载这个文件：
import numpy as np def load_calib_file(filepath): """加载KITTI标定文件并解析为字典""" data = {} with open(filepath, 'r') as f: for line in f.readlines(): key, value = line.strip().split(':', 1) try: data[key] = np.array([float(x) for x in value.split()]).reshape(3, 4) except: data[key] = np.array([float(x) for x in value.split()]) return data这个函数会将calib文件中的每个矩阵转换为3×4的NumPy数组（Tr矩阵虽然是4×4，但在文件中只存储了3×4部分，最后一行[0,0,0,1]是隐含的）。例如，P0矩阵会被存储为：
array([[7.188560e+02, 0.000000e+00, 6.071928e+02, 0.000000e+00], [0.000000e+00, 7.188560e+02, 1.852157e+02, 0.000000e+00], [0.000000e+00, 0.000000e+00, 1.000000e+00, 0.000000e+00]])提示：KITTI数据集中的相机编号规则：P0-左灰度相机，P1-右灰度相机，P2-左彩色相机，P3-右彩色相机。
2. 解析相机投影矩阵P
每个P矩阵实际上是一个3×4的投影矩阵，可以分解为内参矩阵K和外参矩阵[R|t]的乘积。让我们编写代码来提取这些参数：
def decompose_projection_matrix(P): """分解投影矩阵P为内参K和外参[R|t]""" # 使用RQ分解获取内参K和旋转矩阵R K, R = np.linalg.qr(P[:, :3].T) K = K.T R = R.T # 确保K的对角线元素为正 T = np.diag(np.sign(np.diag(K))) K = K @ T R = T @ R # 计算平移向量t t = np.linalg.inv(K) @ P[:, 3] return K, R, t对于P0矩阵（左灰度相机），因为它是参考相机，其外参实际上是单位矩阵，所以P0的前3×3部分就是它的内参矩阵。我们可以这样验证：
calib = load_calib_file('sequence/00/calib.txt') K_cam0 = calib['P0'][:, :3] print("相机0内参矩阵:\n", K_cam0)对于其他相机，它们的P矩阵包含了相对于相机0的外参。例如，相机1的P1矩阵可以表示为：
P1 = K_cam1 [I | t]其中t是相机1相对于相机0的平移。我们可以通过以下方式提取这个平移量：
def get_camera_baseline(P0, P1): """计算两个相机之间的基线距离""" # 相机1相对于相机0的平移t = -R^T * t _, R, t = decompose_projection_matrix(P1) return np.linalg.norm(t) baseline = get_camera_baseline(calib['P0'], calib['P1']) print(f"相机0和相机1之间的基线距离: {baseline:.3f}米")3. 处理雷达到相机的变换矩阵Tr
Tr矩阵表示从Velodyne激光雷达坐标系到相机0坐标系的变换。在calib文件中，它也是一个3×4矩阵，我们需要将其扩展为完整的4×4齐次变换矩阵：
def expand_tr_matrix(tr): """将3×4的Tr矩阵扩展为4×4齐次变换矩阵""" tr_4x4 = np.eye(4) tr_4x4[:3, :4] = tr return tr_4x4 Tr = expand_tr_matrix(calib['Tr']) print("完整的雷达到相机0变换矩阵:\n", Tr)理解这个变换矩阵的物理意义非常重要。它告诉我们激光雷达点云如何映射到相机坐标系中。我们可以用这个矩阵将点云投影到图像上：
def project_velo_to_cam0(points_velo, Tr): """将雷达点云从雷达坐标系转换到相机0坐标系""" # 将点云转换为齐次坐标 points_velo_hom = np.column_stack([points_velo[:, :3], np.ones(len(points_velo))]) points_cam0 = (Tr @ points_velo_hom.T).T return points_cam0[:, :3]4. 完整坐标转换与点云投影
现在，我们整合前面的所有步骤，实现从雷达坐标系到图像坐标系的完整投影流程。这个流程包括：
- 雷达坐标系 → 相机0坐标系（使用Tr）
- 相机0坐标系 → 相机i坐标系（使用相机外参）
- 相机坐标系 → 图像像素坐标（使用相机内参）
def project_velo_to_image(points_velo, P, Tr, filter_front=True): """将雷达点云投影到指定相机图像平面""" # 步骤1：雷达坐标系 → 相机0坐标系 points_cam0 = project_velo_to_cam0(points_velo, Tr) # 过滤掉相机后方的点（z<0） if filter_front: front_mask = points_cam0[:, 2] > 0 points_cam0 = points_cam0[front_mask] points_velo = points_velo[front_mask] # 步骤2：相机0坐标系 → 当前相机坐标系 # 对于P0，这步是恒等变换；对于其他相机需要额外变换 if P is not calib['P0']: _, R, t = decompose_projection_matrix(P) points_cam = (R @ points_cam0.T).T + t else: points_cam = points_cam0 # 步骤3：相机坐标系 → 图像像素坐标 points_img_hom = (P[:, :3] @ points_cam.T).T points_img = points_img_hom[:, :2] / points_img_hom[:, 2:3] return points_img, points_velo为了可视化投影结果，我们可以使用Matplotlib将点云叠加在图像上：
import matplotlib.pyplot as plt def plot_projected_points(image, points_img, points_velo): """绘制投影结果""" plt.figure(figsize=(12, 6)) plt.imshow(image) # 根据点的高度（z坐标）着色 z = points_velo[:, 2] z_min, z_max = np.min(z), np.max(z) colors = (z - z_min) / (z_max - z_min) plt.scatter(points_img[:, 0], points_img[:, 1], c=colors, s=1, cmap='jet') plt.xlim(0, image.shape[1]) plt.ylim(image.shape[0], 0) plt.title('点云投影结果') plt.show()注意：实际应用中，还需要考虑相机的畸变参数。KITTI数据集的图像已经过校正，所以可以直接使用投影矩阵。
5. 验证标定参数的准确性
为了确保我们的标定参数正确，最好的方法是进行交叉验证。这里介绍两种验证方法：
方法一：检查相机基线一致性
我们知道KITTI数据集的四个相机是刚性安装在同一个支架上的，它们之间的相对位置应该固定。我们可以计算各相机之间的基线距离来验证：
def verify_camera_baselines(calib): """验证各相机之间的基线距离是否合理""" P0, P1, P2, P3 = calib['P0'], calib['P1'], calib['P2'], calib['P3'] baselines = { 'P0-P1': get_camera_baseline(P0, P1), 'P0-P2': get_camera_baseline(P0, P2), 'P0-P3': get_camera_baseline(P0, P3), 'P2-P3': get_camera_baseline(P2, P3) } for pair, dist in baselines.items(): print(f"{pair}基线距离: {dist:.4f}m") # 理论上P0-P1和P2-P3的距离应该近似相等（灰度/彩色相机对） assert np.isclose(baselines['P0-P1'], baselines['P2-P3'], atol=0.01), "基线不一致"方法二：投影一致性检查
我们可以选择一个已知的3D点（如雷达检测到的某个角点），分别投影到不同相机，检查投影位置是否符合预期：
def verify_projection_consistency(calib, point_velo): """验证一个3D点在不同相机的投影位置是否一致""" Tr = expand_tr_matrix(calib['Tr']) point_cam0 = project_velo_to_cam0(point_velo[np.newaxis, :], Tr)[0] projections = {} for cam in ['P0', 'P1', 'P2', 'P3']: P = calib[cam] point_img_hom = P @ np.append(point_cam0, 1) point_img = point_img_hom[:2] / point_img_hom[2] projections[cam] = point_img # 左右相机投影的x坐标差应与基线距离成比例 dx_p0p1 = projections['P0'][0] - projections['P1'][0] fx = calib['P0'][0, 0] baseline = get_camera_baseline(calib['P0'], calib['P1']) expected_dx = fx * baseline / point_cam0[2] print(f"观测到的视差: {dx_p0p1:.2f}px") print(f"理论计算的视差: {expected_dx:.2f}px") assert np.isclose(dx_p0p1, expected_dx, rtol=0.05), "投影不一致"6. 实际应用：点云与图像融合
理解了标定参数后，我们可以实现一些实用的应用。比如，将雷达检测到的障碍物投影到图像上：
def draw_3d_boxes_on_image(image, boxes_velo, calib, color=(0, 255, 0), thickness=2): """将雷达坐标系中的3D边界框绘制到图像上""" Tr = expand_tr_matrix(calib['Tr']) P = calib['P2'] # 使用左彩色相机 for box in boxes_velo: # 获取3D框的8个角点 corners = get_box_corners(box) # 假设这个函数返回8个角点 # 将角点投影到图像 corners_img, _ = project_velo_to_image(corners, P, Tr, filter_front=False) # 绘制3D框的边 edges = [(0,1), (1,2), (2,3), (3,0), # 底面 (4,5), (5,6), (6,7), (7,4), # 顶面 (0,4), (1,5), (2,6), (3,7)] # 侧面 for start, end in edges: start_pt = tuple(corners_img[start].astype(int)) end_pt = tuple(corners_img[end].astype(int)) # 只绘制在图像前方且可见的边 if (0 <= start_pt[0] < image.shape[1] and 0 <= start_pt[1] < image.shape[0] and 0 <= end_pt[0] < image.shape[1] and 0 <= end_pt[1] < image.shape[0] and corners[start, 2] > 0 and corners[end, 2] > 0): cv2.line(image, start_pt, end_pt, color, thickness) return image这个功能在自动驾驶感知系统中非常有用，可以直观地验证雷达和视觉检测结果的匹配程度。
