视觉SLAM笔记（36） 3D-2D: PnP

---

1. PnP

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

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

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

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

3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计
是最重要的一种姿态估计方法
PnP 问题有很多种求解方法，例如：
三对点估计位姿的 P3P，直接线性变换（DLT），EPnP（Efficient PnP）， UPnP 等等）

此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解
也就是万金油式的 Bundle Adjustment

---

2. 直接线性变换

考虑某个空间点 $P$，它的齐次坐标为 $P = (X, Y, Z, 1)$^T
在图像 $I$₁ 中，投影到特征点 $x$₁ = ( $u$₁, $v$₁, 1)^T（以归一化平面齐次坐标表示）
此时相机的位姿 R, t 是未知的

与单应矩阵的求解类似，定义增广矩阵 $[R|t]$ 为一个 3 × 4 的矩阵，包含了旋转与平移信息
把它的展开形式列写如下：

用最后一行把 s 消去，得到两个约束：

为了简化表示，定义 T 的行向量：

于是有：

和

请注意 t 是待求的变量，可以看到每个特征点提供了两个关于 t 的线性约束
假设一共有 N 个特征点，可以列出线性方程组：

由于 t 一共有 12 维，因此最少通过六对匹配点，即可实现矩阵 T 的线性求解
这种方法也称为 直接线性变换（Direct Linear Transform， DLT）
当匹配点大于六对时，又可以使用 SVD 等方法对超定方程求最小二乘解

在 DLT 求解中，直接将 T 矩阵看成了 12 个未知数，忽略了它们之间的联系
因为旋转矩阵 R $\in$ SO(3)，用 DLT 求出的解不一定满足该约束，它是一个一般矩阵
平移向量比较好办，它属于向量空间
对于旋转矩阵 R，必须针对 DLT 估计的 T 的左边 3 × 3 的矩阵块，寻找一个最好的旋转矩阵对它进行近似
这可以由 QR 分解完成，相当于把结果从矩阵空间重新投影到 SE(3) 流形上，转换成旋转和平移两部分

需要解释的是，这里的 $x$₁使用了归一化平面坐标，去掉了内参矩阵 K 的影响
这是因为内参 K 在 SLAM 中通常假设为已知
如果内参未知，那么也能用 PnP 去估计 K, R, t 三个量
然而由于未知量的增多，效果会差一些

---

3. P3P

P3P 是另一种解 PnP 的方法
它仅使用三对匹配点，对数据要求较少

P3P 需要利用给定的三个点的几何关系
它的输入数据为三对 3D-2D 匹配点
记 3D点为 A, B, C， 2D 点为 a, b, c，其中小写字母代表的点为大写字母在相机成像平面上的投影

此外， P3P 还需要使用一对验证点
以从可能的解出选出正确的那一个（类似于对极几何情形），记验证点对为 D − d，相机光心为 O

注意，知道的是 A, B, C 在世界坐标系中的坐标，而不是在相机坐标系中的坐标
一旦 3D 点在相机坐标系下的坐标能够算出，就得到了 3D-3D 的对应点
把 PnP 问题转换为了 ICP（Iterative Closest Point，迭代最近点） 问题

首先，显然，三角形之间存在对应关系：

来考虑 $Oab$ 和 $OAB$ 的关系

利用余弦定理，有：

对于其他两个三角形亦有类似性质，于是有：

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

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

可以把第 1 个式子中的 v 放到等式一边，并代入第 2， 3 两式，得：

注意这些方程中的已知量和未知量
由于知道 2D 点的图像位置，三个余弦角cos ⟨a, b⟩ , cos ⟨b, c⟩ , cos ⟨a, c⟩ 是已知的
同时， $u = BC$² $/AB$², $w = AC$² $/AB$² 可以通过A, B, C 在世界坐标系下的坐标算出
变换到相机坐标系下之后，并不改变这个比值

该式中的 x, y 是未知的，随着相机移动会发生变化
因此，该方程组是关于 x, y 的一个二元二次方程（多项式方程）
解析地求解该方程组是一个复杂的过程，需要用 吴消元法
类似于分解 E 的情况，该方程最多可能得到四个解
但可以用验证点来计算最可能的解，得到 A, B, C 在相机坐标系下的 3D 坐标
然后，根据 3D-3D 的点对，计算相机的运动 R, t

从 P3P 的原理上可以看出，为了求解 PnP，利用了三角形相似性质
求解投影点 a, b, c 在相机坐标系下的 3D 坐标
最后把问题转换成一个 3D 到 3D 的位姿估计问题

后文将看到，带有匹配信息的 3D-3D 位姿求解非常容易，所以这种思路是非常有效的
其他的一些方法，例如 EPnP，亦采用了这种思路
然而， P3P 也存在着一些问题：

1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息
2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效

所以后续人们还提出了许多别的方法，如 EPnP、 UPnP 等
它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响
不过，相对于 P3P 来说，原理会更加复杂一些

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

---

4. 光束平差法

除了使用线性方法之外，可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题
前面说的线性方法，往往是先求相机位姿，再求空间点位置
而非线性优化则是把它们都看成优化变量，放在一起优化
这是一种非常通用的求解方式，可以用它对 PnP 或 ICP 给出的结果进行优化
在 PnP 中，这个 光束平差法（Bundle Adjustment）问题，是一个最小化重投影误差（Reprojection error） 的问题

重投影误差示意图：

考虑 n 个三维空间点 $P$ 和它们的投影 $p$，希望计算相机的位姿 $R$， $t$，它的李代数表示为 $ξ$
假设某空间点坐标为 $P$_i = [ $X$_i, $Y$_i, $Z$_i]^T ，其投影的像素坐标为 $u$_i = [ $u$_i, $v$_i]^T

而像素位置与空间点位置的关系如下：

除了用 $ξ$ 为李代数表示的相机姿态之外，别的都和前面的定义保持一致
写成矩阵形式就是：

注意中间隐含着的齐次坐标到非齐次的转换，否则按矩阵的乘法来说，维度是不对的
$exp(ξ$^ $) Pi$ 结果是 4 × 1 的，而它左侧的 K 是 3 × 3 的
所以必须把 $exp(ξ$^ $) Pi$ 的前三维取出来，变成三维的非齐次坐标

现在，由于相机位姿未知以及观测点的噪声，该等式存在一个误差
因此，把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化：

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

使用齐次坐标时，这个误差有 3 维
不过，由于 u 最后一维为 1，该维度的误差一直为零
因而更多时候使用非齐次坐标，于是误差就只有 2 维了

我们通过特征匹配，知道了 p1和 p2 是同一个空间点 P 的投影
但是不知道相机的位姿
在初始值中， P 的投影 $\hat{p}$₂ 与实际的 $p$₂ 之间有一定的距离
于是调整相机的位姿，使得这个距离变小

不过，由于这个调整需要考虑很多个点
所以最后每个点的误差通常都不会精确为零

最小二乘优化问题已经在 视觉SLAM笔记（27） 非线性最小二乘 介绍过了
使用李代数，可以构建无约束的优化问题，很方便地通过 G-N, L-M 等优化算法进行求解
不过，在使用 G-N 和 L-M 之前，需要知道每个误差项关于优化变量的导数，也就是线性化：

这里的 $J$ 的形式是值得讨论的，甚至可以说是关键所在
固然可以使用数值导数，但如果能够推导解析形式时，会优先考虑解析导数
现在，当 e 为像素坐标误差（2 维）， x 为相机位姿（6 维）时， J 将是一个 2 × 6 的矩阵

推导 J 的形式：
在 视觉SLAM笔记（17） 李代数求导与扰动模型 介绍了如何使用扰动模型来求李代数的导数
首先，记变换到相机坐标系下的空间点坐标为 P ′，并且把它前三维取出来：

那么，相机投影模型相对于 P ′ 则为：

展开之：

利用第 3 行消去 s（实际上就是 P ′ 的距离），得：

这与之前讲的相机模型是一致的

当求误差时，可以把这里的 $u$， $v$与实际的测量值比较，求差
在定义了中间变量后，对 $ξ$^ 左乘扰动量 $δξ$，然后考虑 $e$ 的变化关于扰动量的导数
利用链式法则，可以列写如下：

这里的 ⊕ 指李代数上的左乘扰动
第一项是误差关于投影点的导数，在上二式已经列出了变量之间的关系，易得：

而第二项为变换后的点关于李代数的导数
根据在 视觉SLAM笔记（17） 李代数求导与扰动模型 中的推导，得：

而在 P ′ 的定义中，取出了前三维，于是得：

将这两项相乘，就得到了 2 × 6 的雅可比矩阵：

这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系
保留了前面的负号，因为这是由于误差是由 观测值 减 预测值 定义 的
它当然也可反过来，定义成“预测减观测”的形式
在那种情况下，只要去掉前面的负号即可

此外，如果 se(3) 的定义方式是旋转在前，平移在后时
只要把这个矩阵的前三列与后三列对调即可

另一方面，除了优化位姿，还希望优化特征点的空间位置
因此，需要讨论 e 关于空间点 P 的导数
所幸这个导数矩阵相对来说容易一些仍利用链式法则，有：

第一项已在前面推导了，第二项，按照定义：

发现 P ′ 对 P 求导后只剩下 R
于是：

于是，推导了观测相机方程关于相机位姿与特征点的两个导数矩阵
它们十分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（35） 三角化求特征点的空间位置
视觉SLAM笔记（34） 三角测量
视觉SLAM笔记（33） 对极约束求解相机运动
视觉SLAM笔记（32） 2D-2D: 对极几何
视觉SLAM笔记（31） 特征提取和匹配

---