LIO-SAM系统架构

加油JIAX2026-04-14 11:16

目录

1.系统架构

[2.点云预处理节点 /lio_sam_imageProjection](#2.点云预处理节点 /lio_sam_imageProjection)

[2.1 缓存与格式校验 cachePointCloud](#2.1 缓存与格式校验 cachePointCloud)

[2.2 获取运动补偿信息 deskewInfo](#2.2 获取运动补偿信息 deskewInfo)

[2.3 深度图投影与单点去畸变 projectPointCloud](#2.3 深度图投影与单点去畸变 projectPointCloud)

[2.4 提取有效信息 cloudExtraction](#2.4 提取有效信息 cloudExtraction)

[3.特征提取节点 /lio_sam_featureExtraction](#3.特征提取节点 /lio_sam_featureExtraction)

[3.1 计算曲率/平滑度 calculateSmoothness](#3.1 计算曲率/平滑度 calculateSmoothness)

[3.2 标记不可靠的点 markOccludedPoints](#3.2 标记不可靠的点 markOccludedPoints)

[3.3 提取特征点 extractFeatures](#3.3 提取特征点 extractFeatures)

[4.前后端优化节点 /lio_sam_mapOptimization](#4.前后端优化节点 /lio_sam_mapOptimization)

[4.1 获取初始位姿猜测 updateInitialGuess](#4.1 获取初始位姿猜测 updateInitialGuess)

[4.2 提取局部地图 extractSurroundingKeyFrames](#4.2 提取局部地图 extractSurroundingKeyFrames)

[4.3 当前帧下采样 downsampleCurrentScan](#4.3 当前帧下采样 downsampleCurrentScan)

[4.4 Scan-to-Map优化 scan2MapOptimization](#4.4 Scan-to-Map优化 scan2MapOptimization)

[4.5 保存关键帧与因子图优化 saveKeyFramesAndFactor](#4.5 保存关键帧与因子图优化 saveKeyFramesAndFactor)

[4.6 校正历史位姿 correctPoses](#4.6 校正历史位姿 correctPoses)

[4.7 发布里程计 publishOdometry](#4.7 发布里程计 publishOdometry)

[5.imu预积分节点 /lio_sam_imuPreintegration](#5.imu预积分节点 /lio_sam_imuPreintegration)

[5.1 IMUPreintegration类](#5.1 IMUPreintegration类)

[5.1.1 类内成员](#5.1.1 类内成员)

[5.1.2 构造函数](#5.1.2 构造函数)

[5.1.3 优化器与因子图重置](#5.1.3 优化器与因子图重置)

[5.1.4 因子图优化修正积分值](#5.1.4 因子图优化修正积分值)

[5.1.5 失败检测](#5.1.5 失败检测)

[5.1.6 高频积分预测](#5.1.6 高频积分预测)

[5.2 TransformFusion类](#5.2 TransformFusion类)

[5.2.1 构造函数](#5.2.1 构造函数)

[5.2.2 工具函数](#5.2.2 工具函数)

[5.2.3 lidar全局位姿接收回调函数](#5.2.3 lidar全局位姿接收回调函数)

[5.2.4 imu局部里程计回调函数](#5.2.4 imu局部里程计回调函数)

1.系统架构

暂时不启用GPS。上图为LIO-SAM运行时，通过rqt_graph查看得到的节点关系图。可见其中主要有四个节点协同工作，分别为：点云预处理节点/lio_sam_imageProjection 、特征提取节点/lio_sam_featureExtraction 、前后端优化节点/lio_sam_mapOptimization 、imu预积分节点/lio_sam_imuPreintegration。

下图为LIO-SAM作者给出的系统架构图：

系统中维护了两个因子图。imuPreintegration.cpp中前端的因子图以高频IMU预积分量和低频雷达增量里程计构建图优化，主要用于估计准确的系统速度和 IMU 零偏，并定期重置，防止计算量爆炸，保证在 IMU 频率下实时输出相对平滑的局部里程计。mapOptimization.cpp中后端的因子图优化了激光雷达里程因子、闭环因子和 GPS 因子（可选）。

imageProjection.cpp、featureExtraction.cpp、mapOptimization.cpp的内容比较好懂一些，所以先简单介绍一下这三个文件的代码内容，最后再详细地看一下imuPreintegration.cpp中imu预积分是怎样参与到整个SLAM过程中的。

2.点云预处理节点 /lio_sam_imageProjection

该节点订阅：

  1. 原始点云数据(/points_raw)
  2. 原始imu数据(/imu_correct)
  3. 局部状态估计器预测的lidar位姿(/odometry/imu_incremental)

发布：

  1. 去畸变后的点云(/lio_sam/deskew/cloud_deskewed)
  2. 携带完整数据（包含点起止索引、去畸变后的点云等）的cloud_info(/lio_sam/deskew/cloud_info)

需要注意的是，/odometry/imu_incremental是一个纯局部的、带有累积漂移的、没有包含回环和GPS修正的里程计 T(odom_lidar) 。所以使用的时候并不会直接使用T(odom_lidar) ，而是用其计算帧间的相对位姿增量，再拿来进行点云去畸变以及作为scan2map的预测位姿。后面会介绍**/odometry/imu_incremental**是什么以及如何使用它。

imuHandler()和odometryHandler()只将imu数据与imu预测位姿保存到队列。

cloudHandler()的内容多一点：

cpp 复制代码 
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    if (!cachePointCloud(laserCloudMsg))// 1. 缓存并统一数据格式
        return;

    if (!deskewInfo())                  // 2. 获取去畸变所需的IMU和里程计信息
        return;

    projectPointCloud();                // 3. 点云投影到距离图并逐点去畸变

    cloudExtraction();                  // 4. 提取有效点云并记录索引信息

    publishClouds();                    // 5. 发布处理后的点云，供下一环节使用

    resetParameters();                  // 6. 重置参数，准备处理下一帧
}

2.1 缓存与格式校验 cachePointCloud

（1）队列管理：将接收到的ROS点云消息存入队列，保证队列中至少有两帧数据才开始处理。

（2）格式转换 ：LIO-SAM为了兼容多种雷达（Velodyne, Ouster, Livox），会在这一步将非Velodyne格式（如Ouster）的字段（如强度、时间戳t等）映射到统一的 PointXYZIRT (X, Y, Z, Intensity, Ring, Time) 结构体中。

（3）时间戳对齐 ：计算当前帧的起始时间 timeScanCur 和结束时间 timeScanEnd。

（4）字段校验 ：检查点云是否包含必要的 ring（线号）和 time（相对时间戳）字段。如果没有time字段，将无法进行去畸变操作。

2.2 获取运动补偿信息 deskewInfo

通过imuDeskewInfo() 在当前帧的时间范围内，对IMU的角速度进行积分，得到imu数据每一时刻相对于帧起始时刻的姿态变化（Roll, Pitch, Yaw）。

通过odomDeskewInfo() 获取该lidar扫描帧扫描起始时刻和结束时刻的预测位姿，计算出相对位姿变化 。并将扫描起始时刻对应的预测位姿保存进cloud_info，供后续scan2map使用：

2.3 深度图投影与单点去畸变 projectPointCloud

这是最核心的一步。遍历当前帧的每一个点进行：

（1）距离过滤：剔除过近或过远的点。

（2）计算二维索引 ：由于使用的是机械旋转线雷达，每个点都有其线束号（行号）。再根据点的X和Y坐标计算偏航角，从而换算出列号。这样就把3D空间点投影到了 N_SCAN x Horizon_SCAN（例如16线x1800列）的二维深度图矩阵中。

（3）网格过滤：在网格投影过程进行激光点过滤，如果该二维网格已经被填充过（即存在更早投影过来的点），则丢弃当前点。

（4）单点去畸变 (deskewPoint) ：根据该点的相对时间戳 time，在步骤2计算好的IMU和里程计序列中进行插值，得到该点时刻传感器的真实位姿，然后通过坐标变换将该点拽回到当前帧的起始时刻坐标系下。

2.4 提取有效信息 cloudExtraction

（1）遍历投影好的深度图矩阵 rangeMat。

（2）将去畸变后的有效点按顺序依次放入一个一维数组 extractedCloud 中。

（3）记录每一根线（Ring）在 extractedCloud 中的起始索引（startRingIndex）和结束索引（endRingIndex）。这一步极为重要，因为下一环节 featureExtraction.cpp 需要根据这些索引计算局部曲率，提取边缘（Edge）特征和平面（Planar）特征。

3.特征提取节点 /lio_sam_featureExtraction

该节点订阅：

  1. 携带完整数据（包含点起止索引、去畸变后的点云等）的(/lio_sam/deskew/cloud_info)

发布：

  1. 边缘点云(/lio_sam/feature/cloud_corner)
  2. 平面点云(/lio_sam/feature/cloud_surface)
  3. 附加边缘点云和平面点云之后的cloud_info(/lio_sam/feature/cloud_info)

featureExtraction.cpp 的主要任务是从去畸变后的点云中，提取出角点和面点。提取高质量的特征点可以大幅减少参与后端优化的数据量，提高里程计配准的精度和鲁棒性。

laserCloudInfoHandler 是这个节点的核心回调函数：

cpp 复制代码 
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
    cloudInfo = *msgIn; // 1. 获取包含各种索引和深度信息的结构体
    cloudHeader = msgIn->header; 
    pcl::fromROSMsg(msgIn->cloud_deskewed, *extractedCloud); // 解析出去畸变后的点云

    calculateSmoothness(); // 2. 计算每个点的曲率（平滑度）

    markOccludedPoints();  // 3. 标记被遮挡的点和平行光束点，防止它们被误提取为特征

    extractFeatures();     // 4. 核心：根据曲率提取角点和面点

    publishFeatureCloud(); // 5. 发布特征点云，供后端 mapOptimization 使用
}

3.1 计算曲率/平滑度 calculateSmoothness

LIO-SAM沿用了LOAM的做法：选取该点左右两侧各5个相邻点（共10个点），计算它们的深度之和，减去该点深度的10倍。如果在一个平坦表面上，周围深度和中心深度差不多，这个差值接近0；如果在边缘上，差值会变大。

3.2 标记不可靠的点 markOccludedPoints

在提取特征前，需要剔除一些容易造成误匹配的伪特征，如上图中遮挡点B 与平行光束点C。

遮挡点 (Occluded points)：如果相邻两个点在图像列索引上很近，但深度差异很大（>0.3米），说明发生了遮挡。背景物体边缘上的点往往是不稳定的（稍微换个视角可能就扫不到了），因此把较远那侧的连续几个点标记为不可用。

平行光束点 (Parallel beam points)：如果激光束几乎平行于物体表面照射，测距往往是不准的。代码通过判断该点与其前后相邻点的深度差是否都超过了自身深度的 2% 来识别这种情况。

cpp 复制代码 
void markOccludedPoints()
{
    int cloudSize = extractedCloud->points.size();
    // 标记遮挡点和平行光束点
    for (int i = 5; i < cloudSize - 6; ++i)
    {
        // 1. 遮挡点处理
        float depth1 = cloudInfo.pointRange[i];
        float depth2 = cloudInfo.pointRange[i+1];
        // 计算两个相邻点在深度图上的列索引偏差（是否是物理上紧挨着的扫描点）
        int columnDiff = std::abs(int(cloudInfo.pointColInd[i+1] - cloudInfo.pointColInd[i]));

        // 如果列索引相差不到10（说明是连续扫描的）
        if (columnDiff < 10){
            // 如果深度差大于0.3米，说明发生了断层（深度突变）
            if (depth1 - depth2 > 0.3){
                // depth1 更远，它是背景。背景边缘上的点在下一次扫描时极易丢失，因此将其前面连续5个点屏蔽
                cloudNeighborPicked[i - 5] = 1;
                cloudNeighborPicked[i - 4] = 1;
                cloudNeighborPicked[i - 3] = 1;
                cloudNeighborPicked[i - 2] = 1;
                cloudNeighborPicked[i - 1] = 1;
                cloudNeighborPicked[i] = 1;
            }else if (depth2 - depth1 > 0.3){
                // depth2 更远，同理屏蔽后面的点
                cloudNeighborPicked[i + 1] = 1;
                cloudNeighborPicked[i + 2] = 1;
                cloudNeighborPicked[i + 3] = 1;
                cloudNeighborPicked[i + 4] = 1;
                cloudNeighborPicked[i + 5] = 1;
                cloudNeighborPicked[i + 6] = 1;
            }
        }
        
        // 2. 平行光束点处理
        // 当前点与左右相邻点的深度差
        float diff1 = std::abs(float(cloudInfo.pointRange[i-1] - cloudInfo.pointRange[i]));
        float diff2 = std::abs(float(cloudInfo.pointRange[i+1] - cloudInfo.pointRange[i]));

        // 如果左右偏差都大于当前深度的2%，认为激光束几乎平行于打到的面，这种点测距不准，丢弃
        if (diff1 > 0.02 * cloudInfo.pointRange[i] && diff2 > 0.02 * cloudInfo.pointRange[i])
            cloudNeighborPicked[i] = 1;
    }
}

3.3 提取特征点 extractFeatures

这是最重要的一步。为了保证提取的特征点在空间上分布均匀，代码将每一根扫描线（Ring）平均分成了 6 个子区域（扇区）。对每个子区域分别进行提取：

（1）按曲率排序：在当前子区域内，按照曲率从小到大排序。

（2）提取角点：从曲率最大的一端开始倒序遍历。如果点未被筛选过，且曲率大于阈值 edgeThreshold，则判定为角点，放入 cornerCloud（最多提取20个）。为了防止角点扎堆，提取后会将该点及其左右相邻的几个点标记为"已选择"（cloudNeighborPicked = 1）。

（3）标记平坦面点：从曲率最小的一端开始正序遍历。如果点未被筛选过，且曲率小于阈值 surfThreshold，则将其标记为面点（cloudLabel = -1），并同样屏蔽其相邻点。

（4）收集面点 ：实际上，LIO-SAM提取面点的方式更为粗犷。除了被明确判定为角点的点（cloudLabel == 1），剩下的所有点（cloudLabel <= 0）都被统一扔进 surfaceCloudScan 中当作面点候选。

（5）面点下采样 ：由于剩下的点非常多，直接给后端优化会吃不消。因此对每一线的面点使用体素滤波进行下采样，最后汇总到 surfaceCloud 中。

cpp 复制代码 
void extractFeatures()
{
    cornerCloud->clear();
    surfaceCloud->clear();

    pcl::PointCloud<PointType>::Ptr surfaceCloudScan(new pcl::PointCloud<PointType>());
    pcl::PointCloud<PointType>::Ptr surfaceCloudScanDS(new pcl::PointCloud<PointType>());

    // 遍历每一根扫描线（Ring）
    for (int i = 0; i < N_SCAN; i++)
    {
        surfaceCloudScan->clear();

        // 为了使特征分布均匀，将每根线均分为 6 个扇区
        for (int j = 0; j < 6; j++)
        {
            // 计算当前扇区的起始和结束点在一维数组中的索引
            int sp = (cloudInfo.startRingIndex[i] * (6 - j) + cloudInfo.endRingIndex[i] * j) / 6;
            int ep = (cloudInfo.startRingIndex[i] * (5 - j) + cloudInfo.endRingIndex[i] * (j + 1)) / 6 - 1;

            if (sp >= ep) continue;

            // 在当前扇区内，按平滑度(曲率)从小到大排序
            std::sort(cloudSmoothness.begin()+sp, cloudSmoothness.begin()+ep, by_value());

            // ---- A. 提取角点 (曲率大) ----
            int largestPickedNum = 0;
            // 从后往前遍历（因为后面曲率大）
            for (int k = ep; k >= sp; k--)
            {
                int ind = cloudSmoothness[k].ind;
                // 如果点有效且曲率大于设定阈值
                if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > edgeThreshold)
                {
                    largestPickedNum++;
                    // 每个扇区最多提取20个角点
                    if (largestPickedNum <= 20){
                        cloudLabel[ind] = 1; // 标记为角点
                        cornerCloud->push_back(extractedCloud->points[ind]);
                    } else {
                        break;
                    }

                    // 特征点不能扎堆：将该点及相连的相邻点标记为不可用
                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l <= 5; l++) {
                        // 判断是否处于同一扫描段（防止越过遮挡断层）
                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
                        if (columnDiff > 10) break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--) {
                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
                        if (columnDiff > 10) break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            // ---- B. 标记面点 (曲率小) ----
            // 从前往后遍历（前面曲率小）
            for (int k = sp; k <= ep; k++)
            {
                int ind = cloudSmoothness[k].ind;
                // 如果点有效且曲率小于设定阈值
                if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < surfThreshold)
                {
                    cloudLabel[ind] = -1; // 标记为面点
                    cloudNeighborPicked[ind] = 1;

                    // 同理，屏蔽相邻点防止扎堆
                    for (int l = 1; l <= 5; l++) {
                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l - 1]));
                        if (columnDiff > 10) break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--) {
                        int columnDiff = std::abs(int(cloudInfo.pointColInd[ind + l] - cloudInfo.pointColInd[ind + l + 1]));
                        if (columnDiff > 10) break;
                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            // ---- C. 收集所有可能的面点 ----
            for (int k = sp; k <= ep; k++)
            {
                // 注意：这里并非只取 cloudLabel == -1 的点，而是把所有未被标记为角点的点(<=0)都当作面点候选
                if (cloudLabel[k] <= 0){
                    surfaceCloudScan->push_back(extractedCloud->points[k]);
                }
            }
        }

        // ---- D. 面点下采样 ----
        surfaceCloudScanDS->clear();
        // 对当前线(Ring)收集到的面点进行体素滤波，大幅减少数据量
        downSizeFilter.setInputCloud(surfaceCloudScan);
        downSizeFilter.filter(*surfaceCloudScanDS);

        // 将下采样后的当前线面点加入总面点集合中
        *surfaceCloud += *surfaceCloudScanDS;
    }
}

4.前后端优化节点 /lio_sam_mapOptimization

该节点订阅：

  1. /lio_sam/feature/cloud_info

发布：

  1. 闭环约束(/lio_sam/mapping/loop_closure_constrains)
  2. 后端优化后的轨迹(/lio_sam/mapping/path)
  3. 经过后端优化后位姿转换到odom坐标系的当前点云(/lio_sam/mapping/cloud_registered)
  4. scan2map的先验位姿与后验位姿误差累积的位姿(/lio_sam/mapping/odometry_incremental)

mapOptimization节点同时完成前端的scan2map与后端因子图优化。laserCloudInfoHandler 是这个后端模块的主回调函数，整个后端的运作全部由这个函数来调度。

cpp 复制代码 
void laserCloudInfoHandler(const lio_sam::cloud_infoConstPtr& msgIn)
{
    // extract time stamp
    timeLaserInfoStamp = msgIn->header.stamp;
    timeLaserInfoCur = msgIn->header.stamp.toSec();

    // extract info and feature cloud
    cloudInfo = *msgIn;
    pcl::fromROSMsg(msgIn->cloud_corner,  *laserCloudCornerLast);
    pcl::fromROSMsg(msgIn->cloud_surface, *laserCloudSurfLast);

    std::lock_guard<std::mutex> lock(mtx);

    // 控制后端优化的频率。因为 Scan-to-Map 和因子图优化比较耗时，没必要每一帧都处理
    static double timeLastProcessing = -1;
    if (timeLaserInfoCur - timeLastProcessing >= mappingProcessInterval)
    {
        timeLastProcessing = timeLaserInfoCur;

        updateInitialGuess();           // 1.更新初始猜测位姿

        extractSurroundingKeyFrames();  // 2.提取局部地图

        downsampleCurrentScan();        // 3.下采样当前帧扫描点云

        scan2MapOptimization();         // 4.Scan-to-Map

        saveKeyFramesAndFactor();       // 5.关键帧判断、因子图构建与优化

        correctPoses();                 // 6.修正历史关键帧位姿

        publishOdometry();              // 7.发布激光里程计

        publishFrames();                // 8.发布各类可视化点云帧
    }
}

4.1 获取初始位姿猜测 updateInitialGuess

在任何基于优化的 SLAM 系统中，位姿的初始猜测都至关重要。点云配准（Scan-to-Map）本质上是一个寻找非线性目标函数局部最优解的过程。如果初始位姿给得很差，优化算法极易陷入局部极小值，导致点云匹配错位甚至建图崩溃；如果初始位姿给得很准，算法只需极少的迭代次数就能收敛，极大提升了系统的实时性和鲁棒性。

LIO-SAM的初始猜测主要依赖于IMU 预积分节点 (imuPreintegration) 输出的高频预测 /odometry/imu_incremental。但**/odometry/imu_incremental是一个纯局部的、带有累积漂移的、没有包含回环和GPS修正的里程计** 。所以使用的时候并不会直接使用**/odometry/imu_incremental**，而是用其计算帧间的相对位姿增量，叠加到上一时刻的最优位姿上，再作为scan2map的预测位姿。

updateInitialGuess()涵盖了三种不同情况下的处理策略，优先级依次递减：

  1. 系统初始化（第一帧）：没有任何历史轨迹，直接提取原点，并用 IMU 提供的绝对姿态对齐重力方向。
  2. 正常运行状态 ：这是最常见的情况。利用 imuPreintegration 节点发来的前一时刻到当前时刻的位姿增量（包含平移和旋转），叠加到上一帧优化好的地图位姿上，得到当前帧的先验位姿。
  3. 退化/降级状态（仅有 IMU 原始旋转） ：如果预积分里程计失效或未准备好，退而求其次，仅利用 IMU 的角速度积分得到的旋转增量来进行姿态预测。
cpp 复制代码 
void updateInitialGuess()
{
    // 1. 保存前一帧优化后的位姿
    // 在该函数执行前，transformTobeMapped 中存储的还是【上一帧】配准优化后的最终位姿
    // 将其保存到 incrementalOdometryAffineFront 中，后续用于计算增量里程计发布给 ROS
    incrementalOdometryAffineFront = trans2Affine3f(transformTobeMapped);

    // 用于记录上一次的 IMU 绝对姿态（仅含 roll, pitch, yaw，平移为0）
    static Eigen::Affine3f lastImuTransformation; 

    // =========================================================
    // 情况 A：系统初始化（处理第一帧点云）
    // =========================================================
    if (cloudKeyPoses3D->points.empty())
    {
        // 如果关键帧轨迹为空，说明这是建图的第一帧
        // 此时 transformTobeMapped 的平移部分 (x, y, z) 默认为 0
        // 旋转部分用前端传入的 IMU 初始姿态进行对齐（主要是为了对齐重力方向）
        transformTobeMapped[0] = cloudInfo.imuRollInit;
        transformTobeMapped[1] = cloudInfo.imuPitchInit;
        transformTobeMapped[2] = cloudInfo.imuYawInit;

        // 如果配置文件中设定不使用 IMU 的航向角（Yaw）进行初始化，则将其置为 0
        // 这样地图的 X 轴就会对齐机器人启动时的车头方向
        if (!useImuHeadingInitialization)
            transformTobeMapped[2] = 0;

        // 将当前初始化的 IMU 姿态保存下来，供下一帧计算增量时使用
        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); 
        return;
    }

    // =========================================================
    // 情况 B：正常情况（使用 IMU 预积分里程计提供 6D 初始猜测）
    // =========================================================
    static bool lastImuPreTransAvailable = false;
    static Eigen::Affine3f lastImuPreTransformation; // 记录上一帧时刻的预积分里程计位姿

    // cloudInfo.odomAvailable 是在前端 imageProjection 中赋值的
    // 如果为 true，说明接收到了 imuPreintegration 节点发来的高频里程计
    if (cloudInfo.odomAvailable == true)
    {
        // 取出当前帧对应的预积分里程计位姿 (transBack)
        // 注意：这个位姿是在 Odom 坐标系下的，并不是全局 Map 坐标系下的精确位姿
        Eigen::Affine3f transBack = pcl::getTransformation(cloudInfo.initialGuessX,    cloudInfo.initialGuessY,     cloudInfo.initialGuessZ, 
                                                            cloudInfo.initialGuessRoll, cloudInfo.initialGuessPitch, cloudInfo.initialGuessYaw);
        
        if (lastImuPreTransAvailable == false)
        {
            // 如果是第一次收到预积分里程计，只记录，不计算增量
            lastImuPreTransformation = transBack;
            lastImuPreTransAvailable = true;
        } else {
            // 核心数学转换：计算位姿增量 (Delta T)
            // transIncre = (上一帧预积分位姿)^-1 * (当前帧预积分位姿)
            // 这代表了imu积分预测的雷达在这两帧之间真实的物理运动（包含旋转和平移）
            Eigen::Affine3f transIncre = lastImuPreTransformation.inverse() * transBack;
            
            // 将上一帧在 Map 坐标系下的最优位姿转换为仿射矩阵
            Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
            
            // 预测当前帧在 Map 坐标系下的位姿：上一帧最优位姿 * 雷达相对运动增量
            Eigen::Affine3f transFinal = transTobe * transIncre;

            // 将计算结果逆向解析为 x, y, z, roll, pitch, yaw，存入 transformTobeMapped 中
            // 这将作为接下来 Scan-to-Map 优化的初始起点
            pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                            transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

            // 更新历史记录
            lastImuPreTransformation = transBack;
            lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); 
            return;
        }
    }

    // =========================================================
    // 情况 C：退化/降级情况（仅有 IMU 原始姿态，没有预积分平移）
    // =========================================================
    // 如果 IMU 预积分节点挂了，或者第一帧里程计还没发过来，但有 IMU 原始数据
    if (cloudInfo.imuAvailable == true)
    {
        // 提取当前时刻 IMU 的绝对姿态（平移置 0）
        Eigen::Affine3f transBack = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit);
        
        // 计算纯旋转增量：当前 IMU 姿态与上一帧 IMU 姿态的差值
        Eigen::Affine3f transIncre = lastImuTransformation.inverse() * transBack;

        // 获取上一帧的最优位姿
        Eigen::Affine3f transTobe = trans2Affine3f(transformTobeMapped);
        
        // 预测当前帧位姿：应用旋转增量。
        // 注意：由于 transIncre 没有平移部分，这意味着系统"猜测"机器人在这一帧时间内没有发生位移（速度为0的假设）
        Eigen::Affine3f transFinal = transTobe * transIncre;

        // 解算并更新 transformTobeMapped
        pcl::getTranslationAndEulerAngles(transFinal, transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5], 
                                                        transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);

        // 更新历史记录
        lastImuTransformation = pcl::getTransformation(0, 0, 0, cloudInfo.imuRollInit, cloudInfo.imuPitchInit, cloudInfo.imuYawInit); 
        return;
    }
}

4.2 提取局部地图 extractSurroundingKeyFrames

首先，根据最新帧的位姿，在历史帧位姿构造的kd-tree中进行半径搜索，找到在半径范围内的历史关键帧。再从后向前遍历历史帧位姿数组，挑出与当前帧时间间隔在10s内的历史帧。

将符合距离条件和时间条件的关键帧的点云都根据各自的位姿转换到世界坐标系，并且拼接到一起。经过体素滤波降采样后，分别将边缘点local map和面点local map分别传入两个kd-tree，方便scan2map时最近点的搜索。

4.3 当前帧下采样 downsampleCurrentScan

为了加快匹配速度，对当前帧提取出的角点和面点再进行一次体素滤波下采样。

4.4 Scan-to-Map优化 scan2MapOptimization

将当前帧的特征点匹配到局部地图上。进行迭代优化，计算出点到线（角点）、点到面（面点）的距离残差，并计算雅可比矩阵，从而解算出当前帧的精确位姿。具体过程在以前的笔记LVI-SAM/LIO-SAM/LEGO-LOAM中点到线/面残差计算及其雅可比矩阵推导中有介绍。

4.5 保存关键帧与因子图优化 saveKeyFramesAndFactor

判断当前帧是否移动了足够远的距离或角度。如果是，则将其作为新的关键帧保存。

将里程计因子（关键帧间的相对位姿）、GPS因子（如果有）、闭环因子（如果有）加入到 GTSAM 因子图中，并执行图优化。

4.6 校正历史位姿 correctPoses

历史关键帧对应的点云与位姿都各自保存在数组中。GTSAM 优化完成后，世界坐标系下的历史关键帧位姿会被修正，这里需要将修正后的位姿更新到内存中。

4.7 发布里程计 publishOdometry

发布里程计的时候会发布两个不一样的位姿。一个是包含回环/GPS修正的全局绝对位姿 /lio_sam/mapping/odometry（T(map_lidar)），它是直接使用transformTobeMapped构建的。

另一个是纯靠前端scan2map增量平滑累加出的局部位姿 /lio_sam/mapping/odometry_incremental（T(odom_lidar)），最开始的位姿起点是0，其始终靠累加"增量"得来的：

cpp 复制代码 
// incrementalOdometryAffineFront：在 updateInitialGuess() 中记录的当前帧初始猜测位姿
// incrementalOdometryAffineBack：在 transformUpdate() 中记录的经过 Scan-to-Map (LM优化) 匹配后的位姿。
Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
// increOdomAffine靠累加"增量"得来的
increOdomAffine = increOdomAffine * affineIncre;

前端因子图优化中需要以优化后的lidar位姿作为imu预积分的观测。imu因子图优化部分进行的是局部里程计的位姿预测，如果后端优化时检测到闭环，则最新位姿 transformTobeMapped 的值会发生imu预积分无法承受的巨大的、不连续的跳变 。如果直接使用最新位姿 transformTobeMapped 作为观测，则会使因子图优化时的误差严重爆炸，导致整个 IMU 预积分模块崩溃重启。

所以这里会额外发布一个**/lio_sam/mapping/odometry_incremental**，其仅仅代表了雷达前端scan2map带来的微小平滑位移（对imu预测位姿的修正 ），完美地过滤掉了 ISAM2 优化带来的回环/GPS突变。

cpp 复制代码 
void publishOdometry()
{
    // Publish odometry for ROS (global)
    nav_msgs::Odometry laserOdometryROS;
    laserOdometryROS.header.stamp = timeLaserInfoStamp;
    laserOdometryROS.header.frame_id = odometryFrame;
    laserOdometryROS.child_frame_id = "odom_mapping";
    laserOdometryROS.pose.pose.position.x = transformTobeMapped[3];
    laserOdometryROS.pose.pose.position.y = transformTobeMapped[4];
    laserOdometryROS.pose.pose.position.z = transformTobeMapped[5];
    laserOdometryROS.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]);
    pubLaserOdometryGlobal.publish(laserOdometryROS);   // T(map_lidar)
    
    // Publish TF
    static tf::TransformBroadcaster br;
    tf::Transform t_odom_to_lidar = tf::Transform(tf::createQuaternionFromRPY(transformTobeMapped[0], transformTobeMapped[1], transformTobeMapped[2]),
                                                    tf::Vector3(transformTobeMapped[3], transformTobeMapped[4], transformTobeMapped[5]));
    tf::StampedTransform trans_odom_to_lidar = tf::StampedTransform(t_odom_to_lidar, timeLaserInfoStamp, odometryFrame, "lidar_link");
    br.sendTransform(trans_odom_to_lidar);

    // Publish odometry for ROS (incremental)
    static bool lastIncreOdomPubFlag = false;
    static nav_msgs::Odometry laserOdomIncremental; // incremental odometry msg
    static Eigen::Affine3f increOdomAffine; // incremental odometry in affine
    if (lastIncreOdomPubFlag == false)
    {
        lastIncreOdomPubFlag = true;
        laserOdomIncremental = laserOdometryROS;
        increOdomAffine = trans2Affine3f(transformTobeMapped);
    } else {
        // incrementalOdometryAffineFront：在 updateInitialGuess() 中记录的当前帧初始猜测位姿
        // incrementalOdometryAffineBack：在 transformUpdate() 中记录的经过 Scan-to-Map (LM优化) 匹配后的位姿。
        // affineIncre 仅仅代表了雷达前端匹配（Scan-to-Map）带来的微小平滑位移，完美地过滤掉了 ISAM2 优化带来的回环/GPS突变！
        Eigen::Affine3f affineIncre = incrementalOdometryAffineFront.inverse() * incrementalOdometryAffineBack;
        // increOdomAffine靠累加"增量"得来的
        increOdomAffine = increOdomAffine * affineIncre;
        float x, y, z, roll, pitch, yaw;
        pcl::getTranslationAndEulerAngles (increOdomAffine, x, y, z, roll, pitch, yaw);
        if (cloudInfo.imuAvailable == true)
        {
            if (std::abs(cloudInfo.imuPitchInit) < 1.4)
            {
                double imuWeight = 0.1;
                tf::Quaternion imuQuaternion;
                tf::Quaternion transformQuaternion;
                double rollMid, pitchMid, yawMid;

                // slerp roll
                transformQuaternion.setRPY(roll, 0, 0);
                imuQuaternion.setRPY(cloudInfo.imuRollInit, 0, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                roll = rollMid;

                // slerp pitch
                transformQuaternion.setRPY(0, pitch, 0);
                imuQuaternion.setRPY(0, cloudInfo.imuPitchInit, 0);
                tf::Matrix3x3(transformQuaternion.slerp(imuQuaternion, imuWeight)).getRPY(rollMid, pitchMid, yawMid);
                pitch = pitchMid;
            }
        }
        laserOdomIncremental.header.stamp = timeLaserInfoStamp;
        laserOdomIncremental.header.frame_id = odometryFrame;
        laserOdomIncremental.child_frame_id = "odom_mapping";
        laserOdomIncremental.pose.pose.position.x = x;
        laserOdomIncremental.pose.pose.position.y = y;
        laserOdomIncremental.pose.pose.position.z = z;
        laserOdomIncremental.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
        if (isDegenerate)
            laserOdomIncremental.pose.covariance[0] = 1;
        else
            laserOdomIncremental.pose.covariance[0] = 0;
    }
    // T(odom_lidar)
    pubLaserOdometryIncremental.publish(laserOdomIncremental);
}

5.imu预积分节点 /lio_sam_imuPreintegration

LIO-SAM中进行了巧妙设计，并没有让imu预积分参与庞大的全局因子图优化，而是在一个独立的 ROS 节点（imuPreintegration.cpp）中，实例化了两个协同工作的类：IMUPreintegration 和 TransformFusion。

IMUPreintegration类中维护了一个因子图，以scan2map后lidar位姿作为观测进行imu预积分的优化。TransformFusion类以低频但高精度的最新位姿 transformTobeMapped 作为绝对基准锚点，叠加imu预积分高频预测得到的IMU 局部增量， 成功打造了一个既包含全局纠正，又具有 500Hz 实时性的完美里程计输出。

下面看 /lio_sam_imuPreintegration 节点的具体实现**：**

5.1 IMUPreintegration类

IMUPreintegration类订阅：

  1. 原始 IMU 数据 ( /imu_correct)
  2. 过滤了跳变的局部平滑增量里程计 (/lio_sam/mapping/odometry_incremental) [注意：此处绝不能用包含回环的全局里程计，否则会导致 Bias 估计爆炸]

发布：

  1. 高频的 IMU 预测局部里程计 (/odometry/imu_incremental)

IMUPreintegration类是高频局部状态估计器，融合高频 IMU 和后端的低频 Lidar 增量里程计，计算精确的速度并消除 IMU 的发散（Bias 估计）。对外输出高频且相对平滑的里程计增量，供前端去畸变和初值猜测使用。

5.1.1 类内成员

cpp 复制代码 
class IMUPreintegration : public ParamServer
{
public:

    std::mutex mtx;

    ros::Subscriber subImu;
    ros::Subscriber subOdometry;
    ros::Publisher pubImuOdometry;

    bool systemInitialized = false;

    gtsam::noiseModel::Diagonal::shared_ptr priorPoseNoise;
    gtsam::noiseModel::Diagonal::shared_ptr priorVelNoise;
    gtsam::noiseModel::Diagonal::shared_ptr priorBiasNoise;
    gtsam::noiseModel::Diagonal::shared_ptr correctionNoise;
    gtsam::noiseModel::Diagonal::shared_ptr correctionNoise2;
    gtsam::Vector noiseModelBetweenBias;


    gtsam::PreintegratedImuMeasurements *imuIntegratorOpt_;
    gtsam::PreintegratedImuMeasurements *imuIntegratorImu_;

    std::deque<sensor_msgs::Imu> imuQueOpt;     // 优化队列
    std::deque<sensor_msgs::Imu> imuQueImu;     // 预测队列

    gtsam::Pose3 prevPose_;
    gtsam::Vector3 prevVel_;
    gtsam::NavState prevState_;
    gtsam::imuBias::ConstantBias prevBias_;

    gtsam::NavState prevStateOdom;
    gtsam::imuBias::ConstantBias prevBiasOdom;

    bool doneFirstOpt = false;
    double lastImuT_imu = -1;
    double lastImuT_opt = -1;

    gtsam::ISAM2 optimizer;
    gtsam::NonlinearFactorGraph graphFactors;
    gtsam::Values graphValues;

    const double delta_t = 0;

    int key = 1;
    // T_bl: tramsform points from lidar frame to imu frame 
    // T(imu_lidar)
    gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));
    // T_lb: tramsform points from imu frame to lidar frame
    gtsam::Pose3 lidar2Imu = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(extTrans.x(), extTrans.y(), extTrans.z()));
};

5.1.2 构造函数

cpp 复制代码 
IMUPreintegration()
{
    // 订阅原始IMU数据（高频）
    subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic,                   2000, &IMUPreintegration::imuHandler,      this, ros::TransportHints().tcpNoDelay());
    // 激光雷达增量里程计（低频，来自建图模块）
    subOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry_incremental", 5,    &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

    // 经过预积分融合的高频IMU里程计
    pubImuOdometry = nh.advertise<nav_msgs::Odometry> (odomTopic+"_incremental", 2000);     // 

    // 噪声模型初始化
    boost::shared_ptr<gtsam::PreintegrationParams> p = gtsam::PreintegrationParams::MakeSharedU(imuGravity);
    // 加速度计和陀螺仪协方差（连续时间白噪声）
    p->accelerometerCovariance  = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); // acc white noise in continuous
    p->gyroscopeCovariance      = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); // gyro white noise in continuous
    // 积分误差协方差
    p->integrationCovariance    = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2); // error committed in integrating position from velocities
    gtsam::imuBias::ConstantBias prior_imu_bias((gtsam::Vector(6) << 0, 0, 0, 0, 0, 0).finished());; // assume zero initial bias

    // 因子图噪声先验
    // 先验位姿噪声、速度噪声、零偏噪声以及校准噪声
    priorPoseNoise  = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1e-2).finished()); // rad,rad,rad,m, m, m
    priorVelNoise   = gtsam::noiseModel::Isotropic::Sigma(3, 1e4); // m/s
    priorBiasNoise  = gtsam::noiseModel::Isotropic::Sigma(6, 1e-3); // 1e-2 ~ 1e-3 seems to be good
    correctionNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 0.05, 0.05, 0.05, 0.1, 0.1, 0.1).finished()); // rad,rad,rad,m, m, m
    correctionNoise2 = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1, 1, 1, 1, 1, 1).finished()); // rad,rad,rad,m, m, m
    noiseModelBetweenBias = (gtsam::Vector(6) << imuAccBiasN, imuAccBiasN, imuAccBiasN, imuGyrBiasN, imuGyrBiasN, imuGyrBiasN).finished();
    
    // 双积分器机制（核心）：初始化了两个预积分器：
    // 用于高频预测。它负责以最新优化出的状态为起点，积分最新的IMU数据，用于实时发布高频里程计（为点云去畸变提供先验）。
    imuIntegratorImu_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for IMU message thread
    // 用于图优化。它负责把两次激光雷达帧之间的IMU数据积分成一个IMU因子，加入到ISAM2图优化中。
    imuIntegratorOpt_ = new gtsam::PreintegratedImuMeasurements(p, prior_imu_bias); // setting up the IMU integration for optimization        
}

5.1.3 优化器与因子图重置

cpp 复制代码 
void resetOptimization()            // 重置ISAM2优化器和因子图。
{
    gtsam::ISAM2Params optParameters;
    optParameters.relinearizeThreshold = 0.1;
    optParameters.relinearizeSkip = 1;
    optimizer = gtsam::ISAM2(optParameters);

    gtsam::NonlinearFactorGraph newGraphFactors;
    graphFactors = newGraphFactors;

    gtsam::Values NewGraphValues;
    graphValues = NewGraphValues;
}

void resetParams()                  // 重置系统的状态标志位，让系统准备好进行重新初始化。
{
    lastImuT_imu = -1;
    doneFirstOpt = false;
    systemInitialized = false;
}

5.1.4 因子图优化修正积分值

cpp 复制代码 
// "lio_sam/mapping/odometry_incremental"
// 接收来自建图模块（mapOptimization）发来的低频但高精度的激光雷达里程计，将其作为观测值，与IMU的预积
//      分数据在GTSAM因子图中进行联合优化，计算出当前最优的位姿、速度以及IMU零偏（Bias），并用优化后的
//      结果去修正高频IMU积分器。
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    std::lock_guard<std::mutex> lock(mtx);

    double currentCorrectionTime = ROS_TIME(odomMsg);       // 获取当前激光雷达里程计的时间戳

    // make sure we have imu data to integrate
    if (imuQueOpt.empty())
        return;

    float p_x = odomMsg->pose.pose.position.x;              // 提取激光雷达里程计提供的位姿
    float p_y = odomMsg->pose.pose.position.y;
    float p_z = odomMsg->pose.pose.position.z;
    float r_x = odomMsg->pose.pose.orientation.x;
    float r_y = odomMsg->pose.pose.orientation.y;
    float r_z = odomMsg->pose.pose.orientation.z;
    float r_w = odomMsg->pose.pose.orientation.w;
    bool degenerate = (int)odomMsg->pose.covariance[0] == 1 ? true : false;     // 检查前端是否发生退化
    // T(odom_lidar)
    gtsam::Pose3 lidarPose = gtsam::Pose3(gtsam::Rot3::Quaternion(r_w, r_x, r_y, r_z), gtsam::Point3(p_x, p_y, p_z));

    // =========================================================================
    // 步骤 0: 系统初始化 (仅在系统刚启动时执行一次)
    // =========================================================================
    if (systemInitialized == false)
    {
        resetOptimization();

        while (!imuQueOpt.empty())
        {
            // 丢弃比当前雷达里程计更老的IMU数据（对齐时间戳）
            if (ROS_TIME(&imuQueOpt.front()) < currentCorrectionTime - delta_t)
            {
                lastImuT_opt = ROS_TIME(&imuQueOpt.front());
                imuQueOpt.pop_front();
            }
            else
                break;
        }

        // --- 添加先验因子 (Prior Factors) 到图优化中 ---
        
        // 1. 初始位姿：T(odom_imu) = T(odom_lidar) * T(lidar_imu)
        prevPose_ = lidarPose.compose(lidar2Imu);
        gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, priorPoseNoise);
        graphFactors.add(priorPose);
        // 2. 初始速度：假设系统静止启动，速度设为0
        prevVel_ = gtsam::Vector3(0, 0, 0);
        gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, priorVelNoise);
        graphFactors.add(priorVel);
        // 3. 初始零偏 (Bias)：假设为0
        prevBias_ = gtsam::imuBias::ConstantBias();
        gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, priorBiasNoise);
        graphFactors.add(priorBias);
        
        // 添加第0个节点到图中
        graphValues.insert(X(0), prevPose_);
        graphValues.insert(V(0), prevVel_);
        graphValues.insert(B(0), prevBias_);
        // 优化一次
        optimizer.update(graphFactors, graphValues);
        graphFactors.resize(0);
        graphValues.clear();

        // 用优化后的零偏重置两个IMU积分器
        imuIntegratorImu_->resetIntegrationAndSetBias(prevBias_);
        imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);
        
        key = 1;                            // 节点索引设为1，准备接下来的优化
        systemInitialized = true;
        return;
    }

    // =========================================================================
    // 步骤 0.5:定期重置因子图（控制计算量）
    // =========================================================================
    // 当节点数达到100（key == 100）时，利用当前计算出的边缘化协方差作为新的先验噪声，
    //      重置因子图。类似于（但不是）滑动窗口滤波中的边缘化操作。
    if (key == 100)
    {
        // 获取当前最新节点(key-1)优化后的边缘协方差，作为下一次图的新先验噪声
        gtsam::noiseModel::Gaussian::shared_ptr updatedPoseNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(X(key-1)));
        gtsam::noiseModel::Gaussian::shared_ptr updatedVelNoise  = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(V(key-1)));
        gtsam::noiseModel::Gaussian::shared_ptr updatedBiasNoise = gtsam::noiseModel::Gaussian::Covariance(optimizer.marginalCovariance(B(key-1)));
        
        // 重置整个图
        resetOptimization();

        // 用最新状态和上面提取的协方差建立新的先验因子
        // add pose
        gtsam::PriorFactor<gtsam::Pose3> priorPose(X(0), prevPose_, updatedPoseNoise);
        graphFactors.add(priorPose);
        // add velocity
        gtsam::PriorFactor<gtsam::Vector3> priorVel(V(0), prevVel_, updatedVelNoise);
        graphFactors.add(priorVel);
        // add bias
        gtsam::PriorFactor<gtsam::imuBias::ConstantBias> priorBias(B(0), prevBias_, updatedBiasNoise);
        graphFactors.add(priorBias);

        // 添加第0个节点到图中
        graphValues.insert(X(0), prevPose_);
        graphValues.insert(V(0), prevVel_);
        graphValues.insert(B(0), prevBias_);
        // optimize once
        // 当图中只有第 0 个节点和它的先验因子时，不存在任何相对约束（比如没有帧间边），此时调用 update 不会
        //      发生我们通常理解的"由于矛盾而改变位姿"的优化过程（即此时的结果 X(0) 还是等于你的输入 prevPose_）。
        // 但这一步是绝对不可省略的，它的真正作用是初始化 ISAM2 的内部数据结构:
        //      1.构建贝叶斯树的根节点（Initialization）。
        //      2.计算并确立初始协方差（Uncertainty Tracking）。
        optimizer.update(graphFactors, graphValues);
        //      3.清空临时容器准备循环。
        graphFactors.resize(0);
        graphValues.clear();

        key = 1;                // 节点索引归1
    }

    // =========================================================================
    // 步骤 1: 提取lidar位姿帧间的IMU数据进行积分，并构建因子图优化
    // =========================================================================
    while (!imuQueOpt.empty())
    {
        // pop and integrate imu data that is between two optimizations
        // 进行lidar k-1 帧到 k 帧之间的imu积分
        sensor_msgs::Imu *thisImu = &imuQueOpt.front();
        double imuTime = ROS_TIME(thisImu);
        if (imuTime < currentCorrectionTime - delta_t)
        {
            double dt = (lastImuT_opt < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_opt);
            imuIntegratorOpt_->integrateMeasurement(
                    gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                    gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
            
            lastImuT_opt = imuTime;
            imuQueOpt.pop_front();
        }
        else
            break;
    }

    // --- 将各种因子加入因子图 ---

    // A. 预积分IMU因子 (连接上一帧与当前帧的状态)
    const gtsam::PreintegratedImuMeasurements& preint_imu = dynamic_cast<const gtsam::PreintegratedImuMeasurements&>(*imuIntegratorOpt_);
    gtsam::ImuFactor imu_factor(X(key - 1), V(key - 1), X(key), V(key), B(key - 1), preint_imu);
    graphFactors.add(imu_factor);

    // B. Bias 随机游走因子 (假设两帧之间Bias的变化符合高斯分布)
    graphFactors.add(gtsam::BetweenFactor<gtsam::imuBias::ConstantBias>(B(key - 1), B(key), gtsam::imuBias::ConstantBias(),
                        gtsam::noiseModel::Diagonal::Sigmas(sqrt(imuIntegratorOpt_->deltaTij()) * noiseModelBetweenBias)));
    
    // C. 雷达里程计位姿先验因子 (作为图优化的直接观测值约束)
    gtsam::Pose3 curPose = lidarPose.compose(lidar2Imu);
    // 如果前端雷达匹配发生退化，使用更大的噪声
    gtsam::PriorFactor<gtsam::Pose3> pose_factor(X(key), curPose, degenerate ? correctionNoise2 : correctionNoise);
    graphFactors.add(pose_factor);
    
    // --- 为新节点提供初始预估值 ---
    // 利用上一状态和积分量预测当前状态，作为当前节点的初始迭代点插入图
    gtsam::NavState propState_ = imuIntegratorOpt_->predict(prevState_, prevBias_);
    graphValues.insert(X(key), propState_.pose());
    graphValues.insert(V(key), propState_.v());
    graphValues.insert(B(key), prevBias_);
    
    // 执行ISAM2优化
    optimizer.update(graphFactors, graphValues);
    optimizer.update();                             // 推荐多次update以加速收敛
    graphFactors.resize(0);
    graphValues.clear();

    // 获取优化后的最佳估计值（位姿、速度、bias）
    gtsam::Values result = optimizer.calculateEstimate();
    prevPose_  = result.at<gtsam::Pose3>(X(key));
    prevVel_   = result.at<gtsam::Vector3>(V(key));
    prevState_ = gtsam::NavState(prevPose_, prevVel_);
    prevBias_  = result.at<gtsam::imuBias::ConstantBias>(B(key));
    // 利用刚刚优化出的最优Bias，重置下一次迭代的优化用预积分器
    imuIntegratorOpt_->resetIntegrationAndSetBias(prevBias_);

    // 检测优化结果是否发散（如速度异常大或零偏异常大）
    if (failureDetection(prevVel_, prevBias_))      // 如果发散，重置系统等待重新初始化
    {
        resetParams();
        return;
    }

    // =========================================================================
    // 步骤 2: 重新传播 (Re-propagate) - 修正高频输出
    // =========================================================================
    // 由于前面的图优化耗费了一定时间，且只处理了到雷达帧时间戳为止的IMU数据
    //      此时此刻，imuQueImu队列中可能已经积累了比 currentCorrectionTime 更新的IMU数据。
    //      为了使高频发布的数据准确，必须使用最新优化出的Bias把这段时间新积累的IMU数据重新积分一遍。

    prevStateOdom = prevState_;                     // 更新最新全局状态供imuHandler使用
    prevBiasOdom  = prevBias_;                      // 更新最新优化的Bias

    // 从预测用的IMU队列中弹出旧数据（比当前雷达帧老的数据已经用不上了，刚刚优化过了）
    double lastImuQT = -1;
    while (!imuQueImu.empty() && ROS_TIME(&imuQueImu.front()) < currentCorrectionTime - delta_t)
    {
        lastImuQT = ROS_TIME(&imuQueImu.front());
        imuQueImu.pop_front();
    }
    // 只对剩下较新的IMU数据进行重传播 (Repropagation)
    if (!imuQueImu.empty())
    {
        // 关键：用最新优化得出的Bias重置高频预测预积分器
        imuIntegratorImu_->resetIntegrationAndSetBias(prevBiasOdom);
        // 重新积分这些新的IMU数据
        for (int i = 0; i < (int)imuQueImu.size(); ++i)
        {
            sensor_msgs::Imu *thisImu = &imuQueImu[i];
            double imuTime = ROS_TIME(thisImu);
            double dt = (lastImuQT < 0) ? (1.0 / 500.0) :(imuTime - lastImuQT);

            imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu->linear_acceleration.x, thisImu->linear_acceleration.y, thisImu->linear_acceleration.z),
                                                    gtsam::Vector3(thisImu->angular_velocity.x,    thisImu->angular_velocity.y,    thisImu->angular_velocity.z), dt);
            lastImuQT = imuTime;
        }
    }

    ++key;                      // 准备下一个节点的优化
    doneFirstOpt = true;        // 标志位，允许 imuHandler 开始高频发布数据
}

graphFactors 代表约束边。graphValues 代表节点。在 GTSAM 的因子图中，Values 容器专门用来存储所有的变量节点（Variable Nodes）。graphValues.insert(X(0), prevPose_) 的本质就是向因子图中放入一个编号为 0 的位姿节点，并给它赋予一个初始的猜测值（prevPose_）。GTSAM 会在这个初始猜测值附近寻找最优解。

ISAM2（Incremental Smoothing and Mapping 2）是一个增量式的贝叶斯树优化器。 传统的图优化（如 g2o 或 Ceres 的常规用法）是批处理的：每加一个新节点，就把全局的历史数据从头到尾重新算一遍，这在 SLAM 中会导致计算量爆炸。

ISAM2 的用法和优势在于： 你只需要把新产生的因子（graphFactors）和 新加入的节点初始值（graphValues）丢给它，然后调用 optimizer.update()。ISAM2 内部会自动判断这次新加入的数据影响了图中的哪些局部节点，然后只对受影响的局部节点进行重计算和更新，这使得高频的实时优化成为可能。

当图中只有第 0 个节点和它的先验因子时，不存在任何相对约束，此时调用 update 不会发生我们通常理解的的优化过程。但这一步是绝对不可省略的 ，它的真正作用是初始化 ISAM2 的内部数据结构：

  1. 构建贝叶斯树的根节点（Initialization）： ISAM2 底层维护了一棵贝叶斯树（Bayes Tree）。第一次调用 update 时，ISAM2 会把这第 0 个节点以及它的先验因子吞进去，建立这棵树的"树根"。没有这个根，后面的节点（枝叶）根本长不出来。
  2. 计算并确立初始协方差（Uncertainty Tracking）： 虽然状态量的值没变，但优化器通过这次 update，将你设定的先验噪声（priorPoseNoise 等）转化为系统内部的边缘协方差矩阵（Marginal Covariance）。这相当于告诉优化器："当前我们在起点的置信度有多高"。这个初始的不确定度会随着后续的 IMU 积分和雷达观测不断向后传递。
  3. 清空临时容器准备循环： 代码在 update 之后通常会紧接着 graphFactors.resize(0); graphValues.clear();。第一次 update 完成了 ISAM2 引擎的"点火"，将数据固化到了引擎内部，随后清空外部容器，让系统准备好干净地迎接第 1 帧、第 2 帧的增量数据。

5.1.5 失败检测

检查优化出的当前速度（> 30m/s）或Bias（> 1.0）是否异常过大。如果系统发散或受猛烈撞击，返回true触发系统重置，防止错误状态持续传递。

cpp 复制代码 
bool failureDetection(const gtsam::Vector3& velCur, const gtsam::imuBias::ConstantBias& biasCur)
{
    Eigen::Vector3f vel(velCur.x(), velCur.y(), velCur.z());
    if (vel.norm() > 30)
    {
        ROS_WARN("Large velocity, reset IMU-preintegration!");
        return true;
    }

    Eigen::Vector3f ba(biasCur.accelerometer().x(), biasCur.accelerometer().y(), biasCur.accelerometer().z());
    Eigen::Vector3f bg(biasCur.gyroscope().x(), biasCur.gyroscope().y(), biasCur.gyroscope().z());
    if (ba.norm() > 1.0 || bg.norm() > 1.0)
    {
        ROS_WARN("Large bias, reset IMU-preintegration!");
        return true;
    }

    return false;
}

5.1.6 高频积分预测

cpp 复制代码 
void imuHandler(const sensor_msgs::Imu::ConstPtr& imu_raw)
{
    std::lock_guard<std::mutex> lock(mtx);

    // 1.数据转换：将IMU数据从ROS格式转为系统使用的格式。
    sensor_msgs::Imu thisImu = imuConverter(*imu_raw);

    // 2.双队列压入：分别压入供优化用的imuQueOpt和供实时预测用的imuQueImu。
    imuQueOpt.push_back(thisImu);
    imuQueImu.push_back(thisImu);

    // 3.高频积分预测：
    // 3.1 如果系统已通过激光雷达初始化，则使用imuIntegratorImu_对当前单一IMU数据进行积分。
    if (doneFirstOpt == false)
        return;

    double imuTime = ROS_TIME(&thisImu);
    double dt = (lastImuT_imu < 0) ? (1.0 / 500.0) : (imuTime - lastImuT_imu);
    lastImuT_imu = imuTime;
    imuIntegratorImu_->integrateMeasurement(gtsam::Vector3(thisImu.linear_acceleration.x, thisImu.linear_acceleration.y, thisImu.linear_acceleration.z),
                                            gtsam::Vector3(thisImu.angular_velocity.x,    thisImu.angular_velocity.y,    thisImu.angular_velocity.z), dt);

    // 3.2 使用前一次优化得到的最新状态（prevStateOdom, prevBiasOdom）作为起点，
    //      通过predict推导出当前的高频状态（currentState）。
    gtsam::NavState currentState = imuIntegratorImu_->predict(prevStateOdom, prevBiasOdom);

    // 4.发布高频里程计：
    // 由IMU预测的机器人位姿在局部里程计参考系 (odom) 下的实时坐标
    nav_msgs::Odometry odometry;
    odometry.header.stamp = thisImu.header.stamp;
    odometry.header.frame_id = odometryFrame;       // odom
    odometry.child_frame_id = "odom_imu";

    // transform imu pose to ldiar
    // IMU预积分模块预测出的状态（currentState）是IMU硬件本体的位姿，但我们后续发布的高频里程计、前端点云去畸变以及
    //      后端的地图拼接，实际上都是以激光雷达的光心为原点进行的。因此，必须进行坐标系的转换。
    // 4.1 提取 IMU 的odom位姿 T(odom_imu)
    gtsam::Pose3 imuPose = gtsam::Pose3(currentState.quaternion(), currentState.position());
    // 4.2 通过外参计算 LiDAR 的odom位姿 T(odom_lidar) = T(odom_imu) * T(imu_lidar)
    gtsam::Pose3 lidarPose = imuPose.compose(imu2Lidar);

    odometry.pose.pose.position.x = lidarPose.translation().x();
    odometry.pose.pose.position.y = lidarPose.translation().y();
    odometry.pose.pose.position.z = lidarPose.translation().z();
    odometry.pose.pose.orientation.x = lidarPose.rotation().toQuaternion().x();
    odometry.pose.pose.orientation.y = lidarPose.rotation().toQuaternion().y();
    odometry.pose.pose.orientation.z = lidarPose.rotation().toQuaternion().z();
    odometry.pose.pose.orientation.w = lidarPose.rotation().toQuaternion().w();
    
    odometry.twist.twist.linear.x = currentState.velocity().x();
    odometry.twist.twist.linear.y = currentState.velocity().y();
    odometry.twist.twist.linear.z = currentState.velocity().z();
    odometry.twist.twist.angular.x = thisImu.angular_velocity.x + prevBiasOdom.gyroscope().x();
    odometry.twist.twist.angular.y = thisImu.angular_velocity.y + prevBiasOdom.gyroscope().y();
    odometry.twist.twist.angular.z = thisImu.angular_velocity.z + prevBiasOdom.gyroscope().z();
    pubImuOdometry.publish(odometry);       // odometry/imu_incremental
    // 4.3 发布基于imu预测的lidar位姿T(odom_lidar)，为雷达点云运动补偿（去畸变）和scan2map提供极高频率的初值
}

5.2 TransformFusion类

TransformFusion类订阅：

  1. 包含回环/GPS的低频、高精度、有延迟的全局里程计 (/lio_sam/mapping/odometry)
  2. IMUPreintegration 类发布的高频、平滑、无延迟的局部里程计 (/odometry/imu_incremental)

发布：

  1. 补偿延迟后的最终高频全局里程计 (/odometry/imu)
  2. 向整个 ROS 系统广播 TF 坐标树 (odom -> base_link)
  3. 高精度的全局预测位姿轨迹 (/lio_sam/imu/path)，主要用于 Rviz 可视化。

TransformFusion类解决建图模块（mapOptimization）耗时导致的状态延迟问题。将高频的IMU里程计"接"在低频但准确的Lidar里程计末端，推测出现时刻的绝对位姿。

TransformFusion类记录下激光里程计对应的历史时间点，并从该时间点开始，算出IMU在这段"延迟时间"内产生的位姿增量。将这个增量叠加到那个极其精确的历史激光位姿上，就能得到当前时刻高精度且高频（500Hz）的绝对位姿，并向整个ROS系统广播 TF 坐标树。

5.2.1 构造函数

cpp 复制代码 
TransformFusion()
{
    if(lidarFrame != baselinkFrame)     // lidarFrame == baselinkFrame
    {
        try
        {
            tfListener.waitForTransform(lidarFrame, baselinkFrame, ros::Time(0), ros::Duration(3.0));
            tfListener.lookupTransform(lidarFrame, baselinkFrame, ros::Time(0), lidar2Baselink);
        }
        catch (tf::TransformException ex)
        {
            ROS_ERROR("%s",ex.what());
        }
    }

    // mapOptmization节点发布的最新优化后的全局位姿 T(map_lidar)
    subLaserOdometry = nh.subscribe<nav_msgs::Odometry>("lio_sam/mapping/odometry", 5, &TransformFusion::lidarOdometryHandler, this, ros::TransportHints().tcpNoDelay());
    // lidar位姿的预测值 T(odom_lidar)
    subImuOdometry   = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental",   2000, &TransformFusion::imuOdometryHandler,   this, ros::TransportHints().tcpNoDelay());

    // 发布最终融合后的高频全局里程计
    pubImuOdometry   = nh.advertise<nav_msgs::Odometry>(odomTopic, 2000);           // odometry/imu
    // 发布用于可视化的 IMU 轨迹 Path
    pubImuPath       = nh.advertise<nav_msgs::Path>    ("lio_sam/imu/path", 1);
}

5.2.2 工具函数

将 ROS 的 nav_msgs::Odometry 消息格式转换为 Eigen 库的三维仿射变换矩阵。

cpp 复制代码 
Eigen::Affine3f odom2affine(nav_msgs::Odometry odom)
{
    double x, y, z, roll, pitch, yaw;
    x = odom.pose.pose.position.x;
    y = odom.pose.pose.position.y;
    z = odom.pose.pose.position.z;
    tf::Quaternion orientation;
    tf::quaternionMsgToTF(odom.pose.pose.orientation, orientation);
    tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
    // 返回对应的 4x4 变换矩阵
    return pcl::getTransformation(x, y, z, roll, pitch, yaw);
}

5.2.3 lidar全局位姿接收回调函数

cpp 复制代码 
// 接收最新的后端优化位姿，并将其保存为基准。
void lidarOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    std::lock_guard<std::mutex> lock(mtx);

    lidarOdomAffine = odom2affine(*odomMsg);

    lidarOdomTime = odomMsg->header.stamp.toSec();
}

5.2.4 imu局部里程计回调函数

每次收到高频 IMU 里程计，立刻计算出补偿延迟后的当前真实位姿。

cpp 复制代码 
void imuOdometryHandler(const nav_msgs::Odometry::ConstPtr& odomMsg)
{
    // 1.LIO-SAM的简化处理,强制把 map 坐标系和 odom 坐标系对齐
    // T(map_odom) = 0
    static tf::TransformBroadcaster tfMap2Odom;
    static tf::Transform map_to_odom = tf::Transform(tf::createQuaternionFromRPY(0, 0, 0), tf::Vector3(0, 0, 0));
    tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, odomMsg->header.stamp, mapFrame, odometryFrame));

    std::lock_guard<std::mutex> lock(mtx);

    // 2. 将当前收到的高频 IMU 位姿推入队列
    imuOdomQueue.push_back(*odomMsg);

    // 3.对齐时间戳：剔除队列中比"最新激光雷达时间"还要老的数据
    if (lidarOdomTime == -1)
        return;
    while (!imuOdomQueue.empty())
    {
        if (imuOdomQueue.front().header.stamp.toSec() <= lidarOdomTime)
            imuOdomQueue.pop_front();
        else
            break;
    }
    // 【核心数学逻辑：位姿补偿】
    Eigen::Affine3f imuOdomAffineFront = odom2affine(imuOdomQueue.front());
    Eigen::Affine3f imuOdomAffineBack = odom2affine(imuOdomQueue.back());
    // delta_T = T(last_current) = T(last_odom) * T(odom_current)
    Eigen::Affine3f imuOdomAffineIncre = imuOdomAffineFront.inverse() * imuOdomAffineBack;
    // T(map_lidar) = T(map_lidar) * delta_T
    Eigen::Affine3f imuOdomAffineLast = lidarOdomAffine * imuOdomAffineIncre;
    
    // 将计算出的当前最新矩阵转换回 x, y, z, r, p, y
    float x, y, z, roll, pitch, yaw;
    pcl::getTranslationAndEulerAngles(imuOdomAffineLast, x, y, z, roll, pitch, yaw);
    
    // 4. 发布最新的融合高频里程计
    nav_msgs::Odometry laserOdometry = imuOdomQueue.back();     // 借用一下消息头
    laserOdometry.pose.pose.position.x = x;
    laserOdometry.pose.pose.position.y = y;
    laserOdometry.pose.pose.position.z = z;
    laserOdometry.pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
    pubImuOdometry.publish(laserOdometry);

    // 5. 发布 TF 树： odom -> base_link
    static tf::TransformBroadcaster tfOdom2BaseLink;
    tf::Transform tCur;
    tf::poseMsgToTF(laserOdometry.pose.pose, tCur);
    if(lidarFrame != baselinkFrame)
        tCur = tCur * lidar2Baselink;
    tf::StampedTransform odom_2_baselink = tf::StampedTransform(tCur, odomMsg->header.stamp, odometryFrame, baselinkFrame);
    tfOdom2BaseLink.sendTransform(odom_2_baselink);

    // 6. 发布可视化的 IMU 轨迹 (Path)
    static nav_msgs::Path imuPath;
    static double last_path_time = -1;
    double imuTime = imuOdomQueue.back().header.stamp.toSec();
    // 降采样：每隔 0.1 秒向 Path 里面压入一个节点，防止Rviz渲染卡顿
    if (imuTime - last_path_time > 0.1)
    {
        last_path_time = imuTime;
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.stamp = imuOdomQueue.back().header.stamp;
        pose_stamped.header.frame_id = odometryFrame;
        pose_stamped.pose = laserOdometry.pose.pose;
        imuPath.poses.push_back(pose_stamped);
        while(!imuPath.poses.empty() && imuPath.poses.front().header.stamp.toSec() < lidarOdomTime - 1.0)
            imuPath.poses.erase(imuPath.poses.begin());
        if (pubImuPath.getNumSubscribers() != 0)
        {
            imuPath.header.stamp = imuOdomQueue.back().header.stamp;
            imuPath.header.frame_id = odometryFrame;
            pubImuPath.publish(imuPath);
        }
    }
}
相关推荐
加油JIAX2 天前
滑动窗口模型与边缘化
slam·边缘化·滑动窗口优化
Evand J2 天前
【MATLAB例程】基于低精度IMU、GNSS的UAV初始航向（三维角度）校准的仿真，包含卡尔曼滤波、惯导解算与校正
开发语言·matlab·gnss·imu·卡尔曼滤波
加油JIAX2 天前
IMU预积分-旋转残差求雅可比
slam·imu·预积分
CS_Zero3 天前
Faster-LIO论文与代码笔记（1）
笔记·slam·lio
加油JIAX5 天前
IMU预积分（VINS）
slam·imu·预积分·vins
加油JIAX5 天前
贝叶斯滤波与卡尔曼滤波
概率论·slam·kf
kobesdu6 天前
FAST-LIO2 + 蓝海M300激光雷达：从建图到实时栅格图的完整流程
算法·机器人·ros·slam·fast lio
bryant_meng7 天前
【SLAM】Map Folding
计算机视觉·map·slam·激光雷达·回环检测
kobesdu8 天前
综合强度信息的激光雷达去拖尾算法解析和源码实现
算法·机器人·ros·slam·激光雷达
热门推荐
012026年4月技术前沿：AI大模型爆发、智能体革命与量子安全新纪元02一周AI热点速览（2026.03.31-04.06）：GPT-6曝光、谷歌开源Gemma 4、资本狂飙与模型军备竞赛03GitHub 镜像站点04基于 Docker 部署 Hermes Agent 并接入飞书机器人的完整指南05GPT-6核心能力解析及与现有主流大模型对比06免费！不限量！用opencode接入英伟达（NVIDIA）大模型，轻松打造你的 AI 编程助手07LLM Wiki：让大模型替你打理知识库的完整指南08从限购到畅通：GLM-5.1 Coding Plan接入攻略09AI Weekly | 2026年4月第二周 · GitHub热门项目与AI发展趋势深度解析10零成本！Ollama本地部署国产大模型全指南（支持Kimi-K2.5/GLM-5/Qwen，新手秒上手）