美文网首页
ORBSLAM2中的EPnP算法

ORBSLAM2中的EPnP算法

作者: 小幸运Penny | 来源:发表于2019-04-25 19:50 被阅读0次

在看到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}PC肯定是可逆的,因为他们不共面,这样控制点系数的值就计算出来了。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}}^T6\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线程

.

相关文章

  • ORBSLAM2中的EPnP算法

    在看到orbslam2中的Tracking::Relocalization中的PnPsolver::iterate...

  • EPnP笔记

    问题描述 已知n个空间点在世界坐标系下的坐标及其图像坐标系下的坐标,所要求解的问题是如何根据空间点和图像点之间的对...

  • 通俗易懂讲C++ const必要用法(仅此一版)

    关于C++ const 的全面总结(这个链接的字简直不能看,但是内容是好的) 一、ORBSLAM2中const的例...

  • EPnP论文笔记

    目录 EPnP简介 关于控制点 计算相机坐标系下的控制点(GN优化没写) 计算R,t 平面(未完成) 实验(未完成...

  • ORBSLAM2配置

    编译环境:ubuntu14.04+ROS indigo+opencv2.4.11 参考:http://blog.c...

  • ORBSLAM2解析系列

    具体内容如下: 1、特征提取模块[https://www.jianshu.com/p/db3323eaa7a3]2...

  • 通俗易懂理解ORBSLAM2特征提取模块

    一、通俗易懂理解图像金字塔特征点数目、灰度质心圆索引 1.参考资料: [1] ORBSLAM2 source co...

  • 通俗易懂理解ORBSLAM2初始化模块

    一、通俗易懂理解单目初始化快速特征匹配方法 1.参考资料: [1] ORBSLAM2 source code 2....

  • 字符串匹配

    朴素算法=BF算法=暴力算法 KMP算法 去除朴素算法中 配对不上的 和 已配对过的 比较过程 什么是KMP中的n...

  • 离群点分析

    LOF算法 使用基于密度的局部离群点检测算法LOF鉴于LOF算法的特点,使用了文献[1]中的DLOF算法,在文献中...

网友评论

      本文标题:ORBSLAM2中的EPnP算法

      本文链接:https://www.haomeiwen.com/subject/xgmngqtx.html