三维点云处理（十六）：最小二乘拟合算法与鲁棒核函数
# 三维点云处理：最小二乘拟合 (Least Squares Fitting) 与鲁棒核函数
在三维点云几何感知中，从包含噪声的点集里拟合几何基元（如平面、直线、球面或曲面）是场景理解、地面剥离与三维重建的核心任务。最小二乘法（Least Squares, LSQ） 是最基础、最经典的数学拟合工具。
本文系统梳理线性最小二乘的特征值解法推导、非齐次求解、最小二乘对离群噪点的局限性，以及引入 Huber / Cauchy 等鲁棒核函数的非线性优化解法。
一、 模型拟合的工程实际意义与最小二乘基础
1.1 为什么点云需要模型拟合？
在纯数据层面，三维点云只是一堆互不关联的 坐标。模型拟合（Model Fitting） 是将底层离散数据升维到高层连续抽象几何的关键：
- 逆向工程 (Reverse Engineering)：工业 3D 扫描仪扫描出的零件点云无法直接被 CAD 软件编辑。通过将点云局部拟合成完美的圆柱面、平面或球面，可以逆向还原出参数化的 B-Rep (边界表示) CAD 模型。
- 机器人抓取 (Robotic Grasping)：机械臂无法直接抓取“点云”。通过将水杯点云拟合成圆柱体，机器人就能直接获取圆柱的中心轴线（决定抓取姿态）和半径（决定夹爪开度），从而实现精准抓取。
1.2 优化目标：最小化垂直距离
给定二维/三维空间中的点集 ，希望找到拟合直线/平面的最佳参数。
以二维直线模型 为例，拟合的核心是最小化点到直线的垂直几何距离（Perpendicular Distance）平方和：
1.2 齐次线性最小二乘：特征值解法推导
为了防止参数整体缩放出现平凡解（如 ），构造齐次方程组： 定义参数向量 ，约束 。
构造设计矩阵 ：
残差平方和目标可紧凑地写为标准齐次最小二乘优化问题：
证明与闭式解：
由于 ，令实对称矩阵 。根据 [00-核心概念辨析与扩展知识补充 (数学先修课)](file:///i:/Nutstore/1/blog/point-cloud/00-supplementary-knowledge.md) 中关于瑞利商 (Rayleigh Quotient) 定理的推导：
当 时， 取得最小值的充要条件是：
几何投影物理直觉： 矩阵 本质上是点集的非中心化协方差矩阵（结构张量）。
- 寻找特征向量 使得 最小，等价于寻找一个投影方向 ，使得所有点投影到该方向上的方差（散布范围）最小。
- 显然，对于一个平面分布的点云，法向量方向正是点云散布最为扁平、方差最小的方向。因此，最小特征值对应的特征向量直接给出了平面（或二维直线）的法向量！
- 条件限制：矩阵 必须列满秩（即 且点不共线）。
1.3 非齐次线性最小二乘 ()
当拟合模型可以表示为非齐次方程 （如直线斜截式 ）时：
当 且 可逆时，利用求导设为 0：
得到著名的伪逆闭式解 (Normal Equations)：
二、 最小二乘法的致命局限：对离群点极度敏感
虽然线性最小二乘具有全局解析解且计算速度极快，但它在三维点云处理中存在致命缺陷——对离群点（Outliers）极度敏感。
三、 鲁棒核函数 (Robust Loss Functions)
为了削弱离群点大残差对优化目标的霸权控制，用增长更平缓的鲁棒损失函数 替代传统的 二次方残差 。
3.1 常见损失函数对比
| 损失函数类型 | 损失函数表达式 | 特征与对大残差的抑制能力 |
|---|---|---|
| 损失 (Standard LSQ) | 残差平方增长，对离群点最敏感（无任何抑制） | |
| 损失 | $\rho(s) = | s |
| Cauchy 损失 (柯西核) | $\rho(s) = \log(1 + | s |
| Huber 损失 | $\rho(s) = \begin{cases} s^2 & |s| < \delta \ 2\delta( | s |
四、 非线性最小二乘优化 (Non-Linear LSQ)
引入 Huber / Cauchy 等鲁棒核函数后，拟合问题失去了线性解析解，转变为非线性最小二乘优化问题：
4.1 常用非线性优化算法
关于下述非线性最优化算法底层依赖的雅可比、海森矩阵及数学收敛性推导，请详细参考 [00-核心概念辨析与扩展知识补充 (数学先修课)](file:///i:/Nutstore/1/blog/point-cloud/00-supplementary-knowledge.md)。
- 梯度下降法 (Gradient Descent)：沿着负一阶导数（负梯度）方向更新，收敛较慢；
- 高斯-牛顿法 (Gauss-Newton)：直接利用雅可比矩阵 近似海森矩阵 ，一步跨向二次曲面的谷底，收敛极其迅速但依赖良好的初始值；
- Levenberg-Marquardt 算法 (LM 算法)：结合梯度下降与高斯-牛顿法，引入阻尼因子 （），兼顾全局收敛稳定性与局部快速二阶收敛，是点云 Bundle Adjustment (BA) 与非线性拟合的工业级标准工具。
五、 总结与方法选型对比
| 拟合方法 | 计算方式 | 能否处理离群点 (Outliers) | 计算开销 | 推荐应用场景 |
|---|---|---|---|---|
| 齐次线性 LSQ | 特征值分解 闭式解 | 否（极度敏感） | 极低 | 无噪/极少噪点的平面/直线拟合 |
| 非齐次伪逆 LSQ | 闭式解 | 否 | 极低 | 标准线性回归 |
| 非线性鲁棒 LSQ | LM 算法 / 雅可比迭代 | 是（支持少量离群点） | 中等 | 包含少许噪声的球面/复杂曲面拟合 |