视觉定位流程简介

单目视觉SLAM(MONO SLAM)

需要传感器：单个相机
以ORB-SLAM2为例：

主要分为3个线程：
TRACKING
LOCAL MAPPING
LOOP CLOSING
##TRACKING
tracking主要是基于视觉里程计VO来逐帧根据之前所得的各帧位姿来获取该帧的位姿
最原始的方法是：
1.提取新加入帧的特征与上一帧（已知位姿）作匹配。
2.根据对极约束计算出新加入帧与上一帧的相对位姿，并根据上一帧已知位姿统一到同一坐标系下，一般用RANSAC来剔除外点和平差。

存在的问题和在ORBSLAM2中的改变（不一定是首创）：
1.每一帧都提取匹配特征点，计算量过于庞大
1-: 采用ORB特征（描述子降维），提出关键帧概念（帧抽稀）
2.每一帧只与前一帧匹配，连接太弱
2-： 采用局部地图，保存生成的特征点三维坐标信息和特征描述子信息，新加入帧与局部地图里的点进行匹配；为了防止局部地图数据过大使局部地图与新帧匹配效率降低，局部地图持续更新，加入新的三维点并剔除较早的点。 相当于新加入帧与之前N张都有连接。新帧位姿由PnP计算得到（后方交会）。

误差累积
由VO得到的位姿信息误差累积很明显

1. 在能利用到Hessian矩阵的稀疏性后才开始采用BA（BundleAdjustment）进行非线性优化，常用库为g2o（还有ceres solver，sba）。（之前一直是EKF来优化）
2. BA并不能解决误差累积，ORBSLAM采用的是回环检测+图优化的方法（后文介绍）。

##LOCAL MAPPING
local mapping线程一直在等待是否加入新的关键帧，若加入新的关键帧则触发：

1. 交会获得新的特征点，更新局部地图。
2. 局部地图内进行BA，改正数包含了相关的帧位姿（6参）和局部地图点的坐标（3参）。
3. 关键帧更新（去冗余减少优化计算量）

tracking+local mapping可以理解成：
输入：一帧连续图像+局部地图（匹配相关）+全局地图（之前所有帧位姿和交会所得地图点）
输出：新帧位姿（累积误差）+更新局部地图+全局地图

##LOOP CLOSING
如果每次都匹配当前帧与所有帧的特征点，就可以归结为incremental SFM的问题，精度将大大的提高。
如果能不基于特征匹配的方法来识别已经经过的区域（存在已知坐标的地图点），新帧仅与判断为同一区域的之前帧进行匹配将极大的增加计算效率。
Place Recognition问题
识别场景现在一般使用词袋模型（DBOW）的方法，可以理解为：
用一个特征来描述一张图像，新帧仅与该词袋特征相近的旧帧进行匹配。
场景识别有2个好处：

1. 重定位。新帧与相邻之前帧匹配不到足够特征点，即为丢失。再加入的新帧先由词袋特征与相近的旧帧进行匹配（而不是所有旧帧）获得与匹配旧帧的相对位姿加上旧帧已知的位姿，使得重新回到同一坐标系下。
2. 建立起回环约束。当回到旧场景，由于误差的累积，新帧与旧帧之间的误差极大，根据回环闭合的约束可以将误差平均分配到回环内。
   图优化
   不细说，主要是考虑到计算量的问题，可以理解为：
   优化改正的值只有相机位姿而不包含地图点的三维坐标，大大减少计算量。

##高精地图中标识牌的作用

整体流程图如下：

不同之处：

1. 以定位为主要目的，不包含测图任务（更新标志信息可以加上额外的离线流程），所以视觉部分的平差只包含了局部地图内的平差。局部地图存储内容为地面特征点的三维坐标和特征点的特征描述子。
2. 三个线程同时进行，新帧加入首先根据VO判断是否为关键帧（具体判定标准很关键），减少LOCATING的计算量；关键帧插入则触发LOCATING，最主要的目的是更新局部地图；新帧同时进行标牌的识别，如果图像中识别到标牌触发RECOGNITION。
3. 由于多源数据中包含了GPS信息，所以是在绝对坐标系下；当GPS失连，只要在从GPS失连开始到当前状态VO没有丢失，IMU没有丢失（IMU丢失应该不会），都能保证是在统一的坐标系下。
4. RECOGNITION触发后，主要任务是将识别到的标牌部分提取特征（与整体提特征不同，特征描述金字塔层数更高且不再考虑特征点分布均匀，以尽可能多提特征），然后将标牌上的特征点与高精度地图中标牌上保存的特征点信息匹配，建立对极约束关系。
5. RECOGNITION涉及2个匹配步骤：
   （1）根据绝对坐标信息搜索高精地图中当前位置附近的标志；根据VO估计在图像中各个标志的相对位置；根据相对位置关系查找标志并一一对应（不一定一一对应，图像上一个标志对应多个高精度地图的待选标志都行）
   （2) 将图像标志部分的特征点与高精地图中待选已知标志保存的特征点进行匹配（逐个标志匹配）；当前时刻的位姿是有值的，若匹配计算所得坐标（RANSAC计算防止特征点的误匹配，RANSAC误差大则认为是完全错判断，即标志长相都不一样）与已知值差别太大则认为是误判断；
6. 确定标志的对应关系之后，将匹配的光束作为限制条件带入平差，获得精确定位。PS: 在隧道中如果有标志更新位姿，能解决缺少GPS时，IMU和VO误差累积的问题。

需解决的问题:

1. 视觉VO的处理频率，特征点提取匹配耗时影响因素：特征描述子选择，金字塔层数，提取特征点个数，等等。
2. 在高精地图中搜索对应标志的速度，影响的因素有：查找算法速度，根据绝对坐标设定的查找范围的大小，视觉VO估计的标牌位置的精度，等等。
3. 高精度地图需要提供标志上 特征的绝对坐标， 至少要100个左右（因为实际匹配要保证匹配到15个（经验值）以上）， 包含了（X,Y,Z）和特征描述子。
4. 要保证相邻标志之间的相对坐标精度较高，绝对坐标不用保证太高。主要目的还是让计算所得的坐标与高精地图更符合，所以高精度地图本身的绝对精度要求不高。