(SearchByProjection) 函数

1. 函数目标与核心思想

SearchByProjection 是 ORB-SLAM2 中用于连续帧间跟踪 (Frame-to-Frame Tracking) 的核心函数之一。

核心思想：利用上一帧 (LastFrame) 已经构建好的三维地图点 (MapPoint)，结合当前帧 (CurrentFrame) 的初始位姿估计，来预测这些三维点在当前帧图像上的二维投影位置。然后在这些投影位置附近的小范围内搜索，以找到对应的二维特征点，从而建立匹配关系。

这是一种基于模型的预测与搜索策略，比盲目搜索或纯二维匹配更高效和鲁棒。

2. 前提条件

  • LastFrame 中已经有一些特征点被成功三角化，并形成了稳定的 MapPoint（即已知其三维世界坐标）。
  • 对 CurrentFrame 的相机位姿 CurrentFrame.mTcw (从世界坐标系到当前相机坐标系的变换) 有一个初始的估计。这个估计可能来自：
    • 匀速运动模型。
    • 上一帧成功跟踪后的位姿。
    • 重定位成功后的位姿。

3. 主要执行步骤与思想

a. 准备工作与帧间运动估计 (Code Steps 1, 2)

  • 初始化旋转直方图 (rotHist) (Code Step 1):
    • 思想：用于后续的旋转一致性检查，以剔除外点。直方图统计匹配特征点对之间的主方向角度差。
  • 计算当前帧相对于上一帧的运动信息 (Code Step 2):
    • 思想：了解相机是前进、后退还是基本保持距离，对于后续指导特征搜索的尺度（金字塔层级）非常重要。
    • 通过两帧的位姿，计算出当前相机中心在上一帧相机坐标系下的平移向量 tlc。
    • 根据 tlc 的 Z 分量判断前进 (bForward) 或后退 (bBackward) 状态（主要针对非单目）。

b. 投影地图点并进行局部搜索 (Code Steps 3, 4)

  • 遍历 LastFrame 的有效地图点 (MapPoint) (Code Step 3):
    • 对于每个与 LastFrame 特征点关联的 MapPoint：
      1. 获取其三维世界坐标 x3Dw。
      2. 使用 CurrentFrame 的位姿 Rcw, tcw 将 x3Dw 变换到当前相机坐标系下，得到 x3Dc。
      3. 检查有效性：确保点在相机前方 (invzc > 0)。
      4. 投影：使用 CurrentFrame 的相机内参将 x3Dc 投影到图像平面，得到预测的二维像素坐标 (u, v)。
      5. 边界检查：确保 (u, v) 在图像有效范围内。
  • 在投影点附近使用 GetFeaturesInArea 进行局部搜索 (Code Step 4):
    • 思想：由于位姿估计、地图点定位等存在误差，真实匹配点会在投影位置附近。同时，物体由于相机远近移动，其在图像中的尺度会变化。
    • 确定搜索半径 radius：基于上一帧特征点的金字塔层级进行缩放 (th * CurrentFrame.mvScaleFactors[nLastOctave])。尺度大的特征（通常对应更远或更大的物体部分，或者在低分辨率金字塔层级检测到的特征），其不确定性可能更大，搜索半径也相应调整。
    • 自适应搜索金字塔层级：
      • 若相机前进 (bForward)：物体变大，当前帧特征可能在更高或相同层级。搜索范围 [nLastOctave, maxLevel]。
      • 若相机后退 (bBackward)：物体变小，当前帧特征可能在更低或相同层级。搜索范围 [0, nLastOctave]。
      • 若相机运动不明显：在上一帧层级附近搜索，如 [nLastOctave-1, nLastOctave+1]。
    • 调用 CurrentFrame.GetFeaturesInArea(u,v, radius, minLevel, maxLevel) 获得候选匹配特征点列表 vIndices2。

c. 描述子匹配与最佳选择 (Code Step 5)

  • 思想：在位置和尺度上接近的候选点中，通过特征描述子的相似性（外观）来确定最佳匹配。
  • 遍历 vIndices2 中的候选特征点：
    1. 排除已匹配点：若当前帧的候选特征点已有关联地图点，则跳过。
    2. （可选）双目/RGB-D 一致性检查：对右目图像或深度值进行额外约束。
    3. 计算描述子距离：计算 LastFrame 地图点 pMP 的描述子与当前帧候选特征点描述子之间的汉明距离 (DescriptorDistance)。
    4. 选择最佳匹配：在所有候选点中，选择汉明距离最小且小于阈值 TH_HIGH 的作为最佳匹配点 bestIdx2。

d. 存储匹配并更新旋转直方图 (Code Step 6)

  • 如果找到有效匹配：
    1. 在 CurrentFrame 中记录该匹配：CurrentFrame.mvpMapPoints[bestIdx2] = pMP。
    2. 增加成功匹配计数 nmatches。
    3. 若启用朝向检查 (mbCheckOrientation)：计算 LastFrame 特征点与 CurrentFrame 匹配特征点的主方向角度差，更新 rotHist 直方图。

e. 旋转一致性检查与外点剔除 (Code Step 7)

  • 思想：利用刚体场景下特征点旋转角度变化的一致性来剔除误匹配。
  • 分析 rotHist 直方图，找出票数最高的前三个主导旋转角度区间。
  • 遍历所有之前建立的匹配，如果某个匹配的角度差不在这三个主导区间内，则将其视作外点，并从当前帧的匹配中移除 (CurrentFrame.mvpMapPoints[rotHist[i][j]] = NULL)，同时减少 nmatches。

4. 函数优势

  • 高效性：通过投影预测，将搜索范围大大缩小，避免了全局或大范围的盲目搜索。
  • 鲁棒性：结合了位置预测、尺度自适应、外观（描述子）匹配以及几何（旋转）一致性等多重约束，能够有效地找到正确匹配并剔除外点。
  • 利用地图信息：充分利用了已建立的地图点信息，使得跟踪更为稳定。

此方法是视觉 SLAM 中实现稳定、高效帧间跟踪的关键技术之一。