用Python代码拆解KITTI calib文件：从P0到Tr，手把手教你坐标转换

用Python代码拆解KITTI calib文件从P0到Tr手把手教你坐标转换在自动驾驶和机器人感知领域KITTI数据集堪称黄金标准。但当你第一次打开那个神秘的calib.txt文件面对P0、P1、P2、P3和Tr这些矩阵时是否感到一头雾水本文将带你用Python代码解剖这些参数不仅理解它们的数学含义更要通过实际编程验证它们的应用场景。我们将从文件读取开始逐步实现内参提取、外参计算最终完成点云到图像的投影验证——整个过程就像在实验室里拆解一台精密仪器每个螺丝的用途都了然于胸。1. 环境准备与数据加载在开始编码之前我们需要搭建一个适合科学计算和计算机视觉开发的Python环境。推荐使用Anaconda创建独立环境conda create -n kitti_calib python3.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.188560e02, 0.000000e00, 6.071928e02, 0.000000e00], [0.000000e00, 7.188560e02, 1.852157e02, 0.000000e00], [0.000000e00, 0.000000e00, 1.000000e00, 0.000000e00]])提示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. 处理雷达到相机的变换矩阵TrTr矩阵表示从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_frontTrue): 将雷达点云投影到指定相机图像平面 # 步骤1雷达坐标系 → 相机0坐标系 points_cam0 project_velo_to_cam0(points_velo, Tr) # 过滤掉相机后方的点z0 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], ccolors, s1, cmapjet) 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], atol0.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, rtol0.05), 投影不一致6. 实际应用点云与图像融合理解了标定参数后我们可以实现一些实用的应用。比如将雷达检测到的障碍物投影到图像上def draw_3d_boxes_on_image(image, boxes_velo, calib, color(0, 255, 0), thickness2): 将雷达坐标系中的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_frontFalse) # 绘制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这个功能在自动驾驶感知系统中非常有用可以直观地验证雷达和视觉检测结果的匹配程度。