当前位置：首页 > news >正文

faster_lio 原理及代码

news 2025/7/6 7:15:03

文章目录

faster_lio 原理及代码
- 原理类
- - 一、算法原理与核心创新
  - - 核心
    - 核心结构
    - - 伪希尔伯特曲线（PHC）vs 体素线性编码
      - 性能分析
  - 二、算法流程
  - 三、代码实现解析
  - - 1. 代码结构概览
    - 2. iVox结构实现
    - - 2.1 iVox基类 (`ivox3d_node.hpp`)
      - 2.2 iVox体素地图（`ivox3d.h`）
    - 3. 后端IESKF优化
    - - 3.3 残差构建（`ObsModel`）

faster_lio 原理及代码

论文：《Faster-LIO: Lightweight Tightly Coupled Lidar-inertial Odometry using Parallel Sparse Incremental Voxels》

原理类

Faster-LIO（Fast LiDAR-Inertial Odometry）是一种基于**增量稀疏体素（iVox）**的高效激光-惯性里程计算法，由智行者高博团队与清华大学于2022年提出。其核心目标是通过优化点云空间数据结构，显著提升传统LIO系统的计算效率，同时保持定位精度。

原始代码库：👉 https://github.com/gaoxiang12/faster-lio
支持Ubuntu 20.04/ROS Noetic/ 18.04/ROS Melodic。
推荐阅读-https://zhuanlan.zhihu.com/p/468628910

一、算法原理与核心创新

核心

1. iVox 增量稀疏体素建图
  替代传统树结构（如 ikd-Tree），iVox由空间中稀疏分布的体素(小方块)组成。每个体素内部可以存在多个点，体素自身的网格坐标由空间哈希函数映射到哈希键值上，再组成哈希表。：
- 空间哈希函数：将体素坐标映射为哈希键，实现 O(1) 复杂度插入/查询，一个体素块中的3D点映射为同一个哈希索引；
- 线性与PHC双模式：
  - 线性iVox：体素内点数少时直接暴力搜索；
  - PHC-iVox：体素内点数多时，用伪希尔伯特曲线对子空间编码，加速近邻搜索。
- 增量建图：iVox通过动态体素管理（添加时滤波、删除时LRU）实现高效实时地图更新
  - 添加
    - 核心机制：插入新点时动态创建体素，通过体素栅格滤波器过滤冗余点（若邻域已有更接近体素中心的点则跳过插入）。
    - 关键参数：叶尺寸（Leaf Size，默认0.5m），平衡精度与速度。
  - 删除
    - 传统问题：遍历体素哈希地图计算成本高。
    - 创新方案：采用LRU缓存策略（时间窗口淘汰），淘汰最旧体素（O(1)时间复杂度）。
      - 计算高效：添加/删除均为O(1)，适合实时LIO算法。
      - 资源可控：叶尺寸与LRU队列限制内存占用。
      - 鲁棒性：避免传统空间遍历开销，适应动态环境。
1. 紧耦合迭代卡尔曼滤波（IESKF）：
  融合IMU预积分与激光点云匹配残差，后端优化位姿。
性能优势
- 速度：固态雷达达 1000–2000 Hz，32线机械雷达超 200 Hz（CPU实时）；
- 精度：与Fast-LIO2相当，百米漂移误差 ≤1%。

核心结构

Faster-LIO延续了FastLIO2的紧耦合框架，结合迭代误差状态卡尔曼滤波（IESKF） 融合LiDAR点云与IMU数据。其创新点主要在于用iVox替代了传统的ikd-Tree，优化了局部地图的构建与近邻搜索效率。

1. 核心数据结构：iVox（Incremental Voxels）
iVox是一种增量式稀疏体素哈希表，通过空间哈希函数管理动态点云，解决传统体素与树结构（如kd-Tree）的维护开销问题：
- 稀疏性：仅在有实际点的位置分配体素，避免空体素的内存浪费。
- 哈希映射：
  将体素网格坐标 $voxel_size ⌋ v = \lfloor p / \text{voxel\_size} \rfloor$ 通过哈希函数映射到内存地址：
  $\text{hash}(v) = (v_x \cdot a \oplus v_y \cdot b \oplus v_z \cdot c) \mod N$
  其中 $a, b, c$ 为大质数， $N$ 为哈希表容量，减少碰撞概率。
- 冲突处理：依赖低负载因子（负载率<10%）和空间局部性，碰撞时返回无效体素，被后续查询忽略。
2. iVox的两种底层结构
针对不同点密度场景，iVox提供两种实现：
- 线性iVox：
  适用于稀疏点云。体素内部以线性数组存储点，近邻搜索时遍历所有点。时间复杂度 $O (n)$ ，但在点数较少时效率高。
- iVox-PHC（伪希尔伯特曲线）：
  适用于高密度点云。将体素细分为 $2^N \times 2^N \times 2^N$ 子单元，通过希尔伯特空间填充曲线将3D坐标映射到1D索引，实现 $O (1)$ 近邻查询。希尔伯特曲线保留了空间局部性，提升搜索效率。
3. 并行化与近似K-NN查询
- 并行插入/查询：利用OpenMP多线程，同时处理多个点的体素映射与近邻搜索。
- 近似搜索：不追求严格最近邻，通过限制搜索范围（固定体素邻域）牺牲少量精度换取速度，实验表明70%召回率即可满足LIO精度需求。

伪希尔伯特曲线（PHC）vs 体素线性编码

在三维点云处理中，传统的体素线性编码（如按 X-Y-Z 顺序叠加）与基于 伪希尔伯特曲线（PHC） 的编码相比，PHC 的核心优势在于其 空间局部性优化，显著提升了内存访问效率和近邻查询速度。

PHC 的核心价值

硬件友好性：连续内存访问模式最大化利用 CPU 缓存，减少内存带宽压力；
算法适配性：为激光雷达惯性里程计（如 Faster-LIO）提供 2000 Hz 高频数据处理能力；
扩展性：PHC 的线性映射可无缝结合并行计算（如 GPU 加速），支撑大规模点云实时处理。

简言之，PHC 通过 空间填充曲线的数学特性，将三维邻近性转化为一维连续性，从根本上解决了传统编码的内存访问低效问题，成为高性能 LIO/SLAM 算法的基石。

具体对比如下：

内存访问效率优化

传统 XYZ 叠加编码：
将三维坐标 (x,y,z) 映射为一维索引（如 index = x + y*W + z*W*H）。
- 缺陷：物理空间相邻的体素（如 (1,1,1) 和 (1,1,2)）索引可能不连续（例如间隔 W*H），导致内存访问跳跃，缓存命中率低。
PHC 编码：
通过伪希尔伯特曲线将三维空间线性化，邻近物理位置的体素映射为连续或接近连续的一维索引：
```
物理相邻体素 → PHC映射 → 连续内存地址
```
- 优势：内存访问连续性强，缓存命中率提升 30–50%，减少 CPU 等待时间。

近邻搜索加速

传统方法局限：
XYZ 编码中查找某体素的邻域需跨多个内存块（如沿 Z 轴搜索需跳跃 W*H 的地址偏移），计算复杂度高。
PHC 的优化：
由于邻近体素的索引连续，搜索邻域时：
- 定位中心体素的索引 Key_center；
- 直接访问 [Key_center - R, Key_center + R] 区间（R 为搜索半径）；
- 无需全局遍历，时间复杂度降至 O(1)（KD-Tree 为 O(n^{2/3})）。
  效果：k-NN 查询速度提升 5–10 倍。

动态更新效率

增量插入场景（如激光雷达连续输入点云）：
- XYZ 编码：新点插入可能破坏原有连续性，需频繁调整内存布局；
- PHC 编码：新体素按 Hilbert Key 插入连续内存，仅需局部调整，避免全局重构。
  - 优势：更新耗时降低 60–80%，适应实时 SLAM 需求。

指标	XYZ 叠加编码	PHC 编码
内存局部性	❌ 碎片化，缓存命中率低	✅ 连续存储，缓存命中率高
邻域查询速度	⚠️ O(n^{2/3})（慢）	✅ O(1)（微秒级响应）
增量更新成本	⚠️ 高（全局重构）	✅ 低（局部调整）
并行支持	❌ 弱（内存竞争）	✅ 强（线程安全区间访问）

注：复杂度与性能数据源自实际点云处理实验，详见参考文献。

性能分析

iVox：通过哈希表管理体素，实现了高效的点插入和近邻搜索。线性iVox适用于稀疏场景，而PHC iVox适用于密集场景。
IESKF：紧耦合IMU和LiDAR数据，通过迭代更新减少线性化误差。
残差模型：使用点到平面的距离作为残差，利用iVox快速构建局部平面。

效率优势

数据结构	插入耗时（μs/点）	K-NN查询（μs/点）
ikd-Tree	12.7	15.2
iVox-Linear	0.8	1.5
iVox-PHC	0.8	0.9
数据来源：仿真实验对比（N=10^5点）

实时性：
- 32线机械雷达：>200 Hz（FastLIO2的1.5-2倍）。
- 固态雷达（如Livox）：1000-2000 Hz。
精度对比
在KITTI、NCLT数据集上，平移误差（APE）与FastLIO2相当（<0.5%），但计算耗时降低60%。
适用场景
- 自动驾驶：低延迟定位（如城市道路实时导航）。
- 无人机：高速运动下的稠密建图。
- AR/VR：轻量级室内定位。

进一步研究方向

动态环境：未显式处理动态物体，可通过结合语义分割改进。
大规模地图：iVox适合局部地图，全局地图需结合分层结构（如子地图哈希）。
多传感器融合：未来可扩展视觉/雷达紧耦合。

二、算法流程

1. 数据预处理
- LiDAR点云：降采样、去畸变（利用IMU积分运动补偿）。
- IMU预积分：在相邻LiDAR帧间积分角速度与加速度，提供初始位姿估计。
2. 点云配准（Scan-to-Map）
- 近邻搜索：对当前帧每个点，查询iVox中局部地图的 $k$ 个近邻点（通常 $k = 5$ ）。
- 点面残差计算：基于近邻点拟合局部平面，计算点到平面的距离作为残差：
  $\mathbf{e}_i = \mathbf{n}_i^T (\mathbf{T} \cdot \mathbf{p}_i - \mathbf{q}_i)$
  其中， $\mathbf{T}$ 为待优化位姿， $\mathbf{n}_i$ 为平面法向量。
3. 状态优化（IESKF）
迭代优化位姿 $\mathbf{T}$ 和IMU偏差：
- 预测：IMU预积分提供状态初值。
- 更新：LiDAR残差构建观测方程，通过卡尔曼增益更新状态。
- 地图更新：将配准后的点云插入iVox，动态移除超出滑动窗口的旧点。

三、代码实现解析

1. 代码结构概览

基于提交：d7285fda6b0bbd339ad8ab73dafc55833fcc7f27：Merge pull request #93 from lizhengsi/merge_main

faster-lio/
├── CMakeLists.txt
├── LICENSE
├── README.md
├── config/
│   └── avia.yaml
│   └── horizon.yam
│   └── ouster64.yaml
│   └── velodyne.yaml
│   └── velodyne_liosam.yaml
│   └── velodyne_ulhk.yaml
│   └── velodyne_utbm.yaml
├── doc/
│   └── faster-lio.pdf
│   └── ...
├── **include/** 头文件
│   ├── lKFoM_toolkit/
│   │   ├── esekfom/
│   │   │   └── esekfom.hpp
│   │   │   └── util.hpp
│   │   ├── mtk/
│   │   │   ├── src/
│   │   │   │   └── SubManifold.hpp
│   │   │   │   └── mtkmath.hpp
│   │   │   │   └── vectview.hpp
│   │   │   ├── types/
│   │   │   │   └── S2.hpp
│   │   │   │   └── SOn.hpp
│   │   │   │   └── vect.hpp
│   │   │   │   └── wrapped_cv_mat.hpp
│   │   │   └── build_manifold.hpp
│   │   │   └── startIdx.hpp
│   ├── **ivox3d/** 增量体素结构实现
│   │   └── eigen_types.h：定义各种matrix及vector及hash数据。
│   │   └── hilbert.hpp：定义节点类型（线性/PHC），一个node对应一个grid。一个grid存放多个3D点。
│   │   └── ivox3d.h：定义grids_map_，以hash unordered_map 存放node grids。
│   │   └── ivox3d_node.hpp 
│   └── common lib.h：跟点云处理相关的一些底层接口，如距离计算，norm向量，rad&deg转换等等。
│   └── imu_processing.hpp：实现 IMU 数据读取、预积分、偏差估计、用于 IEKF 的运动预测。
│   └── laser_mapping.h：核心类声明，包括同步 LiDAR/IMU、IEKF 状态估计、IVox 地图构建、话题订阅/发布逻辑 。
│   └── options.h：一些硬编码常量（如 INIT\_TIME, LASER\_POINT\_COV 等）
│   └── pointcloud_preprocess.h：多种激光类型支持（AVIA, Velodyne, Ouster），实现滤波、range 限制、时间戳补偿等。
│   └── so3 math.h：6D李群李代数的运算接口，如exp，log等。
│   └── use-ikfom.hpp
│   └── utils.h
├── **src/**
│   └── laser_mapping.cc“实现 `LaserMapping` 类，负责参数读取（含 ROS 和 YAML）、初始化流程（在线/离线）、订阅 LiDAR+IMU、IEKF 迭代融合、地图更新、轨迹与点云输出
│   └── pointcloud_preprocess.cc：实现预处理逻辑：滤除盲区点、下采样、为每个点生成时间戳（Livox: offset\_time；Ouster: t；Velodyne: 时间/角度插值） 
│   └── options.cc
│   └── utils.cc
├── app/ slam入口，调用laser_mapping.h中的`Run()`
│   ├── run_mapping_online.cc：非 ROS 环境下，调用 `LaserMapping` 以在线方式运行
│   └── run_mapping_offline.cc：离线处理 rosbag 数据，支持 数据回放运行
├── launch/
│   └── mapping_avia.launch
│   └── mapping_velodyne.launch
│   └── mapping_horizon.launch
│   └── mapping_ouster64.launch
│   └── mapping_velodyne_liosam.launch
│   └── mapping_velodyne_ulhk.launch
│   └── mapping_velodyne_utbm.launch
├── rviz_cfg/
│   └── loam_livox.rviz
└── thirdparty/├── livox_ros_driver/：Livox 官方驱动代码，可被自动调用或注释掉避免冲突 └── tbb2018_20170726oss_lin.tgz└── ...

Faster-LIO的代码核心主要分为以下几个部分：

前端：包括IMU预处理、点云去畸变、运动补偿等。
后端：基于IESKF的状态估计和优化。
地图管理：使用iVox作为局部地图的存储和查询结构。
工具函数：如数学工具、参数配置等。

主流程： lasermapping.cpp：
- Run(): laser slam入口。
  –
- xxxPCLCallBack()：点云消息回调函数：处理点云，支持两种雷达，分别对应两个处理接口： StandardPCLCallBack/LivoxPCLCallBack。
- IMUCallBack()：IMU消息回调接口：推新的IMU数据到imu_buffer_。
  –
- SyncPackages()：同步雷达和点云数据。时间戳同步，消除点云畸变。
- ObsModel()：注册点云、调用eskf，构建解析点面残差。

Run():

void LaserMapping::Run() {if (!SyncPackages()) {return;}///IMU 预积分，校正、状态初始化p_imu_->Process(measures_, kf_, scan_undistort_);if (scan_undistort_->empty() || (scan_undistort_ == nullptr)) {LOG(WARNING) << "No point, skip this scan!";return;}/// the first scanif (flg_first_scan_) {state_point_ = kf_.get_x();scan_down_world_->resize(scan_undistort_->size());for (int i = 0; i < scan_undistort_->size(); i++) {PointBodyToWorld(&scan_undistort_->points[i], &scan_down_world_->points[i]);}ivox_->AddPoints(scan_down_world_->points);first_lidar_time_ = measures_.lidar_bag_time_;flg_first_scan_ = false;return;}flg_EKF_inited_ = (measures_.lidar_bag_time_ - first_lidar_time_) >= options::INIT_TIME;/// downsampleTimer::Evaluate([&, this]() {voxel_scan_.setInputCloud(scan_undistort_);voxel_scan_.filter(*scan_down_body_);},"Downsample PointCloud");int cur_pts = scan_down_body_->size();if (cur_pts < 5) {LOG(WARNING) << "Too few points, skip this scan!" << scan_undistort_->size() << ", " << scan_down_body_->size();return;}scan_down_world_->resize(cur_pts);nearest_points_.resize(cur_pts);residuals_.resize(cur_pts, 0);point_selected_surf_.resize(cur_pts, true);plane_coef_.resize(cur_pts, common::V4F::Zero());// ICP and iterated Kalman filter updateTimer::Evaluate([&, this]() {// iterated state estimationdouble solve_H_time = 0;// update the observation model, will call nn and point-to-plane residual computationkf_.update_iterated_dyn_share_modified(options::LASER_POINT_COV, solve_H_time);// save the statestate_point_ = kf_.get_x();euler_cur_ = SO3ToEuler(state_point_.rot);pos_lidar_ = state_point_.pos + state_point_.rot * state_point_.offset_T_L_I;},"IEKF Solve and Update");// update local mapTimer::Evaluate([&, this]() { MapIncremental(); }, "    Incremental Mapping");LOG(INFO) << "[ mapping ]: In num: " << scan_undistort_->points.size() << " downsamp " << cur_pts<< " Map grid num: " << ivox_->NumValidGrids() << " effect num : " << effect_feat_num_;// publish or save map pcdif (run_in_offline_) {if (pcd_save_en_) {PublishFrameWorld();}if (path_save_en_) {PublishPath(pub_path_);}} else {if (pub_odom_aft_mapped_) {PublishOdometry(pub_odom_aft_mapped_);}if (path_pub_en_ || path_save_en_) {PublishPath(pub_path_);}if (scan_pub_en_ || pcd_save_en_) {PublishFrameWorld();}if (scan_pub_en_ && scan_body_pub_en_) {PublishFrameBody(pub_laser_cloud_body_);}if (scan_pub_en_ && scan_effect_pub_en_) {PublishFrameEffectWorld(pub_laser_cloud_effect_world_);}}// Debug variablesframe_num_++;

参数配置：订阅topic/传感器内外参设定/地图/path及点云文件保存开启

common:
lid_topic: “/livox/lidar”
imu_topic: “/livox/imu”
…
preprocess:
lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR,
scan_line: 6
…
mapping:
acc_cov: 0.1
gyr_cov: 0.1
b_acc_cov: 0.0001
b_gyr_cov: 0.0001
extrinsic_est_en: false # true: 启动在线标定
extrinsic_T:
extrinsic_R:
…
publish:
path_publish_en: false
scan_publish_en: true # false: close all the point cloud output
scan_effect_pub_en: true # true: publish the pointscloud of effect point
dense_publish_en: false # false: low down the points number in a global-frame point clouds scan.
scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame
…
path_save_en: true # 保存轨迹，用于精度计算和比较
pcd_save:
pcd_save_en: true
interval: -1 # how many LiDAR frames saved in each pcd file; -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames.
feature_extract_enable: false
point_filter_num: 3
max_iteration: 3
filter_size_surf: 0.5
…
ivox_grid_resolution: 0.5 # default=0.2
ivox_nearby_type: 18 # 6, 18, 26
esti_plane_threshold: 0.1 # default=0.1

2. iVox结构实现

iVox的实现在include文件夹ivox3d.h和 ivox3d_node.hpp文件中。
ivox3d_node.hpp定义底层的两种node结构，IVoxNode和 IVoxNodePhc 分别对应线性以及PHC体素结构。node结构对应一个体素grid。一个体素grid中包含多个空间点。
iVox-PHC在每个体素内部分为更小的子单元（例如8x8x8），每个子单元存储一个点（或为空）。通过伪希尔伯特曲线（PHC）将子单元排序，使得空间相邻的子单元在曲线上也相邻。这样，查询时只需要按顺序遍历子单元，直到收集到k个点即可停止，效率很高。

2.1 iVox基类 (`ivox3d_node.hpp`)

class IVoxNode {}; //线性 grid
class IVoxNodePhc {}；//PHC grid//节点接口void InsertPoint(const PointT& pt);插入点inline PointT GetPoint(const std::size_t idx) const; 根据id 取点int KNNPointByCondition(...); 搜索最近邻

2.2 iVox体素地图（`ivox3d.h`）

定义了体素地图，存放体素们。

template <int dim = 3, IVoxNodeType node_type = IVoxNodeType::DEFAULT, typename PointType = pcl::PointXYZ>
class IVox {
... ...explicit IVox(Options options) : options_(options) {options_.inv_resolution_ = 1.0 / options_.resolution_;GenerateNearbyGrids();}void AddPoints(const PointVector& points_to_add); //添加point到地图，调用IVoxNode 中的InsertPointbool GetClosestPoint(xxx); //最近邻搜索
... ...private:
... ...std::unordered_map<KeyType, typename std::list<std::pair<KeyType, NodeType>>::iterator, hash_vec<dim>>grids_map_; // **体素hase map**
... ...std::vector<KeyType> nearby_grids_;   // 最近邻点的id vector

3. 后端IESKF优化

在lasermapping.cpp中，主要流程如下：

3.1 状态变量

// 状态向量包括：位置、速度、旋转（四元数）、加速度计偏置、陀螺仪偏置
state_ikfom state_point;

3.2 主回调函数（xxxPCLCallBack()&IMUCallBack）

 /*** 点云回调处理通用流程* @param cloud_msg 输入的点云消息（标准PCL或Livox格式）* @param is_livox 是否为Livox格式点云的标志*/
void LaserMapping::xxxPCLCallback(const sensor_msgs::PointCloud2::ConstPtr &msg) {
{// 加锁保护共享缓冲区Lock mutex_buffer// 评估代码块执行时间StartTimer("PointCloudPreprocessing")：// 统计点云扫描次数scan_count_++// 获取当前点云时间戳current_timestamp = cloud_msg.header.stamp.toSec()// 时间戳回退检测与处理if current_timestamp < last_timestamp_lidar_ thenLogError("Lidar timestamp loop back detected, clearing buffer")Clear lidar_buffer_end if// 更新最新点云时间戳last_timestamp_lidar_ = current_timestamp// Livox点云特殊处理（时间同步相关）if is_livox then// 检查IMU与LiDAR时间是否不同步if !time_sync_en_ and abs(last_timestamp_imu_ - last_timestamp_lidar_) > 10.0 and !imu_buffer_.empty() and !lidar_buffer_.empty() thenLogInfo("IMU and LiDAR time not synced")end if// 自动计算时间同步偏移量（首次执行）if time_sync_en_ and !timediff_set_flg_ and abs(last_timestamp_lidar_ - last_timestamp_imu_) > 1 and !imu_buffer_.empty() thentimediff_set_flg_ = truetimediff_lidar_wrt_imu_ = last_timestamp_lidar_ + 0.1 - last_timestamp_imu_LogInfo("Auto-synced IMU and LiDAR with time diff: " + timediff_lidar_wrt_imu_)end ifend if// 创建点云对象并预处理point_cloud_ptr = new PointCloudType()// 根据点云类型调用不同的预处理函数if is_livox thenpreprocess_->Process(cloud_msg as livox_msg, point_cloud_ptr)elsepreprocess_->Process(cloud_msg as pcl_msg, point_cloud_ptr)end if// 将预处理后的点云存入缓冲区lidar_buffer_.push_back(point_cloud_ptr)time_buffer_.push_back(current_timestamp)// 结束计时并记录耗时EndTimer("PointCloudPreprocessing")// 解锁共享缓冲区Unlock mutex_buffer
}

/*** IMU数据回调函数 - 处理接收到的IMU消息* @param msg_in 接收到的IMU消息*/
void LaserMapping::IMUCallBack(const sensor_msgs::Imu::ConstPtr &msg_in) {// 统计回调次数，用于控制发布频率publish_count_++;// 复制IMU消息，避免修改原始数据sensor_msgs::Imu::Ptr msg(new sensor_msgs::Imu(*msg_in));// 时间同步处理：如果启用了时间同步且与LiDAR的时间差超过0.1秒if (abs(timediff_lidar_wrt_imu_) > 0.1 && time_sync_en_) {// 调整IMU时间戳，使其与LiDAR时间戳对齐msg->header.stamp = ros::Time().fromSec(timediff_lidar_wrt_imu_ + msg_in->header.stamp.toSec());}// 获取当前IMU消息的时间戳double timestamp = msg->header.stamp.toSec();// 加锁保护IMU缓冲区，防止多线程冲突mtx_buffer_.lock();// 检测时间戳回退（可能由于传感器或驱动问题）if (timestamp < last_timestamp_imu_) {LOG(WARNING) << "imu loop back, clear buffer";// 时间回退时清空缓冲区，避免使用过时数据imu_buffer_.clear();}// 更新最新时间戳并将IMU消息加入缓冲区last_timestamp_imu_ = timestamp;imu_buffer_.emplace_back(msg);// 释放锁mtx_buffer_.unlock();
}

3.3 残差构建（`ObsModel`）

Faster-LIO 中紧耦合优化的核心部分，通过构建点到平面的观测模型，将 LiDAR 点云信息融入到 EKF 状态估计框架中。函数执行流程分为两个主要阶段：

点云匹配与残差计算：
- 将当前帧点云从 body 坐标系转换到世界坐标系
- 使用 iVox 索引结构查找每个点的最近邻点
- 基于最近邻点估计平面参数
- 计算点到平面的距离作为残差
- 进行有效性检查，过滤不可靠的匹配
雅可比矩阵计算：
- 为每个有效匹配点构建观测雅可比矩阵
- 雅可比矩阵包含位置、姿态以及可选的外参部分
- 根据是否启用外参估计决定雅可比矩阵的结构
- 设置测量值向量为点到平面距离的负值

/*** Lidar point cloud registration* will be called by the eskf custom observation model* compute point-to-plane residual here* @param s kf state - 当前KF状态估计* @param ekfom_data H matrix - 用于存储观测雅可比矩阵和测量值*/
void LaserMapping::ObsModel(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data) {// 获取当前降采样点云的点数int cnt_pts = scan_down_body_->size();// 创建点索引数组并初始化std::vector<size_t> index(cnt_pts);for (size_t i = 0; i < index.size(); ++i) {index[i] = i;}// 评估点云匹配部分的执行时间Timer::Evaluate([&, this]() {// 计算LiDAR到世界坐标系的旋转矩阵auto R_wl = (s.rot * s.offset_R_L_I).cast<float>();// 计算LiDAR到世界坐标系的平移向量auto t_wl = (s.rot * s.offset_T_L_I + s.pos).cast<float>();/** closest surface search and residual computation **/// 并行处理每个点，计算点到平面的残差std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const size_t &i) {// 获取当前点在body坐标系和世界坐标系下的引用PointType &point_body = scan_down_body_->points[i];PointType &point_world = scan_down_world_->points[i];/* transform to world frame */// 将点从body坐标系转换到世界坐标系common::V3F p_body = point_body.getVector3fMap();point_world.getVector3fMap() = R_wl * p_body + t_wl;point_world.intensity = point_body.intensity;// 获取当前点的最近邻点集合引用auto &points_near = nearest_points_[i];// 如果EKF已收敛，则进行最近平面搜索if (ekfom_data.converge) {/** Find the closest surfaces in the map **/// 使用iVox索引结构查找最近的几个点ivox_->GetClosestPoint(point_world, points_near, options::NUM_MATCH_POINTS);// 检查是否找到了足够的近邻点point_selected_surf_[i] = points_near.size() >= options::MIN_NUM_MATCH_POINTS;if (point_selected_surf_[i]) {// 估计平面参数（ax+by+cz+d=0）point_selected_surf_[i] =common::esti_plane(plane_coef_[i], points_near, options::ESTI_PLANE_THRESHOLD);}}// 如果成功找到合适的平面if (point_selected_surf_[i]) {// 计算点到平面的距离auto temp = point_world.getVector4fMap();temp[3] = 1.0;float pd2 = plane_coef_[i].dot(temp);// 有效性检查：点到平面的距离应该小于点到原点距离的一定比例bool valid_corr = p_body.norm() > 81 * pd2 * pd2;if (valid_corr) {point_selected_surf_[i] = true;residuals_[i] = pd2;  // 存储点到平面的残差}}});},"    ObsModel (Lidar Match)");// 统计有效的特征点数量effect_feat_num_ = 0;// 调整存储匹配点和法向量的容器大小corr_pts_.resize(cnt_pts);corr_norm_.resize(cnt_pts);// 收集所有有效的匹配点和对应的法向量for (int i = 0; i < cnt_pts; i++) {if (point_selected_surf_[i]) {corr_norm_[effect_feat_num_] = plane_coef_[i];corr_pts_[effect_feat_num_] = scan_down_body_->points[i].getVector4fMap();corr_pts_[effect_feat_num_][3] = residuals_[i];  // 存储残差值effect_feat_num_++;}}// 调整容器大小为有效特征点数量corr_pts_.resize(effect_feat_num_);corr_norm_.resize(effect_feat_num_);// 如果没有找到有效的匹配点，设置观测无效并返回if (effect_feat_num_ < 1) {ekfom_data.valid = false;LOG(WARNING) << "No Effective Points!";return;}// 评估雅可比矩阵计算部分的执行时间Timer::Evaluate([&, this]() {/*** Computation of Measurement Jacobian matrix H and measurements vector ***/// 初始化观测雅可比矩阵 H（每个有效点对应一行，每行12列：位置(3)、姿态(3)、外参旋转(3)、外参平移(3)）ekfom_data.h_x = Eigen::MatrixXd::Zero(effect_feat_num_, 12);// 初始化测量值向量ekfom_data.h.resize(effect_feat_num_);// 重置索引数组大小为有效特征点数量index.resize(effect_feat_num_);// 获取LiDAR到IMU的旋转外参const common::M3F off_R = s.offset_R_L_I.toRotationMatrix().cast<float>();// 获取LiDAR到IMU的平移外参const common::V3F off_t = s.offset_T_L_I.cast<float>();// 获取IMU旋转的转置矩阵const common::M3F Rt = s.rot.toRotationMatrix().transpose().cast<float>();// 并行计算每个有效点的雅可比矩阵行std::for_each(std::execution::par_unseq, index.begin(), index.end(), [&](const size_t &i) {// 获取当前点在body坐标系下的坐标common::V3F point_this_be = corr_pts_[i].head<3>();// 计算该点的反对称矩阵（用于叉乘运算）common::M3F point_be_crossmat = SKEW_SYM_MATRIX(point_this_be);// 将点从body坐标系转换到IMU坐标系common::V3F point_this = off_R * point_this_be + off_t;// 计算转换后点的反对称矩阵common::M3F point_crossmat = SKEW_SYM_MATRIX(point_this);/*** get the normal vector of closest surface/corner ***/// 获取最近平面的法向量common::V3F norm_vec = corr_norm_[i].head<3>();/*** calculate the Measurement Jacobian matrix H ***/// 计算中间变量C = R_t * norm_vec，用于雅可比矩阵计算common::V3F C(Rt * norm_vec);// 计算中间变量A = [point_crossmat] * C，与姿态相关的雅可比部分common::V3F A(point_crossmat * C);// 根据是否启用外参估计设置不同的雅可比矩阵if (extrinsic_est_en_) {// 启用外参估计：雅可比矩阵包含外参部分common::V3F B(point_be_crossmat * off_R.transpose() * C);ekfom_data.h_x.block<1, 12>(i, 0) << norm_vec[0], norm_vec[1], norm_vec[2], A[0], A[1], A[2], B[0],B[1], B[2], C[0], C[1], C[2];} else {// 不启用外参估计：雅可比矩阵不包含外参部分ekfom_data.h_x.block<1, 12>(i, 0) << norm_vec[0], norm_vec[1], norm_vec[2], A[0], A[1], A[2], 0.0,0.0, 0.0, 0.0, 0.0, 0.0;}/*** Measurement: distance to the closest surface/corner ***/// 设置测量值为负的点到平面距离（负号是因为测量模型通常定义为 z = h(x) + v）ekfom_data.h(i) = -corr_pts_[i][3];});},"    ObsModel (IEKF Build Jacobian)");
}