尧图网站开发 
做多传感器融合的朋友十个里头有八个在标定这一步被磨掉半条命。激光雷达和相机各说各话一个给你稀疏但精准的三维点一个给你稠密却丢了深度的二维像素想把它们塞进同一个坐标系里对话标定就是那道必须迈过去的坎。我前后在几个移动机器人项目上折腾过这套流程从最初的标完就飘到后来能把重投影误差压到两三个像素以内中间踩的坑能写满一个笔记本。这篇就聊最原始也最可控的一条路——手动标定。它不依赖复杂优化库、不需要运动激励甚至一台笔记本加两个传感器就能开工特别适合设备刚到手、需要摸清两个传感器脾气或者自动标定反复失败想找个可靠基准的场景。手动标定的核心逻辑很简单人眼去找激光点云和图像里同一个物理点的对应关系把这种我看得见、它测得到的关联喂给求解器算出一个能把点云准确投影到图像上的旋转平移矩阵。听起来笨但正因为笨它每一步的输入都看得见摸得着出了问题能定位、能修正这是很多黑箱式自动标定给不了的安全感。下面我会把整套流程从原理到代码、从选板到排错一条龙讲透。1. 激光相机手动标定的整体设计与思路拆解1.1 先搞清楚要标定的是什么两个坐标系之间的翻译官激光雷达和相机各自有独立的坐标系。相机坐标系一般遵循 OpenCV 的约定X 轴向右、Y 轴向下、Z 轴沿光轴指向物体前方单位是无量纲的像素和归一化平面深度实际三维点用米。激光雷达坐标系各家产品略有差异但常见的是 X 轴向前、Y 轴向左、Z 轴向上。这两套坐标系的轴指向、原点位置、尺度都不同一个三维点 P 在雷达坐标系里的坐标要变成相机坐标系下的坐标中间必须经过一个刚体变换$$P_{cam} R \cdot P_{lidar} t$$其中 R 是 3x3 的旋转矩阵t 是 3x1 的平移向量这六个自由度旋转三个、平移三个就是外参。相机本身还有内参 K焦距 fx、fy 和主点 cx、cy以及畸变系数这些属于相机自己的体质跟雷达无关。要完成点云到图像的投影完整公式是$$p_{pixel} K \cdot [R \mid t] \cdot P_{lidar}$$注意这里默认把雷达点先转到相机坐标系再乘内参投影成像素。所以手动标定的任务被拆成两半一半是把相机内参标出来另一半是求雷达和相机之间的 R、t。前者用棋盘格张正友标定法就能搞定成熟到闭着眼都能做后者才是真正考验手感和耐心的部分。1.2 为什么还要用手动标定自动方案解决的痛点和留下的坑现在自动标定方案不少有基于运动估计的有基于互信息配准的还有深度学习直接回归外参的。这些方法在条件理想时确实省事但我遇到的实际问题是运动激励不够时自动方案容易收敛到错误的局部最优标出来的矩阵看着误差很低实际把点云投到图像上却整体偏移了一大截环境里动态物体多的时候特征匹配会被污染还有些方案对特定靶标依赖极强换个场地就失效。手动标定的价值在于它的确定性和可解释性——每一个参与计算的对应点都是你亲手选的哪个点选歪了、哪张图畸变没去除干净一眼就能看出来。更重要的是手动标定适合做基准真值当你怀疑某个自动算法有问题时用一套精细的手动标定结果去对照立刻就能判断到底是算法崩了还是数据有问题。它不是要取代自动标定而是给你一个随时能回归的、可信的锚点。1.3 方案选型的核心考量标定板、稳定环境和耐心手动标定这条路我认为有三件事决定成败。第一是标定板的选择。最常见的做法是用一块带明显角点的平面靶标比如黑白棋盘格或者带圆孔的白色板。雷达对材质和反射率很敏感纯黑部分在激光点云里可能直接没有回波所以雷达友好的靶标通常做成整块高反射白底中间挖几个规则排列的圆孔或者用黑色胶带贴出清晰的边缘让雷达既能稳定测到平面又能靠边缘突变定位角点。第二是环境。标定时尽量找个视野干净、没有强阳光直射的地方阳光里的红外成分会干扰雷达玻璃、镜面、反光地面会产生鬼点这些都会污染平面拟合。第三是心态和耐心手动点选几十个对应点是体力活选点的精度直接决定最终精度不能急着赶工。1.4 精度预期管理手动标定能做到什么水平先说结论一套认真的手动标定在 10 米范围内重投影误差通常能压到 3 像素以内配合好的标定板甚至能到 1 到 2 像素。这个精度对于多数低速机器人、园区配送、室内巡检场景已经够用。但如果你的应用要求毫米级、要求全场景一致那手动标定更适合做初值和基准最终外参还是要靠精细化优化或者高质量的自动方案去磨。我一般把手动标定当作把误差从几十厘米拉到几厘米的那一步剩下的交给后续微调。清楚这个预期你就不会因为手动标完还有个位数的像素误差而焦虑那是正常现象。2. 核心细节解析与实操要点从相机内参到点云角点2.1 相机内参标定张正友法为什么够用以及怎么拍图内参标定这一步不涉及雷达纯粹是相机自己的事但它是后面外参正确的前提。张正友标定法的思路是用已知物理尺寸的棋盘格在不同位姿下拍多张图检测每张图里的角点像素坐标通过求解单应矩阵再分解反推出内参和畸变。我一般拍 15 到 25 张棋盘格要覆盖画面中心、四个角、上下左右边缘倾斜角度从正对到 45 度都来一点。拍图时有几个细节必须注意棋盘格不能太靠近画面边缘导致角点检测不到也不能全在中心导致边缘畸变没被约束到每张图的姿态要有明显差异光平移不旋转是没用的光照要均匀避免棋盘格上出现强反光和阴影。用 OpenCV 的findChessboardCorners加cornerSubPix做亚像素精化再丢进calibrateCamera核对返回的重投影误差一般低于 0.5 像素算健康高于 1 像素说明某几张图有问题要挑出来重拍。import cv2 import numpy as np import glob # 棋盘格内角点数量例如 9x6 pattern_size (9, 6) objp np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32) objp[:, :2] np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) # 单位是格子实际边长比如 30mm按米填写 0.03 objp * 0.03 objpoints, imgpoints [], [] images glob.glob(calib_images/*.jpg) for fname in images: img cv2.imread(fname) gray cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners cv2.findChessboardCorners(gray, pattern_size, None) if ret: corners2 cv2.cornerSubPix( gray, corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)) objpoints.append(objp) imgpoints.append(corners2) ret, K, dist, rvecs, tvecs cv2.calibrateCamera( objpoints, imgpoints, gray.shape[::-1], None, None) print(重投影误差:, ret) print(内参矩阵:\n, K) print(畸变系数:\n, dist.ravel())提示内参标定一定要用定焦、锁定焦距的相机。如果相机能自动对焦标定过程中焦距一变整个内参就废了。标定完成后把自动对焦关掉后续采集数据时保持焦距不变。2.2 雷达点云中标定板平面的提取雷达这边要做的是把标定板这块平面从一整帧点云里干净地抠出来。操作上先把点云按距离做一次直通滤波只保留标定板大概距离范围内的点把墙面、地面、旁边杂物滤掉。然后对所有点做 RANSAC 平面拟合随机采样三个点确定一个平面统计落在平面阈值内的点数量迭代若干次取内点最多的那个平面。拟合出来的平面方程是 axbyczd0单位法向量 (a,b,c) 就是标定板在雷达坐标系下的朝向。这一步的坑在于如果标定板后面紧挨着墙雷达打过去可能同时测到板和墙两片平面距离很近RANSAC 可能把两者混在一起。解决办法是让标定板前面留足空间或者把板做得比墙前面突出明显一些配合距离阈值把两者分开。import open3d as o3d pcd o3d.io.read_point_cloud(lidar_frame.pcd) # 直通滤波保留标定板附近距离 bbox o3d.geometry.AxisAlignedBoundingBox( min_bound(-0.2, -1.0, -1.0), max_bound(6.0, 1.0, 1.5)) pcd pcd.crop(bbox) plane_model, inliers pcd.segment_plane( distance_threshold0.02, ransac_n3, num_iterations1000) a, b, c, d plane_model print(平面方程: %.4f x %.4f y %.4f z %.4f 0 % (a, b, c, d)) plane_cloud pcd.select_by_index(inliers)2.3 角点在点云里的定位为什么手动点选反而更稳平面上任意一个点都能满足平面方程但我们需要的是角点这种有明确物理位置的点。理论上可以靠边缘检测自动找角点但雷达点云在标定板边缘经常出现拖尾——一束激光一部分打在板上、一部分打到板后的背景测出来的距离是两者的中间值形成一串虚假的悬空点。这些拖尾点恰恰出现在边缘附近自动算法很容易被它们带偏。这时候人眼的优势就出来了你可以旋转点云视角看清楚哪一串点是真的在平面上的哪一串是飞出去的拖尾手动把角点定在平面内那个最靠近理论位置的点上。我通常会把标定板平面内所有点投影到平面所在的二维坐标系里这样板上的圆孔、边缘会形成一个清晰的二维图案在二维图案上点选角点比在三维里凭感觉点准得多。2.4 手动点选的精度控制放大、多帧、重复选手动点选最大的敌人是手抖和视角误差。我总结几条实操经验一是尽量放大再点三维视图缩放后点选位置更精确二是不止用一帧同一个标定板位姿采集 5 到 10 帧每帧都选一遍最后取平均单帧的随机误差会被平均掉三是至少选两组不同位姿的标定板数据因为单个平面无法约束垂直于平面的旋转位姿多样才能把六个自由度都锁死。有位老师傅告诉我手动标定点选时宁可选少而准不要选多而糊十个精准的对应点比三十个歪歪扭扭的点更能给出稳定解因为最小二乘对异常值非常敏感。3. 手动标定完整实操流程与核心环节实现3.1 数据采集一次采一帧同步要牢靠数据采集是整个流程的源头源头脏了后面全白搭。核心要求是雷达和相机在同一时刻对同一场景成像。如果你的设备有硬件同步触发直接用这是最可靠的。没有硬件同步的话让标定板在采集期间保持完全静止这样即使雷达和相机有几毫秒的时间差静止场景下也不会有位移等效于同步。采集时每个标定板位姿采一组数据包含一帧点云和一张图像位姿至少准备四到六个分布在相机视野的不同位置中心、左上、右上、左下、右下以及远近不同的距离。位姿太少约束不够位姿太多点选工作量暴涨我一般取六组兼顾精度和体力。每组位姿采集 5 到 10 帧用于平均但图像和点云里标注板要清晰可见板不能被遮挡、不能过曝、不能太暗。3.2 相机端角点提取与畸变校正拿到图像后先在图像里定位标定板的角点。如果是棋盘格靶标用内参标定时同样的角点检测函数即可一次性把一张图里所有角点都提出来。如果是圆孔靶标用斑点检测或者霍夫圆检测。需要注意必须先用前面标好的内参和畸变系数对图像做去畸变然后再检测角点否则边缘的角点会被畸变拉偏。去畸变用cv2.undistort直接处理整张图或者对提取到的像素坐标用cv2.undistortPoints做点级校正。我个人推荐点级校正因为整图去畸变会重采样边缘会有插值损失点级校正只动我们关心的那几个坐标精度更可控。校正后的角点坐标就是标准归一化相机模型下的坐标可以直接参与后续外参求解。# 点级去畸变 pts np.array([[[x1, y1]], [[x2, y2]], ...], dtypenp.float32) undistorted cv2.undistortPoints(pts, K, dist) # 得到归一化平面坐标再乘 K 变回像素 pixels cv2.projectPoints(undistorted, np.zeros(3), np.zeros(3), K, None)[0]3.3 雷达端角点手动标注的完整操作这是最考验耐心的一步。我会写一个小的点选工具把标定板平面内的点单独显示出来提供俯视和侧视两个视角。操作上先让工具把平面内点投影到平面局部坐标系形成一个二维散点图板上圆孔或边缘在二维图里会呈现为清晰的几何形状。然后你用鼠标在二维图上点击角点工具记录点击位置再反变换回雷达三维坐标。每个位姿至少点选标定板上的三个到四个角点点的分布要尽量展开别都挤在一条边上。全部位姿点完后把图像那边的角点和雷达这边的对应角点配对图像里某个棋盘格角点是第几行第几列雷达里对应的就是物理上同一个角点配对错了会直接算出垃圾外参。# 把平面内点投影到平面局部二维坐标 def project_to_plane(points, plane_model, origin): n np.array(plane_model[:3]) n n / np.linalg.norm(n) # 构造平面内两个正交基 tmp np.array([1.0, 0, 0]) if abs(n[0]) 0.9 else np.array([0, 1.0, 0]) u np.cross(n, tmp); u / np.linalg.norm(u) v np.cross(n, u) rel points - origin xy np.stack([rel u, rel v], axis1) return xy, u, v3.4 外参求解solvePnP 一步到位有了成对的 3D 点雷达坐标和 2D 点图像像素外参求解就是一个标准的 PnP 问题。把雷达角点的三维坐标、对应的图像像素坐标、内参矩阵一起丢给solvePnP它就能解出旋转向量和平移向量再把旋转向量转成矩阵就得到 R。注意这里雷达角点的三维坐标是雷达坐标系下的solvePnP 默认解的是从世界坐标系也就是你输入的三维点所在坐标系即雷达系到相机系的变换得到的正好就是我们要的 R、t。求解时建议用SOLVEPNP_ITERATIVE配合useExtrinsicGuessFalse先用三四个点粗解再用全部点做 LM 优化。求解完之后用所有点对重算重投影误差看每个点的误差分布哪个点误差特别大多半是标错了或者点选偏了。import cv2 import numpy as np # object_points: N x 3 雷达坐标系下角点 # image_points: N x 2 去畸变后的图像像素 object_points np.array(lidar_corners, dtypenp.float32) image_points np.array(img_corners, dtypenp.float32) ret, rvec, tvec cv2.solvePnP( object_points, image_points, K, None, flagscv2.SOLVEPNP_ITERATIVE) R, _ cv2.Rodrigues(rvec) print(旋转矩阵 R:\n, R) print(平移向量 t:\n, tvec) # 重投影验证 proj, _ cv2.projectPoints(object_points, rvec, tvec, K, None) err np.linalg.norm(proj.reshape(-1, 2) - image_points, axis1) print(逐点重投影误差(像素):, err) print(平均误差:, err.mean())3.5 结果验证把点云投到图像上肉眼检查算出来的外参对不对不能只看数字一定要把整个点云按这个外参投影到图像上用肉眼叠加检查。做法是把雷达点云的三维点用 R、t 变换到相机系再乘内参投影成像素把每个点按深度上色画到图像上。观察点云里的物体边缘比如墙角、桌子腿是不是和图像里的对应边缘对齐。如果整体偏移说明某个轴向上有系统误差如果近处对得准、远处越偏越大很可能旋转有残留误差如果上下错位但左右对得准检查一下相机 Y 轴方向和雷达 Z 轴方向的定义是不是搞反了。这一步是手动标定最有价值的地方误差方向能直观反映问题所在不像自动标定只给你一个数字。3.6 精细微调用重投影误差反向优化肉眼检查发现残差后可以做一轮精细微调。把外参作为初值用所有对应点构建重投影误差的代价函数用非线性优化比如 scipy 的least_squares或 Ceres去最小化。优化时把旋转用李代数或者旋转向量参数化避免旋转矩阵正交约束带来的麻烦。如果某个点的残差明显大于其他点先在优化前把它剔除或者降低它的权重因为最小二乘会被这种异常值拖偏。我一般的流程是先手动标一遍得到初值投影检查大致对齐然后用优化把误差从三四个像素压到一两个像素再检查一遍没问题就固化成配置文件。from scipy.optimize import least_squares import numpy as np, cv2 def residuals(params, obj_pts, img_pts, K): rvec params[:3].reshape(3, 1) tvec params[3:].reshape(3, 1) proj, _ cv2.projectPoints(obj_pts, rvec, tvec, K, None) return (proj.reshape(-1, 2) - img_pts).ravel() x0 np.hstack([rvec.ravel(), tvec.ravel()]) res least_squares( residuals, x0, args(object_points, image_points, K), methodlm, losshuber) rvec_opt res.x[:3].reshape(3, 1) tvec_opt res.x[3:].reshape(3, 1) R_opt, _ cv2.Rodrigues(rvec_opt)4. 常见问题与排查技巧实录4.1 标定完了发现整体偏移先查坐标系定义这是最常见的现象点云投到图像上整体朝一个方向偏。九成的原因是相机和雷达的坐标系轴向定义没对齐。相机按 OpenCV 是 Z 向前、Y 向下很多雷达是 X 向前、Z 向上从雷达到相机至少有一个绕轴旋转的关系需要转换。排查办法很简单拿一个明显在正前方的物体看它在点云里的坐标是 (x, 0, 0) 还是 (0, 0, x)再看它在图像里是在中心还是偏上偏下。把两个坐标系的轴向关系理清楚先在脑子里把变换理顺再去求解比盲目调参快得多。下面这张表总结了几种典型偏移现象对应的可能原因。现象可能原因排查方向点云整体左右偏移平移 t 的 X 或 Y 分量有偏或旋转绕 Z 轴有残留检查标定板是否在相机正前方对称分布近处对准远处飘旋转矩阵有残余误差或畸变未完全校正重新检查内参标定增加远处标定板位姿上下整体错位相机 Y 轴与雷达 Z 轴方向定义相反核对两传感器坐标系轴向约定点云比图像大一圈尺度问题雷达点距单位与预期不符确认点云单位为米内参焦距单位一致只有部分点对不齐个别对应点选错或存在拖尾点剔除重投影误差最大的几个点重新求解4.2 重投影误差降不下去从数据和点选两头找如果优化后重投影误差还在五像素以上别急着怀疑算法先查两头。数据端图像有没有运动模糊、有没有过曝导致角点检测偏移、点云有没有大面积缺失或鬼点。点选端对应点配对是不是错了图像里的左上角点配到了雷达里的右上角点这种错误会让误差怎么优化都下不来。我的排查顺序是先把点云和图像并排放大逐对检查配对关系再单独看每个点的残差残差异常大的点十有八九是配错或者点歪了。另外标定板太远时雷达测点稀疏角点定位本身就粗这种点可以降低权重或者直接不用。4.3 标定板在点云里测不到回波有些材质的标定板对激光不友好比如纯黑亚光板几乎吸光雷达拿不到回波或者透明亚克力板激光直接穿过去测到的是后面的墙。解决方向是选雷达友好的材质白色哑光喷漆的高反板通常回波质量最好实在不行用普通白纸贴在硬板上也能用但要注意平整度。还有一点标定板倾斜角度太大时激光入射角太小回波会很弱甚至丢失。标定时尽量让板正对雷达倾斜不要超过 30 度保证足够的回波强度。4.4 手动选点太慢太累用工具提升效率手动标定的痛点就是耗时。我后来做了一个半自动化的小工具把流程拆成RANSAC 自动拟合平面、平面点投影到二维、在二维图上点选角点、自动反算三维坐标。这样每次点选只需要在清晰的二维图案上点几下比在三维点云里凭感觉定位快好几倍而且更准。另一个技巧是如果标定板上用的是规则排列的圆孔可以先自动检测圆孔在二维投影图里的近似位置然后你只需要微调甚至直接确认即可把纯手动的部分压缩到最小。注意半自动工具只是帮你定位最终的确认和修正还是要人来把关。我见过有人完全信任自动检测的角点结果拖尾点混进去标出来的外参偏差好几厘米却浑然不知。机器找得快人眼判得准两者结合才是手动标定效率和质量的最优解。4.5 标定结果怎么保存和复用标定完成后把内参矩阵、畸变系数、外参的 R 和 t 全部写进一个 YAML 或 JSON 文件同时记录标定日期、使用的标定板规格格子尺寸、数量、标定距离范围、重投影误差。这些元信息在后续排查问题时极其有用比如过几个月你发现投影变差了翻出旧记录一对就能判断是标定漂了还是硬件动了。R 和 t 我建议同时存旋转矩阵和四元数两种形式因为不同下游框架要的格式不一样四元数在插值和避免万向锁上更方便旋转矩阵在直接做矩阵乘法时更直观。5. 手动标定相关的工具选型与效率提升5.1 OpenCV 之外点云处理用 Open3D 还是 PCL相机端的角点检测和内参标定OpenCV 几乎是唯一选择成熟稳定。点云端的选择就多了Open3D 和 PCL 是两主力。我个人的偏好是手动标定这种偏交互和快速验证的场景用 Open3D因为它的 Python 接口友好可视化顺手RANSAC 平面拟合、点云裁剪、坐标变换都是几行代码的事做个小工具一两个小时就能搭起来。PCL 适合已经有一套 C 工程、需要和现有代码深度集成的场景功能更全但上手重。如果你只是想快速把手动标定跑通Open3D 足够别一上来就钻进 PCL 的模板和编译里出不来。5.2 可视化工具让点云和图像并排显示手动标定高度依赖肉眼判断一个好的可视化工具能省一半力气。最土的办法是分别用 OpenCV 显示图像、Open3D 显示点云但两边分开看找对应关系很费劲。更好的做法是做一个并排窗口左边是图像角点用彩色十字标出右边是点云平面投影图对应角点用同色标注。点选时两边联动点左边图像某个角点右边对应位置高亮确认配对无误再记录。这种联动工具做一次后面标十台设备都能复用投入产出比极高。5.3 标定板的制作尺寸、图案和材质标定板不是什么高科技但制作上有讲究。尺寸上板的边长最好让它在最远标定距离时在图像里也能占到画面高度的四分之一以上太小了角点检测精度不够同时别大到视场装不下。图案上棋盘格用得多角点密集便于检测但雷达对黑白分界处的回波可能不稳定圆孔靶标对雷达更友好圆孔边缘的回波突变清晰缺点是角点数量少。折中方案是白底黑框加内部圆孔兼顾两者。材质上推荐铝板喷哑光白漆平整、刚性、不易变形用久了也不容易翘边。纸板便宜但不耐反复搬运标定过程中一旦板变形整个结果就不可信了。5.4 从手动到自动的过渡手动结果做初值手动标定最实际的价值之一是给自动标定做初值。很多自动优化方法对初值敏感初值离真值太远会收敛到错误解或者干脆发散。你用手动标定得到一个靠谱的初值再喂给基于互信息或者基于边缘对齐的自动优化往往能快速收敛到更精细的解。我的惯用组合是手动标定负责把误差压到可接受范围并给出正确初值的盆地自动优化负责在这个盆地里做精细下坡。这样既避开了自动方法对初值的挑剔又弥补了手动方法精度上限有限的短板两全其美。5.5 标定质量的自检清单每次标定完我都会走一遍自检清单确认没有漏项。内参重投影误差是否低于 0.5 像素标定板是否覆盖了画面中心和四个角落外参重投影平均误差是否低于 3 像素点云投影叠加检查时近中远三个距离上的物体边缘是否都对齐是否记录了至少两个差异明显的标定板位姿结果文件是否同时保存了矩阵形式和四元数形式。这套清单看着繁琐但能挡住绝大多数标完才发现问题的低级错误实际执行也就五分钟的事。5.6 一个容易被忽略的细节时间同步与运动一致性最后说一个我栽过跟头的地方。标定时场景是静止的时间同步差几毫秒无所谓所以你觉得同步不重要。但标定结果是要用到动态场景里的如果你的雷达和相机没有硬同步实际运行时高速运动的物体会因为时间差产生位置错位这种错位不能被外参修掉因为它是时间误差不是空间误差。所以标定验证完之后务必去检查实际系统的同步机制有硬触发的确认触发线接对软同步的测一下实际时间戳偏差偏差大就上硬件同步。标定解决的永远是空间对齐时间对齐得另外下功夫别把两者混为一谈。这套手动流程我反复用了几轮最大的体会是它逼着你把每个环节的物理含义想明白而不是把一个黑箱丢进去等结果。选点、配对、验证每一步都有反馈做多了之后你对两个传感器的空间关系会形成直觉后面再做自动标定或者排查现场问题时脑子里有张清晰的地图知道误差该往哪个方向找。标定板、工具、代码这些都能复用真正花时间的其实是理解那一层而手动标定恰好是把这层理解讲得最透的路子。 
做多传感器融合的朋友十个里头有八个在标定这一步被磨掉半条命。激光雷达和相机各说各话一个给你稀疏但精准的三维点一个给你稠密却丢了深度的二维像素想把它们塞进同一个坐标系里对话标定就是那道必须迈过去的坎。我前后在几个移动机器人项目上折腾过这套流程从最初的标完就飘到后来能把重投影误差压到两三个像素以内中间踩的坑能写满一个笔记本。这篇就聊最原始也最可控的一条路——手动标定。它不依赖复杂优化库、不需要运动激励甚至一台笔记本加两个传感器就能开工特别适合设备刚到手、需要摸清两个传感器脾气或者自动标定反复失败想找个可靠基准的场景。手动标定的核心逻辑很简单人眼去找激光点云和图像里同一个物理点的对应关系把这种我看得见、它测得到的关联喂给求解器算出一个能把点云准确投影到图像上的旋转平移矩阵。听起来笨但正因为笨它每一步的输入都看得见摸得着出了问题能定位、能修正这是很多黑箱式自动标定给不了的安全感。下面我会把整套流程从原理到代码、从选板到排错一条龙讲透。1. 激光相机手动标定的整体设计与思路拆解1.1 先搞清楚要标定的是什么两个坐标系之间的翻译官激光雷达和相机各自有独立的坐标系。相机坐标系一般遵循 OpenCV 的约定X 轴向右、Y 轴向下、Z 轴沿光轴指向物体前方单位是无量纲的像素和归一化平面深度实际三维点用米。激光雷达坐标系各家产品略有差异但常见的是 X 轴向前、Y 轴向左、Z 轴向上。这两套坐标系的轴指向、原点位置、尺度都不同一个三维点 P 在雷达坐标系里的坐标要变成相机坐标系下的坐标中间必须经过一个刚体变换$$P_{cam} R \cdot P_{lidar} t$$其中 R 是 3x3 的旋转矩阵t 是 3x1 的平移向量这六个自由度旋转三个、平移三个就是外参。相机本身还有内参 K焦距 fx、fy 和主点 cx、cy以及畸变系数这些属于相机自己的体质跟雷达无关。要完成点云到图像的投影完整公式是$$p_{pixel} K \cdot [R \mid t] \cdot P_{lidar}$$注意这里默认把雷达点先转到相机坐标系再乘内参投影成像素。所以手动标定的任务被拆成两半一半是把相机内参标出来另一半是求雷达和相机之间的 R、t。前者用棋盘格张正友标定法就能搞定成熟到闭着眼都能做后者才是真正考验手感和耐心的部分。1.2 为什么还要用手动标定自动方案解决的痛点和留下的坑现在自动标定方案不少有基于运动估计的有基于互信息配准的还有深度学习直接回归外参的。这些方法在条件理想时确实省事但我遇到的实际问题是运动激励不够时自动方案容易收敛到错误的局部最优标出来的矩阵看着误差很低实际把点云投到图像上却整体偏移了一大截环境里动态物体多的时候特征匹配会被污染还有些方案对特定靶标依赖极强换个场地就失效。手动标定的价值在于它的确定性和可解释性——每一个参与计算的对应点都是你亲手选的哪个点选歪了、哪张图畸变没去除干净一眼就能看出来。更重要的是手动标定适合做基准真值当你怀疑某个自动算法有问题时用一套精细的手动标定结果去对照立刻就能判断到底是算法崩了还是数据有问题。它不是要取代自动标定而是给你一个随时能回归的、可信的锚点。1.3 方案选型的核心考量标定板、稳定环境和耐心手动标定这条路我认为有三件事决定成败。第一是标定板的选择。最常见的做法是用一块带明显角点的平面靶标比如黑白棋盘格或者带圆孔的白色板。雷达对材质和反射率很敏感纯黑部分在激光点云里可能直接没有回波所以雷达友好的靶标通常做成整块高反射白底中间挖几个规则排列的圆孔或者用黑色胶带贴出清晰的边缘让雷达既能稳定测到平面又能靠边缘突变定位角点。第二是环境。标定时尽量找个视野干净、没有强阳光直射的地方阳光里的红外成分会干扰雷达玻璃、镜面、反光地面会产生鬼点这些都会污染平面拟合。第三是心态和耐心手动点选几十个对应点是体力活选点的精度直接决定最终精度不能急着赶工。1.4 精度预期管理手动标定能做到什么水平先说结论一套认真的手动标定在 10 米范围内重投影误差通常能压到 3 像素以内配合好的标定板甚至能到 1 到 2 像素。这个精度对于多数低速机器人、园区配送、室内巡检场景已经够用。但如果你的应用要求毫米级、要求全场景一致那手动标定更适合做初值和基准最终外参还是要靠精细化优化或者高质量的自动方案去磨。我一般把手动标定当作把误差从几十厘米拉到几厘米的那一步剩下的交给后续微调。清楚这个预期你就不会因为手动标完还有个位数的像素误差而焦虑那是正常现象。2. 核心细节解析与实操要点从相机内参到点云角点2.1 相机内参标定张正友法为什么够用以及怎么拍图内参标定这一步不涉及雷达纯粹是相机自己的事但它是后面外参正确的前提。张正友标定法的思路是用已知物理尺寸的棋盘格在不同位姿下拍多张图检测每张图里的角点像素坐标通过求解单应矩阵再分解反推出内参和畸变。我一般拍 15 到 25 张棋盘格要覆盖画面中心、四个角、上下左右边缘倾斜角度从正对到 45 度都来一点。拍图时有几个细节必须注意棋盘格不能太靠近画面边缘导致角点检测不到也不能全在中心导致边缘畸变没被约束到每张图的姿态要有明显差异光平移不旋转是没用的光照要均匀避免棋盘格上出现强反光和阴影。用 OpenCV 的findChessboardCorners加cornerSubPix做亚像素精化再丢进calibrateCamera核对返回的重投影误差一般低于 0.5 像素算健康高于 1 像素说明某几张图有问题要挑出来重拍。import cv2 import numpy as np import glob # 棋盘格内角点数量例如 9x6 pattern_size (9, 6) objp np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32) objp[:, :2] np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) # 单位是格子实际边长比如 30mm按米填写 0.03 objp * 0.03 objpoints, imgpoints [], [] images glob.glob(calib_images/*.jpg) for fname in images: img cv2.imread(fname) gray cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) ret, corners cv2.findChessboardCorners(gray, pattern_size, None) if ret: corners2 cv2.cornerSubPix( gray, corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)) objpoints.append(objp) imgpoints.append(corners2) ret, K, dist, rvecs, tvecs cv2.calibrateCamera( objpoints, imgpoints, gray.shape[::-1], None, None) print(重投影误差:, ret) print(内参矩阵:\n, K) print(畸变系数:\n, dist.ravel())提示内参标定一定要用定焦、锁定焦距的相机。如果相机能自动对焦标定过程中焦距一变整个内参就废了。标定完成后把自动对焦关掉后续采集数据时保持焦距不变。2.2 雷达点云中标定板平面的提取雷达这边要做的是把标定板这块平面从一整帧点云里干净地抠出来。操作上先把点云按距离做一次直通滤波只保留标定板大概距离范围内的点把墙面、地面、旁边杂物滤掉。然后对所有点做 RANSAC 平面拟合随机采样三个点确定一个平面统计落在平面阈值内的点数量迭代若干次取内点最多的那个平面。拟合出来的平面方程是 axbyczd0单位法向量 (a,b,c) 就是标定板在雷达坐标系下的朝向。这一步的坑在于如果标定板后面紧挨着墙雷达打过去可能同时测到板和墙两片平面距离很近RANSAC 可能把两者混在一起。解决办法是让标定板前面留足空间或者把板做得比墙前面突出明显一些配合距离阈值把两者分开。import open3d as o3d pcd o3d.io.read_point_cloud(lidar_frame.pcd) # 直通滤波保留标定板附近距离 bbox o3d.geometry.AxisAlignedBoundingBox( min_bound(-0.2, -1.0, -1.0), max_bound(6.0, 1.0, 1.5)) pcd pcd.crop(bbox) plane_model, inliers pcd.segment_plane( distance_threshold0.02, ransac_n3, num_iterations1000) a, b, c, d plane_model print(平面方程: %.4f x %.4f y %.4f z %.4f 0 % (a, b, c, d)) plane_cloud pcd.select_by_index(inliers)2.3 角点在点云里的定位为什么手动点选反而更稳平面上任意一个点都能满足平面方程但我们需要的是角点这种有明确物理位置的点。理论上可以靠边缘检测自动找角点但雷达点云在标定板边缘经常出现拖尾——一束激光一部分打在板上、一部分打到板后的背景测出来的距离是两者的中间值形成一串虚假的悬空点。这些拖尾点恰恰出现在边缘附近自动算法很容易被它们带偏。这时候人眼的优势就出来了你可以旋转点云视角看清楚哪一串点是真的在平面上的哪一串是飞出去的拖尾手动把角点定在平面内那个最靠近理论位置的点上。我通常会把标定板平面内所有点投影到平面所在的二维坐标系里这样板上的圆孔、边缘会形成一个清晰的二维图案在二维图案上点选角点比在三维里凭感觉点准得多。2.4 手动点选的精度控制放大、多帧、重复选手动点选最大的敌人是手抖和视角误差。我总结几条实操经验一是尽量放大再点三维视图缩放后点选位置更精确二是不止用一帧同一个标定板位姿采集 5 到 10 帧每帧都选一遍最后取平均单帧的随机误差会被平均掉三是至少选两组不同位姿的标定板数据因为单个平面无法约束垂直于平面的旋转位姿多样才能把六个自由度都锁死。有位老师傅告诉我手动标定点选时宁可选少而准不要选多而糊十个精准的对应点比三十个歪歪扭扭的点更能给出稳定解因为最小二乘对异常值非常敏感。3. 手动标定完整实操流程与核心环节实现3.1 数据采集一次采一帧同步要牢靠数据采集是整个流程的源头源头脏了后面全白搭。核心要求是雷达和相机在同一时刻对同一场景成像。如果你的设备有硬件同步触发直接用这是最可靠的。没有硬件同步的话让标定板在采集期间保持完全静止这样即使雷达和相机有几毫秒的时间差静止场景下也不会有位移等效于同步。采集时每个标定板位姿采一组数据包含一帧点云和一张图像位姿至少准备四到六个分布在相机视野的不同位置中心、左上、右上、左下、右下以及远近不同的距离。位姿太少约束不够位姿太多点选工作量暴涨我一般取六组兼顾精度和体力。每组位姿采集 5 到 10 帧用于平均但图像和点云里标注板要清晰可见板不能被遮挡、不能过曝、不能太暗。3.2 相机端角点提取与畸变校正拿到图像后先在图像里定位标定板的角点。如果是棋盘格靶标用内参标定时同样的角点检测函数即可一次性把一张图里所有角点都提出来。如果是圆孔靶标用斑点检测或者霍夫圆检测。需要注意必须先用前面标好的内参和畸变系数对图像做去畸变然后再检测角点否则边缘的角点会被畸变拉偏。去畸变用cv2.undistort直接处理整张图或者对提取到的像素坐标用cv2.undistortPoints做点级校正。我个人推荐点级校正因为整图去畸变会重采样边缘会有插值损失点级校正只动我们关心的那几个坐标精度更可控。校正后的角点坐标就是标准归一化相机模型下的坐标可以直接参与后续外参求解。# 点级去畸变 pts np.array([[[x1, y1]], [[x2, y2]], ...], dtypenp.float32) undistorted cv2.undistortPoints(pts, K, dist) # 得到归一化平面坐标再乘 K 变回像素 pixels cv2.projectPoints(undistorted, np.zeros(3), np.zeros(3), K, None)[0]3.3 雷达端角点手动标注的完整操作这是最考验耐心的一步。我会写一个小的点选工具把标定板平面内的点单独显示出来提供俯视和侧视两个视角。操作上先让工具把平面内点投影到平面局部坐标系形成一个二维散点图板上圆孔或边缘在二维图里会呈现为清晰的几何形状。然后你用鼠标在二维图上点击角点工具记录点击位置再反变换回雷达三维坐标。每个位姿至少点选标定板上的三个到四个角点点的分布要尽量展开别都挤在一条边上。全部位姿点完后把图像那边的角点和雷达这边的对应角点配对图像里某个棋盘格角点是第几行第几列雷达里对应的就是物理上同一个角点配对错了会直接算出垃圾外参。# 把平面内点投影到平面局部二维坐标 def project_to_plane(points, plane_model, origin): n np.array(plane_model[:3]) n n / np.linalg.norm(n) # 构造平面内两个正交基 tmp np.array([1.0, 0, 0]) if abs(n[0]) 0.9 else np.array([0, 1.0, 0]) u np.cross(n, tmp); u / np.linalg.norm(u) v np.cross(n, u) rel points - origin xy np.stack([rel u, rel v], axis1) return xy, u, v3.4 外参求解solvePnP 一步到位有了成对的 3D 点雷达坐标和 2D 点图像像素外参求解就是一个标准的 PnP 问题。把雷达角点的三维坐标、对应的图像像素坐标、内参矩阵一起丢给solvePnP它就能解出旋转向量和平移向量再把旋转向量转成矩阵就得到 R。注意这里雷达角点的三维坐标是雷达坐标系下的solvePnP 默认解的是从世界坐标系也就是你输入的三维点所在坐标系即雷达系到相机系的变换得到的正好就是我们要的 R、t。求解时建议用SOLVEPNP_ITERATIVE配合useExtrinsicGuessFalse先用三四个点粗解再用全部点做 LM 优化。求解完之后用所有点对重算重投影误差看每个点的误差分布哪个点误差特别大多半是标错了或者点选偏了。import cv2 import numpy as np # object_points: N x 3 雷达坐标系下角点 # image_points: N x 2 去畸变后的图像像素 object_points np.array(lidar_corners, dtypenp.float32) image_points np.array(img_corners, dtypenp.float32) ret, rvec, tvec cv2.solvePnP( object_points, image_points, K, None, flagscv2.SOLVEPNP_ITERATIVE) R, _ cv2.Rodrigues(rvec) print(旋转矩阵 R:\n, R) print(平移向量 t:\n, tvec) # 重投影验证 proj, _ cv2.projectPoints(object_points, rvec, tvec, K, None) err np.linalg.norm(proj.reshape(-1, 2) - image_points, axis1) print(逐点重投影误差(像素):, err) print(平均误差:, err.mean())3.5 结果验证把点云投到图像上肉眼检查算出来的外参对不对不能只看数字一定要把整个点云按这个外参投影到图像上用肉眼叠加检查。做法是把雷达点云的三维点用 R、t 变换到相机系再乘内参投影成像素把每个点按深度上色画到图像上。观察点云里的物体边缘比如墙角、桌子腿是不是和图像里的对应边缘对齐。如果整体偏移说明某个轴向上有系统误差如果近处对得准、远处越偏越大很可能旋转有残留误差如果上下错位但左右对得准检查一下相机 Y 轴方向和雷达 Z 轴方向的定义是不是搞反了。这一步是手动标定最有价值的地方误差方向能直观反映问题所在不像自动标定只给你一个数字。3.6 精细微调用重投影误差反向优化肉眼检查发现残差后可以做一轮精细微调。把外参作为初值用所有对应点构建重投影误差的代价函数用非线性优化比如 scipy 的least_squares或 Ceres去最小化。优化时把旋转用李代数或者旋转向量参数化避免旋转矩阵正交约束带来的麻烦。如果某个点的残差明显大于其他点先在优化前把它剔除或者降低它的权重因为最小二乘会被这种异常值拖偏。我一般的流程是先手动标一遍得到初值投影检查大致对齐然后用优化把误差从三四个像素压到一两个像素再检查一遍没问题就固化成配置文件。from scipy.optimize import least_squares import numpy as np, cv2 def residuals(params, obj_pts, img_pts, K): rvec params[:3].reshape(3, 1) tvec params[3:].reshape(3, 1) proj, _ cv2.projectPoints(obj_pts, rvec, tvec, K, None) return (proj.reshape(-1, 2) - img_pts).ravel() x0 np.hstack([rvec.ravel(), tvec.ravel()]) res least_squares( residuals, x0, args(object_points, image_points, K), methodlm, losshuber) rvec_opt res.x[:3].reshape(3, 1) tvec_opt res.x[3:].reshape(3, 1) R_opt, _ cv2.Rodrigues(rvec_opt)4. 常见问题与排查技巧实录4.1 标定完了发现整体偏移先查坐标系定义这是最常见的现象点云投到图像上整体朝一个方向偏。九成的原因是相机和雷达的坐标系轴向定义没对齐。相机按 OpenCV 是 Z 向前、Y 向下很多雷达是 X 向前、Z 向上从雷达到相机至少有一个绕轴旋转的关系需要转换。排查办法很简单拿一个明显在正前方的物体看它在点云里的坐标是 (x, 0, 0) 还是 (0, 0, x)再看它在图像里是在中心还是偏上偏下。把两个坐标系的轴向关系理清楚先在脑子里把变换理顺再去求解比盲目调参快得多。下面这张表总结了几种典型偏移现象对应的可能原因。现象可能原因排查方向点云整体左右偏移平移 t 的 X 或 Y 分量有偏或旋转绕 Z 轴有残留检查标定板是否在相机正前方对称分布近处对准远处飘旋转矩阵有残余误差或畸变未完全校正重新检查内参标定增加远处标定板位姿上下整体错位相机 Y 轴与雷达 Z 轴方向定义相反核对两传感器坐标系轴向约定点云比图像大一圈尺度问题雷达点距单位与预期不符确认点云单位为米内参焦距单位一致只有部分点对不齐个别对应点选错或存在拖尾点剔除重投影误差最大的几个点重新求解4.2 重投影误差降不下去从数据和点选两头找如果优化后重投影误差还在五像素以上别急着怀疑算法先查两头。数据端图像有没有运动模糊、有没有过曝导致角点检测偏移、点云有没有大面积缺失或鬼点。点选端对应点配对是不是错了图像里的左上角点配到了雷达里的右上角点这种错误会让误差怎么优化都下不来。我的排查顺序是先把点云和图像并排放大逐对检查配对关系再单独看每个点的残差残差异常大的点十有八九是配错或者点歪了。另外标定板太远时雷达测点稀疏角点定位本身就粗这种点可以降低权重或者直接不用。4.3 标定板在点云里测不到回波有些材质的标定板对激光不友好比如纯黑亚光板几乎吸光雷达拿不到回波或者透明亚克力板激光直接穿过去测到的是后面的墙。解决方向是选雷达友好的材质白色哑光喷漆的高反板通常回波质量最好实在不行用普通白纸贴在硬板上也能用但要注意平整度。还有一点标定板倾斜角度太大时激光入射角太小回波会很弱甚至丢失。标定时尽量让板正对雷达倾斜不要超过 30 度保证足够的回波强度。4.4 手动选点太慢太累用工具提升效率手动标定的痛点就是耗时。我后来做了一个半自动化的小工具把流程拆成RANSAC 自动拟合平面、平面点投影到二维、在二维图上点选角点、自动反算三维坐标。这样每次点选只需要在清晰的二维图案上点几下比在三维点云里凭感觉定位快好几倍而且更准。另一个技巧是如果标定板上用的是规则排列的圆孔可以先自动检测圆孔在二维投影图里的近似位置然后你只需要微调甚至直接确认即可把纯手动的部分压缩到最小。注意半自动工具只是帮你定位最终的确认和修正还是要人来把关。我见过有人完全信任自动检测的角点结果拖尾点混进去标出来的外参偏差好几厘米却浑然不知。机器找得快人眼判得准两者结合才是手动标定效率和质量的最优解。4.5 标定结果怎么保存和复用标定完成后把内参矩阵、畸变系数、外参的 R 和 t 全部写进一个 YAML 或 JSON 文件同时记录标定日期、使用的标定板规格格子尺寸、数量、标定距离范围、重投影误差。这些元信息在后续排查问题时极其有用比如过几个月你发现投影变差了翻出旧记录一对就能判断是标定漂了还是硬件动了。R 和 t 我建议同时存旋转矩阵和四元数两种形式因为不同下游框架要的格式不一样四元数在插值和避免万向锁上更方便旋转矩阵在直接做矩阵乘法时更直观。5. 手动标定相关的工具选型与效率提升5.1 OpenCV 之外点云处理用 Open3D 还是 PCL相机端的角点检测和内参标定OpenCV 几乎是唯一选择成熟稳定。点云端的选择就多了Open3D 和 PCL 是两主力。我个人的偏好是手动标定这种偏交互和快速验证的场景用 Open3D因为它的 Python 接口友好可视化顺手RANSAC 平面拟合、点云裁剪、坐标变换都是几行代码的事做个小工具一两个小时就能搭起来。PCL 适合已经有一套 C 工程、需要和现有代码深度集成的场景功能更全但上手重。如果你只是想快速把手动标定跑通Open3D 足够别一上来就钻进 PCL 的模板和编译里出不来。5.2 可视化工具让点云和图像并排显示手动标定高度依赖肉眼判断一个好的可视化工具能省一半力气。最土的办法是分别用 OpenCV 显示图像、Open3D 显示点云但两边分开看找对应关系很费劲。更好的做法是做一个并排窗口左边是图像角点用彩色十字标出右边是点云平面投影图对应角点用同色标注。点选时两边联动点左边图像某个角点右边对应位置高亮确认配对无误再记录。这种联动工具做一次后面标十台设备都能复用投入产出比极高。5.3 标定板的制作尺寸、图案和材质标定板不是什么高科技但制作上有讲究。尺寸上板的边长最好让它在最远标定距离时在图像里也能占到画面高度的四分之一以上太小了角点检测精度不够同时别大到视场装不下。图案上棋盘格用得多角点密集便于检测但雷达对黑白分界处的回波可能不稳定圆孔靶标对雷达更友好圆孔边缘的回波突变清晰缺点是角点数量少。折中方案是白底黑框加内部圆孔兼顾两者。材质上推荐铝板喷哑光白漆平整、刚性、不易变形用久了也不容易翘边。纸板便宜但不耐反复搬运标定过程中一旦板变形整个结果就不可信了。5.4 从手动到自动的过渡手动结果做初值手动标定最实际的价值之一是给自动标定做初值。很多自动优化方法对初值敏感初值离真值太远会收敛到错误解或者干脆发散。你用手动标定得到一个靠谱的初值再喂给基于互信息或者基于边缘对齐的自动优化往往能快速收敛到更精细的解。我的惯用组合是手动标定负责把误差压到可接受范围并给出正确初值的盆地自动优化负责在这个盆地里做精细下坡。这样既避开了自动方法对初值的挑剔又弥补了手动方法精度上限有限的短板两全其美。5.5 标定质量的自检清单每次标定完我都会走一遍自检清单确认没有漏项。内参重投影误差是否低于 0.5 像素标定板是否覆盖了画面中心和四个角落外参重投影平均误差是否低于 3 像素点云投影叠加检查时近中远三个距离上的物体边缘是否都对齐是否记录了至少两个差异明显的标定板位姿结果文件是否同时保存了矩阵形式和四元数形式。这套清单看着繁琐但能挡住绝大多数标完才发现问题的低级错误实际执行也就五分钟的事。5.6 一个容易被忽略的细节时间同步与运动一致性最后说一个我栽过跟头的地方。标定时场景是静止的时间同步差几毫秒无所谓所以你觉得同步不重要。但标定结果是要用到动态场景里的如果你的雷达和相机没有硬同步实际运行时高速运动的物体会因为时间差产生位置错位这种错位不能被外参修掉因为它是时间误差不是空间误差。所以标定验证完之后务必去检查实际系统的同步机制有硬触发的确认触发线接对软同步的测一下实际时间戳偏差偏差大就上硬件同步。标定解决的永远是空间对齐时间对齐得另外下功夫别把两者混为一谈。这套手动流程我反复用了几轮最大的体会是它逼着你把每个环节的物理含义想明白而不是把一个黑箱丢进去等结果。选点、配对、验证每一步都有反馈做多了之后你对两个传感器的空间关系会形成直觉后面再做自动标定或者排查现场问题时脑子里有张清晰的地图知道误差该往哪个方向找。标定板、工具、代码这些都能复用真正花时间的其实是理解那一层而手动标定恰好是把这层理解讲得最透的路子。