ORBSLAM2中的EPnP算法

在看到orbslam2中的Tracking::Relocalization中的PnPsolver::iterate这个函数的时候新一轮的懵逼又开始了首先对这个算法真心不清楚，在不清楚的状况下，看代码更是一头雾水，所以借鉴了深入EPnP算法，还有orb_slam代码解析(2)Tracking线程有了一些理解，但其中还是有一些细节不是很懂，所以决定看原文和翻看矩阵分析并且按照代码来回顾一遍。首先从EPnP: An Accurate O(n) Solution to the PnP Problem这篇论文的第三部分看起，有些内容就不叙述了，深入EPnP算法写的很明白，我就写我觉得需要知道的地方吧。

EPnP

假设有个已知参考点在世界坐标系中3D坐标和其对应的图像投影点的2D坐标，在实际中要使用EPnP，匹配点的个数应大于15。该方案是将相机坐标表示为控制点(control point)的加权和，对于非平面的情况来说，需要4个非共面的控制点，而对于平面来说只需要3个。

4个控制点的选取：

控制点的坐标选取与所有参考点的世界坐标的质心有关，为什么要选择质心呢？原因是作者在实验中发现其中一个参考点设置为质心后该算法的稳定性会上升，这在某种程度上是说的通的，因为质心的使用相当于通过归一化坐标点的方式来调节下面引用的线性方程组。所以，4个控制点的坐标可以通过3D参考点集表示：

第一个控制点的坐标设置为质心：

进而通过去质心得到矩阵，它是一个的矩阵

通过对矩阵进行SVD分解，由于参考点的坐标基本上是不相同的，所以肯定是非奇异的，就有3个线性无关的特征向量，那么得到的特征值分解为

将剩余的3个控制点表示为

其中和是U对应的特征值和特征向量。在orbslam2中，计算控制点的代码在PnPsolver::choose_control_points中。

控制点系数的计算：

由于，对于世界坐标和相机坐标之间相差了一个位姿变换，通过计算推导可以得到：

所以对于权重来说，不论是在世界坐标系还是相机坐标系下，它的值都是一样的，我们只须通过世界坐标和世界坐标系下得到控制点得到权重。那么它应该怎样计算呢？

将上式展开可以得到：

$\mathbf{p}_i^w=\begin{pmatrix}x_i^w\\y_i^w\\z_i^w\end{pmatrix}=\begin{pmatrix}\mathbf{c}_1^w &\mathbf{c}_2^w &\mathbf{c}_3^w &\mathbf{c}_4^w\end{pmatrix}\begin{pmatrix}\alpha _{i1}\\\alpha_{i2}\\\alpha_{i3}\\\alpha_{i4}\end{pmatrix}$

又有条件，那么可以得到：

$\begin{pmatrix}x_i^w-\mathbf{c}_1^w\\y_i^w-\mathbf{c}_1^w\\z_i^w-\mathbf{c}_1^w\end{pmatrix}=\begin{pmatrix}\mathbf{c}_1^w-\mathbf{c}_1^w &\mathbf{c}_2^w-\mathbf{c}_1^w &\mathbf{c}_3^w -\mathbf{c}_1^w \end{pmatrix}\begin{pmatrix}\alpha_{i2}\\\alpha_{i3}\\\alpha_{i4}\end{pmatrix}\implies P=C\alpha$

所以，肯定是可逆的，因为他们不共面，这样控制点系数的值就计算出来了。orbslam2中代码实现在PnPsolver::compute_barycentric_coordinates中。

M矩阵

M是一个或者是的矩阵，本问题的求解与M息息相关，所以首先说明M是怎么得到的。

假设是相机的内参标定矩阵，是参考点对应的投影点，就有

是投影尺度系数，控制点的坐标表示为，投影点的坐标表示为，内参矩阵为

将这些值代入，得到

$\forall _i，w_i\begin{bmatrix}u_i\\v_i\\1\end{bmatrix}=\begin{bmatrix} f_u &0 &u_c\\ 0 &f_v &u_v\\ 0 & 0 &1\end{bmatrix}\sum_{j=1}^4 \alpha_{ij} {\begin{bmatrix}x_j^c \\ y_j^c\\z_j^c\end{bmatrix} }$

可以看到该方程的未知量有12个，就是4个控制点在相机坐标系下的坐标值。通过将代入，可以将上式展开为2项：

可以将上面两个等式写成矩阵的形式：

$\begin{bmatrix}\alpha_{i1} f_u &0 &\alpha_{i1}(u_c-u_i) &\alpha_{i2} f_u &0 &\alpha_{i2}(u_c-u_i) &\cdots \\\alpha_{i1} f_v &0 &\alpha_{i1}(v_c-v_i) &\alpha_{i2} f_v &0 &\alpha_{i2}(v_c-v_i) &\cdots\end{bmatrix}\begin{bmatrix}x_1^c\\y_1^c\\z_1^c\\x_2^c\\y_2^c\\z_2^c\\\vdots\end{bmatrix}=0\implies \mathbf{M}\mathbf{x}=0$

可以知道一共有n个参考点，所以矩阵的大小为，的维数为。看到这个等式就可以想到矩阵的零空间，根据矩阵的SVD分解可以得到：

称为矩阵的右奇异向量，是的向量，通过特征值分解计算得出零空间对应的向量，由于可能不止一个向量式等式为0，所以将向量表示为：

N表示为使为0的个数，其中是未知的，N为的零空间元素的个数。这里解释一下，为什么可以直接用的零空间呢？主要是因为，M的奇异值分解的奇异值就是通过计算的特征值开平方得到的，所以他们对角阵上为0的点是一样的。对于欧式相机模型，N的值与焦距有关，当焦距很大时，N=4，较小时，N=1（如下图所示）。对于射影相机模型N=4，因为每个参考点的深度变化后仍满足约束。

特征值为0的个数与焦距的关系

在ORBSLAM2中，M矩阵通过PnPsolver::fill_M(CvMat * M, const int row, const double * as, const double u, const double v)来创建。

N的选取

论文中只讨论了当N=1,2,3,4时未知数个数的情况，下面就来具体看看这4种情况。

对于未知数的求解引入公式

（1）

之所以会相等是内参矩阵只是对点进行坐标变换，并不会改变两点之间的距离。

对于N=1的情况，只有，设为的子向量，对应与，将其带入公式（1），得：

其中右边项是已知的，所以该方程的未知数只有一个。如果求得的可以使上式相等，可以通过使用柯西不等式进行求解，所以：

（2）

对于N=2的情况，，将其带入公式（1），可得：

转化为

将左式展开可以知道得到的都是2次项，分别为，将他们分别表示为，所以N=2的时候有3个未知数。由于一共有4个控制点，随机挑选其中的两个作为方程的系数，那么一共有个方程。设一个方程

(3)

其中，是一个的矩阵，为4个控制点其中的两个控制点的差值的模的平方，为的向量。求解上面的公式需要求的伪逆和选择的标志，那么对于相机坐标系下的点的3D坐标都有正的深度。对得到的还需要相同的尺度，使用公式（2）计算，那么相机坐标系下的控制点可以表示为：

对于N=3的情况，与N=2的情况类似，只不过为的向量，而是的矩阵，这里就需要求逆而不是伪逆了。

对于N=4的情况，可以推出它有10个未知数，而方程最多只有6个，论文中解决这个问题通过再定位方法，与找4个控制点的方法类似。首先通过最初的约束方程得到在零空间中的，之后正确的系数通过引用新的二次方程然后线性解决，所以主要再定位。新的二次方程为

其中表示的任一置换。

ORBSLAM2中的计算

对于上面的N的情况已经知道了，在orbslam2中由于N的取值并不是确定的，所以先构造N=4的矩阵，且v的选取只要取特征值分解之后的矩阵的最后4个向量就代表了。通过PnPsolver::compute_L_6x10(const double * ut, double * l_6x10)函数计算，其中一行可以由下式算出：

$\begin{bmatrix} (||v_1^{[i]}-v_1^{[i]}||)^2 & 2(||v_1^{[i]}-v_1^{[i]}||)(||v_2^{[i]}-v_2^{[i]}||) &(||v_2^{[i]}-v_2^{[i]}||)^2 &\cdots \end{bmatrix} \begin{bmatrix} \beta_{11}\\ \beta_{12}\\ \beta_{22} \\ \beta_{13}\\ \vdots \end{bmatrix}$

可以知道的系数为，所以就是他们的系数点乘，如果则系数要乘以2，这就是L每一行的表达式，它一共有4个控制点，所以有6行。

之后通过PnPsolver::compute_rho(double * rho)计算误差。

已知误差和矩阵了，就可以计算N=2,3,4情况下的值了。

计算N=4情况下的值使用的是PnPsolver::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,double * betas)函数。由于，所以就选取了的系数构成前面的系数矩阵，之后进行矩阵求解就得到了这四个值，对着4个值进行求解就得到了的值，注意符号的选取。同理，N=2,3的结果也可以得到，具体详见代码。

初值得到之后，需要通过高斯牛顿法使误差达到减小。

高斯牛顿法就不具体介绍了，主要是要通过偏差对进行求导，这是数对矩阵的求导，比如说是对的求导，那么就是对中的元素求导，具体的就不求了,参照上面的中的元素，有1下标的就可以进行求一阶导。最后得到一个的矩阵，称为矩阵。误差也很容易进行求解保存在中，是一个的矩阵，求解这两个矩阵的代码为PnPsolver::compute_A_and_b_gauss_newton。之后再将A进行QR分解，得到，将其加到上。迭代5次，得到最后的值。

R和t的计算

终于得到了值就可以计算旋转和平移了。首先将值代入求出相机坐标系下四个控制点的坐标。之后通过控制点的系数就可以计算出相机坐标系下参考点的坐标了，得到的坐标需要使深度值为正数所以得对符号进行处理。有了相机坐标系和世界坐标系的对应点就是3D-3D，就可以使用ICP进行求解了。具体过程就不阐述了，可见PnPsolver::estimate_R_and_t。最后通过投影误差来选择R和t。

总结

以上就是EPnP在ORBSLAM2中的使用过程，说的比较粗糙。至于在平面上的情况，那么控制点就变成了3个，M就是的矩阵，最大的不同是2次约束从6变成了3。具体的就看论文吧，我已经写不动了。

参考资料

EPnP: An Accurate O(n) Solution to the PnP Problem Vincent Lepetit · Francesc Moreno-Noguer · Pascal Fua

深入EPnP算法

orb_slam代码解析(2)Tracking线程