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

最小二乘问题详解22抗差估计与增量式SFM的工程稳健实现大家好欢迎来到《最小二乘问题详解》系列的第22篇。前几篇我们聊了标准最小二乘LS的推导、QR分解、Cholesky分解还有非线性优化的高斯牛顿和LM算法。但今天这篇我们要聊点“接地气”的——工程中到底怎么让最小二乘在真实数据上不崩。真实数据里没有“干净”的高斯噪声只有各种野值outlier、错误匹配、遮挡、传感器跳变。如果你直接拿标准最小二乘去优化一个野值就能把你的解拉飞。所以今天我们要讲两个核心主题1.抗差估计Robust Estimation—— 怎么让误差函数对野值不敏感。2.增量式SFMIncremental 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, sizex.shape)# 人为加入野值10个点被严重污染outlier_idx np.random.choice(len(x), 10, replaceFalse)y[outlier_idx] np.random.normal(0, 20, size10)# 标准最小二乘def ls_cost(params): a, b params return np.sum((y - (a*x b))**2)# Huber损失delta1.0def huber_loss(r, delta1.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], methodBFGS)res_huber minimize(huber_cost, [0, 0], methodBFGS)print(f标准LS: a{res_ls.x[0]:.3f}, b{res_ls.x[1]:.3f})print(fHuber: a{res_huber.x[0]:.3f}, b{res_huber.x[1]:.3f})print(f真实值: a{true_a}, b{true_b})# 画图plt.scatter(x, y, alpha0.6, labeldata)plt.plot(x, true_a*x true_b, k--, labeltruth)plt.plot(x, res_ls.x[0]*x res_ls.x[1], r-, labelLS)plt.plot(x, res_huber.x[0]*x res_huber.x[1], g-, labelHuber)plt.legend()plt.show()运行结果你会发现标准LS的拟合线被野值拉得歪七扭八而Huber几乎完美恢复了真实直线。这就是抗差估计的威力。关键点抗差估计的优化目标不再是二次函数通常我们用迭代重加权最小二乘IRLS来求解因为Huber等损失可以转化为权重加权的LS问题。—## 三、增量式SFM从“全局”到“增量”SFMStructure from Motion是从多张二维图像恢复三维结构和相机位姿的问题。传统做法是全局优化Bundle Adjustment把所有点、所有相机一起丢进一个巨大的最小二乘问题里。但工程上当图像数量到几千张时全局BA的计算量会爆炸。于是有了增量式SFM每次加入一张新图像只优化局部相关的变量而不是全部重来。经典的流程是1.初始化选两帧有足够匹配的图像做基础矩阵或本质矩阵估计得到初始相机位姿和三角化点。2.加入新帧用PnPPerspective-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) # 内部用RANSACHuber 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, losshuber, f_scale1.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的losshuber。-增量式SFM不是一次性解全局而是先初始化两帧然后一帧帧加入每步做局部BA定期再做全局BA。既保证了计算效率也通过抗差损失保证了稳健性。工程和理论的差距往往就体现在这些“脏活累活”上。理解了抗差和增量策略你才能真正把最小二乘用在真实场景里——无论是SLAM、三维重建还是标定问题。下一期我们可以聊聊鲁棒核函数在大规模BA中的稀疏求解加速或者位姿图优化的抗差方法。有想听的话题欢迎评论区留言。我们下期见