企业官网建设流程全解析

一、函数整体目的

当匀速运动模型跟踪失败时（例如运动剧烈或速度变化大），系统会调用此函数，尝试用上一帧对应的参考关键帧来重新定位当前帧。它通过词袋匹配找到当前帧与参考关键帧的3D-2D对应关系，然后用PnP + 优化计算位姿。

二、详细步骤

阶段一：计算当前帧的BoW向量

mCurrentFrame.ComputeBoW();

作用：将当前帧的ORB特征描述子转换为词袋向量

原理： 使用预训练的视觉词典，为每个特征点映射到最近的“视觉单词”。

统计单词出现频率，计算TF-IDF权重

目的：加速后续的特征匹配，只在于参考帧共享的单词中进行匹配。

关于词袋向量的详细内容这里不多做介绍，可参考：ORB_SLAM2：词袋模型 Bag of Words-CSDN博客

阶段二：创建 ORBmatcher

ORBmatcher matcher(0.7, true); vector<MapPoint*> vpMapPointMatches; int nmatches = matcher.SearchByBoW(mpReferenceKF, mCurrentFrame, vpMapPointMatches);

ORBmatcher：特征匹配器，参数含义：

• 0.7：描述子距离阈值比例（最优匹配/次优匹配 < 0.7 才接受）。

• true：启用旋转一致性检查（利用特征点方向直方图，剔除旋转不一致的匹配）

SearchByBoW()：

• 输入：参考关键帧（mpReferenceKF）和当前帧（mCurrentFrame）。

• 输出：vpMapPointMatches存储当前帧每个特征点对应的地图点。

• 匹配逻辑：

  1. 获取两帧共有的视觉单词（通过mFeatVec）。

  2. 对每个共有单词，取两帧中对应的特征点列表。

  3. 在这些候选点中计算描述子汉明距离，选择最优匹配。

• 返回匹配数量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（图优化库）进行LM（Levenberg-Marquardt）迭代优化。

• 残差模型：

  ei=观测坐标i−投影(位姿×地图点i)

• 重要特性：优化过程中会自动标记外点（outlier），记录在mCurrentFrame.mvbOutlier[i]中。

  • 外点判定：重投影误差超过阈值（默认10像素）的匹配点。

阶段六：剔除外点并统计有效匹配数

• 如果有效内点地图点数 ≥ 10，认为跟踪成功，否则失败。

• 阈值10：保证至少有10个可靠的3D点支撑位姿估计，避免退化情况。

int nmatchesMap = 0; for(int i =0; i<mCurrentFrame.N; i++) { if(mCurrentFrame.mvpMapPoints[i]) { if(mCurrentFrame.mvbOutlier[i]) { MapPoint* pMP = mCurrentFrame.mvpMapPoints[i]; mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL); mCurrentFrame.mvbOutlier[i]=false; pMP->mbTrackInView = false; pMP->mnLastFrameSeen = mCurrentFrame.mnId; nmatches--; } else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) nmatchesMap++; } } return nmatchesMap>=10;