Relocalization

一、核心目标与触发时机

  • 核心目标： 当系统由于各种原因（如快速运动、场景剧变、遮挡等）导致跟踪丢失，即无法确定当前相机帧相对于已有地图的位姿时，重定位模块的目标是在已构建的地图中重新找回当前帧的准确位姿。
  • 触发时机： 这是 SLAM 系统中的一种**“拯救”或“恢复”机制**。它在常规的、依赖连续性的跟踪方法（如恒速模型跟踪、参考关键帧跟踪）均宣告失败后被激活。

二、核心思想：全局搜索与局部验证

重定位的整个过程可以概括为两个主要阶段，体现了从粗略的全局匹配到精确的局部验证的思想：

  1. 全局外观搜索 (Global Appearance-Based Search)：
    • 由于跟踪已经丢失，系统对当前帧的位姿没有任何先验信息或良好的初始估计。
    • 策略： 利用词袋模型 (Bag-of-Words, BoW) 在整个历史关键帧数据库 (KeyFrame Database) 中进行快速、大规模的搜索。
    • 目的： 找出那些在视觉外观上与当前丢失帧最相似的候选关键帧 (Candidate KeyFrames)。这些候选帧是潜在的、当前相机可能重新观察到的场景。
  2. 局部几何验证与位姿精化 (Local Geometric Verification & Pose Refinement)：
    • 对筛选出的每一个候选关键帧，进行更细致的局部处理。
    • 策略：
      • 特征匹配： 在当前帧和候选关键帧之间进行ORB特征点的匹配。
      • 初始位姿估计： 利用匹配上的3D地图点（来自候选关键帧）和当前帧对应的2D特征点，通过 EPnP (Efficient Perspective-n-Point) 算法结合RANSAC（随机采样一致性）来估计一个初始的、相对准确的当前帧位姿。EPnP是一种高效的从n对3D-2D点对应中恢复相机位姿的算法。
      • 迭代优化： 获得初始位姿后，通过反复的投影匹配（将候选关键帧中更多的地图点根据当前估计位姿投影到当前帧，以寻找新的匹配）和BA优化 (Bundle Adjustment)（此阶段通常仅优化当前帧的位姿，不改变地图点坐标）来迭代地增加可靠的匹配点数量，并进一步精化当前帧的位姿。

三、关键技术与详细流程

A. 倒排索引 (Inverted Index) - 加速候选关键帧搜索

为了避免在可能包含成百上千个关键帧的数据库中进行暴力比对（这在实时系统中是不可接受的），ORB-SLAM2 采用倒排索引来高效地完成全局外观搜索。

  • 定义与对比：

    • 倒排索引 (Inverted Index)：
      • 索引基础： 以“视觉单词 (Word)”（词袋模型中的基本单元）为索引。
      • 存储内容： 对于词袋模型词典中的每一个视觉单词，倒排索引存储一个列表，该列表记录了所有包含这个特定视觉单词的关键帧的标识（如指针或ID）以及可能的权重信息。
      • 核心优势： 可以非常快速地查询到“有哪些关键帧包含了某个（或某组）特定的视觉单词”。这对于判断图像间的视觉相似性（即共享的视觉单词数量和权重）至关重要且效率极高。
    • 直接索引 (Direct Index)：
      • 索引基础： 以“图像 (KeyFrame)”为索引。
      • 存储内容： 每张图像（关键帧）存储其提取到的所有特征点，以及每个特征点在词袋树中所对应的视觉单词（节点ID）。
      • 核心优势： 能够快速获取一个关键帧中属于同一个视觉单词（或词袋树节点）下的所有特征点，这在进行两帧之间的特征匹配和后续的几何关系验证时非常有用。

  • 倒排索引在ORB-SLAM2中的数据结构示例：

    1
    2
    3
    // mvInvertedFile 是一个向量，其索引是 WordId (视觉单词的ID)
    // mvInvertedFile[word_id] 是一个链表，存储了所有包含 word_id 这个视觉单词的关键帧指针
    std::vector<std::list<KeyFrame*>> mvInvertedFile;

  • 倒排索引的维护：

    • 添加新关键帧时 (KeyFrameDatabase::add(KeyFrame *pKF))：
      1. 获取新关键帧 pKF 的词袋向量 pKF->mBowVec（该向量记录了关键帧中包含的所有视觉单词及其权重）。
      2. 遍历此BoW向量中的每一个视觉单词 (WordId, WordValue)。
      3. 对于每一个WordId，将该关键帧 pKF 的指针添加到 mvInvertedFile[WordId] 对应的链表中。
    • 删除关键帧时 (KeyFrameDatabase::erase(KeyFrame* pKF))：
      1. 获取待删除关键帧 pKF 的词袋向量。
      2. 遍历此BoW向量中的每一个视觉单词.
      3. 对于每一个WordId，访问 mvInvertedFile[WordId] 对应的链表，并从中找到并移除关键帧 pKF 的指针。

B. 搜索重定位候选关键帧 (KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F))

此函数的目标是从整个关键帧数据库中，高效地筛选出一小组与当前丢失帧 F 最可能在视觉上匹配的候选关键帧。

  1. 步骤 1：初步筛选 - 找出共享单词的关键帧 (lKFsSharingWords)
    • 遍历当前帧 F 的BoW向量 F->mBowVec 中的每一个视觉单词 (WordId)。
    • 利用倒排索引 mvInvertedFile[WordId]，获取所有包含该WordId的历史关键帧列表。
    • 将这些历史关键帧加入到一个临时的集合 lKFsSharingWords 中。
    • 为每个加入到 lKFsSharingWords 的关键帧 pKFi，用 pKFi->mnRelocWords++ 记录它与当前帧 F 共享的单词数量。
    • 使用 pKFi->mnRelocQuery = F->mnId 作为标记，确保在处理当前帧 F 的不同单词时，同一个历史关键帧 pKFi 只被加入 lKFsSharingWords 一次，并且其 mnRelocWords 能被正确累计。
    • 如果 lKFsSharingWords 在处理完当前帧所有单词后仍为空（即当前帧与数据库中所有关键帧都没有共同的视觉单词），则无法进行重定位，函数返回空列表。
  2. 步骤 2：设定共同单词数阈值1 (minCommonWords)
    • 遍历 lKFsSharingWords 中的所有关键帧。
    • 找到其中与当前帧 F 共享单词数最多的那个值，记为 maxCommonWords。
    • 设定第一个筛选阈值：minCommonWords = maxCommonWords * 0.8f (例如，取最大共享单词数的80%)。
    • 目的： 过滤掉那些虽然与当前帧有共同单词，但共同数量过少的关键帧，减少后续计算量。
  3. 步骤 3：计算BoW相似度得分并进行二次筛选 (lScoreAndMatch)
    • 再次遍历 lKFsSharingWords。
    • 只挑选那些共享单词数 pKFi->mnRelocWords > minCommonWords 的关键帧 pKFi。
    • 对于这些通过了阈值1的关键帧，计算它们各自的BoW向量与当前帧 F 的BoW向量之间的相似度得分 si。这个得分通常由词袋库提供，例如 si = mpVoc->score(F->mBowVec, pKFi->mBowVec);。
    • 将计算得到的得分 si 存入关键帧对象中，如 pKFi->mRelocScore = si;。
    • 将 (si, pKFi) 这样一个得分和关键帧指针的配对存入一个新的列表 lScoreAndMatch。
    • 如果 lScoreAndMatch 在此步骤后为空，则返回空列表。
  4. 步骤 4：计算共视组累积得分并设定阈值2 (minScoreToRetain)
    • 核心思想： 单个关键帧与当前帧的视觉相似度可能存在偶然性。如果一个候选关键帧及其在地图中共视关系紧密的“邻居”们（共同构成一个稳定的场景区域）都与当前帧相似，那么这个区域就更可能是当前相机真实观察到的场景。这增加了重定位的鲁棒性。
    • 遍历 lScoreAndMatch 中的每一个配对 (score, pKFi)：
      • 令 pKFi 为当前考虑的中心候选关键帧。
      • 获取 pKFi 在共视图中连接最紧密的一组关键帧（例如，通过 pKFi->GetBestCovisibilityKeyFrames(10) 获取共视程度最高的10个邻居关键帧 vpNeighs）。
      • 计算这个以 pKFi 为核心的“共视组”的累积得分 accScore：
        • 初始 accScore = pKFi->mRelocScore (即 score from lScoreAndMatch)。
        • 遍历 vpNeighs 中的每一个邻居关键帧 pKF2。
        • 重要条件： 只有当这个邻居 pKF2 也存在于 lScoreAndMatch 中（即 pKF2->mnRelocQuery == F->mnId，意味着它也通过了步骤3的筛选，与当前帧F有足够的直接相似性）时，才将其自身的得分 pKF2->mRelocScore 加入到 accScore 中。
      • 在计算 accScore 的同时，记录这个共视组中，拥有最高个体BoW得分的关键帧及其得分（设为 pBestKFInGroup 和 bestScoreInGroup）。初始时 pBestKFInGroup = pKFi, bestScoreInGroup = pKFi->mRelocScore。
      • 将 (accScore, pBestKFInGroup) 存入一个新的列表 lAccScoreAndMatch。
      • 在遍历所有 lScoreAndMatch 中的 pKFi 后，记录所有共视组中出现的最高累积得分 bestAccScore。
    • 设定第二个筛选阈值：minScoreToRetain = 0.75f * bestAccScore (例如，取最高累积得分的75%)。
  5. 步骤 5：筛选最终候选关键帧组 (vpRelocCandidates)
    • 遍历 lAccScoreAndMatch 中的每一个配对 (accScore, pBestKFInGroup)。
    • 只选择那些其累积得分 accScore > minScoreToRetain 的组。
    • 对于每个通过此阈值的组，将其对应的那个具有最高个体得分的关键帧 pBestKFInGroup 加入到最终的候选关键帧列表 vpRelocCandidates 中。
    • 使用 std::set<KeyFrame*> 来辅助去重，确保同一个关键帧不会被重复添加到 vpRelocCandidates。
    • 返回 vpRelocCandidates。这个列表中的关键帧将是接下来进行实际PnP求解和位姿优化的对象。

C. 执行重定位尝试 (Tracking::Relocalization())

在获得了经过层层筛选的候选关键帧列表后，系统将逐个尝试用它们来恢复当前丢失帧 mCurrentFrame 的位姿。

  1. 步骤 1：计算当前帧的BoW向量。
    • mCurrentFrame.ComputeBoW(); (如果尚未计算)。
  2. 步骤 2：获取候选关键帧列表。
    • 调用 mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame) 得到 vpCandidateKFs。
    • 如果 vpCandidateKFs 为空，则表示没有找到合适的候选，重定位失败，函数返回 false。
  3. 步骤 3：初始化 - 遍历候选，进行初步BoW匹配，准备PnP求解器。
    • 对 vpCandidateKFs 中的每个候选关键帧 pKF：
      • 检查 pKF 是否有效 (例如，pKF->isBad())，无效则跳过。
      • BoW特征匹配： 调用 matcher.SearchByBoW(pKF, mCurrentFrame, vvpMapPointMatches[i])。vvpMapPointMatches[i] 是一个 std::vector<MapPoint*>，存储了当前帧 mCurrentFrame 的特征点与候选关键帧 pKF 的地图点之间的匹配关系。函数返回匹配上的点对数量 nmatches。
      • 初步过滤： 如果 nmatches < 15 (阈值，表示匹配太少)，则认为此候选关键帧不足以进行可靠的位姿估计，将其标记为已丢弃 vbDiscarded[i] = true;，然后继续处理下一个候选。
      • 创建PnP求解器： 如果 nmatches >= 15，则为此候选关键帧创建一个 PnPsolver 实例（ORB-SLAM2中使用的是EPnP算法的实现）。这个求解器会与当前帧 mCurrentFrame 以及刚获得的匹配 vvpMapPointMatches[i] 相关联。同时设置PnP求解器所需的RANSAC参数，如：
        • probability = 0.99 (RANSAC成功概率)
        • minInliers = 10 (RANSAC迭代中，接受一个解所需的最小内点数)
        • maxIterations = 300 (RANSAC最大迭代次数)
        • minSet = 4 (EPnP求解一次位姿所需的最少点对数)
        • epsilon = 0.5 (RANSAC内点比例，用于提前终止)
        • th2 = 5.991 (卡方检验阈值，用于判断一个点是否为内点，会根据特征点金字塔层级调整)
      • 记录有效的候选PnP求解器数量 nCandidates++。
  4. 步骤 4：核心循环 - 迭代尝试PnP求解、位姿优化和投影匹配。
    • 此步骤在一个 while (nCandidates > 0 && !bMatch) 的循环中进行。bMatch 标志位表示是否已有一个候选关键帧成功重定位了当前帧。
    • 在循环中，再次遍历所有未被标记为 vbDiscarded[i] 的候选关键帧及其对应的 PnPsolver vpPnPsolvers[i]：
      • a. EPnP迭代求解初始位姿：
        • 调用 PnPsolver* pSolver = vpPnPsolvers[i];
        • cv::Mat Tcw = pSolver->iterate(5, bNoMore, vbInliers, nInliers);
          • iterate(5, ...): 尝试进行最多5轮RANSAC迭代（或者直到内部判断已充分探索）。
          • bNoMore: 输出参数，如果为 true，表示此 PnPsolver 的RANSAC迭代已达到最大次数或无法找到更多解，应丢弃此候选。
          • vbInliers: 输出参数，一个 std::vector<bool>，标记了EPnP求解后哪些初始匹配点被认为是内点。
          • nInliers: 输出参数，内点的数量。
          • Tcw: 如果求解成功，返回估计的从世界到当前相机的位姿变换矩阵。
        • 如果 bNoMore 为 true，则 vbDiscarded[i] = true; nCandidates--; 并继续处理下一个候选。
      • b. 首次位姿优化 (BA) - 如果EPnP成功：
        • 如果 !Tcw.empty() (EPnP求解得到一个位姿)：
          • 将求解得到的 Tcw 赋给当前帧：Tcw.copyTo(mCurrentFrame.mTcw);。
          • 根据 vbInliers，将EPnP认为是内点的那些匹配（即 vvpMapPointMatches[i][j]）关联到当前帧的地图点列表 mCurrentFrame.mvpMapPoints[j]。其他未标记为内点的，在当前帧中对应位置设为 NULL。
          • 调用 int nGood = Optimizer::PoseOptimization(&mCurrentFrame);。这是一个仅优化位姿的BA，它使用当前帧 mCurrentFrame 中所有已关联的地图点来优化其位姿 mCurrentFrame.mTcw。函数返回优化后的内点数量 nGood。
          • 初步判断： 如果 nGood < 10 (优化后内点数太少)，说明当前EPnP解引导的BA效果不佳。此时不会立即丢弃该候选帧 (vbDiscarded[i] 不变)，而是 continue; 跳过后续的投影匹配等步骤，直接尝试此候选帧的下一次EPnP迭代（如果 pSolver->iterate 允许）或下一个候选帧。
          • 清除BA优化过程中标记为外点 (mCurrentFrame.mvbOutlier[io]) 的地图点在当前帧的关联 (设为 NULL)。
      • c. 首次投影匹配 (尝试增加匹配点 - Rescue Attempt 1) - 如果首次BA内点数不足：
        • 如果 nGood < 50 (例如，阈值设为50，表示首次BA后的内点数仍不够理想，但有挽救价值)：
          • sFound: 一个 std::set<MapPoint*>，包含当前 mCurrentFrame.mvpMapPoints 中所有非空的地图点（即当前已知的内点）。
          • int nadditional = matcher2.SearchByProjection(mCurrentFrame, vpCandidateKFs[i], sFound, 10, 100);
            • mCurrentFrame: 当前帧，其位姿是刚通过首次BA优化过的。
            • vpCandidateKFs[i]: 当前正在处理的候选关键帧。
            • sFound: 告诉 SearchByProjection 不要重复匹配这些已经找到的点。
            • 10: 投影搜索窗口大小（像素）。
            • 100: 可能是对地图点深度或与当前帧的距离施加的限制，或者是描述子匹配的某种阈值参数。
          • 此函数的作用是：将候选关键帧 vpCandidateKFs[i] 中那些尚未与当前帧匹配上的地图点，根据当前帧已优化的位姿 mCurrentFrame.mTcw，投影到当前帧的图像平面上。然后在投影点附近搜索当前帧的特征点，如果找到匹配，就将这个新的3D-2D对应关系添加到 mCurrentFrame 中。nadditional 是新增加的匹配点数量。
      • d. 第二次位姿优化 (BA) - 如果首次投影匹配有效：
        • 如果 nadditional + nGood >= 50 (即首次投影匹配增加了足够的点，使得总数有望达标)：
          • 再次调用 nGood = Optimizer::PoseOptimization(&mCurrentFrame); 使用更多的匹配点（原有的 nGood + 新增的 nadditional）来优化位姿。
      • e. 第二次投影匹配 (更精细地增加匹配点 - Rescue Attempt 2) - 如果二次BA后仍有希望：
        • 如果第二次BA优化后，内点数 nGood 仍未达到最终目标50，但处于一个有希望的区间，例如 30 < nGood < 50：
          • 这表明当前帧的位姿可能因为上一步的BA而变得更准确了。值得用更严格的参数再尝试一次投影匹配，以期找到更多高质量的匹配。
          • 清空 sFound 并重新加入当前所有内点。
          • nadditional = matcher2.SearchByProjection(mCurrentFrame, vpCandidateKFs[i], sFound, 3, 64);
            • 3: 使用更小的投影搜索窗口（更精确的位姿允许更小的搜索范围）。
            • 64: 可能是更严格的描述子匹配阈值（例如，ORB描述子汉明距离上限）。
      • f. 第三次位姿优化 (BA) - 如果第二次投影匹配有效：
        • 如果 nGood + nadditional >= 50 (即第二次投影匹配使得总数达标)：
          • 最后进行一次 nGood = Optimizer::PoseOptimization(&mCurrentFrame);。
          • 清除优化后的外点。
      • g. 判断当前候选关键帧是否成功重定位：
        • 如果在上述 任何一次BA优化 (b, d, 或 f) 之后，内点数量 nGood >= 50 (阈值，表示找到了足够多的可靠匹配)：
          • 则认为使用当前的候选关键帧 vpCandidateKFs[i] 已经成功地重定位了当前帧 mCurrentFrame。
          • 设置 bMatch = true;。
          • break; 跳出当前对所有候选关键帧的遍历循环（即for(int i=0; i<nKFs; i++)循环）。一旦有一个候选成功，就不再考虑其他候选了。
  5. 步骤 5：返回最终结果。
    • 当 while 循环结束（因为 nCandidates <= 0 或 bMatch == true）：
      • 如果 !bMatch，则表示尝试了所有（或所有有希望的）候选关键帧后，均未能成功重定位。函数返回 false。
      • 如果 bMatch 为 true，则重定位成功。
        • 记录当前成功重定位的帧ID：mnLastRelocFrameId = mCurrentFrame.mnId; (这可以用于防止系统在短时间内对同一帧反复进行不必要的重定位尝试)。
        • 函数返回 true。当前帧 mCurrentFrame 的位姿 mTcw 和关联的地图点 mvpMapPoints 已经被更新。

四、重定位效果与重要性

  • 鲁棒性强： 重定位模块的设计使其具有“顽强的生命力”，能够在诸多挑战性场景下（如环境外观发生较大尺度变化、存在动态物体干扰、光照剧变后恢复等）成功找回丢失的相机位姿。
  • 系统连续性保障： 它是保证SLAM系统长期稳定运行和从跟踪失败中恢复的关键。没有有效的重定位，一次跟踪丢失就可能导致整个SLAM任务失败。

五、总结

ORB-SLAM2的重定位是一个精心设计的多阶段过程，它巧妙地结合了：

  • 高效的全局场景识别：通过词袋模型和倒排索引技术，快速从大规模地图中检索潜在匹配区域。
  • 鲁棒的初始位姿估计：利用EPnP算法和RANSAC策略，从稀疏的初始匹配中求解相机位姿。
  • 迭代的位姿精化和数据关联：通过多次BA优化和引导性的投影匹配，不断提升位姿的准确性并增加可靠的3D-2D对应关系。

这个模块的复杂性正体现了其在应对SLAM核心挑战——跟踪丢失——时所付出的努力和实现的高性能。