视觉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 O_1,O_2 O1​,O2​是相机中心， P P P是匹配点在空间中的三维坐标，蓝色平面是成像面（将成像平面从相机中心后方放置到前方）， p 1 , p 2 p_1,p_2 p1​,p2​是 P P P在成像平面的像素坐标。

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

O 1 , O 2 , P O_1,O_2,P O1​,O2​,P都在同一平面上，假设 P P P点在 O 1 O_1 O1​的相机坐标为 P 1 P_1 P1​，在 O 2 O_2 O2​的相机坐标为 P 2 P_2 P2​，相机从 O 1 O_1 O1​运动到 O 2 O_2 O2​，相对位姿变换记为 R , t R,t R,t，则有： ( O 1 O 2 → × O 2 P → ) ⋅ O 2 P → = 0 → ( O 1 O 2 → × O 2 P 2 → ) ⋅ O 2 P 2 → = 0 O 1 O 2 → = t O 1 P 1 → = P 1 = s 1 K − 1 p 1 O 2 P 2 → = P 2 = s 2 K − 1 p 2 = R P 1 + t t × ( R P 1 + t ) ⋅ P 2 = 0 → P 2 T t ∧ R P 1 = 0 P 2 T E P 1 = 0 , E = t ∧ R p 2 T F p 1 = 0 , F = K − T t ∧ R K − 1 (\overrightarrow {O_1O_2}\times \overrightarrow {O_2P})\cdot \overrightarrow {O_2P}=0 \to (\overrightarrow {O_1O_2}\times \overrightarrow {O_2P_2})\cdot \overrightarrow {O_2P_2}=0 \\ \quad \\ \overrightarrow {O_1O_2} = t \\ \overrightarrow {O_1P_1} = P_1=s_1K^{-1}p_1 \\ \overrightarrow {O_2P_2} = P_2 = s_2K^{-1}p_2=RP_1+t \\ \quad \\ t \times (RP_1+t) \cdot P_2 =0 \to P_2^Tt^\land RP_1=0 \\ \quad \\ P_2^TEP_1=0, E=t^\land R \\ p_2^TFp_1=0, F=K^{-T}t^\land RK^{-1} (O1​O2​ ​×O2​P ​)⋅O2​P ​=0→(O1​O2​ ​×O2​P2​ ​)⋅O2​P2​ ​=0O1​O2​ ​=tO1​P1​ ​=P1​=s1​K−1p1​O2​P2​ ​=P2​=s2​K−1p2​=RP1​+tt×(RP1​+t)⋅P2​=0→P2T​t∧RP1​=0P2T​EP1​=0,E=t∧Rp2T​Fp1​=0,F=K−Tt∧RK−1 其中， s 1 , s 2 s_1,s_2 s1​,s2​在计算等式的过程中丢失，因此基于单目的位姿估计缺乏真实的尺度。矩阵 E E E称为本质矩阵， F F F称为基础矩阵。本质矩阵给出了下相机坐标系下，匹配点坐标与相对位姿的关系，基础矩阵给出了像素坐标系下，匹配点坐标与相对位姿的关系。

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

后记

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