ORB_SLAM2: Tracking::Track()

Track是ORB-SLAM2跟踪线程的主循环入口函数，它负责处理每一帧输入图像，完成从状态管理、位姿估计、局部地图优化到关键帧决策的全链路流程。下面详细介绍该函数：

Track() 入口 │ ├─> [状态初始化] NO_IMAGES_YET -> NOT_INITIALIZED │ ├─> [地图锁] 防止LocalMapping干扰 │ ├─> [初始化检查] │ ├─ 未初始化 -> StereoInitialization() / MonocularInitialization() │ └─ 已初始化 -> 进入跟踪 │ ├─> [粗略位姿估计] │ ├─ 正常模式： │ │ ├─ OK -> CheckReplaced -> (速度模型优先 -> 参考帧回退) │ │ └─ LOST -> 重定位 │ └─ 纯定位模式： │ ├─ LOST -> 重定位 │ └─ OK -> (mbVO ? 并行尝试 : 常规跟踪) │ ├─> [精细优化] 跟踪局部地图 TrackLocalMap (投影匹配 + Motion BA) │ ├─> [状态更新] bOK -> mState=OK / LOST │ ├─> [跟踪成功] │ ├─ 更新速度矩阵 (mVelocity) │ ├─ 清理临时点 │ ├─ 判断并创建新关键帧 │ └─ 剔除当前帧外点 │ ├─> [跟踪失败] 若地图过小则重置系统 │ └─> [轨迹记录] 存储 Tcr 用于绘图

阶段一：状态初始化与地图锁（线程安全保护）

if(mState==NO_IMAGES_YET) mState = NOT_INITIALIZED; unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

• 状态流转：系统刚启动时，mState为NO_IMAGES_YET，第一条指令将其切换为未初始化状态。

• 加锁目的：mMutexMapUpdate是地图的互斥锁，用于防止在跟踪当前帧的过程中，局部建图线程（LocalMapping）突然插入新关键帧或删除旧地图点，导致数据不一致。虽然是加锁，但跟踪阶段主要是特征匹配和矩阵运算（毫秒级），对实时性影响微乎其微。

阶段二：系统引导（双目/单目初始化）

详细步骤参考：ORB_SLAM2：单目初始化和双目初始化的区别-CSDN博客

阶段三：粗略位姿估计（核心决策树）

这部分分为两个模式：正常建图模式、纯定位模式

1. 正常建图模式 (!mbOnlyTracking)

状态OK：

首先调用CheckReplacedInLastFrame()：检查上一帧的地图点是否被局部建图线程替换（融合或优化），保证指针有效性。

if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2) { //使用关键帧追踪 bOK = TrackReferenceKeyFrame(); } else { //使用运动模型追踪 bOK = TrackWithMotionModel(); if(!bOK) bOK = TrackReferenceKeyFrame(); }

运动模型追踪原理：

它假设相机在短时间内做匀速运动，用上一帧的位姿和速度去预测当前帧的位姿。

这个假设成立的前提是：

• 上一帧的位姿T_{cw}^{last}是准确可靠的。

• 上一帧到当前帧的速度mVelocity是有效的。

• 相机的运动确实是平滑且连续变化的

当系统刚启动或者刚刚进行重定位时（重定位意味之前的跟踪丢了），不能使用匀速模型进行计算。

状态Lost：

直接调用重定位Relocalization()（利用词袋模型在整个地图中寻找相似关键帧，通过EPnP求解位姿）。

2. 纯定位模式 (mbOnlyTracking)

该模式不建图，仅跟踪。代码引入了mbVO（Visual Odometry）标志，表示当前帧与地图匹配的内点很少，主要靠视觉里程计推算。

• false：上一帧跟踪良好，有足够多的地图点（MapPoint）匹配。

• true：上一帧跟踪质量差，主要靠临时点（VO点）勉强维持。

状态Lost：

跟踪丢失，只能进行重定位

状态OK:

上一帧状态良好：

优先使用运动模型，速度无效使用参考关键帧追踪

if(!mbVO) // 上一帧跟踪到了足够多的地图点（状态良好） { // 优先尝试运动模型跟踪（恒速模型） if(!mVelocity.empty()) { bOK = TrackWithMotionModel(); // ⚠️ 这里缺少了失败后的回退机制！ } else { // 速度无效，直接用参考关键帧跟踪 bOK = TrackReferenceKeyFrame(); } }

上一帧状态不好：

使用匀速和重定位分别计算一遍，然后结果择优录取

先进行变量声明

// 通过运动模型进行跟踪的结果 bool bOKMM = false; // 通过重定位方法进行跟踪的结果 bool bOKReloc = false; // 运动模型跟踪得到的临时结果（用于后续回滚） vector<MapPoint*> vpMPsMM; // 当前帧关联的地图点 vector<bool> vbOutMM; // 每个特征点是否为外点 cv::Mat TcwMM; // 运动模型算出的位姿

如果速度非空，先用匀速模型预测当前位姿，并暂存结果到vpMPsMM 、vbOutMM 、TcwMM。

然后再使用重定位：

• 利用词袋模型（BoW）在当前帧和整个地图的所有关键帧之间快速搜索，找到最相似的候选帧。

• 通过EPnP（高效PnP算法）计算当前帧位姿，并用RANSAC筛选内点。

• 如果匹配到的内点数足够多（> 50 个），则重定位成功。

场景1：运动模型成功 + 重定位失败

if(bOKMM && !bOKReloc) { // 回退到运动模型的结果 mCurrentFrame.SetPose(TcwMM); mCurrentFrame.mvpMapPoints = vpMPsMM; mCurrentFrame.mvbOutlier = vbOutMM; // 增加地图点的被观测次数（提升其权重） if(mbVO) { for(int i =0; i<mCurrentFrame.N; i++) { if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i]) { mCurrentFrame.mvpMapPoints[i]->IncreaseFound(); } } } }

场景2：重定位成功（无论运动模型是否成功）

else if(bOKReloc) { // 只要重定位成功，就认为系统恢复正常 mbVO = false; }

阶段四：跟踪局部地图

if(!mbOnlyTracking) { if(bOK) bOK = TrackLocalMap(); } else { if(bOK && !mbVO) bOK = TrackLocalMap(); }

无论上一步哪种方法估计出位姿，只要bOK == true，都会进入此环节。

• 作用：上一步仅使用了上一帧或参考帧的少量地图点。这一步会将当前帧能看到的所有局部地图点（当前关键帧及其共视关键帧中的所有3D点）投影到当前帧进行匹配。

• 核心优化（仅运动BA）：
  固定所有地图点 Pi的坐标，仅优化当前帧位姿 Tcw，使重投影误差最小化：

其中 π(⋅) 是投影函数，ρ是Huber核函数（用于削弱外点影响）。

• 意义：这是提升跟踪精度的关键，使位姿与周围地图达到几何最优。

阶段五：跟踪成功后的善后工作（更新与决策）

如果bOK为真，执行以下重要维护：

1. 更新运动模型速度（供下一帧使用）：

mVelocity = mCurrentFrame.mTcw * LastTwc;

• 这计算了当前帧相对于上一帧的变换矩阵，作为下一帧匀速运动预测的依据。

• 清理无效地图点：删除观测次数为 0 的临时地图点，释放内存。

• 关键帧决策（NeedNewKeyFrame()+CreateNewKeyFrame()）：
  根据距离上一关键帧的帧数、局部建图线程是否空闲、跟踪质量等条件，决定是否创建新关键帧。决策是多因子加权的结果（无固定数学公式，基于工程经验）。

• 剔除离群点：将在TrackLocalMap优化中被标记为外点（mvbOutlier[i]=true）的地图点从当前帧清掉，防止错误点影响下一帧。

阶段六：失联恢复机制（防止刚启动就崩溃）

if(mState==LOST) { if(mpMap->KeyFramesInMap()<=5) { mpSystem->Reset(); return; } }

• 场景：如果系统刚初始化完（地图中仅有 ≤5 个关键帧），但因为剧烈运动或弱纹理瞬间丢失。

• 策略：此时地图太小，重定位极易失败，直接重置整个系统（推倒重来），比强行在微弱地图中挣扎更高效。

阶段七：轨迹记录（用于可视化与保存）

cv::Mat Tcr = mCurrentFrame.mTcw * mCurrentFrame.mpReferenceKF->GetPoseInverse(); mlRelativeFramePoses.push_back(Tcr);

• Tcr表示当前帧相对于其参考关键帧的位姿变换。这个向量序列被存储下来，用于在FrameDrawer或MapDrawer中绘制相机的实时运动轨迹。

• 即使跟踪丢失，也会用上一帧的数据填充（保持轨迹连续性，但标记为mlbLost.push_back(true)）。