视觉SLAM理论入门——（8）视觉里程计之特征点法—PnP

关注

原创最新推荐文章于 2026-05-16 15:43:32 发布 1.8k 阅读 AI 写同款· GEO 优化 ›

SLAM学习笔记----坐标关系梳理及PnP算法详解一，重要的坐标关系的解析四个坐标系：世界坐标系、相机坐标系、图像坐标系、像素坐标系。世界坐标系：机器人或相机运动过程中，肯定需要知道它的位置，因此需要设定世界坐标系，认定固定不动，作为参考坐标系，描述世界中的任何一点P（Xw,Yw,Zw)。相机坐标系：相机或机器人运动的一个坐标系，通过世界坐标系的变换（旋转R,平移T）计算得到。因此主要是将世界坐标系描述的点坐标P（Xw,Yw,Zw)转换成相机坐标系下描述P（Xc,Yc,Zc)，方便计算得到在成像坐标系的坐标。图像（成像）坐标系：描阅读详情

PnP是求解 3D - 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿

2D-2D的对极几何方法需要八个或八个以上的点对（以八点法为例），且存在着初始化、纯旋转和尺度的问题

然而，如果两张图像中，其中一张特征点的 3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的 3D 位置可以由三角化，或者由 RGB-D 相机的深度图确定

因此，在双目或 RGB-D 的视觉里程计中，我们可以直接使用 PnP 估计相机运动。而在单目视觉里程计中，必须先进行初始化，然后才能使用 PnP

PnP 问题有很多种求解方法，如：P3P、DLT、EPnP、UPnP等。除此以外还可以使用非线性优化方法，构建最小二乘问题并迭代求解（BA）

1、直接线性变换（DLT）

考虑某个空间点 P，它的齐次坐标为 $P = (X,Y,Z,1)^T$ 。在图像 $I_1$ 中，投影到特征点 $x_1 = (u_1 ,v_1 ,1)^T$ （以归一化平面齐次坐标表示）。此时相机的位姿 R,t 是未知的。定义增广矩阵 $T = [R|t]$ （T不是变换矩阵）为一个 3 × 4 的矩阵，包含了旋转与平移信息，展开形式如下：

用最后一行把 s 消去，得到两个约束（因为三个约束中只有两个线性无关）：

将 T 按行分解为 $t_1,t_2,t_3$ ，上面两个约束可以化为

假设一共有 N 个特征点，可以列出线性方程组：

由于 t 一共有 12 维，因此最少通过六对匹配点，实现矩阵 T 的线性求解。当匹配点大于 6 对时，可以使用 SVD 等方法对超定方程求最小二乘解

在DLT求解中，直接将 T 矩阵看成了 12 个未知数，忽略了它们之间的联系。而旋转矩阵 R 属于 SO(3)，DLT的解（只是一般的矩阵）不一定满足 SO(3)的约束

因此必须对 DLT 估计的 T 的左边3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似。这可以由 QR 分解完成，也可以使用下式计算，这相当于把结果从矩阵空间重新投影到 SE(3) 流形上，转换成旋转和平移两部分

上面使用了归一化坐标 $x_1 = (u_1 ,v_1 ,1)^T$ ，默认 K 已知（实际上在SLAM中，K通常是已知的）。如果内参 K 未知，可以用PnP估计 K,R,t 三个量，不过未知数增加，估计效果会差些

2、P3P

P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点。记 3D点（世界坐标）为 A,B,C，2D 点为 a,b,c，相机光心 O 。此外，P3P 还需要使用一对验证点 D - d

上图的三角形之间存在对应关系

利用余弦定理，有：

对上面三式全体除以 $OC^2$ ，并且记 x = OA/OC,y = OB/OC，得：

记 v = $AB^2$ / $OC^2$ ,uv = $BC^2$ / $OC^2$ ,wv = $AC^2$ / $OC^2$ ，有：

将式1中的v代入式2、3，有：

由于我们知道 2D 点的图像位置，三个余弦角是已知的。u、w可以通过A、B、C的世界坐标求得（变换到相机坐标后，不会改变比值u、v）。x、y随相机运动而变化，是未知的

因此，经过上面的推导，可以得到关于x、y的二元二次方程组。使用吴消元法可以求得解析解。类似分解 E ，这个方程组最多可能得到4个解，可以用验证点来计算最可能的解，得到 A,B,C 在相机坐标系下的 3D 坐标。然后，根据 3D-3D 的点对，计算相机的运动 R,t

总的来说，P3P的方法为了求解 PnP，利用了三角形相似性质，求解投影点 a,b,c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题

在 SLAM 当中，通常的做法是先使用 P3P/EPnP 等方法估计相机位姿，然后构建最小二乘优化问题对估计值进行调整

然而，P3P 也存在着一些问题：

1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息

2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效

3、BA

除了使用线性方法之外，还可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题

前面说的线性方法，往往是先求相机位姿，再求空间点位置，而非线性优化则是把它们都看成优化变量，放在一起优化。可以用它对 PnP 或 ICP 给出的结果进行优化

在 PnP 中，BA 问题，是一个最小化重投影误差问题

考虑 n 个三维空间点 P 和它们的投影 p，我们希望计算相机的位姿 R,t，它的李代数表示为 ξ。假设某空间点坐标为 $P_i = [X_i ,Y_i ,Z_i ]^T$ ，其投影的像素坐标为 $u_i = [u_i ,v_i ]^T$

投影过程描述为：

矩阵形式是：

式中隐含着的齐次坐标到非齐次的转换。由于相机位姿未知以及观测点的噪声，该等式存在一个误差，把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

这里的误差项，是将像素坐标（观测到的投影位置）与 3D 点按照当前估计的位姿进行投影得到的位置相比较得到的误差（称为重投影误差）

求解这个最小二乘问题的含义是：通过特征匹配知道 $p_1$ 和 $p_2$ 是同一个空间点 P 的投影，但是不知道相机的位姿。在初始值（相机的位姿）中，P 的投影 $p_2'$ 与实际的 $p_2$ 之间有一定的距离。于是调整相机的位姿，使得这个距离变小，使估计位姿贴近实际位姿。不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零