FAST-LIO src/IMU_Processing.hpp 完整详细讲解

这份IMU_Processing.hpp是 FAST-LIO 前端里最关键的模块之一。它不负责 ikd-Tree 最近邻搜索，也不直接构造点到平面残差；它负责在 LiDAR 匹配前完成两件基础工作：

1．利用高频 IMU，把上一帧结束后的状态传播到当前 LiDAR 扫描结束时刻。 2．利用扫描期间的 IMU 运动轨迹，把一帧中不同时间采到的 LiDAR 点， 统一补偿到“扫描结束时刻”的 LiDAR 坐标系。

也就是：

sync_packages() ↓ 得到当前 LiDAR 帧 + 对应 IMU 数据 ↓ ImuProcess::Process() ↓ IMU 初始化 或 IMU 前向传播 ↓ UndistortPcl() ↓ 去畸变点云 feats_undistort ↓ laserMapping.cpp ↓ ikd-Tree 匹配 + IESKF LiDAR 修正

你给的博客把laserMapping.cpp主链路拆成“同步 → IMU 传播与去畸变 → LiDAR 匹配 → IESKF → 地图更新”；这个IMU_Processing.hpp正好就是其中第二段的完整实现。

1．ImuProcess类整体职责

类定义中的核心接口是：

void Process( const MeasureGroup &meas, esekfom::esekf<state_ikfom, 12, input_ikfom> &kf_state, PointCloudXYZI::Ptr pcl_un_);

三个输入分别是：

meas ：sync_packages() 打好的当前数据包。 里面至少包含当前 LiDAR、LiDAR 起止时间、当前帧需要的 IMU。 kf_state ：FAST-LIO 的 IESKF 状态。 进入本函数时，它保存的是上一帧 LiDAR 优化后的状态； 离开本函数时，它变成当前扫描结束时刻的 IMU 预测状态。 pcl_un_ ：输出去畸变点云。 后续 laserMapping.cpp 会将它作为 feats_undistort 使用。

Process()的工作流很简单：

当前帧 MeasureGroup ↓ 没有 IMU？ ↓ 是 直接 return 仍处于初始化阶段？ ↓ 是 IMU_init() 初始化重力、gyro bias、协方差 ↓ return 初始化完成？ ↓ 是 UndistortPcl() IMU 前向传播 + 逐点去畸变

对应源码主逻辑：

if(meas.imu.empty()) {return;}; if (imu_need_init_) { IMU_init(meas, kf_state, init_iter_num); if (init_iter_num > MAX_INI_COUNT) { imu_need_init_ = false; } return; } UndistortPcl(meas, kf_state, *cur_pcl_un_);

这意味着原版这条流程没有“没有 IMU 就自动使用纯 LiDAR 去畸变”的备用逻辑。没有 IMU 时，它不会做本帧预测，也不会做逐点去畸变。

2．输入MeasureGroup到底包含什么

虽然MeasureGroup定义在其他文件中，但从本文件使用方式可以确定，它至少包含：

meas.lidar ：当前帧经过 preprocess.cpp 处理后的 LiDAR 点云。 meas.imu ：当前 LiDAR 扫描相关的 IMU 队列。 meas.lidar_beg_time ：当前 LiDAR 扫描开始时刻。 meas.lidar_end_time ：当前 LiDAR 扫描结束时刻。

其时间关系是：

LiDAR 当前帧： t_begin -------------------------------- t_end IMU： u0 --- u1 --- u2 --- ... --- un 要求： 最后到达的 IMU 时间已经覆盖 t_end， 但当前打包进 meas.imu 的最后一条 IMU 时间通常不超过 t_end。

所以在UndistortPcl()中，最后一条实际取出的 IMU 很可能略早于扫描结束时间。随后代码会使用这最后一段 IMU 的运动模型，把状态短时间传播到精确的t_end。

最后一条取出的 IMU： t_imu_last ≤ t_end 随后： kf_state.predict(t_end - t_imu_last, Q, in) 得到： 扫描结束时刻的预测状态。

这就是你前面问的“最后取出的 IMU 时间仍然比 end 小，后面怎么办”的代码实现位置。

3．三个核心坐标系与外参

整个IMU_Processing.hpp只要把三个坐标系理清，后面去畸变公式就不乱。

L：LiDAR 坐标系 I：IMU 坐标系 / 车体坐标系 G：世界坐标系 FAST-LIO 中通常叫 camera_init

代码中的外参变量：

M3D Lidar_R_wrt_IMU; V3D Lidar_T_wrt_IMU;

含义是：

^I R_L ：LiDAR 坐标系旋转到 IMU 坐标系的旋转外参。 ^I t_L ：LiDAR 原点在 IMU 坐标系中的位置。

对应关系：

LiDAR 点在采样时刻 ti： ^L_ti p_i LiDAR → IMU： ^I p_i = ^I R_L · ^L_ti p_i + ^I t_L

变量： ^L_ti p_i ：第 i 个原始 LiDAR 点； 坐标表达在该点真正采样时刻的 LiDAR 坐标系。 ^I R_L ：LiDAR 到 IMU 的旋转外参。 ^I t_L ：LiDAR 原点相对 IMU 原点的平移外参。 ^I p_i ：同一个点在 IMU 坐标系中的表示。

为什么平移外参也必须处理？因为 LiDAR 与 IMU 通常并不重合。比如 LiDAR 在 IMU 前方 20 cm，车辆原地旋转时，LiDAR 原点会绕 IMU 原点做圆周运动；若忽略这一段杆臂，近距离墙面和货架边缘会出现明显偏移。

4．FAST-LIO 在 IMU 中传播的状态

本文件使用：

esekfom::esekf<state_ikfom, 12, input_ikfom> kf_state;

这里的12不是状态维度，而是过程噪声维度。根据代码中状态字段与协方差索引，完整状态可理解为：

x = [ ^G p_I, ^G R_I, ^I R_L, ^I t_L, ^G v_I, b_g, b_a, ^G g ]

变量： ^G p_I ：IMU 在世界坐标系中的位置，3 维。 ^G R_I ：IMU 到世界坐标系的姿态，局部误差 3 维。 ^I R_L ：LiDAR 到 IMU 的旋转外参，3 维。 ^I t_L ：LiDAR 原点在 IMU 系中的位置，3 维。 ^G v_I ：IMU 在世界系中的速度，3 维。 b_g ：陀螺仪零偏，3 维。 b_a ：加速度计零偏，3 维。 ^G g ：世界系重力方向。 重力大小固定，所以本质只有 2 个自由度。

总自由度是：

3 + 3 + 3 + 3 + 3 + 3 + 3 + 2 = 23

过程噪声是 12 维：

w = [ n_g, n_a, n_bg, n_ba ]

变量： n_g ：陀螺仪白噪声，3 维。 n_a ：加速度计白噪声，3 维。 n_bg ：陀螺仪零偏随机游走，3 维。 n_ba ：加速度计零偏随机游走，3 维。

本文件中的Q就是这 12 维过程噪声协方差。

5．构造函数：默认配置了什么

构造函数如下：

ImuProcess::ImuProcess() : b_first_frame_(true), imu_need_init_(true), start_timestamp_(-1) { init_iter_num = 1; Q = process_noise_cov(); cov_acc = V3D(0.1, 0.1, 0.1); cov_gyr = V3D(0.1, 0.1, 0.1); cov_bias_gyr = V3D(0.0001, 0.0001, 0.0001); cov_bias_acc = V3D(0.0001, 0.0001, 0.0001); mean_acc = V3D(0, 0, -1.0); mean_gyr = V3D(0, 0, 0); angvel_last = Zero3d; Lidar_T_wrt_IMU = Zero3d; Lidar_R_wrt_IMU = Eye3d; last_imu_.reset(new sensor_msgs::Imu()); }

含义是：

imu_need_init_ = true ：系统刚启动时，先进行 IMU 初始化。 mean_acc = (0, 0, -1) ：加速度均值初值。 后续会被真实 IMU 数据覆盖。 mean_gyr = (0, 0, 0) ：陀螺仪均值初值。 Lidar_T_wrt_IMU = 0 Lidar_R_wrt_IMU = I ：默认假设 LiDAR 与 IMU 重合、轴方向一致。 实际运行前必须通过 set_extrinsic() 写入真实外参。

外参有三种设置方式：

set_extrinsic(transl, rot); set_extrinsic(transl); set_extrinsic(T_4x4);

第一种： 直接传平移 + 旋转。 第二种： 只传平移，默认旋转为单位阵。 第三种： 从 4×4 齐次变换矩阵中取旋转和平移。

这些设置不会立刻改变滤波器状态；真正写入state_ikfom的位置在初始化阶段：

init_state.offset_T_L_I = Lidar_T_wrt_IMU; init_state.offset_R_L_I = Lidar_R_wrt_IMU;

6．IMU_init()：启动时到底初始化什么

初始化函数的目标，源码注释已经写得很明确：

/** 1. initializing the gravity, gyro bias, ** acc and gyro covariance ** 2. normalize the acceleration measurements ** to unit gravity **/

它做四类事情：

1．统计平均加速度。 2．统计平均角速度。 3．根据平均加速度初始化重力方向。 4．根据平均角速度初始化陀螺仪零偏。

初始化阶段隐含前提是：

机器人启动后应静止，至少不能有明显持续加速、持续旋转或强烈震动。

否则平均加速度里会混进真实线加速度，平均角速度里会混进真实转动，导致重力与陀螺仪偏置初值不可靠。

6.1 第一帧初始化

第一次进入IMU_init()时：

if (b_first_frame_) { Reset(); N = 1; b_first_frame_ = false; const auto &imu_acc = meas.imu.front()->linear_acceleration; const auto &gyr_acc = meas.imu.front()->angular_velocity; mean_acc << imu_acc.x, imu_acc.y, imu_acc.z; mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z; first_lidar_time = meas.lidar_beg_time; }

这里的作用是：

第一条 IMU： 作为均值统计的起点。 first_lidar_time： 记录第一帧 LiDAR 时间。 主要用于调试或输出日志。

之后遍历当前meas.imu中所有 IMU：

for (const auto &imu : meas.imu) { ... }

6.2 平均加速度、平均角速度怎么更新

代码使用在线均值：

mean_acc += (cur_acc - mean_acc) / N; mean_gyr += (cur_gyr - mean_gyr) / N;

数学形式：

μ_N = μ_(N-1) + (x_N - μ_(N-1)) / N

变量： μ_(N-1) ：前 N-1 个 IMU 样本的均值。 x_N ：当前新增 IMU 样本。 μ_N ：更新后的均值。

对于本代码：

mean_acc ：启动阶段加速度计平均测量。 mean_gyr ：启动阶段陀螺仪平均测量。

这样不需要保存全部历史 IMU 数据，也可以不断更新均值。

6.3 初始化重力方向

初始化结束前，代码写入：

init_state.grav = S2(- mean_acc / mean_acc.norm() * G_m_s2);

可理解为：

^G g_init = - mean_acc / ||mean_acc|| · g

变量： mean_acc ：静止初始化期间的平均加速度计读数。 ||mean_acc|| ：其模长。 G_m_s2 ：标准重力大小，通常约 9.81 m/s²。 负号 ：因为静止时加速度计测到的是与重力方向相反的比力。

举例：假设 IMU 静止时测得：

mean_acc ≈ (0, 0, 9.81)

则代码会把重力设为：

grav ≈ (0, 0, -9.81)

这意味着世界坐标系中重力向下。

这里初始化的是重力方向，不是完整的绝对姿态。尤其 yaw 方向不能仅由加速度计确定；重力只能约束俯仰和横滚，无法决定朝向角。

6.4 初始化陀螺仪零偏

代码：

init_state.bg = mean_gyr;

即：

b_g_init = mean_gyr

理由是静止时：

ω_m = ω_true + b_g + n_g 静止时： ω_true ≈ 0 因此： mean_gyr ≈ b_g

如果初始化期间小车正在转弯或人工转动雷达，真实角速度会被误认为陀螺仪 bias。结果是后续姿态积分刚开始就会偏，LiDAR 匹配可能要花多帧才能把这个错误修回来。

6.5 初始化协方差P

代码把初始协方差设为单位阵，再对部分状态赋较小方差：

init_P.setIdentity(); init_P(6,6) = init_P(7,7) = init_P(8,8) = 0.00001; init_P(9,9) = init_P(10,10) = init_P(11,11) = 0.00001; init_P(15,15) = init_P(16,16) = init_P(17,17) = 0.0001; init_P(18,18) = init_P(19,19) = init_P(20,20) = 0.001; init_P(21,21) = init_P(22,22) = 0.00001;

按代码索引可理解为：

0 ~ 2 ：位置误差。 3 ~ 5 ：姿态误差。 6 ~ 8 ：旋转外参误差。 9 ~ 11 ：平移外参误差。 12 ~ 14 ：速度误差。 15 ~ 17 ：陀螺仪 bias 误差。 18 ~ 20 ：加速度计 bias 误差。 21 ~ 22 ：重力方向误差。

这表达的是：

位置、姿态、速度： 初始不确定性相对较大。 外参： 更相信配置值。 gyro bias： 静止均值估计后，相对有一定信心。 acc bias： 也有初始约束，但不如 gyro bias 紧。 重力方向： 经过均值加速度确定后，初始不确定性较小。

不过协方差不是“越小越好”。若初始P设置得过小，滤波器会过度相信错误初值，后续 LiDAR 很难把状态拉回来。

6.6 初始化结束条件

代码：

#define MAX_INI_COUNT (10) if (init_iter_num > MAX_INI_COUNT) { imu_need_init_ = false; }

这里的10指的是累计用于初始化统计的 IMU 样本数量，不是 10 帧 LiDAR。

例如：

IMU：200 Hz LiDAR：10 Hz 一帧 LiDAR 大约持续： 100 ms 该帧内 IMU 数量大约： 20 条

这种情况下，初始化可能只经过一帧或很少几帧就结束。

因此这套初始化更像“快速起步初始化”，不是严格长时间静止标定。实车启动时最好给系统留一段静止时间。

7．初始化结束时噪声的实际处理

初始化结束时，源码先执行：

cov_acc *= pow( G_m_s2 / mean_acc.norm(), 2 );

看起来是在根据平均重力模长调整加速度计噪声。

但紧接着又执行：

cov_acc = cov_acc_scale; cov_gyr = cov_gyr_scale;

所以最终运行时真正使用的cov_acc、cov_gyr主要来自外部配置调用：

set_acc_cov(...) set_gyr_cov(...)

因此要注意：

启动统计得到的 cov_acc、cov_gyr 并不是最终一定会被使用。 最终是否采用它们， 取决于后续是否被 cov_acc_scale、 cov_gyr_scale 覆盖。

当前构造函数中并没有显式给：

cov_acc_scale cov_gyr_scale

赋默认值。因此正常运行依赖外部初始化代码调用：

p_imu->set_acc_cov(...) p_imu->set_gyr_cov(...)

若外部没有正确配置，初始化结束时将未明确初始化的 scale 值写回cov_acc、cov_gyr会有风险。这是从当前文件可直接看到的配置依赖点。

8．UndistortPcl()：完整去畸变流程

初始化完成后，Process()调用：

UndistortPcl(meas, kf_state, *cur_pcl_un_);

它不是一上来就逐点变换，而是分成三大阶段：

阶段 A： 建立扫描期间的 IMU 连续轨迹。 阶段 B： 将状态传播到 LiDAR 精确结束时刻。 阶段 C： 对每一个 LiDAR 点， 根据自己的 curvature 找到采样时刻， 补偿到扫描结束时刻。

8.1 为什么把上一帧末尾 IMU 插到当前帧前面

代码：

auto v_imu = meas.imu; v_imu.push_front(last_imu_);

作用是补齐 LiDAR 扫描开始附近的 IMU 时间连续性。

时间轴如下：

上一帧最后 IMU 本帧 LiDAR 开始 本帧第一条 IMU t0 t_begin t1 ●----------------------|------------------------●

当前帧meas.imu可能从t1才开始；但去畸变和传播需要知道t_begin附近的运动状态。因此把上一帧保存的last_imu_放到当前帧队首，形成连续 IMU 区间。

上一帧末尾 IMU ↓ last_imu_ ↓ 插入当前帧 IMU 队首 ↓ 当前扫描开始附近不会缺 IMU 区间

这也是last_imu_每帧结束都要更新的原因。

8.2 当前 LiDAR 的起止时间

常规 LiDAR：

double pcl_beg_time = meas.lidar_beg_time; double pcl_end_time = meas.lidar_end_time;

即：

pcl_beg_time = t_begin pcl_end_time = t_end

MARSIM 有特殊逻辑：

if (lidar_type == MARSIM) { pcl_beg_time = last_lidar_end_time_; pcl_end_time = meas.lidar_beg_time; }

它不把当前点云当成真实扫描内逐点形成的点云，而是用相邻 LiDAR 消息之间的时间差进行状态传播。后面的逐点去畸变也会被跳过。

MARSIM： 传播状态： 上一帧 LiDAR 时间 ↓ 当前 LiDAR 消息时间 逐点补偿： 跳过

这隐含假设是：仿真每帧点云是瞬时快照，不存在真实机械 LiDAR 的扫描内畸变。

8.3 为什么要按curvature排序

代码：

pcl_out = *(meas.lidar); sort( pcl_out.points.begin(), pcl_out.points.end(), time_list );

比较器是：

const bool time_list( PointType &x, PointType &y) { return (x.curvature < y.curvature); };

curvature在这里不是几何曲率，而是当前点相对扫描开始的时间偏移，单位毫秒。

第 i 个点真实采样时刻： t_i = t_begin + curvature_i / 1000

排序后：

点云开始部分： curvature ≈ 0 ms 点云中间部分： curvature ≈ Tscan / 2 点云末尾部分： curvature ≈ Tscan

后面代码从点云尾部倒序处理，正好对应从扫描结束点回到扫描开始点。

9．IMU 前向传播：扫描期间轨迹如何生成

进入UndistortPcl()后，先保存当前状态：

state_ikfom imu_state = kf_state.get_x();

然后建立IMUpose：

IMUpose.clear(); IMUpose.push_back( set_pose6d( 0.0, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix() ) );

IMUpose可以理解为扫描期间的离散轨迹表：

IMUpose[k]： offset_time ：该轨迹节点相对扫描开始的时间。 rot ：该时刻 IMU 世界姿态。 pos ：该时刻 IMU 世界位置。 vel ：该时刻 IMU 世界速度。 acc ：世界坐标系下的线加速度。 gyr ：去掉 gyro bias 后的角速度。

这张表后面用于“某个 LiDAR 点在第几毫秒采到，就查询或插值得到该时刻 IMU 位姿”。

9.1 两条相邻 IMU 如何构成一个传播区间

代码遍历：

for ( auto it_imu = v_imu.begin(); it_imu < (v_imu.end() - 1); it_imu++ )

每次拿一对：

head：当前区间开始 IMU tail：当前区间结束 IMU

head tail t_k ----------------------------- t_k+1

对两端 IMU 取中值：

angvel_avr << 0.5 * ( head->angular_velocity.x + tail->angular_velocity.x ), ...; acc_avr << 0.5 * ( head->linear_acceleration.x + tail->linear_acceleration.x ), ...;

数学形式：

ω_avg = (ω_k + ω_k+1) / 2 a_avg = (a_k + a_k+1) / 2

变量： ω_k、ω_k+1 ：相邻两条 IMU 的角速度测量。 a_k、a_k+1 ：相邻两条 IMU 的加速度计测量。 ω_avg、a_avg ：当前短时间区间内使用的平均量。

这是中值积分近似。它假设一个极短 IMU 区间内，角速度和加速度不会剧烈变化。

9.2 为什么要做加速度模长缩放

代码：

acc_avr = acc_avr * G_m_s2 / mean_acc.norm();

其作用是：

将当前加速度计读数按初始化阶段的平均重力模长缩放， 尽量使静止时加速度模长接近标准重力大小。

公式可写为：

a_scaled = a_raw · g / ||mean_acc||

变量： a_raw ：当前 IMU 原始加速度读数。 mean_acc ：初始化阶段平均加速度。 g ：标准重力大小。 a_scaled ：经过尺度归一化后的加速度计读数。

这里不能把a_scaled直接理解为世界系车辆加速度。它仍然是 IMU 坐标系下的比力。后面状态传播时，才会通过姿态、bias 和重力变成世界系线加速度。

9.3 IMU 的连续状态方程

从滤波意义看，kf_state.predict(dt, Q, in)使用的核心模型可理解为：

p_dot = v R_dot = R · [ω_m - b_g - n_g]_x v_dot = R · (a_m - b_a - n_a) + g

变量： p ：IMU 世界位置。 v ：IMU 世界速度。 R ：IMU 到世界坐标系的旋转。 ω_m ：陀螺仪测得角速度。 a_m ：加速度计测得比力。 b_g ：gyro bias。 b_a ：acc bias。 g ：世界系重力。 n_g、n_a ：IMU 白噪声。

离散化后可近似理解为：

姿态传播： R_(k+1) = R_k · Exp( (ω_m - b_g) · dt )

速度传播： v_(k+1) = v_k + [ R_k · (a_m - b_a) + g ] · dt

位置传播： p_(k+1) = p_k + v_k · dt + 0.5 · [ R_k · (a_m - b_a) + g ] · dt²

真实代码由 IKFoM 内部状态模型执行；当前文件负责提供dt、IMU 输入in和过程噪声Q。

9.4 过程噪声Q怎么写入

每个 IMU 区间都写：

Q.block<3, 3>(0, 0).diagonal() = cov_gyr; Q.block<3, 3>(3, 3).diagonal() = cov_acc; Q.block<3, 3>(6, 6).diagonal() = cov_bias_gyr; Q.block<3, 3>(9, 9).diagonal() = cov_bias_acc;

即：

Q = diag( cov_gyr, cov_acc, cov_bias_gyr, cov_bias_acc )

变量： cov_gyr ：陀螺仪测量噪声方差。 cov_acc ：加速度计测量噪声方差。 cov_bias_gyr ：陀螺仪零偏随机游走方差。 cov_bias_acc ：加速度计零偏随机游走方差。

随后：

kf_state.predict(dt, Q, in);

不仅更新状态，也更新协方差：

P_(k+1) = F_k · P_k · F_k^T + G_k · Q_k · G_k^T

变量： P_k ：当前时刻状态协方差。 F_k ：状态转移 Jacobian。 G_k ：过程噪声 Jacobian。 Q_k ：IMU 过程噪声协方差。 P_(k+1) ：传播后的协方差。

直观上，连续只靠 IMU 传播时，位置、姿态、速度和 bias 的不确定性会不断增加；这就是之后 LiDAR 匹配必须介入修正的原因。

10．为什么要跳过已经处理过的旧 IMU

代码：

if ( tail->header.stamp.toSec() < last_lidar_end_time_ ) { continue; }

以及：

if ( head->header.stamp.toSec() < last_lidar_end_time_ ) { dt = tail->header.stamp.toSec() - last_lidar_end_time_; } else { dt = tail->header.stamp.toSec() - head->header.stamp.toSec(); }

原因是上一帧已经传播到：

last_lidar_end_time_

当前帧队首又插入了上一帧末尾 IMU，因此新旧帧之间可能有时间重叠。

上一帧已经积分过： ------|------------------ last_lidar_end_time 当前 v_imu： old imu --- head --- tail --- ...

若tail仍然在上一帧结束之前，说明这一整段已经被积分过，必须跳过。

若：

head < last_lidar_end_time < tail

则只积分尚未处理的后半段：

dt = t_tail - last_lidar_end_time

这避免重复积分，否则位置、速度和姿态会被重复推进。

11．每个 IMU 时刻为什么都保存到IMUpose

每次predict()后，代码执行：

imu_state = kf_state.get_x(); angvel_last = angvel_avr - imu_state.bg; acc_s_last = imu_state.rot * (acc_avr - imu_state.ba); for(int i = 0; i < 3; i++) { acc_s_last[i] += imu_state.grav[i]; } double offs_t = tail->header.stamp.toSec() - pcl_beg_time; IMUpose.push_back( set_pose6d( offs_t, acc_s_last, angvel_last, imu_state.vel, imu_state.pos, imu_state.rot.toRotationMatrix() ) );

这一步保存的是：

当前 IMU 时刻的： 姿态 位置 速度 世界系加速度 去偏角速度 相对扫描开始时间

后续 LiDAR 点很多，例如一帧 10 万点；系统不可能为每个点都从扫描开始重新积分所有 IMU。正确做法是：

先为每个 IMU 时刻保存一组离散状态 ↓ 点 i 根据自己的 curvature 找到它所在的 IMU 时间段 ↓ 在这一小段内插值 / 短时间传播

这就是IMUpose的作用。

12．传播到精确扫描结束时刻

当前meas.imu中最后一条 IMU 通常略早于pcl_end_time。

源码：

double note = pcl_end_time > imu_end_time ? 1.0 : -1.0; dt = note * (pcl_end_time - imu_end_time); kf_state.predict(dt, Q, in);

正常情况下：

imu_end_time ≤ pcl_end_time

所以：

dt = pcl_end_time - imu_end_time

随后用最后一个 IMU 区间的输入继续传播到精确扫描结束时刻。

最后一条取出的 IMU： t_imu_last = 10.095 s 扫描结束： t_end = 10.0987 s 补传播： dt = 0.0037 s

最终得到：

^G R_I(t_end) ^G p_I(t_end) ^G v_I(t_end) P^-

这就是后续 LiDAR IESKF 优化前的先验状态。

注意：代码中note会使dt始终非负。正常sync_packages()设计下，最后取出的 IMU 时间不应晚于扫描结束，因此它通常等价于正向补传播。若未来代码错误地把晚于t_end的 IMU 放进meas.imu，这里就不是严格的“回退传播”，而会得到不符合物理语义的正时间预测；因此sync_packages()的时间边界必须保持正确。

13．逐点去畸变：真正如何补偿每个 LiDAR 点

前面只是建立了扫描期间 IMU 轨迹，并把状态推进到扫描结束时刻。真正逐点去畸变从这里开始：

if(lidar_type != MARSIM) { auto it_pcl = pcl_out.points.end() - 1; for ( auto it_kp = IMUpose.end() - 1; it_kp != IMUpose.begin(); it_kp-- ) { ... } }

它从最后一个 LiDAR 点开始倒序处理，也从最后一个 IMU 轨迹节点开始倒序处理。

原因是：

点云已按 curvature 从小到大排序。 最后一个 LiDAR 点： 最接近扫描结束时刻。 最后一个 IMUpose： 最接近扫描结束时刻。 因此从后往前处理， 可以快速让每个点进入对应 IMU 时间区间。

13.1 一个 LiDAR 点如何找到对应 IMU 区间

对某个点：

t_point_offset = curvature / 1000

代码判断：

for( ; it_pcl->curvature / double(1000) > head->offset_time; it_pcl-- )

可以理解为：

当前点： head.offset_time < t_point_offset ≤ tail.offset_time

即该点位于当前 IMU 小区间：

IMU head 点 i IMU tail t_h --------------------- t_i ------------------- t_t

这时：

head ：作为当前点插值的起始状态。 tail ：提供该时间段内的加速度、角速度近似值。

13.2 点采样时刻姿态怎么得到

代码：

dt = it_pcl->curvature / double(1000) - head->offset_time; M3D R_i( R_imu * Exp(angvel_avr, dt) );

对应公式：

^G R_I(t_i) = ^G R_I(t_h) · Exp( ω · Δt_i )

变量： ^G R_I(t_h) ：当前 IMU 区间起点姿态。 ω ：代码保存的去偏角速度。 Δt_i ：点时刻相对区间起点的时间。 ^G R_I(t_i) ：第 i 个点采样时刻的 IMU 世界姿态。

这里使用的是短时间近似：一个 IMU 小区间内认为角速度近似恒定。

13.3 点采样时刻位置怎么得到

代码：

V3D T_ei( pos_imu + vel_imu * dt + 0.5 * acc_imu * dt * dt - imu_state.pos );

前半部分是点时刻的位置估计：

^G p_I(t_i) ≈ ^G p_I(t_h) + ^G v_I(t_h) · Δt_i + 0.5 · ^G a_I · Δt_i²

再减去扫描结束位置：

T_ei = ^G p_I(t_i) - ^G p_I(t_e)

变量： ^G p_I(t_i) ：点采样时刻 IMU 世界位置。 ^G p_I(t_e) ：扫描结束时刻 IMU 世界位置。 T_ei ：点时刻 IMU 原点相对结束时刻 IMU 原点的位移； 表达在世界坐标系中。

这一步补偿的是扫描期间的平移。

如果只补姿态、不补位置，车辆前进时墙面仍会拉宽、近处货架仍会重影。

13.4 完整去畸变变换

核心代码：

V3D P_compensate = imu_state.offset_R_L_I.conjugate() * ( imu_state.rot.conjugate() * ( R_i * ( imu_state.offset_R_L_I * P_i + imu_state.offset_T_L_I ) + T_ei ) - imu_state.offset_T_L_I );

它实际完成的是下面四次坐标变换。

第 1 步：LiDAR_ti → IMU_ti ^I p_i = ^I R_L · ^L_ti p_i + ^I t_L

第 2 步：IMU_ti → 世界系 ^G p_i = ^G R_I(t_i) · ^I p_i + ^G p_I(t_i)

第 3 步：世界系 → IMU_te ^I_te p_i = (^G R_I(t_e))^T · ( ^G p_i - ^G p_I(t_e) )

第 4 步：IMU_te → LiDAR_te ^L_te p_i = (^I R_L)^T · ( ^I_te p_i - ^I t_L )

合并后：

^L_te p_i = (^I R_L)^T · [ (^G R_I(t_e))^T · ( ^G R_I(t_i) · ( ^I R_L · ^L_ti p_i + ^I t_L ) + ^G p_I(t_i) - ^G p_I(t_e) ) - ^I t_L ]

变量： ^L_ti p_i ：第 i 个点原始坐标； 位于其实际采样时刻 ti 的 LiDAR 系。 ^L_te p_i ：去畸变后坐标； 统一到扫描结束时刻 te 的 LiDAR 系。 ^I R_L、^I t_L ：LiDAR-IMU 外参。 ^G R_I(t_i)、^G p_I(t_i) ：点采样时刻 IMU 位姿。 ^G R_I(t_e)、^G p_I(t_e) ：扫描结束时刻 IMU 位姿。

最终：

it_pcl->x = P_compensate(0); it_pcl->y = P_compensate(1); it_pcl->z = P_compensate(2);

这一帧所有点都会统一表达在扫描结束 LiDAR 坐标系。

14．源码注释“只用旋转”为什么不准确

代码旁边有注释：

/* Transform to the 'end' frame, using only the rotation */

但从实际计算看，代码并不是只用旋转。

它显式使用：

R_i ：点采样时刻姿态。 imu_state.rot.conjugate() ：扫描结束时刻逆姿态。 T_ei ：扫描期间平移补偿。 offset_T_L_I ：LiDAR-IMU 杆臂平移外参。

所以真实补偿包含：

扫描期间旋转补偿 + 扫描期间平移补偿 + LiDAR-IMU 杆臂补偿

源码末尾还写了：

// not accurate!

这是作者对近似模型的提醒。近似来源包括：

1．IMU 小区间内角速度近似恒定。 2．IMU 小区间内世界系加速度近似恒定。 3．点时刻位置使用二阶运动学公式。 4．每个点不重新跑完整 IESKF 传播， 而是在附近 IMU 轨迹节点间做短时间估计。

在低速、IMU 高频、时间同步准确时，这种近似通常足够；急转、急加速、剧烈震动、外参误差或时间偏差较大时，去畸变误差会明显变大。

15．没有逐点时间、没有 IMU、MARSIM 三种情况

15.1 有 IMU，但点云没有逐点时间

本文件不会帮你补时间。它只读取：

it_pcl->curvature / double(1000)

所以进入这里之前，preprocess.cpp必须已经写好：

curvature_i = 第 i 个点相对扫描开始的时间，单位 ms

机械 LiDAR 若没有原始point.time，可在前处理阶段用：

每条 ring 的首点 yaw + 当前点 yaw + SCAN_RATE

估计时间。

omega_l = 0.361 × SCAN_RATE curvature_i = (yaw_first - yaw_i) / omega_l

15.2 没有 IMU

代码：

if(meas.imu.empty()) {return;};

因此：

没有 IMU ↓ 本帧不初始化 不预测状态 不建立 IMUpose 不逐点去畸变

它不会自动切换为 ICP、NDT、纯 LiDAR odometry 或匀速去畸变。

15.3 MARSIM

MARSIM 分支：

仍进行 IMU 状态传播。 但不进行逐点 LiDAR 坐标补偿。

前提是仿真点云为瞬时快照。如果仿真点云实际上模拟了扫描过程，而curvature又全为 0，车辆运动时仍会出现扫描内畸变。

16．当前代码中值得特别注意的实现细节

下面是从你贴出的这份文件中能直接看出的注意点。

1．last_lidar_end_time_ 在可见构造函数与 Reset() 中没有显式赋值。 它后面参与： tail_time < last_lidar_end_time_ 以及： pcl_beg_time = last_lidar_end_time_ 若对象不是静态零初始化、或外部未先写入该值， 首帧正常传播前可能存在未定义初值风险。

2．acc_s_last 在可见构造函数和 Reset() 中也没有明确置零。 但首次正常 UndistortPcl() 会把它放入： IMUpose.push_back( set_pose6d(0.0, acc_s_last, ...) ) 若没有其他位置提前设置它， 第一帧的初始轨迹节点可能带入未初始化数据。

3．cov_acc_scale、cov_gyr_scale 没有在构造函数内显示初始化。 初始化完成后： cov_acc = cov_acc_scale; cov_gyr = cov_gyr_scale; 因此外层必须调用： set_acc_cov() set_gyr_cov() 否则运行噪声参数可能不可靠。

4．点云中 curvature = 0 的点不会进入最后一段补偿循环。 循环条件是： curvature / 1000 > head->offset_time 第一段 head->offset_time 通常为 0。 因此 curvature 恰好为 0 的点不会被变换， 仍保留扫描开始时刻 LiDAR 坐标。 若这种点数量很多，且机器人扫描期间运动明显， 这是需要关注的边界行为。

5．Reset(double start_timestamp, lastimu) 在类中声明， 但你贴出的内容中没有看到对应实现。 若工程其他文件也没有实现、且外部调用它， 会产生链接错误。 是否实际有问题， 还需要查完整工程调用关系。

6．MARSIM 的时间语义与普通 LiDAR 不同。 普通 LiDAR： 扫描时间由 t_begin 到 t_end。 MARSIM： 传播时间改为上一帧结束到当前帧开始， 并且不做逐点补偿。 不能把普通雷达的时间逻辑直接套到 MARSIM。

总结

src/IMU_Processing.hpp的本质不是“读 IMU 然后算一个位姿”，而是建立一套完整的扫描内连续运动模型。LiDAR 一帧点云不是同一时刻形成的：10 Hz 雷达的一帧通常跨越约 100 ms，扫描开始的点和扫描结束的点之间，机器人可能已经旋转、平移、加速，LiDAR 原点本身的位置和朝向都变了。若把这些点直接视为同一时刻点云送去匹配，静止时可能看不出问题，但车辆一转弯、加速、经过颠簸区域，墙面会变厚、柱子会出现双边、直线会弯曲，后面的 ikd-Tree 搜索和 IESKF 也只能建立在已经畸变的点云上。

这份文件先在启动阶段做快速 IMU 初始化。它用静止期的平均加速度估计重力方向，用平均角速度估计陀螺仪零偏，并初始化外参、噪声与状态协方差。初始化过程隐含“设备应相对静止”的前提，因为一旦小车刚启动就加速或旋转，真实运动会混入平均值，导致系统把真实角速度误当成 bias，把真实线加速度混入重力估计。代码中的MAX_INI_COUNT=10统计的是 IMU 样本数量，不是 10 帧 LiDAR；对于高频 IMU，这一过程实际可能非常快，所以工程上最好让雷达和 IMU 上电后静止一小段时间。

初始化完成后，UndistortPcl()才进入正常工作。它先把上一帧最后一条 IMU 放进当前帧 IMU 队列前端，避免 LiDAR 扫描开始附近没有可用 IMU 区间；接着按curvature对当前点云排序。这里的curvature不是传统意义上的点云几何曲率，而是 preprocess.cpp 写入的“点相对扫描开始时刻的时间偏移”，单位通常是毫秒。每个点的真实采样时间由t_begin + curvature / 1000得到。没有这个逐点时间，IMU 虽然知道小车怎么运动，却不知道每一个 LiDAR 点具体是在扫描的第几毫秒采到，因此也无法做严格逐点去畸变。

随后系统将扫描期间相邻 IMU 组成小时间段，对每一段取角速度和加速度中值，调用kf_state.predict()传播位置、姿态、速度、bias、重力和协方差。每传播到一个 IMU 时刻，代码都会把该时刻的姿态、位置、速度、世界加速度和去偏角速度保存到IMUpose。这相当于建立了一条离散的扫描内 IMU 轨迹。当前帧最后一条被取出的 IMU 通常早于 LiDAR 精确结束时间，因此代码会继续用最后一段 IMU 输入短时间预测到pcl_end_time，得到 LiDAR 匹配前的先验状态x^-和预测协方差P^-。

真正逐点去畸变时，代码从扫描末尾点开始倒序遍历。对于每个点，它先根据curvature找到对应的 IMU 区间；然后使用该区间起始姿态、速度、位置，加上短时间的恒角速度、恒加速度近似，估计点采样时刻的 IMU 姿态和位置。接下来，点先从采样时刻 LiDAR 坐标系通过外参变到采样时刻 IMU 坐标系，再变到世界坐标系，再转换到扫描结束时刻 IMU 坐标系，最后通过外参回到扫描结束时刻 LiDAR 坐标系。这样整帧点云最终都变成“扫描结束瞬间 LiDAR 所看到的环境”，后面的laserMapping.cpp就能把它当作近似同步点云来做体素降采样、ikd-Tree 最近邻搜索、局部平面拟合和 IESKF 优化。

从系统职责上看，IMU 在 FAST-LIO 中提供的是高频连续运动信息，但它会随时间漂移；LiDAR 提供的是低频但几何约束强的环境信息，用于反向修正 IMU 漂移。IMU_Processing.hpp的输出正是二者之间的桥梁：它输出一帧时空统一的去畸变点云，以及扫描结束时刻的 IMU 预测状态。后续 LiDAR 匹配不是替代 IMU，而是在 IMU 提供的先验上进行几何修正。只要逐点时间、LiDAR-IMU 时间同步、外参和噪声模型其中一项严重错误，去畸变就会先出问题，进而影响后面的匹配、优化和地图质量。