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-treestructMapPoint{Eigen::Vector3d p_w;floatintensity;doublelast_seen;};classIncrementalKDTreeMap{public:voidAddPoints(conststd::vectorPointTypepoints,booldownsample);voidDeletePointsInBoxes(conststd::vectorBoxboxes);voidNearestSearch(constPointTypequery,intk,std::vectorPointTypeneighbors,std::vectorfloatsquared_distances);};地图层需要支持近邻搜索给当前点找 5 个左右局部邻居。平面拟合邻域点拟合平面判断点到平面的距离和邻域几何是否可靠。局部地图裁剪机器人移动后删除过远区域避免内存无限增长。体素降采样减少重复点插入否则 KD-tree 会快速膨胀。滤波器结构classPointLioEstimator{public:voidPredict(constImuMeaslast,constImuMeascurr);boolUpdateWithPoint(constLidarPointpoint);voidInsertPointToMap(constLidarPointpoint);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 eventsync_buffer.pop_next_event();if(event.typeIMU){estimator.Predict(event.last_imu,event.curr_imu);continue;}if(event.typeLIDAR_SCAN){autopointspreprocess(event.cloud);sort(points.begin(),points.end(),by_offset_time);for(constautopt_l:points){doublet_pointevent.scan_start_timept_l.offset_time;propagate_imu_until(t_point);if(!map.initialized()){estimator.InsertPointToMap(pt_l);continue;}boolacceptedestimator.UpdateWithPoint(pt_l);if(acceptedshould_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_timeVelodyne/Ouster 可能通过 ring 和 azimuth 推算。对固态雷达不要假设扫描线均匀应使用驱动给出的硬件时间。可做体素或间隔降采样但 Point-LIO 的逐点更新对点数敏感降采样策略直接影响 CPU 占用。常见坑点点时间单位混淆纳秒、微秒、毫秒、秒混用会导致 deskew 完全错误。bag 回放时/use_sim_time与消息时间戳不一致。LiDAR 时间戳是帧结束时间还是帧开始时间不同驱动差异很大。2. IMU 预测输入相邻 IMU 测量、上一状态和协方差。输出当前时刻状态预测和协方差预测。连续模型近似为旋转由去 bias 后角速度积分。速度由世界系加速度和重力积分。位置由速度积分。bias 按随机游走建模。代码中通常写成Vec3 omegaimu.gyro-x.b_g;Vec3 acc_iimu.acc-x.b_a;x.R_w_ix.R_w_i*Exp(omega*dt);x.v_w_ix.v_w_i(x.R_w_i*acc_ix.g_w)*dt;x.p_w_ix.p_w_ix.v_w_i*dt0.5*(x.R_w_i*acc_ix.g_w)*dt*dt;PF_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。在地图里搜索最近k5个邻居。拟合平面n^T p d 0。检查邻域是否近似共面、点到平面距离是否小于阈值。构造残差r n^T p_w d。伪代码boolBuildPlaneResidual(constPointpt_l,Residualresidual){Vec3 p_ix.R_i_l*pt_l.xyzx.p_i_l;Vec3 p_wx.R_w_i*p_ix.p_w_i;autonnmap.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.normalplane.n;residual.point_ip_i;residual.valueplane.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 HBuildJacobian(residual);doubleRlidar_measurement_noise;Mat SH*P*H.transpose()R;Vec KP*H.transpose()*inverse(S);Vec dx-K*residual.value;InjectErrorState(x,dx);P(I-K*H)*P;若采用迭代 ESKF会在同一点或一批点上重复线性化数次for(intiter0;itermax_iter;iter){residualsBuildResiduals(x);H,rLinearize(residuals,x);dxSolveIESKF(P,H,r);InjectErrorState(x,dx);if(dx.norm()eps)break;}Point-LIO 的工程重点是每次更新足够轻而不是每次都做很重的迭代。5. 地图更新与局部地图管理输入被接受的世界系点。输出更新后的局部地图。地图更新策略会强烈影响轨迹质量插入太多点kNN 变慢内存上涨实时性下降。插入太少点局部几何约束不足退化环境易漂。不做局部裁剪长时间运行后 KD-tree 查询延迟不可控。地图点过密平面拟合被同一局部区域重复点主导。建议实现if(distance_to_nearest_map_pointvoxel_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_deleteComputeOutsideBoxes(local_map_box,x.p_w_i);ikdtree.DeletePointsInBoxes(boxes_to_delete);}6. 初始化与失败恢复初始化通常包括收集若干秒静止 IMU估计平均加速度方向为重力方向。初始速度设为 0。初始 bias 从均值估计或先设 0 后在线收敛。等待地图中积累足够点后再启用完整点到面更新。失败恢复建议连续残差过大时增大测量噪声或暂停地图插入。IMU 饱和时标记异常段不要强行积分。地图点不足时退化为纯 IMU 短时预测并降低输出可信度。对动态物体区域可通过残差一致性剔除。数学模型到代码的对应关系IMU 传播对应代码论文中的连续状态方程在代码中一般对应Predict()函数voidPredict(constImuMeasimu0,constImuMeasimu1){doubledtimu1.t-imu0.t;Vec3 gyro0.5*(imu0.gyroimu1.gyro)-x.b_g;Vec3 acc0.5*(imu0.accimu1.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 随机游走。调参时不能只看轨迹是否平滑。噪声设太小会导致滤波器过度相信 IMULiDAR 残差难以拉回设太大会导致姿态抖动和地图毛刺。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_iR_i_l*pt_lt_i_l;Vec3 p_wR_w_i*p_it_w_i;Plane plFitPlane(map.KNN(p_w,5));Residual res;res.rpl.n.dot(p_w)pl.d;res.H_pospl.n.transpose();res.H_rot-pl.n.transpose()*R_w_i.matrix()*Hat(p_i);returnres;}协方差与状态注入误差状态求解得到dx后不是直接把旋转矩阵元素相加而是李群注入x.R_w_ix.R_w_i*Exp(dx.segment3(IDX_ROT));x.p_w_idx.segment3(IDX_POS);x.v_w_idx.segment3(IDX_VEL);x.b_gdx.segment3(IDX_BG);x.b_adx.segment3(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 风格的实时地图构建能力同时降低扫描畸变和输出延迟。具体数值以原文表格为准本文不复述具体数值以避免误抄。复现路线从零写一个最小版本依赖建议C17ROS Noetic 或 ROS2 Humble先用 ROS1 复现实验更接近官方生态EigenSophus 或自写 SO(3) Exp/LogPCLikd-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 噪声或地图插入策略。代码阅读指南官方仓库https://github.com/hku-mars/Point-LIO建议阅读顺序README 与配置文件先看支持的 LiDAR 类型、topic 名、外参配置、IMU 噪声参数。ROS 节点入口找到订阅 IMU / LiDAR、同步缓存和主循环位置理解数据从 callback 到 estimator 的路径。预处理模块重点看不同雷达点类型如何取offset_time以及滤波/降采样策略。Estimator / ImuProcess阅读状态定义、IMU 初始化、预测和协方差传播。地图模块看 ikd-Tree 的 add/delete/nearest search 调用位置而不是一开始深挖 KD-tree 内部。残差与更新函数定位点到平面拟合、Jacobian 构造和 Kalman gain 求解。发布模块确认发布的是 IMU pose、LiDAR pose 还是外参转换后的 body pose。如果仓库结构与 FAST-LIO2 类似核心文件通常围绕laserMapping.cpp主流程、点云处理、地图更新、发布。preprocess.*雷达适配和特征/点过滤。IMU_Processing.*IMU 初始化、传播、去畸变。ikd_Tree.*增量地图。esekfom相关目录误差状态滤波框架。阅读时不要从模板库或滤波框架底层开始否则容易迷失。先跑通一个 bag然后按日志把每个点云数据结构追到残差构造处。工程调参与踩坑时间同步Point-LIO 对点时间极敏感。LiDAR 帧起点/终点理解错误会造成快速运动时地图呈扇形撕裂。外参方向配置里到底是T_lidar_imu还是T_imu_lidar必须确认。方向反了时慢速可能勉强跑快速转动必炸。IMU 噪声单位数据手册常给deg/s/sqrt(Hz)或mg/sqrt(Hz)代码通常要 rad 和 m/s² 单位。初始静止假设如果启动时平台正在运动重力和 bias 初始化会污染后续表现为持续倾斜或速度漂。地图插入过密逐点更新容易把大量近似重复点插入地图导致 KD-tree 越跑越慢。动态物体行人、车流会形成错误平面建议通过残差、邻域稳定性和语义/范围门限剔除。退化环境长走廊、隧道、玻璃墙等会使某些方向约束弱滤波协方差应能反映退化不要强行过小测量噪声。LiDAR 近距离噪声太近的点角度误差和遮挡严重建议设置blind范围。CPU 实时性如果单点更新太频繁可按距离、时间或 voxel 选择部分点更新但地图插入和滤波更新策略要一致。协方差数值稳定性更新后应保持对称正定可使用 Joseph form 或定期对称化P 0.5*(P P.transpose())。坐标系约定ROS 常用 ENUIMU 驱动可能 NED 或机体系定义不同必须核查加速度静止方向。发布延迟如果用于控制应发布点级最新状态如果用于建图可发布 scan end 状态二者语义不同。局限性与改进方向Point-LIO 的主要限制来自局部几何约束和滤波框架本身无全局一致性保证系统主要是 odometry没有强回环和全局 pose graph 时长距离仍会累积漂移。对时间戳依赖强点级更新需要可靠点内时间普通点云驱动若缺少字段优势会下降。动态环境敏感点到静态地图的假设在大面积动态物体场景中会失效。地图内存与实时性权衡高频逐点处理要求严格控制地图规模否则长时间运行会变慢。滤波线性化局限剧烈非线性、初值差、外参错误时ESKF 可能比滑窗优化更难恢复。缺少语义约束对玻璃、植被、水面等不稳定几何没有显式建模。可尝试改进方向加入 lightweight loop closure把 Point-LIO 作为前端后接 pose graph。使用退化检测根据 Hessian / 信息矩阵调整测量噪声。用语义或动态物体检测过滤不稳定点。针对 GPU 平台实现并行 kNN 和批量滤波更新。与视觉特征或事件相机融合改善纹理/几何退化场景。引入 submap 管理把局部地图从单一 KD-tree 扩展为可保存、可回环、可复用的地图块。延伸阅读FAST-LIO2: Fast Direct LiDAR-Inertial OdometryPoint-LIO 的直接前序工作理解 ikd-Tree 和直接点到地图配准必须读。FAST-LIO: A Fast, Robust LiDAR-Inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter理解 IESKF 在 LIO 中的基本形式。LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping滑窗/因子图路线代表便于对比滤波与平滑。LOAM: Lidar Odometry and Mapping in Real-time理解 scan-to-scan / scan-to-map 和特征点 Lidar odometry 的经典基线。ikd-Tree: An Incremental KD Tree for Robotic Applications深入理解 FAST-LIO 系列地图结构的性能来源。