Point-LIO-尧图网络科技

Point-LIO: Robust High-Bandwidth Light Detection and Ranging Inertial Odometry

日期：2026-06-25
论文链接：https://arxiv.org/abs/2208.08233
代码仓库：https://github.com/hku-mars/Point-LIO
作者与机构：Jiarong Lin, Chen Ye, Fu Zhang；香港大学 HKU MARS Lab 等
发表 venue / 年份：IEEE Robotics and Automation Letters / ICRA 2023 相关工作，2023

一句话总结

Point-LIO 解决高频 LiDAR 与 IMU 紧耦合里程计在剧烈运动、运动畸变和实时性上的工程难题：它不再等一整帧点云完成后做 scan-to-map，而是以“点”为单位把 LiDAR 测量直接注入迭代误差状态卡尔曼滤波器，实现高带宽、低延迟的 LiDAR-Inertial Odometry。

它的核心贡献是把 FAST-LIO 系列的 ikd-Tree 增量地图和 IESKF 后端进一步前移到逐点更新，使系统能够在一帧扫描尚未结束时就更新状态，尤其适合固态 LiDAR、非重复扫描 LiDAR、快速运动平台和高动态场景。

工程视角：这篇论文要实现什么

从代码实现角度，Point-LIO 是一个在线 LIO 前端/后端一体化系统，输入是 IMU 流和 LiDAR 点流，输出是高频位姿、速度、IMU bias、世界系点云地图，以及可选的 ROS odometry / path / map topic。

系统可以拆成如下运行时模块：

传感器接入层：订阅 IMU 与 LiDAR ROS topic，解析时间戳、外参、点云字段、扫描线、点内时间 offset。
IMU 预测层：按 IMU 时间顺序积分状态，维护状态协方差，提供任意 LiDAR 点时刻的状态预测。
逐点 LiDAR 更新层：每到一个有效点，就根据当前预测状态把点投到世界系，在局部地图中找邻域平面，构造 point-to-plane 残差并执行滤波更新。
地图层：使用 ikd-Tree / 增量 KD-tree 管理局部地图，支持 nearest neighbor、增量插入、视野外点删除或局部地图裁剪。
输出层：发布里程计、TF、轨迹、局部地图、稀疏/稠密点云。

Point-LIO 的在线目标不是离线建最漂亮的地图，而是以低延迟提供稳定位姿。典型工程约束包括：

LiDAR 数据不能只按整帧处理，否则延迟至少是一帧扫描周期。
IMU 频率高，LiDAR 点也多，状态更新和地图查询必须近似实时。
每个点都做完整非线性优化会太慢，因此必须使用误差状态滤波、邻域平面快速拟合和地图降采样。
系统需要在点云非常稀疏、扫描 pattern 非重复、旋转速度很高时仍然不明显漂移。

核心数据结构与状态量

Point-LIO 的状态本质上是 LiDAR-IMU 系统的误差状态卡尔曼滤波变量。工程中通常维护 nominal state + error state covariance。

状态量

structState{Sophus::SO3d R_w_i;// IMU 到世界旋转Eigen::Vector3d p_w_i;// IMU 在世界系位置Eigen::Vector3d v_w_i;// IMU 在世界系速度Eigen::Vector3d b_g;// 陀螺 biasEigen::Vector3d b_a;// 加计 biasEigen::Vector3d g_w;// 重力向量，部分实现中固定或估计Sophus::SO3d R_i_l;// LiDAR 到 IMU 外参旋转，可固定或在线估计Eigen::Vector3d p_i_l;// LiDAR 到 IMU 外参平移，可固定或在线估计};structErrorState{Vec3 dtheta;Vec3 dp;Vec3 dv;Vec3 dbg;Vec3 dba;Vec3 dg;Vec3 dex_rot;Vec3 dex_pos;};

工程实现里常见维度为 18、21、24 或更高，取决于是否在线估计重力和 LiDAR-IMU 外参。Point-LIO 继承 FAST-LIO 风格，核心优化变量是惯性状态、外参和协方差，而不是滑窗内所有关键帧。

IMU 数据

structImuMeas{doublet;Eigen::Vector3d acc;Eigen::Vector3d gyro;};

IMU 队列需要严格按时间排序。实现时要检查：

IMU 与 LiDAR 时间戳是否同一时钟源。
第一帧前是否已有足够 IMU 用于初始化重力和 bias。
相邻 IMU 时间差是否异常，例如设备丢包或 bag 回放跳变。

LiDAR 点数据

structLidarPoint{floatx,y,z;floatintensity;doubleoffset_time;// 相对当前 scan 起点或点自身绝对时间intring;// 机械式雷达常见；固态雷达可能无 ring};

Point-LIO 的关键在于offset_time。如果点云驱动没有提供点内时间，逐点更新和运动补偿会明显变差，工程上需要从雷达 packet 或扫描线顺序估计。

地图点与增量 KD-tree

structMapPoint{Eigen::Vector3d p_w;floatintensity;doublelast_seen;};classIncrementalKDTreeMap{public:voidAddPoints(conststd::vector<PointType>&points,booldownsample);voidDeletePointsInBoxes(conststd::vector<Box>&boxes);voidNearestSearch(constPointType&query,intk,std::vector<PointType>&neighbors,std::vector<float>&squared_distances);};

地图层需要支持：

近邻搜索：给当前点找 5 个左右局部邻居。
平面拟合：邻域点拟合平面，判断点到平面的距离和邻域几何是否可靠。
局部地图裁剪：机器人移动后删除过远区域，避免内存无限增长。
体素降采样：减少重复点插入，否则 KD-tree 会快速膨胀。

滤波器结构

classPointLioEstimator{public:voidPredict(constImuMeas&last,constImuMeas&curr);boolUpdateWithPoint(constLidarPoint&point);voidInsertPointToMap(constLidarPoint&point);private:State x;Eigen::MatrixXd P;IncrementalKDTreeMap map;};

和滑窗 BA 不同，这里的核心容器不是关键帧数组，而是“当前状态 + 协方差 + 局部地图”。这也是 Point-LIO 能低延迟的主要原因。

算法主流程

传统 LIO 多是每帧点云完整接收后做 deskew、scan matching、优化、地图更新。Point-LIO 更接近事件驱动：IMU 来了就预测，LiDAR 点来了就更新。

总体步骤

系统启动，收集静止或低动态 IMU，估计初始重力方向、陀螺 bias、加计 bias。
接收 LiDAR 点云帧，按点的真实采样时间排序。
对每个 LiDAR 点：
- 用点时间之前的 IMU 数据预测状态。
- 把 LiDAR 点经外参和当前状态转换到世界系。
- 在局部地图中做 kNN，拟合局部平面。
- 构造 point-to-plane 残差。
- 使用 IESKF / ESKF 更新状态与协方差。
- 根据策略把有效点插入地图。
周期性发布当前状态、轨迹和地图。
当当前位置接近局部地图边界时，移动 local map box 并删除远处点。

伪代码

while(ros::ok()){SensorEvent event=sync_buffer.pop_next_event();if(event.type==IMU){estimator.Predict(event.last_imu,event.curr_imu);continue;}if(event.type==LIDAR_SCAN){autopoints=preprocess(event.cloud);sort(points.begin(),points.end(),by_offset_time);for(constauto&pt_l:points){doublet_point=event.scan_start_time+pt_l.offset_time;propagate_imu_until(t_point);if(!map.initialized()){estimator.InsertPointToMap(pt_l);continue;}boolaccepted=estimator.UpdateWithPoint(pt_l);if(accepted&&should_insert(pt_l)){estimator.InsertPointToMap(pt_l);}}estimator.PublishOdometry();estimator.PublishMapIfNeeded();}}

与 FAST-LIO2 的关键流程差异

FAST-LIO2 通常以 scan 为单位构造一批点到地图残差，然后迭代更新当前帧状态。
Point-LIO 把 LiDAR 更新拆成点级事件，使状态可以在一个 scan 内连续更新。
Point-LIO 对高带宽的理解是：输出状态频率不受 LiDAR 整帧频率限制，而更接近 IMU 与点事件驱动频率。

关键模块实现细节

1. 前端：点预处理与时间管理

输入：原始sensor_msgs/PointCloud2或厂商自定义点云消息。
输出：带offset_time的点数组。

实现要点：

去除 NaN、过近点、过远点，例如距离小于 0.5m 的点常来自雷达机身或噪声。
对不同雷达适配字段：Livox 常有offset_time，Velodyne/Ouster 可能通过 ring 和 azimuth 推算。
对固态雷达，不要假设扫描线均匀；应使用驱动给出的硬件时间。
可做体素或间隔降采样，但 Point-LIO 的逐点更新对点数敏感，降采样策略直接影响 CPU 占用。

常见坑点：

点时间单位混淆：纳秒、微秒、毫秒、秒混用会导致 deskew 完全错误。
bag 回放时/use_sim_time与消息时间戳不一致。
LiDAR 时间戳是帧结束时间还是帧开始时间，不同驱动差异很大。

2. IMU 预测

输入：相邻 IMU 测量、上一状态和协方差。
输出：当前时刻状态预测和协方差预测。

连续模型近似为：

旋转由去 bias 后角速度积分。
速度由世界系加速度和重力积分。
位置由速度积分。
bias 按随机游走建模。

代码中通常写成：

Vec3 omega=imu.gyro-x.b_g;Vec3 acc_i=imu.acc-x.b_a;x.R_w_i=x.R_w_i*Exp(omega*dt);x.v_w_i=x.v_w_i+(x.R_w_i*acc_i+x.g_w)*dt;x.p_w_i=x.p_w_i+x.v_w_i*dt+0.5*(x.R_w_i*acc_i+x.g_w)*dt*dt;P=F_x*P*F_x.transpose()+F_w*Q*F_w.transpose();

工程上应使用中值积分或预积分风格处理相邻 IMU，避免低频 IMU 时数值误差过大。

3. LiDAR 点到地图关联

输入：当前 LiDAR 点、当前状态、局部地图。
输出：一个有效的 point-to-plane 残差，或拒绝该点。

步骤：

点从 LiDAR 系变换到 IMU 系：p_i = R_i_l * p_l + p_i_l。
点从 IMU 系变换到世界系：p_w = R_w_i * p_i + p_w_i。
在地图里搜索最近k=5个邻居。
拟合平面n^T p + d = 0。
检查邻域是否近似共面、点到平面距离是否小于阈值。
构造残差r = n^T p_w + d。

伪代码：

boolBuildPlaneResidual(constPoint&pt_l,Residual&residual){Vec3 p_i=x.R_i_l*pt_l.xyz+x.p_i_l;Vec3 p_w=x.R_w_i*p_i+x.p_w_i;autonn=map.NearestSearch(p_w,5);if(nn.size()<5)returnfalse;Plane plane;if(!FitPlane(nn,plane))returnfalse;if(fabs(plane.n.dot(p_w)+plane.d)>max_plane_dist)returnfalse;residual.normal=plane.n;residual.point_i=p_i;residual.value=plane.n.dot(p_w)+plane.d;returntrue;}

复杂度主要来自 kNN。单点更新的频率很高，因此 KD-tree 查询和地图点数量控制非常关键。

4. 后端：IESKF / ESKF 更新

输入：点到平面残差、当前状态、协方差。
输出：更新后的状态和协方差。

Point-LIO 不做大规模 BA，而是把每个点残差当成测量更新。对一个点残差：

r = n^T (R_w_i * (R_i_l * p_l + p_i_l) + p_w_i) + d

误差状态线性化后：

r ≈ r0 + H * δx

代码中关键是写出测量 Jacobian：

对位置误差：∂r/∂δp = n^T
对姿态误差：∂r/∂δθ ≈ -n^T R_w_i [p_i]x
对外参旋转：∂r/∂δθ_li ≈ -n^T R_w_i R_i_l [p_l]x
对外参平移：∂r/∂δp_li = n^T R_w_i
对速度、bias：单个 LiDAR 几何残差直接 Jacobian 多为 0，但通过协方差相关性间接更新。

ESKF 更新伪代码：

Mat H=BuildJacobian(residual);doubleR=lidar_measurement_noise;Mat S=H*P*H.transpose()+R;Vec K=P*H.transpose()*inverse(S);Vec dx=-K*residual.value;InjectErrorState(x,dx);P=(I-K*H)*P;

若采用迭代 ESKF，会在同一点或一批点上重复线性化数次：

for(intiter=0;iter<max_iter;++iter){residuals=BuildResiduals(x);H,r=Linearize(residuals,x);dx=SolveIESKF(P,H,r);InjectErrorState(x,dx);if(dx.norm()<eps)break;}

Point-LIO 的工程重点是每次更新足够轻，而不是每次都做很重的迭代。

5. 地图更新与局部地图管理

输入：被接受的世界系点。
输出：更新后的局部地图。

地图更新策略会强烈影响轨迹质量：

插入太多点：kNN 变慢，内存上涨，实时性下降。
插入太少点：局部几何约束不足，退化环境易漂。
不做局部裁剪：长时间运行后 KD-tree 查询延迟不可控。
地图点过密：平面拟合被同一局部区域重复点主导。

建议实现：

if(distance_to_nearest_map_point>voxel_leaf_size){map_buffer.push_back(p_w);}if(map_buffer.size()>insert_batch_size){ikdtree.AddPoints(map_buffer,true);map_buffer.clear();}if(NeedMoveLocalMap(x.p_w_i)){autoboxes_to_delete=ComputeOutsideBoxes(local_map_box,x.p_w_i);ikdtree.DeletePointsInBoxes(boxes_to_delete);}

6. 初始化与失败恢复

初始化通常包括：

收集若干秒静止 IMU，估计平均加速度方向为重力方向。
初始速度设为 0。
初始 bias 从均值估计，或先设 0 后在线收敛。
等待地图中积累足够点后再启用完整点到面更新。

失败恢复建议：

连续残差过大时增大测量噪声或暂停地图插入。
IMU 饱和时标记异常段，不要强行积分。
地图点不足时退化为纯 IMU 短时预测，并降低输出可信度。
对动态物体区域可通过残差一致性剔除。

数学模型到代码的对应关系

IMU 传播对应代码

论文中的连续状态方程在代码中一般对应Predict()函数：

voidPredict(constImuMeas&imu0,constImuMeas&imu1){doubledt=imu1.t-imu0.t;Vec3 gyro=0.5*(imu0.gyro+imu1.gyro)-x.b_g;Vec3 acc=0.5*(imu0.acc+imu1.acc)-x.b_a;PropagateNominalState(gyro,acc,dt);PropagateCovariance(gyro,acc,dt);}

公式里的噪声项对应配置文件中的 IMU 参数，例如：

gyr_cov：陀螺白噪声。
acc_cov：加计白噪声。
b_gyr_cov：陀螺 bias 随机游走。
b_acc_cov：加计 bias 随机游走。

调参时不能只看轨迹是否平滑。噪声设太小会导致滤波器过度相信 IMU，LiDAR 残差难以拉回；设太大会导致姿态抖动和地图毛刺。

LiDAR 测量模型对应代码

论文中的点到平面测量模型：

0 = n^T (T_w_i T_i_l p_l - q) + noise

在代码里就是三步：

TransformPoint()：把p_l变到p_w。
NearestSearch()+FitPlane()：得到n和d。
BuildJacobian()：填充H的姿态、位置、外参列。

一个最小可读版本：

ResidualMakeResidual(Point pt_l){Vec3 p_i=R_i_l*pt_l+t_i_l;Vec3 p_w=R_w_i*p_i+t_w_i;Plane pl=FitPlane(map.KNN(p_w,5));Residual res;res.r=pl.n.dot(p_w)+pl.d;res.H_pos=pl.n.transpose();res.H_rot=-pl.n.transpose()*R_w_i.matrix()*Hat(p_i);returnres;}

协方差与状态注入

误差状态求解得到dx后，不是直接把旋转矩阵元素相加，而是李群注入：

x.R_w_i=x.R_w_i*Exp(dx.segment<3>(IDX_ROT));x.p_w_i+=dx.segment<3>(IDX_POS);x.v_w_i+=dx.segment<3>(IDX_VEL);x.b_g+=dx.segment<3>(IDX_BG);x.b_a+=dx.segment<3>(IDX_BA);

姿态更新方向必须和 Jacobian 推导一致。左扰动、右扰动混用会导致系统看起来能跑，但快速转弯时发散。

关键创新点

逐点 LiDAR 更新：从 scan-level update 改为 point-level update，代码层面需要事件驱动式同步、点时间排序、按点状态传播和按点滤波更新。
高带宽状态输出：状态不再只在一帧点云结束时更新，而是在扫描过程中持续更新，降低控制闭环和高速运动感知延迟。
直接点到地图残差：继承 FAST-LIO2 的 raw point registration 思路，不依赖角点/面点特征分类，适配 Livox 等非重复扫描 LiDAR。
增量地图结构：使用 ikd-Tree 支持在线 kNN、增量插入和删除，避免每帧重建 KD-tree。
滤波而非滑窗大优化：维护当前状态和协方差，在算力有限平台上更容易实时部署。
适合高动态平台：点级更新时间与 IMU 高频传播结合，减轻扫描周期内的运动畸变累积。

实验与结果

论文主要在不同类型 LiDAR 和高速运动平台上验证，包括 Livox 系列固态 LiDAR、机械式 LiDAR 以及手持/无人机等场景。对比对象通常包括 FAST-LIO2、LIO-SAM、LOAM 系列或其他 LiDAR-Inertial Odometry 方法。

评价指标包括：

轨迹精度：ATE / RPE，或与 motion capture、RTK、公开数据集 ground truth 对比。
实时性：每帧处理时间、状态输出频率、CPU 占用。
鲁棒性：快速旋转、快速平移、稀疏结构、非重复扫描、退化环境。
地图质量：点云厚度、结构一致性、闭合区域重影程度。

主要结论是：Point-LIO 在高动态运动和高频输出方面优于典型 scan-level LIO；在许多场景中能保持 FAST-LIO2 风格的实时地图构建能力，同时降低扫描畸变和输出延迟。具体数值以原文表格为准，本文不复述具体数值以避免误抄。

复现路线：从零写一个最小版本

依赖建议

C++17
ROS Noetic 或 ROS2 Humble，先用 ROS1 复现实验更接近官方生态
Eigen
Sophus 或自写 SO(3) Exp/Log
PCL
ikd-Tree，可从 FAST-LIO2 / Point-LIO 仓库抽取
Ceres/GTSAM 不是必须，滤波更新可直接 Eigen 实现

数据准备

先用官方仓库提供或推荐的数据集。
确认 LiDAR 点云中包含点内时间字段。
准备 LiDAR-IMU 外参，先固定外参跑通，再考虑在线估计。
用rosbag info检查 IMU 与 LiDAR 频率、时间跨度、topic 名。

MVP 模块拆分

point_lio_minimal/ src/ main_node.cpp sensor_sync.cpp imu_predictor.cpp eskf.cpp lidar_residual.cpp ikd_map.cpp preprocess.cpp include/ state.hpp math_utils.hpp config.hpp config/ livox.yaml velodyne.yaml

最小可运行版本顺序

写State和 SO(3) 工具函数，只做 IMU dead reckoning，发布 odom。
加入 LiDAR 点读取和点内时间排序，打印每帧时间范围。
用初始若干帧点云建立静态地图，不做滤波更新。
实现 kNN + 平面拟合，离线验证残差分布。
实现单点 ESKF 更新，只更新 pose，不在线估计外参。
加入地图插入和体素降采样。
加入局部地图裁剪和异常点剔除。
最后再打开外参在线估计、重力估计、复杂雷达适配。

建议验证指标

每个点残差均值和 95% 分位数。
有效平面比例，例如valid_plane / processed_points。
单帧平均处理时间和最大处理时间。
KD-tree 点数和 kNN 平均耗时。
IMU-LiDAR 时间差统计。
位姿输出频率和延迟。

调试日志建议

[SYNC] lidar scan: start=..., end=..., imu_count=... [IMU] dt_mean=..., gyro_norm=..., acc_norm=... [LIDAR] raw=..., used=..., plane_valid=... [ESKF] residual_mean=..., dx_norm=..., P_trace=... [MAP] size=..., insert=..., delete=..., knn_ms=...

这些日志比只看 RViz 更重要。很多 LIO 问题在 RViz 里表现为“地图歪了”，但根因通常是时间戳、外参、IMU 噪声或地图插入策略。