激光雷达点云协方差传播原理与工程实践

1. 点云协方差传播的核心价值与挑战

在激光雷达惯性里程计（LIO）系统中，点云协方差传播是一个看似简单却至关重要的环节。想象一下，当激光雷达扫描到环境中的一个点时，这个点的位置信息本身就带有测量噪声。而当我们将这个点从激光雷达坐标系转换到世界坐标系时，不仅需要考虑点自身的噪声，还要考虑传感器位姿估计的不确定性。这就好比在暴风雨中试图用颤抖的手在地图上标记位置——我们需要同时考虑笔尖的抖动（点云噪声）和整张地图的晃动（位姿估计误差）。

LIVO2作为紧耦合激光雷达-视觉-惯性里程计系统，其点云协方差传播的准确性直接影响着状态估计的精度。错误的协方差估计会导致两种严重后果：一是低估不确定性，使得系统过度信任噪声数据，导致优化发散；二是高估不确定性，使得有效约束被忽视，造成精度下降。这两种情况都会像多米诺骨牌一样，引发后续状态估计的连锁反应。

2. 坐标变换模型与不确定性来源

2.1 基本坐标变换框架

点云从体素坐标系到世界坐标系的变换遵循标准的刚体变换模型：

p_w = R * p_b + t

其中：

• p_b ∈ ℝ³：点在体素（激光雷达）坐标系下的坐标
• p_w ∈ ℝ³：点在世界坐标系下的坐标
• R ∈ SO(3)：旋转矩阵（对应代码中的state_.rot_end）
• t ∈ ℝ³：平移向量（对应代码中的state_.pos_end）

这个看似简单的公式背后隐藏着三个主要的不确定性来源，它们共同决定了最终点在世界坐标系中的可信度。

2.2 三大不确定性来源解析

在实际系统中，我们需要考虑以下三种不确定性：

1. 点自身测量噪声（Σ_b）：
   来源于激光雷达的测量误差和体素化过程中的量化误差。在代码中体现为body_cov_list_[i]，通常由传感器厂商提供或通过离线标定获得。

2. 旋转状态噪声（Σ_φ）：
   反映IMU积分和状态估计中的旋转不确定性。代码中通过rot_var = state_.cov.block<3,3>(0,0)获取。这里有个关键细节：LIVO2采用右乘扰动模型更新旋转，即R ← R*exp(δφ^∧)，其中δφ~N(0,Σ_φ)。

3. 平移状态噪声（Σ_t）：
   表示位置估计的不确定性，代码中对应t_var = state_.cov.b