全景拼接学习-原理篇 (3) 从对极几何单相机拍摄的画面估计运动轨迹

https://zhuanlan.zhihu.com/p/141799551

在机器人SLAM、自动驾驶中经常会遇到一个问题：如何通过相机拍摄的一组画面反推出相机在真实世界中的运动轨迹。这就是典型的视觉里程计问题。

一般来说，对于通过单相机拍摄的画面估计运动轨迹，需要用到对极几何知识。所以本文主要分析如何通过两张图对极几何估计相机运动，也是对《视觉SLAM十四讲》中视觉里程计章节中没有说的地方进行补充。

一、基础知识

在讨论对极几何之前，先来看一些向量和矩阵的基础知识。

内积（点乘）

有向量 $\vec{a}= \begin{pmatrix} x_1 \\ y_1\\ z_1 \end{pmatrix}$ 和 $\vec{b}= \begin{pmatrix} x_2 \\ y_2\\ z_2 \end{pmatrix}$ ，他们的內积为：

$\vec{a}\cdot \vec{b}=|\vec{a}||\vec{b}|\cos\theta=(x_1, y_1, z_1)\begin{pmatrix} x_2 \\ y_2\\ z_2 \end{pmatrix}\tag{1.1}$

显然当 $\vec{a}$ 和 $\vec{b}$ 互相垂直时 $\vec{a}\cdot \vec{b}=0$ 。

外积（叉乘）

与内积 $\vec{a}\cdot \vec{b}$ 结果是一个数值不同，外积 $\vec{a}\times\vec{b}$ 的结果是一个向量。

假设 $\vec{a}$ 和 $\vec{b}$ 向量所在平面 $\pi$ ，那么从几何上看 $\vec{a}\times\vec{b}$ 是一个垂直于 $\pi$ 平面的向量。

$\begin{align} \vec{a}\times\vec{b}&= \left |\begin{array}{cccc} \vec{i} & \vec{j} & \vec{k} \\ x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ \end{array}\right| \\&= (y_1z_2-y_2z_1)\vec{i}-(x_1z_2-x_2z_1)\vec{j}+(x_1y_2-x_2y_1)\vec{k} \end{align} \tag{1.2}$

写成坐标形式：

$\vec{a}\times\vec{b}= \Big((y_1z_2-y_2z_1), -(x_1z_2-x_2z_1), (x_1y_2-x_2y_1)\Big)^T\tag{1.3}$

由于 $\vec{a}\times\vec{b}$ 垂直于 $\vec{a}$ 和 $\vec{b}$ ，那么必然有：

$(\vec{a}\times\vec{b})\cdot\vec{a}=(\vec{a}\times\vec{b})\cdot\vec{b}=0\tag{1.4}$

将向量叉乘写成矩阵乘以坐标点形式：

$\vec{a}\times\vec{b}=\begin{bmatrix} 0 & -z_1 & y_1 \\ z_1 & 0 & -x_1 \\ -y_1 & x_1 & 0\end{bmatrix}\begin{pmatrix} x_2 \\ y_2 \\ z_2 \end{pmatrix}=[\vec{a}]_\times\vec{b}\tag{1.5}$

已知 $\vec{a}= \begin{pmatrix} x_1 \\ y_1\\ z_1 \end{pmatrix}$ ，定义叉乘矩阵 $[\vec{a}]_\times$ 为：

$[\vec{a}]_\times=\begin{bmatrix} 0 & -z_1 & y_1 \\ z_1 & 0 & -x_1 \\ -y_1 & x_1 & 0\end{bmatrix}\tag{1.6}$

这个矩阵之后会用到。

反对称矩阵

若 $A$ 满足 $-A=A^T$ 关系，则 $A$ 为反对称矩阵（其中 $A^T$ 代表矩阵转置）。

显然上面的叉乘矩阵 $[\vec{a}]_\times$ 是反对称矩阵。

此处不经证明给出，对于任意3x3实反对称矩阵 $A$ 都可以表示为：

$A=UZU^T=U\begin{bmatrix} 0 & 1 & 0 \\ -1 & 0 & 0 \\ 0 & 0 & 0\end{bmatrix}U^T\tag{1.7}$

其中 $U$ 是3x3正交矩阵。这个结论很重要，一会要用到。

二、对极几何（本质矩阵和基础矩阵）

在之前相机模型文章中有分析到，从相机坐标系 $xyz$ 变换为像素坐标系 $uv$ 的公式为：

$z\begin{pmatrix} u \\ v \\ 1 \end{pmatrix}=\begin{bmatrix} f_x&0&u_0\\ 0 & f_y & v_0 \\ 0 & 0 & 1 \end{bmatrix}\begin{pmatrix} x \\ y \\ z \end{pmatrix}=K\begin{pmatrix} x \\ y \\ z \ \end{pmatrix}\tag{2.1}$

其中相机内参矩阵 $K$ 是已知的（预先标定好）。

世界中有一点 $P(x,y,z)$ ，相机 $o$ 拍摄 $P$ 并在成像平面行形成像素点 $p(u,v)$ 。由于相机在成像过程中丢失了深度信息，所以我们无法知道 $P$ 的深度（即 $z$ ）。

那么问题来了，当相机 $o$ 移动到另一个外置 $o'$ 再拍一张图时如何？

假设相机 $o$ 拍摄 $P$ 在成像平面形成对应的像素点 $p$ ，此时我们只能知道 $P$ 在 $\vec{op}$ 射线上（但是无法确定具体位置）；当相机移动到 $o'_{ }$ 后再去拍摄 $P$ 生成新的像素点 $p’$ ，如果图像通过特征点匹配确定 $p$ 和 $p'$ 是同一点，那么这时 $P$ 的位置也就确定了。

本质矩阵

需要特别说明，这里的 $\vec{oP}=\begin{pmatrix} x \\ y \\ z \end{pmatrix}$ 是坐标系 $o$ 下的向量；而 $\vec{o'P}=\begin{pmatrix} x' \\ y' \\ z' \end{pmatrix}$ 是坐标系 $o'$ 下的向量。

现在我们要将所有的向量和坐标统一在同一个坐标系内，那么在 $o'$ 坐标系下 $\vec{oP}$ 为：

$\vec{oP}=R\begin{pmatrix} x \\ y \\ z \end{pmatrix} +t\tag{2.2}$

其中 $R$ 是3x3旋转矩阵， $t$ 是3x1平移向量。

由于 $\vec{oP}=R\begin{pmatrix} x \\ y \\ z \end{pmatrix} +t$ 、 $\vec{o'P}=\begin{pmatrix} x' \\ y' \\ z' \end{pmatrix}$ 和 $\vec{o'o}=-t$ 共面，所以他们的混合积为0：

参考前面点积

因为三线共面，o'o和oP的点积垂直平面，故和平面内的o'p在相乘为0

$\vec{o'P}\cdot(\vec{o'o}\times \vec{oP})=0\tag{2.3}$

那么在 $o'$ 坐标系下，可以表示为：

$\begin{pmatrix} x' \\ y'\\ z'\end{pmatrix}^T \Big(-[t]_{\times}(R\begin{pmatrix} x \\ y\\ z\end{pmatrix}+t)\Big)=0\tag{2.4}$

由于 $[t]_\times \cdot t=\vec{t}\times \vec{t}=0$ （只是一条线，没有面）

$\begin{pmatrix} x' \\ y'\\ z'\end{pmatrix}^T ([t]_{\times}R)\begin{pmatrix} x \\ y\\ z\end{pmatrix}=0\tag{2.5}$

这里的 $E=[t]_\times R$ 就是对极几何中的本质矩阵(essential matrix)。

特别注意 $E$ 是由反对称矩阵 $[t]_\times$ 和正交矩阵 $R$ 相乘获得，这是一个非常重要的性质！

基础矩阵

进一步，又由相机模型可以得到到像素坐标的转换关系：

$\begin{pmatrix} x \\ y\\ z\end{pmatrix}=zK^{-1}\begin{pmatrix} u \\ v\\ 1 \end{pmatrix}\tag{2.6}$

$\begin{pmatrix} x' \\ y'\\ z'\end{pmatrix}=z'K^{-1}\begin{pmatrix} u' \\ v'\\ 1 \end{pmatrix}\tag{2.7}$

带入后：

$\Big[z'K^{-1}\begin{pmatrix} u’ \\ v'\\ 1 \end{pmatrix}^T\Big]E\Big[zK^{-1}\begin{pmatrix} u \\ v\\ 1 \end{pmatrix}\Big]=0\tag{2.8}$

等式右边为0，直接消去常数 $z$ 和 $z'$ ：

$\Big[K^{-1}\begin{pmatrix} u' \\ v'\\ 1 \end{pmatrix}^T\Big]E\Big[K^{-1}\begin{pmatrix} u \\ v\\ 1 \end{pmatrix}\Big]=0\tag{2.9}$

整理得：

$\begin{pmatrix} u' \\ v'\\ 1 \end{pmatrix}^T\Big[(K^{-1})^TEK^{-1}\Big]\begin{pmatrix} u \\ v\\ 1 \end{pmatrix}=0\tag{2.10}$

这里的 $F=(K^{-1})^TEK^{-1}$ 就是基础矩阵(fundamental matrix)，基础矩阵实际与本质矩阵只是相差了相机内参 $K$ 。

三、本质矩阵求解相机运动

在实际中我们可以通过特征点匹配（如ORBSLAM2使用ORB特征）计算出相机运动前后两幅视图中较多组匹配特征点坐标 $p(u,v)$ 与 $p'(u',v')$ ，另外相机内参矩阵 $K$ 已知。

通过对极几何恢复相机运动的过程为：特征点匹配 → 计算本质矩阵 → 恢复相机运动。

计算本质矩阵 $E$

以OpenCV cv::findEssentialMat函数计算 $E$ 为例，具体代码如下：

// _points1对应p，_points2对应p', _cameraMatrix是相机内参矩阵K
cv::Mat cv::findEssentialMat( InputArray _points1, InputArray _points2, InputArray _cameraMatrix,
                              int method, double prob, double threshold, OutputArray _mask)
{
    CV_INSTRUMENT_REGION();

    // 将匹配的特征点和相机矩阵等数据转化为浮点型
    Mat points1, points2, cameraMatrix;
    _points1.getMat().convertTo(points1, CV_64F);
    _points2.getMat().convertTo(points2, CV_64F);
    _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F);

    int npoints = points1.checkVector(2);
    CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints &&
                              points1.type() == points2.type());

    CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1);

    if (points1.channels() > 1)
    {
        points1 = points1.reshape(1, npoints);
        points2 = points2.reshape(1, npoints);
    }

    double fx = cameraMatrix.at<double>(0,0);
    double fy = cameraMatrix.at<double>(1,1);
    double cx = cameraMatrix.at<double>(0,2);
    double cy = cameraMatrix.at<double>(1,2);

    // 按照公式（19）和公式（20）计算 E
    points1.col(0) = (points1.col(0) - cx) / fx;
    points2.col(0) = (points2.col(0) - cx) / fx;
    points1.col(1) = (points1.col(1) - cy) / fy;
    points2.col(1) = (points2.col(1) - cy) / fy;

    // Reshape data to fit opencv ransac function
    points1 = points1.reshape(2, npoints);
    points2 = points2.reshape(2, npoints);

    threshold /= (fx+fy)/2;

    Mat E;
    if( method == RANSAC )
        createRANSACPointSetRegistrator(makePtr<EMEstimatorCallback>(), 5, threshold, prob)->run(points1, points2, E, _mask);
    else
        createLMeDSPointSetRegistrator(makePtr<EMEstimatorCallback>(), 5, prob)->run(points1, points2, E, _mask);

    return E;
}

以本质矩阵 $E$ 的定义公式 $(2.5)$ 来看，两边乘以 $\frac{1}{zz'}$ ，得：

$\Big[\frac{1}{z'}\begin{pmatrix} x' \\ y'\\ z'\end{pmatrix}^T \Big]E\Big[\frac{1}{z}\begin{pmatrix} x \\ y\\ z\end{pmatrix}\Big]=0\tag{3.1}$

化简得

$\begin{pmatrix} x'/z' \\ y'/z'\\ 1\end{pmatrix}^T E\begin{pmatrix} x/z \\ y/z\\ 1\end{pmatrix}=0\tag{3.2}$

再回头来看公式 $(2.1)$ ：

$\left\{ \begin{aligned} z\cdot u &= f_xx+z\cdot v_0 \\ z\cdot v &= f_xx+z\cdot v_0 \end{aligned} \right.\Rightarrow\left\{ \begin{aligned} \frac{x}{z} &= \frac{u-u_0}{f_x} \\ \frac{y}{z} &= \frac{v-v_0}{f_y}\end{aligned} \right. \tag{3.3}$

其中 $c_x=u_0$ 且 $c_y=v_0$ ，带表相机主点位置。这时明显可以看到findEssentialMat代码通过上述过程计算本质矩阵 $E$ 。

奇异值分解 $E$ 计算相机运动

$E=[t]_\times R=UZU^TR\tag{3.4}$

对 $Z$ 进行初等行变换：

$[Z|I]= \begin{bmatrix} 0 & 1 & 0 & | & 1 &0 & 0\\ -1&0&0&|&0&1&0\\0&0&0&|&0&0&1\end{bmatrix}\Rightarrow \begin{bmatrix} 1 & 0 & 0 & | & 0 & -1 & 0\\ 0&1&0&|&1&0&0\\0&0&0&|&0&0&1\end{bmatrix}=[\text{diag}(1,1,0)|W]\tag{22}$

这里的 $W$ 是一个正交矩阵（ $WW^T=W^TW=I$ ）。通过上述初等行变换可以得到：

$ZW=\text{diag}(1,1,0)\Rightarrow Z=\text{diag}(1,1,0)W^T \tag{3.5}$ $ZW^T=-\text{diag}(1,1,0)\Rightarrow Z=-\text{diag}(1,1,0)W \tag{3.6}$

那么通过公式 $(3.5)$ 改写 $E$ 为：

$\begin{align} E=UZU^TR&=U\Big[\text{diag}(1,1,0)W^{-1}\Big]U^TR\\ &=U\text{diag}(1,1, 0)W^TU^TR \\ &=U\text{diag}(1,1, 0)V_1^T \end{align}\tag{3.7}$

同理通过公式 $(3.6)$ 改写 $E$ 为：

$\begin{align} E=UZU^TR&=U\text{diag}(1,1, 0)(-WU^TR) \\ &=U\text{diag}(1,1, 0)V_2^T \end{align}\tag{3.8}$

观察公式 $(3.7)$ 和 $(3.8)$ ，这就是典型的奇异值分解（SVD）：

$E=U\Sigma V^T=U\begin{bmatrix} \sigma_1 & 0 & 0 \\ 0 & \sigma_2 & 0 \\ 0 & 0 & \sigma_3\end{bmatrix}V^T\tag{3.9}$

其中 $U$ 和 $V$ 都是正交矩阵， $\sigma_1=\sigma_2\ne0$ 且 $\sigma_3=0$ 。同时可以得到结论：

一个矩阵是本质矩阵的充要条件是其奇异值中有两个相等且第三个是0。

那么看到这儿应该比较清晰了，通过两幅图对极几何计算相机运动的方法是：

特征点匹配计算 $p_i$ 和 $p_i'$
通过对极约束 $(p_i)^TEp_i=0$ 计算本质量矩阵 $E$
用奇异值分解 $E=U\Sigma V^T$ 计算 $R$ 和 $[t]_\times$ ，从而得到旋转矩阵 $R$ 和平移向量 $t$

相机运动 $R$ 和 $t$ 的四个解

已知 $E$ 并进行奇异值分解求得 $U$ 和 $V$ 之后，那么根据公式 $(25)$ 和 $(26)$ 可以得到旋转矩阵的两个解：

$R_1=UWV^T,R_2=UW^TV^T\tag{3.10}$

接着可以得到：

$[t]_\times=ER^T\tag{3.11}$

对于对极约束 $(p’)^TEp=0$ ，两边乘以 $-1$ 等式依然成立；对本质矩阵的数值解来说， $E$ 和 $-E$ 只是相差一个负号；但是对于平移向量， $t$ 和 $(-t)$ 代表方向相反。

$[-t]_\times=-ER^T\tag{3.12}$

综合考虑，求解得到的 $R$ 和 $t$ 共有4种如下可能位置：

由于真实物理条件限定，两个相机和被拍摄点在同一侧，且被拍摄点在相机前方，所以只有上图中(a)是符合真实的解。

在OpenCV中cv::recoverPose函数帮我们做这一堆事情：

int cv::recoverPose( InputArray E, InputArray _points1, InputArray _points2,
                            InputArray _cameraMatrix, OutputArray _R, OutputArray _t, double distanceThresh,
                     InputOutputArray _mask, OutputArray triangulatedPoints)

输入本质矩阵、匹配特征点 _points1 + _points2、相机矩阵 _cameraMatrix，输出旋转矩阵 _R和平移向量 _t 。代码太长就不列出来了。

四、若干问题

对极几何不能求解纯旋转问题

$\begin{pmatrix} x' \\ y'\\ z'\end{pmatrix}^T ([t]_{\times}R)\begin{pmatrix} x \\ y\\ z\end{pmatrix}=0\tag{4.1}$

当相机无平移且只有旋转时 $[t]_\times$ 为全 $0$ 矩阵，等式两边都等于 $0$ 显然会导致无法求解 $R$ 。反过来当相机只有平移且无旋转时 $R=I$ ，并不影响 $[t]_\times$ 的求解，这时问题退化成双目测距。

对极几何“结构性”恢复运动

对于对极约束 $(4.1)$ ，在等式两边乘以任意不为 $0$ 的常数 $k$ 都成立：

$\begin{pmatrix} x' \\ y'\\ z'\end{pmatrix}^T (k[t]_{\times}]R)\begin{pmatrix} x \\ y\\ z\end{pmatrix}=0\tag{4.2}$

换句话说，如果 $\vec{t}=(t_x,t_y,t_z)^T$ 是 $(4.1)$ 的解，那么 $\vec{t}=(kt_x,kt_y,kt_z)^T$ 也都是 $(4.1)$ 的解。这就是说，即使求解出平移向量 $\vec t$ ，我们也无法知道 $\vec t$ 的单位具体是米、厘米还是毫米。所以计算出出的 $\vec t$ 只是恢复移动的“结构性”，并不是真实值。