做这个系列做到第四篇前面已经啃完传感器标定的原理部分这篇就把激光雷达3D点云映射到相机图像这件事彻底跑通。说白了就是让雷达看到的世界精准地画到照片上——每个3D点落在图像的哪个像素位置还得处理遮挡、裁剪、上色这些实际工程问题。适合正在做多传感融合相关的项目、作业或demo的同学尤其是手头有激光雷达和相机数据、但不知道怎么把两者对齐的人。这篇是下篇侧重代码实现和踩坑记录配合上篇的数学原理应该能让你从公式看得懂到代码跑得通。1. 投影链路整体拆解3D点是怎么变成2D像素的1.1 本质两个传感器各说各话需要一个翻译官激光雷达输出的是三维点云每个点有xyz坐标它描述的是传感器周围世界的几何结构相机输出的是二维图像每个像素有RGB值它描述的是光线的颜色和强度。要让这两种数据对齐就得把三维点云的坐标变换到相机的成像平面上。这个过程的本质是把一个在雷达坐标系下描述的3D点转换到相机坐标系下描述再按照相机的成像模型投影到像素坐标系。想想两个人在不同位置看同一栋房子你说它在我的东北方向30米我说它在我的正前方20米。两个人都没错但要用一个统一的坐标去讨论就得知道两个人的相对位置和朝向——这就是外参矩阵干的事。1.2 链路里的四个关键环节一次完整的投影从上到下依次要经过四个环节点云数据读取从雷达采集文件里取出每个点的xyz坐标通常还会带反射强度或时间戳但投影时只需要坐标。外参变换利用标定好的旋转矩阵R和平移向量t把点从雷达坐标系移动到相机坐标系。这一步解决的是两个传感器位置和朝向不同的问题。内参投影利用相机的焦距、主点和畸变参数把相机坐标系下的3D点投影到2D像素坐标。这一步解决的是三维世界如何成像到二维传感器的问题。像素筛选与渲染投影出来的2D点不一定都在图像范围内有的跑到图像左边外面去了有的跑到上边外面去了得裁剪如果多个3D点投影到同一个像素附近还要决定让谁显示。1.3 一次投影的完整数据流拿一个具体的例子走一遍假设有一个3D点P它在雷达坐标系下的坐标是(10, 2, -1)单位米。这里x是前方y是左侧z是上方这是KITTI风格点云最常见的约定。第一步给P补一个齐次分量变成4维向量(10, 2, -1, 1)然后左乘外参矩阵Tr_velo_to_cam。这个矩阵是3x4的由旋转矩阵3x3和平移向量3x1拼成。结果得到P在相机坐标系下的坐标比如(9.8, 1.5, 5.2)——含义是这个点在相机前方9.8米、右侧1.5米、下方5.2米处相机坐标系y轴向下。第二步对相机坐标系下的点再做一次矫正旋转R0_rect这是双目/多目相机去畸变对齐用的一个3x3矩阵然后补上齐次分量左乘内参投影矩阵P2。P2是一个3x4矩阵前三列是内参矩阵K最后一列是平移通常为0。得到的结果是(2345.6, 867.3, 5.2)。第三步除以z分量完成透视除法u 2345.6 / 5.2 ≈ 451v 867.3 / 5.2 ≈ 167。这就是像素坐标。这个流程看起来简单但换上真实数据后坑就一个接一个地冒出来坐标系方向搞反、矩阵维度不对、点云里有无效点、投影到图像外的点没过滤、远处点把近处点遮挡了等等。下面把每个环节的细节拆开讲。2. 坐标系约定与内外参最容易出错的工程细节2.1 激光雷达坐标系约定大多数自动驾驶数据集里的雷达点云遵循这个约定x轴指向正前方y轴指向左侧z轴指向上方。注意这是一个右手坐标系x叉乘y得到z右手定则x向前、y向左、z向上的确满足右手系。不同雷达厂商可能有不同定义有的用x向右、y向前有的用z向下所以拿到点云第一件事就是确认坐标系定义不要假设。怎么确认直观方法把点云画出来看看车头方向对应哪个轴更严谨的方法是看数据集的文档或calib文件说明。如果方向搞错了后面投影结果一定是扭曲或镜像的。2.2 相机坐标系约定相机坐标系是另一个约定z轴指向相机前方拍摄方向x轴指向右侧y轴指向下方。这和图像坐标系的u向右、v向下是对应的。也就是说一个在相机坐标系下坐标为(X, Y, Z)的点其投影像素坐标大致是u fx * X / Z cxv fy * Y / Z cy。关键点来了雷达坐标系的y轴向左到了相机坐标系变成了y轴向下z轴从向上变成了向前。所以从雷达到相机的变换本质上是一个轴的重新排列加旋转。如果你在写旋转矩阵时把顺序搞乱又或者平移向量符号写反结果轻则投影点错位重则整体镜像。2.3 外参矩阵R和t到底怎么填外参矩阵通常写成Tr_velo_to_cam [R | t]其中R是3x3旋转矩阵t是3x1平移向量。这个矩阵的数学含义是将雷达坐标系下的点p_velo变换到相机坐标系下p_cam R * p_velo tt的含义是雷达坐标系原点在相机坐标系下的位置而不是相机在雷达坐标系下的位置。这一点特别容易混。如果你的标定结果给出的是后者需要取相反方向变换即t_cam_from_velo -R * t_velo_from_cam。实践中多数标定文件会直接给Tr_velo_to_cam名字里就写清楚了方向直接用就行。还有一个容易被忽略的点R和t不能反过来用。有人图省事把Tr_velo_to_cam的R取了转置来当逆向变换用这是对的——旋转矩阵的逆等于转置。但如果把平移向量也直接取负就错了正确公式是p_velo R.T * (p_cam - t)因为p_cam R * p_velo t p_velo R.T * (p_cam - t)这个细节在写反向投影从相机到雷达时很关键虽然正向投影用不到但调试时经常需要来回验算。2.4 内参矩阵与畸变模型内参矩阵K长这样K [fx, 0, cx; 0, fy, cy; 0, 0, 1]fx和fy是焦距单位是像素cx和cy是主点图像中心的像素坐标。投影公式就是前面提到的u fx * X / Z cx v fy * Y / Z cy这里的X、Y、Z是相机坐标系下的坐标。这个公式是针孔相机模型不考虑畸变。真实相机有镜头畸变包括径向畸变k1, k2, k3和切向畸变p1, p2一般在做相机标定时会一并求出。但注意很多自动驾驶公开数据集提供的图像已经做过畸变校正calib文件里也直接给出P2投影矩阵这种情况下就不需要再手动处理畸变直接用P2投就好。如果你用的是原始图像或自己标定的相机则需要在投影后做畸变修正或者用OpenCV的cv2.projectPoints一步到位。这里有一个常见的困惑P2到底是什么P2是3x4的投影矩阵它本身包含了内参K和一个外参旋转平移通常是矫正后的坐标系到左相机图像坐标系的变换。在KITTI格式的calib文件里P2 K * [R_rect | t_rect]其中R_rect是矫正旋转矩阵R0_rect扩展成4x4的样子。实际代码可以先做Tr_velo_to_cam再做R0_rect最后用P2直接投也可以把Tr和R0_rect乘在一起整体作为外参再左乘P2。3. 完整实操从点云文件到彩色投影图3.1 数据准备与文件读取实操阶段我用一套公开的自动驾驶场景数据举例。文件结构通常是一个点云bin文件float32数组每4个数一组x, y, z, intensity一个calib.txt标定文件包含若干行矩阵一张相机图像。calib文件里的关键字段字段维度含义P0/P1/P2/P33x4不同相机的投影矩阵R0_rect3x3矫正旋转矩阵Tr_velo_to_cam3x4雷达到相机的外参读取calib的代码思路逐行解析把每行的矩阵reshape成对应维度。注意P2是3x4直接读成3行4列就行R0_rect是3x3但使用时要补成4x4左上角放R0右下角放1。3.2 核心投影代码纯numpy不依赖OpenCV也能跑下面这段是投影的骨架每一步都写了注释。建议你自己敲一遍不要复制粘贴完事。import numpy as np import cv2 def load_calib(filepath): 解析calib.txt返回各矩阵。假设格式为 P2: 数值... calib {} with open(filepath, r) as f: for line in f: if : not in line: continue key, values line.split(:, 1) values np.array([float(x) for x in values.strip().split()]) key key.strip() if key in [P0, P1, P2, P3]: calib[key] values.reshape(3, 4) elif key R0_rect: calib[key] values.reshape(3, 3) elif key Tr_velo_to_cam: calib[key] values.reshape(3, 4) return calib def load_point_cloud(bin_file): 读取雷达点云每行4个数 x,y,z,intensity仅取xyz。 points np.fromfile(bin_file, dtypenp.float32).reshape(-1, 4) return points[:, :3] # N x 3 def project_velo_to_image(pts_3d, calib): 输入雷达坐标系下的Nx3点云 输出像素坐标 u, v, 深度 depth以及有效点的索引 # 1. 过滤无效点NaN 或 inf finite_mask np.isfinite(pts_3d).all(axis1) pts_3d pts_3d[finite_mask] # 2. 补齐次坐标 - N x 4 n pts_3d.shape[0] pts_3d_homo np.hstack((pts_3d, np.ones((n, 1)))) # N x 4 # 3. 雷达到相机坐标 Tr calib[Tr_velo_to_cam] # 3x4 cam_pts (Tr pts_3d_homo.T).T # N x 3 # 4. 矫正旋转 R0_rect R0 calib[R0_rect] # 3x3 cam_pts (R0 cam_pts.T).T # N x 3 # 5. 过滤相机后方的点z 0 必须扔掉 valid_z cam_pts[:, 2] 0 cam_pts cam_pts[valid_z] # 6. 补齐次坐标并做内参投影 m cam_pts.shape[0] cam_pts_homo np.hstack((cam_pts, np.ones((m, 1)))) # M x 4 P2 calib[P2] # 3x4 pixel_homo (P2 cam_pts_homo.T).T # M x 3 # 7. 透视除法取 u,v u pixel_homo[:, 0] / pixel_homo[:, 2] v pixel_homo[:, 1] / pixel_homo[:, 2] depth cam_pts[:, 2] return u, v, depth, valid_z, finite_mask这段代码有几点值得注意齐次坐标的乘法要弄清维度Tr是3x4点云是Nx4Tr 点云.T得到3xN再转置成Nx3。别写反。R0_rect虽然是3x3但补成4x4再做矩阵乘法更省事。如果你想省一步可以在外参矩阵层面直接合并R0_ext np.eye(4) R0_ext[:3, :3] calib[R0_rect] Tr_ext np.eye(4) Tr_ext[:3, :] calib[Tr_velo_to_cam] combined (R0_ext Tr_ext)[:3, :] # 3x4这样后面投影只需一个矩阵乘。过滤z 0的点是必须的。相机后方的点如果不过滤计算出的u/v坐标会是负值或翻转值看起来像噪点一样乱跳。这也是新手最容易忽略的地方。3.3 边界裁剪与可见性处理投影得到的u、v是浮点数先转成整数然后判断是否落在图像范围内img_h, img_w img.shape[:2] u_int np.round(u).astype(int) v_int np.round(v).astype(int) within (u_int 0) (u_int img_w) (v_int 0) (v_int img_h) u_valid u_int[within] v_valid v_int[within] depth_valid depth[within]这时候图上有大量点如果不做遮挡处理直接全部画上去远处的点会覆盖近处的点画面会显得很乱。更合理的做法是对每个像素位置只保留最近的那个点。可以用字典或排序实现# 简单深度排序法painters algorithm order np.argsort(depth_valid) # 从近到远 # 先画远的再画近的这样近的点覆盖远处的点 u_sorted u_valid[order] v_sorted v_valid[order] depth_sorted depth_valid[order] # 也可以用 z-buffer 思路建一个深度数组记录每个像素的最小深度对于静态图像调试直接按深度从远到近绘制就够直观了。如果要做实时融合建议用z-buffer维护一个与图像等大的深度数组初始化无穷大遍历点云只有当前点的深度小于数组中存储的深度时才更新像素颜色和深度。这能避免大量排序开销。3.4 可视化与上色有了u、v、depth画到图像上就很简单了。可以按距离设置颜色近处红色、远处蓝色中间黄色过渡类似热力图。用matplotlib或者OpenCV都行。import matplotlib.pyplot as plt # 颜色映射深度归一化到0~1使用jet colormap norm_depth (depth_sorted - depth_sorted.min()) / (depth_sorted.max() - depth_sorted.min() 1e-8) colors plt.cm.jet(norm_depth)[:, :3] # N x 3 fig, ax plt.subplots(figsize(12, 6)) ax.imshow(img) ax.scatter(u_sorted, v_sorted, ccolors, s1) plt.show()也可以反过来把图像的颜色赋给点云得到彩色点云colors_rgb img[v_valid, u_valid] # 注意先v后u # 然后用 Open3D 或 matplotlib 绘制带颜色的3D点云这个彩色点云对后续做语义分割、目标检测都很实用相当于把相机的纹理信息贴到了雷达的几何结构上。我在做融合可视化时最喜欢这个玩法图像上叠加雷达深度点点云上又叠加图像颜色两个方向来回验证。4. 纠错与优化常见问题排查和工程经验4.1 投影结果不对先查这四个地方问题一投影出来的点完全镜像或上下颠倒大概率是相机坐标系方向搞错或者旋转矩阵填反了。检查方法先打印几个已知坐标的点比如雷达正前方10米的一个点看它投影后是不是在图像中央偏下的位置。如果跑到图像上方或左右镜像就把旋转矩阵的行列顺序调整一下。我个人调试时习惯先用一个极小的点云几个点做测试确认方向对了再上全量数据。问题二图像上只有零星几个点大部分消失可能原因有两个一是z0的点被过滤后剩下的太少说明外参平移量或旋转有问题把点都转到相机后面去了二是边界裁剪条件写错比如把v 0写成了v 0。建议把过滤前点数z有效点数边界内点数都打印出来看哪一步骤数量骤降。问题三点云投影结果像一团乱麻没规律这通常是矩阵乘法维度或顺序错了。常见错误包括Tr_velo_to_cam和R0_rect的顺序搞反应该先做Tr再做R0或者P2直接用成了P0而P0对应的是另一台相机。如果项目里有多台相机一定要确认图像来自哪台相机用对应的投影矩阵。问题四图像上出现了两个方向相反的鬼影说明没有过滤相机后方的点。比如一个在相机后面但z坐标为负的点除以负的z会得到一个看似合理的像素坐标但实际方向是反的。把所有z0的点直接丢弃这个问题就消失了。4.2 工程化经验实时性、精度与可视化实时融合场景下点云动辄几万到十几万个点用for循环逐点投影肯定是扛不住的。上面代码全部采用numpy矩阵运算一次完成所有点的变换速度很快。如果还想再快可以先用体素滤波或直通滤波把不必要的点去掉比如只保留相机前方50米范围内的点。深度上色时雷达返回的z值跨度很大近处1米、远处80米直接线性映射会让近处细节几乎看不清。我用过一个简单方法对深度取对数再归一化这样近处和远处的层次都能体现。类似log_depth np.log1p(depth_sorted) norm_depth log_depth / log_depth.max()还有一个经验调试阶段不要把点画得太小先画成半径2~3像素的小圆点肉眼判断对齐质量更容易。对齐准确的话点应该恰好落在图像中对应物体轮廓上比如路灯杆的点云投影成一条竖线车辆的点云投影成一个规则的矩形轮廓。看到这个效果你才能确认外参内参都标定正确了。精度方面如果投影点和物体边缘总是差几个像素可能是标定文件里的外参精度不够或者相机图像分辨率与内参不匹配。差得不多的情况可以在像素坐标上加一个固定的偏移补偿但这是临时手段治本还是要重新标定。5. 扩展到彩色点云与双向验证5.1 相机图像给点云上色能得到什么把图像的颜色赋给点云之后原本只有xyz和强度信息的稀疏点云一下子就有了纹理信息。比如远处一辆车的点云从几何形状可能只是几个小块但贴上颜色后能看清车身的红漆这对识别算法非常有价值。实现上很简单先用投影得到每个点的u,v坐标再从图像的对应像素取值。注意图像的坐标顺序像素数组是img[v, u]不要写成img[u, v]。这一步写反的话颜色会错乱而且肉眼很难察觉我踩过这个坑。5.2 反向验证让二维图像点回到三维空间验证投影是否正确的一个好方法是做反向操作。取图像上某个物体的像素坐标结合深度值反投影到雷达坐标系看得到的3D点是否落在原来物体的点云位置上。这个正投影反投影闭环检查比单纯看一张效果图可靠得多。我在实操中会用这个办法验证外参找图像上一个明显的角点比如路标边缘的转角记录像素坐标再在点云里找到同一个物理点比较两者投影结果是否一致。如果差得远说明标定文件可能用过时的参数需要重新标定。5.3 实时融合场景的一点建议如果要把这个流程做成实时可视化建议把投影计算和渲染拆开投影部分用numpy或者更底层的CUDA实现渲染部分交给图形管线。每一帧只对新增点云做变换历史点云可以缓存。大多数项目不需要做到这种程度但提前想好架构后面扩展不痛苦。一些最后想说的经验这套流程我前前后后跑通过很多遍最大的体会是多传感融合里算法公式远没有坐标系方向和矩阵拼接顺序害人。很多同学卡在投影结果乱七八糟上不是不懂原理而是某个旋转矩阵少转了一列、某个平移向量符号写反了或者在处理齐次坐标时把行向量和列向量的乘法写错了。调试的时候建议把所有中间结果都打印出来逐环节验证。先做小规模测试确认方向正确再全量跑这样能省下大量排查时间。还有一个小技巧calib文件里的矩阵如果拿不准方向就先用单位矩阵代替某一部分观察投影结果的变化趋势往往一眼就能看出问题在哪。