下面讲第 7 步回到laserMapping.cpp精读h_share_model()。这一节是 FAST-LIO LiDAR 前端观测模型的核心。你可以把它理解成esekfom.hpp 负责 iEKF 迭代更新 x 和 P。 h_share_model() 负责给 iEKF 提供 LiDAR 残差 h 和雅可比 H。也就是说esekfom.hpp不知道 LiDAR 点怎么和地图匹配它只会问外部测量函数“你给我当前状态下的残差和雅可比。”而h_share_model()就是回答这个问题的函数。FAST-LIO 在初始化滤波器时把h_share_model注册进kf.init_dyn_share(...)后面每次kf.update_iterated_dyn_share_modified(...)执行迭代更新时都会反复调用这个测量模型重新计算当前状态下的h和H。源码里可以看到kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi)以及主循环中调用kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time)。1.h_share_model()在整条链路里的位置FAST-LIO 一帧 LiDAR 的主链路可以这样看IMU 预测完成 ↓ 当前状态 x⁻ ↓ 当前帧点云已经去畸变 feats_undistort ↓ 点云降采样 feats_down_body ↓ 进入 iEKF 更新 ↓ iEKF 调用 h_share_model(s, ekfom_data) ↓ h_share_model 根据当前状态 s 构造 LiDAR 残差 h 和雅可比 H ↓ iEKF 根据 h、H、P⁻、R 求修正量 δx ↓ 状态 x 被更新 ↓ 下一轮迭代再次调用 h_share_model()这里的重点是h_share_model()不是只调用一次。在 IESKF 迭代中状态x每轮都会变所以点云投到世界系的位置也会变最近邻地图点、拟合平面、残差、雅可比都可能变。因此 iEKF 每轮都会带着当前状态s重新调用h_share_model()。h_share_model()的入口大概是void h_share_model( state_ikfom s, esekfom::dyn_share_datastructdouble ekfom_data )这里s就是当前迭代状态也就是当前 iEKF 正在尝试的状态ekfom_data是测量共享数据结构用来把结果返回给滤波器。最后它会往里面填ekfom_data.h_x // LiDAR 残差对状态的雅可比 H ekfom_data.h // LiDAR 残差向量 h ekfom_data.valid // 当前观测是否有效2. 第一步清空上一轮结果函数开头会清空一些容器laserCloudOri-clear(); corr_normvect-clear(); total_residual 0.0;含义是laserCloudOri 保存本轮有效 LiDAR 点坐标仍在 body/IMU 或 LiDAR-body 相关坐标系下。 corr_normvect 保存每个有效点对应的地图平面法向量以及点到平面的残差。 total_residual 统计所有有效点残差用于计算平均残差。也就是说每次进入h_share_model()都要重新开始构造当前状态下的观测。因为状态s可能已经被上一轮迭代更新过了之前的残差和雅可比不能直接复用。3. 第二步把当前 LiDAR 点变到世界坐标系对每个降采样后的点源码会取出PointType point_body feats_down_body-points[i]; PointType point_world feats_down_world-points[i];其中feats_down_body是当前帧去畸变、降采样后的点云点坐标在 body/LiDAR-body 相关坐标系中feats_down_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 );源码确实是在h_share_model()中用s.rot * (s.offset_R_L_I*p_body s.offset_T_L_I) s.pos把点转到世界系然后写入point_world。对应公式就是p_W R_WI · ( R_IL · p_L t_IL ) t_WI这里变量含义是p_L 当前点在 LiDAR/body 坐标系下的位置。 R_IL LiDAR 到 IMU 的旋转外参对应 s.offset_R_L_I。 t_IL LiDAR 到 IMU 的平移外参对应 s.offset_T_L_I。 R_WI IMU 到世界系的旋转对应 s.rot。 t_WI IMU 在世界系中的位置对应 s.pos。 p_W 当前点投影到世界/地图坐标系后的位置。从右往左理解第一步 R_IL · p_L t_IL 把 LiDAR 点变到 IMU 坐标系。 第二步 R_WI · (...) t_WI 把 IMU 坐标系下的点变到世界/地图坐标系。这一步很关键。因为后面最近邻搜索是在地图里做的地图点在世界坐标系下所以当前帧点也必须先投到世界坐标系。4. 第三步ikd-Tree 最近邻搜索点变到世界系后h_share_model()会拿这个point_world去局部地图的 ikd-Tree 里找最近邻ikdtree.Nearest_Search( point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis );源码里可以看到当ekfom_data.converge为真时函数会执行ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis)然后根据邻居数量和最远邻居距离判断该点是否可用。这里的逻辑是当前点 p_W ↓ 在局部地图 ikd-Tree 中搜索最近的 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;含义是如果邻近点数量不够 不能拟合可靠平面丢掉。 如果第 NUM_MATCH_POINTS 个邻居太远 说明附近地图点不够近匹配不可靠丢掉。 否则 这个点暂时认为可以尝试拟合平面。这一步的本质是不是每个当前帧点都参与更新只有能在地图中找到可靠邻域的点才有资格参与 LiDAR 残差构建。5. 第四步用邻近地图点拟合局部平面如果当前点通过了最近邻筛选接下来会拟合平面VF(4) pabcd; if (esti_plane(pabcd, points_near, 0.1f)) { ... }pabcd里面保存的是平面参数pabcd [a, b, c, d]对应平面方程a x b y c z d 0也可以写成nᵀ p d 0其中n [a, b, c]ᵀ就是局部地图平面的法向量。esti_plane(...)做的事情可以理解为输入 当前点附近的若干地图点 points_near 处理 判断这些点是否近似共面 拟合一个局部平面 输出 平面参数 [a, b, c, d] 如果平面拟合质量不好 返回 false这个 LiDAR 点不参与更新这一步的物理意义是当前 LiDAR 点附近的地图点如果能形成一个稳定平面比如墙面、地面、货架侧面那么当前点到这个平面的距离就可以作为约束如果邻居点很乱不能形成平面那这个点的残差不可靠直接丢掉。6. 第五步计算点到平面残差平面拟合成功后源码计算float pd2 pabcd(0) * point_world.x pabcd(1) * point_world.y pabcd(2) * point_world.z pabcd(3);也就是pd2 a x_W b y_W c z_W d对应数学公式r nᵀ p_W d其中p_W 当前点根据当前状态 s 投到世界系的位置。 n 地图局部平面法向量。 d 平面偏置项。 r / pd2 当前点到地图平面的带符号距离。如果pd2 0说明当前点正好落在地图平面上。如果|pd2|很大说明当前点离地图平面比较远要么状态不准要么匹配错了要么这个点属于动态物体或者噪声。源码之后又计算一个权重/筛选分数float s 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());然后判断if (s 0.9) { point_selected_surf[i] true; normvec-points[i].x pabcd(0); normvec-points[i].y pabcd(1); normvec-points[i].z pabcd(2); normvec-points[i].intensity pd2; res_last[i] abs(pd2); }源码中确实将pd2作为normvec-points[i].intensity保存下来并把法向量pabcd(0..2)保存到normvecres_last[i]保存的是abs(pd2)。这一步的含义是如果残差太大 这个点可能匹配错丢掉。 如果残差合理 保留这个点 保存它对应的平面法向量 n 保存它的残差 r。所以normvec这个名字虽然叫“法向量”但它的x,y,z保存法向量intensity保存残差pd2。7. 第六步收集所有有效点前面对每个点都做了投影到世界系 最近邻搜索 平面拟合 残差计算 有效性筛选接下来把最终有效的点统一收集起来if (point_selected_surf[i]) { laserCloudOri-points[effct_feat_num] feats_down_body-points[i]; corr_normvect-points[effct_feat_num] normvec-points[i]; total_residual res_last[i]; effct_feat_num ; }源码中就是把有效点保存进laserCloudOri把对应的法向量和残差保存进corr_normvect并统计effct_feat_num。这里要注意laserCloudOri 保存有效的当前帧点。 corr_normvect 保存每个有效点对应的地图平面法向量和残差。 effct_feat_num 当前帧最终参与 iEKF 更新的有效点数量。如果有效点数量太少if (effct_feat_num 1) { ekfom_data.valid false; ROS_WARN(No Effective Points!); return; }含义是没有可用 LiDAR 约束这一帧无法进行有效 LiDAR 更新。8. 第七步构造测量雅可比矩阵H有了有效点之后h_share_model()开始构造ekfom_data.h_x和ekfom_data.hekfom_data.h_x MatrixXd::Zero(effct_feat_num, 12); //23 ekfom_data.h.resize(effct_feat_num);源码里h_x的列数是12注释写了//23。这点很重要FAST-LIO 的完整误差状态是 23 维但 LiDAR 点到平面的几何残差在这里直接构造的是对前 12 维的雅可比也就是位置 3 维 姿态 3 维 LiDAR-IMU 旋转外参 3 维 LiDAR-IMU 平移外参 3 维合起来3 3 3 3 12为什么不是 23因为单个点到平面的瞬时几何距离直接由当前点在世界系的位置决定。而当前点在世界系的位置直接受下面四类量影响1. IMU 位置 pos 2. IMU 姿态 rot 3. LiDAR-IMU 旋转外参 offset_R_L_I 4. LiDAR-IMU 平移外参 offset_T_L_I速度、gyro bias、acc bias、重力不会直接出现在当前点的空间变换公式里p_W R_WI · ( R_IL · p_L t_IL ) t_WI所以它们不是当前点到平面残差的直接观测量。但它们会通过协方差耦合被间接更新。9. 单个点的雅可比怎么来对第i个有效点源码先取当前点const PointType laser_p laserCloudOri-points[i]; V3D point_this_be(laser_p.x, laser_p.y, laser_p.z);这里point_this_be可以理解为当前点在 body/LiDAR-body 坐标系下的位置也就是p_L。然后计算它经过外参后的 IMU 系坐标V3D point_this s.offset_R_L_I * point_this_be s.offset_T_L_I;对应p_I R_IL · p_L t_IL接着构造叉乘矩阵point_be_crossmat SKEW_SYM_MATRX(point_this_be); point_crossmat SKEW_SYM_MATRX(point_this);叉乘矩阵[p]×的作用是处理小角度旋转扰动。直观理解如果姿态有一个小旋转 δθ 点的位置变化可以近似写成和 [p]× δθ 有关。然后取出该点对应的平面法向量const PointType norm_p corr_normvect-points[i]; V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);norm_vec就是n [a, b, c]ᵀ也就是地图局部平面的单位法向量。10.C、A、B分别是什么源码中最关键的三行是V3D C(s.rot.conjugate() * norm_vec); V3D A(point_crossmat * C); 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);源码中确实是这样填充h_x前 3 列是norm_p.x/y/z中间是A和B最后是C若extrinsic_est_en关闭则外参相关项填 0。现在解释这几个量。C R_WIᵀ · n源码里写的是C s.rot.conjugate() * norm_vec如果s.rot表示R_WI那么s.rot.conjugate()就是R_IW也就是把世界系法向量转到 IMU 系C R_IW · n物理意义n 是世界系中的平面法向量 C 是这个法向量在 IMU 坐标系下的表达。为什么要转到 IMU 系因为后面姿态、外参扰动通常是在局部坐标系中表示用同一坐标系表达更方便。A [p_I]× · C源码A point_crossmat * C其中point_crossmat是p_I的叉乘矩阵。A对应的是IMU 姿态误差对残差的影响。直观解释如果 IMU 姿态转一点 整个 LiDAR 点 p_I 会绕 IMU 原点转一点 点到地图平面的距离会发生变化 A 就描述这种变化。所以A填到h_x中的位置对应姿态误差δθ的雅可比。B [p_L]× · R_LIᵀ · C源码B point_be_crossmat * s.offset_R_L_I.conjugate() * CB对应的是LiDAR-IMU 旋转外参误差对残差的影响。直观解释如果 LiDAR 和 IMU 之间的安装旋转外参错了一点 同一个 LiDAR 点转到 IMU 系的位置就会变 最终投到地图中也会变 点到平面的残差就会变。所以B填到h_x中的位置对应δθ_LI。最后的Ch_x最后 3 列填的是C对应LiDAR-IMU 平移外参误差对残差的影响。直观解释如果 LiDAR 相对 IMU 的平移外参 t_IL 改一点 点在 IMU 系中的位置也整体平移一点 这个平移投到平面法向量上就改变点到平面的残差。所以最终一行雅可比可以理解成H_i [ nᵀ, // 对位置 pos 的雅可比 A, // 对姿态 rot 的雅可比 B, // 对外参旋转 offset_R_L_I 的雅可比 C // 对外参平移 offset_T_L_I 的雅可比 ]也就是H_i [ ∂r_i/∂pos, ∂r_i/∂rot, ∂r_i/∂offset_R_L_I, ∂r_i/∂offset_T_L_I ]11. 如果不估计外参雅可比怎么变源码中有一个判断if (extrinsic_est_en) { h_x n, A, B, C; } else { h_x n, A, 0, 0, 0, 0, 0, 0; }也就是说extrinsic_est_en true LiDAR 残差会同时约束位置、姿态、外参旋转、外参平移。 extrinsic_est_en false LiDAR 残差只约束位置和姿态 外参旋转、外参平移对应的雅可比置零。这说明 FAST-LIO 可以选择是否在线估计 LiDAR-IMU 外参。如果外参已经标定得非常准可以关闭在线外参估计如果外参不够准可以开启让滤波器根据 LiDAR 残差慢慢修正外参。12. 残差向量h为什么是-norm_p.intensity源码中最后写ekfom_data.h(i) -norm_p.intensity;前面保存的是normvec-points[i].intensity pd2;所以这里ekfom_data.h(i) -pd2也就是h_i -r_i为什么是负号因为滤波器里更新公式可能写成h ≈ H δx或者把残差定义成“希望被修正掉的误差”。如果前面几何残差是r nᵀp_W d那么为了让残差变成 0需要求一个修正量让r Hδx ≈ 0等价于Hδx ≈ -r所以代码里给滤波器的是h -r这就是ekfom_data.h(i) -norm_p.intensity的原因。源码也在这一行注释为 “Measurement: distance to the closest surface/corner”。13.h_share_model()输出给 iEKF 的到底是什么最终h_share_model()输出两样最重要的东西ekfom_data.h 大小 effct_feat_num × 1 每一行是一个有效 LiDAR 点的负残差 -r_i。 ekfom_data.h_x 大小 effct_feat_num × 12 每一行是该点残差对位置、姿态、外参旋转、外参平移的雅可比。然后esekfom.hpp里的update_iterated_dyn_share_modified(...)会拿这些量做输入 h H P⁻ R 计算 Kalman 增益 K 状态修正量 δx 更新 x ← x ⊞ δx注意h_share_model()不负责更新状态x也不负责更新协方差P。它只负责回答在当前状态 s 下 当前帧 LiDAR 点云和局部地图之间的残差是多少 这些残差对状态的雅可比是多少14. 为什么这一节是 FAST-LIO 前端核心你前面学的esekfom.hpp是滤波器核心它负责“拿到残差后怎么迭代更新”。但真正让 LiDAR 约束进入滤波器的地方就是h_share_model()。它完成的是当前点云 ↓ 当前状态投影到世界系 ↓ 局部地图最近邻 ↓ 平面拟合 ↓ 点到平面残差 ↓ 残差对状态的雅可比 ↓ 送入 iEKF所以h_share_model()本质上就是 FAST-LIO 的scan-to-map measurement model也就是 LiDAR 观测模型。它不是传统意义上单独跑一个 ICP/NDT 然后输出一个完整位姿而是把大量点到平面的残差直接作为滤波器观测让 iEKF 自己根据 IMU 先验和 LiDAR 残差联合求解状态修正量。FAST-LIO 论文描述其使用 tightly-coupled iterated EKF 融合 LiDAR feature points 和 IMU并提出让 Kalman 增益计算复杂度依赖状态维度而不是测量维度的方式这正对应这里h_share_model()提供大量点到平面测量esekfom.hpp负责高效融合这些测量。15总结h_share_model()是 FAST-LIO 中非常关键的 LiDAR 前端观测模型函数。它的作用不是直接更新状态也不是直接更新协方差而是负责把当前帧 LiDAR 点云转换成 iEKF 可以使用的观测量也就是残差向量h和雅可比矩阵H。前面学习esekfom.hpp时重点是理解 iEKF 如何根据残差和雅可比迭代更新状态x和协方差P而这一节的h_share_model()重点就是理解这些残差和雅可比到底是从哪里来的。FAST-LIO 的整体流程可以理解为IMU 先通过predict()给出预测状态x⁻和预测协方差P⁻这个预测状态包含位置、姿态、速度、IMU bias、LiDAR-IMU 外参、重力方向等信息。但是 IMU 预测会漂移所以当前帧 LiDAR 到来后系统需要检查当前点云按照x⁻投到地图里后是否能和局部地图对齐。h_share_model()做的正是这个检查过程它把当前帧点云投影到世界坐标系在局部地图里找最近邻点拟合局部平面计算点到平面的残差再计算残差对状态的雅可比最后把这些结果交给kf.update_iterated_dyn_share_modified(...)。具体来说h_share_model()的输入里有当前迭代状态s。这个s不是固定不变的预测状态而是 iEKF 当前这一轮迭代正在使用的状态。因为 iEKF 每轮都会更新一次x所以每一轮调用h_share_model()时点云投到世界系的位置都可能不同最近邻地图点、拟合平面、残差和雅可比也可能不同。因此h_share_model()会在每轮迭代中重新计算 LiDAR 观测而不是只在一开始计算一次。函数首先会清空上一轮保存的有效点、法向量和残差等缓存。然后对当前帧降采样后的每个点进行处理。当前点最开始在 LiDAR/body 坐标系下记作p_L。系统利用当前状态中的 LiDAR-IMU 外参offset_R_L_I、offset_T_L_I先把点从 LiDAR 系变到 IMU 系再利用当前 IMU 在世界系下的姿态rot和位置pos把点从 IMU 系变到世界/地图系。公式可以写成p_W R_WI · (R_IL · p_L t_IL) t_WI其中p_W是当前点在世界/地图坐标系中的位置。这个变换非常关键因为局部地图点都在世界坐标系下当前帧点只有投到世界系才能和地图进行匹配。接下来h_share_model()会拿p_W去 ikd-Tree 中做最近邻搜索。ikd-Tree 保存的是局部地图点最近邻搜索的目的是找到当前点附近的若干地图点。如果邻居点数量不够或者最近邻距离太远就说明当前点附近地图信息不足不能形成可靠约束这个点会被丢弃。只有当当前点附近有足够多、足够近的地图点时才继续进行局部平面拟合。局部平面拟合是h_share_model()构造几何残差的核心。它会用当前点附近的地图邻居点拟合一个平面平面方程可以写成nᵀp d 0其中n是平面法向量d是平面偏置。如果邻域点本身比较散乱不能稳定拟合成平面那么这个点也不会参与后续更新。只有平面拟合成功系统才会计算当前 LiDAR 点到该平面的带符号距离r nᵀp_W d这个r就是点到平面的残差。如果r接近 0说明当前点按照当前状态投影后刚好贴在地图平面上当前状态比较合理如果|r|很大说明当前点和地图平面偏离较多可能是状态预测不准也可能是匹配错误、动态物体、噪声点。因此 FAST-LIO 还会根据残差大小做一次筛选只保留残差合理、平面可靠的点。被保留下来的点会进入有效点集合对应的法向量和残差也会被保存下来。当所有点处理完成后h_share_model()得到一批有效点。假设有效点数量是m那么每个有效点都会提供一个点到平面的残差最终形成一个m维的残差向量。注意在源码里保存到ekfom_data.h的通常是-r也就是残差的相反数。这是因为滤波器内部的线性化更新形式可以理解为要求Hδx ≈ -r通过求解修正量δx来把当前残差抵消掉。负号只是公式定义方式的问题本质仍然是让修正后的点云尽量贴合地图。除了残差h_share_model()还要构造雅可比矩阵H。雅可比表示状态小变化时点到平面残差会怎样变化。FAST-LIO 的完整误差状态是 23 维但是 LiDAR 单点几何残差直接依赖的主要是前 12 维位置误差δp、姿态误差δθ、LiDAR-IMU 旋转外参误差δθ_LI、LiDAR-IMU 平移外参误差δt_LI。因为当前点投到世界系的公式只直接包含位置、姿态和外参p_W R_WI · (R_IL · p_L t_IL) t_WI速度、陀螺仪 bias、加速度计 bias、重力方向并不直接出现在这个瞬时点到平面的几何残差里所以它们不是 LiDAR 单点残差的直接观测对象。不过这并不代表 LiDAR 不能修正它们因为协方差P中记录了这些状态与位置、姿态之间的耦合关系。例如 gyro bias 会影响姿态姿态会影响速度速度会影响位置。当 LiDAR 发现位置和姿态偏了滤波器可以通过协方差间接修正 bias、速度和重力方向。在源码里h_x通常构造成m × 12每一行对应一个有效点的雅可比。前 3 列是残差对位置的导数本质上就是平面法向量n中间 3 列是残差对 IMU 姿态的导数描述机器人姿态小幅旋转时点到平面的距离如何变化再后 3 列是残差对 LiDAR-IMU 旋转外参的导数描述外参旋转误差如何影响点的投影位置最后 3 列是残差对 LiDAR-IMU 平移外参的导数描述外参平移误差如何改变残差。如果关闭在线外参估计那么外参相关的雅可比会被置零也就是 LiDAR 残差只用于修正位置和姿态不再修正外参。源码中常见的C、A、B可以这样理解C是把世界系下的平面法向量转到 IMU 坐标系后的表达它主要用于外参平移雅可比A与当前点在 IMU 系下的位置叉乘有关表示 IMU 姿态误差对残差的影响B与当前点在 LiDAR/body 系下的位置以及外参旋转有关表示 LiDAR-IMU 旋转外参误差对残差的影响。最终每个有效点的一行雅可比可以概念化理解成H_i [ ∂r_i/∂pos, ∂r_i/∂rot, ∂r_i/∂offset_R_L_I, ∂r_i/∂offset_T_L_I ]这就是h_share_model()给 iEKF 提供的测量模型。它不负责求最终修正量δx也不负责更新x和P。它只负责告诉滤波器“在当前状态下当前点云和地图之间有多不贴合如果状态的某些维度变一点这些残差会怎么变。”之后update_iterated_dyn_share_modified(...)会拿这些h和H结合 IMU 预测协方差P⁻和 LiDAR 观测噪声R计算 Kalman 修正量并通过boxplus更新状态。所以h_share_model()可以看成 FAST-LIO 前端的“观测构造器”。它把原始 LiDAR 点云和局部地图之间的几何关系转换成滤波器能理解的数学形式残差向量和雅可比矩阵。它的整条逻辑就是当前点云先根据状态变换到世界系然后在局部地图中找最近邻利用邻近地图点拟合平面计算点到平面的残差筛选可靠约束最后构造残差对状态的雅可比。没有h_share_model()iEKF 就没有 LiDAR 观测没有esekfom.hpp的更新函数LiDAR 观测也无法真正修正状态。两者合在一起才构成了 FAST-LIO 的核心闭环IMU 预测给出连续运动先验LiDAR 前端构造点到地图的几何残差iEKF 根据残差迭代修正状态最终实现实时、稳定的 LiDAR-IMU 里程计。