sync_packages()是 FAST-LIO 每帧处理真正开始前的“拼包函数”。它不做 IMU 积分、不做点云去畸变、不做 ikd-Tree 匹配也不做 IESKF 优化它只负责把一帧 LiDAR 点云 该帧扫描结束前已经到达的 IMU 数据 当前帧的 LiDAR 起止时间打包进MeasureGroup Measures然后交给p_imu-Process(Measures, kf, feats_undistort);后面的ImuProcess才会利用这个包完成 IMU 前向传播和逐点去畸变。sync_packages()的本质是保证当前 LiDAR 帧只有在“时间边界确定、IMU 已覆盖扫描结束时刻”后才允许进入后端。1它位于整条 FAST-LIO 链路的哪里LiDAR 回调 ↓ p_pre-process() ↓ lidar_buffer time_buffer IMU 回调 ↓ imu_buffer last_timestamp_imu ↓ sync_packages(Measures) ↓ 得到 Measures.lidar Measures.lidar_beg_time Measures.lidar_end_time Measures.imu ↓ p_imu-Process() ↓ IMU 前向传播 点云去畸变 ↓ feats_undistort ↓ 体素降采样 ↓ ikd-Tree 点到平面约束 ↓ IESKF ↓ map_incremental()因此sync_packages()的输出不是位姿也不是去畸变点云而是一个“当前时段传感器数据包”。后续模块默认认为这个包的时间关系可信。2先明确它同步的到底是什么很多人会把sync_packages()理解成“给 LiDAR 和 IMU 做精确时间同步”。这不够准确。它实际完成的是【数据包同步 / 数据就绪判断】 1选定当前最早未处理的一帧 LiDAR。 2确定该帧的扫描起止时间。 3等待 IMU 时间戳已经到达扫描结束时刻之后。 4从 IMU 队列取出本帧需要的 IMU 数据。 5返回 true让后续模块处理这一帧。它不直接完成下面三件事1不做 LiDAR-IMU 外参标定。 2不逐点匹配 LiDAR 时间和 IMU 时间。 这件事在 UndistortPcl() 中完成。 3不真正消除硬件时钟偏差。 它只能使用当前消息时间戳进行拼包。所以更准确地说sync_packages()是“帧级数据拼包器 时间覆盖门控器”不是高精度时钟同步算法。LiDAR 与 IMU 的硬件时间偏差、ROS 时间戳偏差、驱动时间戳语义错误都会直接影响它后续拼出的包。3进入sync_packages()前缓存里有什么函数依赖四个最重要的全局缓存和状态变量。【LiDAR 缓存】 lidar_buffer 保存已经经过 preprocess.cpp 处理的 LiDAR 点云。 time_buffer 保存每帧 LiDAR 消息 header 时间戳。 【IMU 缓存】 imu_buffer 按时间顺序保存 IMU 消息。 last_timestamp_imu 当前已接收到的最新 IMU 时间戳。 【拼包状态】 lidar_pushed 当前 LiDAR 帧是否已经锁定到 Measures 中。 lidar_end_time 当前锁定 LiDAR 帧的扫描结束时刻。 lidar_mean_scantime 历史有效扫描周期均值。 scan_num 已用于统计扫描周期的有效帧数。标准 LiDAR 回调中点云会先经过p_pre-process(msg, ptr);然后进入lidar_buffer.push_back(ptr); time_buffer.push_back(msg-header.stamp.toSec());IMU 回调则把经过时间偏移修正后的消息压入imu_buffer并更新last_timestamp_imu。这里有一个非常重要的隐含假设time_buffer 中 LiDAR header.stamp 被当作“当前帧扫描开始时刻”。因为后面直接写meas.lidar_beg_time time_buffer.front();如果某个驱动发布的 header 时间实际表示“扫描结束时刻”或“包中间时刻”FAST-LIO 仍会把它误当扫描起点导致整帧时间整体偏移。这个问题会让去畸变看起来像外参错误或 IMU 漂移但根因其实是 LiDAR 时间戳语义不一致。该实现确实直接将消息时间戳写入time_buffer并作为lidar_beg_time使用。4函数入口为什么 LiDAR 或 IMU 缓存为空就直接返回函数开头逻辑是if (lidar_buffer.empty() || imu_buffer.empty()) { return false; }含义很直接没有 LiDAR 无法形成一帧点云。 没有 IMU 无法估计扫描期间运动。 因此 当前帧不能进入 ImuProcess。这里返回false并不表示“定位失败”只表示“数据还没到齐继续等”。主循环会反复调用sync_packages()ros::spinOnce(); if (sync_packages(Measures)) { p_imu-Process(Measures, kf, feats_undistort); ... }只有返回true后续的 IMU 去畸变、点云匹配和 IESKF 才会开始。5第一步锁定当前最早的一帧 LiDAR真正的 LiDAR 拼包逻辑由lidar_pushed控制。if (!lidar_pushed) { meas.lidar lidar_buffer.front(); meas.lidar_beg_time time_buffer.front(); ... lidar_pushed true; }这一段的关键不是“取第一帧点云”这么简单而是当前 LiDAR 帧一旦被选中就会被锁定直到 IMU 覆盖其扫描结束时刻。例如当前缓存 lidar_buffer [L0, L1, L2] imu_buffer [U0, U1, U2, U3] 当前要处理 L0第一次进入时Measures.lidar L0 Measures.lidar_beg_time time(L0) lidar_pushed true但如果此刻 IMU 还没到 L0 的扫描结束时刻函数会返回false。下一次进入时由于lidar_pushed true它不会重新拿 L1也不会重新计算 L0 的开始时间更不会把 L0 从队列弹出。它会继续等 L0 对应的 IMU 到齐。第一次调用 L0 已锁定 IMU 不足 ↓ return false 第二次调用 仍然是 L0 继续等待 IMU ↓ return false 第三次调用 IMU 已覆盖 L0 结束时刻 ↓ 拼包成功这保证 LiDAR 和 IMU 不会错帧配对。6第二步当前 LiDAR 帧的结束时间怎么计算这部分是sync_packages()最关键的时间逻辑。一帧 LiDAR 不是瞬间得到的。假设 LiDAR 为 10 Hz一帧扫描大约持续 100 ms。系统必须知道扫描结束时刻才能知道要等待 IMU 到什么时间。代码通常使用当前点云最后一个点的PointType::curvature来估计当前帧扫描结束时间。【正常扫描结束时间】 t_end t_begin curvature_last / 1000 变量 t_begin 当前 LiDAR 帧开始时间。 对应 meas.lidar_beg_time。 curvature_last 当前点云最后一个有效点的相对时间偏移。 /1000 因为 curvature 在 FAST-LIO 中按毫秒 ms 保存 除以 1000 后变成秒 s。 t_end 当前 LiDAR 扫描结束时刻。例如【示例10 Hz LiDAR】 t_begin 100.0000 s curvature_last 98.7 ms t_end 100.0000 98.7 / 1000 100.0987 s这说明当前帧从开始到结束持续约 98.7 ms。这里的curvature不是普通意义的几何曲率。它是在preprocess.cpp中被复用成“逐点相对扫描时间”。Livox 通常来自offset_timeOuster 通常来自tVelodyne 没有逐点时间时可以通过 yaw 和扫描频率估计。6.1 为什么不用下一帧 LiDAR 的时间戳当结束时间理论上可以用下一帧起点 - 当前帧起点估计扫描周期。但这样有两个问题问题 1 必须等待下一帧 LiDAR 到来 当前帧会额外增加一个扫描周期延迟。 问题 2 下一帧消息时间可能抖动、丢帧、乱序 不能保证准确代表当前帧扫描结束。FAST-LIO 直接利用当前点云内部最后一点的逐点时间因此可以在下一帧 LiDAR 到来前确定当前帧的结束时刻。这个设计可以降低帧级等待延迟。6.2 为什么有“扫描周期均值兜底”代码有两类异常情况不会直接相信最后点的curvature。情况 1 当前点云点数 1。 情况 2 最后点相对时间明显太小 curvature_last / 1000 0.5 × lidar_mean_scantime此时使用【异常时的扫描结束时间】 t_end t_begin T_scan_mean 变量 T_scan_mean 历史有效扫描周期均值。 对应变量 lidar_mean_scantime。这属于经验保护。比如系统历史上判断当前 LiDAR 一帧通常为 100 ms但当前最后点时间却只有 2 ms。若直接相信t_end t_begin 0.002 s系统会错误认为点云是瞬时完成的只取到极少 IMU 数据后续去畸变会严重错误。所以代码判断若最后点时间小于历史平均扫描周期的一半 更可能是逐点时间缺失、点云截断、点顺序异常或时间字段错误 于是改用历史平均周期。正常帧会更新历史均值【在线更新扫描周期均值】 T_mean(n) T_mean(n-1) [ T_scan(n) - T_mean(n-1) ] / n 变量 T_mean(n-1) 前 n-1 帧的平均扫描周期。 T_scan(n) 当前有效 LiDAR 帧扫描周期。 n scan_num。这不是滑动窗口均值而是从启动以来的累计均值。扫描频率稳定时它很平滑若 LiDAR 频率中途动态改变它会有明显滞后。6.3points.back().curvature有一个隐含前提源码直接使用meas.lidar-points.back().curvature因此它隐含要求当前 LiDAR 点云中最后一个点 必须接近扫描时间上最晚的点。也就是说点云顺序应基本按扫描时间递增。如果驱动或预处理过程打乱点云顺序例如真实时间顺序 0 ms → 20 ms → 40 ms → 60 ms → 80 ms → 100 ms 实际容器顺序 0 ms → 100 ms → 20 ms → 80 ms → 40 ms那么points.back()可能对应 40 ms而不是 100 ms。sync_packages()会错误认为扫描已经在 40 ms 时结束导致后半段真实运动没有被正确覆盖。后续UndistortPcl()会对点云按curvature排序但那已经发生在sync_packages()之后。因此在当前实现里扫描结束时间依赖 preprocess 输出点的原始排列顺序。这是源码行为直接推导出的重要前提。7MARSIM 为什么把扫描结束时间设成开始时间代码对 MARSIM 有特殊分支if (lidar_type MARSIM) { lidar_end_time meas.lidar_beg_time; }也就是t_end t_begin含义是当前 MARSIM 点云被假设为“瞬时快照”。 整帧点云不被当作 从 t_begin 扫到 t_end 的连续扫描。因此sync_packages()不等待一个完整扫描周期的 IMU 覆盖而是按当前点云时间戳处理。但这只是sync_packages()层面的定义。你前面看的IMU_Processing.cpp中MARSIM 后续会使用上一帧 LiDAR 时间 ↓ 当前 LiDAR 时间作为 IMU 传播的时间区间同时跳过普通逐点 LiDAR 补偿。也就是说MARSIM 的整体设计假设是“每帧点云本来就是瞬时产生”。8第三步为什么必须等 IMU 覆盖 LiDAR 扫描结束时刻LiDAR 帧结束时间确定后代码检查if (last_timestamp_imu lidar_end_time) { return false; }对应逻辑是【当前帧允许处理的条件】 t_imu_latest t_lidar_end 变量 t_imu_latest 当前已接收到的最后一条 IMU 时间戳。 t_lidar_end 当前 LiDAR 帧扫描结束时刻。原因是 FAST-LIO 的去畸变目标是把所有点统一补偿到扫描结束时刻 LiDAR 坐标系。要做到这一点至少要知道扫描结束附近 IMU 的姿态、速度和位置。例如当前 LiDAR 扫描开始100.000 s 扫描结束100.098 s 当前最新 IMU 100.080 s此时 100.080 s 到 100.098 s 这段运动还未知。若系统立刻开始处理扫描后半段点 ↓ 无法获得可信的结束时刻姿态 ↓ 去畸变参考坐标错误 ↓ 点云后半段容易拉伸、扭曲、重影所以必须先返回false保留当前 LiDAR 帧等新的 IMU 到达。注意这里等待的是时间戳覆盖不是“IMU 数据数量足够多”。即使当前已经有 100 条 IMU但最后一条仍然停在t_end前面也不能处理反过来即使 IMU 数量不多只要结束时刻被覆盖代码就允许继续。9第四步从 IMU 缓存中取出当前帧需要的 IMU当last_timestamp_imu lidar_end_time满足后函数才真正开始填充meas.imu逻辑可以概括为从 imu_buffer 队首开始 若 imu_time lidar_end_time 加入 meas.imu 从 imu_buffer 删除 若 imu_time lidar_end_time 停止 保留在 imu_buffer 中形式化写成【当前帧取出的 IMU 集合】 Measures.imu { IMU_j | t_j t_lidar_end } 同时 第一个 t_j t_lidar_end 的 IMU 继续留在 imu_buffer 中。这有两个关键作用。第一当前帧只消费自己时间边界之前的 IMU。当前帧 [ t_begin ------------------ t_end ] 取出 所有 timestamp t_end 的 IMU。第二不消费下一帧可能需要的未来 IMU。下一条 IMU t_imu t_end 不弹出 ↓ 保留在 imu_buffer ↓ 下一帧继续使用这避免一条 IMU 被“过早拿走”。源码中虽然先通过last_timestamp_imu lidar_end_time确认缓存里已经有足够新的 IMU但真正塞进Measures.imu的是时间不晚于扫描结束的 IMU第一条晚于结束时刻的 IMU 会留在队列中。9.1 为什么等到了结束后的 IMU却不把它也塞进当前包例如LiDAR t_begin 100.000 s t_end 100.0987 s IMU 缓存 100.000 100.005 100.010 ... 100.095 100.100系统先确认latest IMU 100.100 100.0987因此说明 IMU 已经覆盖当前 LiDAR 扫描结束时刻。但真正装入Measures.imu的通常是100.000 100.005 ... 100.095而100.100会留给下一帧。原因是当前ImuProcess::UndistortPcl()会先利用扫描内已有 IMU 轨迹推进到最后一个 IMU 时刻再将状态短时间传播到精确的 LiDAR 扫描结束时刻。它不要求把第一条晚于t_end的 IMU 消费掉。你前面看的UndistortPcl()正是这样在最后补传播到pcl_end_time。因此等待 IMU 覆盖结束时刻 ≠ 把结束后的未来 IMU 塞给当前帧前者保证当前帧时间边界完整后者则会破坏下一帧的数据连续性。10第五步拼包成功后缓存如何出队当 LiDAR、起止时间和 IMU 都填好后函数执行lidar_buffer.pop_front() time_buffer.pop_front() lidar_pushed false return true这表示当前 L0 帧已经完整交给 Measures。 L0 从 LiDAR 缓存移除。 L0 对应起始时间从 time_buffer 移除。 允许下一次调用锁定 L1。 当前拼包结束。虽然lidar_buffer.pop_front()把缓存中的指针删掉了但Measures.lidar本身仍然持有该点云的智能指针所以后续p_imu-Process(Measures, ...);仍然可以正常访问当前帧点云。这一点是shared_ptr语义带来的从缓存队列移除的是一份引用不是立刻释放点云对象。11把整个函数理解成状态机sync_packages()最好不要理解成一个普通if函数而应理解成四状态状态机。状态 S0没有可处理 LiDAR 或 IMU 条件 lidar_buffer.empty() 或 imu_buffer.empty() 行为 return false状态 S1锁定 LiDAR计算扫描结束时间 行为 Measures.lidar lidar_buffer.front() Measures.lidar_beg_time time_buffer.front() 根据 curvature 计算 lidar_end_time lidar_pushed true状态 S2LiDAR 已锁定但 IMU 尚未覆盖结束时间 条件 last_timestamp_imu lidar_end_time 行为 不弹出 LiDAR 不修改 lidar_pushed return false状态 S3IMU 已覆盖扫描结束时间 行为 取出所有 t_imu lidar_end_time 的 IMU 弹出当前 LiDAR 和时间戳 lidar_pushed false return true这就是为什么lidar_pushed是整段代码的核心状态变量。它避免在等待 IMU 时重复换帧、重复估计结束时间或者把下一帧 LiDAR 错配给当前 IMU 数据。12一个完整时间轴例子假设 LiDAR 为 10 HzIMU 为 200 Hz。LiDAR 当前帧 t_begin 10.0000 s points.back().curvature 97.5 ms t_end 10.0975 sIMU 队列此时为9.995 10.000 10.005 10.010 ... 10.090 10.095 10.100第一次调用sync_packages()last_timestamp_imu 10.095 10.095 10.0975 结论 IMU 未覆盖扫描结束时刻。 行为 L0 被锁定。 lidar_pushed true。 不弹出 L0。 return false。新的 IMU 到来10.100第二次调用last_timestamp_imu 10.100 10.100 10.0975 结论 IMU 已覆盖扫描结束时刻。函数取出9.995 10.000 10.005 ... 10.095但保留10.100然后Measures.lidar L0 Measures.lidar_beg_time 10.0000 Measures.lidar_end_time 10.0975 Measures.imu [ 9.995, 10.000, ... 10.095 ]接着返回true后续ImuProcess会利用这段 IMU 数据构建扫描期间轨迹并从最后一个 IMU 时刻短时间预测到10.0975 s。13sync_packages()与真正的时间同步不是一回事当前实现中IMU 回调会先做timestamp_imu_corrected timestamp_imu_raw - time_diff_lidar_to_imu若启用time_sync_enLivox 分支还会在检测到 LiDAR 与 IMU 时间基差距较大时进行一次启发式偏移修正。但这不代表它能解决所有同步问题。【sync_packages 能解决】 LiDAR 比 IMU 先到 等待。 IMU 比 LiDAR 先到 缓存。 当前 LiDAR 的 IMU 尚未覆盖结束时刻 不处理继续等待。 【sync_packages 不能自动解决】 LiDAR 与 IMU 固定偏差 5 ms。 LiDAR header 实际是结束时间 代码却将它当开始时间。 LiDAR 点时间单位错了 1000 倍。 IMU 消息存在系统性延迟但 header 未修正。 LiDAR 与 IMU 使用不同硬件时钟且持续漂移。所以sync_packages()只能保证“代码看来时间戳已经对齐”不能保证“真实物理采样时刻已经对齐”。如果硬件真实偏差为 10 ms而 header 时间戳看似正确函数照样能返回true但去畸变仍可能产生墙面重影、转弯变形或地图抖动。14当前源码中需要重点注意的几个问题14.1curvature单位必须是毫秒代码使用curvature / 1000因此它假设curvature 单位 ms若你误传curvature 秒系统会多除一次 1000认为扫描周期极短。若误传curvature 微秒系统会把扫描周期放大 1000 倍可能一直等不到足够新的 IMU。【正确】 curvature 100 ms curvature / 1000 0.1 s 【错误微秒被误当毫秒】 curvature 100000 us curvature / 1000 100 s结果是系统可能等待 100 秒后的 IMU表现为sync_packages()长时间一直返回false。14.2 点云最后一个点必须对应接近扫描末尾由于源码用points.back().curvature当前点云应满足points[0].curvature 接近 0 ms ... points.back().curvature 接近一帧扫描周期例如 10 Hz LiDAR预期 最后点 curvature ≈ 100 ms若日志显示最后点 curvature 2 ms但点云明明是完整 10 Hz 一帧通常要检查逐点时间是否被正确写入。 点云是否被重排序。 最后点是否真的是时间最晚点。 LiDAR 驱动是否丢失逐点时间。 time_unit 是否配置正确。14.3 第一个异常帧的兜底时间可能不可靠初始化时lidar_mean_scantime 0 scan_num 0如果第一帧就满足点数太少 或 最后点时间异常那么兜底逻辑得到的可能是t_end t_begin 0也就是把当前帧当作瞬时完成。后续一旦出现正常帧lidar_mean_scantime才会逐渐建立起来。因此启动阶段最好保证LiDAR 点云正常。 逐点时间正常。 IMU 已先稳定发布一小段时间。这是由当前初始化值和兜底逻辑可直接推导出的边界情况。14.4 LiDAR 时间回退时time_buffer也要注意一致性LiDAR 回调检测到时间倒退后会清空lidar_buffer但在上游这段回调代码中清空逻辑并没有同时显示清空time_buffer而这两个队列必须始终保持一一对应lidar_buffer[i] ↔ time_buffer[i]如果真发生 LiDAR 时间回退、ROS 仿真时间重置或 bag 重放跳时必须特别检查time_buffer是否也同步重置否则新的 LiDAR 点云可能配上旧的时间戳后续meas.lidar_beg_time会错位。源码中 LiDAR 回调确实在回退时清空lidar_buffer而时间队列的同步清理需要结合你实际版本进一步核查。15排查sync_packages()时最值得打印什么排查时不要只打印“同步成功/失败”最有价值的是下面这组时间。【LiDAR】 lidar_beg_time last_point_curvature_ms lidar_end_time lidar_end_time - lidar_beg_time 【IMU】 imu_buffer.front()-stamp imu_buffer.back()-stamp last_timestamp_imu Measures.imu.size() 【状态】 lidar_pushed lidar_buffer.size() time_buffer.size() imu_buffer.size()正常 10 Hz LiDAR 的典型预期lidar_end_time - lidar_beg_time ≈ 0.1 s last_point_curvature_ms ≈ 100 ms last_timestamp_imu lidar_end_time Measures.imu.size() ≈ IMU频率 / LiDAR频率 例如 200 Hz IMU 10 Hz LiDAR 每帧大约 20 条 IMU若出现lidar_end_time - lidar_beg_time ≈ 0 可能 curvature 全为 0、 首帧异常、 MARSIM、 点时间未写入。 lidar_end_time - lidar_beg_time ≈ 100 s 可能 微秒 / 纳秒误当毫秒。 last_timestamp_imu 一直小于 lidar_end_time 可能 LiDAR 和 IMU 时间基不一致、 IMU 发布时间慢、 time_diff_lidar_to_imu 配置错误、 LiDAR header 时间不是开始时刻。 Measures.imu.size() 0 可能 IMU 队列时间排序异常、 LiDAR 时间异常、 扫描结束时间计算异常。总结sync_packages()是 FAST-LIO 的“帧级时间边界控制器”。它先从lidar_buffer锁定一帧尚未处理的 LiDAR 点云将消息 header 时间当作扫描开始时间然后从点云最后一个点的curvature推出扫描结束时间。curvature在 FAST-LIO 中不是几何曲率而是逐点相对扫描时间单位必须是毫秒。若最后点时间异常或点云过少函数使用历史平均扫描周期兜底。当前 LiDAR 帧被锁定后sync_packages()不会立即处理而是检查最新 IMU 时间戳是否已经到达扫描结束时刻。若未覆盖函数返回false但 LiDAR 帧保留在队列中lidar_pushed维持为真下一次调用继续等待同一帧而不会错误跳到下一帧。只有当 IMU 已覆盖扫描结束时间函数才将不晚于该结束时刻的 IMU 放入Measures.imu保留第一条晚于结束时刻的 IMU 给下一帧随后弹出当前 LiDAR 和对应时间戳返回true。这一步保证了ImuProcess接收到的是完整的“当前 LiDAR 扫描 足够 IMU 时间覆盖”组合从而能够把 IMU 状态传播到扫描结束时刻并将整帧点云统一去畸变到结束时刻 LiDAR 坐标系。它不负责真正的点级插值也不负责解决真实硬件时间偏差它只保证在当前时间戳体系下数据包的开始、结束和缓存边界是完整的。真正决定其稳定性的是 LiDAR header 时间语义、逐点curvature单位与排序、IMU 时间戳单调性以及 LiDAR 与 IMU 是否处于同一时间基准。