激光SLAM中点到平面距离计算与误差传播详解

1. 项目概述

FAST-LIVO2是一个基于激光雷达和视觉融合的SLAM系统，其核心算法之一就是点到平面距离的计算及其不确定性传播。这个看似简单的数学问题，在实际SLAM应用中却有着举足轻重的作用。每次激光雷达扫描到环境中的点云时，我们都需要精确计算这些点到对应平面的距离，并理解这种测量中的不确定性如何在整个系统中传播。

在实际工作中，我发现很多工程师虽然会调用现成的库函数来计算点到平面距离，但对背后的数学原理和误差传播机制理解不深。这导致在系统调试时遇到精度问题往往无从下手。本文将带你深入这个基础但关键的数学问题，从几何原理到概率统计，完整推导整个过程。

2. 点到平面距离的基础理论

2.1 平面表示方法

在三维空间中，一个平面可以有多种表示方式。最常用的是海塞(Hesse)法式：

n·x + d = 0

其中n是平面的单位法向量，d是平面到原点的有向距离。这种表示法的优势在于几何意义明确，而且法向量n直接反映了平面的朝向。

另一种常见表示是广义平面方程：

ax + by + cz + d = 0

这里(a,b,c)不一定是单位向量。两种表示可以相互转换，只需将广义方程的各项除以√(a²+b²+c²)即可归一化。

注意：在实际编程实现中，务必确保法向量是单位长度的，否则距离计算公式会产生偏差。

2.2 点到平面距离公式推导

给定平面π:n·x + d = 0和点p=(x0,y0,z0)，点到平面的距离公式可以通过向量投影轻松推导：

1. 从平面π上任取一点q，满足n·q + d = 0
2. 向量v = p - q
3. 距离就是v在法向量n上的投影长度：d = |v·n| = |(p-q)·n| = |p·n - q·n| = |p·n + d|

因此得到经典公式：

distance = |n·p + d|

这个公式的几何意义非常直观：将点坐标代入平面方程，取绝对值就是距离。如果使用广义平面方程，则距离公式为：

distance = |ax0 + by0 + cz0 + d| / √(a²+b²+c²)

3. 不确定性传播理论

3.1 误差来源分析

在SLAM系统中，点到平面距离计算的不确定性主要来自两个方面：

1. 点坐标的测量误差：激光雷达测距存在噪声，通常建模为零均