Point-LIO

原创于 2026-06-25 20:24:41 发布 · 76 阅读

0 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

标签

#算法 #几何学

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。

状态量

struct State {
    Sophus::SO3d R_w_i;      // IMU 到世界旋转
    Eigen::Vector3d p_w_i;   // IMU 在世界系位置
    Eigen::Vector3d v_w_i;   // IMU 在世界系速度
    Eigen::Vector3d b_g;     // 陀螺 bias
    Eigen::Vector3d b_a;     // 加计 bias
    Eigen::Vector3d g_w;     // 重力向量，部分实现中固定或估计

    Sophus::SO3d R_i_l;      // LiDAR 到 IMU 外参旋转，可固定或在线估计
    Eigen::Vector3d p_i_l;   // LiDAR 到 IMU 外参平移，可固定或在线估计
};

struct ErrorState {
    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 数据

struct ImuMeas {
    double t;
    Eigen::Vector3d acc;
    Eigen::Vector3d gyro;
};

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

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

LiDAR 点数据

struct LidarPoint {
    float x, y, z;
    float intensity;
    double offset_time;   // 相对当前 scan 起点或点自身绝对时间
    int ring;             // 机械式雷达常见；固态雷达可能无 ring
};

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

地图点与增量 KD-tree

struct MapPoint {
    Eigen::Vector3d p_w;
    float intensity;
    double last_seen;
};

class IncrementalKDTreeMap {
public:
    void AddPoints(const std::vector<PointType>& points, bool downsample);
    void DeletePointsInBoxes(const std::vector<Box>& boxes);
    void NearestSearch(const PointType& query, int k,
                       std::vector<PointType>& neighbors,
                       std::vector<float>& squared_distances);
};

地图层需要支持：

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

滤波器结构

class PointLioEstimator {
public:
    void Predict(const ImuMeas& last, const ImuMeas& curr);
    bool UpdateWithPoint(const LidarPoint& point);
    void InsertPointToMap(const LidarPoint& 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) {
        auto points = preprocess(event.cloud);
        sort(points.begin(), points.end(), by_offset_time);

        for (const auto& pt_l : points) {
            double t_point = event.scan_start_time + pt_l.offset_time;
            propagate_imu_until(t_point);

            if (!map.initialized()) {
                estimator.InsertPointToMap(pt_l);
                continue;
            }

            bool accepted = 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。

伪代码：

bool BuildPlaneResidual(const Point& 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;

    auto nn = map.NearestSearch(p_w, 5);
    if (nn.size() < 5) return false;

    Plane plane;
    if (!FitPlane(nn, plane)) return false;
    if (fabs(plane.n.dot(p_w) + plane.d) > max_plane_dist) return false;

    residual.normal = plane.n;
    residual.point_i = p_i;
    residual.value = plane.n.dot(p_w) + plane.d;
    return true;
}

复杂度主要来自 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);
double R = 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 (int iter = 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)) {
    auto boxes_to_delete = ComputeOutsideBoxes(local_map_box, x.p_w_i);
    ikdtree.DeletePointsInBoxes(boxes_to_delete);
}

6. 初始化与失败恢复

初始化通常包括：

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

失败恢复建议：

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

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

IMU 传播对应代码

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

void Predict(const ImuMeas& imu0, const ImuMeas& imu1) {
    double dt = 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 的姿态、位置、外参列。

一个最小可读版本：

Residual MakeResidual(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);
    return res;
}

协方差与状态注入

误差状态求解得到 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 噪声或地图插入策略。