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)，点到平面的距离公式可以通过向量投影轻松推导：
- 从平面π上任取一点q，满足n·q + d = 0
- 向量v = p - q
- 距离就是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系统中，点到平面距离计算的不确定性主要来自两个方面：
- 点坐标的测量误差：激光雷达测距存在噪声，通常建模为零均值高斯噪声
- 平面参数的估计误差：从点云拟合平面时也存在不确定性
这些误差会在距离计算过程中传播，最终影响位姿估计的精度。理解这种传播机制对系统调参和性能优化至关重要。
3.2 一阶不确定性传播
对于一般函数y = f(x)，其中x是随机变量，其一阶近似协方差传播公式为：
Σ_y = J Σ_x J^T
其中J是f关于x的雅可比矩阵。
应用到我们的距离公式d = |n·p + d|，暂时忽略绝对值（因为方差不受线性变换影响），考虑函数：
d = n·p + d = n_x p_x + n_y p_y + n_z p_z + d
将p和π的参数都视为随机变量，令θ = [p_x, p_y, p_z, n_x, n_y, n_z, d]^T，则雅可比矩阵为：
J = [∂d/∂p_x, ∂d/∂p_y, ∂d/∂p_z, ∂d/∂n_x, ∂d/∂n_y, ∂d/∂n_z, ∂d/∂d]
= [n_x, n_y, n_z, p_x, p_y, p_z, 1]
因此距离的方差为：
σ_d² = J Σ_θ J^T
这个公式清晰地展示了点和平面参数的不确定性如何影响最终的距离测量精度。
4. 实际应用中的实现细节
4.1 平面拟合的不确定性
在实际SLAM中，平面通常是通过局部点云拟合得到的。常用的方法是最小二乘法：
- 对局部点云{p_i}计算协方差矩阵Σ = cov({p_i})
- 对Σ进行特征分解，最小特征值对应的特征向量即为法向量n
- d = -n·μ，其中μ是点云质心
这种拟合过程本身也会引入不确定性。根据误差传播定律，平面参数的不确定性取决于点云的分布和噪声特性。
4.2 高效计算实现
在FAST-LIVO2这样的实时系统中，计算效率至关重要。以下是几个优化技巧：
- 预先归一化平面参数，避免每次计算都做除法
- 利用SIMD指令并行计算多个点到同一平面的距离
- 对于固定平面，可以预先计算并缓存部分中间结果
- 使用近似计算当精度要求不高时
C++代码示例：
cpp复制inline float pointToPlaneDistance(const Eigen::Vector3f& point,
const Eigen::Vector3f& normal,
float d) {
return std::abs(normal.dot(point) + d);
}
// 带不确定性传播的版本
float pointToPlaneDistanceWithUncertainty(
const Eigen::Vector3f& point, const Eigen::Matrix3f& point_cov,
const Eigen::Vector3f& normal, float d,
const Eigen::Matrix3f& normal_cov, float d_var) {
Eigen::Matrix<float, 7, 1> theta;
theta << point.x(), point.y(), point.z(), normal.x(), normal.y(), normal.z(), d;
Eigen::Matrix<float, 7, 7> theta_cov = Eigen::Matrix<float, 7, 7>::Zero();
theta_cov.block<3,3>(0,0) = point_cov;
theta_cov.block<3,3>(3,3) = normal_cov;
theta_cov(6,6) = d_var;
Eigen::Matrix<float, 1, 7> J;
J << normal.x(), normal.y(), normal.z(), point.x(), point.y(), point.z(), 1;
float variance = J * theta_cov * J.transpose();
return variance;
}
5. 实验分析与验证
5.1 仿真验证
为了验证我们的不确定性传播公式的正确性，我设计了如下蒙特卡洛实验：
- 生成一个固定平面π和一组带噪声的点云
- 用解析公式计算距离方差
- 用蒙特卡洛模拟统计实际距离的方差
- 比较两种结果的一致性
实验结果表明，在噪声较小的情况下，一阶近似与蒙特卡洛结果非常接近。但当噪声较大时，高阶项的影响变得显著。
5.2 实际系统中的应用
在FAST-LIVO2中，点到平面距离的不确定性被用于：
- 构造ICP目标函数的权重矩阵
- 位姿优化时的信息矩阵
- 关键帧选择和质量评估
- 回环检测的匹配评分
通过合理建模不确定性，系统在挑战性环境中的鲁棒性得到了显著提升。特别是在部分遮挡或动态物体干扰的情况下，加权的最小二乘比普通ICP表现更加稳定。
6. 常见问题与解决方案
6.1 平面退化问题
当局部点云分布接近线性时（如墙边缘），平面拟合会变得不稳定。解决方案：
- 检查平面拟合的条件数，拒绝病态平面
- 增加更多邻域点参与拟合
- 改用边缘或线特征补充
6.2 误差低估问题
一阶近似往往会低估真实的误差传播，特别是在非线性较强时。改进方法：
- 使用二阶近似考虑Hessian项
- 采用保守估计，适当放大方差
- 对关键参数进行边界分析
6.3 计算效率瓶颈
当需要处理大量点-平面匹配时，不确定性传播可能成为计算瓶颈。优化策略：
- 对远距离点使用简化模型
- 并行化计算
- 预计算和缓存不变部分
7. 扩展与进阶话题
7.1 点到其他几何基元的距离
类似的推导可以应用于其他几何基元：
- 点到直线的距离
- 点到圆柱的距离
- 点到二次曲面的距离
每种情况都需要特定的雅可比矩阵推导，但核心思想相同。
7.2 鲁棒代价函数
在存在外点的情况下，平方距离代价函数不够鲁棒。可以考虑：
- Huber损失函数
- Tukey双权函数
- 自适应截断阈值
这些函数在保持可导性的同时，能有效抑制外点的影响。
7.3 现代优化框架中的实现
在现代SLAM系统如GTSAM或Ceres中，可以自定义点到平面的误差项：
cpp复制class PointToPlaneFactor : public ceres::SizedCostFunction<1, 7> {
public:
PointToPlaneFactor(const Eigen::Vector3d& point,
const Eigen::Vector3d& normal,
double d)
: point_(point), normal_(normal), d_(d) {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
// 实现误差和雅可比计算
}
private:
Eigen::Vector3d point_;
Eigen::Vector3d normal_;
double d_;
};
这种实现方式可以无缝集成到基于图的优化框架中。