1.关键帧

如果当前帧被认为是关键帧，则进入函数FullSystem::makeKeyFrame：

1. 和非关键帧一样，利用当前帧对前面关键帧中的未成熟点进行逆深度更新FullSystem::traceNewCoarse；
2. 标记后面需要边缘化（从活动窗口踢出）的帧FullSystem::flagFramesForMarginalization；
3. 将当前帧加入到滑动窗口中frameHessians.push_back(fh)，并计算一下该窗口中其他帧与当前帧之间的一些参数比如相对光度、距离等FullSystem::setPrecalcValues；
4. 将当前帧加入到总的能量函数中EnergyFunctional（ef）；
5. 遍历窗口中之前所有帧的成熟点pointHessians，构建它们和新的关键帧的点帧误差PointFrameResidual，加入到ef中；
6. 激活窗口中之前所有帧中符合条件的未成熟点，将其加入到ef中FullSystem::activatePointsMT；
7. 利用高斯牛顿法对活动窗口中的所有变量进行优化FullSystem::optimize；
8. 去除外点FullSystem::removeOutliers；
9. 边缘化不需要的点EnergyFunctional::marginalizePointsF；
10. 在当前帧中提取未成熟点FullSystem::makeNewTraces；
11. 边缘化不需要的帧FullSystem::marginalizeFrame。

1.1 边缘化决策

主要两点（FullSystem::flagFramesForMarginalization）：

1. 当活跃的帧的数量大于最低要求（５个）时，边缘化一帧中活跃的点少于５%或者和最新的帧相比光度参数变化剧烈( |a1−a2|>0.7)的帧（从最早的帧开始遍历）；
2. 如果过程１没有找到需要边缘化的帧，则从全部帧中找到除最近的两帧外离当前帧最远的一帧。

1.2 点的激活

DSO代码中PointHessian表示可用于追踪和参与局部优化的点，除了初始化的第一帧外，它来源于每次生成关键帧时对未成熟点的提取FullSystem::makeNewTraces，并在后续关键帧生成时进行激活FullSystem::activatePointsMT，成功激活的点就由ImmaturePoint变为PointHessian，激活的基本步骤如下：

1. 根据当前窗口中已有的成熟点的数量ef->nPoints，设置激活阈值currentMinActDist；
2. 将所有的成熟点投影到当前帧，生成距离地图CoarseDistanceMap::makeDistanceMap（比如位置p有一个投影点了，那么位置p的值设为0，周围一圈像素设为１，再外面一圈设为２，以此类推，迭代进行）；
3. 遍历所有的未成熟点，如果满足一些条件比如上一次的投影轨迹长度（极线）小于8，quality（次最小误差比最小误差）大于3等，就将逆深度设为其最大值和最小值的平均，将其投影到当前帧，然后考虑其在距离地图上的值，如果该值足够大（用到了第一步中的激活阈值），可以认为该点附近没有成熟点，所以将其加入待优化序列里，反之，则删除该点；
4. 对待优化序列里的未成熟点进行优化FullSystem::activatePointsMT_Reductor，然后激活；

未成熟点的优化是对其逆深度迭代优化。逆深度求导的过程和DSO初始化中的类似，额外加入了一个和点的梯度有关的系数$w_p$，梯度越大，权重系数越小：

$J_{\rho_{1}}=\frac{\partial f(\mathbf{x})}{\partial \rho_{1}}=w_{p} \sqrt{w_{h}} \rho_{1}^{-1} \rho_{2}\left(\nabla I_{x} f_{x}\left(t_{x}-u_{2}^{\prime} t_{z}\right)+\nabla I_{y} f_{y}\left(t_{y}-v_{2}^{\prime} t_{z}\right)\right)$

2 滑动窗口法

求导的参数：相对的光度参数，相对位姿，主帧上点的逆深度，相机内参

对相机位姿求导：对应代码中：Vec6f Jpdxi[2]

$\frac{\partial r_{k}}{\partial \delta \xi_{21}}=\frac{\partial r_k}{\partial x_2}*\frac{\partial x_{2}}{\partial \xi_{21}}=[d_x,d_y]*\frac{\partial x_{2}}{\partial \xi_{21}}$

$\frac{\partial x_{2}}{\partial \xi_{21}}=\left[\begin{array}{ccccc}f_{x} \rho_{2} & 0 & -f_{x} \rho_{2} u_{2}^{\prime} & -f_{x} u_{2}^{\prime} v_{2}^{\prime} & f_{x}\left(1+u_{2}^{\prime 2}\right) & -f_{x} v_{2}^{\prime} \\ 0 & f_{y} \rho_{2} & -f_{y} \rho_{2} v_{2}^{\prime} & -f_{y}\left(1+v_{2}^{\prime 2}\right) & f_{y} u_{2}^{\prime} v_{2}^{\prime} & f_{y} u_{2}^{\prime} \\ 0 & 0 & 0 & 0 & 0 & 0\end{array}\right]$

对逆深度求导：对应代码中：Vec2f Jpdd;

$\left[\begin{array}{l}f_{x} \rho_{1}^{-1} \rho_{2}\left(t_{21}^{x}-u_{2}^{\prime} t_{21}^{z}\right) \\ f_{y} \rho_{1}^{-1} \rho_{2}\left(t_{21}^{y}-v_{2}^{\prime} t_{21}^{z}\right)\end{array}\right]$

对相机内参求导：VecCf Jpdc[2];

$\begin{array}{c}\frac{\partial x_{2}}{\partial C}=\left[\begin{array}{cccc}\frac{\partial u_{2}}{\partial f_{x}} & \frac{\partial u_{2}}{\partial f_{y}} & \frac{\partial u_{2}}{\partial c_{x}} & \frac{\partial u_{2}}{\partial c_{y}} \\ \frac{\partial v_{2}}{\partial f_{x}} & \frac{\partial v_{2}}{\partial f_{y}} & \frac{\partial v_{2}}{\partial c_{x}} & \frac{\partial v_{2}}{\partial c_{y}}\end{array}\right] \\ \end{array}$

$\begin{aligned} \frac{\partial u_{2}}{\partial f_{x}} &=u_{2}^{\prime}+f_{x} \frac{\partial u_{2}^{\prime}}{\partial f_{x}} =u_{2}^{\prime}+\rho_{2} \rho_{1}^{-1}\left(r_{31} u_{2}^{\prime}-r_{11}\right) f_{x}^{-1}\left(u_{1}-c_{x}\right) \\ \frac{\partial u_{2}}{\partial f_{y}} &=f_{x} \frac{\partial u_{2}^{\prime}}{\partial f_{y}} =f_{x} f_{y}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{32} u_{2}^{\prime}-r_{12}\right) f_{y}^{-1}\left(v_{1}-c_{y}\right) \\ \frac{\partial u_{2}}{\partial c_{x}} &=f_{x} \frac{\partial u_{2}^{\prime}}{\partial c_{x}}+1 =\rho_{2} \rho_{1}^{-1}\left(r_{31} u_{2}^{\prime}-r_{11}\right)+1 \\ \frac{\partial u_{2}}{\partial c_{y}} &=f_{x} \frac{\partial u_{2}^{\prime}}{\partial c_{y}} =f_{x} f_{y}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{32} u_{2}^{\prime}-r_{12}\right) \\ \frac{\partial v_{2}}{\partial f_{x}} &=f_{y} \frac{\partial v_{2}^{\prime}}{\partial f_{x}} =f_{y} f_{x}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{31} v_{2}^{\prime}-r_{21}\right) f_{x}^{-1}\left(u_{1}-c_{x}\right) \\ \frac{\partial v_{2}}{\partial f_{y}} &=v_{2}^{\prime}+f_{y} \frac{\partial v_{2}^{\prime}}{\partial f_{y}} =v_{2}^{\prime}+\rho_{2} \rho_{1}^{-1}\left(r_{32} v_{2}^{\prime}-r_{22}\right) f_{y}^{-1}\left(v_{1}-c_{y}\right) \\ \frac{\partial v_{2}}{\partial c_{x}} &=f_{y} \frac{\partial v_{2}^{\prime}}{\partial c_{x}} =f_{y} f_{x}^{-1} \rho_{2} \rho_{1}^{-1}\left(r_{31} v_{2}^{\prime}-r_{21}\right) \\ \frac{\partial v_{2}}{\partial c_{y}} &=f_{y} \frac{\partial v_{2}^{\prime}}{\partial c_{y}}+1 =\rho_{2} \rho_{1}^{-1}\left(r_{32} v_{2}^{\prime}-r_{22}\right)+1 \end{aligned}$

对光度参数求导：对应代码VecNRf JabF[2];

$\begin{aligned} \delta_{\text {photo }} &=\left[-e^{a_{j i}},-b_{j i}\right] \\ \mathrm{J}_{\text {photo }} &=\frac{\partial r_{k}}{\partial \delta_{\text {photo }}}=\left[\frac{\partial r_{k}}{\partial-e^{a_{j i}}} \frac{\partial r_{k}}{\partial-b_{j i}}\right] \\ &=\left[I_{i}\left[\mathbf{p}_{i}\right]-b_{i} \quad 1\right] \end{aligned}$

3.优化过程中零空间的处理

对于纯视觉SLAM，其零空间有7维：3 rotation+3 translation+1 scale；对于VIO系统而言，充分的加速度计激励可以为系统提供尺度信息，加速度计同时可以提供重力方向，使得pitch和roll可观，因此其不客观的维度为4维：3 translation+1 yaw。

对于位置的不可观可以这样理解，当地图整体移动，整个SLAM的优化问题是不变的。所以SLAM中的零空间其实是整个优化问题的零空间，而不是说是优化中某个节点的零空间。就是说整个优化问题存在不可观的维度，这个不客观的维度会通过优化问题进而影响到某个节点的优化，导致那个节点出现问题，常见的比如说纯视觉SLAM在转弯的时候，表现在地图中可能是地图的尺度突然缩小，相机移动量也对应缩小，同时不会对能量函数造成影响

在数学上： 对于正规方程： $\mathbf{H} \Delta x=\mathbf{b}$ (1)

其中hession矩阵的零空间满足：$\mathbf{H} \Delta x_{n s}=0$ (2)

结合两个式子，一定有：$\mathbf{H}\left(\Delta x+\Delta x_{n s}\right)=\mathbf{b}$

因此在零空间上的漂移不会违反系统的约束,但是会对结果产生影响

初始化一般进行归一化，相当于是人为设了一个尺度，为什么还会有尺度问题呢？这是因为DSO采用的滑动窗口法进行优化，当前面的关键帧离开滑动窗口后，最多只

LocalClosing模块是SLAM系统非常重要的一部分，由于VO过程存在累计误差（漂移），LocalClosing模块主要任务是检测闭环，即确定是否曾到达过此处，并在找到闭环之后计算Sim3变换，进行关键帧位姿、地图点的优化（后端优化），可以将这个累计误差缩小到一个可接受的范围内。闭环检测模块使用LocalMapping模块传送过来的关键帧，插入关键帧的操作是在LocalMapping线程主循环剔除冗余关键帧之后进行的，执行语句mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame)。

检测闭环

对应LoopClosing::DetectLoop()函数。检测回环候选（在KeyFrameDataBase中找到与mlpLoopKeyFrameQueue相似的闭环候选帧）并检查共视关系（检查候选帧连续性），函数会将所有通过一致性检测的候选关键帧统一放到集合mvpEnoughConsistentCandidates中，由下一步最终确定闭环帧。主要过程如下：

计算当前帧mpCurrentKF和所有与当前帧有共视关系的关键帧的Bow得分，得到最小得分**minScore** ；

根据这个最小得分minScore 从mpKeyFrameDB（关键帧库）中找出候选的关键帧集合**vpCandidateKFs**；

使用KeyFrameDatabase::DetectLoopCandidates完成候选帧的筛选，具体过程：

• Bow得分>minScore；
• 统计满足1的关键帧中有共同单词最多的单词数maxcommonwords；
• 筛选出共同单词数大于mincommons(=0.8maxcommons)的关键帧；
• 相连的关键帧分为一组，计算组得分（总分），得到最大总分bestAccScore，筛选出总分大于minScoreToRetain(=0.75*bestAccScore)的组，用组中得分最高的候选帧lAccScoreAndMatch代表该组。计算组得分的目的是剔除单独一帧得分较高，但是没有共视关键帧，作为闭环来说不够鲁棒。

对于**vpCandidateKFs里面的每一个关键帧，作为当前关键帧。找出其有共视关系的关键帧组成一个当前集合spCandidateGroup。如果当前关键帧是vpCandidateKFs中第一帧的话，直接把这个spCandidateGroup集合，以分数0直接放到mvConsistentGroups中。如果不是第一帧的话，就从mvConsistentGroups中依次取出里面的元素pair,int>的first，这些元素也是关键帧们组成以前集合sPreviousGroup。只要是当前集合中的任意一个关键帧能在以前集合里面找到，就要将当前集合的得分加1，并把当前集合放到mvConsistentGroups里面。如果当前集合的得分大于3（mnCovisibilityConsistencyTh）的话，当前帧就通过了一致性检测**，把当前帧放到**mvpEnoughConsistentCandidates，最后会找到很多候选的关键帧。下一步用sim3找出闭环帧。**

计算Sim3

对应LoopClosing::ComputeSim3()函数。该函数计算相似变换，从mvpEnoughConsistentCandidates中找出真正的闭环帧mpMatchedKF。主要过程如下：

1. 通过SearchByBow，搜索当前关键帧中和候选帧匹配的地图点，当匹配的数目超过20，就为该候选帧和当前帧构造一个sim3Solver，即相似求解器；

2. 用相似求解器Sim3Solver求解出候选帧与当前帧之间的相似变换Scm，这里使用RANSAC方法；

3. 根据计算出的位姿，通过相似变换求找出更多的匹配地图点（SearchBySim3），更新vpMapPointMatches；

4. 使用更新后的匹配，使用g2o优化Sim3位姿（OptimizeSim3），优化的结果足够好的话（内点数nInliers>20）就把候选的关键帧作为当前帧的闭环帧mpMatchedKF，break，跳过对其他候选闭环帧的判断，同时也得到了mScw；

5. 获取mpMatchedKF及其相连关键帧对应的地图点。调用SearchByProjection将这些地图点通过上面优化得到的mScw变换投影（重投影）到当前关键帧进行匹配，若匹配点>=40个，则返回ture，进行闭环调整；否则，返回false，线程暂停5ms后继续检查Tracking发送来的关键帧队列。

   注意SearchByProjection得到的当前关键帧中匹配上闭环关键帧共视地图点（mvpCurrentMatchedPoints），将用于后面CorrectLoop时当前关键帧地图点的冲突融合。

至此，完成第4、5操作后，不仅确保了当前关键帧与闭环帧之间匹配度高，而且保证了闭环帧的共视图中的地图点和当前关键帧的特征点匹配度更高（20—->40），说明该闭环帧是正确的。

闭环矫正

对应LoopClosing::CorrectLoop()函数，完成回环地图点融合和位姿图优化。具体来说，在回环检测到之后，对当前帧（回环帧）周围的帧的影响、回边的连接以及地图点做出的相应的改变。闭环矫正时，LocalMapper和Global BA必须停止。注意Global BA使用的是单独的线程。主要过程如下：

1. 使用计算出的当前帧的相似变换Sim3，即mScw，对当前位姿进行调整，并且传播到当前关键帧相连的关键帧（相连关键帧之间相对位姿是知道的，通过当前关键帧的Sim3计算相连关键帧的Sim3）；
2. 经过上一步处理，回环两侧的关键帧完成对齐，然后利用调整过的位姿更新这些相连关键帧对应的地图点（修正关键帧看到的地图点）；
3. 将闭环帧及其相连帧的地图点都投影到当前帧以及相连帧上，投影匹配上的和Sim3计算过的地图点进行融合（就是替换成质量高的）；
4. 涉及融合的关键帧还需要更新其在共视地图中的观测边关系，因为找到了闭环，需要在covisibility里面增加闭环边（不止一条边）。这是为了剥离出因为闭环产生的新的连接关系LoopConnections，用于优化Essential Graph。添加当前帧与闭环匹配帧之间的边，该边不参与优化；
5. 然后进行EssentialGraph优化（只是优化一些主要关键帧）；
6. 新开一个线程进行全局优化，优化所有的位姿与MapPoints。

LocalMapping模块作用是将Tracking中送来的关键帧放在mlNewKeyFrame列表中；处理新关键帧、地图点检查剔除、生成新地图点、Local BA、关键帧剔除。主要工作在于维护地图，也就是SLAM中的Map。

Tracking模块会将满足一定条件的图像帧加入关键帧，但是其实ORB-SLAM中关键帧的加入是比较密集的，这样确保了定位的精度。同时LocalMapping线程会对关键帧进行剔除，确保了关键帧的数量不会无限增加，不会对large scale的场景造成计算负担。Tracking模块会判断是否需要将当前帧创建为关键帧，对地图中的关键帧、地图点具体的处理，包括如何加入、如何删除的工作是在LocalMapping线程完成的。

1 处理新关键帧

LocalMapping::ProcessNewKeyFrame()

处理新关键帧与局部地图点之间的关系。具体步骤如下

1. **获取关键帧。**从mlNewKeyFrame队列（待插入的关键帧）中弹出队首的关键帧做为当前帧，并计算该关键帧的BoW，后面三角化恢复地图点有用；
2. **获取关键帧中与关键点关联的地图点。**将TrackLocalMap中跟踪局部地图匹配上的地图点关联到当前关键帧（在Tracking线程中只是利用关键点的匹配关系进行位姿计算，优化当前关键帧姿态）。具体来说，可以分两种情况，地图点没有关联到关键帧，则完成关联（AddObservation），更新地图点normal和描述子，这种地图点是在追踪过程创建并被关键帧关联的，它们在UpdateLastFrame过程就已经有了空间位置；否则记录该地图点为最新添加（这些地图点是在创建关键帧时创建的，已经关联到关键帧），加入mlpRecentAddedMapPoints；
3. **更新共视图连接关系。**使用UpdateConnections函数更新加入当前关键帧之后关键帧之间的连接关系，包括更新Covisibility图和Essential图（最小生成树spanning tree）；
4. 关键帧插入地图。

2.地图点剔除

LocalMapping::MapPointCulling()。任务是对上一函数获取到的最新加入的局部地图点mlpRecentAddedMapPoints进行检查，每个地图点要被保留，在该地图点被创建后的三个关键帧里必须要经过严格的测试，这样保证其能被正确的跟踪和三角化。满足如下条件之一就被剔除：

1. 该地图点是坏点，直接从检查列表去掉；
2. 跟踪（匹配上）到该地图点的普通帧帧数（IncreaseFound）<应该观测到该地图点的普通帧数量（25%*IncreaseVisible），即比值mnFound/mnVisible<0.25，设置为坏点，并从检查列表去掉。比值低说明这样的地图点该地图点虽在视野范围内，但很少被普通帧检测到；
3. 从添加该地图点的关键帧算起，当前关键帧至少是第三个添加该地图点的关键帧的条件下，看到该地图点的帧数<=2（双目和RGBD模式是帧数<=3），设置为坏点，并从检查列表去掉；因此在地图点刚建立的阶段，要求比较严格，很容易被剔除；而且单目的要求更严格，需要三帧都看到；
4. 若从添加该地图点的关键帧算起，一共有了大于三个关键帧，还存在列表中，则说明该地图点是高质量的，从检查列表中去掉。

3.生成新地图点（三角化方法）

对应LocalMapping::CreateNewMapPoints()函数。任务是根据当前关键帧恢复出一些新的地图点，不包括和当前关键帧匹配的局部地图点（已经在ProcessNewKeyFrame中处理，单目模式除了初始化过程会生成地图点外，其它地图点都在这里生成）。具体步骤如下：

1. 找出与当前帧有共视关系的10个（单目模式是20个）关键帧，准备使用对极约束搜索匹配并进行三角化，循环进行以下操作，注意是在当前帧和每一帧共视关键帧之间进行；

2. 检测基线是否过短；

3. 计算基础矩阵F；

4. 搜索满足对极约束的匹配；

   matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);

5. 通过各种条件判断是不是要三角化这些地图点；

6. 完成对匹配结果的三角化，创建新的地图点；

7. 为新的地图点进行一系列配置。

4.临近关键帧搜索更多匹配

对应LocalMapping::SearchInNeighbors()函数。如果关键帧队列中还有新的关键帧，则进行该操作，即在临近的关键帧中搜索到更多的匹配，更新并融合当前关键帧以及两级相连（共视关键帧及其共视关键帧）的关键帧的地图点。

5.Local Bundle Adjustment

对应Optimizer::LocalBundleAdjustment()函数。这里优化的是当前帧，以及与当前帧在covisibility graph里面有连接关系的那些关键帧的位姿，以及这些帧看到的地图点，其实就是对局部地图进行优化。详细分析参见优化部分博客。

6.冗余关键帧剔除

对应LocalMapping::KeyFrameCulling()函数。候选的pKF是LocalMapping中当前处理的关键帧的共视关键帧，不包括第一帧关键帧与当前关键帧。如果一个关键帧检测到的90%的地图点，在其他不少于三个具有相同或更精确尺度的关键帧里面都被检测到，就认定该关键帧冗余（该关键帧的存在提供的地图点观测信息有限），并剔除

ORBSLAM分定位和SLAM两个模式，定位模式localmap不工作，不插入新的关键帧以及更新地图

在跟踪时，初始化完成后首先调用的是GrabImageMonocular函数，对图像格式进行转换，将RGB转为灰度图。

然后通过图片初始化一个Frame类

mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth)

进入Track()函数，完成四个任务：

1.单目初始化；
2.通过上一帧获得初始位姿估计或者重定位。也就是求出当前帧在世界坐标系下的位姿
3.跟踪局部地图TrackLocalMap() 求得位姿估计, 这一步判断了当前帧的特征点和map中的哪些mappoint匹配。
4.判断是否需要给localmapping插入关键帧

1. 单目初始化

双目和RGBD只需要一帧图片即可初始化，单目需要两帧才能完成，其流程为：

1.根据得到的两帧图像的特征匹配，我们就可以计算单应矩阵H，和基础矩阵F

2.计算出的单应矩阵H，和基础矩阵F会有对应的得分，根据得分情况来选择使用单应矩阵H，还是基础矩阵F来进行初始化

3.通过H或者F分解为R，t时，会有多个R，t的组合，需要将匹配的特征点通过三角化重投影后，更具投影结果来选择具体是哪个R，t的组合

4.R，t后，我们自然也通过三角化得到了通过三角化重投影成功的匹配特征点的相对第一帧的3d坐标。剩下那些不能通过R，t重投影成功的匹配点，在后面将它剔除

5.将初始化第一帧和第二帧转化为关键帧keyframe，将三角化重投影成功的匹配特征点的3d坐标包裹为mappoint，全部添加到map中。

6.在创建mappoint过程中，会记录它能被哪些keyframe的哪些特征点匹配。

1
2
3
4
5
6
7
8
9
10
11

// step 4.3：表示该KeyFrame的哪个特征点可以观测到哪个3D点
    pKFini->AddMapPoint(pMP,i);
    pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
// a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);

// b.从众多观测到该MapPoint的特征点中挑选区分度最高的描述子
pMP->ComputeDistinctiveDescriptors();
// c.更新该MapPoint平均观测方向以及观测距离的范围
pMP->UpdateNormalAndDepth();

7.更新关键帧间的图连接关系

​ 1.首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键帧与其它所有关键帧之间的共视程度，对每一个找到的关键帧，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。

map<KeyFrame*,int> KFcounter; // 关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数

​ 2.并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）

for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++) { // 更新具有最佳共视关系的关键帧信息 if(mit->second>nmax) { nmax=mit->second; // 找到对应权重最大的关键帧（共视程度最高的关键帧） pKFmax=mit->first; }

if(mit->second>=th)//th=15 { // 对应权重需要大于阈值，对这些关键帧建立连接 vPairs.push_back(make_pair(mit->second,mit->first)); // 对方关键帧也要添加这个信息 // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它关键帧与当前帧的连接权重 (mit->first)->AddConnection(this,mit->second); } }

​ 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理

​ 4.更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树

    // 更新图的连接(权重)
        mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights，更新当前帧与其它关键帧的连接权重
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end()); 更新生成树的连接
    if(mbFirstConnection && mnId!=0)
    {
        // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
        mpParent = mvpOrderedConnectedKeyFrames.front();
        // 建立双向连接关系
        mpParent->AddChild(this);
        mbFirstConnection = false;
    }`

8. BA优化两帧以及地图点

9. 将MapPoints的中值深度归一化到1，并归一化两帧之间变换

评估当前关键帧场景深度，q=2表示中值. 只是在单目情况下才会使用，其实过程就是对当前关键帧下所有地图点的深度进行从小到大排序,返回距离头部其中1/q处的深度值也就是中位数作为当前场景的平均深度

float medianDepth = pKFini->ComputeSceneMedianDepth(2);
float invMedianDepth = 1.0f/medianDepth;//两个条件,一个是平均深度要大于0,另外一个是在当前帧中被观测到的地图点的数目应该大于100
if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
{
    cout << "Wrong initialization, reseting..." << endl;
    Reset();
    return;
}`

将深度中值作为单位一，归一化第二帧的位姿与所有的地图点 。

//将两帧之间的变换归一化到平均深度1的尺度下 // Scale initial baseline cv::Mat Tc2w = pKFcur->GetPose(); // x/z y/z 将z归一化到1 Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth; pKFcur->SetPose(Tc2w);

// 把3D点的尺度也归一化到1 //? 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches(); for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++) { if(vpAllMapPoints[iMP]) { MapPoint* pMP = vpAllMapPoints[iMP]; pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth); } }

至此单目初始化完成。

2.相机位姿跟踪

初始化完成后，对于相机获取当前图像mCurrentFrame，通过跟踪匹配上一帧mLastFrame特征点的方式，可以获取一个相机位姿的初始值；为了兼顾计算量和跟踪鲁棒性，追踪部分主要用了三种模型：运动模型（TrackWithMotionModel）、关键帧（TrackReferenceKeyFrame）和重定位（Relocalization）。三种跟踪模型都是为了获取相机位姿一个粗略的初值，后面会通过跟踪局部地图TrackLocalMap对位姿进行BundleAdjustment（捆集调整），进一步优化位姿。

几个命名的区别

1. 上一帧mLastFrame：即上一帧图像
2. 上一关键帧mpLastKeyFrame：最邻近当前帧的关键帧，不一定是上一帧，因为图像帧要经过判断后，满足条件才能称为关键帧
3. 参考关键帧mpReferenceKF：就是当前帧的上一关键帧；若创建了新的关键帧，参考关键帧就更新为新创建的关键帧；或者是局部地图中与当前帧共享地图点最多的关键帧。

1. TrackwithMotionModel

当上一帧跟踪正常的时候，我们会生成一个速度模型mVelocity，使得mVelocity不为空，下一帧就会进入TrackWithMotionModel()。

速度模型mVelocity就是上一帧和上上帧的位姿变化。由于在短时间内，速度模型不会变化太大，这样我们可以认为上一帧的速度模型（即上一帧和上上帧的位姿变化）等于当前帧的速度模型（即当前帧和上帧的位姿变化）。根据上一帧的求得的速度模型，以及上一帧在世界坐标系的位姿得到当前帧在世界坐标系中的初始位姿T0

       if(!mLastFrame.mTcw.empty())
        {
            // step 2.3：更新恒速运动模型 TrackWithMotionModel 中的mVelocity
            cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
            //转换成为了相机相对世界坐标系的旋转
            mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
            mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
            mVelocity = mCurrentFrame.mTcw*LastTwc; // 其实就是 Tcl
        }
        else
            //否则生成空矩阵
            mVelocity = cv::Mat();

我们将上一帧匹配的mappoint，通过T0 投影到当前帧上，然后在投影位置附近搜索当前帧的特征点以加速上一帧mappoint和当前帧的特征点匹配。也就是说，以上一帧为跳板，将当前帧的特征点与空间中mappoint进行匹配。

int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);

有了当前帧的初始位姿，以及当前帧的特征点与空间中mappoint进行匹配关系。可以在通过bundle adjustment方法优化得到当前帧的较为精确的位姿T1

得到当前帧较为精确的位姿后，当前帧的有些特征点和mappoint匹配通过这个T1 将不成立，形成误匹配于是将它标记为outliner后取消它们之间的匹配。

2. TrackReferenceKeyFrame

如果恒速模型不被满足,那么就只能够通过参考关键帧来定位了， 可以尝试和最近一个关键帧（即参考关键帧）去做匹配。利用了bag of words（BoW）来加速匹配。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配；最后，利用匹配的特征优化位姿

int nmatches = matcher.SearchByBoW( mpReferenceKF, //参考关键帧 mCurrentFrame, //当前帧 vpMapPointMatches); //存储匹配关系

3.Relocalization

如果正常的初始化不成功,或者 mState=LOST ，那么就只能重定位了。

当速度模型不能用，参考关键帧也没有的时候，就意味着tracking已经跟丢了。这时候，我们先将当前帧转化为BOW，然后将代表当前帧的BOW交给KeyFrameDatabase，它就会返回一个和当前帧很相似的集合作为候选帧。

vector<KeyFrame*>vpCandidateKFs= mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

接着，我们对当前帧和候选关键帧进行通过BOW加速的特征点匹配。并剔除特征匹配数少于15的候选帧。

int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);

对于剩下的候选关键帧我们通过epnp结合RANSAC算法进行筛选，并算出当前帧在世界坐标系中的位姿T2

PnPsolver* pSolver = vpPnPsolvers[i]; cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

对求解结果使用BA优化，如果内点较少，则反投影当前帧的地图点到候选关键帧获取额外的匹配点；若这样依然不够，放弃该候选关键帧，若足够，则将通过反投影获取的额外地图点加入，再进行优化。

如果内点满足要求(>50)则成功重定位，将最新重定位的id更新：mnLastRelocFrameId = mCurrentFrame.mnId;　否则返回false。

3.TrackLocalMap

通过上面三种模型获取了初始的相机位姿和初始的特征匹配，就可以将完整的地图（地图点）投影到当前帧中，去搜索更多的匹配。但是投影完整的地图，在large scale的场景中是很耗计算而且也没有必要的，因此，这里使用了局部地图LocalMap来进行投影匹配。对局部地图的更新包括对局部关键帧和局部地图点的更新。局部地图包含：与当前帧相连的关键帧K1（所有能观察到当前帧对应地图点的关键帧，图中Pos2），以及与K1相连的关键帧K2（一级二级相连关键帧，图中Pos1），并且限制了关键数量不超过80；K1、K2对应的地图点（图中X1）；参考关键帧Kf。局部地图就是红色椭圆圈出的部分，参与局部优化，其中红色代表取值会被优化，灰色代表取值保持不变。

1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints

UpdateLocalMap();

局部关键帧的选择策略：

策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧

策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧

​ 最佳共视的10帧，如果共视帧不足10帧,那么就返回所有具有共视关系的关键帧；自己的子关键帧

2.在局部地图中查找与当前帧匹配的MapPoints, 其实也就是对局部地图点进行跟踪

SearchLocalPoints();

3.更新局部所有MapPoints后对位姿再次优化

Optimizer::PoseOptimization(&mCurrentFrame);

4. 更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果

4.LocalMapping局部建图

LocalMapping线程由LocalMapping::Run()进入；
它主要做了以下几件事：

• 向map中插入关键帧；
• 新近插入的mappoint的剔除；
• 新建mappoint；
• 局部捆集调整（BA）；
• 剔除冗余关键帧；

首先判断是否需要插入关键帧

1：如果用户在界面上选择定位模式，那么将不插入关键帧

2：判断是否距离上一次插入关键帧的时间太短

if( mCurrentFrame.mnId < mnLastRelocFrameId + mMaxFrames && //距离上一次重定位不超过1s nKFs>mMaxFrames) //地图中的关键帧已经足够 return false;

3:得到参考关键帧跟踪到的MapPoints数量, which 参考帧地图点的被观测的数目大于等于 nMinObs

int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);

4.查询局部地图管理器是否繁忙,也就是当前能否接受新的关键帧

bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

5.决策是否需要插入关键帧：

1. 距离上一次重定位距离至少20帧；
2. 局部地图线程空闲，或者距离上一次加入关键帧过去了20帧；如果需要关键帧插入（过了20帧）而LocalMapping线程忙，则发送信号给LocalMapping线程，停止局部地图优化，使得新的关键帧可以被及时处理（20帧，大概过去了不到1s）；
3. 当前帧跟踪至少50个点；确保了跟踪定位的精确度；
4. 当前帧跟踪到LocalMap中参考帧的地图点数量少于90%；确保关键帧之间有明显的视觉变化。

创建新的关键帧

void Tracking::CreateNewKeyFrame()

将关键帧传递到LocalMapping线程（mpLocalMapper->InsertKeyFrame(pKF)），再由LocalMapping完成其他工作。此外，在创建关键帧之后，对于双目、RGB-D的情况，会使用深度值大于0的关键点重投影得到地图点，得到的地图点会和当前关键帧关联，并加入到Map。

最后记录位姿信息，用于轨迹复现