一、函数整体目的当匀速运动模型跟踪失败时例如运动剧烈或速度变化大系统会调用此函数尝试用上一帧对应的参考关键帧来重新定位当前帧。它通过词袋匹配找到当前帧与参考关键帧的3D-2D对应关系然后用PnP 优化计算位姿。二、详细步骤阶段一计算当前帧的BoW向量mCurrentFrame.ComputeBoW();作用将当前帧的ORB特征描述子转换为词袋向量原理 使用预训练的视觉词典为每个特征点映射到最近的“视觉单词”。统计单词出现频率计算TF-IDF权重目的加速后续的特征匹配只在于参考帧共享的单词中进行匹配。关于词袋向量的详细内容这里不多做介绍可参考ORB_SLAM2词袋模型 Bag of Words-CSDN博客阶段二创建 ORBmatcherORBmatcher matcher(0.7, true); vectorMapPoint* vpMapPointMatches; int nmatches matcher.SearchByBoW(mpReferenceKF, mCurrentFrame, vpMapPointMatches);ORBmatcher特征匹配器参数含义0.7描述子距离阈值比例最优匹配/次优匹配 0.7 才接受。true启用旋转一致性检查利用特征点方向直方图剔除旋转不一致的匹配SearchByBoW()输入参考关键帧mpReferenceKF和当前帧mCurrentFrame。输出vpMapPointMatches存储当前帧每个特征点对应的地图点。匹配逻辑获取两帧共有的视觉单词通过mFeatVec。对每个共有单词取两帧中对应的特征点列表。在这些候选点中计算描述子汉明距离选择最优匹配。返回匹配数量nmatches。阶段三检查匹配数量是否足够if(nmatches 15) return false;经验阈值15确保有足够的3D-2D对应点来求解PnP至少3对但15对更鲁棒。若匹配不足说明当前帧与参考关键帧差异大无法可靠估计位姿。阶段四初始化当前帧的位姿及地图点mCurrentFrame.mvpMapPoints vpMapPointMatches; mCurrentFrame.SetPose(mLastFrame.mTcw);赋值地图点将匹配到的地图点赋给当前帧的特征点。初始化位姿将当前帧的位姿设为上一帧的位姿作为优化初值。为什么用上一帧因为时间连续位姿变化小接近最优解能加速优化收敛。mLastFrame.mTcw是上一帧的世界→相机变换矩阵Tcw。阶段五执行位姿优化PnP优化ORB_SLAM2 Optimizer::PoseOptimization-CSDN博客Optimizer::PoseOptimization(mCurrentFrame);优化目标最小化重投影误差优化当前帧的位姿Tcw。优化变量仅优化相机的SE(3)位姿固定地图点位置。算法使用g2o图优化库进行LMLevenberg-Marquardt迭代优化。残差模型ei观测坐标i−投影(位姿×地图点i)重要特性优化过程中会自动标记外点outlier记录在mCurrentFrame.mvbOutlier[i]中。外点判定重投影误差超过阈值默认10像素的匹配点。阶段六剔除外点并统计有效匹配数如果有效内点地图点数 ≥ 10认为跟踪成功否则失败。阈值10保证至少有10个可靠的3D点支撑位姿估计避免退化情况。int nmatchesMap 0; for(int i 0; imCurrentFrame.N; i) { if(mCurrentFrame.mvpMapPoints[i]) { if(mCurrentFrame.mvbOutlier[i]) { MapPoint* pMP mCurrentFrame.mvpMapPoints[i]; mCurrentFrame.mvpMapPoints[i]static_castMapPoint*(NULL); mCurrentFrame.mvbOutlier[i]false; pMP-mbTrackInView false; pMP-mnLastFrameSeen mCurrentFrame.mnId; nmatches--; } else if(mCurrentFrame.mvpMapPoints[i]-Observations()0) nmatchesMap; } } return nmatchesMap10;