ORBSLAM2中的EPnP算法

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

EPnP

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

4个控制点的选取：

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

第一个控制点的坐标设置为质心： $\mathbf{c}_1^w=\frac{1}{n}\sum_{i=1}^n \mathbf{p}_i^w$

进而通过去质心得到矩阵 $\mathbf{A}=\begin{bmatrix}(\mathbf{p}_1^w)^T-(\mathbf{c}_1^w)^T\\\vdots\\(\mathbf{p}_n^w)^T-(\mathbf {c}_ 1 ^w)^T\end{bmatrix}$ ，它是一个 $n\times 3$ 的矩阵

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

$A^TA=U\Sigma U^{-1}$

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

$\mathbf{c}_j^w=\mathbf{c}_1^w+\lambda _{c,j-1}^\frac{1}{2}v_{c,j-1}，j=2,3,4$

其中 $\lambda _{c,j-1}$ 和 $v_{c,j-1}$ 是U对应的特征值和特征向量。在orbslam2中，计算控制点的代码在PnPsolver::choose_control_points中。

控制点系数的计算：

由于 $\mathbf{p}_i^w=\sum_{i=1}^4 \alpha _{ij}\mathbf{c}_j^w，\sum_{i=1}^4 \alpha _{ij}=1$ ，对于世界坐标和相机坐标之间相差了一个位姿变换 $[R|t]$ ，通过计算推导可以得到：

$\mathbf{p}_i^c=\sum_{i=1}^4 \alpha _{ij}\mathbf{c}_j^c$

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

将上式展开可以得到：

$\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}$

又有条件 $\sum_{i=1}^4 \alpha _{ij}=1$ ，那么 $\alpha_{i1}=1-\alpha_{i2}-\alpha_{i3}-\alpha_{i4}$ 可以得到：

$\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$

所以 $\alpha = C^{-1}P$ ， $C$ 肯定是可逆的，因为他们不共面，这样控制点系数的值就计算出来了。orbslam2中代码实现在PnPsolver::compute_barycentric_coordinates中。

M矩阵

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

假设 $\mathbf{K}$ 是相机的内参标定矩阵， $\{ \mathbf{u}_i\}_{i=1,...,n}$ 是参考点 $\{\mathbf{p}_{i} \}_{i=1,..,i}$ 对应的投影点，就有

$\forall _i，w_i\begin{bmatrix}\mathbf{u}_i\\1\end{bmatrix}=\mathbf{K}p_i^c=\mathbf{K}\sum_{j=1}^4 \alpha_{ij} \mathbf{c}_j^c$

$w_i$ 是投影尺度系数，控制点 $\mathbf{c}_j^c$ 的坐标表示为 ${\begin{bmatrix}x_j^c, y_j^c,z_j^c\end{bmatrix} }^T$ ，投影点 $\mathbf{u}_i$ 的坐标表示为 ${\begin{bmatrix}u_i,v_i\end{bmatrix}}^T$ ，内参矩阵为

$\mathbf{K}=\begin{bmatrix}f_u &0 &u_c\\0 &f_v &u_v\\0 & 0 &1\end{bmatrix}$

将这些值代入，得到

$\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个控制点在相机坐标系下的坐标值。通过将 $w_i=\sum_{j=1}^4\alpha_{ij}z_j^c$ 代入，可以将上式展开为2项：

$\sum_{j=1}^4(\alpha_{ij}f_ux_j^c+\alpha_{ij}(u_c-u_i)z_j^c)=0$

$\sum_{j=1}^4(\alpha_{ij}f_vy_j^c+\alpha_{ij}(v_c-v_i)z_j^c)=0$

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

$\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个参考点，所以矩阵 $\mathbf{M}$ 的大小为 $2n\times 12$ ， $\mathbf{x}$ 的维数为 $12\times1$ 。看到这个等式就可以想到矩阵的零空间 $N(x)=\{x|Ax=0 \}$ ，根据矩阵 $\mathbf{M}$ 的SVD分解可以得到：

$\mathbf{M}v_i=\begin{cases}\sigma_iu_i, &\quad i=1,2,\dots ,r \\0, &\quad i=r+1,\dots, 12\end{cases}$

$v_i$ 称为矩阵 $\mathbf{M}$ 的右奇异向量，是 $12\times1$ 的向量，通过 $\mathbf{M}^T\mathbf{M}$ 特征值分解计算得出零空间对应的向量，由于可能不止一个向量式等式为0，所以将向量 $\mathbf{x}$ 表示为：

$\mathbf{x} =\sum_{i=1}^N \beta _iv_i$

N表示为使 $\mathbf{M}v_i$ 为0的个数，其中 $\beta_i$ 是未知的，N为 $M^TM$ 的零空间元素的个数。这里解释一下，为什么可以直接用 $M^TM$ 的零空间呢？主要是因为，M的奇异值分解的奇异值就是通过计算 $M^TM$ 的特征值开平方得到的，所以他们对角阵上为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种情况。

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

${||c_i^c-c_j^c||}^2={||c_i^w-c_j^w||}^2$ （1）

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

对于N=1的情况，只有 $\mathbf{x}=\beta \mathbf{v}$ ，设 $v^{[i]}$ 为 $3\times1$ 的子向量，对应与 $c_i^c$ ，将其带入公式（1），得：

${||\beta v^{[i]}-\beta v^{[j]}||}^2={||c_i^w-c_j^w||}^2$

其中右边项是已知的，所以该方程的未知数只有一个。如果求得的 $\beta$ 可以使上式相等，可以通过使用柯西不等式进行求解 $||ab||\leq \sqrt{||a||\cdot ||b||}$ ，所以：

$\beta=\frac{\sum_{i,j=1}^4 ||v^{[i]}-v^{[j]}||\cdot ||c_i^w-c_j^w|| }{{\sum_{i,j=1}^4 ||v^{[i]}-v^{[j]}||}^2}$ （2）

对于N=2的情况， $\mathbf{x}=\beta_1\mathbf{v}_1+\beta_2\mathbf{v}_2$ ，将其带入公式（1），可得：

${||(\beta_1 v_1^{[i]}+\beta_2v_2^{[i]})-(\beta_1 v_1^{[j]}+\beta_2v_2^{[j]})||}^2={||c_i^w-c_j^w||}^2$

转化为 ${||(\beta_1 v_1^{[i]}-\beta_1 v_1^{[j]})-(\beta_2v_2^{[i]}-\beta_2v_2^{[j]})||}^2={||c_i^w-c_j^w||}^2$

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

$\mathbf{L} \mathbf{\beta}= \mathbf{\rho }$ (3)

其中 $\mathbf{\beta} ={ \begin{bmatrix}\beta_{11},\beta_{12},\beta_{22}\end{bmatrix}}^T$ ， $\mathbf{L}$ 是一个 $6\times3$ 的矩阵， $\mathbf{\rho}$ 为4个控制点其中的两个控制点的差值的模的平方，为 $6\times1$ 的向量。求解上面的公式需要求 $\mathbf{L}$ 的伪逆和选择 $\beta_a$ 的标志，那么对于相机坐标系下的点的3D坐标都有正的深度。对得到的 $\beta_1，\beta_2$ 还需要相同的尺度，使用公式（2）计算，那么相机坐标系下的控制点可以表示为：

$c_i^c=\beta(\beta_1 v_1^{[i]}+\beta_2v_2^{[i]})$

对于N=3的情况，与N=2的情况类似，只不过 $\mathbf{\beta} ={ \begin{bmatrix}\beta_{11},\beta_{12},\beta_{13} ,\beta_{22}, \beta_{23}, \beta_{33}\end{bmatrix}}^T$ 为 $6\times1$ 的向量，而 $\mathbf{L}$ 是 $6\times6$ 的矩阵，这里就需要求逆而不是伪逆了。

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

$\beta_{ab}\beta_{cd}=\beta_a\beta_b\beta_c\beta_d=\beta_{a^{\prime}}\beta_{b^{\prime}}\beta_{c^{\prime}}\beta_{d^{\prime}}$

其中 $\{ a^{\prime},b^{\prime},c^{\prime},d^{\prime}\}$ 表示 $\{a,b,c,d\}$ 的任一置换。

ORBSLAM2中 $\beta$ 的计算

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

$\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}$

可以知道 $\beta_j$ 的系数为 $||v_j^{[i]}-v_j^{[i]}||$ ，所以 $\beta_{ab}$ 就是他们的系数点乘，如果 $a\neq b$ 则系数要乘以2，这就是L每一行的表达式，它一共有4个控制点，所以有6行。

之后通过PnPsolver::compute_rho(double * rho)计算误差 $\mathbf{\rho}$ 。

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

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

初值得到之后，需要通过高斯牛顿法使误差 $e=\sum_{i,j,s.t.(i<j)}({||c_i^c-c_j^c||}^2-{||c_i^w-c_j^w||}^2)$ 达到减小。

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

R和t的计算

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

总结

以上就是EPnP在ORBSLAM2中的使用过程，说的比较粗糙。至于在平面上的情况，那么控制点就变成了3个，M就是 $2n\times9$ 的矩阵，最大的不同是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线程

人面猴
序言：七十年代末，一起剥皮案震惊了整个滨河市，随后出现的几起案子，更是在滨河造成了极大的恐慌，老刑警刘岩，带你破解...
沈念sama阅读 212,332评论 6赞 493
死咒
序言：滨河连续发生了三起死亡事件，死亡现场离奇诡异，居然都是意外死亡，警方通过查阅死者的电脑和手机，发现死者居然都...
沈念sama阅读 90,508评论 3赞 385
救了他两次的神仙让他今天三更去死
文/潘晓璐我一进店门，熙熙楼的掌柜王于贵愁眉苦脸地迎上来，“玉大人，你说我怎么就摊上这事。” “怎么了？”我有些...
开封第一讲书人阅读 157,812评论 0赞 348
道士缉凶录：失踪的卖姜人
文/不坏的土叔我叫张陵，是天一观的道长。经常有香客问我，道长，这世上最难降的妖魔是什么？我笑而不...
开封第一讲书人阅读 56,607评论 1赞 284
港岛之恋（遗憾婚礼）
正文为了忘掉前任，我火速办了婚礼，结果婚礼上，老公的妹妹穿的比我还像新娘。我一直安慰自己，他们只是感情好，可当我...
茶点故事阅读 65,728评论 6赞 386
恶毒庶女顶嫁案：这布局不是一般人想出来的
文/花漫我一把揭开白布。她就那样静静地躺着，像睡着了一般。火红的嫁衣衬着肌肤如雪。梳的纹丝不乱的头发上，一...
开封第一讲书人阅读 49,919评论 1赞 290
城市分裂传说
那天，我揣着相机与录音，去河边找鬼。笑死，一个胖子当着我的面吹牛，可吹牛的内容都是我干的。我是一名探鬼主播，决...
沈念sama阅读 39,071评论 3赞 410
双鸳鸯连环套：你想象不到人心有多黑
文/苍兰香墨我猛地睁开眼，长吁一口气：“原来是场噩梦啊……” “哼！你这毒妇竟也来了？” 一声冷哼从身侧响起，我...
开封第一讲书人阅读 37,802评论 0赞 268
万荣杀人案实录
序言：老挝万荣一对情侣失踪，失踪者是张志新（化名）和其女友刘颖，没想到半个月后，有当地人在树林里发现了一具尸体，经...
沈念sama阅读 44,256评论 1赞 303
护林员之死
正文独居荒郊野岭守林人离奇死亡，尸身上长有42处带血的脓包…… 初始之章·张勋以下内容为张勋视角年9月15日...
茶点故事阅读 36,576评论 2赞 327
白月光启示录
正文我和宋清朗相恋三年，在试婚纱的时候发现自己被绿了。大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
茶点故事阅读 38,712评论 1赞 341
活死人
序言：一个原本活蹦乱跳的男人离奇死亡，死状恐怖，灵堂内的尸体忽然破棺而出，到底是诈尸还是另有隐情，我是刑警宁泽，带...
沈念sama阅读 34,389评论 4赞 332
日本核电站爆炸内幕
正文年R本政府宣布，位于F岛的核电站，受9级特大地震影响，放射性物质发生泄漏。R本人自食恶果不足惜，却给世界环境...
茶点故事阅读 40,032评论 3赞 316
男人毒药：我在死后第九天来索命
文/蒙蒙一、第九天我趴在偏房一处隐蔽的房顶上张望。院中可真热闹，春花似锦、人声如沸。这庄子的主人今日做“春日...
开封第一讲书人阅读 30,798评论 0赞 21
一桩弑父案，背后竟有这般阴谋
文/苍兰香墨我抬头看了看天上的太阳。三九已至，却和暖如春，着一层夹袄步出监牢的瞬间，已是汗流浃背。一阵脚步声响...
开封第一讲书人阅读 32,026评论 1赞 266
情欲美人皮
我被黑心中介骗来泰国打工，没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留，地道东北人。一个月前我还...
沈念sama阅读 46,473评论 2赞 360
代替公主和亲
正文我出身青楼，却偏偏与公主长得像，于是被迫代替她去往敌国和亲。传闻我的和亲对象是个残疾皇子，可洞房花烛夜当晚...
茶点故事阅读 43,606评论 2赞 350