orb_slam2易忘知识点总结，改代码关掉局部BA,全局BA，回环

ORB写的好的博客：

orb_slam代码解析(2)Tracking线程 - h_立青人韦 - 博客园

ORB-SLAM2初步--局部地图构建 - 达达MFZ - 博客园

orb_slam2有注释的代码：ORB-SLAM2: 注释版，Ubuntu/Windows

TRACKING 线程

• 地图初始化：适用于平面场景的单应矩阵H和适用于非平面场景的基础矩阵F，然后根据一些具体的方法来选择一个模型。

       从前一帧估计位姿：先利用运动学模型与上一帧匹配，如果数量少，则与关键帧匹配，利用词袋模型加速。

• 局部地图跟踪：是在上步运行完，当前帧与局部地图能够寻找更多匹配点，匹配所有地图点，然后被优化。

  FUCTION2.2.3.1.1:TrackLocalMap

  local map意思是当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系，1. 更新局部地图，包括局部关键帧和关键点、2. 对局部MapPoints进行投影匹配、3. 根据匹配对估计当前帧的姿态、4. 根据姿态剔除误匹配。

  motion-only BA：有时候意思是计算单帧的重投影误差。

• 重定位：计算当前帧的词袋向量与之前关键帧的词典数据比较，利用PNP+RANSAC 求出位姿。

• 正式跟踪的步骤：

• 1.先判断mVelocity（之前求得的“t-2”,"t-1"两帧之间的位姿变换）速度模型是否计算成功，如果成功就用速度模型，这时获得当前帧的一个初始位姿了，然后当前帧与上一帧进行匹配意思是将上一帧特征点对应3Dmappoints投影到当前帧，其中用到了project加速匹配，用的是3D点的描述子。

• 2.如果速度模型没有成功，或者成功了但是匹配失败了，则使用TrackReferenceKeyframe（）,位姿初值就用上一帧的，通过当前帧与参考关键帧进行匹配（使用了BOW加速），优化每个特征点对应的3D重投影误差得到当前帧的位姿。参考关键帧就选与当前帧共视程度最高的关键帧。

• 3.重定位：计算与当前帧具有公共单词数的关键帧，计算当前帧与这些关键帧的相似度得到一个候选帧向量，然后对每一帧进行BOW加速匹配，如果匹配点对超过15，则调用SolvePnpRansac,一直迭代直到候选帧数为0。

• 4.TrackLocalMap(),当前帧能看到的所有地图点为MapPoints，遍历MapPoints,找到同样能观测到它的关键帧和相邻关键帧（相邻的意思是与有共识关系帧的那些最佳共视的关键帧）上述这些关键帧及其Mappoints作为LocalMap。遍历局部地图中的所有地图点（但要去除掉MapPoints，因为已经有了），判断1.其投影过去是否在当前帧的图像范围内，2.计算MapPoint到相机中心的距离是否在尺度范围内，3.计算MapPoint视角与平局场景视角，如果角度小则可以。对满足上述三条的点，通过投影加速（重投影加速中包括，金字塔层级的剔除外点，和方向直方图的剔除外带逆），与当前帧的特征点进行匹配，做只改变当前帧位姿的优化。

• 5.收尾工作：NeedNewKeyFrame():如下面关键帧判别标准。

LOCAL MAPPING 线程

• 地图点的删除：为了保存地图点，必须在创建该点云的前三帧测试通过约束，才能真正被保存，这样才能保证可跟踪且不容易在三角化时出现较大误差。一个点要被加入Map，需要满足下面条件： 

（1）这个点要在可预测到能够观察到该点的关键帧中，有超过25%的关键帧能够跟踪到这个点； 
（2）如果一个地图点被构建，它必须被超过三个关键帧观察到（在代码中，可以发现如果是单摄像头，这个阈值被设置为2）。

• 新的地图点的创建：

    通过将检测到的ORB特征点，找到Covisibility Graph中与之相连的关键帧Kc，进行特征匹配，然后将匹配到的特征点进行三角化。对于没有匹配上的点，本文又与其他关键帧中未被匹配的特征点进行匹配。匹配方法使用的是之前的方法，并且将不满足对极几何约束的匹配点舍弃。ORB特征点对三角化后，检查正向景深、视差、反投影误差和尺度一致性，这时才得到地图点。一个地图点是通过两个关键帧观察到的，而它也可以投影到与之相连的其他关键帧中，这个时候可以使用Tracking部分的跟踪局部地图来在附近的关键帧中找到匹配。

• 关键帧的判别标准：

1. 在上一个全局重定位后，又过了20帧； 
2. 局部建图闲置，或在上一个关键帧插入后，又过了20帧； 
3. 当前帧跟踪到大于50个点； 
4. 当前帧跟踪到的比参考关键帧少90%
5. localMapper处于空闲状态

• 关键帧的插入：首先将新的关键帧Ki作为新的节点Ki加入Covibility Graph，并且更新与那些能够共享地图点的关键帧节点相连接的边。同时更新关键帧Ki的生长树，并计算表示关键帧的词袋BOW。这一部分的接口是在LocalMapping.cc中的。

• 关键帧剔除标准KeyFrameCulling：

1. 冗余地图点数目占地图点数目超过90%的局部关键帧
2. 冗余地图点：

1)被超过3个其他关键帧观测到

遍历该局部关键帧的MapPoints，判断是否90%以上的MapPoints能被其它关键帧（至少3个）观测到，该局部关键帧90%以上的MapPoints能被其它关键帧（至少3个）观测到，则认为是冗余关键帧。

• 关键帧删除

1. (1)剔除和地图点之间的关联关系
2. (2)更新连接图、Essential Graph和生成树

共视图(covisibility graph)：将每个关键帧作为一个节点，若两个关键帧存在一定数量（至少15个）共视地图点则这两个这两个关键帧之间存在一条边，边的权重为共视地图点的数量。

本质图(essential graph)：使用位姿图来校正闭环处的累积误差，由于共视图中边的数量较多，我们创建共视图的子图-本质图。本质图包含生成树、共视地图点数量超过100的边和回环的边

生成树(spanning tree)：ORB-SLAM系统在第一个关键帧开始就维护一个生成树，每个关键帧只与共视点最多的相邻帧产生边。本质图包含生成树、共视地图点数量超过100的边和回环的边。

• 局部集束调整

LOOP CLOSING 线程

• loop candidates detection

首先计算 KiKi 和 convisibility graph 相连的帧最小相似度 sminsmin，回环检测时在 recognition database 中去掉相似度小于 sminsmin 的帧，并且去掉 convisibility graph 中和当前帧相连的帧。在判断某一帧为回环时，还需要判断连续的 3 帧都为回环（To accept a loop candidate we must detect consecutively three loop candidates that are consistent (keyframes connected in the covisibility graph).）

单目的 SLAM 中，重建的地图在旋转，平移和尺度上都可以有漂移，所以在 close a loop 时，需要计算当前帧和回环帧之间的相似变换。首先计算当前帧和回环帧之间的匹配点，匹配点用 vocabulary tree 搜索，找到的匹配点是 3D-3D 的点，然后用 RANSAC 算法对这这些点计算一个相似变换，对于得到的相似变换如果能有足够的 inliers，则接受回环。

• Loop Fusion

回环的地方，地图会被重建多次，所以在 loop fusion 时，需要把回环两端的地图融合在一起，并且回环两端的 convisibility graph 也需要添加新的边。 
首先将计算的相似变换作用到当前帧，并且传播到和当前帧连接的关键帧，通过这样做回环两端的地图可以对齐。 
然后回环帧，和回环帧相连的关键帧的所有地图点，向当前帧，和当前帧相连接的关键帧投影寻找匹配点，所有的这些匹配点和计算相似变换时的匹配点进行融合，融合会使得回环两端的关键帧建立新的共视点，从而需要 convisibility graph 中更新关键帧之间的边连接。

Essential Graph Optimization

Essential Graph /pose graph Optimization：To effectively close the loop, we perform a pose graph optimization over the Essential Graph，对于单目 SLAM 优化是针对相似变换做的，优化后地图点根据能观测到它的一帧关键帧进行位置的更新。

优化的参数：每个关键帧的 pose，优化参数 pose 是个相似变换，优化完后再作用到关键帧位姿，作用后，关键帧位姿仍然是个欧式变换，优化后也作用到地图点。

优化约束 
这里的约束都是位姿约束，位姿约束指帧和帧之间的位姿变换。 
约束有： 
当前回环两端新建立的边（回环两端地图点融合产生连接回环两端新边）形成的位姿约束。 
spanning tree 位姿约束。 
每个关键帧的回环约束。 
convisibility graph 中权值大于一定阈值的边约束。

Global BA

Global BA：优化地图中所有关键帧的 pose 和所有地图点的坐标。优化的变量包括所有关键帧 pose，所有特征点坐标，优化的约束是所有特征点所有观测的投影误差。

代码部分:

如果在./build_ros.sh 中，export了后仍然不好使，则参考：http://86rev0.smartapps.cn/pages/blog/article-detail?userName=Robot_Starscream&articleId=90245477    就ok了

关掉局部BA需要注释掉LocalMapping.cc的第80行和81行，即：

// Local BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

关掉回环需要注释掉LoopClosing.cc中的第71-75行：

               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }

关掉全局BA需要注释掉LoopClosing.cc中的第579行：

mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);

   关掉生成的点云，Tracking.cc 注释掉

// insert Key Frame into point cloud viewer
    //mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth );

system.cc 里 mpPointCloudMapping 关键字的句子注释掉，搜pointcloud注释掉