SLAM总结（四）-视觉前端

前端的主要功能一般是求解当前帧的位姿。前端需要保持与传感器帧率相同的频率，以达到实时性。相机和激光雷达都是观测外部环境的传感器，每来一帧，需要将帧与帧之间的点关联起来。数据关联之后进行姿态解算，求得当前帧的位姿。前端一般用于跟踪，只负责求解当前帧的位姿，不会生成新的路标点（路标点指具有在世界坐标系下3D坐标的特征点）和优化原有路标点。这样可以减少前端的计算量，从而达到实时性。

1.数据关联

1.特征法

特征包括点、线、面和物体等特征，常见的是点特征。特征法包括特征提取和特征匹配两步。特征法要求在旋转角度、尺度和实现角度等发生变化时也能正确和尽可能多地提取与匹配出各帧中共视的特征。
人工设计特征需具备以下几个特征：
1）可重复性：可以在不同图片中找出来
2）可区别性：不同特征之间差异较大
3）高效率：图片中特征点数量应远小于图像像素
4）局部性：特征只与一小片区域有关

1、点特征
点特征用其2D或3D坐标系表示即可。克服旋转不变性的方法是寻找点周围匹配区域的方向，克服尺度不变性的典型方法是图像金字塔。
a）特征提取
提取图像中的特征点有以下三种比较具有代表性的方法。
b）特征匹配
根据特征点周围小区域的像素构建的描述子进行匹配。
下面介绍几种典型的点特征：
1）SIFT：充分考虑光照、尺度和旋转等变化，但计算量大，普通PC无法实时运行。花费时间约5228.7ms。使用GPU加速可以满足实时性要求。
2) SURF：花费时间约217.3ms
3）ORB：改进FAST关键点，使其具有方向性，使用256位二进制BRIEF描述子。大大加速了特征提取与匹配，花费时间与15.3ms，使SLAM的实时运行成为可能，且利用特征点的方向和图像金字塔获得旋转和尺度不变性，在ORB-SLAM2、VINS-Mono都使用了ORB特征。特征提取步骤如下：
[1]遍历每个像素点p，设其亮度为I_p
[2]以点p为圆心，半径为3选取16个像素点
[3]若16个像素点中有连续N个点的亮度大于I_p+T或小于I_p-T（T=0.2I_p），N一般去9、11和12，则认为该点为特征点。加速匹配方法是先在16个点的1、5、9、13判断是否有3个点同时满足条件，不满足则说明不是特征点，满足则计算满足条件几个点之间的其他像素。

[4]若像素满足条件，计算特征点方向。首先使用下面公式计算特征点周围一小片区域B的质心，质心C为（m₁₀/m₀₀，m₀₁/m₀₀），注意这里xy都化成以p为坐标原点O，坐标系与图像坐标系平行；

以OC作为特征点方向，角度为θ=arctan(m₀₁/m₁₀)

一般会在图像上均匀的选取特征点，这样可以减少计算量和误匹配，提升算法性能
特征匹配
[1]计算所有特征点的描述子：以特征点方向作为x轴，比较该坐标系固定的256对像素（p，q）值大小，I_p>I_q,256位描述子对于位取1，I_p<I_q,256位描述子对于位取0
[2]得到两帧图像特征点和描述子后便是进行匹配，两描述子的汉明距离表示相似程度，匹配方法有暴力匹配、快速近似最近邻匹配（FLANN)分区域匹配和BoW匹配等方法，ORB-SLAM2提高了10种匹配方法，在ORB-SLAM2的源码解析再详细分析。
下面主要介绍一下BoW匹配方法的思想：待补充
[3]匹配完之后会存在一些误匹配，在之后算法中有很多地方有去除误匹配的操作，有RANSAC和鲁棒核等
4）光流法：随着相机运动，空间点对应像素点会在图像中运动，光流法用于最终像素点在图像中的运动。根据追踪点的数量，分为稀疏光流和稠密光流。典型方法是LK光流。跟踪角点更加稳定，跟踪边缘会出现滑动现象。而跟踪其他地方（区块）的点跳动频繁。计算时间稍小于描述子的计算和匹配。误匹配较少。相机运动越慢，采样频率越高越不容易跟丢。
提取SIFT、SURF和ORB点特征后，可以使用光流法进行跟踪。该方法适合连续出现的特征点，适合跟踪过程，不适合闭环检测，使用描述子的方法比较适合闭环检测。
具体方法如下：待补充
2、线特征
3、面特征
4、物体

2.直接法：

直接法要求已知各像素点在相机坐标系3D位置，深度相机已知深度可以直接获得该值，单目相机没有深度，需将深度一起优化，实现较复杂。然后，最小化整张图片地光度误差直接求解机体位姿。直接法适合低纹理（特征缺失）的地方，但受光照影响大。直接法工作的条件是场景存在明暗变化，可以是渐变，不存在局部的图像梯度。为增加算法鲁棒性，可以最小化图像块的灰度。直接法得到位姿后可考虑使用极线搜索的方法寻找特征点的匹配关系。
图像法的优点是可以在梯度渐变没有特征点的区域工作，缺点是灰度不变假设很强，要求相机是非自动曝光的。图像是非凸的，小块内凸性较强，适合位移较小的情况。需要大量像素点才能确定位姿。根据像素点数量可分为：
1）稀疏法：先提取特征点，在使用直接法最小化光度误差直接求解机体位姿。这种方法需提取特征点，称为半直接法（融合方法）。在CPU有实时运行的可能。
2）半稠密法：提取有梯度的像素，然后再使用直接法。没有梯度的地方不能起到优化作用。
3）稠密方法：对整幅图像像素点使用直接法。

2.姿态解算

已知两帧之间的匹配特征，求解两帧的相对位姿(R,t)，那么随着源源不断地帧的到来，不断求解相对位姿，便能求解出所有帧相对第一帧的位姿，因为只有空间相近的帧共视的特征才会比较多，所以这个求解过程是增量式的，因此会存在累积误差。
1、2D-2D：至少需要多少对正确匹配？
点特征：
单目SLAM初始化时，经过特征提取和匹配，可以获得两帧中匹配的特征点对（几百），此时特征点在两帧中的3D位姿都是未知的，已知特征点在两帧中的2D像素坐标p₁=(u₁,v₁)、p₂=(u₂,v₂)，故称2D-2D。其中可能存在一些误匹配。补充对极几何的重要概念：如下图，O₁O₂P称为极平面，O₁O₂与像平面I₁、I₂的交点称为极点，e₁p₁、和e₂p₂称为极线，O₁O₂称为基线。
2D-2D是已知大量匹配点对p₁、p₂求解像平面I₁、I₂的相对位姿和P的3D位姿。
当已知p₁和像平面I₁、I₂的相对位姿，理想情况下P在I₂的投影会落在e₂p₂上。
当已知P分别在像平面I₁、I₂的3D位姿时，理想情况下P会落在像平面I₁、I₂的p₁、p₂上。

[1]使用这些匹配求出基础矩阵F和单应矩阵H：使用RANSAC和对极几何筛选出正确的匹配，求得F和H矩阵，至少需要5对匹配点才能求得F，因为噪声的存在和避免R中的非线性约束，一般使用8对匹配点求解最小二乘解求得F，要求8对点组成的系数矩阵秩为8。需要4对匹配点求得H。
F和E的推导：一般情况下，所有匹配点在3D空间并不共面
已知匹配点对p₁、p₂后，设旋转和位移为R₂₁、t₂₁，因为单目中t₂₁的尺度是不确定的，故其只有2个自由度，故旋转和平移是5个自由度，至少需要5对匹配点求得。
将像素坐标转换到归一化平面
P₁=K^-1p₁
P₂=K^-1p₂
则有：
P₂=R₂₁P₁+t₂₁， 其中t₂₁的模长不确定，比例确定
两边同乘t₂₁^∩,则t₂₁^∩t₂₁可看成t₂₁叉乘t₂₁，故t₂₁^∩t₂₁=0，有：
t₂₁^∩P₂=t₂₁^∩R₂₁P₁
两边再同乘P₂^T,则P₂^Tt₂₁^∩P₂可看成P₂点乘t₂₁^∩P₂，由叉乘知t₂₁^∩P₂垂直P₂，故P₂^Tt₂₁^∩P₂=0，则：
P₂^Tt₂₁^∩R₂₁P₁=0
则有：
p₂^TK^-Tt₂₁^∩R₂₁K^-1p₂=0
上面这两个式子称为对极约束，它的几何意义是O₁、O₂、P三点共面
本质矩阵E取：E=t₂₁^∩R₂₁
基础矩阵F取：F=K^-Tt₂₁^∩R₂₁K^-1=K^-TEK^-1
其中，K一般是已知的

H的推导：有时所有特征点可能共面，F矩阵自由度下降，会出现退化，若共面的情况使用F矩阵方法来分解则结果可能不准确，最后三角化得到的3D点并不共面。
当空间点P共面时，设平面的法向量为n（平面指向光心）、光心到平面的距离为d，则有：
n^TP₁+d=0 得： -n^TP₁/d=1
F推导中有：P₂=R₂₁P₁+t₂₁(-n^TP₁/d)
则：
p₂=K(R₂₁P₁+t₂₁)=K[R₂₁P₁+t₂₁(-n^TP₁/d)]=K(R₂₁-t₂₁n/d)P₁=K(R₂₁-t₂₁n/d)K^-1p₁
令：H=K(R₂₁-t₂₁n/d)K^-1
则有：p₂=Hp₁

[2]从E或H矩阵中分解出尺度不确定的两帧相对旋转和平移
选择[1]重投影误差或约束误差小的情况分解
E分解：待补充
（1）八点法求得的E一般不满足E的内在性质：E的奇异值为[σ，σ，0]^T，先对E进行奇异值分解，求得E的奇异值为[σ₁,σ₂,σ₃]^T(σ₁>=σ₂>=σ₃),将奇异值改为[(σ₁+σ₂)/2,(σ₁+σ₂)/2,0]^T,再求得E
（2）

H分解：待补充，非零因子，非零因子下成立？
[3]三角化：求得特征点的3D位姿。已知匹配点对p₁、p₂，通过前两步得到了世界坐标系到两帧的位姿T_1w、T_2w，求匹配点对对应路标点在世界坐标系下的3D位姿P(X,Y,Z)。初始化时以第一帧作为世界坐标系，是其中一个特列。求解步骤如下图。三角化时需选择合适的平移，平移越大，精度越高，但两帧视线角度越大，共视特征越少。增加相机分辨率也能提高精度，但会增大计算量。

[4]如果可以通过深度相机获得点的深度或IMU和编码器获得两帧的位移距离，便能恢复尺度

H分解：待补充，没有深度的情况下，n和d有什么意义？
2、3D-2D：
单目跟踪和重定位过程中，经过特征提取和匹配，可以获得两帧中匹配的特征点对，已知特征点在世界坐标系或第一帧中的3D位姿，在第二帧中的2D像素坐标，求解第二帧在世界坐标系或第一帧中的位姿，故称3D-2D。至少需要3对匹配点计算位姿，另外需要一对匹配点验证结果。方法有P3P（至少3对）、直接线性变换法（DLT）（至少6对）、EPnP、UPnP和非线性优化BA。**深度相机的每帧都有足够的3D点，可使用该方法求得两帧之间位移，可不使用2D-2D方法初始化。**在ORB-SLAM2中，跟踪未丢失时，有较好的初值，直接使用BA求解即可，跟踪丢失重定位时，则使用EPnP方法求解。在SLAM中，经常利用P3P、DLT、EPnP、UPnP等闭式求解方法求得初值，再使用BA做非线性优化。BA方法在后端会详细介绍，下面简单介绍一下其他方法，主要介绍一下EPnP（ORB-SLAM2、VINS-Mono和PL-VIO中使用）方法。
已知点在世界坐标系的坐标，和点在相机归一化平面的坐标。
[1]P3P：使用3对匹配点利用三角形相似关系列出二元二次方程求出点在相机坐标系的坐标。然后使用ICP求出相机坐标系相对于世界坐标系的位姿。
[2]DLT:先将齐次坐标系矩阵看成3x4的无约束的矩阵，利用6对匹配点构建12个方程直接求解矩阵中12个未知元素，然后再使用QR分解恢复旋转矩阵的正交性。
[3]BA:闭环求解方法求得的位姿、关键帧位姿或次新帧加速度预测位姿等作为初值，最小化重投影误差求解。
[4]EPnP：待补充
3、3D-3D：
闭环检测时，需要求得当前帧和闭环帧之间的匹配关系，已知特征点在两帧中的3D位姿，故称3D-3D。单目还存在尺度的漂移。虽然，特征点在世界坐标系的位置和相机的位姿经过长时间会存在较大漂移。但是特征点在相机坐标系系的位置精度较高，深度相机可直接得到，一般不存在漂移；单目相机局部优化最小化重投影误差，故特征点在相机坐标系的位置也是较准确的。为了消除单目的尺度漂移一般使用相似变换（sim）得到当前帧和闭环帧的相似变换。一般深度相机深度测量不太准确，使用3D-3D变换得到的相对位姿没那么准确，激光雷达精度较高，得到的相对位姿也较准确。但激光雷达两帧特征匹配和检测闭环较困难。下面主要介绍视觉SLAM中常用的3D-3D方法，其他方法在本系列激光SLAM前端再介绍。
[1]SVD方法：已知特征点分别在两帧相机坐标系中的3D坐标，求两帧的相对位姿。其中一帧也可以是世界坐标系，那么得到的便是该帧在世界坐标系的位姿。待补充。
[2]sim方法：待补充

3.初始化

2D-2D，对t归一化相当于固定了尺度。另一种固定尺度就是取t使所有点平均深度为1。以光心为旋转轴的纯旋转和沿光轴前后移动不利于初始化。在光轴垂直平面内移动有利于初始化。

4.局部地图

仅使用帧与帧之间运动估计运动，一般精度还不够。一般会维护局部地图来提高跟踪精度和鲁棒性。为了权衡精度和实时性，为了保证实时性，只能使用当前帧跟踪一部分帧，一般使用固定滑动窗口或ORB-SLAM2中使用的共视帧窗口，它们与当前帧共视点相对其他帧多很多。前端主要用于跟踪（得到当前帧位姿），据此选择关键帧交给后端，后端再进行三角化新的路标点、删错冗余和错误的路标点和帧、优化各帧位姿和路标点位姿（一般在世界坐标系表示）。

5.选取关键帧策略

选取关键帧需要权衡关键帧的间隔（一般指旋转、位移和时间）和跟踪质量。关键帧间隔越小，关键帧越多，前端更容易跟踪和重定位，跟踪质量越好；后端计算量会越大，优化时间会增加。