TrackWithMotionModel

一、核心思想与定义

恒速模型跟踪基于一个核心假设：在连续的图像帧之间极短的时间内（通常为几十毫秒），相机的运动状态可以近似为匀速运动。

因此，可以利用上一帧的位姿以及先前估计得到的**帧间相对运动（速度和角速度，统称为速度 V）来预测当前帧的位姿，并以此为初始值进行后续的匹配和优化。

二、为何使用恒速模型跟踪？

效率优先：
- 在地图初始化并通过参考关键帧跟踪成功获得初始速度后，恒速模型提供了一种更简单、计算更快的位姿估计方法。
- 相比于参考关键帧跟踪（涉及词袋匹配等），恒速模型直接利用运动先验，对实时性要求高的SLAM系统非常重要。
常用策略：
- 一旦系统有了速度信息，恒速模型便成为跟踪线程中的首选和最常用的跟踪方法。

三、关键公式

速度 (Velocity) $\mathbf{V}$ ：
- 表示从上一帧 (l) 到当前帧 (c) 的相对位姿变换 $\mathbf{T}_{\mathrm{cl}}$ 。
- 如果上一帧的世界位姿为 $\mathbf{T}{\mathrm{lw}}$ ，当前帧的世界位姿为 $\mathbf{T}{\mathrm{cw}}$ ，则：
  $\mathbf{V} = \mathbf{T}_{\mathrm{cl}} = \mathbf{T}_{\mathrm{cw}} (\mathbf{T}_{\mathrm{lw}})^{-1} = \mathbf{T}_{\mathrm{cw}} \mathbf{T}_{\mathrm{wl}}$
- 在实践中， $\mathbf{V}$ 通常是上上帧到上一帧的运动。
当前帧位姿预测 $\mathbf{T}_{\mathrm{cw}}$ ：
- 利用速度 $\mathbf{V}$ 和上一帧的世界位姿 $\mathbf{T}_{\mathrm{lw}}$ ：
  $\mathbf{T}_{\mathrm{cw}} = \mathbf{V} \mathbf{T}_{\mathrm{lw}}$
- 此预测结果作为当前帧位姿优化的初始值。

四、关键步骤与原理

恒速模型跟踪主要包含两个大的阶段：UpdateLastFrame() 的预处理和 TrackWithMotionModel() 的核心跟踪流程。

A. 预处理：更新上一帧信息 (`UpdateLastFrame()`)

此步骤在正式跟踪当前帧之前，对上一帧 (mLastFrame) 进行关键的更新。

更新上一帧的世界位姿 $\mathbf{T}_{\mathrm{lw}}$ ：
- 目的： 确保用于预测当前帧的上一帧位姿是最新的、最准确的。
- 方法： 不直接使用上一帧跟踪后得到的位姿，而是通过其对应的参考关键帧 (pRef) 来间接更新。
  $\mathbf{T}_{\mathrm{lw}} = \mathbf{T}_{\mathrm{lr}} \mathbf{T}_{\mathrm{rw}}$ $T_{l w} = T_{l r} T_{r w}$
  其中：
  - $\mathbf{T}_{\mathrm{lw}}$ : 上一帧在世界坐标系下的位姿 (待更新)。
  - $\mathbf{T}_{\mathrm{lr}}$ : 上一帧相对于其参考关键帧的位姿变换 (已知，存储如 mlRelativeFramePoses)。
  - $\mathbf{T}_{\mathrm{rw}}$ : 上一帧的参考关键帧在世界坐标系下的位姿 (该位姿可能已被局部建图线程优化过，更准确)。
- 原理： 普通帧（如果未被选为关键帧）是“用完即抛”的，其位姿不会被后端持续优化。而关键帧的位姿会通过局部建图和回环检测不断优化。通过上述公式，可以将关键帧的优化成果传递给上一普通帧，从而提高其位姿的准确性。
为双目/RGB-D相机的上一帧创建临时地图点：
- 适用条件： 仅当传感器为双目相机 (System::STEREO) 或 RGB-D 相机 (System::RGBD) 时执行，且上一帧不是关键帧。单目相机 (System::MONOCULAR) 则跳过此步。
- 目的： 利用双目/RGB-D可直接获取深度信息的优势，为上一帧中一些有深度但尚未关联地图点的特征点创建临时地图点 (Temporal MapPoints)。这些点能显著增加下一帧（即当前帧）跟踪时的3D信息，提高投影匹配的稳定性和成功率。
- 特性：
  - 临时性： 这些点仅用于当前帧的跟踪，不会加入到全局地图中，在跟踪流程的后续阶段会被清除（例如，在创建新关键帧之前）。
  - 简化属性： 创建时可能不包含完整的地图点属性，因为其生命周期很短。
- 创建流程：
  1. 筛选： 找出上一帧中所有具有有效深度值 (mLastFrame.mvDepth[i] > 0) 的特征点。
  2. 排序： 将这些点按深度值从小到大排序。
  3. 创建判断： 遍历排序后的点：
    - 如果该特征点在上一帧中没有对应的地图点 (!mLastFrame.mvpMapPoints[i])。
    - 或者其对应的地图点质量差（例如，被创建后从未被其他帧观测到 pMP->Observations() < 1）。
      则认为需要为该特征点创建一个新的临时地图点。
  4. 生成： 利用特征点的2D坐标和深度值，通过反投影 (mLastFrame.UnprojectStereo(i)) 计算其在上一帧相机坐标系下的3D坐标，然后转换到世界坐标系下，创建新的MapPoint对象。
  5. 存储： 将新创建的临时地图点存入 mLastFrame.mvpMapPoints 的对应位置，并加入一个临时列表 mlpTemporalPoints 以便后续统一清理。
  6. 终止条件： 当创建的临时点达到一定数量（如 nPoints > 100）且当前处理的点的深度已超过设定阈值 (mThDepth，例如40倍基线）时，停止创建（太远或太多的点可能反而降低效率或引入不确定性）。

B. 核心跟踪流程 (`TrackWithMotionModel()`)

调用预处理：
- 执行 UpdateLastFrame() 来更新上一帧的位姿并（如果适用）创建临时地图点。
初始位姿估计：
- 使用恒速模型公式预测当前帧 mCurrentFrame 的初始世界位姿：
  mCurrentFrame.SetPose(mVelocity * mLastFrame.mTcw);
投影匹配 (SearchByProjection)：
- 数据源： 将上一帧 mLastFrame 中所有有效的地图点（包括常规地图点和刚刚为双目/RGB-D创建的临时地图点）投影到当前帧 mCurrentFrame 的图像平面上（基于当前帧的初始估计位姿）。
- 匹配： 在每个投影点周围的一个预设搜索窗口内（单目通常 th=15 像素，双目/RGB-D th=7 像素），为当前帧的ORB特征点寻找匹配的地图点。
- 鲁棒性增强 (二次搜索)：
  - 如果初次投影匹配找到的匹配点数量不足（如 nmatches < 20），系统会认为初始位姿估计可能不够准确或场景变化稍大。
  - 此时，会将搜索窗口的半径扩大一倍（2*th），并重新进行一次投影匹配，尝试找回更多潜在的匹配。
- 如果二次搜索后匹配点仍然过少，则认为恒速模型跟踪失败。
位姿优化 (Optimizer::PoseOptimization)：
- 基于投影匹配成功的3D地图点及其在当前帧中对应的2D特征点，通过非线性优化算法（如Bundle Adjustment的仅优化位姿版本）最小化重投影误差，进一步精确当前帧的位姿。
- 注意： 此阶段仅优化当前帧的位姿，不改变3D地图点的坐标。
外点剔除：
- 在位姿优化后，根据重投影误差等准则，识别并剔除那些可能是错误匹配的特征点（外点）。
- 更新当前帧内点（mvpMapPoints）和外点标记（mvbOutlier）。
成功判断：
- 统计最终有效的内点匹配数量 (nmatchesMap)。
- 如果内点数量大于等于一个阈值（例如，在非“仅定位”模式下 nmatchesMap >= 10），则认为恒速模型跟踪成功，返回 true。否则返回 false。
- 在“仅定位”模式下 (mbOnlyTracking)，对内点数量有更严格的要求（如 nmatchesMap < 10 会标记 mbVO 为 true，表示可能退化为视觉里程计，且总匹配数 nmatches > 20 才认为成功）。

五、优缺点总结

优点：
1. 高效性： 仅依赖上一帧信息和已知的速度模型，计算量小，速度快，是系统正常运行时的主要跟踪手段。
2. 稳定性增强技巧：
  - 双目/RGB-D模式下动态生成临时地图点，有效增加了可用于跟踪的3D点数量。
  - 投影匹配不足时，采用扩大搜索半径的二次尝试策略，提高了对轻微运动变化的鲁棒性。
  - 通过参考关键帧更新上一帧位姿，间接利用了后端优化的成果。
缺点：
1. 理想化假设： “匀速运动”的假设在相机发生剧烈运动（如快速旋转、急停/急加速）或系统帧率较低（导致帧间变化大）时可能不成立。
2. 易丢失： 当上述假设不满足时，预测的初始位姿与真实位姿偏差可能过大，导致投影匹配失败，进而跟踪丢失。此时系统需要回退到参考关键帧跟踪或进行重定位。