Track是ORB-SLAM2跟踪线程的主循环入口函数它负责处理每一帧输入图像完成从状态管理、位姿估计、局部地图优化到关键帧决策的全链路流程。下面详细介绍该函数Track() 入口 │ ├─ [状态初始化] NO_IMAGES_YET - NOT_INITIALIZED │ ├─ [地图锁] 防止LocalMapping干扰 │ ├─ [初始化检查] │ ├─ 未初始化 - StereoInitialization() / MonocularInitialization() │ └─ 已初始化 - 进入跟踪 │ ├─ [粗略位姿估计] │ ├─ 正常模式 │ │ ├─ OK - CheckReplaced - (速度模型优先 - 参考帧回退) │ │ └─ LOST - 重定位 │ └─ 纯定位模式 │ ├─ LOST - 重定位 │ └─ OK - (mbVO ? 并行尝试 : 常规跟踪) │ ├─ [精细优化] 跟踪局部地图 TrackLocalMap (投影匹配 Motion BA) │ ├─ [状态更新] bOK - mStateOK / LOST │ ├─ [跟踪成功] │ ├─ 更新速度矩阵 (mVelocity) │ ├─ 清理临时点 │ ├─ 判断并创建新关键帧 │ └─ 剔除当前帧外点 │ ├─ [跟踪失败] 若地图过小则重置系统 │ └─ [轨迹记录] 存储 Tcr 用于绘图阶段一状态初始化与地图锁线程安全保护if(mStateNO_IMAGES_YET) mState NOT_INITIALIZED; unique_lockmutex lock(mpMap-mMutexMapUpdate);状态流转系统刚启动时mState为NO_IMAGES_YET第一条指令将其切换为未初始化状态。加锁目的mMutexMapUpdate是地图的互斥锁用于防止在跟踪当前帧的过程中局部建图线程LocalMapping突然插入新关键帧或删除旧地图点导致数据不一致。虽然是加锁但跟踪阶段主要是特征匹配和矩阵运算毫秒级对实时性影响微乎其微。阶段二系统引导双目/单目初始化详细步骤参考ORB_SLAM2单目初始化和双目初始化的区别-CSDN博客阶段三粗略位姿估计核心决策树这部分分为两个模式正常建图模式、纯定位模式1. 正常建图模式 (!mbOnlyTracking)状态OK首先调用CheckReplacedInLastFrame()检查上一帧的地图点是否被局部建图线程替换融合或优化保证指针有效性。if(mVelocity.empty() || mCurrentFrame.mnIdmnLastRelocFrameId2) { //使用关键帧追踪 bOK TrackReferenceKeyFrame(); } else { //使用运动模型追踪 bOK TrackWithMotionModel(); if(!bOK) bOK TrackReferenceKeyFrame(); }子条件含义触发场景mVelocity.empty()速度矩阵为空未初始化系统刚启动、刚完成初始化、或者上一帧跟踪失败导致速度未更新mCurrentFrame.mnId mnLastRelocFrameId 2当前帧的ID小于“最近一次重定位帧ID 2”刚完成重定位Relocalization后的前两帧运动模型追踪原理它假设相机在短时间内做匀速运动用上一帧的位姿和速度去预测当前帧的位姿。这个假设成立的前提是上一帧的位姿T_{cw}^{last}是准确可靠的。上一帧到当前帧的速度mVelocity是有效的。相机的运动确实是平滑且连续变化的当系统刚启动或者刚刚进行重定位时重定位意味之前的跟踪丢了不能使用匀速模型进行计算。状态Lost直接调用重定位Relocalization()利用词袋模型在整个地图中寻找相似关键帧通过EPnP求解位姿。2. 纯定位模式 (mbOnlyTracking)该模式不建图仅跟踪。代码引入了mbVOVisual Odometry标志表示当前帧与地图匹配的内点很少主要靠视觉里程计推算。false上一帧跟踪良好有足够多的地图点MapPoint匹配。true上一帧跟踪质量差主要靠临时点VO点勉强维持。状态Lost跟踪丢失只能进行重定位状态OK:上一帧状态良好优先使用运动模型速度无效使用参考关键帧追踪if(!mbVO) // 上一帧跟踪到了足够多的地图点状态良好 { // 优先尝试运动模型跟踪恒速模型 if(!mVelocity.empty()) { bOK TrackWithMotionModel(); // ⚠️ 这里缺少了失败后的回退机制 } else { // 速度无效直接用参考关键帧跟踪 bOK TrackReferenceKeyFrame(); } }上一帧状态不好使用匀速和重定位分别计算一遍然后结果择优录取先进行变量声明// 通过运动模型进行跟踪的结果 bool bOKMM false; // 通过重定位方法进行跟踪的结果 bool bOKReloc false; // 运动模型跟踪得到的临时结果用于后续回滚 vectorMapPoint* vpMPsMM; // 当前帧关联的地图点 vectorbool 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; imCurrentFrame.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为真执行以下重要维护更新运动模型速度供下一帧使用mVelocity mCurrentFrame.mTcw * LastTwc;这计算了当前帧相对于上一帧的变换矩阵作为下一帧匀速运动预测的依据。清理无效地图点删除观测次数为 0 的临时地图点释放内存。关键帧决策NeedNewKeyFrame()CreateNewKeyFrame()根据距离上一关键帧的帧数、局部建图线程是否空闲、跟踪质量等条件决定是否创建新关键帧。决策是多因子加权的结果无固定数学公式基于工程经验。剔除离群点将在TrackLocalMap优化中被标记为外点mvbOutlier[i]true的地图点从当前帧清掉防止错误点影响下一帧。阶段六失联恢复机制防止刚启动就崩溃if(mStateLOST) { 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)。