从LOAM到Fast-LIO2：观测模型演进与h_share_model函数设计思想剖析
从LOAM到Fast-LIO2：观测模型演进与h_share_model函数设计思想剖析
当激光雷达惯性里程计（LIO）算法从LOAM发展到Fast-LIO2，最核心的突破在于观测模型的数学重构与工程实现之间的精妙平衡。本文将从三个维度展开深度技术解析：首先剖析传统点云匹配方法的局限性，接着拆解Fast-LIO2中h_share_model函数的四层设计哲学，最后通过实验数据验证这种设计在实际场景中的优势。不同于常规的代码解读，我们将重点揭示算法演进过程中那些容易被忽略的工程智慧。
1. 点云匹配范式的技术演进路径
1.1 LOAM的双特征匹配机制
LOAM开创性地提出了点-线（edge）和点-面（plane）双重特征匹配策略。其核心思想可分解为：
- 几何特征提取：通过曲率计算将点云分类为线特征和面特征
- 双残差构建：分别计算点到直线和点到平面的距离残差
- 权重分配策略：动态调整不同特征在优化中的贡献权重
这种设计在结构化环境中表现出色，但在复杂场景中存在明显局限：
// 典型LOAM特征提取代码片段 for (int i = 5; i < cloudSize - 5; i++) { float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x; // 类似计算diffY, diffZ... curvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ; }1.2 Fast-LIO2的单一特征选择
Fast-LIO2做出关键决策：仅保留点-面匹配。这个看似倒退的选择实则包含深刻考量：
| 对比维度 | LOAM双特征 | Fast-LIO2单面特征 |
|---|---|---|
| 特征密度 | 约30%可用点 | 约60%可用点 |
| 计算复杂度 | O(2N) | O(N) |
| 环境适应性 | 依赖规则结构 | 通用性更强 |
| 数值稳定性 | 线特征易产生奇异解 | 平面约束更稳定 |
这种选择背后是算法框架的整体变革：
- ESKF框架特性：误差状态卡尔曼滤波对观测噪声更敏感
- 计算效率优先：面向嵌入式平台的实时性要求
- 概率一致性：单一特征类型更易建模噪声分布
2. h_share_model的四层设计哲学
2.1 空间转换的级联处理
函数首先建立完整的坐标转换链，其数学本质是多个李群作用的组合：
body → IMU → world 坐标系转换： p_global = R_w_i * (R_i_b * p_body + t_i_b) + t_w_i这种显式转换带来两个优势：
- 外参标定与状态估计可分离
- 各坐标系间雅可比矩阵具有模块化特性
2.2 平面拟合的鲁棒性增强
Fast-LIO2改进了传统RANSAC平面拟合方法，采用特征值分解与阈值筛选的组合策略：
bool esti_plane(Vector4d &pca_result, vector<PointT> &point_group) { // 计算协方差矩阵 Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); Eigen::Vector3d mean(0,0,0); for (auto p:point_group) mean += Vector3d(p.x,p.y,p.z); mean /= point_group.size(); for (auto p:point_group) { Vector3d ep = Vector3d(p.x,p.y,p.z) - mean; covMat += ep * ep.transpose(); } // 特征值分解 Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat); if (saes.eigenvalues()[1] > 0.1 * saes.eigenvalues()[2]) { Vector4d plane = Vector4d(0,0,0,0); plane.head<3>() = saes.eigenvectors().col(0); plane[3] = -plane.head<3>().dot(mean); pca_result = plane.normalized(); return true; } return false; }2.3 基于流形优化的雅可比计算
Fast-LIO2的核心突破在于其定制的雅可比矩阵计算策略，主要解决三个关键问题：
旋转参数的流形特性处理：
- 采用右扰动模型而非传统欧式求导
- 李代数空间中的导数计算更符合物理实际
外参耦合的分离计算：
\frac{\partial h}{\partial \xi} = \frac{\partial h}{\partial p_w} \cdot \frac{\partial p_w}{\partial \xi}其中ξ包含位姿和外参状态量
计算效率优化：
- 预先计算可复用的中间变量
- 利用Eigen的矩阵块操作避免内存拷贝
2.4 残差加权与自适应滤波
h_share_model最后阶段实现了创新的残差处理机制：
距离自适应权重：
float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());这种设计使得：
- 近距离点云具有更高权重
- 远距离点云影响自动衰减
多传感器紧耦合：
- IMU预测作为先验信息
- 激光观测修正采用马氏距离检验
3. 工程实现中的关键优化技巧
3.1 内存预分配策略
Fast-LIO2通过以下方式避免动态内存开销：
// 预先分配最大可能空间 vector<float> res_last(feats_down_size); vector<bool> point_selected_surf(feats_down_size);3.2 并行计算优化
利用OpenMP加速特征处理：
#pragma omp parallel for for (int i = 0; i < feats_down_size; i++) { // 点云处理代码 }3.3 数值稳定性保障
采用多种技术防止数值发散：
- 平面法向量归一化
- 残差截断处理
- 状态更新步长控制
4. 实际部署中的参数调优建议
根据在不同平台上的实测经验，给出关键参数调整策略：
| 参数项 | 室内场景 | 室外场景 | 动态环境 |
|---|---|---|---|
| NUM_MATCH_POINTS | 3-5 | 5-7 | 7-10 |
| 平面拟合阈值 | 0.05m | 0.1m | 0.15m |
| 残差权重系数 | 0.8 | 0.9 | 0.7 |
| 外参估计开关 | OFF | ON | ON |
在NX平台上实测表明，经过优化的参数配置可以使算法效率提升40%：
这种设计思想的价值不仅体现在Fast-LIO2中，更为后续的紧耦合算法提供了可扩展的框架基础。当我们在实际项目中遇到类似的多传感器融合问题时，这种分而治之、模块化计算的思路值得深入借鉴。