FAST-LIO2激光惯性里程计工程优化实践

1. 项目背景与核心价值

去年接触LIO-SAM时就被激光-IMU紧耦合的前端里程计设计惊艳到，而FAST-LIO2作为其升级版本，在计算效率和点云配准精度上都有显著提升。这个开源项目特别适合做移动机器人定位算法的二次开发基础，但实际部署时会发现不少需要适配的细节。最近在给巡检机器人改造定位模块时，针对FAST-LIO2做了系列修改，这里把关键调整点整理成技术笔记。

FAST-LIO2的核心优势在于：

• 采用ikd-Tree实现动态更新的点云地图
• 前向传播与反向传播结合的IMU补偿机制
• 基于迭代误差状态卡尔曼滤波(IESKF)的紧耦合框架

但在实际工程落地时，我们发现原始代码存在几个典型问题：点云预处理耗时波动大、IMU初始化策略对剧烈运动敏感、地图保存功能缺失等。下面就从代码层面对这些痛点进行针对性优化。

2. 关键修改点解析

2.1 点云预处理耗时优化

原始代码的pointCloudPreprocess函数存在两个性能瓶颈：

1. 无效点过滤采用逐点判断方式
2. 点云下采样与特征提取顺序不合理

修改后的处理流程：

cpp复制// 修改后的预处理核心逻辑
void preprocess(PointCloudXYZI::Ptr cloud) {
    // 先做快速直通滤波移除超范围点
    pcl::PassThrough<pcl::PointXYZI> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.1, 5.0);
    pass.filter(*cloud);

    // 并行化无效点检测
    #pragma omp parallel for
    for (size_t i = 0; i < cloud->size(); ++i) {
        auto& p = cloud->points[i];
        if (p.x * p.x + p.y * p.y + p.z * p.z < 0.1) {
            p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN