ORB-SLAM2系列第六章—— 跟踪线程
系列文章目录
ORB-SLAM2系列第一章—— 简介
ORB-SLAM2系列第二章—— ORB 特征点提取
ORB-SLAM2系列第三章—— 地图初始化
ORB-SLAM2系列第四章—— 地图点、关键帧、图结构
ORB-SLAM2系列第五章—— 特征匹配
ORB-SLAM2系列第六章—— 跟踪线程
文章目录
- 系列文章目录
- 前言
- 一、跟踪线程整体流程
- 二、参考关键帧跟踪
-
- 1.应用场景
- 2.思路
- 3.具体流程
- 三、恒速模型跟踪
-
- 1.应用场景
- 2.思想
- 3.具体流程
- 四、重定位跟踪
-
- 1.应用场景
- 2.思想
- 3.具体流程
- 四、局部地图跟踪
-
- 1.应用场景
- 2.具体流程
前言
迎浏览我的SLAM专栏，包括slam安装运行、代码注释、原理详解，一起加油淦穿SLAM。
一、跟踪线程整体流程
ORB-SLAM2跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。
二、参考关键帧跟踪
对应函数 tracking::TrackReferenceKeyFrame()
1.应用场景
没有速度信息的时候、刚完成重定位、或者恒速模型跟踪失败后使用，大部分时间不用。只利用到了参考帧的信息。
- 匹配方法是 SearchByBoW，匹配当前帧和关键帧在同一节点下的特征点，不需要投影，速度很快
- BA优化（仅优化位姿），提供比较粗糙的位姿
2.思路
当使用恒速模型匹配到的特征点数较少时，就会选用关键帧模式跟踪。
思路是：尝试和最近一个关键帧去做匹配。为了快速匹配，利用了bag of words（BoW）来加速匹配。
3.具体流程
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地图点
lower_bound( begin,end,num)：从数组的begin位置到end-1位置二分查找第一个大于或等于num的数字，找到返回该数字的地址，不存在
则返回end。通过返回的地址减去起始地址begin,得到找到数字在数组中的下标。
upper_bound( begin,end,num)：从数组的begin位置到end-1位置二分查找第一个大于num的数字，找到返回该数字的地址，不存在则返
回end。通过返回的地址减去起始地址begin,得到找到数字在数组中的下标。
例子：http://www.cplusplus.com/reference/algorithm/lower_bound/
三、恒速模型跟踪
对应函数Tracking::TrackWithMotionModel()，示意图如下：
1.应用场景
大部分时间都用这个跟踪，只利用到了上一帧的信息。
- 用恒速模型先估计一个初始位姿
- 用该位姿进行投影匹配 SearchByProjection，候选点来自GetFeaturesInArea，未使用BoW
- BA优化（仅优化位姿），提供比较粗糙的位姿
2.思想
假设短时间内（相邻帧）物体处于匀速运动状态，可以用上一帧的位姿和速度来估计当前帧的位姿。
移动模式跟踪 跟踪前后两帧 得到 变换矩阵。
上一帧的地图3d点反投影到当前帧图像像素坐标上，在不同尺度下不同的搜索半径内，做描述子匹配 搜索 可以加快匹配。
在投影点附近根据描述子距离进行匹配（需要>20对匹配，否则匀速模型跟踪失败,运动变化太大时会出现这种情况），然后以运动模型预测
的位姿为初值，优化当前位姿，
优化完成后再剔除外点，若剩余的匹配依然>=10对，则跟踪成功，否则跟踪失败，需要Relocalization。
3.具体流程
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：根据内点的匹配数量，判断 跟踪上一帧是否成功。
坐标系变换
推导 tlc = Rlw*twc+tlw
四、重定位跟踪
对应函数Tracking::Relocalization()
1.应用场景
跟踪丢失的时候使用，很少使用。利用到了相似候选帧的信息。
- 用BoW先找到与该帧相似的候选关键帧（函数DetectRelocalizationCandidates）
- 遍历候选关键帧，用SearchByBoW快速匹配，
- 匹配点足够的情况下用EPnP 计算位姿并取出其中内点做BA优化（仅优化位姿），
- 如果优化完内点较少，通过关键帧投影生成新的匹配（函数SearchByProjection），
- 对匹配结果再做BA优化（仅优化位姿）。
2.思想
当TrackWithMotionModel 和 TrackReferenceKeyFrame 都没有跟踪成功，位置丢失后，需要在之前的关键帧中匹配最相近的关键帧，进
而求出位姿信息。
使用当前帧的BoW特征映射，在关键帧数据库中寻找相似的候选关键帧，因为这里没有好的初始位姿信息，需要使用传统的3D-2D匹配点的
EPnP算法来求解一个初始位姿，之后再使用最小化重投影误差来优化更新位姿。
3.具体流程
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 ，则重定位成功。
检测重定位候选关键帧
对应函数 DetectRelocalizationCandidates
EPnP 算法原理详解（很复杂，略过了）
四、局部地图跟踪
对应函数tracking::TrackLocalMap
1.应用场景
前面3种跟踪方式得到当前帧地图点后的后处理，每次跟踪都使用。前提是必须知道当前帧的位姿和地图点（尽管不准确），利
用到了当前帧的两级共视关键帧的信息，使得位姿更加准确。
2.具体流程
- 首先根据前面得到的当前帧的地图点来找能观测到当前帧的一级共视关键帧，将这些一级共视关键帧的二级关键共视帧、子关键帧、父
关键帧一起作为局部关键帧; - 取出上述局部关键帧中所有的地图点作为局部地图点；
- 将局部地图点投影到当前帧，去掉不在视野内的无效的地图点，剩下的局部地图点投影到当前帧进行匹配（函数SearchByProjection）
- 对匹配结果再做BA优化（仅优化位姿）
当前帧：mCurrentFrame（当前帧是普通帧）
参考关键帧: 与当前帧共视程度最高的关键帧作为参考关键帧，mCurrentFrame.mpReferenceKF
在KeyFrame::UpdateConnections() 里确定关键帧的父子关系（当前帧必须是关键帧）
父关键帧：和当前关键帧共视程度最高的关键帧
子关键帧：是上述父关键帧的子关键帧
mvpLocalMapPoints 示意图
就是下图中红色地图点，用来在TrackLocalMap里跟踪当前帧
mvpLocalKeyFrames示意图
