【视觉SLAM笔记】第八章 视觉里程计2

一、理论

1. 特征点法估计相机运动的缺点

尽管特征点法以其高精度和鲁棒性著称，但它并非完美无缺。其缺点主要源于其核心思想——依赖于稀疏的、可重复的特征点。

---

1. 信息丢失严重，地图稀疏（Sparsity）

这是特征点法最根本的缺点。

• 描述

  ：一张一百万像素的图像，特征点法可能只从中提取出几百个特征点。这意味着超过99% 的像素信息被直接丢弃了。算法只关心那些“角点”，而忽略了图像中大片的平面、边缘等区域。

• 后果

  ：

• • 稀疏地图

    ：构建出的三维地图是由离散的三维点组成的稀疏点云。这个地图对于相机定位来说是足够的，但它无法提供环境的完整几何信息。你无法用它来进行精细的导航、避障或者进行逼真的AR渲染。它只是一堆空间中的“钉子”，而不是一个“表面”。

  • 定位精度受限

    ：相机的位姿精度最终受限于这些稀疏特征点的分布和数量。如果特征点分布不均（例如，都集中在视野的某一侧），定位精度就会下降。

---

2. 在特征缺失场景下完全失效（Failure in Low-texture Environments）

特征点法的“命门”在于能否提取到足够多且稳定的特征。

• 描述

  ：当相机面对弱纹理或无纹理的场景时，例如一面纯白的墙、一条空旷的走廊、模糊的地板或天空，特征提取算法（如FAST, ORB）将找不到足够的角点。

• 后果

  ：

• • 跟踪失败

    ：如果没有足够的特征点进行匹配，PnP或对极几何等运动估计算法将无法工作，导致系统跟踪丢失（Lost）。

  • 应用场景受限

    ：这使得特征点法在许多室内人造环境（如办公室、仓库）或特定室外场景（如雪地、沙漠）中表现不佳。

---

3. 计算成本高，前端耗时（Computational Cost）

特征点的提取和匹配是一个计算密集型过程。

• 描述

  ：在每一帧图像上，都需要执行以下耗时操作：

1. 特征提取

   ：遍历整个图像来检测角点（如FAST）。

2. 特征描述

   ：为每个检测到的角点计算描述子（如BRIEF）。

3. 特征匹配

   ：在当前帧和参考帧（或地图）之间进行描述子的暴力匹配或近似最近邻搜索。

• 后果

  ：

• • 对CPU要求高

    ：这些操作会消耗大量的CPU资源，限制了系统能够处理的图像帧率和分辨率。在嵌入式设备或移动平台上，这可能成为性能瓶颈。

  • 无法做到非常稠密

    ：即使是像ORB这样经过优化的特征，我们也很难在实时系统上处理成千上万个特征点，这进一步加剧了其稀疏性的问题。

  ---

  4. 对快速运动和动态模糊敏感（Sensitivity to Motion Blur）

  特征点的“可重复性”假设在剧烈运动时会被打破。

  • 描述

    ：当相机快速旋转或移动时，图像会产生运动模糊。这种模糊会“抹平”角点的尖锐性，使得特征点变得模糊不清，甚至无法被检测到。同时，模糊也会改变特征点周围的像素模式，导致其描述子发生变化。

  • 后果

    ：

  • • 匹配困难

      ：即使特征点能被检测到，其描述子也可能与之前帧中的描述子相去甚远，导致匹配失败。

    • 鲁棒性下降

      ：这使得特征点法在无人机、手持快速移动的设备等高动态场景下的鲁棒性不如直接法。

  ---

  5. 存在量化误差（Quantization Error）

  这是一个比较细微但确实存在的问题。

  • 描述

    ：特征点的像素坐标是整数。从图像中提取关键点位置时，这个过程本身就存在一个亚像素级别的量化误差。虽然可以通过高斯拟合等方法进行亚像素精度的优化，但误差无法完全消除。

  • 后果

    ：

  • • 精度瓶颈

      ：这些微小的定位误差会传递到后续的运动估计和三角化过程中，成为系统精度的瓶颈之一。相比之下，直接法利用了像素的原始灰度值，理论上可以避免这种量化误差。

  ---

  总结表格

  缺点类别	具体描述	对相机运动估计的影响
  信息丢失/稀疏性	丢弃了>99%的像素信息，只关注角点。	1. 无法利用平面等几何结构来约束运动。 2. 导致地图稀疏，无法用于稠密建图任务。
  特征缺失场景失效	在白墙、走廊等弱纹理区域无法提取到足够特征。	系统直接跟踪失败，无法估计运动。这是灾难性的失败。
  计算成本高	特征提取、描述、匹配过程耗时。	限制了系统运行的帧率和实时性，尤其在低功耗设备上。
  对动态模糊敏感	快速运动导致图像模糊，破坏了特征点的稳定性和描述子。	匹配成功率急剧下降，导致运动估计不稳定或失败。
  量化误差	特征点在图像上的定位存在亚像素级别的误差。	引入了无法消除的噪声源，影响最终运动估计的精度。

  正是因为这些缺点的存在，才催生了直接法（Direct Method） 和半直接法（Semi-direct Method） 等其他视觉SLAM技术路线，它们试图通过利用更多的图像信息来克服特征点法的局限性。s

  2. 克服这些缺点的办法

  针对特征点法在估计相机运动时遇到的每一个缺点，学术界和工业界都发展出了一系列巧妙的克服或缓解方法。我们来逐一分析：

  ---

  1. 克服“信息丢失严重，地图稀疏”的缺点

  问题根源：只用了角点，丢弃了其他像素。

  解决方案：

  • 特征点法 + 稠密重建 (Feature-based SLAM + Dense Reconstruction)

  • • 方法

      ：这是一种“两步走”策略。首先，用经典的特征点法（如ORB-SLAM）运行一个鲁棒且高精度的稀疏SLAM系统，得到精确的相机关键帧位姿。然后，将这些精确的位姿作为已知输入，利用**多视图立体视觉（Multi-View Stereo, MVS）**技术对关键帧之间的像素进行稠密匹配，生成稠密点云或表面网格。

    • 优点

      ：兼顾了特征点法的定位精度和稠密重建的完整性。

    • 代表

      ：将ORB-SLAM的输出喂给开源的MVS算法（如COLMAP, OpenMVS）。

  • 混合法/紧耦合方法 (Hybrid/Tightly-coupled Methods)

  • • 方法

      ：在同一个优化框架内，同时利用稀疏特征点和稠密的像素信息。例如，VINS-Mono/Fusion 在优化时不仅有特征点的重投影误差，还加入了IMU的预积分误差。一些更前沿的工作会加入直接法（像素灰度误差）的残差项。

    • 优点

      ：让不同类型的信息互相约束，提升整体性能。

  ---

  2. 克服“在特征缺失场景下完全失效”的缺点

  问题根源：没有角点，算法就“饿死了”。

  解决方案：

  • 多传感器融合 (Multi-sensor Fusion)

  • • 方法

      ：这是最有效、最根本的解决方案。通过融合IMU（惯性测量单元），可以提供一个高频的、不受外界视觉环境影响的运动先验。

    • 工作原理

      ：在视觉特征短暂丢失时（例如，经过一面白墙），SLAM系统可以依靠IMU的积分来“盲推”一小段距离，保持跟踪。当相机再次看到有特征的区域时，视觉信息会修正IMU的累积漂移。

    • 优点

      ：极大地提升了系统的鲁棒性，使其能够在各种挑战性场景下工作。

    • 代表

      ：ORB-SLAM3, VINS-Mono/Fusion 都是典型的视觉-惯性紧耦合SLAM系统。

  • 使用更鲁棒的特征 (More Robust Features)

  • • 方法

      ：不局限于角点，引入线特征 (Line Features) 或 面特征 (Plane Features)。在人造环境中，直线和平面非常常见，即使角点很少，也往往存在大量的平行线、正交平面等几何约束。

    • 优点

      ：为SLAM系统提供了额外的、正交的几何信息源，特别适合室内等结构化场景。

    • 代表

      ：PL-SLAM (Point-Line SLAM), Structure-SLAM (利用点线面)。

  • 主动视觉 (Active Vision)

  • • 方法

      ：如果SLAM系统搭载在机器人上，可以控制相机主动地去寻找有纹理的区域，避免长时间停留在弱纹理场景。或者使用结构光投影仪主动向环境中投射图案，人为地创造特征。

    • 优点

      ：从根源上解决问题，但需要对机器人有控制权。

  ---

  3. 克服“计算成本高，前端耗时”的缺点

  问题根源：特征提取和匹配是计算密集型操作。

  解决方案：

  • 高效的特征和算法 (Efficient Features and Algorithms)

  • • 方法

      ：使用计算成本更低的特征，如 ORB，它就是为了在实时性上取代SIFT/SURF而设计的。ORB使用速度极快的FAST进行角点检测，并使用计算高效的BRIEF二进制描述子（通过汉明距离匹配，速度远快于浮点描述子的欧氏距离）。

    • 优点

      ：在保持较好性能的同时，大大降低了计算负担。

  • 关键帧机制 (Keyframe-based SLAM)

  • • 方法

      ：这是所有现代SLAM系统的标配。系统不处理每一帧图像，而是有选择性地将某些帧选为“关键帧”。耗时的操作（如大规模特征匹配、地图点创建、后端优化）只在关键帧上进行。对于非关键帧，只进行快速的位姿跟踪。

    • 优点

      ：将计算量集中在少数有代表性的帧上，实现了实时性与精度的平衡。

  • 硬件加速 (Hardware Acceleration)

  • • 方法

      ：利用GPU、FPGA或专用的视觉处理器（VPU）来加速特征提取和匹配过程。

    • 优点

      ：从硬件层面解决性能瓶颈。

  • 特征传播/光流跟踪 (Feature Propagation / Optical Flow)

  • • 方法

      ：在连续帧之间，不进行重新检测和暴力匹配，而是使用**光流法（如KLT）**来预测和跟踪上一帧的特征点在当前帧的位置。只有在特征点丢失较多或间隔较长时间后，才重新进行一次完整的特征检测。

    • 优点

      ：对于高帧率的视频流，跟踪的成本远低于检测+匹配。

  ---

  4. 克服“对快速运动和动态模糊敏感”的缺点

  问题根源：运动模糊破坏了特征的稳定性和描述子。

  解决方案：

  • 多传感器融合 (IMU Fusion)

  • • 方法

      ：IMU再次成为“救星”。IMU可以提供高频的角速度和加速度信息。这可以用来：

    • 优点

      ：IMU的引入极大地增强了系统在高动态场景下的鲁棒性。

  1. 运动补偿

     ：在相机曝光期间，利用IMU数据对图像进行“去模糊”处理。

  2. 运动预测

     ：为特征匹配提供一个非常精准的搜索窗口先验，大大减少匹配的搜索范围，提高匹配成功率。

• 使用支持全局快门(Global Shutter)的相机

• • 方法

    ：使用全局快门相机可以从硬件上消除果冻效应（Rolling Shutter Jello Effect），使得图像在高速运动下也能保持清晰，不会扭曲变形。

  • 优点

    ：从源头改善了图像质量，让特征点法能更好地工作。

  ---

  5. 克服“量化误差”的缺点

  问题根源：像素坐标是离散的。

  解决方案：

  • 亚像素级精炼 (Sub-pixel Refinement)

  • • 方法

      ：在检测到整数像素坐标的角点后，分析其邻域的灰度梯度信息，通过插值或拟合（如高斯曲面拟合）来计算出更精确的浮点数坐标。

    • 优点

      ：可以在不增加太多计算量的情况下，显著提高关键点定位的精度。

  • 结合直接法 (Combining with Direct Methods)

  • • 方法

      ：在后端优化中，除了最小化特征点的重投影误差（几何误差），还可以加入**光度误差（Photometric Error）**作为约束。光度误差直接比较像素块的亮度差异，不依赖于精确的亚像素定位。

    • 优点

      ：融合两种不同类型的误差模型，可以得到更精确和鲁棒的结果。

  总结

  可以看出，现代高性能的视觉SLAM系统，尤其是那些能够在复杂现实环境中稳定运行的系统（如AR/VR头显、无人机、机器人），几乎都不是“纯粹”的特征点法系统。它们广泛地采用了**多传感器融合（尤其是与IMU的紧耦合）和混合多种算法思想（如结合直接法、线特征等）**的策略，取长补短，从而在整体性能上实现了质的飞跃。

  3. 光流法与直接法

  这是一个非常核心且容易混淆的概念。光流法（Optical Flow） 和 直接法（Direct Method） 在思想上紧密相关，但在视觉SLAM的语境中，它们的角色和范畴有所不同。

  可以这样理解它们的关系：

  • 光流法

    是一种具体的算法/技术，用于计算图像中像素点的运动。

  • 直接法

    是一种更宏观的SLAM方法论/框架，它依赖的核心思想与光流法一致，但目标是完整的SLAM（定位与建图）。

  我们来详细拆解。

  

  4. 2D光流

  

  我们来系统地、深入地讲解2D光流（2D Optical Flow）。这是一个在计算机视觉中非常基础且重要的概念，也是理解直接法SLAM的基石。

  

  5. 直接法原理

  

  

  6. 直接法的推导

  我们来详细推导直接法SLAM中用于求解相机位姿的优化问题，特别是其核心——雅可比矩阵（Jacobian Matrix）的推导。

  我们将以一个最基础的直接法模型为例：已知参考帧中的一个3D点，求解相机从参考帧到当前帧的位姿。

  

  二、代码笔记

  1. optical_flow.cpp 

  该程序手动实现了Lucas-Kanade (LK)光流法，用于追踪两张连续图像之间的特征点。它展示了从基础的单层光流到更鲁棒的多层光流，以及正向与反向法的区别，并与OpenCV的官方实现进行了性能和效果的对比。

  程序的主要功能和流程如下：

  1. 特征点检测：程序首先在第一张图像上使用**GFTT (Good Features to Track)**算法检测出一批角点，这些角点将被作为追踪的目标。

  2. 单层LK光流 (OpticalFlowSingleLevel)：

  • 这是LK光流的基础版本。对于第一张图中的每个特征点，它假设该点及其邻域（一个小的图像块，patch）在第二张图中仅发生了微小的平移。

  • 核心思想

    ：基于光度不变性假设（一个点的灰度在短时间内不变），通过最小化两张图中对应图像块的**光度误差平方和（SSD）**来求解这个平移量（即光流）。

  • 求解方法

    ：这是一个非线性优化问题，程序使用高斯-牛顿法进行迭代求解。

• 多层LK光流 (OpticalFlowMultiLevel)：

• • 单层LK光流法对大的运动非常敏感，容易追踪失败。为了解决这个问题，程序实现了多层（金字塔）光流法。

  • 核心思想

    ：首先将两张原始图像降采样，构建出由粗到精的图像金字塔。追踪从金字塔的顶层（最低分辨率）开始，由于图像模糊且运动被缩小，即使原始运动很大，在顶层也变成了小运动，可以被单层LK成功追踪。然后，将顶层的追踪结果作为初始值，传递到下一层（更高分辨率）继续追踪，逐层优化，直到在原始图像上完成追踪。这种由粗到精（Coarse-to-Fine）的策略极大地增强了算法对大运动的鲁棒性。

• 正向法 vs 反向法 (Inverse Compositional)：

• • 程序在实现中考虑了两种计算雅可比矩阵的方式。

  • 正向法

    ：在每次迭代中，雅可比矩阵（图像梯度）都需要在第二张图的当前估计位置重新计算。

  • 反向法

    ：通过数学上的巧妙变换，可以将雅可比矩阵固定为在第一张图的原始位置计算，且在整个迭代过程中保持不变。这大大减少了计算量，因为雅可比和Hessian矩阵只需计算一次。代码中通过inverse参数来切换这两种模式。

• 对比与验证：程序最后将自己实现的单层、多层光流的结果与OpenCV官方的calcOpticalFlowPyrLK函数的结果进行对比，并可视化展示，以验证其正确性和性能。

• 

  • 单层与逆向 LK 迭代公式；

  • 多层金字塔框架的上下层传递；

  • 光度残差、雅可比、Hessian 的构造与增量更新过程。

  

  2. direct_method.cpp

  

  

  • 利用直接法以光度误差最小化估计相机位姿；

  • 雅可比推导与 Gauss–Newton 增量公式；

  • 多层图像金字塔框架；

  • 并行化 Hesse 与梯度累加。