淦ORB

淦ORB

tracking干了啥事

• Tracking线程函数主要功能和流程
• • TrackWithMotionModel
  • TrackReferenceKeyFrame
  • Relocalization
  • TrackLocalMap
  • 跟踪过程整体流程图

tracking线程可以说是ORB_SLAM2中最重要的一部分，其主要完成两项工作（1）完成相机位姿估计（2）跟踪局部地图
tracking的代码非常的繁琐和复杂，所以我建议配合流程图来进行研读，效果是最好的，一边看流程一边看对应函数是如何实现的，能够帮你快速入手。（流程图是来自 这里）

可以看到上篇文章中提到的单目矩阵初始化也包含在了其中。

Tracking线程函数主要功能和流程

TrackWithMotionModel

按照恒速运动模型模式来进行Track，按照上一帧的速度与位姿作为初始，估计这次特征点的像素坐标，进行投影优化

1. 先通过上一帧的位姿和速度预测当前帧相机的位姿
2. 通过PnP方法估计相机位姿，在将上一帧的地图点投影到当前固定大小范围的帧平面上，如果匹配点少，那么扩大两倍的采点范围。
3. 然后进行一次BA算法，通过最小二乘法优化相机的位姿。
4. 优化位姿之后，对当前帧的关键点和地图点，抛弃无用的杂点，剩下的点供下一次操作使用。

应用场景
大部分时间都用这个跟踪，只利用到了上一帧的信息。

1. 用恒速模型先估计一个初始位姿
2. 用该位姿进行投影匹配 SearchByProjection，候选点来自GetFeaturesInArea，未使用BoW
3. BA优化（仅优化位姿），提供比较粗糙的位姿

思路
假设短时间内（相邻帧）物体处于匀速运动状态，可以用上一帧的位姿和速度来估计当前帧的位姿。
移动模式跟踪前后两帧 得到 变换矩阵。
上一帧的地图3d点反投影到当前帧图像像素坐标上，在不同尺度下不同的搜索半径内，做描述子匹配 搜索 可以加快匹配。
在投影点附近根据描述子距离进行匹配（需要>20对匹配，否则匀速模型跟踪失败,运动变化太大时会出现这种情况），然后以运动模型预测的位姿为初值，优化当前位姿，优化完成后再剔除外点，若剩余的匹配依然>=10对，则跟踪成功，否则跟踪失败，需要Relocalization
具体流程
1：创建 ORB特征点匹配器 最小距离 < 0.9次小距离 匹配成功
2：更新上一帧的位姿和地图点
单目：只计算了上一帧世界坐标系位姿就退出了。TlrTrw = Tlw
双目或rgbd相机：根据上一帧有效的深度值产生为上一帧生成新的临时地图点，之所以说是“临时”因为
这些新加的地图点不加入到Map中，只是为了当前帧间跟踪更稳定，用完会删除，过河拆桥啊！
3：使用当前的运动速度(之前前后两帧位姿变换)和上一帧的位姿来初始化 当前帧的位姿R,t
4：在当前帧和上一帧之间搜索匹配点（matcher.SearchByProjection)
通过投影(使用当前帧的位姿R,t)，对上一帧的特征点(地图点)进行跟踪.
上一帧3d点投影到当前坐标系下，在该2d点半径th范围内搜索可以匹配的匹配点
遍历可以匹配的点，计算描述子距离，记录最小的匹配距离，小于阈值的，再记录匹配点特征方向
差值
进行方向验证，剔除方向差直方图统计中，方向差值数量少的点对，保留前三个数量多的点对。
5：如果找到的匹配点对如果少于20，则扩大搜索半径th=2*th,使用SearchByProjection()再次进行搜
索。
6：使用匹配点对对当前帧的位姿进行优化 G2O图优化
7：如果2d-3d匹配效果差，被标记为外点，则当前帧2d点对于的3d点设置为空，留着以后再优化
8：根据内点的匹配数量，判断 跟踪上一帧是否成功。

TrackReferenceKeyFrame

按照关键帧来进行Track，从关键帧中查找Bow相近的帧，进行匹配优化位姿

1. 按照关键帧进行Track的方法和运动模式恢复相机运动位姿的方法接近。首先求解当前帧的BOW向量。
2. 再搜索当前帧和关键帧之间的关键点匹配关系，如果这个匹配关系小于15对的话，就Track失败了。
3. 接着讲当前帧的位置假定到上一帧的位置那里
4. 并通过最小二乘法优化相机的位姿。
5. 最后依然是抛弃无用的杂点，当match数大于等于10的时候，返回true成功。

应用场景：
没有速度信息的时候、刚完成重定位、或者恒速模型跟踪失败后使用，大部分时间不用。只利用到了参考帧的信息。

1. 匹配方法是 SearchByBoW，匹配当前帧和关键帧在同一节点下的特征点，不需要投影，速度很快.
2. BA优化（仅优化位姿），提供比较粗糙的位姿

思路:
当使用运动模式匹配到的特征点数较少时，就会选用关键帧模式跟踪。
思路是：尝试和最近一个关键帧去做匹配。为了快速匹配，利用了bag of words（BoW）来加速匹配
具体流程
1 .计算当前帧的BoW；
2 .通过特征点的bow加快当前帧和参考帧之间的特征点匹配。使用函数matcher.SearchByBoW()。对属于同一node（同一node才可能是匹配点）的特征点通过描述子距离进行匹配，遍历该node中特征点，特征点最小距离明显小于次小距离才作为成功匹配点，记录特征点对方向差统计到直方图
记录特征匹配成功后每个特征点对应的MapPoint（来自参考帧），用于后续3D-2D位姿优化，通过角度投票进行剔除误匹配
3 .将上一帧的位姿作为当前帧位姿的初始值（加速收敛），通过优化3D-2D的重投影误差来获得准确位姿。3D-2D来自第2步匹配成功的参考帧和当前帧，重投影误差 e = (u,v) - project(Tcw*Pw)，只优化位姿Tcw，不优化MapPoints的坐标。
顶点 Vertex: g2o::VertexSE3Expmap()，初始值为上一帧的Tcw
边 Edge（单目）: g2o::EdgeSE3ProjectXYZOnlyPose()，一元边 BaseUnaryEdge

• 顶点 Vertex：待优化当前帧的Tcw
• 测量值 measurement：MapPoint在当前帧中的二维位置(u,v)
• 误差信息矩阵 InfoMatrix: Eigen::Matrix2d::Identity()*invSigma2(与特征点所在的尺度有关)
  +附加信息： 相机内参数： e->fx fy cx cy
  3d点坐标 ： e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
  优化多次，根据边误差，更新2d-3d匹配质量内外点标记，当前帧设置优化后的位姿。

4 .剔除优化后的outlier地图点

Relocalization

重定位，从之前的关键帧中找出与当前帧之间拥有充足匹配点的候选帧，利用Ransac迭代，通过PnP求解位姿。

1. 先计算当前帧的BOW值，并从关键帧数据库中查找候选的匹配关键帧
2. 构建PnP求解器，标记杂点，准备好每个关键帧和当前帧的匹配点集
3. 用PnP算法求解位姿，进行若干次P4P Ransac迭代，并使用非线性最小二乘优化，直到发现一个有充足inliers支持的相机位置。
4. 返回成功或失败

应用场景
跟踪丢失的时候使用，很少使用。利用到了相似候选帧的信息。

1. 用BoW先找到与该帧相似的候选关键帧（函数DetectRelocalizationCandidates）
2. 遍历候选关键帧，用SearchByBoW快速匹配，
3. 匹配点足够的情况下用EPnP 计算位姿并取出其中内点做BA优化（仅优化位姿），
4. 如果优化完内点较少，通过关键帧投影生成新的匹配（函数SearchByProjection），
5. 对匹配结果再做BA优化（仅优化位姿）。
   思路
   当TrackWithMotionModel 和 TrackReferenceKeyFrame 都没有跟踪成功，位置丢失后，需要在之前的关键帧中匹配最相近的关键帧，进而求出位姿信息。
   使用当前帧的BoW特征映射，在关键帧数据库中寻找相似的候选关键帧，因为这里没有好的初始位姿信息，需要使用传统的3D-2D匹配点的EPnP算法来求解一个初始位姿，之后再使用最小化重投影误差来优化更新位姿。

具体流程
1：计算当前帧的BoW向量和Feature向量
2：在关键帧数据库中找到与当前帧相似的候选关键帧组
3：创建 ORB特征点匹配器 最小距离 < 0.75*次小距离 匹配成功。
ORBmatcher matcher(0.75,true);
4：遍历每一个候选关键帧使用BOW特征向量加速匹配，匹配太少的去掉，选择符合要求的候选关键帧
用其地图点为其创建pnp优化器
5：使用PnPsolver 位姿变换求解器,更加3d-2d匹配点
6点直接线性变换DLT,后使用QR分解得到 R,t, 或者使用(P3P)，3点平面匹配算法求解。
这里会结合 Ransac 随采样序列一致性算法，来提高求解的鲁棒性。
6：EPnP算法迭代估计姿态作为当前帧的初始位姿，使用最小化重投影误差BA算法来优化位姿
7：如果优化时记录的匹配点对内点数量少于50，想办法再增加匹配点数量：通过投影的方式对之前未
匹配的点进行3D-2D匹配，又给了一次重新做人的机会
8： 如果新增的数量加上之前的匹配点数量 大于50，再次使用 位姿优化算法进行优化
9：如果上面优化后的内点数量还比较少，还想挽留一下，就缩小搜索窗口重新投影匹配（比之前使用更多的地图点了），如果最后匹配次数大于50，就认为是可以勉强扶起来的阿斗，再给BA位优化一次。否则，放弃了（真的已经仁至义尽了！）
10：如果经过上面一系列的挽救操作，内点数量 大于等于50 ，则重定位成功。

TrackLocalMap

通过投影，从已经生成的地图点中找到更多的对应关系，精确结果

1. 更新Covisibility Graph， 更新局部关键帧
2. 根据局部关键帧，更新局部地图点，接下来运行过滤函数 isInFrustum
3. 将地图点投影到当前帧上，超出图像范围的舍弃
4. 当前视线方向v和地图点云平均视线方向n, 舍弃n*v<cos(60)的点云
5. 舍弃地图点到相机中心距离不在一定阈值内的点
6. 计算图像的尺度因子 isInFrustum 函数结束
7. 进行非线性最小二乘优化
8. 更新地图点的统计量

应用场景：
前面3种跟踪方式得到当前帧地图点后的后处理，每次跟踪都使用。前提是必须知道当前帧的位姿和地图点（尽管不准确），利用到了当前帧的两级共视关键帧的信息，使得位姿更加准确。
具体步骤：

1. 首先根据前面得到的当前帧的地图点来找能观测到当前帧的一级共视关键帧，将这些一级共视关键帧的二级关键共视帧、子关键帧、父关键帧一起作为局部关键帧;
2. 取出上述局部关键帧中所有的地图点作为局部地图点；
3. 将局部地图点投影到当前帧，去掉不在视野内的无效的地图点，剩下的局部地图点投影到当前帧进行匹配（函数SearchByProjection）
4. 对匹配结果再做BA优化（仅优化位姿）

跟踪过程整体流程图

淦ORB

tracking干了啥事

• Tracking线程函数主要功能和流程
• • TrackWithMotionModel
  • TrackReferenceKeyFrame
  • Relocalization
  • TrackLocalMap
  • 跟踪过程整体流程图

tracking线程可以说是ORB_SLAM2中最重要的一部分，其主要完成两项工作（1）完成相机位姿估计（2）跟踪局部地图
tracking的代码非常的繁琐和复杂，所以我建议配合流程图来进行研读，效果是最好的，一边看流程一边看对应函数是如何实现的，能够帮你快速入手。（流程图是来自 这里）

可以看到上篇文章中提到的单目矩阵初始化也包含在了其中。

Tracking线程函数主要功能和流程

TrackWithMotionModel

按照恒速运动模型模式来进行Track，按照上一帧的速度与位姿作为初始，估计这次特征点的像素坐标，进行投影优化

1. 先通过上一帧的位姿和速度预测当前帧相机的位姿
2. 通过PnP方法估计相机位姿，在将上一帧的地图点投影到当前固定大小范围的帧平面上，如果匹配点少，那么扩大两倍的采点范围。
3. 然后进行一次BA算法，通过最小二乘法优化相机的位姿。
4. 优化位姿之后，对当前帧的关键点和地图点，抛弃无用的杂点，剩下的点供下一次操作使用。

应用场景
大部分时间都用这个跟踪，只利用到了上一帧的信息。

1. 用恒速模型先估计一个初始位姿
2. 用该位姿进行投影匹配 SearchByProjection，候选点来自GetFeaturesInArea，未使用BoW
3. BA优化（仅优化位姿），提供比较粗糙的位姿

思路
假设短时间内（相邻帧）物体处于匀速运动状态，可以用上一帧的位姿和速度来估计当前帧的位姿。
移动模式跟踪前后两帧 得到 变换矩阵。
上一帧的地图3d点反投影到当前帧图像像素坐标上，在不同尺度下不同的搜索半径内，做描述子匹配 搜索 可以加快匹配。
在投影点附近根据描述子距离进行匹配（需要>20对匹配，否则匀速模型跟踪失败,运动变化太大时会出现这种情况），然后以运动模型预测的位姿为初值，优化当前位姿，优化完成后再剔除外点，若剩余的匹配依然>=10对，则跟踪成功，否则跟踪失败，需要Relocalization
具体流程
1：创建 ORB特征点匹配器 最小距离 < 0.9次小距离 匹配成功
2：更新上一帧的位姿和地图点
单目：只计算了上一帧世界坐标系位姿就退出了。TlrTrw = Tlw
双目或rgbd相机：根据上一帧有效的深度值产生为上一帧生成新的临时地图点，之所以说是“临时”因为
这些新加的地图点不加入到Map中，只是为了当前帧间跟踪更稳定，用完会删除，过河拆桥啊！
3：使用当前的运动速度(之前前后两帧位姿变换)和上一帧的位姿来初始化 当前帧的位姿R,t
4：在当前帧和上一帧之间搜索匹配点（matcher.SearchByProjection)
通过投影(使用当前帧的位姿R,t)，对上一帧的特征点(地图点)进行跟踪.
上一帧3d点投影到当前坐标系下，在该2d点半径th范围内搜索可以匹配的匹配点
遍历可以匹配的点，计算描述子距离，记录最小的匹配距离，小于阈值的，再记录匹配点特征方向
差值
进行方向验证，剔除方向差直方图统计中，方向差值数量少的点对，保留前三个数量多的点对。
5：如果找到的匹配点对如果少于20，则扩大搜索半径th=2*th,使用SearchByProjection()再次进行搜
索。
6：使用匹配点对对当前帧的位姿进行优化 G2O图优化
7：如果2d-3d匹配效果差，被标记为外点，则当前帧2d点对于的3d点设置为空，留着以后再优化
8：根据内点的匹配数量，判断 跟踪上一帧是否成功。

TrackReferenceKeyFrame

按照关键帧来进行Track，从关键帧中查找Bow相近的帧，进行匹配优化位姿

1. 按照关键帧进行Track的方法和运动模式恢复相机运动位姿的方法接近。首先求解当前帧的BOW向量。
2. 再搜索当前帧和关键帧之间的关键点匹配关系，如果这个匹配关系小于15对的话，就Track失败了。
3. 接着讲当前帧的位置假定到上一帧的位置那里
4. 并通过最小二乘法优化相机的位姿。
5. 最后依然是抛弃无用的杂点，当match数大于等于10的时候，返回true成功。

应用场景：
没有速度信息的时候、刚完成重定位、或者恒速模型跟踪失败后使用，大部分时间不用。只利用到了参考帧的信息。

1. 匹配方法是 SearchByBoW，匹配当前帧和关键帧在同一节点下的特征点，不需要投影，速度很快.
2. BA优化（仅优化位姿），提供比较粗糙的位姿

思路:
当使用运动模式匹配到的特征点数较少时，就会选用关键帧模式跟踪。
思路是：尝试和最近一个关键帧去做匹配。为了快速匹配，利用了bag of words（BoW）来加速匹配
具体流程
1 .计算当前帧的BoW；
2 .通过特征点的bow加快当前帧和参考帧之间的特征点匹配。使用函数matcher.SearchByBoW()。对属于同一node（同一node才可能是匹配点）的特征点通过描述子距离进行匹配，遍历该node中特征点，特征点最小距离明显小于次小距离才作为成功匹配点，记录特征点对方向差统计到直方图
记录特征匹配成功后每个特征点对应的MapPoint（来自参考帧），用于后续3D-2D位姿优化，通过角度投票进行剔除误匹配
3 .将上一帧的位姿作为当前帧位姿的初始值（加速收敛），通过优化3D-2D的重投影误差来获得准确位姿。3D-2D来自第2步匹配成功的参考帧和当前帧，重投影误差 e = (u,v) - project(Tcw*Pw)，只优化位姿Tcw，不优化MapPoints的坐标。
顶点 Vertex: g2o::VertexSE3Expmap()，初始值为上一帧的Tcw
边 Edge（单目）: g2o::EdgeSE3ProjectXYZOnlyPose()，一元边 BaseUnaryEdge

• 顶点 Vertex：待优化当前帧的Tcw
• 测量值 measurement：MapPoint在当前帧中的二维位置(u,v)
• 误差信息矩阵 InfoMatrix: Eigen::Matrix2d::Identity()*invSigma2(与特征点所在的尺度有关)
  +附加信息： 相机内参数： e->fx fy cx cy
  3d点坐标 ： e->Xw[0] Xw[1] Xw[2] 2d点对应的上一帧的3d点
  优化多次，根据边误差，更新2d-3d匹配质量内外点标记，当前帧设置优化后的位姿。

4 .剔除优化后的outlier地图点

Relocalization

重定位，从之前的关键帧中找出与当前帧之间拥有充足匹配点的候选帧，利用Ransac迭代，通过PnP求解位姿。

1. 先计算当前帧的BOW值，并从关键帧数据库中查找候选的匹配关键帧
2. 构建PnP求解器，标记杂点，准备好每个关键帧和当前帧的匹配点集
3. 用PnP算法求解位姿，进行若干次P4P Ransac迭代，并使用非线性最小二乘优化，直到发现一个有充足inliers支持的相机位置。
4. 返回成功或失败

应用场景
跟踪丢失的时候使用，很少使用。利用到了相似候选帧的信息。

1. 用BoW先找到与该帧相似的候选关键帧（函数DetectRelocalizationCandidates）
2. 遍历候选关键帧，用SearchByBoW快速匹配，
3. 匹配点足够的情况下用EPnP 计算位姿并取出其中内点做BA优化（仅优化位姿），
4. 如果优化完内点较少，通过关键帧投影生成新的匹配（函数SearchByProjection），
5. 对匹配结果再做BA优化（仅优化位姿）。
   思路
   当TrackWithMotionModel 和 TrackReferenceKeyFrame 都没有跟踪成功，位置丢失后，需要在之前的关键帧中匹配最相近的关键帧，进而求出位姿信息。
   使用当前帧的BoW特征映射，在关键帧数据库中寻找相似的候选关键帧，因为这里没有好的初始位姿信息，需要使用传统的3D-2D匹配点的EPnP算法来求解一个初始位姿，之后再使用最小化重投影误差来优化更新位姿。

具体流程
1：计算当前帧的BoW向量和Feature向量
2：在关键帧数据库中找到与当前帧相似的候选关键帧组
3：创建 ORB特征点匹配器 最小距离 < 0.75*次小距离 匹配成功。
ORBmatcher matcher(0.75,true);
4：遍历每一个候选关键帧使用BOW特征向量加速匹配，匹配太少的去掉，选择符合要求的候选关键帧
用其地图点为其创建pnp优化器
5：使用PnPsolver 位姿变换求解器,更加3d-2d匹配点
6点直接线性变换DLT,后使用QR分解得到 R,t, 或者使用(P3P)，3点平面匹配算法求解。
这里会结合 Ransac 随采样序列一致性算法，来提高求解的鲁棒性。
6：EPnP算法迭代估计姿态作为当前帧的初始位姿，使用最小化重投影误差BA算法来优化位姿
7：如果优化时记录的匹配点对内点数量少于50，想办法再增加匹配点数量：通过投影的方式对之前未
匹配的点进行3D-2D匹配，又给了一次重新做人的机会
8： 如果新增的数量加上之前的匹配点数量 大于50，再次使用 位姿优化算法进行优化
9：如果上面优化后的内点数量还比较少，还想挽留一下，就缩小搜索窗口重新投影匹配（比之前使用更多的地图点了），如果最后匹配次数大于50，就认为是可以勉强扶起来的阿斗，再给BA位优化一次。否则，放弃了（真的已经仁至义尽了！）
10：如果经过上面一系列的挽救操作，内点数量 大于等于50 ，则重定位成功。

TrackLocalMap

通过投影，从已经生成的地图点中找到更多的对应关系，精确结果

1. 更新Covisibility Graph， 更新局部关键帧
2. 根据局部关键帧，更新局部地图点，接下来运行过滤函数 isInFrustum
3. 将地图点投影到当前帧上，超出图像范围的舍弃
4. 当前视线方向v和地图点云平均视线方向n, 舍弃n*v<cos(60)的点云
5. 舍弃地图点到相机中心距离不在一定阈值内的点
6. 计算图像的尺度因子 isInFrustum 函数结束
7. 进行非线性最小二乘优化
8. 更新地图点的统计量

应用场景：
前面3种跟踪方式得到当前帧地图点后的后处理，每次跟踪都使用。前提是必须知道当前帧的位姿和地图点（尽管不准确），利用到了当前帧的两级共视关键帧的信息，使得位姿更加准确。
具体步骤：

1. 首先根据前面得到的当前帧的地图点来找能观测到当前帧的一级共视关键帧，将这些一级共视关键帧的二级关键共视帧、子关键帧、父关键帧一起作为局部关键帧;
2. 取出上述局部关键帧中所有的地图点作为局部地图点；
3. 将局部地图点投影到当前帧，去掉不在视野内的无效的地图点，剩下的局部地图点投影到当前帧进行匹配（函数SearchByProjection）
4. 对匹配结果再做BA优化（仅优化位姿）

跟踪过程整体流程图