拆解LIO-SAM的因子图：从IMU预积分到Scan-to-Map优化的数据流与状态估计

LIO-SAM因子图架构深度解析：从IMU预积分到激光里程计优化的全链路剖析

1. 多传感器融合的SLAM架构设计

LIO-SAM作为紧耦合激光-惯性里程计系统的典型代表，其核心创新在于将IMU预积分、激光里程计、GPS等多源观测统一建模为因子图优化问题。不同于传统松耦合架构中各传感器独立解算再融合的方式，LIO-SAM通过GTSAM框架构建了包含四种关键因子的概率图模型：

• IMU预积分因子：连接连续关键帧间的运动约束
• 激光里程计因子：提供scan-to-map匹配的位姿观测
• GPS因子：引入绝对位置约束抑制漂移
• 闭环因子：通过场景识别修正累积误差

这种因子图架构的优势在于：

1. 前端轻量化：IMU提供高频状态预测，激光匹配仅在关键帧执行
2. 后端全局优化：ISAM2增量平滑算法实现高效优化
3. 多源数据统一处理：不同传感器以因子形式自然融合

// 典型因子图构建示例（GTSAM语法） NonlinearFactorGraph graph; graph.add(PriorFactor<Pose3>(0, initialPose, priorNoise)); graph.add(BetweenFactor<Pose3>(0, 1, odomMeasurement, odomNoise)); graph.add(ImuFactor(0, 1, preintegratedMeasurements));

2. IMU预积分机制详解

2.1 预积分理论推导

IMU预积分的核心思想是将两关键帧之间的IMU观测积分转换为相对运动约束，避免因优化导致的重复积分。对于连续时间IMU模型：

$$ \begin{aligned} \mathbf{R}_{t+\Delta t} &= \mathbf{R}_t \cdot \text{Exp}((\boldsymbol{\omega}t - \mathbf{b}^g_t)\Delta t) \ \mathbf{v}{t+\Delta t} &= \mathbf{v}_t + \mathbf{R}_t(\mathbf{a}t - \mathbf{b}^a_t)\Delta t + \mathbf{g}\Delta t \ \mathbf{p}{t+\Delta t} &= \mathbf{p}_t + \mathbf{v}_t\Delta t + \frac{1}{2}\mathbf{R}_t(\mathbf{a}_t - \mathbf{b}^a_t)\Delta t^2 + \frac{1}{2}\mathbf{g}\Delta t^2 \end{aligned} $$

预积分量计算的关键步骤：

1. bias校正：补偿加速度计和陀螺仪零偏
2. 噪声传播：考虑测量噪声和随机游走噪声
3. 雅可比矩阵：提供优化时所需的导数信息

2.2 LIO-SAM实现剖析

代码中通过PreintegratedImuMeasurements类实现预积分：

// IMU数据预处理（坐标系转换） sensor_msgs::Imu thisImu = imuConverter(*imuMsg); // 预积分器更新 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);

关键参数配置：

p->accelerometerCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuAccNoise, 2); p->gyroscopeCovariance = gtsam::Matrix33::Identity(3,3) * pow(imuGyrNoise, 2); p->integrationCovariance = gtsam::Matrix33::Identity(3,3) * pow(1e-4, 2);

3. 激光里程计因子构建

3.1 Scan-to-Map匹配优化

LIO-SAM采用特征点匹配方式构建激光因子，其核心流程：

1. 特征提取：基于曲率提取边缘点和平面点
2. 局部地图构建：融合历史关键帧特征点
3. 非线性优化：最小化点到几何特征距离

边缘点残差计算： 对于找到的5个最近邻点，通过PCA分析确认线特征有效性后，构建点到线的距离残差：

$$ d_{\mathcal{E}} = \frac{|(\tilde{\mathbf{X}}{(k,j)} - \mathbf{X}{(k,i)}) \times (\tilde{\mathbf{X}}{(k,j)} - \mathbf{X}{(k,l)})|}{|\mathbf{X}{(k,j)} - \mathbf{X}{(k,l)}|} $$

平面点残差计算： 对于平面点，利用最小二乘拟合平面后计算点到平面距离：

$$ d_{\mathcal{H}} = \frac{|\mathbf{n}^T \cdot (\tilde{\mathbf{X}}{(k,j)} - \mathbf{X}{(k,m)})|}{|\mathbf{n}|} $$

3.2 代码实现关键

优化问题构建：

cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0)); cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0)); cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0)); cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));

退化处理机制：

if (matD1.at<float>(0, 0) < eignThre[i]) { for (int j = 0; j < 6; j++) { matV2.at<float>(i, j) = 0; } isDegenerate = true; }

4. 因子图优化与系统集成

4.1 优化框架设计

LIO-SAM采用ISAM2增量平滑算法进行高效优化，其核心优势在于：

• 增量更新：仅对受新测量影响的变量重新线性化
• 部分重新排序：动态调整变量消除顺序
• Clique树缓存：保留先前计算结果

关键配置参数：

ISAM2Params parameters; parameters.relinearizeThreshold = 0.1; parameters.relinearizeSkip = 1; isam = new ISAM2(parameters);

4.2 数据流管理

系统通过多个队列实现异步数据处理：

5. 关键实现技巧与工程优化

1. 坐标系对齐策略：

// IMU到LiDAR的变换补偿（仅平移） gtsam::Pose3 imu2Lidar = gtsam::Pose3(gtsam::Rot3(1, 0, 0, 0), gtsam::Point3(-extTrans.x(), -extTrans.y(), -extTrans.z()));

2. 运动补偿优化：

// 点云去畸变（线性插值） Eigen::Affine3f transBt = transStartInverse * transFinal; newPoint.x = transBt(0,0)*point->x + transBt(0,1)*point->y + transBt(0,2)*point->z + transBt(0,3);

3. 内存管理机制：

// 局部地图缓存清理 if (laserCloudMapContainer.size() > 1000) laserCloudMapContainer.clear();

4. 实时性保障：

// 多线程处理 ros::MultiThreadedSpinner spinner(4); spinner.spin();

6. 性能评估与参数调优

关键参数对系统性能的影响：

实际部署中的调试建议：

1. IMU噪声参数：通过艾伦方差标定获得真实传感器参数
2. 体素滤波尺寸：室外场景建议增大降采样粒度
3. 关键帧策略：根据运动速度动态调整添加阈值

7. 前沿改进方向

1. 语义辅助因子：引入深度学习提取的语义信息增强特征匹配
2. 多机协同建图：扩展因子图支持分布式优化
3. 动态物体处理：结合运动检测实现鲁棒估计
4. 新型传感器集成：如事件相机、毫米波雷达等

注：本文涉及的坐标系变换和因子图优化理论需要结合具体实践逐步深入理解。建议通过官方数据集进行逐模块调试，重点关注IMU预积分与激光匹配的耦合关系。