ORB-SLAM的重定位中使用的EPnP算法解析

EPnP: An Accurate O(n) Solution to the PnPProblem详解

EPnP算法的中心思想就是以四个世界坐标系下的控制点$[c_w^1{c}_w^2{c}_w^3{c}_w^4]$通过投影约束和欧式变换下的距离不变约束，求解相机坐标系下的相应控制点$[c_c^1{c}_c^2{c}_c^3{c}_c^4]$，最后使用高斯牛顿优化法，对欧式变换的距离约束进行优化，优化参数为重新线性化技术的权重参数。

1. 算法亮点

• 算法是封闭式非迭代算法，算法的时间复杂度只有$O(n)$
• 在封闭式非迭代算法中，该算法求解PnP问题的准确率最高
• 当加入高斯牛顿优化器，进行重新线性化权重参数优化时，其准确率接近迭代算法中准确率最高的方法==《Fast and globally convergent pose estimation from video images》==
• 除此之外，EPnP的结果可以作为迭代算法的迭代初始值

2. 算法前置知识

2.1 奇异值和奇异向量

• 奇异值分解（SVD），可以将矩阵$A$$(m\times n)$分解为$A=U\Sigma V^T$
• 左奇异向量为矩阵$U$的列
• 右奇异向量为矩阵$V$的列

2.2 奇异向量和特征向量

• 右奇异向量是$A^{T}A$的特征向量
• 左奇异向量是$A{A}^T$的特征向量

2.3 重新线性化技术

重新线性化技术是一种密码公钥HFE分析技术，通过引入其他约束，可以解决欠定方程的根求解问题。

• 针对欠定方程$Mx=0$
  • 方程的解x可以表示为$x={\sum }_{i=1}^N{\beta }_i{v}_i$，其中$v_i$是矩阵M的0奇异值对应的右奇异向量，${\beta }_i$是待定系数
  • 可以通过矩阵$M^{T}M$的零特征值对应的零特征向量给出矩阵$M$的零奇异值对应的右奇异向量
  • 引入其他约束后，可以对欠定方程的根进行求解

3. 算法流程

3.1 世界坐标系控制点选取

• 将输入的世界坐标系下的3D点的质心作为其中一个$c_w$（世界坐标系控制点）
• 采用与数据主方向对齐的方式选择其余点（这是一种和DLT算法相似的标准化方式）

3.2 根据控制点确定重心坐标

• p i w = ∑ j = 1 4 α i j c j w ∑ j = 1 4 α i j = 1 ⟶ [ x 1 x 2 x 3 x 4 y 1 y 2 y 3 y 4 z 1 z 2 z 3 z 4 1 1 1 1 ] [ α 1 α 2 α 3 α 4 ] = [ X w Y w Z w 1 ] p_i^{w}=\sum_{j=1}^4 \alpha_{ij}c_j^w \quad \sum_{j=1}^4 \alpha_{ij}=1 \longrightarrow \begin{bmatrix} x_1 & x_2 & x_3 & x_4 \\ y_1 & y_2 & y_3 & y_4 \\ z_1 & z_2 & z_3 & z_4 \\ 1 & 1 & 1 & 1\end{bmatrix} \begin{bmatrix} \alpha_1 \\ \alpha_2 \\ \alpha_3 \\ \alpha_4 \end{bmatrix}=\begin{bmatrix} X_w \\ Y_w \\ Z_w \\ 1 \end{bmatrix} piw​=∑j=14​αij​cjw​∑j=14​αij​=1⟶ ​x1​y1​z1​1​x2​y2​z2​1​x3​y3​z3​1​x4​y4​z4​1​ ​ ​α1​α2​α3​α4​​ ​= ​Xw​Yw​Zw​1​ ​，可求解${\alpha }_i$
• $p_i^c={\sum }_{j=1}^4{\alpha }_{ij}{c}_j^c$（相机坐标系下的三维点表示）

3.3 相机坐标的投影约束

• w i [ u i v i 1 ] = [ f u 0 u c 0 f v v c 0 0 1 ] ∑ j = 1 4 α i j [ x j c y j c z j c ] ⟶ { ∑ j = 1 4 α i j f u x j c + α i j ( u c − u i ) z j c = 0 ∑ j = 1 4 α i j f v y j c + α i j ( v c − v i ) z j c = 0 w_i \begin{bmatrix} u_i\\v_i\\1 \end{bmatrix}=\begin{bmatrix} f_u & 0 & u_c \\ 0 & f_v & v_c \\ 0 & 0 & 1 \end{bmatrix}\sum_{j=1}^4\alpha_{ij}\begin{bmatrix} x_j^c \\ y_j^c \\ z_j^c \end{bmatrix} \longrightarrow \left \{ \begin{array}{lcl} \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 \end{array}\right. wi​ ​ui​vi​1​ ​= ​fu​00​0fv​0​uc​vc​1​ ​∑j=14​αij​ ​xjc​yjc​zjc​​ ​⟶{∑j=14​αij​fu​xjc​+αij​(uc​−ui​)zjc​=0∑j=14​αij​fv​yjc​+αij​(vc​−vi​)zjc​=0​
• 选取$N$个点，可得线性系统$Mx=0$，其中$M$矩阵为$2n\times 12$维，$x=[c_1^T,c_2^T,c_3^T,c_4^T{]}^T$

3.4 求解方程$Mx=0$

• 根据重新线性化技术，可得解为$x={\sum }_{i=1}^N{\beta }_i{v}_i$
• 根据相机焦距和矩阵$M$奇异值的实验发现，随着相机焦距的增加，$N$也随之增加，总体来讲在1到4之间
• EPnP会对N取$\left\{1,2,3,4\right\}$，计算四个结果，然后根据重投影误差，选出最少的那个作为解

3.5 分类讨论（引入刚性变换下的向量二范数的距离约束，来求解${\beta }_i$）

3.5.1 对于非平面的情况，四个控制点

• 当$N=1$时，$x=\beta v$
  • $||x^{[i]}-x^{[j]}|{|}^2=||c_i^w-c_j^w|{|}^2⟶||\beta v^{[i]}-\beta v^{[j]}|{|}^2=||c_i^w-c_j^w|{|}^2$
  • 由于在世界坐标下给出了相应的点的真实位置，因此$\beta =\frac{{\sum }_{ij\in [1,4]}||\beta v^{[i]}-\beta v^{[j]}|{|}^2}{{\sum }_{ij\in [1,4]}||v^i-v^j|{|}^2}$
• 当$N=2$时，$x={\beta }_1{v}_1+{\beta }_2{v}_2$
  • $||({\beta }_1{v}_1^{[i]}+{\beta }_2{v}_2^{[i]})-({\beta }_1{v}_1^{[j]}+{\beta }_2{v}_2^{[j]})|{|}^2=||c_i^w-c_j^w|{|}^2$
  • 通过引入向量$\beta ={\left[\begin{array}{ccc}{\beta }_{11}& {\beta }_{12}& {\beta }_{22}\end{array}\right]}^T⟶{\left[\begin{array}{ccc}{\beta }_1^2& {\beta }_1{\beta }_2& {\beta }_2^2\end{array}\right]}^T$，使得方程$L\beta =\rho$成立
  • 其中，$L$是$v_1$和$v_2$组成的矩阵，$\rho$是世界坐标系点之间的距离范数
  • 通过$SVD$分解，或者$L$矩阵的伪逆来计算向量$\beta$
  • 对$\beta$进行分解，保证计算得到的相机坐标系下的3D点都具有正的深度，来筛选一组最合适的${\beta }_1{\beta }_2{\beta }_3$
  • 最后，为了消除尺度的影响，采用$N=1$的方式进行缩放系数的求解$c_i^c=\beta ({\beta }_1{v}_1^{[i]}+{\beta }_2{v}_2^{[i]})$
• 当$N=3$时，$x={\beta }_1{v}_1+{\beta }_2{v}_2+{\beta }_3{v}_3$
  • 按照$N=2$的方式，进行求解
  • 注意，这里唯一不同的是，矩阵$L$是方阵，可以使用矩阵的逆而不是伪逆求解向量
• 当$N=4$时，$x={\beta }_1{v}_1+{\beta }_2{v}_2+{\beta }_3{v}_3+{\beta }_4{v}_4$
  • 按照$N=2$的方式进行求解
  • 得到的矩阵$L$的维度是$6\times 10$维的，因此不能直接进行求解
  • 通过重新线性化的方式进行求解，求解的方式与确定控制点的方式相同
    • $\beta ={\sum }_{i=1}^N{k}_i{v}_i$，$v_i$是矩阵$L$的0奇异值对应的右奇异向量，可以将$\beta$采用$k_i$进行表示
    • 根据乘法交换律，添加新的约束${\beta }_{ab}{\beta }_{cd}={\beta }_a{\beta }_b{\beta }_c{\beta }_d={\beta }_a^{\prime }{\beta }_b^{\prime }{\beta }_c^{\prime }{\beta }_d^{\prime }$

3.5.2 对于平面的情况，3个控制点

• 矩阵$M$的维度会变成$2n\times 9$，因为控制点变成了三个，从而向量$x$变成了9维列向量
• $x={\sum }_{i=1}^N{\beta }_i{v}_i$的改变
  • 当$N=1$时，与非平面，四个控制点无区别
  • 当$N=2$时，矩阵$L$的维度变为$3\times 3$，可以直接通过求逆的方式解出来，与非平面$N=3$的情况类似
  • 当$N=3$时，矩阵$L$的维度变为$3\times 6$，通过乘法交换律约束，进行重新线性化，与同平面$N=4$的情况
  • 当$N=4$时，矩阵$L$的维度变为$3\times 10$，通过乘法交换律约束，进行重新线性化，与同平面$N=4$的情况

3.6 高斯牛顿优化法

• 通过对上述的$N$进行分情况讨论，可以选择一个重投影误差最小的$N$
• 然后对应的$\beta ={\left[\begin{array}{cccc}{\beta }_1& {\beta }_2& \dots & {\beta }_N\end{array}\right]}^T$进行高斯牛顿优化，当$N=4$时，高斯牛顿优化具有最高的时间复杂度，优化函数为$Error(\beta )={\sum }_{(i,j)s.t.i<j}(||c_i^c-c_j^c|{|}^2-||c_i^w-c_j^w|{|}^2)$，其中$c_i^c={\sum }_{j=1}^N{\beta }_j{v}_j^{[i]}$，由于需要优化的参数最多为4，并且优化的方程复杂度为6，因此优化时间可以认为是固定时间，且很短

3.7 ICP方法恢复运动$Rt$

• $\{\begin{array}{l}{R}_{cw}{c}_1^w+t_{cw}=c_1^c\\ R_{cw}{c}_2^w+t_{cw}=c_2^c\\ R_{cw}{c}_3^w+t_{cw}=c_3^c\\ R_{cw}{c}_4^w+t_{cw}=c_4^c\end{array}$
• 根据罗德里格斯公式，将公式中的旋转矩阵使用旋转向量来表示$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta (n\times )$，方程的自由度变成6
• 使用DLT直接线性变换将上述方程变成$Ax=0$的形式，进行SVD分解，求解方程的最小二乘解