06 几何求解 3D 视觉视觉 SLAM 机器人

2D–3D Correspondence & Perspective-n-Point

2D–3D 对应
与 PnP

PnP 用已知相机模型和一组“模型点—图像像素”对应，恢复将物体或世界坐标变到相机坐标的旋转与平移。真正决定系统上限的往往不只是求解器名称，而是对应是否正确、几何是否可观、RANSAC 是否合理，以及最终优化是否仍使用一致的成像模型。

Problem Contract

已知 3D 点和它们的像素，反求 object-to-camera

本站沿用右手系、列向量和 OpenCV optical frame。给定物体系中的 3D 点 Pi、对应像素 pi、相机内参 K 与畸变 D，PnP 求解 ^C R_O 与 ^C t_O，使变换后的模型点通过同一相机模型投回观测像素。

PnP observation model

\mathbf p_i=\pi\!\left(\mathbf K,\mathbf D,{}^{C}\mathbf R_{O}\mathbf P_i+{}^{C}\mathbf t_{O}\right)+\boldsymbol\epsilon_i,\qquad i=1,\ldots,n

π 同时包含透视除法、畸变和内参映射；Pi 与 ti 必须使用相同长度单位。OpenCV 风格 rvec、tvec 表达的正是 object/world → camera，而不是相机在物体/世界中的 pose。

Object pose in camera frame

{}^{C}\mathbf P_i={}^{C}\mathbf R_{O}\,{}^{O}\mathbf P_i+{}^{C}\mathbf t_{O}

6D 物体位姿任务最常用：CAD / 物体坐标点经过 ^C T_O 到相机系，再投影到图像。t 直接是物体原点在相机系中的坐标。

Camera pose in object / world frame

{}^{O}\mathbf R_{C}=({}^{C}\mathbf R_{O})^\top,\qquad {}^{O}\mathbf t_{C}=-({}^{C}\mathbf R_{O})^\top{}^{C}\mathbf t_{O}

SLAM 与相机定位常要 camera-to-world。不能只把 t 取负；必须先用 Rᵀ 旋转。相机中心 C_O = −Rᵀt。

6 DoF

未知量虽然是 6 个，但旋转带非线性正交约束，不能只靠“每点两条方程”粗暴计数。

→

P3P

三条已知 3D 点间距离与三条相机 bearing 约束可求最小问题，但通常存在多个离散候选。

→

4th point

实际接口常要求或建议第四个对应，用它验证与排序 P3P 候选；不同 API 的点数约定要单独核对。

→

n points

更多且分布良好的对应提供冗余，可做鲁棒估计、加权与非线性优化。

Where Correspondences Come From

PnP 不负责寻找对应，它只负责解释对应

对应可能来自人工标记、特征匹配、语义关键点或稠密物体坐标。它们的噪声、相关性、空间布局和错误模式不同，因此应使用不同的采样、权重与质量诊断。

对应来源	3D 侧	2D 侧	优势	主要风险
语义关键点	CAD 上预定义角点、孔位、轴端点	网络热图或坐标回归	语义清晰、对应数量少	遮挡、对称点交换与布局集中
局部特征匹配	SfM / CAD 纹理上的 3D 特征	SIFT、SuperPoint 或基础模型特征	适合相机重定位与纹理物体	重复纹理与错误匹配需要强鲁棒估计
稠密物体坐标	每个前景像素对应 CAD / NOCS 坐标	像素网格或采样后的前景点	对应多、覆盖广、适合学习系统	相邻像素误差相关，不能把数量等同于独立信息量
人工标记 / 平面板	已知方形角点或标定板坐标	亚像素角点检测	精度高、工程可控	严格共面且常有两个姿态候选
渲染 / 模板匹配	候选姿态下的表面点或轮廓	观测边缘、mask 或特征位置	可与迭代精修结合	对应会随位姿更新，属于交替优化问题

IDENTITY

点序本身就是数据

imagePoints[i] 必须观测 objectPoints[i]。对称点换位会产生几何上自洽但语义错误的姿态。

INDEPENDENCE

一万像素不等于一万独立约束

稠密坐标误差在局部高度相关，应采样、聚类或按预测不确定性加权。

BASELINE

覆盖比数量更重要

分散到物体不同面、图像不同区域的少量点，可能比集中在局部的更多点更稳定。

VISIBILITY

不可见模型点不能被强配对

自遮挡、截断和外部遮挡会改变可用点集合；训练与推理应保持同一可见性语义。

Solver Taxonomy

候选生成、通用初值和平面专用解不是同一个角色

选择求解器时先看最小样本、点的三维布局、是否需要多解、是否用于 RANSAC，以及后面是否还有迭代精修。不要只根据一个 benchmark 的平均速度选择。

求解器	点数	几何	候选	典型角色	注意事项
P3P / AP3P	理论 3；常用接口以第 4 点消歧	非共线最小集	最多 4 个候选	RANSAC 候选生成；AP3P 是更高效稳定的代数实现	必须枚举并用额外点、正深度与残差排序
EPnP	n ≥ 4	一般点；也可处理平面情形	通常返回一个	O(n) 非迭代通用初值，大量对应时常用	有噪声、小样本或病态布局时仍建议非线性精修
SQPnP	n ≥ 3	一般与共面点	可返回多个最优候选	快速、全局最优的通用求解器	“全局最优”针对其目标，不会修复错误对应、错误 K 或物体对称歧义
IPPE	n ≥ 4	必须共面	显式返回 2 个	平面目标、AR marker 与标定板	近正视、小目标时双解残差可能很接近
IPPE_SQUARE	严格 4 点	正方形且点序固定	显式返回 2 个	方形 marker 的专用快速解	尺寸、原点和四个角的顺序必须完全符合接口定义
ITERATIVE / LM	通常 n ≥ 4；初始化要求依接口而定	一般点或共面点	依初值进入局部解	最小化像素重投影误差，常用于最终精修	不是全局初始化器；初值、鲁棒损失和参数化会影响收敛

ROBUST MINIMAL

AP3P + RANSAC

错误对应较多时，用小样本快速提出候选，再由全体对应评分。

GENERAL

SQPnP 或 EPnP

无明显离群、点数较多时生成通用初值；随后仍应检查正深度并精修。

PLANAR

IPPE / IPPE_SQUARE

主动保留两个候选，而不是让通用单解接口静默选择错误分支。

FINAL

LM / VVS / BA

在可信初值与内点集上最小化重投影目标，加入权重、鲁棒核或先验。

Interactive Lab

把对应质量、采样预算和几何布局放在一起看

第一个视图观察像素残差与阈值如何形成共识集；第二个视图用精确公式计算不同最小样本大小的 RANSAC 预算；第三个视图比较空间、共面、近共线与集中布局。交互刻意把“求解器实现”和“所有 PnP 共享的鲁棒估计逻辑”分开。

当前假设投影阈值内观测阈值外观测连线只表达 correspondence，不表示真实空间射线长度

Consensus

当前共识集

8 / 10阈值判断来自当前位姿假设的像素残差

Residual

重投影 RMS

—全部对应：—

Coverage

二维覆盖

—观测点凸包面积 / 图像面积

Pose contract

OpenCV 风格输出

^CT_O将物体模型点变换到相机坐标系

这不是某个 PnP 求解器的数值实现。 该视图专门演示所有求解器共享的“观测—投影—残差—内点”层。真正的工程流程应由 P3P/AP3P、EPnP、SQPnP 或 IPPE 生成候选，再用这里的残差规则筛选并精修。

RANSAC Is a Pipeline

RANSAC 的输出不应是某一次幸运抽样的最小解

标准流程是最小集提出假设、全数据建立共识、动态预算、局部优化，再用全部内点重估。阈值、样本大小和几何退化检查共同决定鲁棒性与速度。

01
抽取最小集
从全部 2D–3D 对应中随机抽取 s 个点；在抽样前拒绝重复点、近共线点和明显病态布局。
02
生成全部有效候选
P3P/IPPE 可能返回多个姿态。每个候选都要检查旋转合法性、正深度与数值有限性。
03
对全部对应评分
用同一 K、畸变模型和像素坐标计算重投影残差，以阈值 τ 构造共识集；可结合置信度或协方差。
04
保留最优并动态更新预算
按内点数、截断代价或 M-estimator 排序；由当前最佳内点率估计所需迭代数，提前终止。
05
局部优化与全内点重估
不要把最小集解直接作为最终结果。用全部内点重新求解、更新内点，再做 LM/VVS 或 BA 式精修。
06
输出质量诊断
同时记录内点数、内点率、RMS/分位数、图像覆盖、最小深度、候选间距与终止原因。

Ideal RANSAC iteration budget

N\geq \frac{\log(1-p)}{\log(1-w^s)}

p 是目标成功概率，w 是内点率，s 是最小样本大小。w 的小幅下降或 s 的增加都会显著放大 N；公式假设独立均匀采样且全内点样本不退化。

Pixel reprojection residual

r_i=\left\|\mathbf p_i-\pi\!\left(\mathbf K,\mathbf D,\mathbf R\mathbf P_i+\mathbf t\right)\right\|_2,\qquad i\in\mathcal I\iff r_i<\tau

τ 的单位是当前 imagePoints 的像素单位。若输入先去畸变为归一化射线，阈值也必须换到归一化坐标尺度；不能继续照搬 3 px。

Degeneracy & Ambiguity

可解、唯一、稳定是三件不同的事

严格共面目标不是天然不可解：它可以通过平面单应性或 IPPE 求姿态，但常需要处理两个候选。近共线点则缺少关键方向约束；局部集中点可能数学满秩，却因条件数差而把像素噪声放大为明显位姿误差。

COLLINEAR

近共线：最小集直接拒绝

检查中心化 3D 点协方差的第二特征值，或三角形面积；继续增加同一直线上的点没有帮助。

PLANAR

共面：保留并排序双解

正视、远距离、小目标时两个候选的重投影误差可能接近，需要时序、非共面点或任务先验消歧。

CLUSTERED

集中：误差谷狭长或扁平

扩大 3D 基线和图像凸包通常比单纯提高点数更有效，尤其能改善平面外旋转与深度。

SYMMETRY

对称：对应身份本身多义

若两个模型点在观测上不可区分，应输出等价姿态、多假设或使用对称感知评分，而不是强迫唯一标签。

Refinement & Uncertainty

从代数初值走到任务可用位姿，还差一次正确的优化

最终优化应使用与观测生成一致的投影函数，可选鲁棒损失、关键点协方差、姿态先验与时间先验。它改善局部精度，却不能从错误分支或严重错误对应中自动恢复。

Weighted robust reprojection refinement

\min_{\boldsymbol\xi}\sum_{i\in\mathcal I}\rho\!\left(\mathbf r_i(\boldsymbol\xi)^\top\mathbf\Sigma_i^{-1}\mathbf r_i(\boldsymbol\xi)\right),\qquad \mathbf T\leftarrow\exp(\widehat{\Delta\boldsymbol\xi})\mathbf T

ξ 是 SE(3) 局部参数，Σi 描述二维观测不确定性，ρ 可选 Huber/Cauchy 等鲁棒核。更新乘在左侧还是右侧会改变雅可比定义，必须与代码推导一致。

HESSIAN / JᵀWJ

局部条件与协方差

优化雅可比的谱可以揭示哪些姿态方向约束弱；残差小不代表每个自由度都可信。

MULTI-HYPOTHESIS

候选间距

平面或对称场景应保留多个候选及其分数，让跟踪、深度或机器人任务在后续消歧。

TASK SPACE

像素误差不等于任务误差

远处小物体可能重投影很好但深度不准；抓取应进一步评估 TCP、碰撞和接近方向误差。

Across Application Domains

同一个 PnP，在不同系统中输出语义不同

6D POSE

CAD / object → camera

模型点来自物体系，输出 ^CT_O。常与检测、分割、关键点或稠密对象坐标网络组合。

SLAM

map / world → camera

3D 地图点与当前帧特征匹配后得到 ^CT_W；用于轨迹输出时通常再求逆为 ^WT_C。

ROBOTICS

camera pose enters a transform chain

PnP 只给视觉相对位姿，还要通过手眼外参变到 base / end-effector，并统一米、毫米和时间戳。

AR / MARKER

平面双解与时序稳定

方形标记适合 IPPE_SQUARE；实际显示还需候选消歧、时序滤波和 OpenCV→图形学坐标转换。

Engineering recipe · OpenCV Python

候选、共识与精修分开写

示例使用 AP3P 在 RANSAC 中生成候选，再以全部内点做 VVS 精修。实际阈值应由当前分辨率和关键点噪声确定。

ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, K, dist_coeffs,
    iterationsCount=1000,
    reprojectionError=3.0,
    confidence=0.999,
    flags=cv2.SOLVEPNP_AP3P,
)

if ok and inliers is not None and len(inliers) >= 4:
    ids = inliers[:, 0]
    rvec, tvec = cv2.solvePnPRefineVVS(
        object_points[ids], image_points[ids],
        K, dist_coeffs, rvec, tvec,
    )

DTYPE / SHAPE

显式检查 float32/float64、N×3、N×2 和连续内存；不要让 squeeze 后的单点破坏形状。

MULTIPLE SOLUTIONS

平面或最小问题需要 solvePnPGeneric 枚举候选，再按任务信息排序。

POST CHECK

重新投影并记录 inlier 分位数、凸包、最小 Z、det(R) 与候选间的旋转/平移差。

Debug Protocol

先证明输入对应和相机模型正确，再怀疑求解器

PnP 的错误经常来自前后处理：点序交换、resize 后 K 未更新、畸变状态混用、CAD 单位错误或输出方向读反。用可视化重投影和分层断点比盲目切换 flag 更有效。

01
用真值位姿重投影输入模型点
若真值都无法落到 imagePoints，错误在点序、K/D、图像变换、坐标系或标注，不在 PnP。
02
逐点画编号与残差向量
一致方向的整体偏移常提示主点/裁剪错误；边缘径向误差提示畸变；少量超长箭头提示错配。
03
核对输出方向与相机中心
OpenCV rvec/tvec 将模型点送到相机系。需要 camera pose 时使用 Rᵀ 和 −Rᵀt，而不是只给 t 加负号。
04
检查所有变换后点的 Z
大多数可见模型点应有 Z > 0。负深度候选应拒绝；只有少数可见点时需结合裁剪和遮挡解释。
05
检查 3D 布局与 2D 覆盖
对中心化 3D 点做 SVD，计算图像凸包；“内点很多但都集中”仍可能是弱约束。
06
枚举多解而不是重复加迭代
平面和 P3P 场景先取全部候选；错误分支不是增加 LM 次数就能跨越的局部小问题。
07
最后比较不同求解器
固定完全相同的输入、内点集、精修方式和输出方向，再比较速度、失败率与任务误差。

算法、接口与原始来源展开查看 OpenCV 官方文档和代表性论文

Next geometry topic · 07

3D–3D 对齐与点云配准

当 RGB-D、双目或多视角重建提供了两组 3D 对应，问题从像素重投影转为刚体对齐。下一专题将系统展开 Kabsch、Umeyama、SVD 反射处理、RANSAC、point-to-point / plane ICP、初值与局部最优。

进入完整专题

2D–3D 对应与 PnP

已知 3D 点和它们的像素，反求 object-to-camera

PnP 不负责寻找对应，它只负责解释对应

点序本身就是数据

一万像素不等于一万独立约束

覆盖比数量更重要

不可见模型点不能被强配对

候选生成、通用初值和平面专用解不是同一个角色

AP3P + RANSAC

SQPnP 或 EPnP

IPPE / IPPE_SQUARE

LM / VVS / BA

把对应质量、采样预算和几何布局放在一起看

当前共识集

重投影 RMS

二维覆盖

OpenCV 风格输出

一次全内点概率

理论所需迭代

当前成功概率

预算判断

RANSAC 的输出不应是某一次幸运抽样的最小解

抽取最小集

生成全部有效候选

对全部对应评分

保留最优并动态更新预算

局部优化与全内点重估

输出质量诊断