SLAM之本质矩阵分解得相对变换的R和t

1 前言

对极几何当中的基础矩阵\(F\)与本质矩阵\(E\)是SLAM当中2D-2D恢复出相对变换的重要变量,其相关推导可参看文献[1]。

在相关矩阵计算时，一般会在两幅图像中，根据特征找到对应匹配对后估计出基础矩阵\(F\)或本质矩阵\(E\)，如果是直接估计出基础矩阵\(F\)，且知道两幅图中的相机参数分别为\(K_1\)、\(K_2\)，则可以直接得到本质矩阵\(E\)

(1-1)$$E=K_2^TFK_1$$

对于本质矩阵与相对变换的\(R\)和\(t\)有如下关系(注意这里指的相对变换是从第一幅图变换到第二幅图的位姿相对变换)

(1-2)$$E=t^\wedge R$$

其中本质矩阵\(E\)还具有如下的性质

(1-3)$$rank(E)=2$$

(1-4)$$2EE^TE=tr(EE^T)E$$

 

2 常规分解[2]

平常从\(E\)得到两幅图像相对变换的\(R\)和\(t\)，主要通过奇异值分解得到，即对\(E\)进行SVD分解可得

(2-1)$$E=USV^T$$

这里先放置一个常量\(W\)

(2-2)$$W=\begin{bmatrix}
 0&  -1&0 \\ 
 1& 0 &0 \\ 
 0&0  & 1
\end{bmatrix}$$

那么最终可以得到如下的\(R\)和\(t\)

(2-3)$$\begin{align*}
t_1^\wedge &= UWSU^T \\ 
t_2 &= -t1\\ 
R_1 &=UWV^T \\ 
R_2 &= UW^TV^T
\end{align*}$$

上面总共存在4组可能的解\((t_1,R_1)、(t_1,R_2)、 (t_2,R_1)、(t_2,R_2)\)，在这4组解中，仅有一组满足所有投影点在两个相机中都为正深度，因此代入匹配点计算出坐标，即可选出正确的一组。

这里需要说明一点，当计算的本质矩阵\(E\)不是严格满足其性质时，其中\(E\)有一个性质是其特征值前两个为相等的非0值，最后一个为0，即公式(2-1)当中的\(S\)应该为如下形式

(2-4)$$S=\begin{bmatrix}
\sigma  & 0 & 0\\ 
 0&  \sigma &0 \\ 
0 & 0 & 0
\end{bmatrix}$$

这时，可以假设一个严格满足本质矩阵的\(E'\)去逼近给的\(E\)矩阵，这里假设\(E'\)为如下形式

(2-5)$$E'=US'V^T$$

那么可以通过最小化\( \left \| E – E' \right \|_F\)来得到最佳的本质矩阵，即最小化如下方程

(2-6)$$\begin{align*}
min  \left \| E – E' \right \|_F &= min  \left \| U^T(E – E')V \right \|_F \\
 &=min  \left \| U^TEV – U^TE'V \right \|_F \\
 &=min  \left \| S – S' \right \|_F \\
 &=min [(\sigma_1 – \sigma)^2 + (\sigma_2 – \sigma)^2 + \sigma_3^2]
\end{align*}
\Rightarrow
\sigma = (\sigma_1+\sigma_2)/2$$

即当\(S'\)满足如下形式时，有最佳逼近

(2-6)$$S'=\begin{bmatrix}
 (\sigma _1 + \sigma _2)/2& 0 & 0\\ 
0 &  (\sigma _1 + \sigma _2)/2 & 0\\ 
0 & 0 & 0
\end{bmatrix}$$

其中\(E\)的特征值由大到小排列为\(\sigma _1\)、\(\sigma _2\) 、\(\sigma _3\)

因此最终使用(2-7)算式当中的\(S'\)替换(2-3)当中的\(S\)即可。

 

3 快速分解[3]

上面是通过奇异值分解得到\(R\)和\(t\)，这里直接求取\(R\)和\(t\)。

先导出一个等式

(3-1)$$\begin{align*}
E^Tt&=(t^\wedge R)^Tt\\
 &= R^T(t^\wedge)^Tt\\ 
 &= R^T(-t^\wedge)t\\ 
 &= -R^T(t^\wedge t)\\ 
 &= 0
\end{align*}$$

从上面公式(3-1)中，可以发现，通过\(E\)可以直接解一个方程\(E^Tt=0\)得到一组非0特殊解\(t\)(真实\(t\)为这个解的\(k\)倍，这个\(k\)目前还不知道，后面可以通过计算反算出真实的\(t\))

从(2-1)可得到\(E\)的模

(3-2)$$\left \| E \right \|_F=\left \| S \right \|_F=\sqrt{2} \sigma$$

而从(2-3)可以得到

(3-3)$$\sqrt{2} \left \| t \right \|_2= \left \| t^\wedge \right \|_F=\left \| WS \right \|_F=\sqrt{2} \sigma$$

结合(3-2)与(3-3)有

(3-4)$$\left \| E \right \|_F=\sqrt{2}  \left \| t \right \|_2$$

从(3-4)可以发现，真实的\(E\)的模为\(t\)的\(\sqrt{2}\)倍，在这个情况下，同时缩放不影响\(R\)的旋转性质，因此不妨取\(E\)的模为\(\sqrt{2}\)，\(t\)的模为1，即这时将\(E\)与\(t\)进行缩放，得到

(3-5)$$E'=\sqrt{2}E/ \left \| E \right \|_F$$

(3-6)$$t'=t/ \left \| t \right \|_2$$

则对于公式(1-2)展开有

(3-7)$$\begin{bmatrix}
e_1^T\\ 
e_2^T\\ 
e_3^T
\end{bmatrix}=E'=t'^\wedge R=\begin{bmatrix}
t_1^T\\ 
t_2^T\\ 
t_3^T
\end{bmatrix}R \Leftrightarrow 
\left\{\begin{matrix}
Re_1=t_1\\ 
Re_2=t_2\\ 
Re_3=t_3
\end{matrix}\right. $$

上面公式中的\(e_i\)、\(t_i\)分别表示\(E'\)、\(t'^\wedge \)的第\(i\)行数据。

在(3-7)中需要注意，由于\(E'\)矩阵不可逆(其本身性质决定，它的秩为2)因此并不能直接求得\(R\)，但是因为\(E'\)秩为2，因此在(3-7)中的\(e_i\)中必有两个线性无关向量，这里假设选取的两个线性无关的向量为\(e_1\)、\(e_2\),其中\(e_3\)必定可以使用\(e_1\)、\(e_2\)的线性表示。

对于公式(3-7)中的变量进行如下操作

(3-8)$$\begin{align*}
t_1 \times t_2 &= (Re_1)\times (Re_2) \\ 
 &= (Re_1) ^\wedge (Re_2)\\ 
 &= Re_1^\wedge R^T(Re_2)\\
 &=Re_1^\wedge R^TRe_2\\
&=Re_1^\wedge e_2\\
&=R(e_1 \times e_2)
\end{align*}$$

根据叉乘的定理可知\(e_1 \times e_2\)必定与\(e_1\)、\(e_2\)线性无关，因此可以得到一个如下的可逆矩阵\(A\)

(3-9)$$A=\begin{bmatrix}
e_1 & e_2 & e_1 \times e_2
\end{bmatrix}$$

以及一个矩阵\(B\)

(3-10)$$B=\begin{bmatrix}
t_1 & t_2 & t_1 \times t_2
\end{bmatrix}$$

则由(3-7)到(3-10)可以得到如下方程

(3-8)$$ RA=B$$

最终可以得到旋转矩阵

(3-9)$$ R = B A^{-1}$$

由于求得的\(t\)有一个缩放，如果想恢复成原本\(E\)的大小，则可以根据公式(1-2)与(3-9)得到的\(R\)得到\(t\)

(3-10)$$ t^\wedge = ER^T$$

需要说明，由于当\(E=-E\)时也可以通过上面方式得到一个\(R\)，因此最终的求解方式是：

①当\(E=E\)时，求得一组\(R_1 , t_1\)

②当\(E=-E\)时，求得一组\(R_2 , t_2\)

这时\(t_1=t_2\)或者\(t_1=-t_2\)，这里直接记\(t1=\left | t_1 \right |\)，\(t2=-\left | t_1 \right |\)

则最终可能产生4组可能的解\((t_1,R_1)、(t_1,R_2)、 (t_2,R_1)、(t_2,R_2)\)，和第2点一样，可以使用正深度法选择出正确的那组解。

可以发现，这个方法分解\(R\)、\(t\)只有解方程与求逆，并且操作矩阵仅为3阶，都有比较简单的常规表达式，因此比起奇异值分解得到变换的\(R\)和\(t\)，具有更高效的特点。

 

4 三角测量

当通过以上方法的得到两张图对应的变换\(R\)和\(t\)后，需要恢复出对应点的在空间中的位置时（上面从多组解中筛选出正确的那最解就需要用到），在单目SLAM中，就需要通过三角测量来估计其深度。

配对的两个图像上的特征点，假设归一化后的坐标为\(x_1\)与\(x_2\)，这时根据得到的变换，则它们满足

(4-1)$$s_2x_2=s_1Rx_1+t$$

公式中的的\(s_1\)、\(s_2\)分别为两个特征点对应的深度。由于求解变量只有2个，则直接解一个线性方程组即可求得最佳的\(s_1\)、\(s_2\)

 

参考:

[1] 杨述强. 基于单目视觉的场景三维重建与飞行器位姿求解关键技术研究[D]. 国防科学技术大学, 2014.

[2] 高翔 , 张涛. 视觉SLAM十四讲从理论到实践[M]. 北京:电子工业出版社, 2017.3

[3]李国栋, 田国会, 王洪君,等. 弱标定立体图像对的欧氏极线校正框架[J]. 光学精密工程, 2014, 22(7):1955-1961.