重定位（Relocalization）指的是：

当系统丢失当前位姿估计（例如跟踪失败、漂移过大、回环检测时发现历史关键帧）后，通过匹配当前观测与已知地图或历史关键帧来重新确定相机/传感器的全局位姿。

跟踪失败 = 当前帧无法可靠估计相机位姿。

这通常是因为：
光流跟踪的特征点太少（导致姿态约束不足）；
跟踪的点分布太集中（退化，不能解出姿态）；
光流匹配误差大（特征漂移）；
图像模糊、遮挡、强光、快速旋转、黑暗等导致特征检测失败；
或 IMU 预测与视觉测量不一致（残差太大）。

1️⃣以 VINS-Fusion 为例光流跟踪流程

VINS-Fusion 前端使用 KLT（Lucas–Kanade 光流） 跟踪角点。流程如下：
在关键帧上检测 N 个角点（默认 N≈150~200 个）；
对每个新帧，用金字塔 LK 光流跟踪前一帧特征；
计算每个点的跟踪成功率、残差等；

若某点：
越出图像边界；
光流匹配残差过大；
光流优化不收敛；
则认为该点 跟踪失败；
若成功点数过少或分布退化，则认为整帧 跟踪失败（Tracking lost）。

2️⃣ “多少点失败算跟踪失败？”

没有绝对阈值，但在 VINS-Fusion 的实践中：

成功跟踪点数	状态	说明
>100	正常	跟踪良好
50~100	边缘状态	跟踪质量下降，需补点
<50	严重退化	可能无法估计位姿（Tracking lost）
<20	丢失	系统可能需要重定位

💡 经验规律：
当成功跟踪点 < 30 且分布在画面一角时，几乎不可能恢复6自由度姿态。
在代码中（feature_tracker.cpp），VINS-Fusion 会在每帧中：
检查跟踪到的特征数；
若数量过少则触发特征重检测；
若持续多帧特征数低，则进入“Tracking Failure”状态。

视野变化与“特征出图”问题

非常细节且关键！当相机发生旋转或平移时，原来画面边缘的特征可能会：

• 被遮挡或移动出视野外（即坐标超出图像尺寸）；
• 视差太大导致光流失败；
• 深度变化导致匹配错误。
  这些点会被立即剔除（KLT中 status[i]=0）
  然后系统会在当前帧中 重新检测新的角点来补充特征点，保证每帧始终保持足够的角点数量（如 ~150 个）。
  这一机制称为：特征维护（Feature Management）

目标：

始终维持足够数量且分布均匀的特征； 剔除越界或漂移特征； 在稀疏区域自动检测新点。

漂移过大

“漂移”在 SLAM 中指系统估计的轨迹与真实轨迹间的累计误差。
但在“前端跟踪”的语境下，漂移过大 通常指：

光流估计的位置偏差过大，或视觉预测与IMU积分预测不一致。

在 VINS-Fusion 中，漂移判断主要体现在：

1. 光流匹配残差大（像素级差距过大，RANSAC剔除过多点）；
2. IMU 预测位姿与视觉测量差距大（导致优化残差大）；
3. 局部地图优化不收敛（观测方程不满足几何一致性）。

具体量化方式：

• 若平均重投影误差 > 3 像素；
• 或特征匹配内点比例 < 50%；
• 或连续几帧优化失败（Hessian 矩阵条件数过大）；

→ 则认为发生漂移或退化。

重定位可以分为两种典型场景：

类型	场景	方法
失跟重定位（Tracking failure relocalization）	跟踪丢失（例如快速运动或遮挡）后恢复位姿	基于图像特征匹配或局部地图匹配
回环检测重定位（Loop closure relocalization）	系统检测到当前帧与历史帧来自同一地点	基于特征描述子或全局外观匹配（BoW、NetVLAD等）

跟踪失败总结

概念	含义	典型阈值（经验）	处理方式
特征点跟踪失败	光流无法找到对应位置	单点失败率约30%以内可接受	剔除失败点，重新检测新角点
帧跟踪失败	可用特征点太少，无法估计姿态	有效点 < 30~50	暂停视觉更新，仅IMU传播或重定位
出图特征	特征点移出视野边界	坐标越界	删除该点，不再跟踪
漂移过大	光流残差大、优化不收敛	残差>3px、RANSAC内点率<50%	进入失效检测或重定位流程

重定位的核心机制

1️⃣ 基于特征的匹配与PnP估计
常用于 VSLAM（如 ORB-SLAM）

流程：

1. 提取当前帧特征点（ORB、SIFT、SuperPoint 等）。
2. 从关键帧数据库（或地图点云）中找到候选匹配帧（BoW / DBoW2 / NetVLAD）。
3. 通过特征匹配得到2D–3D对应关系。
4. 使用 PnP + RANSAC 求解相机位姿。
5. 检查匹配数量、重投影误差等标准，如果通过则完成重定位。

关键标准：

• 匹配点数 ≥ 阈值（如 > 30 个）
• 重投影误差 < 2 像素
• RANSAC 一致率 > 某阈值（如 > 0.7）

2️⃣ 基于几何/点云的匹配
常用于 LiDAR-SLAM

流程：

1. 当前扫描帧与局部地图点云进行 ICP / NDT / FPFH 特征匹配。
2. 计算相对变换 Tmap2current
3. 估计配准残差与一致性。

评价标准：

• ICP 收敛（误差下降、迭代稳定）
• 残差 RMSE < 阈值（如 0.05m）
• 协方差矩阵的主轴方差较小（表明姿态可观测）

3️⃣ 融合地图或全局优化
重定位成功后，系统需进行姿态融合或后端图优化：

• 将重定位得到的约束添加到后端优化图中（作为一个 loop edge）
• 触发 Pose Graph Optimization（PGO） 或 Bundle Adjustment（BA）
• 优化整个轨迹与地图一致性

当多次定位结果不一致时，如何评价优劣？这时要从一致性与可信度两个角度评估。

评价指标	含义	用于判定
重投影误差 / ICP残差	当前帧与地图匹配误差	越小越可信
匹配数量 / 内点比例	成功匹配的特征点或配准点数量	越多越好
优化后残差变化量	BA/PGO 优化前后误差变化	越大说明修正有效
协方差或置信椭球体积	位姿估计的不确定度	越小越可信
一致性检测	比较新估计和预测位姿差异	若偏差过大，拒绝重定位结果

四、实际系统中的判断策略举例

ORB-SLAM2：

• 使用 DBoW2 建立关键帧数据库；
• 检测候选帧并验证几何一致性；
• 用 PnP + RANSAC 求位姿；
• 验证匹配点数、重投影误差、一致性；
• 若通过，则更新当前位姿并恢复跟踪。

LOAM / LIO-SAM：

• 通过回环检测模块匹配当前 LiDAR 帧与历史帧；
• 通过 ICP 验证几何一致性；
• 若匹配质量高（RMSE < 阈值），则加入回环约束；
• 否则丢弃该回环。

五、总结一句话

重定位的本质是：在当前观测与已知地图间建立可靠几何约束。

判断依据：匹配质量 + 残差误差 + 一致性 + 不确定度。

若多次定位结果不一致，优先选择误差更小、置信度更高且几何一致的解，否则拒绝更新。