PnP的定义

PnP = Perspective-n-Point，中文常叫：

透视n点位姿估计问题
或
n点透视定位问题

它的标准定义是：

已知n个三维点在某个参考坐标系下的坐标，以及它们在图像中的二维投影点，同时已知相机内参，求相机相对于这些三维点所在坐标系的位姿，也就是R,t。

OpenCV 对 solvePnP 的定义也是这个意思：给定 object points、对应 image projections、相机内参和畸变参数，估计目标物体的位姿。

所谓三维点在某个参考坐标系下的坐标，一般是以某个刚性物体的几何中心（或者某个关键传感器）作为原点的“物体坐标系”。最典型的例子就是无人机三维跟踪任务中的“被观测的飞机的机体坐标系”。

PnP数学描述与求解

假设这么一个场景，现在要做无人机3d目标跟踪，观测相机保持不动（实际上完全可以相对移动世界坐标系运动，但这里为简单做出假设）。

我们提前标定了无人机的5个特征点（比如4个电机与顶部LED灯的距离参数，电机也带LED灯）。那么我们可以实现进通过单帧观测就能求解出无人机位姿吗？答案是肯定的，而且这就是经典的PnP问题。

我们稍微抽象一下：

已知无人机机体坐标系下 5 个特征点的 3D 坐标，例如 4 个电机 LED + 顶部 LED；
已知相机内参；
在一帧图像中检测到其中至少 4 个特征点的 2D 像素坐标；
求无人机坐标系相对于相机坐标系的旋转 R 和平移t。

核心投影模型是：

s \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}=K \begin{bmatrix} R\mid t \end{bmatrix} \begin{bmatrix} X \\ Y \\ Z \\ 1 \end{bmatrix}

其中：(X,Y,Z) 是特征点在无人机机体系下的 3D 坐标；(u,v) 是图像中的像素坐标；K 是相机内参；R,t 就是要求的无人机相对相机位姿；s 是深度尺度因子。

注意这里需要满足几个条件：

1. 相机内参必须已知

包括：

f_x, f_y, c_x, c_y

以及畸变参数。如果是长焦、广角、鱼眼相机，畸变校正尤其重要。

2. 无人机上的特征点 3D 坐标必须已知

不是只知道“几个点之间的大概距离”，最好是建立一个无人机机体系，例如以机体中心为原点：

P_1 = (x_1,y_1,z_1)

P_2 = (x_2,y_2,z_2)

...

例如四个电机大概率在同一平面：

(\pm a, \pm b, 0)

顶部 LED 可以设成：

(0,0,h)

这样 5 个点就不是完全共面了，位姿求解会更稳定。

3. 图像中必须知道每个点对应哪个 3D 点

也就是说，不能只是检测到 5 个亮点，还要知道：

这个是左前电机；
这个是右前电机；
这个是左后电机；
这个是右后电机；
这个是顶部 LED。

如果四个电机 LED 完全一样，且无人机外形对称，就可能出现点匹配歧义，导致解出来的朝向翻转或者旋转 90°。

解决办法包括：

不同 LED 使用不同颜色；
不同 LED 使用不同闪烁频率；
顶部 LED 与电机 LED 外观不同；
利用前后灯颜色区分机头方向；
结合上一帧跟踪结果做时序约束。

至少需要几个点？

理论上：

3 个点：P3P，可以求解，但通常有多个候选解；
4 个点：可以消除多数歧义，常用；
5 个及以上点：更稳，可以做最小二乘优化和异常点剔除。

工程上建议：

检测 2D LED 点
→ 建立 2D-3D 对应
→ solvePnP / EPnP / P3P + RANSAC
→ 非线性优化 refine
→ 输出 R,t
→ 接入 Kalman / EKF / UKF 做 3D 跟踪

OpenCV 里就是：

cv::solvePnP(
    objectPoints,   // 无人机机体系下的 3D 点
    imagePoints,    // 图像中的 2D 点
    cameraMatrix,   // 相机内参
    distCoeffs,     // 畸变参数
    rvec,
    tvec
);

如果有误检或漏检，建议用：

cv::solvePnPRansac(...)

可能遇到的问题：四个电机大概率共面

如果只用 4 个电机点，它们通常都在一个平面上。此时问题退化成类似平面靶标位姿估计，本质上可以通过 homography 求解。

但共面点有几个问题：

存在镜像歧义风险；
远距离时深度方向不稳定；
正对相机时姿态估计还可以，斜视/小视角时容易抖；
如果四个点近似成正方形，对称性会导致 yaw 歧义。

所以“顶部 LED 灯”很关键。它让 5 个点形成非共面 3D 结构，这会明显提高 PnP 的稳定性。

换句话说：

只用四个电机 LED：能做，但容易退化；
四个电机 LED + 顶部 LED：非常适合做单目 6DoF 位姿估计。

相机保持不动有什么帮助？

相机保持不动意味着相机坐标系可以看作世界坐标系，或者提前知道：

T_{world}^{camera}

那么每一帧求得的是：

T_{camera}^{drone}

如果要转到世界坐标系：

T_{world}^{drone} = T_{world}^{camera} T_{camera}^{drone}

因此单帧可以得到无人机的：

3D 位置：x,y,z
姿态：roll, pitch, yaw
后续多帧还可以估计速度、角速度、轨迹。

这和“3D目标跟踪”的关系

单帧 PnP 解决的是：

3D pose estimation

多帧连续估计之后，就是：

3D tracking

典型系统可以这样设计：

LED检测 / 关键点检测
        ↓
2D关键点匹配
        ↓
PnP求单帧6DoF位姿
        ↓
重投影误差检查
        ↓
Kalman / EKF / UKF / 因子图平滑
        ↓
输出稳定的3D轨迹和姿态

跟踪时可以用运动模型约束，例如：

x_t = x_{t-1} + v_{t-1}\Delta t

这样即使某一帧只看到 3 个点，甚至短暂丢点，也可以靠预测和重投影继续关联。