最小二乘问题详解22：抗差估计与增量式SFM的工程稳健实现

最小二乘问题详解22：抗差估计与增量式SFM的工程稳健实现

大家好，欢迎来到《最小二乘问题详解》系列的第22篇。前几篇我们聊了标准最小二乘（LS）的推导、QR分解、Cholesky分解，还有非线性优化的高斯牛顿和LM算法。但今天这篇，我们要聊点“接地气”的——工程中到底怎么让最小二乘在真实数据上不崩。真实数据里没有“干净”的高斯噪声，只有各种野值（outlier）、错误匹配、遮挡、传感器跳变。如果你直接拿标准最小二乘去优化，一个野值就能把你的解拉飞。所以，今天我们要讲两个核心主题：1.抗差估计（Robust Estimation）—— 怎么让误差函数对野值不敏感。2.增量式SFM（Incremental Structure from Motion）—— 在大规模三维重建里，怎么高效且稳健地不断加入新图像，而不是每次都从头解一遍。我会结合代码示例，尽量讲得通俗。—## 一、为什么标准最小二乘这么“脆”？先看一个最简单的线性回归问题。假设我们有数据点(x_i, y_i)，想拟合一条直线y = ax + b。标准最小二乘的目标是：minimize Σ (y_i - (a*x_i + b))²这个二次代价函数意味着误差越大，惩罚是平方增长的。如果某个点是个野值（比如传感器故障，y值偏离了10倍），它的残差平方会主导整个目标函数，最终把拟合线硬生生拉向它。这就是“脆”的根源：平方损失函数对长尾误差无约束。—## 二、抗差估计：让误差“饱和”抗差估计的核心思想是：替换损失函数，让大残差带来的惩罚不再无限增长，而是趋于饱和。常用的损失函数有：-Huber损失：小误差用平方，大误差用线性。-Cauchy损失：更平滑的饱和。-Tukey损失：超过阈值直接权重归零（完全丢弃野值）。我们来看一个简单的Python实现，对比标准最小二乘和Huber抗差估计的效果。pythonimport numpy as npimport matplotlib.pyplot as pltfrom scipy.optimize import minimize# 生成一些带野值的数据np.random.seed(42)x = np.linspace(0, 10, 50)true_a, true_b = 2.0, 1.0y = true_a * x + true_b + np.random.normal(0, 0.5, size=x.shape)# 人为加入野值（10个点被严重污染）outlier_idx = np.random.choice(len(x), 10, replace=False)y[outlier_idx] += np.random.normal(0, 20, size=10)# 标准最小二乘def ls_cost(params): a, b = params return np.sum((y - (a*x + b))**2)# Huber损失（delta=1.0）def huber_loss(r, delta=1.0): r = np.abs(r) return np.where(r <= delta, 0.5 * r**2, delta * (r - 0.5*delta))def huber_cost(params): a, b = params residual = y - (a*x + b) return np.sum(huber_loss(residual))# 优化res_ls = minimize(ls_cost, [0, 0], method='BFGS')res_huber = minimize(huber_cost, [0, 0], method='BFGS')print(f"标准LS: a={res_ls.x[0]:.3f}, b={res_ls.x[1]:.3f}")print(f"Huber: a={res_huber.x[0]:.3f}, b={res_huber.x[1]:.3f}")print(f"真实值: a={true_a}, b={true_b}")# 画图plt.scatter(x, y, alpha=0.6, label='data')plt.plot(x, true_a*x + true_b, 'k--', label='truth')plt.plot(x, res_ls.x[0]*x + res_ls.x[1], 'r-', label='LS')plt.plot(x, res_huber.x[0]*x + res_huber.x[1], 'g-', label='Huber')plt.legend()plt.show()运行结果你会发现，标准LS的拟合线被野值拉得歪七扭八，而Huber几乎完美恢复了真实直线。这就是抗差估计的威力。关键点：抗差估计的优化目标不再是二次函数，通常我们用迭代重加权最小二乘（IRLS）来求解，因为Huber等损失可以转化为权重加权的LS问题。—## 三、增量式SFM：从“全局”到“增量”SFM（Structure from Motion）是从多张二维图像恢复三维结构和相机位姿的问题。传统做法是全局优化（Bundle Adjustment），把所有点、所有相机一起丢进一个巨大的最小二乘问题里。但工程上，当图像数量到几千张时，全局BA的计算量会爆炸。于是有了增量式SFM：每次加入一张新图像，只优化局部相关的变量，而不是全部重来。经典的流程是：1.初始化：选两帧有足够匹配的图像，做基础矩阵或本质矩阵估计，得到初始相机位姿和三角化点。2.加入新帧：用PnP（Perspective-n-Point）估计新相机的位姿。3.三角化新点：新图像与已有图像匹配，三角化出新的三维点。4.局部BA：只优化与当前帧相关的相机和点，控制窗口大小。5.全局BA（定期触发）：当累积一定误差后，做一次全量优化。其中，抗差估计在每一步都至关重要——因为特征匹配不可避免会有错误匹配（野值）。比如在PnP中，我们用DLT或EPnP求初始解，然后用RANSAC剔除野值，再用抗差BA精化。下面是一个简化的增量式SFM核心流程伪代码（用Python描述，简化了相机模型）：pythonimport numpy as npfrom scipy.sparse import lil_matrixfrom scipy.optimize import least_squares# 假设我们有一系列相机位姿（简化：只存旋转和平移）class Camera: def __init__(self, R, t): self.R = R # 3x3 旋转 self.t = t # 3x1 平移# 模拟一个简单的增量式SFMdef incremental_sfm(frames, matches): # frames: list of dict, 每个包含2D特征点 # matches: 帧间匹配关系 cameras = [] points_3d = [] # 三维点列表 point_observations = [] # (cam_idx, point_idx, 2D坐标) # Step 1: 初始化前两帧 # 用基础矩阵 + 三角化（这里省略具体实现） R0, t0 = np.eye(3), np.zeros(3) R1, t1 = estimate_essential_matrix(frames[0], frames[1]) # 假设实现 cameras.append(Camera(R0, t0)) cameras.append(Camera(R1, t1)) # 三角化初始点 pts = triangulate_two_views(frames[0], frames[1], cameras[0], cameras[1]) points_3d.extend(pts) # Step 2: 增量加入后续帧 for i in range(2, len(frames)): # 用PnP估计新相机位姿 # 先找与已有3D点的匹配 correspondences = get_2d_3d_matches(frames[i], points_3d, matches) R_new, t_new = solve_pnp_robust(correspondences) # 内部用RANSAC+Huber cameras.append(Camera(R_new, t_new)) # 三角化新的3D点 new_pts = triangulate_with_previous(frames[i], frames[i-1], cameras[i], cameras[i-1]) points_3d.extend(new_pts) # 局部BA：只优化最近K帧和相关的3D点 if i % 5 == 0: local_bundle_adjustment(cameras[-5:], points_3d, point_observations) # Step 3: 最后全局BA global_bundle_adjustment(cameras, points_3d, point_observations) return cameras, points_3d# 实际BA中，我们会构造一个稀疏雅可比矩阵def bundle_adjustment(cameras, points_3d, observations): # 构造稀疏矩阵，使用scipy的least_squares # 这里只展示核心思想 def residual(params): # 重投影误差 errs = [] for obs in observations: cam_idx, pt_idx, (u, v) = obs R, t = cameras[cam_idx].R, cameras[cam_idx].t X = points_3d[pt_idx] proj = project(R, t, X) # 投影函数 errs.append(proj - (u, v)) return np.concatenate(errs) # 使用Huber损失的抗差BA res = least_squares(residual, initial_params, loss='huber', f_scale=1.0) return res.x这段代码省略了很多几何运算细节，但核心骨架就是：增量加入 → 局部BA → 定期全局BA，并且每一步都用抗差损失。—## 四、工程稳健性的一些“坑”和技巧在实际工程中，光有理论还不够，还得注意几个细节：1.阈值选择：Huber的delta、RANSAC的内点阈值，都需要根据图像噪声水平、特征匹配精度来调。太严则丢内点，太松则留野值。2.增量BA的窗口大小：窗口太小，误差会累积；窗口太大，计算量又上去了。通常经验值是5-10帧。3.相机退化：如果新图像与已有视图重叠太少，三角化出来的点会很差。这时候要检测“退化”情况，比如检查最小特征值，必要时拒绝加入该帧。4.浮点误差与归一化：在计算本质矩阵或PnP之前，要对2D坐标做归一化（centroid + scaling），否则数值不稳定。—## 五、总结今天我们讲了两个工程上不可或缺的“防弹衣”：-抗差估计：通过Huber、Cauchy等损失函数，让最小二乘对野值不敏感。核心实现是IRLS或直接调用scipy.optimize.least_squares的loss='huber'。-增量式SFM：不是一次性解全局，而是先初始化两帧，然后一帧帧加入，每步做局部BA，定期再做全局BA。既保证了计算效率，也通过抗差损失保证了稳健性。工程和理论的差距，往往就体现在这些“脏活累活”上。理解了抗差和增量策略，你才能真正把最小二乘用在真实场景里——无论是SLAM、三维重建还是标定问题。下一期我们可以聊聊鲁棒核函数在大规模BA中的稀疏求解加速，或者位姿图优化的抗差方法。有想听的话题欢迎评论区留言。我们下期见！