FAST-LIO2源码精读：IMU到ESKF全流程解析

m0_59010573

514人浏览 · 2026-06-05 09:51:17

m0_59010573 · 2026-06-05 09:51:17 发布

本文基于 FAST-LIO2 开源代码逐函数分析其完整算法流程，帮助读者深入理解 LiDAR-IMU 紧耦合状态估计的工程实现。适合准备 SLAM 算法岗面试、或正在学习激光惯性里程计的同学。

面试速查（30秒版）

模块	一句话职责	核心源码位置
状态定义	24维流形状态：pos, rot, vel, bg, ba, grav, 外参	`use-ikfom.hpp`
IMU初始化	静止状态下估计重力方向和陀螺仪零偏	`IMU_Processing.hpp:159-214`
IMU前向传播	逐IMU帧做状态预测，记录中间位姿	`IMU_Processing.hpp:250-296`
点云去畸变	反向传播，将每个点补偿到帧末时刻	`IMU_Processing.hpp:307-345`
最近邻搜索	ikd-Tree中搜索5个最近点并拟合平面	`laserMapping.cpp:650-693`
观测模型H	点到面距离残差 + Jacobian矩阵构建	`laserMapping.cpp:719-753`
iterated ESKF	多次迭代更新，收敛后得到最优估计	`laserMapping.cpp:960`
地图更新	增量式插入新点到ikd-Tree	`laserMapping.cpp:427-474`

FAST-LIO2 核心贡献：

不需要特征提取，直接用原始点云配准（raw point → scan-to-map）
ikd-Tree 支持增量式地图维护，无需每帧重建
iterated ESKF 实现紧耦合，计算效率极高（单帧<10ms）

一、整体架构与数据流

IMU + LiDAR
    │
    ▼
① 时间同步 ──▶ ② IMU前向传播 ──▶ ③ 点云去畸变 ──▶ ④ 降采样
                                                        │
    ┌───────────────────────────────────────────────────┘
    ▼
⑤ iterated ESKF 更新 ◄──── ikd-Tree(全局地图) ────▶ ⑦ FOV管理
    │                           ▲
    ▼                           │
⑥ 地图增量更新 ─────────────────┘
    │
    ▼
输出位姿 + 地图

主循环入口（`laserMapping.cpp:865-1015`）

FAST-LIO2 的主循环简洁清晰：

while (status) {
    if (sync_packages(Measures)) {        // 1. 时间同步：对齐LiDAR与IMU数据
        p_imu->Process(Measures, kf, feats_undistort);  // 2. IMU处理+去畸变
        lasermap_fov_segment();           // 3. 地图FOV管理
        downSizeFilterSurf.filter(*feats_down_body);    // 4. 体素降采样
        kf.update_iterated_dyn_share_modified(...);     // 5. iterated ESKF更新
        map_incremental();                // 6. 地图增量更新
    }
}

二、状态定义（`use-ikfom.hpp`）

FAST-LIO2 使用 IKFoM 库在流形上定义状态：

MTK_BUILD_MANIFOLD(state_ikfom,
    ((vect3, pos))          // 位置 [3]         idx: 0-2
    ((SO3, rot))            // 姿态(SO3) [3]    idx: 3-5
    ((SO3, offset_R_L_I))   // LiDAR-IMU旋转外参 [3]  idx: 6-8
    ((vect3, offset_T_L_I)) // LiDAR-IMU平移外参 [3]  idx: 9-11
    ((vect3, vel))          // 速度 [3]         idx: 12-14
    ((vect3, bg))           // 陀螺仪零偏 [3]   idx: 15-17
    ((vect3, ba))           // 加速度计零偏 [3]  idx: 18-20
    ((S2, grav))            // 重力(S2流形) [2]  idx: 21-22
);

面试要点：

全量状态维度：24（嵌入空间），误差状态维度：23（切空间，因为SO3的切空间是3维，S2的切空间是2维）
重力用 S2 流形（二维球面）表示而非 R3 向量，只有2个自由度，因为重力大小已知（≈9.81），只需估计方向
外参 offset_R_L_I、offset_T_L_I 可在线标定（extrinsic_est_en=true 时）

运动模型 `get_f()`

Eigen::Matrix<double, 24, 1> get_f(state_ikfom &s, const input_ikfom &in) {
    Eigen::Matrix<double, 24, 1> res = Eigen::Matrix<double, 24, 1>::Zero();
    vect3 omega;
    in.gyro.boxminus(omega, s.bg);          // ω = gyro - bg
    vect3 a_inertial = s.rot * (in.acc - s.ba);  // a_w = R * (acc - ba)
    for(int i = 0; i < 3; i++) {
        res(i) = s.vel[i];                   // ṗ = v
        res(i + 3) = omega[i];               // Ṙ = ω (李代数)
        res(i + 12) = a_inertial[i] + s.grav[i];  // v̇ = R*(acc-ba) + g
    }
    return res;
}

即连续时间运动方程：

$p˙=v\dot{p} = v$
$R˙=R⋅[ω−bg]×\dot{R} = R \cdot [\omega - b_g]_\times$
$v˙=R⋅(am−ba)+g\dot{v} = R \cdot (a_m - b_a) + g$
$b˙g=nbg\dot{b}_g = n_{bg}$
$b˙a=nba\dot{b}_a = n_{ba}$

状态转移 Jacobian `df_dx()`

Eigen::Matrix<double, 24, 23> df_dx(state_ikfom &s, const input_ikfom &in) {
    // F矩阵的关键非零块：
    cov.block<3,3>(0, 12) = I;                    // ∂ṗ/∂v = I
    cov.block<3,3>(12, 3) = -R*[acc-ba]×;         // ∂v̇/∂θ = -R*hat(acc-ba)
    cov.block<3,3>(12, 18) = -R;                   // ∂v̇/∂ba = -R
    cov.block<3,2>(12, 21) = ∂g/∂S2;              // ∂v̇/∂grav (S2流形)
    cov.block<3,3>(3, 15) = -I;                    // ∂Ṙ/∂bg = -I
}

三、IMU初始化（`IMU_Processing.hpp:159-214`）

目的

在系统启动时，利用静止阶段的IMU数据估计：

重力方向：通过加速度计均值获得
陀螺仪零偏：通过角速度均值获得

核心实现

void ImuProcess::IMU_init(const MeasureGroup &meas, ..., int &N) {
    // 增量式计算均值（Welford算法）
    for (const auto &imu : meas.imu) {
        mean_acc += (cur_acc - mean_acc) / N;    // 加速度均值 → 重力方向
        mean_gyr += (cur_gyr - mean_gyr) / N;    // 角速度均值 → 陀螺仪零偏
        // 同时增量计算协方差
        cov_acc = cov_acc*(N-1)/N + (cur_acc-mean_acc)²*(N-1)/(N*N);
        N++;
    }
    
    // 初始化状态
    init_state.grav = S2(-mean_acc / mean_acc.norm() * G_m_s2);  // 重力方向
    init_state.bg = mean_gyr;                                      // 陀螺仪零偏
}

面试要点：

使用 Welford 增量均值算法，避免存储所有历史数据
重力初始化：假设静止时 acc ≈ -g，所以 g = -mean_acc/||mean_acc|| * 9.81
需要累积足够多的 IMU 帧（MAX_INI_COUNT=10 个 LiDAR 帧的 IMU 数据）
初始化完成的标志：init_iter_num > MAX_INI_COUNT

四、IMU前向传播与点云去畸变（`IMU_Processing.hpp:216-346`）

这是 FAST-LIO2 的精华部分之一。

4.1 前向传播（Forward Propagation）

逐个 IMU 测量做状态预测，并记录每个 IMU 时刻的位姿：

for (auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++) {
    // 中值积分取前后IMU的均值
    angvel_avr = 0.5 * (head->angular_velocity + tail->angular_velocity);
    acc_avr    = 0.5 * (head->linear_acceleration + tail->linear_acceleration);
    
    // 重力补偿（按比例缩放到标准重力）
    acc_avr = acc_avr * G_m_s2 / mean_acc.norm();
    
    // 调用ESKF预测步
    in.acc = acc_avr;
    in.gyro = angvel_avr;
    kf_state.predict(dt, Q, in);
    
    // 记录IMU位姿（用于后续去畸变）
    IMUpose.push_back(set_pose6d(offs_t, acc_s_last, angvel_last, vel, pos, rot));
}

关键细节：

中值积分：取相邻两个IMU测量的均值，比前向欧拉法精度更高
时间对齐：如果 head 时间早于上一帧LiDAR结束时间，需要截断 dt
acc缩放：acc_avr * G_m_s2 / mean_acc.norm() 补偿加速度计的标度因数误差

4.2 反向传播去畸变（Backward Propagation）

LiDAR 一帧扫描需要约100ms，在此期间载体在运动，每个点对应不同时刻的位姿。去畸变就是将所有点统一到帧末时刻的坐标系下：

// 从后往前遍历点云
for (auto it_kp = IMUpose.end()-1; it_kp != IMUpose.begin(); it_kp--) {
    for (; it_pcl->curvature/1000 > head->offset_time; it_pcl--) {
        dt = it_pcl->curvature/1000 - head->offset_time;
        
        // 计算该点时刻的IMU位姿（在IMU位姿间线性插值）
        M3D R_i = R_imu * Exp(angvel_avr, dt);
        V3D T_ei = pos_imu + vel_imu*dt + 0.5*acc_imu*dt*dt - imu_state.pos;
        
        // 从LiDAR系 → IMU系 → 世界系 → 帧末IMU系 → 帧末LiDAR系
        V3D P_compensate = R_L_I^T * (R_e^T * (R_i * (R_L_I * P_i + t_L_I) + T_ei) - t_L_I);
        
        it_pcl->x = P_compensate(0);
        it_pcl->y = P_compensate(1);
        it_pcl->z = P_compensate(2);
    }
}

去畸变公式的物理含义（面试必讲）：

$Pcomp=TLI−1⋅Te−1⋅Ti⋅TLI⋅PiLP_{comp} = T_{L_I}^{-1} \cdot T_{e}^{-1} \cdot T_i \cdot T_{L_I} \cdot P_i^L$

展开为：

$TL_I⋅PiLT_{L\_I} \cdot P_i^L$ ：LiDAR系 → IMU系
$Ti⋅(...)T_i \cdot (...)$ ：点采集时刻IMU系 → 世界系
$Te−1⋅(...)T_e^{-1} \cdot (...)$ ：世界系 → 帧末IMU系
$TL_I−1⋅(...)T_{L\_I}^{-1} \cdot (...)$ ：帧末IMU系 → 帧末LiDAR系

面试追问：为什么反向传播而不是正向？

因为点云按时间升序排列，最后一个点对应帧末时刻。反向遍历时，IMU位姿序列天然对应点云时间顺序，便于二分定位每个点所属的IMU区间。此外，补偿到帧末对应的是 ESKF 预测完成后的状态，物理意义明确。

五、观测模型 — 点到面残差（`laserMapping.cpp:638-753`）

这是 ESKF 的观测更新模型，函数名 h_share_model 表示这是一个被多次迭代调用的共享观测函数。

5.1 最近邻搜索与平面拟合

for (int i = 0; i < feats_down_size; i++) {
    // 1. 将点从body系转到世界系
    V3D p_global = s.rot * (s.offset_R_L_I * p_body + s.offset_T_L_I) + s.pos;
    
    // 2. ikd-Tree中搜索5个最近邻点
    ikdtree.Nearest_Search(point_world, NUM_MATCH_POINTS, points_near, pointSearchSqDis);
    
    // 3. 5点拟合平面：Ax+By+Cz+D=0
    esti_plane(pabcd, points_near, 0.1f);
    
    // 4. 计算点到平面距离作为残差
    float pd2 = pabcd(0)*x + pabcd(1)*y + pabcd(2)*z + pabcd(3);
    
    // 5. 距离阈值筛选有效匹配点
    float s = 1 - 0.9 * fabs(pd2) / sqrt(p_body.norm());
    if (s > 0.9) point_selected_surf[i] = true;
}

平面拟合方法（common_lib.h:226-257）：

平面方程： $A x + B y + C z + D = 0$
转化为： $(A / D) x + (B / D) y + (C / D) z = - 1$
构建最小二乘： $A5×3⋅x3×1=b5×1A_{5\times3} \cdot x_{3\times1} = b_{5\times1}$ ，其中 $b = [-1,...,-1]^T$
使用 QR 分解求解法向量

5.2 Jacobian 矩阵 H 的构建

这是面试中最常被追问的数学推导部分。

观测方程为点到平面距离：
$n^T \cdot (R \cdot (R_L \cdot p^L + t_L) + p) + d$

其中 $n$ 是平面法向量， $p^L$ 是 LiDAR 系下的点坐标。

对误差状态求导：

for (int i = 0; i < effct_feat_num; i++) {
    V3D point_this = s.offset_R_L_I * point_be + s.offset_T_L_I;  // IMU系下的点
    
    V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);  // 平面法向量(世界系)
    
    // ∂h/∂pos = n^T (直接就是法向量)
    // ∂h/∂θ = n^T * R * [point_this]× = n^T的旋转到body再叉乘
    V3D C = s.rot.conjugate() * norm_vec;       // n在body系下的表示
    V3D A = point_crossmat * C;                  // [p_imu]× * R^T*n
    
    // 如果估计外参：∂h/∂θ_ext
    V3D B = point_be_crossmat * s.offset_R_L_I.conjugate() * C;
    
    // H矩阵的第i行：[∂h/∂pos, ∂h/∂θ, ∂h/∂θ_ext, ∂h/∂t_ext]
    h_x.block<1,12>(i,0) << n_x, n_y, n_z, A(0), A(1), A(2), B(0), B(1), B(2), C(0), C(1), C(2);
    
    // 观测值（残差）：负的点到面距离
    ekfom_data.h(i) = -pd2;
}

Jacobian推导详解：

设 $pw=R⋅pI+tp_w = R \cdot p_I + t$ ，其中 $pI=RL⋅pL+tLp_I = R_L \cdot p^L + t_L$

对旋转 $R$ 的扰动 $δθ\delta\theta$ ：
$∂h∂δθ=nT⋅∂(R⋅pI)∂δθ=nT⋅R⋅(−[pI]×)=−(RTn)T[pI]×=[pI]×⋅(RTn)\frac{\partial h}{\partial \delta\theta} = n^T \cdot \frac{\partial(R \cdot p_I)}{\partial \delta\theta} = n^T \cdot R \cdot (-[p_I]_\times) = -(R^T n)^T [p_I]_\times = [p_I]_\times \cdot (R^T n)$

代码中 A = point_crossmat * C 正是 $[pI]×⋅(RTn)[p_I]_\times \cdot (R^T n)$ 。

对位置 $t$ 的扰动：
$∂h∂t=nT\frac{\partial h}{\partial t} = n^T$

代码中直接用 norm_p.x, norm_p.y, norm_p.z。

六、iterated ESKF 更新

6.1 初始化ESKF（`laserMapping.cpp:828`）

kf.init_dyn_share(get_f, df_dx, df_dw, h_share_model, NUM_MAX_ITERATIONS, epsi);

参数含义：

get_f：运动模型 $f (x, u)$
df_dx：状态转移 Jacobian $F_x$
df_dw：噪声 Jacobian $F_w$
h_share_model：观测模型（每次迭代重新调用）
NUM_MAX_ITERATIONS=4：最大迭代次数
epsi=0.001：收敛阈值

6.2 迭代更新流程

kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);

iterated ESKF 的伪代码：

输入：预测状态 x̂⁻, 协方差 P⁻
for iter = 1 to NUM_MAX_ITERATIONS:
    1. 在当前线性化点计算 H 和 z（调用 h_share_model）
    2. 计算卡尔曼增益：K = P⁻Hᵀ(HP⁻Hᵀ + R)⁻¹
    3. 更新误差状态：δx = K(z - H*δx_iter) + (I-KH)(x̂⁻ ⊖ x̂_iter)
    4. 更新状态：x̂⁺ = x̂⁻ ⊕ δx
    5. 检查收敛：if ||δx|| < ε, break
输出：最优状态 x̂⁺, 更新协方差 P⁺ = (I-KH)P⁻

面试追问：为什么要迭代？

因为观测方程 $h (x)$ 是非线性的（包含旋转），一次线性化在远离真值时不够精确。通过多次迭代，每次在新的估计值处重新线性化，逐步逼近最优解。通常4次迭代即可收敛。

面试追问：迭代和非迭代的本质区别？

非迭代EKF只在预测值处做一次线性化；iterated EKF等价于在每次迭代时重新评估H和z，本质上是在做高斯-牛顿优化（MAP估计），精度更高。

七、地图管理

7.1 FOV动态管理（`laserMapping.cpp:231-277`）

FAST-LIO2 维护一个以当前位置为中心的立方体局部地图：

void lasermap_fov_segment() {
    // 检查当前位置是否接近局部地图边界
    for (int i = 0; i < 3; i++) {
        dist_to_map_edge[i][0] = |pos_LiD(i) - vertex_min[i]|;
        if (dist_to_edge <= MOV_THRESHOLD * DET_RANGE) need_move = true;
    }
    // 如果需要移动，通过 box-wise 删除超出范围的点
    ikdtree.Delete_Point_Boxes(cub_needrm);
}

7.2 增量式地图更新（`laserMapping.cpp:427-474`）

void map_incremental() {
    for (int i = 0; i < feats_down_size; i++) {
        pointBodyToWorld(&feats_down_body->points[i], &feats_down_world->points[i]);
        // 智能降采样：只有当新点比已有最近点更接近体素中心时才插入
        if (need_add) PointToAdd.push_back(point);
    }
    ikdtree.Add_Points(PointToAdd, true);        // 需要降采样的点
    ikdtree.Add_Points(PointNoNeedDownsample, false);  // 无需降采样的点
}

降采样策略：对于每个体素格（filter_size_map_min），只保留最接近体素中心的那个点。新点插入时，如果它比已有点更接近中心，就替换。

八、关键工具函数

8.1 SO(3) 指数映射 — Rodrigues公式（`so3_math.h:18-34`）

template<typename T>
Eigen::Matrix<T, 3, 3> Exp(const Eigen::Matrix<T, 3, 1> &&ang) {
    T ang_norm = ang.norm();
    if (ang_norm > 1e-7) {
        Eigen::Matrix<T, 3, 1> r_axis = ang / ang_norm;  // 转轴
        Eigen::Matrix<T, 3, 3> K;
        K << SKEW_SYM_MATRX(r_axis);                      // 反对称矩阵
        return I + sin(θ)*K + (1-cos(θ))*K*K;             // Rodrigues
    } else {
        return I;  // 小角度近似
    }
}

公式： $exp⁡([ϕ]×)=I+sin⁡θθ[ϕ]×+1−cos⁡θθ2[ϕ]×2\exp([\phi]_\times) = I + \frac{\sin\theta}{\theta}[\phi]_\times + \frac{1-\cos\theta}{\theta^2}[\phi]_\times^2$

8.2 SO(3) 对数映射（`so3_math.h:82-87`）

template<typename T>
Eigen::Matrix<T,3,1> Log(const Eigen::Matrix<T, 3, 3> &R) {
    T theta = (R.trace() > 3.0-1e-6) ? 0.0 : acos(0.5*(R.trace()-1));
    Eigen::Matrix<T,3,1> K(R(2,1)-R(1,2), R(0,2)-R(2,0), R(1,0)-R(0,1));
    return (abs(theta) < 0.001) ? (0.5*K) : (0.5*theta/sin(theta)*K);
}

九、数据同步（`laserMapping.cpp:368-424`）

bool sync_packages(MeasureGroup &meas) {
    // 策略：找到覆盖一帧LiDAR时间范围内的所有IMU数据
    // LiDAR帧时间：[lidar_beg_time, lidar_end_time]
    // 要求：imu_buffer中最后一个IMU时间 > lidar_end_time
    
    // 点云的时间戳存在 curvature 字段中（单位：ms）
    lidar_end_time = lidar_beg_time + points.back().curvature / 1000.0;
    
    // 收集该时间段内所有IMU数据
    while (imu_time < lidar_end_time) {
        meas.imu.push_back(imu_buffer.front());
        imu_buffer.pop_front();
    }
}

设计亮点：

点云每个点的相对时间戳存在 curvature 字段（复用PCL字段，节省内存）
时间同步支持自动检测 LiDAR-IMU 时间偏移（timediff_lidar_wrt_imu）

面试真题与答题要点

Q1: 请简述 FAST-LIO2 的完整算法流程？

答题框架（30秒版）：

FAST-LIO2 是一个基于 iterated ESKF 的 LiDAR-IMU 紧耦合里程计。数据输入后先做 LiDAR-IMU 时间同步，然后 IMU 前向传播做状态预测，同时记录中间位姿用于点云去畸变。去畸变后的点云做体素降采样，在 ikd-Tree 全局地图中搜索最近邻并拟合平面，构建点到面距离残差。通过多次迭代的 ESKF 更新得到最优状态估计，最后将新点增量式插入 ikd-Tree。

Q2: 为什么 FAST-LIO2 不需要特征提取？

ikd-Tree 支持高效的增量式最近邻搜索，直接对原始点做 scan-to-map 匹配

点到面残差天然提供了几何约束，不需要显式提取 edge/planar 特征

使用全部点（降采样后）比只用特征点信息量更大，在退化场景中更鲁棒

Q3: FAST-LIO2 中去畸变是怎么实现的？

利用 IMU 前向传播记录的中间位姿序列，通过反向遍历将每个点的坐标从采集时刻补偿到帧末时刻。具体变换链为：LiDAR系→IMU系→世界系→帧末IMU系→帧末LiDAR系。

Q4: iterated ESKF 相比普通 EKF 有什么优势？

误差状态是小量，线性化更精确

迭代更新等价于做 MAP 估计/高斯-牛顿优化

旋转在切空间操作，避免了 SO(3) 非向量空间的问题

通常4次迭代即可收敛，计算量可控

Q5: FAST-LIO2 的重力为什么用 S2 表示？

重力向量的模已知（≈9.81），只有方向2个自由度。用 S2 流形表示减少了1个参数（23维误差状态而非24维），避免了过参数化带来的奇异性。

Q6: FAST-LIO2 vs LIO-SAM 的核心区别？

维度	FAST-LIO2	LIO-SAM
后端	iterated ESKF（滤波）	因子图优化（GTSAM）
特征	无需特征提取	edge + planar 特征
地图	ikd-Tree全局地图	关键帧 + 局部submap
回环	无	ScanContext / GPS
外参	支持在线标定	需要离线标定
实时性	~5ms/帧	~30ms/帧
适用	小范围高频实时	大范围全局一致性

所有评论(0)

查看更多评论

m0_59010573

@m0_59010573

已为社区贡献3条内容

FAST-LIO2源码精读：IMU到ESKF全流程解析

m0_59010573

面试速查（30秒版）

一、整体架构与数据流

主循环入口（laserMapping.cpp:865-1015）

二、状态定义（use-ikfom.hpp）

运动模型 get_f()

状态转移 Jacobian df_dx()

三、IMU初始化（IMU_Processing.hpp:159-214）

目的

核心实现

四、IMU前向传播与点云去畸变（IMU_Processing.hpp:216-346）

4.1 前向传播（Forward Propagation）

4.2 反向传播去畸变（Backward Propagation）

五、观测模型 — 点到面残差（laserMapping.cpp:638-753）

5.1 最近邻搜索与平面拟合

5.2 Jacobian 矩阵 H 的构建

六、iterated ESKF 更新

6.1 初始化ESKF（laserMapping.cpp:828）

6.2 迭代更新流程

七、地图管理

7.1 FOV动态管理（laserMapping.cpp:231-277）

7.2 增量式地图更新（laserMapping.cpp:427-474）

八、关键工具函数

8.1 SO(3) 指数映射 — Rodrigues公式（so3_math.h:18-34）

8.2 SO(3) 对数映射（so3_math.h:82-87）

九、数据同步（laserMapping.cpp:368-424）

面试真题与答题要点

Q1: 请简述 FAST-LIO2 的完整算法流程？

Q2: 为什么 FAST-LIO2 不需要特征提取？

Q3: FAST-LIO2 中去畸变是怎么实现的？

Q4: iterated ESKF 相比普通 EKF 有什么优势？

Q5: FAST-LIO2 的重力为什么用 S2 表示？

Q6: FAST-LIO2 vs LIO-SAM 的核心区别？

延伸阅读

所有评论(0)

温馨提示：您尚未绑定手机号

m0_59010573

主循环入口（`laserMapping.cpp:865-1015`）

二、状态定义（`use-ikfom.hpp`）

运动模型 `get_f()`

状态转移 Jacobian `df_dx()`

三、IMU初始化（`IMU_Processing.hpp:159-214`）

四、IMU前向传播与点云去畸变（`IMU_Processing.hpp:216-346`）

五、观测模型 — 点到面残差（`laserMapping.cpp:638-753`）

6.1 初始化ESKF（`laserMapping.cpp:828`）

7.1 FOV动态管理（`laserMapping.cpp:231-277`）

7.2 增量式地图更新（`laserMapping.cpp:427-474`）

8.1 SO(3) 指数映射 — Rodrigues公式（`so3_math.h:18-34`）

8.2 SO(3) 对数映射（`so3_math.h:82-87`）

九、数据同步（`laserMapping.cpp:368-424`）