C++AI算法
FASTLIVO2 算法解析与系统架构详解
FASTLIVO2 是一种融合激光雷达、相机和 IMU 的 SLAM 系统。它通过顺序更新的 ESIKF 框架解决维度不匹配问题，利用体素八叉树管理统一地图。核心创新包括平面先验优化、参考图像块更新策略、在线曝光时间估计及按需光线投射。系统采用先激光雷达后视觉的顺序更新策略，提升计算效率与鲁棒性，适用于复杂环境下的机器人定位与建图。
FASTLIVO2 是一种融合激光雷达、相机和 IMU 的 SLAM 系统。它通过顺序更新的 ESIKF 框架解决维度不匹配问题，利用体素八叉树管理统一地图。核心创新包括平面先验优化、参考图像块更新策略、在线曝光时间估计及按需光线投射。系统采用先激光雷达后视觉的顺序更新策略，提升计算效率与鲁棒性，适用于复杂环境下的机器人定位与建图。
FASTLIVO2 系统融合了三种互补的传感器：激光雷达（LiDAR）、相机（Camera）和惯性测量单元（IMU）。它们在感知方式、输出数据和环境适应性上各具特点，通过融合实现优势互补。
| 特性 | 激光雷达（LiDAR） | 相机（Camera） | IMU |
|---|---|---|---|
| 工作方式 | 主动发射激光，通过反射测量距离和方位 | 被动接收环境光，捕捉 2D 图像信息 | 主动测量自身运动 |
| 感知内容 | 环境几何结构（深度、形状、表面） | 环境纹理与颜色（语义、细节、动态物体） | 自身运动状态（姿态、速度、加速度） |
| 数据输出 | 3D 点云（精确深度） | 2D 像素矩阵（RGB 或灰度） | 6 自由度运动参数 |
| 优势 | - 直接深度测量，精度高- 不受光照影响- 在结构化环境中鲁棒 | - 丰富语义信息- 成本低- 高分辨率细节捕捉 | - 高频更新（100–400 Hz）- 短期运动估计精确- 不受环境退化影响 |
| 劣势 | - 缺乏颜色信息- 在无结构环境（如单一墙面）易退化- 近距离盲区失效 | - 依赖光照和纹理- 无直接深度（需三角化）- 动态物体 / 光照变化易失效 | - 长期漂移累积误差- 仅提供相对运动，无法感知环境 |
| 环境敏感性 | 对几何结构退化敏感（如狭窄隧道） | 对光照和纹理退化敏感（如黑暗、无纹理表面） | 几乎不受环境影响 |
| 互补性体现 | ✅ 为相机提供几何先验（如平面、法向量）✅ 补偿相机在黑暗 / 纹理缺失时的失效 | ✅ 为 LiDAR 点云添加颜色 / 纹理✅ 在 LiDAR 退化时通过图像对齐提供约束 | ✅ 为 LiDAR / 相机提供高频运动先验✅ 在传感器退化时维持系统可观性 |
激光雷达 - 惯性 - 视觉里程计（LIVO）系统在实际部署中面临四个核心挑战：
FAST-LIVO2 在 FAST-LIVO 的基础上进行了五项关键改进：
FAST-LIVO2 与当前主流方案 R3LIVE 在多个维度存在显著差异：
| 维度 | R3LIVE | FAST-LIVO2 |
|---|---|---|
| 鲁棒性 | - VIO 中操作单个像素- 采用两阶段对齐：1. 帧间光流（frame-to-frame）获取初始位姿 2. 帧到地图（frame-to-map）优化位姿- 此流程对初始位姿敏感，在快速运动或弱纹理场景易失败 | - VIO 中操作图像块级别- 创新点：直接帧到地图稀疏图像对齐（一步到位）- 基于 ESIKF 滤波器紧耦合融合 IMU、LiDAR 和相机数据- 通过 LiDAR 点云构建的体素地图提供几何先验- 省去光流初始化，显著提升在退化场景（如纯色墙面、低光照）的稳定性 |
| 计算性能 | - VIO 模块采用稠密直接法- 需对大量像素（甚至全图像素）计算光度误差，计算开销大 | - 稀疏直接法仅选取关键视觉地图点（附着图像块）构建残差- 视觉地图点来源自 Lidar 点云，无需额外特征提取- 图像对齐仅在局部图像块（8×8 像素）进行- 配合图像金字塔分层优化（由粗至精），进一步减小计算量 |
| 地图管理 | - 受限于其点地图的分辨率 | - 激光雷达和视觉模块共享单一统一地图- 可以直接利用激光雷达点提供的平面先验来加速图像对齐- 利用了原始图像块分辨率级别的信息 |
| 英文术语 | 中文翻译 | 简要解释 |
|---|---|---|
| Error State Iterated Kalman Filter (ESIKF) | 误差状态迭代卡尔曼滤波器 | 一种在误差状态空间进行迭代优化的卡尔曼滤波算法。用于提高估计精度。 |
| Direct Method | 直接法 | 直接利用传感器原始数据（如像素强度、点云）进行优化的方法，无需预先提取特征点。 |
| Photometric Error | 光度误差 | 基于图像像素强度值差异计算得到的一种误差度量方式，常用于直接法中作为优化目标。 |
| Voxel Map | 体素地图 | 将三维空间分割成许多小立方体单元来表示环境信息的地图形式，适合于表示复杂场景。 |
| Image Alignment | 图像对齐 | 通过调整参数使得两张或多张图像之间或图像与模型之间的对应关系达到最佳匹配的过程。 |
| Plane Prior | 平面先验 | 已知或者假设存在的局部平面几何信息，在某些优化问题中被用作约束条件以提高解的质量。 |
| RayCasting | 射线投射 | 模拟传感器发出的射线（例如激光雷达的光线）与环境中物体交互过程的技术，广泛应用于渲染和感知。 |
| Visual Landmark | 视觉地图点 | 复用 LiDAR 模块已经构建好的高精度 3D 点云来充当视觉地图点，3D 坐标由 Lidar 直接提供。将图像块'附着'到激光雷达点云上 |
| Image Patch | 图像块 | 视觉地图点投影的像素坐标为中心，划一个一定大小像素范围的块（如 8×8 像素）,用于视觉匹配和光度误差计算，动态更新以保持纹理细节 |
| Reference Image Patch | 参考图像块 | 视觉地图点的多个图像块中质量最好的图像块（基于光度相似性和视角） |
| Pyramid of Images | 图像金字塔 | 多尺度图像表示方法，每一层都是原图像的一个缩放 |
| Scan Reassembly | 点云/扫描重组 | 将异步采集的激光雷达点云数据，在相机拍摄时刻重新组合成完整的扫描帧，确保多传感器数据时间对齐 |
| Sequential Update | 顺序更新 | 在卡尔曼滤波中，先处理激光雷达数据更新位置状态，再处理图像数据优化状态，避免同时处理导致维度不匹配问题 |
| Point-to-Plane Residual from Scan to Map | 帧到地图的点 - 面残差 | 计算当前激光雷达扫描点与地图中已有平面之间的位置误差，用于校正定位偏差 |
| On-demand RayCasting | 按需光线投射 | 当视野中地图点不足时，模拟光线投射生成虚拟点云，补充约束以增强定位鲁棒性。 |
| Visible Voxel Query | 可见体素查询 | 快速查找地图中位于当前相机视野范围内的体素单元（小方块区域）,仅处理可见部分以提升效率。 |
FASTLIVO2 系统由四个核心模块组成，形成完整的感知 - 估计 - 建图闭环。下图展示了系统的整体架构和数据流：
四个核心模块包括：
其中模块 1 主要负责 ESIKF 状态预测/更新方程，模块 3 和 4 则提供 ESIKF 的状态观测方程。
状态向量（19 维）：
// 状态向量 x = [
// 旋转 δθ (3) - 旋转误差（李代数表示）
// 位置 δp (3) - 位置误差
// 曝光时间 δt_exp (1) - 曝光时间倒数误差
// 速度 δv (3) - 速度误差
// 陀螺仪偏差 δb_g (3) - 陀螺仪零偏误差
// 加速度计偏差 δb_a (3) - 加速度计零偏误差
// 重力 δg (3) - 重力向量误差
// ]
核心功能：
ESIKF 数学补充：
误差状态卡尔曼滤波（ESKF）的核心思想是维护一个名义状态（nominal state）和一个误差状态（error state）。名义状态跟踪大信号，误差状态跟踪小扰动。这种分离使得：
ESIKF 在 ESKF 基础上引入迭代更新（Iterated Update），通过多次线性化提高非线性测量下的估计精度。其迭代过程为：
for i = 1:N_iter {
// 计算测量残差 z - h(x_nominal ⊕ δx)
// 计算雅可比矩阵 H = ∂h/∂δx
// 计算卡尔曼增益 K
// 更新误差状态 δx = K * residual
}
// 将误差状态注入名义状态：x_nominal = x_nominal ⊕ δx
// 重置误差状态 δx = 0
代码映射：
include/common_lib.h 中的 StatesGroup 结构体src/LIVMapper.cpp 中的 stateEstimationAndMapping()基于体素八叉树的高效局部地图管理，用于存储激光雷达点云和视觉地图点。
地图结构：
体素八叉树 (VoxelOctoTree)
├── 叶节点（VoxelPlane）
│ ├── 中心点 center_
│ ├── 法向量 normal_
│ ├── 协方差 covariance_
│ ├── 平面参数 d_
│ ├── 点数量 points_size_
│ └── 是否平面 is_plane_
└── 根节点（OctoTree）
├── 8 个子节点 leaves_[8]
├── 体素中心 voxel_center_
└── 层级信息 layer_
关键特性：
激光雷达和视觉模块共享同一个体素地图：
体素地图内容
├── 激光雷达平面（VoxelPlane）
│ ├── 几何结构（中心、法向量、协方差）
│ └── 点面残差约束
└── 视觉地图点（VisualPoint）
├── 参考图像块（8×8 像素）
├── 参考位姿
├── 法向量（单独线程优化）
└── 光度残差约束
优势：
实战案例补充：
在实际部署中，体素地图的动态生长特性对于室内外混合场景尤为重要。例如，在从开阔广场进入狭窄走廊时，点云密度急剧变化。FASTLIVO2 的八叉树能自动调整深度：在开阔区域使用较大体素（低深度）以节省内存，在狭窄区域自动分割为小体素（高深度）以保持精度。这种自适应机制无需手动参数调整，显著提升了系统的环境适应性。
代码映射：
include/voxel_map.h 中的 VoxelOctoTree 和 VoxelMapManager 类src/voxel_map.cpp 中的 BuildVoxelMap(), UpdateVoxelMap()功能描述：基于点面（Point-to-Plane）距离约束的激光雷达观测方程，用于几何配准。
观测模型：
残差 r_LIO = n^T · (R_wb · p_b + t_wb - p_plane)
其中：
- n: 平面法向量（从体素地图获取）
- p_b: 激光雷达点（body 系）
- R_wb, t_wb: 位姿估计（ESIKF 状态）
- p_plane: 平面上某点（体素中心）
流程：
代码映射：
include/voxel_map.h 中的 PointToPlane 结构体src/voxel_map.cpp 中的 BuildResidualListOMP(), build_single_residual()功能描述：基于稀疏直接法的视觉里程计，通过图像块光度误差实现视觉约束。
观测模型：
残差 r_VIO = I_cur(x_warped) - I_ref(x_ref)
其中：
- I_cur: 当前图像
- I_ref: 参考图像（存储在视觉地图点中）
- x_warped: 基于位姿估计投影的像素坐标
- x_ref: 参考像素坐标
关键创新：
流程：
顺序更新优势补充：
FASTLIVO2 采用'先激光雷达，后视觉'的顺序更新策略，而非并行更新。这种设计具有两个关键优势：
代码映射：
include/vio.h 中的 VIOManager 和 VisualPoint 类src/vio.cpp 中的 computeJacobianAndUpdateEKF(), processFrame()传感器数据流
├── 激光雷达点云
│ ├── 点云重组（按相机采样时刻）
│ ├── 预处理（降采样、去运动畸变）
│ ├── 体素地图查询
│ ├── 构建点面残差
│ └── ESIKF 状态更新（LIO）
├── IMU 数据
│ ├── 静态初始化（重力、偏差）
│ ├── 状态传播（连续）
│ └── 去畸变辅助
└── 相机图像
├── 视觉地图点检索（FOV 内）
├── 参考图像块获取
├── 仿射变换与光度误差
└── ESIKF 状态更新（VIO）
FASTLIVO2 采用顺序更新策略，而非并行更新：
时间轴 t0 → t1 → t2 → t3 → ...
每帧的处理流程：
Step 1: IMU 状态传播
┌─────────────────────────────────────┐
│ 从 t0 到 t1: │
│ - IMU 积分，预测位姿 │
│ - 协方差传播 │
│ - 输出先验状态 │
└─────────────────────────────────────┘
Step 2: 激光雷达更新（LIO）
┌─────────────────────────────────────┐
│ - 点云分割重组（按相机采样时刻） │
│ - 点云预处理（降采样、去畸变） │
│ - 体素地图查询，获取平面参数 │
│ - 构建点面残差 │
│ - ESIKF 状态更新 │
│
FAST-LIVO2/
├── include/
│ ├── LIVMapper.h # 主映射器（系统控制器）
│ ├── IMU_Processing.h # IMU 处理（状态传播、去畸变）
│ ├── vio.h # 视觉惯性里程计（VIO 模块）
│ ├── voxel_map.h # 体素地图（地图管理）
│ ├── preprocess.h # 预处理（点云降采样）
│ ├── frame.h # 图像帧（图像金字塔）
│ └── common_lib.h # 公共定义（状态向量、数据结构）
├── src/
│ ├── LIVMapper.cpp # 主逻辑（顺序更新流程）
│ ├── IMU_Processing.cpp # IMU 实现细节
│ ├── vio.cpp # VIO 实现细节
│ ├── voxel_map.cpp # 体素地图实现细节
│ └── main.cpp # ROS 入口
└── config/ # 配置文件（传感器参数、算法参数）
FASTLIVO2 系统通过四个核心模块的紧密协作，实现了激光雷达 - 视觉 - 惯性传感器的紧耦合。其创新点主要体现在：
系统采用'先激光雷达，后视觉'的顺序更新策略，既保证了维度匹配，又提高了计算效率。体素地图的双哈希表设计实现了激光雷达和视觉数据的统一管理，为多传感器融合提供了坚实的基础。
微信公众号「极客日志」，在微信中扫描左侧二维码关注。展示文案：极客日志 zeeklog
使用加密算法（如AES、TripleDES、Rabbit或RC4）加密和解密文本明文。 在线工具，加密/解密文本在线工具，online
生成新的随机RSA私钥和公钥pem证书。 在线工具，RSA密钥对生成器在线工具，online
基于 Mermaid.js 实时预览流程图、时序图等图表，支持源码编辑与即时渲染。 在线工具，Mermaid 预览与可视化编辑在线工具，online
将字符串编码和解码为其 Base64 格式表示形式即可。 在线工具，Base64 字符串编码/解码在线工具，online
将字符串、文件或图像转换为其 Base64 表示形式。 在线工具，Base64 文件转换器在线工具，online
将 Markdown（GFM）转为 HTML 片段，浏览器内 marked 解析；与 HTML转Markdown 互为补充。 在线工具，Markdown转HTML在线工具，online