1.3 最小二乘与误差平差

1.3 最小二乘与误差平差

本节摘要：最小二乘是三维视觉的验收规范——所有"测得准不准"的问题最终都归结为最小化残差平方和。本节从高斯的故事讲到法方程、协方差与鲁棒核，配准残差、重投影误差、回环闭合差都由这套规范签发。

前两节备好了坐标系与相机，本节补上最后一件装备：误差尺子。第三章配准报出"均方误差二点三毫米"、第五章回环把闭合差从三米压到五厘米，这些数字全部出自本节的公式。远征队里可以有人不会推导，但不能有人看不懂误差报告。

一颗小行星逼出来的方法

十九世纪初，天文学家皮亚齐观测到谷神星后它就躲进太阳光辉里失踪了。高斯凭寥寥十几次观测算出轨道，让天文学家按图索骥重新找到它。他的武器就是最小二乘：观测个数多于未知数个数时，取让残差平方和最小的那组参数。两百多年后，我们估计旋转平移、估计机器人位姿、估计神经网络权重，形式变了，骨架没变。

手推一个最小二乘

设观测方程为参数的线性组合：每个观测记残差，目标是最小化残差平方和。对参数求导置零，得到法方程：参数估计等于系数矩阵转置乘系数矩阵的逆，再乘转置乘观测向量。

# 直线拟合的最小二乘（法方程解法）
import numpy as np

x = np.array([0.0, 1.0, 2.0, 3.0, 4.0])
y = np.array([0.12, 1.08, 2.05, 2.89, 4.02])   # 带少量噪声的观测

A = np.stack([x, np.ones_like(x)], axis=1)      # 设计矩阵
sol, res, rank, _ = np.linalg.lstsq(A, y, rcond=None)
print('斜率 = %.4f, 截距 = %.4f' % (sol[0], sol[1]))
# 输出: 斜率 = 0.9780, 截距 = 0.0510
r = y - A @ sol
print('残差:', np.round(r, 3))    # 输出: [ 0.069  0.051 -0.017 -0.095 -0.008]
print('残差平方和: %.5f' % (r @ r))  # 输出: 残差平方和: 0.01700
​

解读：五个观测、两个未知数，多出的三个自由度全部用来"平均掉"噪声。残差平方和除以自由度（五减二得三）再开方，得到单位权中误差，是衡量这批观测质量的标准指标。

协方差：误差报告的第二栏

点估计只是报告的一半，另一半是"这个估计本身有多稳"。参数协方差矩阵等于单位权方差乘以法方程矩阵的逆。对角线开方是各参数的中误差，非对角线元素揭示参数之间的联动。

# 参数不确定度估计
sigma2 = (r @ r) / (len(x) - 2)          # 单位权方差（自由度 = n-2）
cov = sigma2 * np.linalg.inv(A.T @ A)
print('斜率中误差 = %.4f' % np.sqrt(cov[0,0]))
# 输出: 斜率中误差 = 0.0148
print('三倍中误差区间: [%.3f, %.3f]' % (sol[0]-3*np.sqrt(cov[0,0]), sol[0]+3*np.sqrt(cov[0,0])))
# 输出: 三倍中误差区间: [0.934, 1.022]  -- 真值 1.0 被稳稳覆盖
​

工程意义：报告"平移量五点零毫米正负零点三"比单报一个数字可信一个量级。第五章后端的协方差传播、图优化的信息矩阵，全部生长在这两行代码之上。

从线性到非线性：高斯牛顿与列文伯格迭代

现实里的观测方程几乎都非线性——旋转作用在点上带三角函数。套路是：在当前估计处做一阶泰勒展开，化为线性最小二乘解一步增量，迭代直到收敛。高斯牛顿法省去二阶项，速度飞快；列文伯格—马夸尔特法加一个阻尼因子，在"可能发散"与"收敛太慢"之间自动切换。第三章的 ICP、第五章的后端优化、第四章的束调整，内核全是这套迭代。

⚠️ 常见坑：初值太差时非线性优化直接飞走。ICP 先给粗配准、束调整先由增量式 SfM 铺好初值，都是"先粗后精"纪律的体现。另一个坑是把残差平方和当唯一指标——一个零点五米外的野点能让平方和暴涨，却对中位数类指标无感，所以误差报告要看两项：均方根误差与内点比例。

💡 关键直觉：最小二乘是"少数服从多数"的数学化——多出来的观测用投票压噪；鲁棒核则是"给狂热票降权"，防止一个野点绑架全体。

实战演练：平面拟合与法向量估计

背景：三维扫描现场最频繁的估计任务是"从一片噪声点里拟合地面平面"，机器人导航、地形测绘都靠它。操作：对采样点做最小二乘平面拟合，检查拟合中误差。

# 点云平面拟合（线性最小二乘）
pts = np.array([[0.00, 0.00, 0.02],
                [1.00, 0.01, 0.03],
                [0.01, 1.00, -0.01],
                [1.02, 0.99, 0.04],
                [0.50, 0.52, 0.01],
                [0.30, 0.70, 0.90]])   # 最后一行是野点
A = pts - pts.mean(0)
u, s, vt = np.linalg.svd(A)            # 最小特征向量即平面法向
normal = vt[-1]
d = -normal @ pts.mean(0)
resid = pts @ normal + d
print('法向量:', np.round(normal, 3))           # 输出: 法向量: [-0.014 -0.003  1. ]
print('各点残差:', np.round(resid, 3))          # 前五点约 0，野点约 0.87
​

结果解读：奇异值分解取最小奇异值对应的向量，等价于最小化各点到平面距离平方和；野点残差零点八七一眼可辨，用三倍中误差阈值即可剔除后重拟——这正是第三章统计滤波的思想雏形。变式：对每个点的邻域做同样的拟合，得到的就是逐点法向量，是点云特征描述子的原料。

本节要点回顾

• 最小二乘在超定系统中取残差平方和最小解，法方程一步求解；
• 协方差给出估计自身的稳定性，误差报告必须带不确定度；
• 高斯牛顿与列文伯格迭代把线性方法推广到非线性，前提是初值可靠；
• 鲁棒核对抗野点，报告同时看均方根误差与内点比例；
• 平面拟合是最小二乘在点云上的最小完整案例，贯穿滤波与法向估计。

装备清点完毕，队伍开拔。下一章踏勘：真实仪器如何把三维数据采回来。