【VSLAM系列】六：ORB-SLAM2代码笔记

ORB-SLAM2代码

1、基础知识

1.1 3D-3D ICP与SE3/Sim3

对3D点求质心点，整体最小那么令与t0有关的项为0，最小化前面的公式求解R，然后用t0=0求解t

尺度

关于SVD分解的证明https://zhuanlan.zhihu.com/p/108858766

1. 根据两组已知的点集算出，并对其做奇异值分解得到 W=UΣVT
2. 计算 R∗=UVT 。
3. 计算 t∗=p−R∗p′

1.2 3D-2D PnP相关算法

已知n个3D点坐标以及与之匹配的2D点像素坐标, 求外参R,t P3P EPnP DLT

EPnP算法 引入4个控制点找到点云的重心，以及点云的三个主方向，使得世界坐标系每一个3D点坐标都可以被4个控制点坐标的线性组合表示
$$hb(p_i^w)=⎡a_{i1}{a}_{i2}{a}_{i3}{a}_{i4}⎦$$
利用刚体变换结构不变性， 世界坐标系下的控制点线性组合与相机坐标系下的线性组合系数一样

1.3 2D-3D Triangulation

//通过绝对位姿计算相对位姿
cv::Mat R12 = R1w*R2w.t();
cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;

     // 基于卡方检验计算出的阈值（假设测量有一个像素的偏差），单目统计样本是2自由度的u，v   
if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
 if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)//双目的统计样本是3自由度的u，v,ur

1.4 2D-2D 对极几何

如果目标点不在统一平面，使用本质矩阵E和基础矩阵F

如果目标点在同一平面或相机发生纯旋转时，则求解单应性矩阵H x2 = Hx1, H= R-tn/d

1.5 Bag-of-Words

字典树的形成

给定一组图像集，将图像中的N个特征描述符，利用Kmean等聚类方法进行聚类，得到k个簇，对每个簇再进行聚类，又得到k个簇，以此类推，形成L层的树状结构，树的叶子节点即为单词（1个单词可能包含了1个或多个特征）6、Orb 特征提取与匹配

**正向索引（direct index）**匹配好的图像匹配特征点
正向索引中，存储了图像，图像中的特征以及特征所在的节点，正向索引用于特征匹配，即确定了另两幅相似的图像后（也就是SLAM中找到了回环图像后），对两幅图像中的特征点通过节点进行快速匹配

反向索引，利用单词匹配图像
反向索引存储的是单词的序号，以及包含该单词的图像序号。反向索引用于判断一幅新图像与已有图像中的哪一幅匹配。匹配过程为：（1）当获得一幅新图像Q后，提取Q中的特征，确定图像中包含了哪些单词；（2）根据获得的单词，在索引中找到对应的位置，该位置存储着包含该单词的一组图像（每个图像都有对应的编号）；（3）某个编号的图像出现一次，在voting array中对应的基数+1，投票最多的前几个为Candidate图像。不需要计算与所有图像之间的相似度，加速搜索。

权重
图像可以由单词进行表示，类似于(w1, w2,…wn)的序列，其中wn为单词的索引。如果图像中属于某一类单词的特征很多，那我们一般认为这个单词的重要性会更大。另一方面来说，如果某个单词在我们构造的vocabulary tree中出现频率很低（生僻字），那么如果图像中出现了该单词，则这个单词的重要性也应该大一些（少见）。也就是涉及到了单词的权值计算问题，DboW2使用了TF-IDF（Term Frequency-Inverse Document Frequency）来计算单词权值。

1.词频Term Frequency（TF）：某个词在图像中出现的次数。为了归一化，词频也可以定义成，某个词在图像中出现的次数 / 图像中的总词数。如果一个词比较少见，那么区分度就大。
2.逆向文件频率Inverse document frequency（IDF）：log（单词树/ 包含该词的图像 +1 ）。如果一个词越常见，那么分母就越大，逆文档频率就越小，越接近0。单词在词袋中出现次数少，这个单词的逆频率就大
3.TF-IDF：词频（TF）* 逆向文件频率（IDF）。如果某个词比较少见，但是它在一幅图像中多次出现，那么它很可能就反映了这幅图像的特性，正是我们所需要的关键词。某个词对图像的重要性越高，它的TF-IDF值就越大。

1.6 ORB特征提取

orbslam2中的特征提取与匹配的技巧：
1 .如何避免图像一个区域内提取的特征点过于冗余或无特征点的情况（特征点均匀化）
(1)总共要提取n个特征点，那图像金字塔每一层提取多少个特征点
第i层金字塔提取的特征数N=n*(1-S)/(1-S^i)其中s表示上下金字塔图像相对尺度

(2)如何解决弱纹理区域提取不到特征点的问题
将图像划分为多个区域进行提取，先用正常阈值进行FAST特征提取，若提取不到特征点则降低阈值

(3)如何解决某些区域提取特征点密度过高的问题
采用四叉树对提取到的特征点进行裁剪，四叉树的节点中有多个特征点该节点就再次划分，当四叉树的总叶子数大于特征点阈值后，每个叶子结点里面保留一个特征点即可

2 .如何加速特征点的匹配以及提高匹配的正确性（匹配筛选）

**加速：**BOWS语法树、结合特征点深度，金字塔层数
**筛选：**旋转主方向、最优次优匹配距离、极线约束、对极几何Ransac筛选

问题2.1：如何用Bows语法树缩小匹配范围，加速匹配
step1：将匹配image1 与被匹配图像image2的特征点都转为FeatureVector
step2: image1中每个特征点只匹配image2中与该特征点有相同node_id的特征点

问题2-2/2-3 如何结合特征点深度与SE(3)/Sim(3)位姿，3D投影的方式缩小重配范盖
step1：特征点结合深度可以恢复出3D点
step2: 3D点通过SE(3)/Sim(3)可以将3D点从匹配坐标系变换到被匹配坐标系，并得到在被匹配图像上的投影点
step3 可以通过3D点的深度预测被匹配特征点所在的金字塔层数
step4: imagel 每个特征点只匹配image2中投影点附近满足预测的特征点

问题2.4 图像金字塔与特征点尺度对应关系

问题2.5 根据图片旋转主方向、最优匹配与次优匹配距离比，减少错误匹配

1）可以统计整张图像的平均旋转量（使用直方图的方式），如果匹配点描述子之间的相对旋转量和平均旋转量差异较大，则为错误匹配
2）匹配过程中最优匹配的描述子距离满足阈值，但最优匹配与次优匹配距离相当，则判定匹配错误概率较大，匹配失败

问题2-6 如何根据极线约束减少错误匹配
image1中每个特征点可以通过基础矩阵得到在imag2上的极线,
如果被匹配特征点距离极线太远，则跳过该匹配

使用RANSAC模型约束进行外点剔除
分别使用基础矩阵模型和单应矩阵模型，通过匹配点对进行位姿转换求解并统计内点个数，内点多的模型重新计算位姿，并剔除外点

2、数据结构

主要逻辑文件

System.cpp --> class System
Initializer.cpp --> class Initializer
Tracking.cpp --> class Tracking
LocalMapping.cpp --> class LocalMapping
LoopClosing.cpp --> class LocalClosing
Viewer.cpp --> class Viewer

主要类

class Frame
class KeyFrame
class KeyFrameDatabase
class MapPoint
class Map
class Optimizer

2.1 Frame帧

具有特征点（特征点分块的栅格），描述子信息，词袋树，状态矩阵，光心坐标
四个构造函数，在构造函数里面进行特征提取、畸变矫正（只矫特征点）、特征点划分块、（双目RGBD进行深度值计算）

 // 复制构造函数
    Frame(const Frame &frame);
// 双目相机的构造函数
    Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
// RGB-D相机构造函数
    Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
// 单目相机构造函数
    Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);

2.2 KeyFream关键帧

每个KeyFrame基本属性是它是一个Frame，KeyFrame初始化的时候需要Frame，KeyFrame包含Frame、地图3D点、以及BoW
KF还维护了一个共视关系图，多叉树

/**
 * @brief 用Fream对象构造一个keyFream对象
 * 
 * @param F 普通帧，具有R t
 * @param pMap 地图，与tracking(), localMap里面的地图是同一个
 * @param pKFDB 关键帧数据集
 */
KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):

共视关键帧，用生成树来维护关键帧之间的共视关系

std::map<KeyFrame*,int> mConnectedKeyFrameWeights;    //与该关键帧相连的所有其他关键帧及其权重

void KeyFrame::UpdateConnections()
    /**
 * @brief 更新关键帧共视图的连接
 * 
 * 1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度
 *    对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
 * 2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
 * 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理
 *    更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树
 */

1. 取出当前关键帧mpCurrentKeyFrame所有的MapPoints—>mvpMapPoints
2. 取出能观测到这些MapPoints的关键帧—>observations
3. 统计每个关键帧观测到这些MapPoints的个数—>Kfcounter<KeyFrame*, int>
4. 建立共视有向连接图
   • 如果这些关键帧中观测到MapPoints的个数大于阈值,则与当前关键帧建立连接 —> vPairs
   • 得到共视关系最好的关键帧 —>pKFmax
   • 如果这些关键帧中观测到MapPoints的个数均小于阈值,则将pKFmax与当前帧建立连接—> vPairs
   • 对vPairs按照共视的的程度进行排序
   • vParis中共视关系最强的关键帧作为mpCurrentKeyFrame的父关键帧,同时mpCurrentKeyFrame作
   为该帧的子关键帧

2.3 KeyFrameDatabase关键帧数据库

关键帧数据库在构造关键帧是会给关键帧（但是关键帧只会将自己从里面删除不会添加进去），回环检测的时候会在里面插入帧

该类的主要作用就是利用词袋数据，在已有的关键帧中查找和当前帧最接近的帧。

这个功能有两个作用：
一是重定位时候，通过检测当前帧和哪个关键帧最接近，来确定相机当前的位置和姿态，对应的检测函数是DetectRelocalizationCandidates。
二是在闭环检测时，通过检测来确定当前关键帧需要和哪些关键帧建立闭环修正的边，对应的检测函数是DetectLoopCandidates。 二者的区别不大，唯一的区别是闭环检测时不需要遍历和自己在闭环检测之前就已经有共视关系的关键帧。

检测的主要步骤如下：

1）找出与当前帧pKF有公共单词的所有关键帧pKFi，不包括与当前帧相连的关键帧。

2）统计所有闭环候选帧中与pKF具有共同单词最多的单词数，只考虑共有单词数大于0.8*maxCommonWords以及匹配得分大于给定的minScore的关键帧，存入lScoreAndMatch。

3）对于第二步中筛选出来的pKFi，每一个都要抽取出自身的共视（共享地图点最多的前10帧）关键帧分为一组，计算该组整体得分（与pKF比较的），记为bestAccScore。所有组得分大于0.75*bestAccScore的，均当作闭环候选帧。

/**
 * @brief 在闭环检测中找到与该关键帧可能闭环的关键帧
 *
 * 1. 找出和当前帧具有公共单词的所有关键帧（不包括与当前帧相连的关键帧）
 * 2. 只和具有共同单词较多的关键帧进行相似度计算
 * 3. 将与关键帧相连（权值最高）的前十个关键帧归为一组，计算累计得分
 * 4. 只返回累计得分较高的组中分数最高的关键帧
 * @param pKF      需要闭环检测的关键帧
 * @param minScore 每一组相似性分数最低要求
 * @return         可能闭环的关键帧，组得分 >0.75*最高组得分的组的单个得分最高的帧
 * @see III-E Bags of Words Place Recognition
 */
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)

MapPoint 路标点

MapPoint是地图中的特征点，它自身的参数是三维坐标和描述子，在这个类中它需要完成的主要工作有以下方面：
1）维护关键帧之间的共视关系
2）通过计算描述向量之间的距离，在多个关键帧的特征点中找最匹配的特征点
3）在闭环完成修正后，需要根据修正的主帧位姿替换特征点
4）对于非关键帧，也产生MapPoint，只不过是给Tracking功能临时使用

//构造函数有两个，分别对应于关键帧和非关键帧
MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap);
MapPoint(const cv::Mat &Pos,  Map* pMap, Frame* pFrame, const int &idxF);

//在关键帧成员变量std::vector<MapPoint*> mvpMapPoints中存储该帧能看到的地图点，KF和MapPoint双向联系
//地图点在某个关键帧中被观测到的特征点的索引map<能观测到该点的帧， 该点在该帧中的索引>
std::map<KeyFrame*,size_t> mObservations;
     // 用于快速匹配的最佳描述子
     cv::Mat mDescriptor;
    //地图点在世界坐标系中的位置
     cv::Mat mWorldPos;

Map局部地图

包含地图点集合set，和关键帧集合set，主要功能：增删关键帧和地图点

public:
    Map();
    void AddKeyFrame(KeyFrame* pKF);
    void AddMapPoint(MapPoint* pMP);
    void EraseMapPoint(MapPoint* pMP);
    void EraseKeyFrame(KeyFrame* pKF);
    void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);
    void InformNewBigChange();
    int GetLastBigChangeIdx()
protected:
    std::set<MapPoint*> mspMapPoints;//所有地图点
    std::set<KeyFrame*> mspKeyFrames;//所有关键帧

3、位姿求解相关代码

3.1 Sim3Solver

/ORB_SLAM2/src/Sim3solver.cpp

核心函数：ICP匹配求解两个相机位姿

/**
 * @brief 构造Sim3解算器：：Sim 3求解器对象
 * 
 * @param pKF1 关键帧1
 * @param pKF2 关键帧2
 * @param vpMatched12 在地图点中两个关键帧都匹配上的点
 * @param bFixScale 是否固定尺度
 */
Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale):

/**
 * @brief 求解mvX3Dc1和mvX3Dc2之间Sim3，函数返回第二帧到第一帧的Sim3变换
 * 
 * @param [in] nIterations 迭代次数
 * @param [out] bNoMore 是否没有足够的点 true点不够
 * @param [out] vbInliers 内点状态
 * @param [out] nInliers 内点个数
 * @return * cv::Mat 
 */
cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)

在ORB-SLAM2中ICP匹配求解的是四元数，解为N矩阵最大奇异值对应的特征向量

3.2 PnPSlower

/ORB_SLAM2/src/PnPsolver.cpp

PnP求解：已知世界坐标系下的3D点与图像坐标系对应的2D点，求解相机的外参(R t)，即从世界坐标系到相机坐标系的变换。这里的pnp求解用的是EPnP的算法。

EPnP的思想是：
将世界坐标系所有的3D点用四个虚拟的控制点来表示，将图像上对应的特征点转化为相机坐标系下的四个控制点
根据世界坐标系下的四个控制点与相机坐标系下对应的四个控制点（与世界坐标系下四个控制点有相同尺度）即可恢复出(R t)

                                   |x|
    |u|    |fx r  u0| |r11 r12 r13 t1| |y|
 s |v| = |0  fy v0| |r21 r22 r23 t2||z|
    |1|     |0  0  1 | |r32 r32 r33 t3| |1|

step1: 构造质心坐标系，用四个控制点来表达所有的3D点
p_w = sigma(alphas_j * pctrl_w_j), j从0到4
p_c = sigma(alphas_j * pctrl_c_j), j从0到4
sigma(alphas_j) = 1, j从0到4

c0是点云的几何中心，其他的点使用PCA主成分分析法得到
利用SVD分解去质心坐标P'TP可`以获得P的主分量
其他3个点就是SVD分解的特征向量

再计算每个3D点用控制点表示的四个系数，使用了N=4的模型求解beita

**step2:**根据针孔投影模型
s * u = K * sigma(alphas_j * pctrl_c_j), j从0到4

**step3:**将step2的式子展开, 消去s
sigma(alphas_j * fx * Xctrl_c_j) + alphas_j * (u0-u)*Zctrl_c_j = 0
sigma(alphas_j * fy * Xctrl_c_j) + alphas_j * (v0-u)Zctrl_c_j = 0

**step4:**将step3中的12未知参数（4个控制点*3维参考点坐标）提成列向量
Mx = 0,计算得到初始的解x后可以用Gauss-Newton来提纯得到四个相机坐标系的控制点

**step5:**根据得到的p_w和对应的p_c，svd-base icp求解即可获得R, t

3.3 Triangulate 三角化

/ORB_SLAM2/src/Initializer.cpp->void Initializer::Triangulate()

使用的是多视图几何的方法，认为对极几何约束是带有噪声的

摄像机矩阵指的是内参和外参合成的矩阵

对于图像中的点通过摄像机矩阵应该满足：

最后通过两张图得到AX=0

使用SVD分解得到的最小特征向量即为3D点

/**
 * @brief 给定投影矩阵P1,P2和图像上的点kp1,kp2，从而恢复3D坐标
 *
 * @param kp1 特征点, in reference frame
 * @param kp2 特征点, in current frame
 * @param P1  投影矩阵P1
 * @param P2  投影矩阵P２
 * @param x3D 三维点
 * @see      这里三角化过程中恢复的3D点深度取决于 t 的尺度，
 */
void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
{
    // 在DecomposeE函数和ReconstructH函数中对t有归一化
    // 这里三角化过程中恢复的3D点深度取决于 t 的尺度，
    // 但是这里恢复的3D点并没有决定单目整个SLAM过程的尺度
    // 因为CreateInitialMapMonocular函数对3D点深度会缩放，然后反过来对 t 有改变

    cv::Mat A(4,4,CV_32F);

    A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
    A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
    A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
    A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);

    cv::Mat u,w,vt;
    cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
    x3D = vt.row(3).t();
    x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}

3.4 Fundamental对极几何

DRT-l

LIGT

/ORB_SLAM2/src/Initializer.cpp->void Initializer::FindFundamental()计算F矩阵

/* 假设场景为非平面情况下通过前两帧求取Fundamental矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
 */
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
    //首先对图像特征点的坐标进行归一化操作（不是求归一化坐标系，而是特征点均匀化）
    // |sX  0  -meanx*sX|   meanx像素坐标平均值
    // |0   sY -meany*sY|   
    // |0   0      1    |
      meanDevX += fabs(vNormalizedPoints[i].x);//求绝对值
      sX =  N/meanDevX;

核心函数

/**
 * @brief 从特征点匹配求fundamental matrix（normalized 8点法）
 * @param  vP1 归一化后的点, in reference frame
 * @param  vP2 归一化后的点, in current frame
 * @return     基础矩阵
 * @see        Multiple View Geometry in Computer Vision - Algorithm 11.1 p282 (中文版 p191)
 */
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
// x'Fx = 0 整理可得：Af = 0
// A = | x'x x'y x' y'x y'y y' x y 1 |, f = | f1 f2 f3 f4 f5 f6 f7 f8 f9 |
// 通过SVD求解Af = 0，A'A最小特征值对应的特征向量即为解

使用卡方检验对结果打分

bool Initializer::ReconstructF()从F矩阵中分解出R和T

/**
 * @brief 分解Essential矩阵
 * 
 * F矩阵通过结合内参可以得到Essential矩阵，分解E矩阵将得到4组解 \n
 * 这4组解分别为[R1,t],[R1,-t],[R2,t],[R2,-t]
 * @param E  Essential Matrix
 * @param R1 Rotation Matrix 1
 * @param R2 Rotation Matrix 2
 * @param t  Translation
 * @see Multiple View Geometry in Computer Vision - Result 9.19 p259
 */
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)

       //四个结果中选择正确的结果，函数里面同时完成三角化
    int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
    //........

E矩阵的分解https://zhuanlan.zhihu.com/p/368843742

3.5 单应性约束

/ORB_SLAM2/src/Initializer.cpp->void Initializer::FindHomography计算F矩阵

 \* 假设场景为平面情况下通过前两帧求取Homography矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
 */
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
 //H矩阵分解R T ,并且三角化路标点
     bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
                      cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{

F矩阵分解

在初始化里面

使用两个线程分别进行F 和H求解，在求解的时候会返回两种求解结果的打分，然后选择得分高的方案进行R T分解并同时三角化路标点

thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    // 计算fundamental matrix并打分
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

    // Wait until both threads have finished
    threadH.join();
    threadF.join();

    // 步骤4：计算得分比例，选取某个模型
    float RH = SH/(SH+SF);

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    // 步骤5：从H矩阵或F矩阵中恢复R,t
    // 参数50: 满足checkRT检测的3D点个数（checkRT时会恢复3D点）
    // 参数1.0：进行checkRT时恢复的3D点视差角阈值
    if(RH>0.40)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

4、特征提取与描述子计算

https://www.bilibili.com/read/cv8854917/

/ORB_SLAM2/src/ORBextractor.cpp

FAST特征的提取过程：

1. 构建高斯金字塔：ComputePyramid()

第一层为原图像，往上依次递减

将图像保存至mvImagePyramid[]中

4.1 特征点提取

计算每层图像的特征点ComputeKeyPointsOctTree()

对金字塔图像进行遍历，将图像分割成cCols X nRows 个30*30的小块

对每个小块进行FAST兴趣点能提取，并将提取到的特征点保存在vToDistributeKeys vector中

对vToDistributeKeys中的特征点进行四叉树节点分配DistributeOctTree()

这一步将提取出来的所有特征点都保存在变量allKeypoints中，但是这里注意allKeypoints的形式是vector < vector >，也就是说并不是所有的特征点都混在一起，外层的vector表示不同的金字塔层，也就是level；

/**
 * @brief 提取每层金字塔下的特征点
 * @param allKeypoints vector.size()对应金字塔层数，vector[i].size()对应每一层提取的特征点总数
 */
void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
    //先划分区域30 * 30大小
//提取角点
FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX), vKeysCell,iniThFAST,true);
//四叉树分割特征点均匀化
std::vector<cv::KeyPoint> ORB_SLAM2::ORBextractor::DistributeOctTree(const std::vector<cv::KeyPoint> &vToDistributeKeys, const int &minX, const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
//根据每层的特征点数量来筛选提取到的特征点，使用四叉树划分进行特征点均匀化

4.2 计算描述子

例如某个特征点的描述子a:

Mat discribe [116,  89,  30,  96,   9, 205,  83, 164, 235, 184, 174,   8, 246, 243,  65, 114, 128, 244, 108,  74,   9,  33, 218,  48, 249, 185,  37,  89, 192,  51,  66,  35]
1*32,每个元素是8bite uint,如116=01110100

描述子是在每层金字塔下计算的，但是对应的特征点最后恢复到了原图像上

/**
 * @brief 计算一个特征点对应的描述子向量（256列）
 * 
 * @param [in] kpt 特征点
 * @param [in] img  图像
 * @param [in] pattern 存储计算描述子的对比像素点的坐标，出入的是数组的首地址
 * @param [out] desc 描述子向量，传入一维数组的首地址
 */
static void computeOrbDescriptor(const KeyPoint& kpt,
const Mat& img, const Point* pattern, uchar* desc)

5、特征点匹配

5.1 SearchByProjection()

a. 跟踪局部地图(在局部地图中寻找与当前帧特征点匹配的)。因为在TrackReferenceKeyFrame和TrackWithMotionModel中，仅仅是两帧之间跟踪，会跟丢地图点，这里通过跟踪局部地图，在当前帧中恢复出一些当前帧的地图点。　　其中的阈值th一般根据单目还是双目，或者最近有没有进行过重定位来确定，代表在投影点的这个平面阈值范围内寻找匹配特征点。匹配点不仅需要满足对极几何，初始位姿的约束；还需要满足描述子之间距离较小。

int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th);

b. 匹配上一帧的地图点，即前后两帧匹配，用于TrackWithMotionModel。

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono);

c. 在当前帧中匹配所有关键帧中(使用路标点)的特诊点，用于Relocalization。

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist);

d. 在当前关键帧中匹配地图中路标点，需要计算sim3，用于Loop Closing。

int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th);

2. 两个重载的SearchByBow函数（注意这里形参表示的匹配的主被动关系和SearchByProjection是反的），用于

a. 在当前帧中匹配关键帧中的地图点，用于TrackReferenceKeyFrame和Relocalization。

int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches);

b. 在当前关键帧中匹配所有关键帧中的地图点，用于Loop Closing。

int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12);

3. 用于单目初始化的SearchForInitialization，以及利用三角化，在两个关键帧之间恢复出一些地图点SearchForTriangulation。

int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize);
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo);

4. 两个重载的Fuse函数，用于地图点的融合：

地图点能匹配上当前关键帧的地图点，也就是地图点重合了，选择观测数多的地图点替换；地图点能匹配上当前帧的特征点，但是该特征点还没有生成地图点，则生成新的地图点）

通过重载/ORB_SLAM2/src/ORBmatcher.cpp->SearchForInitialization()函数来实现2D-2d、3D-3D的特征点匹配

1) 2D-2D特征点匹配 追踪过程相邻两帧匹配，帧具有位姿

两个帧都已经算出了状态矩阵，将上一帧中记录的3D路标点投影到这一帧下，最终匹配上的点的存储形式是当前帧的匹配3D地图点

这里区域查找函数GetFeaturesInArea是Frame类的成员函数
最优次优距离阈值、旋转主方向筛选匹配

/**
 * @brief 对上一帧每个3D点通过投影在小范围内找到这一帧中最匹配的2D点。从而实现当前帧CurrentFrame对上一帧LastFrame 3D点的匹配跟踪。用于tracking中前后帧跟踪
 * 上一帧中包含了MapPoints，对这些MapPoints进行tracking，由此增加当前帧的MapPoints \n
 * 1. 将上一帧的MapPoints投影到当前帧(根据速度模型可以估计当前帧的Tcw)
 * 2. 在投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
 * @param  CurrentFrame 当前帧，具有R和t
 * @param  LastFrame    上一帧, 具有R和t
 * @param  th           阈值
 * @param  bMono        是否为单目
 * @return              成功匹配的数量，最终还修改了当前帧的mapPoints
 * @see SearchByBoW()
 */
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)

2）普通帧与地图匹配3D-2D的匹配 追踪过程地图点与图像特征点匹配

这里区域查找函数GetFeaturesInArea是Frame类的成员函数

/**
 * @brief 对于每个局部地图3D点通过投影到帧中在小范围内找到和最匹配的2D点。从而实现Frame对Local MapPoint的跟踪。用于tracking过程中实现当前帧对局部3D点的跟踪。
 * 注意，地图点投影到这帧中不是在这个函数里面实现的，是已经完成的
 * 将Local MapPoint投影到当前帧中, 由此增加当前帧的MapPoints \n
 * 在SearchLocalPoints()中已经将Local MapPoints重投影（isInFrustum()）到当前帧 \n
 * 并标记了这些点是否在当前帧的视野中，即mbTrackInView \n
 * 对这些MapPoints，在其投影点附近根据描述子距离选取匹配，以及最终的方向投票机制进行剔除
 * @param  F           当前帧
 * @param  vpMapPoints 局部地图路标点
 * @param  th          搜索范围因子：r = r * th * ScaleFactor
 * @return             成功匹配的数量
 * @see SearchLocalPoints() isInFrustum()
 */
int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)

3）关键帧与地图匹配，帧具有位姿

GetFeaturesInArea是KeyFrame类的成员函数

/*************************************************************************************************************
 * @brief  用于闭环检测中将MapPoint和关键帧的特征点进行关联。根据Sim3变换，将每个vpPoints投影到pKF上，并根据尺度确定一个搜索区域，
 * 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新vpMatched
 * 
 * @param pKF 待匹配的关键帧
 * @param Scw 相机的转移矩阵T
 * @param vpPoints 地图点
 * @param [out] vpMatched 匹配到的地图点
 * @param th 阈值
 * @return int 匹配总数
 */
int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th)

4）当前帧与关键帧匹配3D-2D，帧具有位姿

/**************************************************************
 * @brief 对关键帧每个3D点通过投影到当前帧，小范围内找到和最匹配的2D点。从而实现当前帧CurrentFrame对关键帧3D点的匹配跟踪。用于重定位时特征点匹配
 * 
 * @param CurrentFrame 当前帧，具有R和t
 * @param pKF 关键帧
 * @param sAlreadyFound 表示关键帧以及与地图匹配上的点set集合
 * @param th 窗口大小比例
 * @param ORBdist 最佳匹配距离阈值
 * @return int 匹配特征点的数目， 修改了CurrentFrame.mvpMapPoints变量
 */
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)

使用了关键帧的匹配路标点作为匹配项，并没有直接用关键帧的特征点

const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();//关键帧中匹配的地图路标点

5.2 SearchByBoW()

帧还没有计算出位姿

重载/ORB_SLAM2/src/ORBmatcher.cpp->SearchByBoW()

1 重载1 .匹配当前帧与关键帧

/**
 * @brief 通过语法树加速关键帧与当前帧之间的特征点匹配
 * F的node是在Frame中进行计算的
 * Step 1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
 * Step 2：遍历KF中属于该node的特征点
 * Step 3：遍历F中属于该node的特征点，寻找最佳匹配点
 * Step 4：根据阈值 和 角度投票剔除误匹配
 * Step 5：根据方向剔除误匹配的点
 * @param  pKF               KeyFrame关键帧
 * @param  F                 Current Frame待匹配帧
 * @param  [out] vpMapPointMatches F中MapPoints对应的匹配，NULL表示未匹配
 * @return                   成功匹配的数量
 */
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)

重载2 匹配关键帧1与关键帧2， 在闭环检测发生后使用

/**
 * @brief 通过语法树加速两个关键帧之间的特征匹配。该函数用于闭环检测时两个关键帧间的特征点匹配
 * 
 * 通过bow对pKF1和pkf2中的特征点进行快速匹配（不属于同一node的特征点直接跳过匹配） \n
 * 对属于同一node的特征点通过描述子距离进行匹配 \n
 * 根据匹配，更新vpMatches12 \n
 * 通过距离阈值、比例阈值和角度投票进行剔除误匹配
 * @param  pKF1               KeyFrame1
 * @param  pKF2               KeyFrame2
 * @param  [out] vpMatches12  pKF2中与pKF1匹配的MapPoint，null表示没有匹配，大小与KF1中mapPoints大小相同
 * @return                    成功匹配的数量
 */
int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)

5.3 SearchForInitialization()

初始化2D-2D特征点匹配

用于初始化，只在原图上匹配，普通帧与参考帧匹配，帧不具有位姿表达，

单纯的认为两帧图像的特征点像素坐标距离不远，在F2中一F1特征点坐标为重心，边长为2R的矩形里面查找特征点，加速匹配，

这里区域查找函数GetFeaturesInArea是Frame类的成员函数
最优次优距离阈值、旋转主方向筛选匹配

/**
 * @brief 两帧之间的特征点匹配
 * 将上一帧中跟踪的MapPoints投影到当前帧并搜索匹配项。用于单目初始化中参考帧和当前帧的匹配
 * 
 * @param [in] F1 帧1，参考帧
 * @param [in] F2 帧2 
 * @param vbPrevMatched 本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标
 * @param vnMatches12 保存参考帧F1中特征点是否匹配上，index对应是F1对应特征点索引，值保存的是匹配好的F2特征点索引
 * @param windowSize 初始化时假设F1和F2图像变化不大，在windowSize范围进行匹配，外部调用中windowSize = 100
 * @return int 成功匹配的特征点数目
 */
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
{

// Step 1 构建旋转直方图，HISTO_LENGTH = 30,相当于vector<vector>,元素存储的是某个直方图里面的特征点索引
// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点 
// Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
// Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
// Step 5 计算匹配点旋转角度差所在的直方图
// Step 6 筛除旋转直方图中“非主流”部分
// Step 7 将最后通过筛选的匹配好的特征点保存到vbPrevMatched

5.4 SearchBySim3()

两幅关键帧以及计算出位姿，并且所有特征点都以及以及完成了三角化，利用帧看到的路标点投影到另一帧中进行特征点匹配，正反匹配两次

/**
 * @brief    通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
 *           在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12（之前使用SearchByBoW进行特征点匹配时会有漏匹配）
 * @param pKF1  关键帧1，已经计算出来R t
 * @param pKF2 关键帧2，已经计算出来R t
 * @param vpMatches12 关键帧2中的两幅图像匹配到的路标点的特征点id，传入的时候里面有值，对外点进行剔除并其添加没有匹配到的
 * @param s12 两帧变换的相对尺度
 * @param R12 两帧的相对旋转
 * @param t12 
 * @param th 
 * @return int 返回总匹配的特征数
 */
int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint*> &vpMatches12,
                             const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)

5.5 :SearchForTriangulation()

已经计算出R t的两个关键帧，利用基础矩阵进行特征匹配，作用：当pKF1中特征点没有对应的3D点时，通过匹配的特征点产生新的3d点

/**
 * @brief 利用基本矩阵F12，在pKF1和pKF2之间找特征匹配。作用：当pKF1中特征点没有对应的3D点时，通过匹配的特征点产生新的3d点
 * 
 * @param pKF1          关键帧1
 * @param pKF2          关键帧2
 * @param F12           基础矩阵
 * @param vMatchedPairs 存储匹配特征点对，piar<KF1的特征点索引，KF2的特征点索引>
 * @param bOnlyStereo   在双目和rgbd情况下，要求特征点在右图存在匹配
 * @return              成功匹配的数量
 */
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
                                       vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)

6、 system主线程

1）读取ORB字典，为后期的回环检测做准备；
2）创建关键帧数据库KeyFrameDatabase，用于存放关键帧相关数据；
3）初始化Tracking线程。其实Tracking线程是在main线程中运行的，可以简单的认为main线程就是Tracking线程；
4）初始化并启动LocalMapping线程；
5）初始化并启动LoopClosing线程；
6）初始化并启动窗口显示线程mptViewer；
7）在各个线程之间分配资源，方便线程彼此之间的数据交互。

class Viewer;
class FrameDrawer;
class Map;
class Tracking;
class LocalMapping;
class LoopClosing;
class System;

①用词袋初始化关键帧数据库（ 用于重定位 和 回环检测 ）
②初始化一个Map类，该类用于存储指向所有关键帧和地图点的指针
③初始化画图工具，用于可视化。
④初始化 Tracking线程 ，主线程 ，使用this指针（只初始化不启动，启动在 main 函数里 TrackMonocular() 启动）
⑤初始化 Local Mapping线程并启动
⑥初始化 Loop Closing线程并启动
⑦初始化 Viewer线程并启动，也使用了this指针；给 Tracking线程设置Viewer
⑧三个线程每两个线程之间设置指针

7、Tracking追踪线程

7.1 视觉跟踪与建图

主线程System入口

案例中创建System对象SLAM——>
然后循环调用SLAM的TrackMonocular()或者TrackStereo()或TrackRGBD()函数——>
这个函数调用Tracking对象的GrabImageMonocular()函数进行图像处理——>
最后，不论选择那种传感器，最终都进入Track()函数中进行跟踪

GrabImageStero(imRectLeft, imRectRight)
//输入图像
GrabImageRGBD(imRectLeft, imRectRight)
GrabImageMonocular(im)
Stero: mImGray, imGrayRight
//转为灰度图
RGBD: mImGray, imDepth
Mono: mImGray
Stero: Frame(mImGray, imGrayRight, mpORBextractorLeft,
mpORBextractorRight)
//构造Frame
RGBD: Frame(mImGray,imDepth,mpORBextractorLeft)
Mono(未初始化): Frame(mImGray,mpIniORBextractor)
Mono(已初始化): Frame(mImGray,mpORBextractorLeft)
Track
//数据流进入Tracking线程
注:mpIniORBextractor相比mpORBextractorLeft提取的特征点多一倍

Track()跟踪线程主函数

1）创建特征点提取对象并生成Frame

初始化提取的特征点数目是2*nFeatures = 1200 *2 (默认)

// tracking过程都会用到mpORBextractorLeft作为特征点提取器
    mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器
    if(sensor==System::STEREO)
        mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

// 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器
    if(sensor==System::MONOCULAR)
        mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

7.2 初始化

双目初始化

 * @brief 双目和rgbd的地图初始化，由于stereo有深度图，可以单帧初始化
 *        1.设定位姿
 *        2.构造关键帧
 *        3.恢复每个特征点的3D坐标
 *        4.为每个地图点添加该帧为观测帧
 *        5.向地图中添加地图点
 *
 * 由于具有深度信息，直接生成MapPoints
 */
void Tracking::StereoInitialization()

单目初始化

/home/jeet/SLAM_Code/ORB_SLAM2/src/Initializer.cpp

1、构造初始化器的时候就传入了第一帧Fream

2、第二帧的时候调用Initializer::Initialize()函数进行初始化
填入匹配特征点索引pair<int,int>
随机选择 迭代次数 * 8个匹配对
两个线程分别求解H和F
选择得分高的方式 H >2/3F 恢复R t
3、在恢复R t的函数里面还要进行check，check的同时进行路标点三角化

4、通过恢复出来的R t 和3D点，把3D点打包成mapPoint给每个帧和地图

5、对地图进行一次BA优化

6、地图点深度Z归一化到第一帧下，第二帧的旋转矩阵的第三行归一化，点云坐标x,y,z都缩放一个深度中值

/**
 * @brief 给定参考帧构造Initializer
 * 
 * 用reference frame来初始化，这个reference frame就是SLAM正式开始的第一帧
 * @param ReferenceFrame 参考帧
 * @param sigma          使用卡方检验的方差
 * @param iterations     RANSAC迭代次数
 */
Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)

/**
 * @brief  初始化求解。并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
 * 
 * @param [in] CurrentFrame 当前帧，参考帧已经在构造对象的时候取得了
 * @param [in] vMatches12 参考帧与当前帧的匹配对，vector的索引对应参考帧的特征点ID，vector存入的值即为此帧的特征点ID
 * @param [out] R21 
 * @param [out] t21 
 * @param [out] vP3D 恢复出的3D点，世界坐标系（参考帧即为世界坐标系）
 * @param [out] vbTriangulated 该帧中的特征点内点状态
 * @return true 初始化成功
 * @return false 
 */
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
                             vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)

步骤1：组织特征点对 {i, matched(i)}
骤2：在所有匹配特征点对中随机选择8对匹配特征点为一组，共选择mMaxIterations组
步骤3：调用多线程分别用于计算fundamental matrix和homography
步骤4：计算得分比例，选取某个模型 RH = SH/(SH+SF);H > 2/3F

7.3 跟踪

if(!mbOnlyTracking)
        {
            if(mState==OK)// 正常初始化成功，进行恒速运动模型跟踪，或参考上一帧跟踪
            {             
                CheckReplacedInLastFrame();
                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)                 
          bOK = TrackReferenceKeyFrame();//重定位根据参考帧进行跟踪
                else bOK = TrackWithMotionModel();//恒速模型跟踪
                    if(!bOK)                       
                        bOK = TrackReferenceKeyFrame();//恒速模型跟踪失败进行参考帧跟踪
                }
            }
            else bOK = Relocalization();// 重定位 BOW搜索，PnP求解位姿       }
        else
        {
            if(mState==LOST)//跟踪丢失
                bOK = Relocalization();
            else {                
                if(!mbVO) {
                    if(!mVelocity.empty())
                        bOK = TrackWithMotionModel();      
                           else
                        bOK = TrackReferenceKeyFrame();
                   
                }
                else
                {
                    
if(!mbOnlyTracking)
    if(bOK) bOK = TrackLocalMap();
        else //mbVO为true表示地图中与MapPoints的匹配很少。无法检匹配局部地图，因此不执行TrackLocalMap（）。
            //一旦系统重定位成功，将再次使用局部地图进行优化跟踪
            if(bOK && !mbVO)
                bOK = TrackLocalMap();
        }

有运动模型就运动模型跟踪，否则就参考帧跟踪，跟踪失败就重定位
如果是纯定位模式：跟踪丢失就重定位，正常就运动模型或参考帧跟踪，如果跟踪点数少于10，重定位和运动模型跟踪都进行，只要重定位成功就使用重定位的结果
最后进行局部地图跟踪并优化(此时已经具有了该帧的位姿)

更新局部关键帧
更新局部地图点
在局部地图里面查找能投影到该帧上的地图点并进行匹配，这时候该帧的路标点就变多了
通过局部地图里面的匹配特征点进行相机位姿优化

跟踪成功之后
更新运动模型，也就是该帧相对于上一帧的T

7.4 关键帧选择与插入

关键帧判断

// 断当前帧是否为关键帧
bool Tracking::NeedNewKeyFrame()

1）重定位不插入关键帧
2）如果局部地图被闭环检测使用，则不插入关键帧
3）或距离上一次重定位不超过1s(1s内的帧数）并且总关键帧数大于帧率，则不插入关键帧

插入关键帧的判断
1）很长时间没有插入关键帧
2）localMapper处于空闲状态
3）追踪点数小于 0.25 * 共视>3的点，或匹配地图点/该帧总匹配特征点<0.3

必须满足的条件
匹配点数>15

   // 很长时间没有插入关键帧
    const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
    // localMapper处于空闲状态
    const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
    // 跟踪要跪的节奏，0.25和0.3是一个比较低的阈值
    const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || ratioMap<0.3f) ;
    // 阈值比c1c要高，与之前参考帧（最近的一个关键帧）重复度不是太高
    const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || ratioMap<thMapRatio) && mnMatchesInliers>15);

if((c1a||c1b||c1c)&&c2)

关键帧插入

//创建新的关键帧,对于非单目的情况，同时创建新的MapPoints
void Tracking::CreateNewKeyFrame()

如果是双目或者RGBD直接在创建关键帧的时候就对特征点反投影然后加入地图点到地图和帧中，
局部地图中插入关键帧

8、局部建图与优化

​ 处理追踪线程传来的关键帧，剔除质量不好的地图点、新增观测地图点、去除冗余关键帧，为回环检测线程传送关键帧。

8.1 处理关键帧并插入至地图

1）从缓冲队列取关键帧
2）计算关键帧的特征点BoW词袋
3）跟踪局部地图过程中新匹配上的MapPoints和当前关键帧绑定
被观测过的特征点存入了mlpRecentAddedMapPoints中
4）更新关键帧链接关系图
5）关键帧插入地图

void LocalMapping::ProcessNewKeyFrame()
    // 关联当前关键帧至MapPoints，并更新MapPoints的平均观测方向和观测距离范围,因为在追踪线程中进行了局部地图与关键帧的匹配，修改了关键帧的路标点数据

8.2 地图点剔除与三角化

剔除不好的特征点

MapPointCulling
作用:剔除mlpRecentAddedMapPoints中不好的MapPoints
mlpRecentAddedMapPoints来自于:
• 对于双目会在创建关键帧时新建一些MapPoints
• 通过CreateNewMapPoints函数,关键帧之间三角化恢复一些MapPoints
剔除MapPoint的标准为(观测一致性,从被创建开始,未被连续可靠观测到):
• mnFound / mnVisible < 25%
• 从MapPoint被创建开始,到现在已经过了不小于2帧关键帧,但是该MaPoints
被观测到的关键帧数小于阈值(单目为2,双目为3)
•从MapPoint被创建开始,到现在已经超过帧关键帧

/**
 * @brief 剔除ProcessNewKeyFrame和CreateNewMapPoints函数中引入的质量不好的MapPoints */
void LocalMapping::MapPointCulling()
    
//删除条件：状态isbad || 被帧匹配总数 与 在帧视野中的比例 < 0.25 || 被观测次数<阈值（单目2，双目3） || 该点被3个以上的KF观测(这个条件仅在链表里面删除了它并没有设置点的状态为bad)

特征点三角化

选择与当前关键帧共视的前nn(单目为20，双目为10)帧，进行特征点匹配
对匹配好的两帧进行特征点视差角帅选与三角化（视差角度大（cos小）时用三角法恢复3D点，视差角小时用双目恢复3D点（双目以及深度有效）

if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
            {
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);

cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
if(x3D.at<float>(3)==0) continue;
// 四维向量归一化为其次坐标，前三维世界坐标系坐标 看到这
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
 }
else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)//视差角小，用双目的特征点三角化，不用相邻两帧的数据
x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);

//添加地图点信息
pMP->AddObservation(mpCurrentKeyFrame,idx1);            
            pMP->AddObservation(pKF2,idx2);

            mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
            pKF2->AddMapPoint(pMP,idx2);

            pMP->ComputeDistinctiveDescriptors();

            pMP->UpdateNormalAndDepth();

            mpMap->AddMapPoint(pMP);

            // 步骤6.8：将新产生的点放入检测队列
            // 这些MapPoints都会经过MapPointCulling函数的检验
            mlpRecentAddedMapPoints.push_back(pMP);

8.3 地图点融合

/* 检查并融合当前关键帧与相邻帧（两级相邻）重复的MapPoints
 * 将该帧的地图点融合到其他帧上去，将其他帧的地图点融合到该帧上
 */
void LocalMapping::SearchInNeighbors()

步骤1：获得当前关键帧在covisibility图中权重排名前nn的邻接关键帧
步骤2：将当前帧的MapPoints分别与一级二级相邻帧(的MapPoints)进行融合
步骤3：将一级二级相邻帧的MapPoints分别与当前帧（的MapPoints）进行融合
步骤4：更新当前帧MapPoints的描述子，深度，观测主方向等属性
步骤5：更新当前帧的MapPoints后更新与其它帧的连接关系

8.4 冗余关键帧剔除

90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧

Tracking中先把关键帧交给LocalMapping线程
并且在Tracking中InsertKeyFrame函数的条件比较松，交给LocalMapping线程的关键帧会比较密

1）取当前帧的所有共视帧
2）遍历所有共视帧PKF
3）遍历PKF的路标点MapPoints，并计算该点尺度s1
4）获取该路标点的观测帧（如果观测帧>3）
5）遍历这些观测帧，统计尺度s2 (s2 <= s1 +1)
满足条件的观测帧>3,路标点统计+1
满足条件的路标点>90% 删除该帧

 /* @brief 关键帧剔除
 * 在Covisibility Graph中的关键帧，其90%以上的MapPoints能被其他关键帧（至少3个）观测到，则认为该关键帧为冗余关键帧。
 * @see VI-E Local Keyframe Culling
 */
void LocalMapping::KeyFrameCulling()
    //.....
if(nRedundantObservations>0.9*nMPs)
            pKF->SetBadFlag();//删除该帧

9、回环检测

while(1){
        if(CheckNewKeyFrames()){
            if(DetectLoop()){//回环检测
                if(ComputeSim3()){//计算回环之间的位姿变换
                   CorrectLoop();//全局优化与地图调整
         }}}
        ResetIfRequested();
        if(CheckFinish()) break;
std::this_thread::sleep_for(std::chrono::milliseconds(5));}

9.1 DetectLoop()

系统认为连续产生了三次闭环才认为这是一个闭环

组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
        子候选组(CandidateGroup): 对于某个候选的回环关键帧，其和其具有共视关系的关键帧组成的一个"组";
        连续(Consistent)：不同的组之间如果共同拥有一个及以上的关键帧，那么称这两个组之间具有连续关系
        连续性(Consistency):称之为连续长度可能更合适，表示累计的连续的链的长度:A--B 为1, A--B--C--D 为3等;具体反映在数据类型 ConsistentGroup.second上。
        连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时，新的被检测出来的具有连续性的多个组的集合。由于组之间的连续关系是个网状结构，因此可能存在
一个组因为和不同的连续组链都具有连续关系，而被添加两次的情况(当然连续性度量是不相同的)
        连续组链：自造的称呼，类似于菊花链A--B--C--D这样形成了一条连续组链。对于这个例子中，由于可能E、F都和D有连续关系，因此连续组链会产生分叉；为了简化计算，连续组中将只会保存最后形成连续关系的连续组们(见下面的连续组的更新)
        子连续组: 上面的连续组中的一个组
        连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0(相当于新开了一个连续链)
         连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组;
        换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组。

步骤1：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
步骤2：遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore
步骤3：使用最低得分minScore为阈值，在闭环检测中找到与该关键帧可能闭环的闭环备选关键帧
步骤4：检查候选关键帧的连续性，如果当前的闭环候选关键帧的子候选关键帧和上一帧的所有闭环候选关键帧连续组有一组有相同关键帧存在，我们认为该帧连续性达标。

LocalMapping线程每次关键帧处理完之后就会发送给回环检测中，因此每次有回环的时候连续发送的几个帧都是具有回环匹配帧的，连续性检测第一次恒大可能不连续，但是接下来的几帧可能就连续了

9.2 Sim3球解坐标变换

bool LoopClosing::ComputeSim3()

步骤1：从筛选的闭环候选帧中取出一帧关键帧pKF，并通过SearchByBow与当前帧进行特征点匹配，匹配点过少的候选帧剔除

步骤2：对剩下的每个候选帧构造Sim3求解器，并使用ICP，Sim3Solver求解相对位姿变换，一次遍历i，只要求解到一个满足条件的就停止迭代

步骤3：通过步骤2求取的Sim3变换引导关键帧匹配弥补步骤2中的漏匹配，并对相对位姿进行优化，注意：优化的是相对位姿R(当前帧->候选帧的变换)

步骤7：将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配

步骤8：判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配，大于40个才算成功

9.3 全局优化

void LoopClosing::CorrectLoop()//回环修正函数

步骤0：请求局部地图停止，防止局部地图线程中InsertKeyFrame函数插入新的关键帧

步骤1：根据共视关系更新当前帧与其它关键帧之间的连接

步骤2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
步骤2.1：通过位姿传播，得到Sim3调整后与当前帧相连其它关键帧的位姿（只是得到，还没有修正）
步骤2.2：步骤2.1得到调整相连帧位姿后，修正这些关键帧的MapPoints
步骤2.3：将Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
步骤2.4：根据共视关系更新当前帧与其它关键帧之间的连接

步骤3：检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补

步骤4：通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换

步骤5：更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系

最后进行全局优化，在这里面进行最后的帧位姿调整与地图点位置调整

void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF） 
//这是与后端优化函数的接，在这个函数里面调用
 Optimizer::GlobalBundleAdjustemnt(mpMap,// 地图点对象
                    10, // 迭代次数
                    &mbStopGBA,// 外界控制 GBA 停止的标志
                    nLoopKF,// 形成了闭环的当前关键帧的id
                    false); // 不使用鲁棒核函数

优化完成后对于优化的同时跟踪的那几个帧，使用跟踪的相对参考帧的位姿 * 参考帧全局优化后的位姿来进行调整，地图点的位姿调整也是根据其参考帧调整后的位姿进行更新，注意在全局优化函数里面并没有直接修改帧的位姿Tcw，二而是优化完之后在回环检测里面调整的

10、g2o优化

（1）全局BA优化（在单目初始化之后、回环检测之后进行全局BA优化。对所有关键帧的位姿、路标点进行优化）
（2）局部BA优化（在局部建图之后进行，对当前帧及其共视关键帧的位姿、路标点进行优化），优化后检测出outlier，剔除outlier后又继续优化
（3）位姿图优化（仅优化位姿，在恒速运动模型、参考关键字模型、重定位、局部地图跟踪的时候执行）
（4）本质图优化（在回环校正过程中进行优化，将闭环误差分配到本质图中）
（5）Sim3优化（检测到回环之后，会进行闭环帧和当前帧间的Sim3估计，随后进行Sim3优化）

// pMap中所有的MapPoints和关键帧做bundle adjustment优化
// 这个全局BA优化在本程序中有两个地方使用：
// a.单目初始化：CreateInitialMapMonocular函数
// b.闭环优化：RunGlobalBundleAdjustment函数
void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)

/**
 * @brief 位姿优化，在跟踪中使用。仅优化相机位姿，为了构造出投影方程，需要把MapPoints的位置作为常量加入，
 * 
 * 3D-2D 最小化重投影误差 e = (u,v) - project(Tcw*Pw) \n
 * 只优化Frame的Tcw，不优化MapPoints的坐标
 * 
 * 1. Vertex: g2o::VertexSE3Expmap()，即当前帧的Tcw
 * 2. Edge:
 *     - g2o::EdgeSE3ProjectXYZOnlyPose()，BaseUnaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + measurement：MapPoint在当前帧中的二维位置(u,v)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *     - g2o::EdgeStereoSE3ProjectXYZOnlyPose()，BaseUnaryEdge
 *         + Vertex：待优化当前帧的Tcw
 *         + measurement：MapPoint在当前帧中的二维位置(ul,v,ur)
 *         + InfoMatrix: invSigma2(与特征点所在的尺度有关)
 *
 * @param   pFrame Frame
 * @return  inliers数量
 */
int Optimizer::PoseOptimization(Frame *pFrame)

本质图中节点也是所有关键帧，但是连接边更少，只保留了联系紧密的边来使得结果更精确。本质图中包含其主要包含三个部分:1.扩展树连接关系。2.形成闭环的连接关系，闭环后地图点变动新增加的连接关系。3.共视关系非常好(至少100个共视地图点)的连接关系。

/**
 * @brief 闭环检测后，EssentialGraph优化，仅优化所有关键帧位姿，不优化地图点
 *
 * 1. Vertex:
 *     - g2o::VertexSim3Expmap，Essential graph中关键帧的位姿
 * 2. Edge:
 *     - g2o::EdgeSim3()，BaseBinaryEdge
 *         + Vertex：关键帧的Tcw，MapPoint的Pw
 *         + measurement：经过CorrectLoop函数步骤2，Sim3传播校正后的位姿
 *         + InfoMatrix: 单位矩阵     
 *
 * @param pMap               全局地图
 * @param pLoopKF            闭环匹配上的关键帧
 * @param pCurKF             当前关键帧
 * @param NonCorrectedSim3   未经过Sim3传播调整过的关键帧位姿
 * @param CorrectedSim3      经过Sim3传播调整过的关键帧位姿
 * @param LoopConnections    因闭环时地图点调整而新生成的边
 */
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,const LoopClosing::KeyFrameAndPose &CorrectedSim3,const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)

顶点

首先要明确优化的目标，那就是本质图中所有关键帧的 Sim3 位姿，虽然顶点类型都是 g2o::VertexSim3Expmap，但是需要分两种情况进行对待：
( 01 ) ： \color{blue} (01)：(01)： 如果该关键帧在闭环时通过Sim3传播调整过，优先用调整后的Sim3位姿
( 02 ) ： \color{blue} (02)：(02)： 如果该关键帧在闭环时没有通过Sim3传播调整过，用跟踪时的位姿，尺度为1