PnP学习

$PnP$ (Perspective-n-Point) 是求解 $3D$ 到 $2D$ 点对运动的方法。它描述了当我们知道 $n$ 个 $3D$ 空间点以及它们的投影位置时，如何估计相机所在的位姿。前面已经说了, $2D-2D$ 的对极几何方法需要八个或八个以上的点对（以八点法为例）, 且存在着初始化、纯旋转和尺度的问题。然而, 如果两张图像中, 其中一张特征点的 $3D$ 位置已知, 那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。特征点的 $3D$ 位置可以由三角化, 或者由 RGB-D 相机的深度图确定。因此, 在双目或 RGB-D 的视觉里程计中, 我们可以直接使用 $PnP$ 估计相机运动。而在单目视觉里程计中, 必须先进行初始化, 然后才能使用 $PnP$。$3D-2D$ 方法不需要使用对极约束, 又可以在很少的匹配点中获得较好的运动估计，是最重要的一种姿态估计方法。
$PnP$ 问题有很多种求解方法,例如用三对点估计位姿的 $P3P$, 直接线性变换 $DLT$, $EPnP$(Efficient PnP), $UPnP$ 等等)。此外，还能用非线性优化的方式, 构建最小二乘问题并迭代求解, 也就是万金油式的光束法平差(Bundle Adjustment, BA)。我们先来看 $DLT$, 然后再讲 Bundle Adjustment。

7.7.1 直接线性变换 (DLT)
我们考虑这样一个问题：已知一组3D点的位置，以及它们在某个相机的投影位置，求该相机的位姿。这个问题也可以用于求解给定地图和图像时的相机状态问题。如果把3D点看成在另一个相机坐标系中的点的话，则也可以用来求解两个相机的相对运动问题。我们从简单的问题出发。

考虑某个空间点 $P$，它的齐次坐标为 $P=(X,Y,Z,1{)}^T$。在图像 $I1$ 中，投影到特征点 $x1=(u1,v1,1{)}^T$（以归一化平面齐次坐标表示）。此时相机的位姿 $R,t$ 是未知的。与单应矩阵的求解类似，我们定义增广矩阵 $[R|t]$ 为一个 3 × 4 的矩阵，包含了旋转与平移信息。我们把它的展开形式列写如下：

$s\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{cccc}{t}_1& t_2& t_3& t_4\\ t_5& t_6& t_7& t_8\\ t_9& t_{10}& t_{11}& t_{12}\end{array}\right)\left(\begin{array}{c}X\\ Y\\ Z\\ 1\end{array}\right)$

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

$u_1=\frac{t_{1}X+t_{2}Y+t_{3}Z+t_4}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$

$v_1=\frac{t_{5}X+t_{6}Y+t_{7}Z+t_8}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$

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

${\mathbf{t}}_1={\left(t_1,t_2,t_3,t_4\right)}^T,{\mathbf{t}}_2={\left(t_5,t_6,t_7,t_8\right)}^T,{\mathbf{t}}_3={\left(t_9,t_{10},t_{11},t_{12}\right)}^T$

于是有：

${\mathbf{t}}_1^T\mathbf{P}-{\mathbf{t}}_3^T\mathbf{P}{u}_1=0$

和

${\mathbf{t}}_2^T\mathbf{P}-{\mathbf{t}}_3^T\mathbf{P}{v}_1=0$

请注意 $t$ 是待求的变量，可以看到每个特征点提供了两个关于 $t$ 的线性约束。假设一共有 $N$ 个特征点，可以列出线性方程组：
(第一个点，第二个点，第三个点……)
$\left(\begin{array}{ccc}{P}_1{}^T& 0& -u_1{P}_1^T\\ 0& P_1^T& -v_1{P}_1^T\\ P_2^T& 0& -u_1{P}_2^T\\ 0& P_2^T& -u_1{P}_2^T\\ ⋮& ⋮& ⋮\\ P_N^T& 0& -u_N{P}_N^T\\ 0& P_N^T& -v_N{P}_N^T\end{array}\right)\left(\begin{array}{c}{t}_1\\ t_2\\ t_3\end{array}\right)=0$

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

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

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