SLAM--求解2D-2D图像间的运动

一、数学基础

1.1 反对称矩阵的性质

我们令$A=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$，该矩阵是一个反对称矩阵，即$A^T=-A$;

利用$\det (\lambda E-A)=0$来反对称矩阵A的特征值，化简得：
$$\lambda ({\lambda }^2+m)=0,m=a_1^2+a_2^2+a_3^2\text{\hspace{1em}(1)}$$
从特征多项式我们可以看出，这个$3\times 3$的矩阵的秩为2，有一个特征值为0，另外两个特征值相等且为虚数。

---

下面我们对反对称矩阵进行分解：

首先我们对于对称矩阵来说，如果矩阵$A$是一个对称矩阵（在复数域上被称为Hermite矩阵），即$A^T=A$，则存在一个正交矩阵$U(U{U}^T=U^{T}U=I)$，使得$A=UD{U}^T$，$D$为对角矩阵，另外$A$不同特征值对应的特征向量正交；

返回到反对称矩阵上，若$S$为反对称矩阵（在复数域上被称为反Hermite矩阵），存在正交矩阵$U$，使得:
$$S=UB{U}^T,B=\kappa \cdot diag(z_1,z_2,...,z_m,0,0,...,0)\text{\hspace{1em}(2)}$$
其中$\kappa$为常数对角矩阵$\kappa =diag({\kappa }_1,{\kappa }_2,...,{\kappa }_m,0,0,...,0)$，$z_i=\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right],(i=1,2,...,m)$。

我们主要分析$3\times 3$的矩阵，令$S$为$3\times 3$的反对称矩阵，另外令：
$$W=\left[\begin{array}{ccc}0& -1& 0\\ & & \\ 1& 0& 0\\ & & \\ 0& 0& 1\end{array}\right],Z=\left[\begin{array}{ccc}0& 1& 0\\ & & \\ -1& 0& 0\\ & & \\ 0& 0& 0\end{array}\right]\text{\hspace{1em}(3)}$$
根据上述性质，能够推出$S$和$Z$合同，即：
$$S=\kappa \cdot UZ{U}^T\text{\hspace{1em}(4)}$$
为了在奇异值分解中构造奇异值 1,1,0，所以构造了正交矩阵$W$，满足$Z=-diag(1,1,0)W=diag(1,1,0)W^T$,所以有：
$$\begin{array}{r}S=-\kappa \cdot U\cdot diag(1,1,0)W\cdot U^T\\ \\ 或者S=\kappa \cdot U\cdot diag(1,1,0)W^T\cdot U^T\end{array}\text{\hspace{1em}(5)}$$
参考文献: Multiple View Geometry in Computer Vision Second Edition. P258-259

接下来我们介绍奇异值分解；
 
 

1.2 奇异值分解

奇异值分解简单介绍，具体参考：奇异值分解（SVD）----作者：漫漫成长
 
对于一个矩阵$A_{m\times n}$，$rank(A)=r(r⩽n,m)$，则$A^{T}A$矩阵有$r$个非零特征值$({\lambda }_1,{\lambda }_2,...,{\lambda }_r)$，存在两个正交矩阵$U,V$，使得$A$可以被分解为：
$$A=U{S}_0{V}^T,S=diag({\lambda }_1,{\lambda }_2,...,{\lambda }_r,0,...,0{)}_{n\times n}\text{\hspace{1em}(6)}$$
$S$中的元素被称为$A$的奇异值，其中$V$为$A^{T}A$特征值对应的单位特征向量，$U$为$A{A}^T$特征值对应的单位特征向量。

由3阶反对称矩阵的特征值特点可以知道其奇异值一定是$diag(\sigma ,\sigma ,0)$，因为其有特征值0，以及两个相等的特征值。

 
 

二、本质矩阵的推导

如图所示，我们需要通过不同的2D图像，利用相同的路标点（特征点），来推算相机之间的运动R和t，下面我们根据对极几何的特性进行推导。

我们假设存在着一个路标点$P_w$，对应两个不同相机位姿的像素坐标为$p_1、p_2$，分别对应着图像$I_1和I_2$， 另外我们假设图像$I_1$对应的相机坐标与世界坐标重合(即$T_{1,w}={\mathbf{E}}_{4\times 4}$)。

由相机模型可得：
$$\begin{array}{rl}{s}_1{\mathbf{p}}_1& =\mathbf{K}{\mathbf{T}}_{1,\mathbf{w}}{\mathbf{P}}_{\mathbf{w}}=\mathbf{K}{\mathbf{P}}_{\mathbf{w}}\\ & \\ s_2{\mathbf{p}}_2& =\mathbf{K}{\mathbf{T}}_{2,\mathbf{w}}{\mathbf{P}}_{\mathbf{w}}=\mathbf{K}(\mathbf{R}{\mathbf{P}}_{\mathbf{w}}+\mathbf{t})\end{array}\text{\hspace{1em}(7)}$$
实际上，$T_{2,w}$也就是$T_{2,1}$，即相机1到相机2的变换矩阵。

则有：
$$\begin{gathered} {\mathbf{K}}^{-1}{\mathbf{p}}_1=\frac{1}{s_1}{\mathbf{P}}_w \\ {\mathbf{K}}^{-1}{\mathbf{p}}_2=\frac{1}{s_2}(\mathbf{R}{\mathbf{P}}_{\mathbf{w}}+\mathbf{t})\text{\hspace{1em}(8)} \end{gathered}$$

我们令:
$${\mathbf{x}}_1={\mathbf{K}}^{-1}{\mathbf{p}}_1,{\mathbf{x}}_2={\mathbf{K}}^{-1}{\mathbf{p}}_2\text{\hspace{1em}(9)}$$
由公式可知，$x_1$和$x_2$是点${\mathbf{P}}_w$在两个相机坐标系下的归一化坐标点（深度坐标化为1，只需给出相机内参$K$和像素坐标即可得到）

由式8可以得到：

$$s_2{\mathbf{K}}^{-1}{\mathbf{p}}_2=s_1\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1+\mathbf{t}\text{\hspace{1em}(10)}$$
两边同时左乘$t^{\wedge }$可得：
$$t^{\wedge }{s}_2({\mathbf{K}}^{-1}{\mathbf{p}}_2)=t^{\wedge }{s}_1\mathbf{R}({\mathbf{K}}^{-1}{\mathbf{p}}_1)+0\text{\hspace{1em}(11)}$$
在左乘一个${({\mathbf{K}}^{-1}{\mathbf{p}}_2)}^T$，由叉乘的性质可得，向量$({\mathbf{K}}^{-1}{\mathbf{p}}_2)$与$t^{\wedge }({\mathbf{K}}^{-1}{\mathbf{p}}_2)$正交（垂直），同时深度信息也消失了（失去了一个自由度）：
$$0={({\mathbf{K}}^{-1}{\mathbf{p}}_2)}^T{t}^{\wedge }\sout{{\text{s}}_1}\mathbf{R}({\mathbf{K}}^{-1}{\mathbf{p}}_1)\text{\hspace{1em}(12)}$$
由式12和式9可得：

$${{\mathbf{x}}_2}^T{t}^{\wedge }\mathbf{R}{\mathbf{x}}_1=0$$
其中，$\mathbf{E}=t^{\wedge }\mathbf{R}$被称为本质矩阵，所以在本质矩阵中，包含了旋转和平移的信息。

接下来就是求解本质矩阵。
 
 

三、求解本质矩阵Rt

我们可以通过8个匹配点来求取本质矩阵$\mathbf{E}$，这里继续介绍如何分解本质矩阵。

对本质矩阵进行SVD分解，首先对反对称矩阵$t^{\wedge }$分解：

$$\begin{gathered} t^{\wedge }=\kappa \cdot \mathbf{U}\mathbf{Z}{\mathbf{U}}^T=\kappa \cdot \mathbf{U}\cdot diag(1,1,0){\mathbf{W}}^T\cdot {\mathbf{U}}^T \\ 或者t^{\wedge }=-\kappa \cdot \mathbf{U}\cdot diag(1,1,0)\mathbf{W}\cdot {\mathbf{U}}^T \\ 其中，\mathbf{W}=\left[\begin{array}{ccc}0& -1& 0\\ & & \\ 1& 0& 0\\ & & \\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(13)} \end{gathered}$$
所以本质矩阵有两种表示法：
$$\begin{gathered} \mathbf{E}=t^{\wedge }\mathbf{R}=\kappa \cdot \mathbf{U}\cdot diag(1,1,0){\mathbf{W}}^T\cdot {\mathbf{U}}^T\mathbf{R} \\ \mathbf{E}=t^{\wedge }\mathbf{R}=-\kappa \cdot \mathbf{U}\cdot diag(1,1,0)\mathbf{W}\cdot {\mathbf{U}}^T\mathbf{R} \end{gathered}$$
对本质矩阵奇异值分解：
$$\begin{gathered} \mathbf{E}=\mathbf{U}{\mathbf{S}}_0{\mathbf{V}}^T \\ 则{\mathbf{V}}^T=\mathbf{W}\cdot {\mathbf{U}}^T\mathbf{R}或{\mathbf{W}}^T\cdot {\mathbf{U}}^T\mathbf{R} \end{gathered}$$；

由于$t^{\wedge }$特殊的特征值结构，所以可以推出本质矩阵奇异值一定是$diag(\sigma ,\sigma ,0)$的形式，所以$\mathbf{\kappa }$向量中的各元素相等，式13可以写成：

$$\mathbf{E}=\mathbf{U}\cdot diag(\kappa ,\kappa ,0)\cdot (\mathbf{W}\cdot {\mathbf{U}}^T\mathbf{R})\text{\hspace{1em}(14)}$$
分解得到$\mathbf{U}、\mathbf{V}$后，我们可以求出$R$和$t$：

$$\begin{gathered} \mathbf{R}=\mathbf{U}\mathbf{W}{\mathbf{V}}^T或\mathbf{R}=\mathbf{U}{\mathbf{W}}^T{\mathbf{V}}^T \\ {\mathbf{t}}^{\wedge }=\kappa \mathbf{U}\cdot \mathbf{Z}\cdot {\mathbf{U}}^T或{\mathbf{t}}^{\wedge }=-\kappa \mathbf{U}\cdot \mathbf{Z}\cdot {\mathbf{U}}^T \\ 其中，\mathbf{Z}=\left[\begin{array}{ccc}0& 1& 0\\ & & \\ -1& 0& 0\\ & & \\ 0& 0& 0\end{array}\right],\mathbf{W}=\left[\begin{array}{ccc}0& -1& 0\\ & & \\ 1& 0& 0\\ & & \\ 0& 0& 1\end{array}\right] \end{gathered}$$
又由于其尺度的等价性，可以忽略掉常数$\kappa$，所以我们通常又可以把平移矩阵记为：
$${\mathbf{t}}^{\wedge }=\pm \sout{\kappa }\mathbf{U}\cdot \mathbf{Z}\cdot {\mathbf{U}}^T$$

如图所示，我们解出的值有四种组合，这里就有了四种情况，但只有一种$R$和$t$使得相机的深度为正值；

我们可以利用计算得到的值$\mathbf{R}$和$\mathbf{t}$，代入到$\mathbf{K}(\mathbf{R}{\mathbf{P}}_{\mathbf{w}}+\mathbf{t})$中进行验证，得到最优的解;

---

另外我们也可以利用OpenCV自带的函数进行求解，利用findEssentialMat函数进行求解：

Mat essential_matrix;
essential_matrix = findEssentialMat ( points1, points2, focal_length, principal_point, RANSAC );
recoverPose ( essential_matrix, points1, points2, R, t, focal_length, principal_point );

也可以通过基础矩阵求解：

Mat fundamental_matrix;
fundamental_matrix = findFundamentalMat ( points1, points2, CV_FM_8POINT );

至此，我们就可以估计相机的运动了.

 
 

四、三角测量

由式10可以得到：
$$s_2{\mathbf{K}}^{-1}{\mathbf{p}}_2=s_1\mathbf{R}{\mathbf{K}}^{-1}{\mathbf{p}}_1+\mathbf{t}\text{\hspace{1em}(15)}$$
即
$$s_2{\mathbf{x}}_2=s_1\mathbf{R}{\mathbf{x}}_1+\mathbf{t}\text{\hspace{1em}(16)}$$
我们只要知道了s1，就能求出s2，一般来说，由于噪声的存在，我们用的是最小二乘解，而不是零解。我们可以利用cv::triangulatePoints函数来求解方程。

参考代码：

void triangulation ( 
    const vector< KeyPoint >& keypoint_1, 
    const vector< KeyPoint >& keypoint_2, 
    const std::vector< DMatch >& matches,
    const Mat& R, const Mat& t, 
    vector< Point3d >& points )
{
    Mat T1 = (Mat_<float> (3,4) <<
        1,0,0,0,
        0,1,0,0,
        0,0,1,0);
    Mat T2 = (Mat_<float> (3,4) <<
        R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
        R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
        R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0)
    );
    
    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
    vector<Point2f> pts_1, pts_2;
    for ( DMatch m:matches )
    {
        // 将像素坐标转换至相机坐标
        pts_1.push_back ( pixel2cam( keypoint_1[m.queryIdx].pt, K) );
        pts_2.push_back ( pixel2cam( keypoint_2[m.trainIdx].pt, K) );
    }
    
    Mat pts_4d;
    cv::triangulatePoints( T1, T2, pts_1, pts_2, pts_4d );
    
    // 转换成非齐次坐标
    for ( int i=0; i<pts_4d.cols; i++ )
    {
        Mat x = pts_4d.col(i);
        x /= x.at<float>(3,0); // 归一化
        Point3d p (
            x.at<float>(0,0), 
            x.at<float>(1,0), 
            x.at<float>(2,0) 
        );
        points.push_back( p );
    }
}

参考文献

1.Multiple View Geometry in Computer Vision Second Edition. P258-259.
2.奇异值分解（SVD）
3.视觉SLAM十四讲—第7讲：视觉里程计
4.百度百科：实反对称矩阵