视觉SLAM十四讲第七讲笔记（2）

3D-2D: PnP

PnP(perspective-n-Point) 是求解3D到2D点对运动的方法。 它描述了当我们知道$n$个3D空间点以及它们的投影位置时候，如何估计相机所在位姿。 2D-2D的对极几何方法需要八个或八个以上的点对(以八点法为例)，且存在着初始化、纯旋转和尺度的问题。如果两张图像中，其中一张特征点的3D位置已知，那么最少只需三个点对(需要至少一个额外点验证结果)就可以估计相机运动。特征点的3D位置可以由三角化，或者由RGB-D相机的深度图确定。因此，在双目或RGB-D的视觉里程计中，我们可以直接使用 PnP估计相机运动。 而在单目视觉里程计中，必须先进行初始化，然后才能使用PnP。

PnP问题有很多种求解方法，例如用三对点估计位姿的P3P，直接线性变换(DLT)，EPnP (EPnPEficient PnP)， UPnP等等。此外，还能用非线性优化的方式，构建最小二乘问题并迭代求解，也就是万金油式的Bundle Adjustment。

(1) 直接线性变换(DLT)

考虑某个空间点$P$， 它的齐次坐标$P=(X,Y,Z,1{)}^T$。在图像$I_1$中，投影到特征点$x_1=(u_1,v_1,1{)}^T$(以归一化平面齐次坐标表示)。此时相机的位姿$R,t$是未知的。与单应矩阵的求解类似，我们定义增广矩阵$[R|t]$为一个3x4的矩阵，包含了旋转和平移信息，其展开信息如下:
$$s\left(\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right)=\left(\begin{array}{rl}& 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_s}{t_{9}X+t_{10}Y+t_{11}Z+t_{12}}$$

为了简化表示，定义$T$:
$$t_1=(t_1,t_2,t_3,t_4{)}^T,t_2=(t_5,t_6,t_7,t_8{)}^T,t_3=(t_9,t_{10},t_{11},t_{12}{)}^T$$

于是有:
$$t_1^{T}P-t_3^{T}P{u}_1=0$$
和

$$t_2^{T}P-t_3^{T}P{v}_1=0$$

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

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

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

(2) 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问题。 上图可以看出:
$$△Oab-△OAB,△Obc-△OBC,△Oac-△OAC$$
考虑第一组$△Oab-△OAB$的关系。利用余弦定理，可得:
$$O{A}^2+O{B}^2-2OA\cdot OB\cdot cos⟨a,b⟩=A{B}^2$$
对于其他两个三角形有类似的性质，可得:
$$\begin{gathered} O{A}^2+O{B}^2-2OA\cdot OB\cdot cos⟨a,b⟩=A{B}^2 \\ O{B}^2+O{C}^2-2OB\cdot OC\cdot cos⟨b,c⟩=B{C}^2 \\ O{A}^2+O{C}^2-2OA\cdot OC\cdot cos⟨a,c⟩=A{C}^2 \end{gathered}$$

对于上面三式子全体除以$O{C}^2$，并且记$x=OA/OC,y=OB/OC$得到:
$$\begin{gathered} x^2+y^2-2xycos⟨a,b⟩=A{B}^2/O{C}^2 \\ {y}^2+1^2-2ycos⟨b,c⟩=B{C}^2/O{C}^2 \\ {x}^2+1^2-2xcos⟨a,c⟩=A{C}^2/O{C}^2 \end{gathered}$$
记$v=A{B}^2/O{C}^2,uv=B{C}^2/O{C}^2,wv=A{C}^2/O{C}^2$,可得:
$$\begin{gathered} x^2+y^2-2xycos⟨a,b⟩-v=0 \\ {y}^2+1^2-2ycos⟨b,c⟩-uv=0 \\ {x}^2+1^2-2xcos⟨a,c⟩-wv=0 \end{gathered}$$
我们可以把第一个式子中的$v$放到等式一边，并代入第2，3两式子1，得:
$$\begin{gathered} (1-u)y^2-u{x}^2-cos⟨b,c⟩y+2uxycos⟨a,b⟩+1=0 \\ (1-w)x^2-w{y}^2-cos⟨a,c⟩x+2wxycos⟨a,b⟩+1=0 \end{gathered}$$

由于我们知道2D点的图像位置，三个余弦角$cos⟨a,b⟩,cos⟨b,c⟩,cos⟨a,c⟩$是已知的。同时， $u=B{C}^2/A{B}^2,w=A{C}^2/A{B}^2$ 可以通过 A, B, C 在世界坐标系下的坐标算出，变换到相机坐标系下之后，并不改变这个比值。该式中的$x,y$是未知的，随着相机移动会发生变化。 因此，该方程组是关于$x,y$的一个二元二次方程(多项式方程)。解析地求解该方程组是一个复杂的过程，需要用吴消元法。

P3P也存在着一些问题:

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

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

(3) Bundle Adjustment

除了使用线性方法之外，我们可以把PnP问题构建成一个定义于李代数上的非线性最小二乘问题。前面说的线性方法，往往是先求相机位姿，再求空间点位置，非线性优化则是把它们都看成优化变量，放在一起优化。在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$。根据第五讲内容，可得:
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=KT\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$
其中$T$用李代数表示，可得:
$$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=Kexp({ϵ}^{\wedge })\left[\begin{array}{c}{X}_i\\ Y_i\\ Z_i\\ 1\end{array}\right]$$

写成矩阵的形式就是:
$$s_i{u}_i=Kexp({ϵ}^{\wedge })P_i$$
现在，由于相机位姿未知以及观测点的噪声，该等式存在一个误差。因此，我们把误差求和，构建最小二乘问题，然后寻找最好的相机位姿，使它最小化:

$${ϵ}^{\ast }=\underset{ϵ}{\mathrm{argmin}}\frac{1}{2}\sum _{i=1}^n||u_i-\frac{1}{s_i}Kexp({ϵ}^{\wedge })P_i|{|}_2^2$$

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

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

我们通过特征匹配，知道了$p_1$和$p_2$是同一个空间点$P$的投影，但是我们不知道相机的位姿。在初始值中，P的投影${\hat{p}}_2$与实际的$p_2$之间有一定的距离。于是我们调整相机的位姿，使得这个距离变小。不过，由于这个调整需要考虑很多个点，所以最后每个点的误差通常都不会精确为零。 使用李代数，可以构建无约束的优化问题，很方便地通过 G-N, L-M 等优化算法进行求解。在使用 G-N 和 L-M 之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化:

$$e(x+\Delta x)\approx e(x)+J\Delta x$$

这里的$J$的形式是值得讨论的，甚至可以说是关键所在。现在，当$e$为像素坐标误差(2维)，$x$为相机位姿(6维)时，$J$将是一个2 × 6的矩阵。我们来推导$J$的形式。

首先，记变换到相机坐标系下的空间点坐标为$P^{\prime }$,并且把它的前三维取出来:
$$P^{\prime }=(exp({ϵ}^{\wedge })P{)}_{1:3}=[X^{\prime },Y^{\prime },Z^{\prime }{]}^T$$

注意:
$exp({ϵ}^{\wedge })P_i$结果是4 × 1的，而它左侧的$K$是3 × 3的，所以必须把$exp({ϵ}^{\wedge })P_i$的前三维取出来，变成三维的非齐次坐标。

那么相机投影模型相对于$P^{\prime }$为:
$$su=K{P}^{\prime }$$
展开为:
$$\left[\begin{array}{r}su\\ sv\\ s\end{array}\right]=\left[\begin{array}{c}{f}_{x}0c_x\\ 0f_y{c}_y\\ 001\end{array}\right]\left[\begin{array}{c}{X}^{\prime }\\ Y^{\prime }\\ Z^{\prime }\end{array}\right]$$

利用第三行消去$s$(实际上就是$P^{\prime }$的距离)，得:
$$u=f_x\frac{X^{\prime }}{Z^{\prime }}+c_x,v=f_y\frac{Y^{\prime }}{Z^{\prime }}+c_y$$

当我们求误差时，可以把这里的$u,v$与实际的测量值比较，求差。我们对${ϵ}^{\wedge }$左乘扰动量$\delta ϵ$，然后考虑$e$的变化关于扰动量的导数。利用链式法则，可以写出:
$$\frac{\partial e}{\partial \delta ϵ}=\underset{\delta ϵ\to 0}{\lim }\frac{e(\delta ϵ\oplus ϵ)}{\delta ϵ}=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial \delta ϵ}$$

这里的$\oplus$指的是李代数上的左乘扰动。第一项是误差关于投影点的导数。我们进一步得到:
$$\frac{\partial e}{\partial P^{\prime }}=-\left[\begin{array}{c}\frac{\partial u}{\partial X^{\prime }}\frac{\partial u}{\partial Y^{\prime }}\frac{\partial u}{\partial Z^{\prime }}\\ \frac{\partial v}{\partial X^{\prime }}\frac{\partial v}{\partial Y^{\prime }}\frac{\partial v}{\partial Z^{\prime }}\end{array}\right]=-\left[\begin{array}{c}\frac{f_x}{Z^{\prime }}0-\frac{f_x{X}^{\prime }}{{Z^{\prime }}^2}\\ 0\frac{f_y}{Z^{\prime }}-\frac{f_y{Y}^{\prime }}{{Z^{\prime }}^2}\end{array}\right]$$
而第二项为变换后的点关于李代数的导数,进一步可得:
$$\frac{\partial (TP)}{\partial \delta ϵ}=(TP{)}^{\odot }=[\begin{array}{c}I-P^{\prime \wedge }\\ 0^T{0}^T\end{array}]$$
而在$P^{\prime }$的定义中，我们取出了前三维,于是得:
$$\frac{\partial P^{\prime }}{\partial \delta ϵ}=[I,-P^{\prime \wedge }]$$
將这两项相乘，就得到了$2\times 6$的雅可比矩阵:
$$\frac{\partial e}{\partial \delta ϵ}=-\left[\begin{array}{c}\frac{f_x}{Z^{\prime }}0-\frac{f_x{X}^{\prime }}{Z^{\prime 2}}-\frac{f_x{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}{f}_x+\frac{f_x{X}^2}{Z^{\prime 2}}-\frac{f_x{Y}^{\prime }}{Z^{\prime }}\\ 0\frac{f_y}{Z^{\prime }}-\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}-f_y-\frac{f_y{Y}^{\prime 2}}{Z^{\prime 2}}\frac{f_y{X}^{\prime }{Y}^{\prime }}{Z^{\prime 2}}\frac{f_y{X}^{\prime }}{Z^{\prime }}\end{array}\right]$$
这个雅可比矩阵描述了重投影误差关于相机位姿李代数的一阶变化关系。保留了前面的负号，因为这是由于误差是由观测值减预测值定义的。

另一方面，除了优化位姿，我们还希望优化特征点的空间位置。需要讨论$e$关于空间点$P$的导数。仍然利用链式法则:
$$\frac{\partial e}{\partial P}=\frac{\partial e}{\partial P^{\prime }}\frac{\partial P^{\prime }}{\partial P}$$
第一项前面推导了，第二项，根据定义:
$$P^{\prime }=exp({ϵ}^{\wedge })P=RP+t$$
我们发现$P^{\prime }$对P求导后只剩下$R$。于是:
$$\frac{\partial e}{\partial P}=-\left[\begin{array}{c}\frac{f_x}{Z^{\prime }}0-\frac{f_x{X}^{\prime }}{Z^{\prime 2}}\\ 0\frac{f_y}{Z^{\prime }}-\frac{f_y{Y}^{\prime }}{Z^{\prime 2}}\end{array}\right]R$$
于是，我们推导了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们十分重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。