单帧 PnP：从四个角点到三维位姿
完整理解角点、二维—三维对应、相机模型、IPPE 候选和 camera→tracker 坐标变换。
这一页只回答一个问题：一次曝光里的四个装甲板角点，怎样变成跟踪器可以使用的一条三维位姿观测？
完整主线是：
图像
→ 检测并恢复四个原图角点
→ 建立同序的四组 2D–3D 对应
→ 用 K、D 和 IPPE 求若干组 (rvec, tvec)
→ 按重投影误差选择一组完整位姿
→ 将相机坐标 tvec 变换到 tracker 坐标
→ 得到本次曝光的 {position, yaw}这条链走通以后，完整自瞄流程就可以继续进入跟踪和预测。角点噪声、长尾和 PnP 误差分布属于研究选读，不是理解或运行这条主线的前置条件。
1. 识别结束后究竟得到了什么
检测器首先给出一个装甲板候选和四个图像坐标。四个点可能来自传统视觉提取的灯条端点，也可能来自关键点网络；无论来源如何，送入 PnP 前都要恢复到当前原图的像素坐标，不能继续使用网络输入、letterbox、裁剪 ROI 或缩放图上的坐标。
当前生产顺序统一为：
bl → tl → tr → br
左下 → 左上 → 右上 → 右下程序还会检查坐标是否有限、四边形是否发生自交，并尝试亚像素修正；修正失败时回退到原始角点。PnP 真正关心的不是英文名字，而是二维数组和三维数组的第 i 项必须指向同一个物理角点。
| 索引 | 二维图像点 | 三维模型点语义 |
|---|---|---|
| 0 | p_bl = (u_bl, v_bl) | 装甲板左下角 |
| 1 | p_tl = (u_tl, v_tl) | 装甲板左上角 |
| 2 | p_tr = (u_tr, v_tr) | 装甲板右上角 |
| 3 | p_br = (u_br, v_br) | 装甲板右下角 |
2. 为什么还需要一块三维装甲板模型
一个像素点只确定从相机光心出发的一条射线，不能告诉我们角点位于射线上的一米、三米还是五米处。PnP 能恢复尺度，是因为比赛目标的宽、高已知。
在装甲板自身坐标系 A 中，把原点放在板中心，+x_A 指向板面外，另外两轴沿板面。当前小装甲板模型为 135 mm × 55 mm，大装甲板模型为 225 mm × 55 mm。以宽 w、高 h 表示，同序模型点可以写成：
bl = (0, +w/2, -h/2)
tl = (0, +w/2, +h/2)
tr = (0, -w/2, +h/2)
br = (0, -w/2, -h/2)四点都位于 x_A = 0 的同一平面。识别出的装甲板类型决定使用小板还是大板模板；尺寸选错会直接把尺度错误传播到距离。
3. 相机内参和畸变做了什么
相机标定给出内参矩阵 K 和畸变参数 D：
K = [ fx 0 cx ]
[ 0 fy cy ]
[ 0 0 1 ]fx、fy描述横向和纵向焦距；cx、cy是主点；D描述径向和切向镜头畸变。
标定参数必须与当前相机、分辨率和图像裁剪方式一致。常见的两种合法用法是：
- 原始像素角点配合
K、D直接求解； - 先将角点去畸变，再配合与去畸变结果一致的相机模型求解。
不能先去畸变，再把同一组 D 重复传入求解器，否则相当于校正两次。
4. PnP 在数学上求什么
PnP 要求装甲板坐标系 A 到 OpenCV 相机坐标系 C 的刚体变换：
p_C = R_CA · p_A + t_CA其中：
R_CA表示装甲板相对相机的旋转；t_CA表示装甲板坐标系原点在相机坐标系中的位置；- OpenCV 用 Rodrigues 向量
rvec紧凑表示旋转矩阵R_CA； tvec就是t_CA。
空间点经过刚体变换、透视投影和镜头畸变后，应当落到检测器给出的像素点。忽略畸变写成齐次形式就是：
s · [u, v, 1]ᵀ = K · (R_CA · p_A + t_CA)求解器寻找一组 R_CA、t_CA，让四个模型角点重新投影到图像后尽量贴近四个检测角点。常用评分是重投影 RMS：
RMS = sqrt(Σ ||p_detected_i - p_projected_i||² / 4)已知尺寸让四条射线之间具有固定的空间距离约束，因此四点共同提供了位置和朝向信息。
5. IPPE 候选不是“只有旋转的候选”
装甲板是平面目标。带噪四边形有时允许两组很接近的空间姿态都产生相似投影，因此 solvePnPGeneric(..., SOLVEPNP_IPPE) 可能返回多组解。
最容易混淆的地方是：每一个候选从一开始就是完整的位姿对。
候选 0 = (rvec₀, tvec₀, reprojection_rms₀)
候选 1 = (rvec₁, tvec₁, reprojection_rms₁)当前生产实现会：
- 枚举 IPPE 返回的有限候选；
- 用原始装甲板模型重新投影四角点；
- 计算每个完整
(rvec, tvec)的重投影 RMS； - 稳定排序并选择第 0 组完整候选。
当前 free-IPPE translation 选解不使用相邻帧历史。时间连续性可以是其他系统的扩展判据，但不能写成当前生产 PnP 已经采用的规则。
6. 为什么选中的 tvec 就是相机坐标位置
这不是选解之后额外发生的一次坐标转换。回到刚体方程：
p_C = R_CA · p_A + t_CA装甲板模型原点就在板中心，因此板中心满足 p_A = (0,0,0)。代入后：
p_C(center) = t_CA = tvec所以选中候选的 tvec = [x_C, y_C, z_C]ᵀ，天然就是装甲板中心在 OpenCV 相机坐标系中的位置。当前求解器内部使用毫米：
+x_C：图像右方；+y_C：图像下方；+z_C：相机前方，也就是主要的视线深度。
选解只是在若干组完整的相机位姿中决定使用哪一组，并不会把 rvec 再“转换成”一个 tvec。
7. 从 camera frame 变换到 tracker frame
相机坐标适合成像，却不适合直接描述车辆周围的运动。云台一转，即使目标静止，它在相机坐标中的数值也会改变。因此选中的 p_C = tvec 还要经过：
OpenCV 相机坐标 C → 云台坐标 G → tracker / 车体坐标 T本教程使用：
| 坐标系 | 轴方向 | 来源 |
|---|---|---|
相机 C | 右、下、前 | OpenCV PnP |
云台 G | 前、左、上 | 相机—云台安装标定 |
tracker T | 前、左、上 | 同曝光云台光学姿态 |
静态相机外参给出 G←C 的旋转和平移：
p_G = R_GC · p_C + t_GC再用本次曝光的云台姿态得到 T←G：
p_T = R_TG(exposure) · p_G合起来就是：
p_T = R_TG(exposure) · (R_GC · p_C + t_GC)进入这一步前要把 PnP 的毫米统一换成外参与 tracker 使用的米。方向或法向量只乘旋转，不能加平移。
8. 时间到底出现在哪里
单帧 PnP 方程不需要 FPS、历史或 dt。时间只出现在 PnP 周围的两个阶段：
| 阶段 | 时间要求 | 原因 |
|---|---|---|
| 四角点与单帧 PnP | 不需要 dt | 只解当前图像的几何关系 |
camera → tracker | 需要本次曝光时刻 | 必须使用拍下这张图时的云台姿态，不能使用稍后的处理时刻姿态 |
| tracker / EKF | 需要连续时间戳和 dt | 用于速度估计、预测、漏检和超时 |
因此，“同曝光时间戳”不是 IPPE 的数学输入。它保证后续坐标变换没有把另一时刻的云台姿态配到当前图像上；真正的帧间时序建模从 tracker/EKF 才开始。
9. 本项目怎样分别处理 translation 和 yaw
当前普通装甲板的生产 translation 直接使用选中 free-IPPE 候选的 tvec。yaw 则是另一个问题：项目结合选中 tvec、装甲板在 tracker/车体坐标中的固定 +15° 倾角，以及同曝光光学姿态做约束重投影优化。
因此不要把 free-IPPE rvec 的某个分量直接当作车体坐标 yaw，也不要认为固定倾角参与了 translation 候选的选择。位置和 yaw 共享当前四角点，但它们在生产实现中的求解路径并不完全相同。
10. 一帧结果应该怎样验收
最直接的检查是把选中的三维模型重新投影到原图：
- 四个重投影点是否贴近检测角点；
- 深度是否为有限正值；
- 小板/大板尺寸和单位是否正确；
K、D是否属于当前相机与分辨率；C → G → T的轴方向是否正确；- 静止目标在云台转动时，tracker-frame 位置是否仍基本稳定；
C → T → C往返是否能够闭合。
重投影误差小只说明这组位姿能够解释当前四个像素点，不自动证明物理深度、yaw 或跨帧状态完全准确。
至此，主线已经得到一条 tracker-frame 单帧位姿观测。接下来可以直接进入跟踪与预测。如果想进一步研究角点噪声、平面深度弱约束和误差分布，再阅读研究选读：PnP 观测误差与验证。这部分研究不会阻断完整自瞄流程。