1.函数声明与作用
TrackLocalMap()是Tracking类的成员函数之一，为Protected属性。它没有输入值，因为它直接读取了Tracking类的各种成员变量。其返回值是bool类型的变量，用于指示跟踪是否成功。
该函数的主要作用是利用局部地图对位姿进行进一步跟踪与优化。在跟踪得到当前帧初始姿态后，对Local Map进行跟踪得到更多的匹配，从而优化当前位姿。前面无论是TrackWithMotionModel()还是TrackWithReferenceFrame()只是跟踪一帧得到初始位姿。但在这里我们会搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化。这里主要涉及到当前帧、当前帧的MapPoints、当前关键帧与其它关键帧共视关系。
2.函数调用
TrackLocalMap()函数在Tracking类中主要在Track()函数中被调用，如下图所示。
如果之前的初始跟踪OK的话，就再进行TrackLocalMap()，对初始位姿进行优化。
3.主要流程
函数的主要流程如下所示。
整个函数可以分为三个部分，第一部分是对现有帧和地图点的预处理和初步统计(UpdateLocalMap()、SearchLocalPoints()等函数)，第二部分就是对位姿的优化，根据不同传感器进入不同函数(PoseOptimization()、PoseInertialOptimizationLastFrame()、PoseInertialOptimizationLastKeyFrame())，第三部分就是对优化后的内外点以及系统状态进行判断，以此判断TrackLocalMap是否成功。可以看到，整个函数的核心是对于位姿的优化(PoseOptimization()、PoseInertialOptimizationLastFrame()、PoseInertialOptimizationLastKeyFrame())。这也是符合我们上面提到的它当初设计的功能。位姿优化以后，有一系列统计内点的步骤。最后根据不同状态、不同内点的数量，判断TrackLocalMap是成功还是失败，返回True或False。
这里再对函数中用到的一些核心外部函数再简单进行一些介绍。
3.1 UpdateLocalMap()
更新局部地图，函数本身非常简单，是对UpdateLocalKeyFrames()和UpdateLocalPoints()的二次封装。局部地图可以看做由两部分构成：局部地图点以及观测到它们的关键帧。
这里首先需要说明的是在Tracking类中，局部地图点和对应关键帧分别用mvpLocalMapPoints和mvpLocalKeyFrams这两个变量表示，它们都是Protected类型。进一步，UpdateLocalKeyFrames()和UpdateLocalPoints()两个函数简介如下。
3.1.1 UpdateLocalKeyFrames()
函数主要用于更新关键帧相关的变量，主要是更新mvpLocalKeyFrames(局部关键帧列表)。这个函数的核心作用是，根据初始得到的地图点与估计的关键帧位姿和状态，判断哪些关键帧是和当前局部地图点绑定的(可以观测的)，为之后优化构建观测边打基础。进一步，函数可以分为四个步骤：
- 第一步就是基于现有地图点进行统计(GetObservations())，看哪些关键帧观测到它们；
- 第二步是将所有能够观测到局部地图点的关键帧都添加进来；
- 第三步是寻找和已经加入局部关键帧列表的相邻关键帧也加入进来；
- 第四步是增加最后10帧关键帧(主要为了IMU)。
这里简单说一下MapPoint的观测相关概念。在MapPoint类中，观测以如下方式定义。
一个观测由两部分组成：KeyFrame和(int,int)元组。这两部分共同构成了一个C++ STL中的map。它是一种键值对容器,里面的数据都是成对出现的。每一对中的第一个值称之为关键字(key),每个关键字只能在map中出现一次;第二个称之为该关键字的对应值。
可以这样理解：现在我们说的所有变量和函数都是针对MapPoint的，所以我们从MapPoint的角度看关键帧(而不是整个地图)。我们只需要看对于某个地图点而言，有多少个关键帧观测到了它，这就是第一部分的作用。当然仅仅知道关键帧还不够精确，还需要知道当前地图点在关键帧中所对应的特征的具体索引，这就是第二部分的作用。而之所以是一个元组，原因在于传感器可能是双目的，这样对于某一个地图点而言，在左右影像上会分别有两个索引。当然如果是单目就不存在这个问题了。下面四个函数是与地图点观测相关的函数。
AddObservation()
PrintObservation()
GetObservations()
所以，第一步，在UpdateLocalKeyFrames()函数中，首先对当前帧中的特征点进行遍历，如果它有对应的地图点，再进一步判断地图点装态是否OK(!pMP->isBad())。如果状态OK的话，就会调用上面提到的MapPoint的成员函数GetObservations()来获取当前地图点的观测。得到观测之后，我们会遍历这个map，统计其包含的关键帧个数(关键帧A有多少个，关键帧B有多少个…)，将统计的结果放到keyframeCounter中。
然后，我们会把当前的所有局部关键帧列表清空，并预留3倍原先大小的vector，如下图所示。
第二步，我们遍历刚刚得到的keyframeCounter变量，依次将关键帧添加进来，如下图所示。
在遍历的时候，由于keyframeCounter本身是一个map，所以第一个元素(键)是我们需要的关键帧，先将其获取到。然后对其做一系列的判断，看看是否是异常情况。如果不是异常情况，就把当前关键帧添加到局部关键帧列表mvpLocalKeyFrames中来，并把其参考关键帧ID设为当前帧。
第三步，通过上述步骤，我们已经把每个地图点中记录的、符合条件的观测关键帧添加到了局部关键帧列表mvpLocalKeyFrames中来。但我们认为这样还是不太够，我们还尝试把这些关键帧的相邻关键帧也加到mvpLocalKeyFrames中来。因为每一个关键帧中都有对应的变量表示相邻关键帧，所以这也是很容易实现的，如下图所示。
首先，为了性能考虑，如果当前局部地图列表中已经有80个关键帧了，就不再添加新的关键帧了。如果没有就可以继续添加。这里同样也用到了一个KeyFrame的成员函数GetBestCovisibilityKeyFrames()。从名字就可以看出来这个函数的用途，就是返回指定个数个最优共视关键帧，以vector<KeyFrame*>输出，如下所示。
获得当前帧的共视关键帧之后，再逐个遍历这个共视关键帧列表，如果这个关键帧状态OK，并且其参考关键帧ID不等于当前帧ID的话，就将其添加到局部关键帧列表mvpLocalKeyFrames中来。
然后，又调用KeyFrame的成员函数GetChilds()来获得子关键帧，返回的是set类型的变量。
获得之后，还是和上面类似的，逐个遍历这个set中的关键帧，如果满足条件，就添加到局部关键帧。
最后，是调用KeyFrame的GetParent()函数获取父关键帧。
同样的逻辑，判断父关键帧是否满足条件，如果满足就添加到局部关键帧列表中来。
这里简单再说一下什么是子关键帧和父关键帧。这种连接关系是在KeyFrame的成员函数UpdateConnections()中被定义和修改的。简单理解是，所谓父关键帧指的是与当前关键帧共视程度最高的关键帧，同时当前关键帧就成了父关键帧的子关键帧。所以，整个第三步其实就是在通过各种办法，进一步向局部关键帧列表mvpLocalKeyFrames里添加关键帧。
第四步，如果当前传感器有IMU，并且局部关键帧列表mvpLocalKeyFrames中关键帧个数还没够80的话，就继续添加一些关键帧。
具体而言，我们会以当前关键帧为起始，添加最近的20个关键帧(虽然注释写的是10，但代码中是20)。具体操作就是通过访问KeyFrame的mpLastKeyFrame变量来找到上一个关键帧。而在循环中，通过将上个关键帧赋给tempKeyFrame这个临时变量，实现不断迭代。
所以可以看到，通过以上四步，我们向局部关键帧列表mvpLocalKeyFrames中添加了足够多的关键帧。简而言之，我们是按照如下标准添加关键帧的：
* 首先，我们将地图点中记录的所有满足条件的关键帧添加进来；
* 然后，对每个已添加关键帧，获取它最优共视的10个关键帧，满足条件也添加进来；
* 同时，对每个已添加关键帧，获取它的子关键帧，满足条件也添加进来；
* 同时，对每个已添加关键帧，获取它的父关键帧，满足条件也添加进来；
* 最后，如果有IMU，再获取最近新20个关键帧，满足条件也添加进来。
如下图所示，是一个简化的场景。
对于MP1，它的观测就是指可以直接观测到它的关键帧，这里就是KF1、KF2、KF3。其余地图点同理。那么根据上面添加关键帧的规则，这里KF0-KF8都是局部关键帧。
3.1.2 UpdateLocalPoints()
用于更新局部地图点。
mvpLocalMapPoints表示当前的局部地图点，mvpLocalKeyFrames表示当前的局部关键帧。如上图所示，函数在一开始就会将当前全部局部地图点清空，然后遍历局部关键帧列表mvpLocalKeyFrames。对于每一个关键帧，都调用GetMapPointMatches()成员函数，获取匹配的地图点(得到一个MapPoint类型的vector，其实就是返回的KeyFrame类的mvpMapPoints成员变量)。然后逐个遍历这个vector，对于其中的每个地图点都进行判断：如果为空，跳过；如果该地图点的参考帧ID和当前帧ID相同，跳过；如果当前地图点是坏的(isBad())，也跳过。如果上面的条件都不满足，那么就将其放到局部地图点列表mvpLocalMapPoints，并将该地图点的参考帧ID设为当前帧。
所以在这个函数中，可能会出现一个潜在的可能，就是本来还有地图点(mvpLocalMapPoints不为空)，但是由于迭代的时候每个地图点都不满足判断条件，导致一个都没添加到局部地图。而且因为函数在一开始就清空了所有地图点，又一个都没添加，导致的结果就是mvpLocalMapPoints为空了。
3.2 SearchLocalPoints()
其核心步骤是将局部地图点投影到当前帧，去掉不在视野内的无效的地图点，剩下的局部地图点投影到当前帧进行匹配(SearchByProjection()函数)。
检查可视关系，是否在视野之内：
对投影的地图点进行匹配：
3.3 PoseOptimization()
主要用于位姿优化，仅优化位姿，不优化地图点，用于跟踪过程。输入是Frame影像帧，输出是内点个数。其核心流程简单总结如下。
首先，程序会构造求解器，并对其进行一系列初始化，如下所示。
然后，会将输入的影像帧Frame作为优化的一个节点。我们将节点的初值设置为传入帧的估计位姿，节点ID设为0。同时由于我们这里优化的就是帧的位姿，所以它不能是固定的。但在之后的添加地图点的过程中，这些地图点就是固定的了。
最后，我们将构造好的节点添加到优化器中。
接下来，我们就要根据影像帧所对应的地图点添加节点并且将其和刚刚添加的帧节点进行连接。
首先，我们对于输入帧中的地图点mvpMapPoints列表进行逐个遍历，对于每个地图点mvpPoints[i]首先判断其是否为NULL，如果不为NULL，继续进行下面的判断。如果是传统的相机，进入第一个分支，否则进入第二个分支。对于传统SLAM而言，继续判断是否为双目，进而进一步选择不同分支。
对于传统SLAM单目分支，其添加过程如下。
首先，我们获取到该地图点的观测，也就是其在影像上对应的像素坐标x、y，然后新建一个边对象。然后将刚刚的观测添加到该边，并设置信息矩阵。同时，也设置RobustKernel、Dalta、该条边对应的相机模型、地图点的世界坐标。设置完成后，将该边添加到优化器中。同时为了之后便于管理，也将该边push_back到vpEdgesMono中。
对于传统SLAM双目分支，其添加过程也是类似的，如下。
首先还是获取到该地图点对应的像素坐标，然后构造边。之后设置该边的一系列观测、信息矩阵、RobustKernel、Delta、相机的内参(fx、fy、cx、cy)、双目基线(bf)以及该地图点的世界坐标(Xw)。然后将其添加到优化器中，同时为便于管理，也将其放到vpEdgesStereo中。
对于非传统SLAM，也有两个判断，如果地图点的索引是小于Nleft，是属于左相机的观测，否则是属于右相机的观测。这里需要注意的是，这里的左右相机和上面的双目相机不是一回事。更准确的说，这里的左右指的是鱼眼广角相机。这种相机会呈现非常广的视角，但是都在一张影像中(并非像传统双目一样，有独立的两个影像输入)。对于这种数据，还是按照双目的办法进行处理，可以简单理解为图像左边部分看作是左相机拍的，右部分看作是右相机拍的。
对于此种情况的左相机，添加边的过程如下。
总体流程依旧类似，获取地图点的观测、新建边、向边添加观测、信息矩阵、RobustKernel、Delta、相机模型、世界坐标。然后添加地图点的世界坐标。最后，向优化器中添加边，并且将其添加到vpEdgesMono中。
对于此种情况的右相机，添加边的过程如下。
总体流程是一样的，唯一不同是我们将边添加到vpEdgeMono_FHR。
在添加边完成之后，就要开始实际的优化过程。
具体而言，我们会进行4次优化。每次优化三个部分：vpEdgeMono、vpEdgesMono_FHR、vpEdgesStereo。当然，正如上面流程所示，对于不同的传感器，我们会添加不同的边。所以到这一步，我们并不会同时优化所有这三个类型的边。这里面肯定存在为0的列表，这样就直接跳过了。
具体而言，对于单目的边，对每条边都计算误差，如下。
对于广角相机的右相机，也添加边，如下。
对于双目相机，添加边，如下。
最后，将优化后的位姿重新赋给传入的影像帧Frame，并且返回内点的个数，完成优化。
3.4 PoseInertialOptimizationLastFrame()
与PoseOptimization()函数相比，其不同之处主要在于IMU信息的引入，但主要流程是相同的。
首先对优化器进行初始化，如下图所示。
然后设置帧节点，如下。
这里和之前不一样的地方在于，除了增加了位姿节点(VertexPose)，还另外包括了速度节点(VertexVelocity)、陀螺仪偏置节点(VertexGyroBias)、加速度偏置节点(VertexAccBias)。
然后，准备设置MapPoint节点。
对于每一个地图点，逐个进行判断。如果其不为NULL，进行下一步判断。然后如果是单目或者是广角的左相机、双目的右相机、广角的右相机，就分别进入不同分支。
如果是左相机，按如下流程添加MapPoint节点与观测边。
如果是双目，按如下流程添加MapPoint节点与观测边。
如果是右相机，按如下流程添加MapPoint节点与观测边。
然后，设置上一帧节点
然后添加IMU预积分边：
添加陀螺仪边：
添加加速度边：
最后，添加IMU位姿约束边：
然后，类似的，我们进行4次优化。
对于不同的情况，分别计算不同的误差(单目)。
对于双目情况，计算误差如下。
优化完成后，恢复优化的位姿。
3.4 PoseInertialOptimizationLastKeyFrame()
相比于PoseInertialOptimizationLastFrame()，此函数的作用是类似的，但是针对关键帧进行位姿优化。
初始化优化器，如下。
设置帧节点。
与之前类似的，除了位姿节点(VertexPose)，还包含速度节点(VertexVelocity)、陀螺仪偏置节点(VertexGyroBias)、加速度偏置节点(VertexAccBias)。
如下图所示，设置地图节点。
然后逐个遍历地图节点，进行节点添加。
单目情况：
双目情况：
广角模式的右相机：
与之前PoseInertialOptimizationLastFrame()函数不同的是，这里又多了一个关键帧节点。
之后，就是与PoseInertialOptimizationLastFrame()函数类似的，IMU预积分节点：
陀螺仪节点：
加速度计节点：
然后，开始执行4次优化：
单目观测情况：
双目观测情况：
优化之后，通过一系列操作，获得优化后的位姿以及内点个数。
本文作者原创,未经许可不得转载,谢谢配合
