Fast-LIO2代码实战深入解析h_share_model函数与观测雅可比矩阵在激光雷达惯性里程计LIO系统中Fast-LIO2以其高效的滤波框架和稳定的性能表现脱颖而出。作为算法核心之一的h_share_model函数承担着观测模型构建与雅可比矩阵计算的关键任务。本文将带您深入代码实现细节从数学原理到工程实践全面解析这一关键模块。1. Fast-LIO2框架概述与坐标系关系Fast-LIO2采用迭代卡尔曼滤波IKFOM框架通过IMU数据实现状态预测利用激光雷达点云进行观测校正。系统涉及三个主要坐标系Body坐标系激光雷达传感器固定坐标系IMU坐标系惯性测量单元坐标系World坐标系全局固定坐标系状态变量定义在use-ikfom.hpp中使用MTK库构建流形空间MTK_BUILD_MANIFOLD(state_ikfom, ((vect3, pos)) // 位移 ((SO3, rot)) // 旋转矩阵李群表示 ((SO3, offset_R_L_I)) // Body到IMU的旋转变换 ((vect3, offset_T_L_I)) // Body到IMU的平移变换 ((vect3, vel)) // 速度 ((vect3, bg)) // 陀螺仪偏置 ((vect3, ba)) // 加速度计偏置 ((S2, grav)) // 重力向量S2流形表示 );2. 点云匹配与平面特征提取h_share_model函数首先完成点云到地图的匹配过程这是构建观测模型的基础for (int i 0; i feats_down_size; i) { PointType point_body feats_down_body-points[i]; // Body系下点坐标 PointType point_world feats_down_world-points[i]; // World系下点坐标 // 坐标变换到World系 V3D p_body(point_body.x, point_body.y, point_body.z); V3D p_global(s.rot * (s.offset_R_L_I * p_body s.offset_T_L_I) s.pos); point_world.x p_global(0); point_world.y p_global(1); point_world.z p_global(2); // 使用ikd-tree查找最近邻点 vectorfloat pointSearchSqDis(NUM_MATCH_POINTS); auto points_near Nearest_Points[i]; ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis); // 平面特征判断条件 point_selected_surf[i] points_near.size() NUM_MATCH_POINTS ? false : pointSearchSqDis[NUM_MATCH_POINTS - 1] 5 ? false : true; if (!point_selected_surf[i]) continue; VF(4) pabcd; // 平面参数axbyczd0 if (esti_plane(pabcd, points_near, 0.1f)) { float pd2 pabcd(0)*point_world.x pabcd(1)*point_world.y pabcd(2)*point_world.z pabcd(3); float s 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm()); if (s 0.1) { point_selected_surf[i] true; normvec-points[i].x s*pabcd(0); normvec-points[i].y s*pabcd(1); normvec-points[i].z s*pabcd(2); normvec-points[i].intensity s*pd2; // 存储加权残差 res_last[i] abs(pd2); // 点到平面距离 } } }关键点说明esti_plane函数通过RANSAC拟合局部平面返回归一化的平面参数鲁棒系数s综合考虑了点云距离和平面拟合质量最终保留的点需满足平面拟合内点比例和距离阈值条件3. 观测雅可比矩阵的数学推导观测雅可比矩阵的推导是理解h_share_model函数的核心。我们首先定义观测方程$$ h(X) \mathbf{n}^T(\mathbf{R}(\mathbf{R}_{LI}\mathbf{p}b \mathbf{t}{LI}) \mathbf{t}) d $$其中$\mathbf{n}$为平面法向量$\mathbf{R}, \mathbf{t}$为IMU到World的旋转和平移$\mathbf{R}{LI}, \mathbf{t}{LI}$为Body到IMU的外参$\mathbf{p}_b$为Body系下的点坐标$d$为平面方程常数项3.1 对位移的偏导数位移$\mathbf{t}$的偏导数最为直接$$ \frac{\partial h}{\partial \mathbf{t}} \mathbf{n}^T $$对应代码中的ekfom_data.h_x.block1, 12(i, 0) norm_p.x, norm_p.y, norm_p.z, ...;3.2 对旋转的偏导数右扰动模型采用右扰动模型旋转$\mathbf{R}$的偏导数为$$ \frac{\partial h}{\partial \phi} -\mathbf{n}^T\mathbf{R}(\mathbf{R}_{LI}\mathbf{p}b \mathbf{t}{LI})^\wedge $$代码实现V3D C(s.rot.conjugate() * norm_vec); // R^T * n V3D A(point_crossmat * C); // [p]_\times * R^T * n其中point_crossmat是IMU系下点的反对称矩阵。3.3 对外参平移的偏导数外参平移$\mathbf{t}_{LI}$的偏导数为$$ \frac{\partial h}{\partial \mathbf{t}_{LI}} \mathbf{n}^T\mathbf{R} $$代码对应V3D C(s.rot.conjugate() * norm_vec); // R^T * n3.4 对外参旋转的偏导数右扰动模型外参旋转$\mathbf{R}_{LI}$的偏导数为$$ \frac{\partial h}{\partial \theta} -\mathbf{n}^T\mathbf{R}\mathbf{R}_{LI}\mathbf{p}_b^\wedge $$代码实现V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C);其中point_be_crossmat是Body系下点的反对称矩阵。4. 雅可比矩阵的代码实现综合上述推导h_share_model中雅可比矩阵的构建代码如下for (int i 0; i effct_feat_num; i) { const PointType laser_p laserCloudOri-points[i]; V3D point_this_be(laser_p.x, laser_p.y, laser_p.z); M3D point_be_crossmat; point_be_crossmat SKEW_SYM_MATRX(point_this_be); V3D point_this s.offset_R_L_I * point_this_be s.offset_T_L_I; M3D point_crossmat; point_crossmat SKEW_SYM_MATRX(point_this); const PointType norm_p corr_normvect-points[i]; V3D norm_vec(norm_p.x, norm_p.y, norm_p.z); V3D C(s.rot.conjugate() * norm_vec); V3D A(point_crossmat * C); if (extrinsic_est_en) { V3D B(point_be_crossmat * s.offset_R_L_I.conjugate() * C); ekfom_data.h_x.block1, 12(i, 0) norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), VEC_FROM_ARRAY(B), VEC_FROM_ARRAY(C); } else { ekfom_data.h_x.block1, 12(i, 0) norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(A), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; } ekfom_data.h(i) -norm_p.intensity; // 观测残差 }关键参数说明代码变量数学含义维度norm_p.x, norm_p.y, norm_p.z$\partial h/\partial \mathbf{t}$3VEC_FROM_ARRAY(A)$\partial h/\partial \phi$3VEC_FROM_ARRAY(B)$\partial h/\partial \theta$ (外参旋转)3VEC_FROM_ARRAY(C)$\partial h/\partial \mathbf{t}_{LI}$ (外参平移)35. 工程实践中的关键细节在实际实现和调试过程中有几个关键点需要特别注意反对称矩阵的计算#define SKEW_SYM_MATRX(v) 0.0, -v[2], v[1], v[2], 0.0, -v[0], -v[1], v[0], 0.0这个宏定义实现了向量到反对称矩阵的转换是李代数运算的基础。外参估计的开关控制if (extrinsic_est_en) { // 包含外参雅可比计算 } else { // 外参部分置零 }根据配置决定是否估计外参影响雅可比矩阵的维度。残差计算ekfom_data.h(i) -norm_p.intensity;这里使用负的加权距离作为残差与平面法向量方向一致。鲁棒核函数应用float s 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());通过距离加权降低异常点的影响提高系统鲁棒性。6. 性能优化与调试技巧在实现和优化h_share_model函数时可以采用以下方法并行化处理#pragma omp parallel for for (int i 0; i feats_down_size; i) { // 点云处理代码 }点云处理过程可以并行化加速特别是对于大规模点云。数值稳定性检查if (!point_crossmat.allFinite()) { continue; // 跳过无效点 }添加数值检查避免NaN或Inf导致滤波器发散。雅可比矩阵验证# Python数值梯度验证示例 def check_jacobian(): eps 1e-6 # 实现数值梯度计算 # 与解析解对比可通过数值梯度验证雅可比矩阵的正确性。内存预分配ekfom_data.h_x.resize(effct_feat_num, 12); ekfom_data.h.resize(effct_feat_num);提前分配内存避免运行时动态分配带来的性能波动。7. 不同场景下的参数调整根据应用场景的不同可能需要调整以下参数参数典型值调整建议NUM_MATCH_POINTS5点云稠密时可增大平面拟合阈值0.1m根据点云噪声调整鲁棒系数权重0.9噪声大时减小外参估计开关false标定不准确时开启实际调试中建议通过录制数据集进行离线测试逐步调整参数至最优。