三维空间中点到平面距离计算与不确定性传播

1. 点到平面距离的几何基础

在三维空间几何中，计算点到平面的距离是一个基础但至关重要的操作。这个看似简单的计算背后蕴含着丰富的数学原理和工程考量。让我们从最基本的几何定义开始，逐步深入理解这个问题的本质。

1.1 基本定义与符号约定

首先明确几个关键概念的定义：

• 空间点(p)：表示为三维向量 p = [x, y, z]ᵀ，定义在世界坐标系中
• 平面参考点(c)：通常选择平面上的某个特征点，如几何中心
• 单位法向量(n)：垂直于平面的单位向量，满足 ||n|| = 1
• 平面参数向量(q)：将法向量和参考点组合成的6维向量 [nᵀ, cᵀ]ᵀ

这些定义构成了我们后续推导的基础框架。在实际工程应用中，特别是SLAM（同步定位与地图构建）系统中，这些参数通常来自传感器测量或算法估计，因此都带有一定的不确定性。

1.2 有符号距离公式

点到平面的有符号距离计算公式为：
d = nᵀ(p - c)

这个简洁的公式有几个关键特性值得深入理解：

1. 有符号性：距离值d的正负号表示点位于平面的哪一侧。当d>0时，点在法向量指向的一侧；d<0则在另一侧；d=0表示点在平面上。

2. 几何解释：公式本质上是向量(p-c)在法向量n方向上的投影长度。这种投影操作将三维空间关系转化为一维距离度量。

3. 归一化要求：公式成立的前提是法向量必须是单位向量(||n||=1)。如果使用非单位法向量，则需要额外除以||n||进行归一化。

工程实践提示：在优化问题中（如FAST-LIVO2），必须显式约束法向量的模长为1，否则会导致距离计算失真，进而影响优化结果。

1.3 距离公式的矩阵形式展开

为了更清晰地理解距离计算，我们可以将公式展开为矩阵运算形式：

d = nᵀp - nᵀc = [nᵀ -nᵀ][p c]ᵀ

这种形式揭示了距离计算可以表示为平面参数和点坐标的线性组合。这种表达方式为后续的不确定性传播推导奠定了基础，因为它明确了各个变量之间的函数关系。

在实际应用中，这种矩阵形式的表达也便于代码实现，可以直接对应到线性代数库（如Eigen）的矩阵运算操作，提高计算效率。

2. 不确定性传播的理论框架

在现实世界的工程应用中，无论是点坐标还是平面参