做自动驾驶和机器人感知的朋友，迟早都会撞上“激光雷达+相机联合标定”这堵墙。我第一次拿着KITTI数据做Velodyne雷达点到图像投影的时候，以为就是套几个矩阵公式，结果出来的画面让我怀疑人生——明明是同一辆车，雷达点全飘到道路尽头的树梢上，整个点云在图像里像打翻的芝麻，跟我预期的“精准贴合”差了十万八千里。后来反复对参数、查资料，才发现问题出在一堆不起眼的细节上：齐次坐标没归一化、旋转矩阵忘了扩维、甚至把P2矩阵里那一行平移量当成了噪声。

这篇文章就围绕KITTI数据的激光雷达与相机标定展开，讲清楚坐标转换的完整推导、标定文件里每个参数的真实含义、以及我在实操中踩过的坑和排查方法。无论你是刚接触多传感器融合的学生，还是做目标检测、数据标注、仿真开发的工程师，只要你需要把雷达点云和图像对应起来，这篇文章都能帮你少走弯路。看完之后，你不仅能在KITTI数据上跑通雷达点到像素坐标的完整投影，还能把这套逻辑迁移到自己的设备上。

1. 为什么激光雷达和相机非要“对齐”：坐标系统与联合标定的底层逻辑

1.1 一个例子讲清楚：雷达看到一个框，图像里却是歪的

假设你正在做自动驾驶目标检测，激光雷达在正前方20米处检测到一辆车，点云聚类结果是一个3D包围框，你想在相机图像里把这个框画出来。如果雷达坐标系和相机坐标系之间没有任何对应关系，你根本不知道该把框画在图像的哪个位置。雷达给出的是“从车顶雷达原点出发，向前X米、向左Y米、向上Z米”的坐标，而图像给出的是“第几行第几列的像素亮度”，两者是两套完全不同的语言。

联合标定要解决的就是这个翻译问题：通过一组旋转矩阵和平移向量，把雷达坐标系下的三维点先转换到相机坐标系，再通过相机内参投影到图像平面，最终得到像素坐标。整个过程看起来就是几个矩阵相乘，但“左乘还是右乘”“矩阵扩维还是缩维”“除以z还是除以w”这些细节一旦搞错，结果就是图像上漫山遍野的乱点。

1.2 四个坐标系的关系：雷达系、相机系、图像系、像素系

做雷达相机标定，脑子里必须时刻绷紧“坐标系”这根弦。KITTI数据里涉及了四个坐标系，分别是：

• 激光雷达坐标系（Velodyne Coordinate）：原点在车顶激光雷达中心，X轴指向车辆前方，Y轴指向车辆左侧，Z轴竖直向上，属于右手坐标系。雷达点云文件里每一行的(x, y, z)就是在这个坐标系下的坐标。
• 相机坐标系（Camera Coordinate）：原点在相机光心，Z轴指向场景前方（即相机朝向），X轴指向右侧，Y轴指向下方。注意这里Y轴方向和雷达坐标系正好相反，所以雷达到相机的旋转往往带着接近90度的分量。
• 图像坐标系（Image Coordinate）：把相机坐标系中的三维点投影到成像平面后，得到以光心投影点为原点的二维坐标，单位是毫米这类物理单位，需要再转成像素。
• 像素坐标系（Pixel Coordinate）：图像左上角为原点，单位是像素。我们在图像上看到的每一个点，最终都是这个坐标系下的(u, v)。

四个坐标系的转换关系就像流水线：雷达点先做刚性变换（旋转加平移）到相机坐标系，再从相机坐标系做透视投影到图像坐标系，最后从毫米单位换算成像素坐标。每一步都有对应的矩阵在管，KITTI的标定文件本质上就是把这些矩阵打包好的“翻译官”。

1.3 时间同步：比空间标定更先要解决的问题

很多新手搞定了空间坐标转换，投影出来还是重影、拖尾，这时候大概率不是标定参数的问题，而是时间没对齐。激光雷达一般10Hz左右，相机可能是20Hz、30Hz，两边的数据不是同一时刻采集的。车辆在运动时，哪怕差50毫秒，目标在图像里可能已经移动了好几个像素，雷达点自然就“对不上脸”。

KITTI数据集本身做了同步处理，所有传感器数据的采集时间戳严格对齐，这也是为什么很多入门项目首选KITTI——可以暂时跳过时间同步这个坑。但如果用自己采集的数据，就需要按时间戳找最近邻帧，或者做插值。空间标定和时间同步是两件事，但要做出漂亮的效果，缺一不可。我在第五节会专门讲自己采集数据时的时间对齐做法。

2. 准备工作：KITTI数据下载、标定文件里到底写了什么

2.1 KITTI数据怎么选：RGB图像、点云和标定文件对应关系

KITTI数据集常见的用法是下载object development kit或者raw data。如果你的目标是验证雷达点到图像的投影，我建议直接下载object数据集里的left color images、velodyne point cloud和calibration files三样东西。其中图像数据在data_object_image_2目录下，点云数据在data_object_velodyne目录下，标定文件在data_object_calib目录下。

这里有一个很容易搞混的点：KITTI为每一帧数据提供了四个相机（两个灰度、两个彩色），对应P0、P1、P2、P3四个投影矩阵，以及一个R0_rect旋转矩阵和一个Tr_velo_to_cam变换矩阵。其中P2对应的是左侧彩色相机，也就是我们平时看到的那张RGB图像。在做投影验证时，只需要用P2矩阵，其他三个相机矩阵可以先忽略。

2.2 看懂calib_cam_to_cam.txt与calib_velo_to_cam.txt

进入标定文件夹后，你会看到几个文本文件，重点看两个：calib_cam_to_cam.txt和calib_velo_to_cam.txt。

calib_cam_to_cam.txt里包含了相机内参相机外参相关的完整信息，核心字段是P0、P1、P2、P3和R0_rect。各字段含义如下表：

这里说的“校正旋转矩阵”可能让新手有点懵。KITTI的四个相机经过了立体校正处理，使得同一行像素在不同相机中是对齐的。R0_rect描述的就是原始相机坐标系到校正后相机坐标系的旋转关系。做单相机投影时，通常也要乘上它，否则投影结果会有细微错位。

calib_velo_to_cam.txt则只有一行核心数据——Tr_velo_to_cam，尺寸是3x4，描述激光雷达到相机坐标系的刚性变换，由旋转矩阵和平移向量拼接而成。除此之外，还有一个calib_imu_to_velo.txt，它是IMU到激光雷达的外参，如果是纯雷达点投影到图像，用不到它。

2.3 参数矩阵P2、R0_rect、Tr_velo_to_cam的真实含义

很多人看到这些矩阵里密密麻麻的数字就头大，其实把它们拆开看就没那么吓人。拿KITTI某帧的真实标定参数举例：

P2: 7.215377e+02 0.000000e+00 6.095593e+02 4.485728e+01 0.000000e+00 7.215377e+02 1.728540e+02 2.163791e-01 0.000000e+00 0.000000e+00 1.000000e+00 2.745884e-03 R0_rect: 9.999239e-01 9.837760e-03 -7.445048e-03 -9.869795e-03 9.999421e-01 -4.278459e-03 7.402527e-03 4.351614e-03 9.999631e-01 Tr_velo_to_cam: 7.533745e-03 -9.999714e-01 -6.166020e-04 -4.069766e-03 1.480249e-02 7.280733e-04 -9.998902e-01 -7.631618e-02 9.998621e-01 7.523790e-03 1.480755e-02 -2.717806e-01

P2矩阵的前三行前三列是相机内参，也就是焦距和光心：fx约721.54，fy约721.54，cx约609.56，cy约172.85。前三行第四列是相机坐标系原点在校正后坐标系里的平移分量，数值一般很小。R0_rect是3x3的旋转矩阵，Tr_velo_to_cam是3x4的变换矩阵，代表雷达到相机的旋转和偏移。真正做投影时，这三个矩阵一个都省不了，具体怎么用下面详细说。

3. 手把手实现雷达点到像素的坐标转换

3.1 转换公式推导：从三维点到图像坐标的完整流程

把激光雷达坐标系下的一个三维点(x, y, z)投影到像素坐标(u, v)，完整公式是这样的：

[x_cam, y_cam, z_cam, 1]^T = R0_rect_4x4 * Tr_velo_to_cam_4x4 * [x, y, z, 1]^T [u, v, w]^T = P2 * [x_cam, y_cam, z_cam, 1]^T u_pixel = u / w v_pixel = v / w

第一步，把雷达坐标系的点补成齐次坐标（x, y, z, 1）。这里要注意Tr_velo_to_cam是3x4的矩阵，没法直接乘4维向量，得先给它补一行[0, 0, 0, 1]变成4x4矩阵。同理，R0_rect也要从3x3扩成4x4，左上角放原矩阵，右上角补3个0，左下角补3个0，右下角补1。

第二步，用P2矩阵（3x4）把相机坐标系下的点投影到图像平面。这里的P2矩阵已经内含了相机内参和部分校正信息，不需要再单独拆出内参矩阵。相乘之后得到的是一个三维向量(u, v, w)，其中w其实是相机坐标系下的深度值z_cam（或者说是齐次缩放因子）。所以最后必须用u除以w、v除以w，才能得到真正的像素坐标。很多新手在最后一步偷懒，直接用u、v画点，结果图像上一片乱麻，就是这个原因。

3.2 Python代码实现：加载标定文件并完成投影

我建议把标定文件解析和投影封装成函数，以后换数据、换设备都能复用。下面是一段我常用的Python代码，注释写得很详细：

import numpy as np import cv2 def load_calib(calib_dir): """ 读取KITTI标定文件，返回P2、R0_rect、Tr_velo_to_cam矩阵。 注意：R0_rect和Tr_velo_to_cam都要扩展成4x4。 """ def read_matrix(file_path, key): with open(file_path, 'r') as f: for line in f.readlines(): if line.startswith(key): values = line.strip().split()[1:] return np.array([float(v) for v in values]).reshape(3, 4) return None # 读取相机内参/校正矩阵 cam_file = calib_dir + '/calib_cam_to_cam.txt' P2 = read_matrix(cam_file, 'P2:') R0 = read_matrix(cam_file, 'R0_rect:') # 扩展R0_rect为4x4 R0_4x4 = np.eye(4) R0_4x4[:3, :3] = R0 # 读取雷达到相机外参 velo_file = calib_dir + '/calib_velo_to_cam.txt' Tr = read_matrix(velo_file, 'Tr_velo_to_cam:') # 扩展Tr_velo_to_cam为4x4 Tr_4x4 = np.vstack([Tr, np.array([0, 0, 0, 1])]) return P2, R0_4x4, Tr_4x4 def project_velo_to_image(pts_velo, P2, R0_4x4, Tr_4x4): """ 将Velodyne点云(N, 3)投影到图像坐标(N, 2)。 返回点在图像上的像素坐标，以及在相机前方的深度值。 """ n = pts_velo.shape[0] pts_velo_homo = np.hstack([pts_velo, np.ones((n, 1))]) # N x 4 # 雷达到相机，再校正 pts_cam = (R0_4x4 @ Tr_4x4 @ pts_velo_homo.T).T # N x 4，最后一维是1 # 只保留相机前方的点（z > 0） z_cam = pts_cam[:, 2] valid = z_cam > 0 # 投影到像素系 pts_cam_homo = pts_cam.T # 4 x N pts_img = (P2 @ pts_cam_homo).T # N x 3 u = pts_img[:, 0] / pts_img[:, 2] v = pts_img[:, 1] / pts_img[:, 2] return u, v, z_cam, valid

这里的核心逻辑就是把三个矩阵串起来。数据读取时，我习惯直接用np.eye(4)构造单位矩阵再填充，避免手写4x4矩阵时把0和1的位置搞错。

3.3 可视化校验：点云深度着色与ROI裁剪

投影完成之后，如果不做可视化，代码写得再漂亮也白搭。我常用的校验方法很简单：把投影到图像上的雷达点按照距离着色，距离近的点用红色，距离远的点用蓝色，然后叠加到原始图像上。如果标定参数正确，你会看到点云轮廓和图像里的车辆、行人、路沿严丝合缝。

下面是一段可视化代码：

import matplotlib.pyplot as plt def draw_projection(image, u, v, depth, valid, max_depth=80): img = image.copy() mask = valid & (u >= 0) & (u < image.shape[1]) & (v >= 0) & (v < image.shape[0]) u, v, depth = u[mask], v[mask], depth[mask] # 按深度归一化颜色：近红远蓝 depth_norm = np.clip(depth / max_depth, 0, 1) colors = plt.cm.jet(1 - depth_norm)[:, :3] * 255 for i in range(len(u)): cv2.circle(img, (int(u[i]), int(v[i])), 2, colors[i].tolist(), -1) plt.figure(figsize=(12, 6)) plt.imshow(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) plt.axis('off') plt.show()

需要注意，KITTI图像是用cv2读取的BGR格式，显示时记得转成RGB。另外，我设置了max_depth=80，把80米以外的点统一压到颜色区间边界，这样近距离物体的颜色细节更丰富，画面看起来更清楚。

3.4 结果分析：如何判断投影是否对齐

很多朋友第一次跑通代码，看到图像上密密麻麻的点就以为万事大吉，其实还需要仔细判断对齐质量。我一般看三个地方：

第一，看边缘轮廓。找一根电线杆或者路灯杆，看雷达点是否刚好落在杆子的像素轮廓上，如果点全部偏到杆子左边或右边，说明外参里有平移误差。第二，看路面。地面上的点应该构造成一个平整的、与图像道路区域重合的平面，如果地面点飘到天上或者扎进路面以下很深的地方，说明雷达俯仰角标定有问题。第三，看远处物体。远处的车辆和行人轮廓是否大致吻合，虽然点会比较稀疏，但不应出现整体偏移。

如果整个画面点云方向一致地偏移，比如所有点都往右上角偏，多半是外参矩阵的平移向量出了问题。如果是远处的点发散严重、近处还好，可能是相机内参的畸变参数没有被正确处理。这些内容我在下一章展开细讲。

4. 常见坑位与排查技巧：吃了亏才知道的那些细节

4.1 投影错位到离谱，先查这几处

我在实际排错中总结了一套“由快到慢”的检查顺序，每次都帮我快速定位问题：

其中“忘了除以w”是出现频率最高的错误。P2矩阵乘完齐次坐标后，得到的三维向量第三分量是深度w，如果直接用前两个分量当像素坐标，那么所有点会沿着一条射线发散，近处的点可能还勉强在图像内，远处的点直接飞出屏幕。

另外，扩维顺序也是一个不起眼但致命的细节。Tr_velo_to_cam本身是3x4，如果你直接把点云坐标(3,)或(N,3)拿去乘，代码会报维度错误，这时候很多人会强行reshape，反而把矩阵结构弄乱。正确做法是先把点云补成Nx4，再让4x4的变换矩阵去乘它。

4.2 时间戳不同步导致的重影与鬼影

自己采集数据时，时间戳不同步几乎是所有人的噩梦。我见过一位朋友用频率10Hz的雷达和30Hz的相机做融合，直接取了两个传感器各自最近的一帧数据，结果车辆转弯时雷达点全部“粘”在图像中的墙上。这是因为雷达和相机的采集时刻差了将近50毫秒，车辆已经移动了一小段距离，反映在图像上就是几个像素到十几个像素的空间错位。

解决思路有两类。第一类是硬件同步，通过外部触发让雷达和相机在同一时刻曝光，这是工业级方案；第二类是软件同步，在算法上为每一帧图像找一个时间戳最近的雷达帧，或者反过来为雷达帧找最近图像帧，再辅以线性插值。对于入门验证，软件同步足够了。

另外，很多ROS设备在话题发布时会带有时间戳，但在数据录制和回放过程中，如果rostime出现跳变，时间戳也会失真。我自己会在预处理阶段先画一条时间戳曲线，看看是否存在异常跳变，再去做时间对齐。

4.3 深度值与像素z的混淆

还有一个非常隐蔽的坑：把雷达点的“深度”和相机坐标系下的z_cam混淆。激光雷达返回的每个点通常还有一个距离值，即点到雷达原点的欧氏距离sqrt(x^2 + y^2 + z^2)，而投影公式里P2矩阵乘出来的第三分量w，对应的是点在相机坐标系下的z值，也就是点到相机光心平面的垂直距离。这两个值在大部分情况下不相等。

如果你在筛选“相机前方的点”时，误用了雷达点的欧氏距离做判断，就会把雷达背后的点也保留下来，投影到图像上显示成杂乱的“飞点”。正确做法是先用变换矩阵算出z_cam，再用z_cam > 0作为有效性判断，最后才用z_cam做深度着色。

4.4 外参标定工具发散：Autoware、ACAT等工具实操经验

有了KITTI现成的标定参数，你可以直接跑通投影。但换到自己的设备上，就需要自己做外参标定。市面上常见工具包括Autoware的Calibration Tool和ACAT，很多人在Ubuntu 18.04上安装这些工具时被依赖问题折腾得够呛。

我的经验是：不要硬刚源码编译，优先找现成Docker镜像，或者直接用ROS 1 Noetic自带的一些标定包。Autoware标定工具的核心流程是：采集包含标定板的雷达点云和相机图像，在点云中手动框选标定板的三个角点，在图像中点击对应的角点，通过多帧数据求解外参。

这里最大的坑是角点选取精度不够导致外参发散。手动点击时，尽量把图像放大到单个像素级别，雷达点云要选标定板角落最锐利的那个点。另外，多帧数据的分布要足够分散——只采集标定板正前方的数据，解算出来的外参在侧向会有比较大的偏差。我建议至少采集10帧，标定板分别出现在画面的左上、左下、中间、右上、右下五个区域，再用RANSAC或者中值滤波剔除异常解。

5. 从KITTI到自己设备：转换代码迁移与多传感器标定的进阶姿势

5.1 自己采集雷达+相机数据时的对齐方法

KITTI数据是“天上掉下来的礼物”，因为时间同步、相机内参、雷达外参全都给你准备好了。换到自己的小车或者机器人平台上，第一件事就是解决数据对齐问题。

我自己常用的方法是：先录制rosbag，同时记录雷达和图像的topic时间戳；预处理时以图像时间为基准，对每一帧图像寻找最近的雷达帧。如果雷达是10Hz、图像是20Hz，那么大约一半图像会配到同一帧雷达，另一半图像也会有对应的最近帧。这种最近邻方法实现简单，适合低速运动场景。如果车辆速度较快，可以尝试根据帧间运动做雷达点云的补偿插值——先用里程计或者IMU估计两帧之间的位姿变化，再把雷达点云变换到图像对应时刻的雷达坐标系下。这个思路在不少开源项目中已经实现，可以直接参考。

5.2 内参标定的坑：棋盘格、D435i、双目相机

外参固然重要，但相机内参不准，外参标定结果也不会好到哪里去。给普通相机做内参标定，最常用的是张正友棋盘格标定法。很多人用OpenCV跑一遍就完事，但我建议至少做三件事：一是采集棋盘格在画面各个位置、各个倾斜角度的图像，不要只拍正前方；二是检查重投影误差，一般要小于0.5像素才算合格；三是剔除不合格角点——热词里提到的“双目相机标定剔除不合格角点”就是这个意思，棋盘格一旦出现高光、反光或者运动模糊，那一帧数据直接扔掉。

对应到具体设备，Intel RealSense D435i这类深度相机自带出厂内参，但出厂参数不一定适合你当前的畸变情况，尤其是经历运输、磕碰之后。我的习惯是在新环境跑一次内参标定，然后把生成的相机参数写入配置。双目相机标定更加麻烦，两个相机的内参分开标定，之后再标定相对外参。新手常见的错误是只标定了内参就去做双目视差，结果深度图上有大片空洞和错位。

5.3 从标定到SLAM：外参不准会让建图飘吗

很多做激光雷达SLAM的朋友问我，为什么Cartographer建图总是“飘”，方向没问题但地图边缘糊掉。这里面确实有外参不准的锅。激光雷达SLAM的输入是点云，点云的质量取决于雷达本身的标定是否准确。在多传感器融合建图场景中，如果雷达与IMU、相机的外参不对齐，融合算法会得到相互矛盾的观测，最终反映在地图上就是重影、错位、甚至轨迹漂移。

严格来说，SLAM建图“飘”首要是定位问题，但外参不准会显著增加系统的不确定性。如果你的雷达是16线的，点云本身比较稀疏，外参误差哪怕只有1度，在20米外就会产生约35厘米的空间偏移。所以做建图之前，最好先用本文介绍的投影方法验证一遍外参——把雷达点投到图像上，看看轮廓是否贴合，再去跑建图。

5.4 最后的检查清单

根据我自己的项目经验，整理了一份标定与投影验证的检查清单，分享给大家：

• 标定文件是否对应正确的相机编号，彩色图用P2，灰度图用P0
• R0_rect和Tr_velo_to_cam是否已经扩展成4x4
• 是否在投影后对(u, v, w)做了齐次归一化（除以w）
• 是否用z_cam > 0筛掉了相机后方的点
• 是否检查过像素坐标是否在图像尺寸范围内
• 是否对时间戳做了对齐，尤其是自己采集的数据
• 是否用可视化逐帧确认投影效果，而不是只跑一帧
• 换设备或拆装传感器后，是否重新标定过外参

把这些都过一遍，你的投影结果基本就不会翻车了。

我个人在实际操作中的体会是，标定这件事非常忌讳“一步到位”的心态。拿到KITTI数据时，先用简单脚本跑通投影、看到点云落在图像上，再去研究矩阵里的每个元素，学习效率会高很多。换到自己设备后更要如此，先粗标定让点云大致贴合，再采集多帧数据精细化，最后用可视化验证收尾。还有一个小技巧：把标定文件解析、投影函数、可视化脚本整理成一套模板，以后换任何传感器，只需要修改文件路径和标定参数，马上就能看到结果。这套方法我用了很久，从KITTI到自采数据到项目交付，一路都很稳。