做视觉定位、AR 叠加、机械臂抓取或者三维重建的人迟早都要跟“相机外参”这四个字打交道。我第一次认真研究这个问题是因为一个 AR 小项目虚拟物体怎么摆都不贴地稍微转个视角它就开始飘。后来才想明白问题根本不在渲染引擎而是我给相机定义的那张外参矩阵一开始就是错的。相机外参说白了就是相机在世界坐标系里的位置和朝向。而 PnPPerspective-n-Point解决的问题是已知一组三维点的世界坐标和它们在图像里的像素坐标反推出这个“位置和朝向”。听起来简单实际做起来牵扯到坐标系约定、退化配置、畸变处理、初值选择一堆破事。这篇文章把我从零开始理解外参、到用 PnP 算出外参、再到踩坑修正的全过程写下来适合刚接触视觉定位、或者已经调过 solvePnP 但结果总不对劲的读者。1. 相机外参与完整成像链路先搞清楚我们要求的东西到底长什么样1.1 内参管“怎么看”外参管“在哪看”把相机成像拆成两段来理解会非常顺。内参Intrinsics描述的是相机自身的“眼睛”特性焦距、主点、像素的物理尺寸、畸变。它回答的问题是“三维点落在图像传感器的哪个位置”。内参只跟镜头和 sensor 有关相机挪到哪儿都不变。外参Extrinsics描述的是相机坐标系相对世界坐标系的刚体变换也就是旋转矩阵 R 和平移向量 t。它回答的问题是“相机在世界里站在哪、朝着哪看”。同一个相机你把它从桌子上挪到墙角外参就全变了。很多人在这一步就混淆了。我见过不少同学拿着一组图像点坐标上来就套外参公式结果把像素坐标当成相机坐标用——那是内参的活外参管的是相机坐标和世界坐标之间的关系别搞串。1.2 外参矩阵的标准形式与自由度严格来说外参是一个 3×4 的矩阵[R | t]把世界坐标系的点变换到相机坐标系X_c R · X_w t写成齐次形式就是[X_c] [R | t] [X_w] [Y_c] [ ] [Y_w] [Z_c] [ ] [Z_w] [1 ] [1 ]R 是 3×3 的旋转矩阵属于 SO(3)有 3 个自由度绕 x、y、z 三轴旋转t 是 3 维平移向量也是 3 个自由度。所以外参一共 6 个自由度。这个数字很重要后面算 PnP 最少需要几组点时要用到。R 必须满足正交且行列式为 1 的性质。实际计算里很少直接存 9 个数一般存旋转向量Rodrigues 表示3 个数方向是旋转轴模长是旋转角或者欧拉角用的时候再转成矩阵。1.3 从世界坐标到像素坐标的完整链路把内外参串起来一个世界点P_w (X, Y, Z)到像素坐标(u, v)的完整投影是s · [u, v, 1]T K · [R | t] · [X, Y, Z, 1]T其中 K 是内参矩阵K [fx 0 cx] [ 0 fy cy] [ 0 0 1]fx、fy 是焦距单位是像素cx、cy 是主点。s 是这个点在相机坐标系下的深度Z_c。这条链路我建议你亲手推一遍因为很多 PnP 代码写错本质上是这条链上的某个环节理解错了。后面第 4 节会基于这条公式写完整的验证代码。2. PnP 问题说到底是在解什么2.1 每个特征点提供的约束方程假设我们拿到了 n 组对应点世界坐标P_i和像素坐标(u_i, v_i)。把外参写成行向量的形式设 R 的三行分别是 r₁、r₂、r₃每个都是 1×3 的行向量那么投影展开后s r₃ · P_i t_z u_i fx · (r₁ · P_i t_x) / s cx v_i fy · (r₂ · P_i t_y) / s cy注意每个对应点给出了两个方程但分母里含有未知数r₃ 和 t_z所以方程是非线性的。这就是 PnP 不像解线性方程组那么直接的原因。2.2 自由度算账为什么 3 组点是最少配置外参一共 6 个自由度每组 2D-3D 对应点提供 2 个约束方程。按这个账3 组点就能给出 6 个方程理论上可以解。这就是 P3P 方法的由来——只用 3 组点求解。但“方程数够”不等于“解唯一”。因为旋转矩阵本身的单位正交约束是非线性的3 组点解出来的结果最多有 4 个候选解需要用第 4 组点来剔除歧义。这也是为什么实际工程里至少准备 4 组点稳妥一点准备 8 到 12 组。2.3 为什么不能简单地线性化一个常见的直觉是把s消掉把方程写成线性形式然后最小二乘。这确实能做也就是 DLT 方法但要付出代价。如果完全忽略 R 的正交约束把[R|t]当成 12 个独立未知数每个点给 2 个线性方程至少需要 6 组点才能解。而且解出来的 12 个数拼成的矩阵不满足旋转矩阵的性质还得用 SVD 把它“掰回”到 SO(3) 上这个掰的过程会引入误差。所以 DLT 通常只用来生成初值真正的精修要靠迭代优化。3. 主流 PnP 解法与 OpenCV 的选型逻辑3.1 DLT线性打底精度有限DLT 把未知数当 12 个独立量来解优点是快、无需初值缺点是精度一般、结果不保证是合法旋转矩阵。我一般只在需要快速给其他方法提供初值时才用。3.2 P3P 与 AP3P最小配置配合 RANSAC 很香P3P 只用 3 个点求解过程基于三角形余弦定理得到最多 4 个候选位姿。OpenCV 里的SOLVEPNP_P3P和SOLVEPNP_AP3P就是这么干的。单独用它对噪声极其敏感——一个点偏几个像素结果能飞掉。但它非常适合嵌在 RANSAC 框架里每次随机抽 3 个点算假设再用所有点投票这样误匹配再多也能扛住。3.3 EPnP通用场景下的均衡选择EPnP 的核心思想是选 4 个控制点把所有的三维点表示成控制点的加权和然后把未知量压缩到控制点在相机坐标系下的 12 个坐标通过线性方程求解复杂度是 O(n)。它在点数较多、分布较散的时候精度和速度都很均衡是 OpenCV 默认推荐的方案之一也是我在大多数标定和定位任务里的首选初值来源。3.4 迭代法最后一步精修迭代法OpenCV 的SOLVEPNP_ITERATIVE直接用非线性最小二乘最小化重投影误差本质上就是 Levenberg-Marquardt 优化。它的精度上限最高但很吃初值。初值给得离谱迭代直接发散初值给得好能把误差压到亚像素。实际工程里我常用的组合是EPnP 算初值再用SOLVEPNP_ITERATIVE精修。相当于先用粗办法把位姿拉到正确的“山谷”附近再用精办法顺着梯度走到谷底。几种方法对比如下方法最少点数速度抗噪能力适用场景DLT6很快一般快速初值P3P3最快敏感RANSAC 内嵌EPnP4快较好通用场景首选迭代 LM3较慢依赖初值最终精修4. 用 Python 完整走一遍 PnP 求解4.1 先用合成数据验证全链路写代码之前先造一套“标准答案”这是排查一切问题的最快路径。我用已知的内参和外参生成图像点再用 PnP 反解对比误差。import numpy as np import cv2 # 假设的相机内参fxfy900主点在图像中心 K np.array([ [900.0, 0.0, 640.0], [0.0, 900.0, 360.0], [0.0, 0.0, 1.0] ], dtypenp.float64) # 生成一组世界坐标系下的三维点注意不要全部共面 rng np.random.default_rng(42) obj_pts rng.uniform(-0.5, 0.5, size(12, 3)) obj_pts[:, 2] 0.5 # 给 Z 方向一个偏移打破平面退化 # 给一个已知的旋转向量和平移向量 rvec_gt np.array([0.3, -0.2, 0.15], dtypenp.float64) tvec_gt np.array([0.1, -0.05, 2.0], dtypenp.float64) # 用真实外参投影得到像素坐标 img_pts, _ cv2.projectPoints( obj_pts, rvec_gt, tvec_gt, K, None ) img_pts img_pts.reshape(-1, 2)这里有个值得注意的细节cv2.projectPoints的畸变参数我传的是None表示“无畸变”。后面solvePnP里也传None两边模型必须一致否则对不上。4.2 求解并验证别只看“解出来了”ok, rvec, tvec cv2.solvePnP(obj_pts, img_pts, K, None) # 重投影误差求解结果必须能重新投回图像 proj_pts, _ cv2.projectPoints(obj_pts, rvec, tvec, K, None) proj_pts proj_pts.reshape(-1, 2) errors np.linalg.norm(img_pts - proj_pts, axis1) print(平均重投影误差:, errors.mean(), 像素)判断 PnP 解得好不好最重要的指标就是重投影误差。理想情况下误差接近 0。如果出现“解出来了但重投影误差 5 个像素以上”先别怀疑算法去检查三件事对应点是否正确、内参是否真的准、坐标系是否搞反。比较旋转向量时要小心旋转向量模长相差 2π 的整数倍表示同一个旋转。直接用rvec - rvec_gt的模长对比会得到很大的假误差。要比较先把两个旋转向量都转成矩阵再算相对旋转角R1, _ cv2.Rodrigues(rvec) R2, _ cv2.Rodrigues(rvec_gt) R_rel R1.T R2 angle np.arccos(np.clip((np.trace(R_rel) - 1) / 2, -1, 1)) print(旋转误差(弧度):, angle)4.3 真实场景标定板 特征点检测合成数据跑通之后换成真实图像。最常见的是棋盘格标定板# 假设已经用 findChessboardCorners 拿到了角点 # obj 是标定板角点的世界坐标按格子尺寸换算 square_size 0.03 # 每个格子 3cm obj np.zeros((6 * 8, 3), np.float32) obj[:, :2] np.mgrid[0:8, 0:6].T.reshape(-1, 2) * square_size ok, corners cv2.findChessboardCorners(img, (8, 6), None) corners_sub cv2.cornerSubPix(...) # 亚像素精修 ok, rvec, tvec cv2.solvePnP(obj, corners_sub, K, dist_coeffs)这里有一个特别常见的坑标定板的世界坐标原点在哪、X 轴朝哪、Y 轴朝哪完全由你的obj数组定义。你用mgrid生成时默认 X 朝右、Y 朝下、Z0。如果你后续要把这个外参喂给机械臂或渲染引擎而那个系统的 Y 轴朝上就必须自己做一次坐标变换不能直接拿这套外参用。5. 实测中真正坑人的几个点5.1 平面点集的退化问题把特征点都放在一个平面上比如棋盘格、ArUco 板所有点 Z0时投影关系退化成单应变换。平面目标的正反两面都能解释同一张图一个解是相机在板前面看另一个解是相机在板后面看。这两个解关于平面互为镜像像素上的重投影误差几乎一模一样。我实际遇到的情况是同一张棋盘格图像用 EPnP 解出来的外参物体坐标系 Z 轴指向背离相机换SOLVEPNP_ITERATIVE解Z 轴又指向相机了。两个解都自洽但物理上一个对、一个错。解决办法是在求解后加一个物理约束检查把世界原点标定板左上角用解出的外参变换到相机坐标系检查 Z_c 是否为正值。Z_c 必须大于 0物体才在相机前方。不为正就说明解被“翻面”了需要把 R 和 t 做镜像处理或者重新求解。5.2 坐标系手性混乱OpenCV 的相机坐标系是X 朝右Y 朝下Z 朝前从相机看向场景。这是右手系但 Y 朝下跟很多机器人领域习惯的“X 朝前、Y 朝左、Z 朝上”完全不一样。我踩过的坑是这样的机械臂的视觉引导系统里相机外参我拿到手后直接拿来变换点云结果所有点的深度都是负的。排查半天发现求解时用的世界坐标是标定板坐标系Y 朝下而下游工控程序期望的是机器人基座坐标系Z 朝上。中间少了一次R_world2robot的变换。这种问题没有捷径只能靠验证习惯解出外参后画坐标轴到图像上人眼确认三个轴的方向是否符合预期。代码就几行axis np.float32([ [0.1, 0, 0], [0, 0.1, 0], [0, 0, 0.1], [0, 0, 0] ]).reshape(-1, 3) img_axis, _ cv2.projectPoints(axis, rvec, tvec, K, None) # 把原点、X 轴端点、Y 轴端点、Z 轴端点画到图上5.3 畸变参数和外参的耦合很多人用solvePnP时图像已经用undistort处理过了但还是习惯性地把标定得到的畸变系数传给solvePnP。这会导致求解器内部再对“已经去畸变的点”做一次畸变补偿结果边缘区域的点被二次扭曲外参产生系统性偏差。我的习惯是二选一要么传原始图和真实畸变系数要么传去畸变后的图和None或全零。千万别混。另外内参本身不准时PnP 会把内参误差“吸收”进外参里。最典型的是主点 (cx, cy) 标定偏了几个像素解出来的平移向量就跟着偏。这是无解的——外参和内参本来就耦合在同一个投影公式里内参不准的前提下谈“绝对准确的外参”没有意义。所以先做好单目标定再来谈 PnP。6. 让外参结果更稳的工程经验6.1 特征点的选取与分布PnP 对特征点的空间分布非常敏感。全用一条直线附近的点退化成只约束了 5 个自由度全在一个平面上的点存在我刚才说的“翻面”歧义。最理想的是点在三维空间里散开深度上有层次感。如果你的场景只能用平面标定板那就用大板子、多取几帧不同角度的图像分别解外参后做加权平均或直接做一次联合优化。单帧平面板的解精度上限摆在那里不要指望它做到毫米级。另一个普遍经验特征点不要只集中在图像中心。中心区域的点对旋转的约束很弱边缘的点才对旋转敏感。用标定板时让板子尽量占满整个画面但别出画面。6.2 误匹配与 RANSAC 兜底如果是手工点选对应点或者用特征匹配算法自动找对应难免混入误匹配。一个误匹配能把 PnP 结果带偏到完全不可用。这时候用cv2.solvePnPRansacok, rvec, tvec, inliers cv2.solvePnPRansac( obj_pts, img_pts, K, None, iterationsCount1000, reprojectionError3.0, # 3 像素以内的点算内点 confidence0.99, flagscv2.SOLVEPNP_EPNP ) # 拿到内点后再用纯 EPnP 迭代精修阈值reprojectionError按你的任务精度要求设2 到 4 像素是常见区间。设太小会把正常噪声也剔掉导致内点太少设太大又挡不住真正的误匹配。6.3 多帧跟踪时的初值策略视频序列里逐帧解 PnP 时我强烈建议用上一帧的位姿作为当前帧的初值走SOLVEPNP_ITERATIVE加useExtrinsicGuessTrue。这样每帧之间位姿变化小迭代法一次就收敛而且天然带了时间上的平滑。代价是如果某一帧误匹配很严重解会被带偏后面几帧还会继续偏下去。所以一定要配合重投影误差检查哪一帧的平均误差突然从 0.5 跳到 5那一帧几乎可以断定是匹配出问题了需要重新检测或用 RANSAC 重新拉回来。还有一个我每次必做的验证动作解出外参后把几个关键三维点的重投影画到图像上叠加显示。肉眼看一下“点是否压在角点上”比看任何数值都直观。很多次我以为数值漂亮就没做这步结果坐标系翻转、轴方向画错白白多排查几个小时。看完了这些回到开头我那个 AR 项目。当时虚拟物体飘根源就是平面板求解出了一个“翻面”的外参。后来加了 Z 轴符号检查和重投影验证问题直接消失。如果你也遇到了类似现象不要急着改代码先按第 5 节的三个检查点过一遍大概率就能定位问题。