视觉SLAM十四讲学习5 位姿估计（1）ORB特征，对极几何与本质矩阵

前言

《十四讲》基础篇还剩下非线性优化，不过我在数值计算中已经把牛顿法、LM法这些记录过了，因此先跳过非线性优化，从位姿估计开始。

ORB特征

通过图像还原相机相邻位姿之间的变化，首先需要确定相邻图像上哪一对点是空间中的同一点，通常采用特征点匹配的方法，比如FAST、SIFT、SURF、ORB等。

特征点匹配包括特征点提取，描述子计算，特征匹配三个步骤。

目前，ORB(Oriented FAST and Rotated BRIEF)由于其提取速度快，匹配准确性较好的原因，是最常用的特征提取方法。ORB实际上是具有方向的FAST角点+旋转BRIEF描述子，具有一定的旋转不变性和尺度不变性（原始ORB并没有尺度不变性，OpenCV实现中加入了特征金字塔弥补了这个缺点）。大致效果如下图。

具体的ORB算法实现流程这里就不深入了。除了传统特征点法，目前也有使用深度特征点匹配方法做位姿估计的论文。

对极几何

通过ORB在相邻帧匹配到一对点后，接下来就是通过匹配点的图像坐标恢复相机位姿，即相对位姿估计，可以通过对极几何实现，如下图。

图中，$O_1,O_2$是相机中心，$P$是匹配点在空间中的三维坐标，蓝色平面是成像面（将成像平面从相机中心后方放置到前方），$p_1,p_2$是$P$在成像平面的像素坐标。

根据图中的几何关系进行推理。

$O_1,O_2,P$都在同一平面上，假设$P$点在$O_1$的相机坐标为$P_1$，在$O_2$的相机坐标为$P_2$，相机从$O_1$运动到$O_2$，相对位姿变换记为$R,t$，则有：
$$\begin{gathered} (\overrightarrow{O_1{O}_2}\times \overrightarrow{O_{2}P})\cdot \overrightarrow{O_{2}P}=0\to (\overrightarrow{O_1{O}_2}\times \overrightarrow{O_2{P}_2})\cdot \overrightarrow{O_2{P}_2}=0 \\ \overrightarrow{O_1{O}_2}=t \\ \overrightarrow{O_1{P}_1}=P_1=s_1{K}^{-1}{p}_1 \\ \overrightarrow{O_2{P}_2}=P_2=s_2{K}^{-1}{p}_2=R{P}_1+t \\ t\times (R{P}_1+t)\cdot P_2=0\to P_2^T{t}^{\wedge }R{P}_1=0 \\ {P}_2^{T}E{P}_1=0,E=t^{\wedge }R \\ {p}_2^{T}F{p}_1=0,F=K^{-T}{t}^{\wedge }R{K}^{-1} \end{gathered}$$
其中，$s_1,s_2$在计算等式的过程中丢失，因此基于单目的位姿估计缺乏真实的尺度。矩阵$E$称为本质矩阵，$F$称为基础矩阵。本质矩阵给出了下相机坐标系下，匹配点坐标与相对位姿的关系，基础矩阵给出了像素坐标系下，匹配点坐标与相对位姿的关系。

因此，求解位姿分为两步：①根据匹配点坐标求本质矩阵；②根据本质矩阵求位姿$R,t$.

后记

下篇记录本质矩阵与位姿的计算过程，以及单应矩阵的概念与计算。