Part I · 第 6 章 优化 · 6.9

非线性最小二乘与因子图

机器人从不直接"看到"自己的状态：它只有测距、里程计、像素这些间接的、带噪声的测量。把每条测量写成"预测与实际之差"的残差，再让所有残差的平方和最小，就是非线性最小二乘。把"哪条测量涉及哪些变量"画成图，就得到因子图：SLAM、光束法平差、相机标定背后共同的结构，而它的稀疏性正是能解大问题的关键。

约 30 分钟 前置：1.13 最小二乘 · 3.3 雅可比 · 6.6 Newton、Gauss-Newton 与 LM 4 个交互

    1为什么机器人需要它

    想知道自己在哪，机器人手里只有间接的线索：到几个基站的距离、轮子转了多少圈、相机里某个角点出现在第几行第几列像素。这些测量 \(\mathbf z\) 与我们想要的状态 \(\x\) 之间隔着一个非线性的测量函数 \(h(\x)\)：距离要开平方根，像素要除以深度，旋转要三角函数。而且每条测量都带噪声，彼此还未必一致。

    在高斯噪声的假设下（4.6 节），最合理的状态估计就是让"预测 \(h(\x)\) 与实际测量 \(\mathbf z\) 的差距（残差）"总体最小的那个 \(\x\)，也就是最大似然估计，它正好等价于残差平方和最小。1.13 节的最小二乘是 \(h\) 为线性时的特例；一旦 \(h\) 非线性，我们就需要迭代：每步把 \(h\) 在当前点线性化，解一次线性最小二乘（6.6 节的 Gauss-Newton），再重复。

    一个机器人建图 SLAM 问题可以有几十万个位姿和路标变量、几百万条测量。如果把它当成一团稠密的矩阵来解，一辈子也算不完。好在每条测量只涉及两三个变量，因子图把这件事画了出来，也指明了怎样利用它：矩阵几乎全是零。

    2直觉：残差、线性化、再来一次

    最简单的例子是测距定位：平面上有几个已知位置的基站 \(\mathbf b_i\)，机器人测得到每个基站的距离 \(r_i\)，要估计自己的位置 \(\x\in\R^2\)。第 \(i\) 条测量的残差是

    $$ \hl{res}{e_i(\x)} = \|\x-\mathbf b_i\| - r_i, \qquad \text{minimize}\quad f(\x)=\tfrac12\sum_i e_i(\x)^2 $$

    几何上，每条测量是一个以基站为圆心、半径 \(r_i\) 的圆，机器人应该在圆上。没有噪声时三个圆交于一点；有噪声时它们不会交于一点，\(f\) 的最低点是"离所有圆总体最近"的位置。残差对位置的导数是一个单位向量（从基站指向机器人），它就是雅可比 \(J\) 的第 \(i\) 行。

    Gauss-Newton 的一步：把每个 \(e_i\) 在当前点一阶展开 \(e_i(\x+\boldsymbol\delta)\approx e_i+J_i\boldsymbol\delta\)，解这个线性最小二乘得到 \(\boldsymbol\delta\)，更新，重复。下图让 梯度下降、Gauss-Newton 和 Levenberg-Marquardt 三种方法从同一个起点同时出发。

    FIG 1测距定位：三种求解器的比赛
    \(\log_{10}(f-f_{\rm opt})\) 与迭代次数
    三角形是基站，细圆是测得的距离（带噪声），背景是代价 \(f\) 的等值线（越偏青色越低）。拖动空心圆设定三种方法共同的起点。红：梯度下降，黄：Gauss-Newton，蓝：LM（带自适应阻尼）。右图是各自离最优代价的距离（对数），Gauss-Newton 在最优点附近几乎是"二次收敛"。切到"两个基站"会出现镜像的两个最小值；起点落在两个基站的连线上时 \(J^\top J\) 奇异。
    试一试
    • 按"运行"：让 Gauss-Newton 在 6 步内收敛到最优，而 梯度下降 还没到。
    • 切到"两个基站"，把起点拖到两个基站的连线上（会自动吸附）：两个 \(J\) 行平行，\(J^\top J\) 奇异，Gauss-Newton 要么动不了、要么被甩到远处，而 LM 的阻尼让它仍然收敛。
    核心洞见

    Gauss-Newton 把非线性最小二乘变成一串线性最小二乘，每一步只用雅可比 \(J\)，不需要二阶导数。它在最优点附近（残差小）像 Newton 法一样快；离得远时线性化不可信，需要 LM 的阻尼把它拉回"小步、近似梯度下降"的稳妥模式。

    3从许多残差到因子图

    现实问题的变量很多：一串位姿 \(\x_0,\dots,\x_n\)、一批路标。而每条测量只"认识"其中两三个：里程计连接相邻两个位姿，回环连接两个相隔很远的位姿，一次观测连接一个位姿和一个路标。把变量画成圆点，把测量（因子）画成小方块，用线把它们连起来，就是因子图。

    它的数学含义很直接：总代价是各因子代价之和，\(f(\x)=\tfrac12\sum_k\|\mathbf r_k(\x_{S_k})\|^2_{\Sigma_k}\)，每个因子 \(k\) 只依赖一小撮变量 \(S_k\)。所以雅可比 \(J\) 的第 \(k\) 个块行只有 \(|S_k|\) 个非零块。正规方程的矩阵 \(H=J^\top J\)（也叫信息矩阵）在位置 \((a,b)\) 非零，当且仅当变量 \(a\)、\(b\) 共同出现在某个因子里：\(H\) 的非零图案就是由因子图导出的变量邻接图（两个变量共享某个因子就相连）。

    解 \(H\boldsymbol\delta=-J^\top\mathbf r\) 时通常用 Cholesky 分解 \(H=LL^\top\)。分解时消去一个变量，会让它所有邻居两两相连，这叫填充 (fill-in)。消元顺序决定了填充多少：下面先看一遍。

    FIG 2因子图、稀疏矩阵与填充
    消元顺序
    上：圆点是变量，小方块是因子，点击两个圆点可添加或删除它们之间的因子（0 号变量带一个先验因子，固定整体"锚点"）。下：从左到右依次是 \(J\)（每行一个因子）、\(H=J^\top J\) 的非零图案、按所选顺序做 Cholesky 后的 \(L\)；红色格子是填充。"路标"预设里 0 号变量是被所有位姿看到的路标，先验因子改挂在 1 号位姿上。
    • 选"路标"预设，顺序用"自然"：消去 0 号路标产生满填充。换成"最小度"，填充降为 0。
    • 选"链"预设，点击 0 号和 9 号变量添加一个回环因子，看 \(H\) 和 \(L\) 的变化。
    真实系统里

    光束法平差 (bundle adjustment) 的情形正好相反：路标有几百万个，每个只被少数几台相机看到，路标之间也从不共享因子。所以先消去路标（Schur 补 技巧）代价很小，剩下一个只含相机位姿、较稠密但很小的方程组。FIG 2 里一个路标被所有位姿看到，是这种顺序的最坏情形。GTSAM、g2o、Ceres 这些库用 AMD、COLAMD 之类的算法自动找低填充的顺序；iSAM2 则只在新测量到来时更新受影响的那一小部分，所以能实时增量式地做 SLAM。

    4再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的元素会被高亮（残差、雅可比方向）。

    问题与线性化

    把所有残差堆成一个向量 \(\mathbf r(\x)\in\R^m\)（\(m\) 条测量，每条先用噪声协方差 \(\Sigma_k\) 做"白化" \(\Sigma_k^{-1/2}\mathbf r_k\)，让每个分量的噪声都是单位方差）。问题是：

    $$ \min_{\x}\ f(\x)=\tfrac12\|\mathbf r(\x)\|^2 \qquad\text{linearize:}\quad \mathbf r(\x+\boldsymbol\delta)\approx \mathbf r+\hl{jac}{J}\,\boldsymbol\delta,\quad J=\frac{\partial\mathbf r}{\partial\x} $$

    代入后 \(\tfrac12\|\mathbf r+J\boldsymbol\delta\|^2\) 是 \(\boldsymbol\delta\) 的二次函数，令导数为零得到正规方程（1.13 节），这就是 Gauss-Newton 的一步。LM 在左边加了阻尼项：

    $$ \underbrace{J^\top J\,\boldsymbol\delta=-J^\top\mathbf r}_{\text{Gauss–Newton}} \qquad\qquad \underbrace{\big(J^\top J+\lambda\,D\big)\boldsymbol\delta=-J^\top\mathbf r}_{\text{Levenberg–Marquardt}},\quad D=\operatorname{diag}(J^\top J) $$

    \(\lambda\to 0\) 是 Gauss-Newton；\(\lambda\to\infty\) 时 \(\boldsymbol\delta\approx -D^{-1}J^\top\mathbf r/\lambda\)，是沿负梯度 \(-\nabla f=-J^\top\mathbf r\) 方向、按 \(D\) 逐坐标缩放的一小步，即梯度下降（6.3 节）。这里用的是 6.6 节提到的 Marquardt 原版缩放 \(D=\operatorname{diag}(J^\top J)\)，而不是 \(\lambda I\)。LM 的策略是：新点代价下降就接受并减小 \(\lambda\)（更信任线性化），否则拒绝并增大 \(\lambda\)（缩小步长）。FIG 1 里的蓝线就是这样。

    为什么可以丢掉二阶项？\(f\) 的真实 Hessian 是 \(J^\top J+\sum_i e_i\nabla^2 e_i\)。Gauss-Newton 省略了后一项。当残差 \(e_i\) 很小（模型拟合得好）或 \(h\) 接近线性时，这一项可忽略；所以它在好的问题上几乎和 Newton 法一样快，而不必求二阶导。

    位姿图的残差

    平面位姿 \(\x_i=(\mathbf t_i,\theta_i)\)。一条边 \((i,j)\) 带一个测量 \(\mathbf z_{ij}=(\Delta\mathbf t,\Delta\theta)\)：它说"在 \(i\) 的坐标系里看，\(j\) 在 \(\Delta\mathbf t\)，朝向差 \(\Delta\theta\)"。残差是预测的相对位姿与测量之差：

    $$ \mathbf r_{ij}=\begin{bmatrix} R(\theta_i)^\top(\mathbf t_j-\mathbf t_i)-\Delta\mathbf t \\ \operatorname{wrap}(\theta_j-\theta_i-\Delta\theta) \end{bmatrix} $$

    它只依赖 \(\x_i,\x_j\) 两个变量，所以雅可比只有两个 \(3\times3\) 块（FIG 3 里用的就是这些解析表达式，以 \(\mathbf d=\mathbf t_j-\mathbf t_i\)）：

    $$ \frac{\partial\mathbf r_{ij}}{\partial\x_i}=\begin{bmatrix} -R_i^\top & \tfrac{\partial R_i^\top}{\partial\theta_i}\mathbf d \\ \mathbf 0 & -1\end{bmatrix},\qquad \frac{\partial\mathbf r_{ij}}{\partial\x_j}=\begin{bmatrix} R_i^\top & \mathbf 0 \\ \mathbf 0 & 1\end{bmatrix} $$

    注意两件事。第一，整个图的代价在整体平移和旋转下不变，所以 \(H\) 是奇异的（有 3 维零空间）。实践中固定一个位姿（FIG 3 里的 0 号），或给它加一个先验因子，这叫规范固定 (gauge fixing)。第二，在 3D 里位姿住在 \(\SE\) 上，不能直接做 \(\x\leftarrow\x+\boldsymbol\delta\)，要用 \(\Exp\) 回到流形上，这是 6.10 节的主题。

    鲁棒核

    平方损失对错误测量（外点）极其敏感：偏离 \(r\) 倍，代价涨 \(r^2\) 倍。把 \(\tfrac12 r^2\) 换成增长更慢的 \(\rho(r)\)（如 Huber：\(|r|\le\delta\) 时 \(\tfrac12 r^2\)，否则 \(\delta(|r|-\tfrac12\delta)\)），就削弱了外点的影响。求解用迭代重加权最小二乘 (IRLS)：每轮给第 \(k\) 条残差一个权重

    $$ w_k=\frac{\rho'(r_k)}{r_k}\ \ \Big(\text{Huber: } w=\min\big(1,\tfrac{\delta}{|r|}\big)\Big),\qquad \text{then solve}\quad \big(J^\top W J\big)\boldsymbol\delta=-J^\top W\mathbf r $$

    外点残差大，权重小，于是在加权的正规方程里影响变小。FIG 3、FIG 4 都用了它。

    5机器人中的它：位姿图 SLAM 与回环

    机器人绕一圈回到起点。里程计（轮速或视觉里程计）每一步都有一点点误差，累积起来，推算出的轨迹是一条没闭合的"螺旋"：这叫漂移。当机器人认出"这里我来过"（回环检测），就得到一条连接起点和终点的额外测量。位姿图优化把每个里程计测量和每个回环测量当作因子，同时调整所有位姿，使所有残差的平方和最小，漂移就被均匀地"摊"到整条轨迹上。

    下图中的机器人沿圆走了 24 个位姿。里程计带噪声和系统性偏航漂移，初始估计（蓝）是纯里程计累加。点击两个位姿可以在它们之间加一条回环约束；"迭代"按下后做真正的 Gauss-Newton（带极小阻尼）；右边是 \(H\) 的块稀疏图案：每个蓝格是一个里程计因子，紫格是回环因子（错误回环为红格），回环给对角带外面添了"远距离"的非零块。

    FIG 3位姿图 SLAM：回环把漂移拉回来
    \(H=J^\top\Omega J\) 的块图案（24×24 个块）
    虚线灰圆是真实轨迹（仅玩具问题里才知道）。蓝线是当前估计。回环约束画成直线，颜色随其白化残差变化（绿：满足，黄：中等，红：残差大），错误回环用虚线。"加入回环"用真实相对位姿加小噪声；"加入错误回环"模拟误识别：它声称第 5 号和第 17 号位姿几乎在同一个位置。切换 Huber 核后重新迭代，看错误回环的影响如何被压低。
    • 自己加一条回环：先点击一个位姿，再点击另一个相隔较远的位姿。
    • 加入真实回环并迭代，让估计的均方根误差降到 0.35 以下。
    • 先加入真实回环，再加入错误回环，迭代：轨迹被扭曲。打开 Huber 核后重新开始并迭代，把均方根误差压到 0.8 以下。

    另一个小实验：外点与鲁棒核

    鲁棒核的作用最容易在拟合一条直线时看清。下面是 10 个点，其中一个是外点。拖动任意点。红线是普通最小二乘，蓝线是 IRLS（每个点上的竖线粗细表示它被分到的权重）。

    FIG 4直线拟合：L2 与鲁棒核
    真实直线是 \(y=0.5x+1\)（灰色虚线）。IRLS 先从 L2 解出发，然后做 30 轮重加权。阈值 \(\delta\)（Cauchy 里是尺度 \(c\)）越小，对外点越不敏感，但对正常点的噪声也越"偏心"。
    • 把靠近一端的点拖到离直线很远的地方（例如把最右边的点拖到底部）：红线被明显拽歪，蓝线几乎不动。
    真实系统里

    视觉 SLAM 的后端（如 ORB-SLAM 的局部光束法平差、VINS-Fusion 的滑窗优化）都是这类问题：变量是相机位姿和三维路标点，残差是重投影误差（预测像素减观测像素）。ORB-SLAM 用 g2o 的 Levenberg-Marquardt 并加 Huber 核抵御误匹配；VINS-Fusion 用 Ceres 和一个鲁棒损失求解它的滑窗问题。激光 SLAM 的位姿图（如 Cartographer、LIO-SAM 的回环优化）与本节玩具几乎一样，只是在 \(\SE\) 上。工具箱有 GTSAM、g2o、Ceres Solver 和 SymForce。

    6常见误区

    误区 1：初值不好，却以为是算法不行

    非线性最小二乘的代价通常不是凸的：FIG 1 里"两个基站"就有两个镜像的最小值，GN 落进哪个由起点决定。SLAM 的初值来自里程计累加，一般还可以；光束法平差的初值则要靠三角化、PnP 等几何方法先求出来（第 III 部分）。Gauss-Newton 和 LM 都只保证找到局部最小值。

    误区 2：忘了白化，把不同单位的残差直接相加

    位姿图里的位置残差用米，角度残差用弧度，重投影误差用像素。如果直接平方相加，数值大的残差会淹没其他的。正确做法是除以各自噪声的标准差（乘以 \(\Sigma^{-1/2}\)），使每个残差无量纲且方差为 1。这样"1"就意味着"1 个 \(\sigma\)"，鲁棒核的阈值 \(\delta\) 才有统一的含义。

    误区 3：角度残差没有"卷绕"

    \(\theta_j-\theta_i-\Delta\theta\) 如果得到 \(2\pi-0.01\)，其实只差 \(-0.01\) 弧度。忘了把角度残差卷绕到 \((-\pi,\pi]\)，优化器会看到一个巨大的残差，拼命把某个位姿转一圈。更一般地，在流形上（\(\SO\)、\(\SE\)）残差和更新都应该用 \(\Log\) 和 \(\Exp\) 来定义（5.6、6.10 节）。

    误区 4：一个错误回环毁掉整张地图

    FIG 3 已经演示：错误回环把轨迹硬拉到一起。鲁棒核只能减弱，不能消除。真实系统会在加入回环前做几何验证（如 RANSAC、一致性检查 PCM），或使用可以"关掉"因子的方法（switchable constraints、GNC 的逐步非凸化）。把鲁棒核当作最后一道防线，不要当第一道。

    7小练习

    1. 机器人估计位置在原点 \((0,0)\)，基站在 \((3,4)\)，测得距离 4.6。残差 \(e=\|\x-\mathbf b\|-r\) 与雅可比行 \(J=\partial e/\partial\x\) 是？
    \(\|\x-\mathbf b\|=5\)，所以 \(e=5-4.6=0.4\)；\(J=(\x-\mathbf b)/\|\x-\mathbf b\|=(-3,-4)/5=(-0.6,-0.8)\)，是一个单位向量。Gauss-Newton 一步 \(\boldsymbol\delta=-J^\top e/(JJ^\top)=(0.24,0.32)\)，朝基站移动 0.4，正好消掉残差。
    2. Levenberg-Marquardt 的阻尼 \(\lambda\) 取得很大时，一步的行为接近？
    \((J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r\)，\(\lambda\) 很大时左边约为 \(\lambda D\boldsymbol\delta\)，所以 \(\boldsymbol\delta\approx-D^{-1}J^\top\mathbf r/\lambda\)：沿梯度方向（按 \(D\) 缩放）的一小步。\(\lambda\to0\) 才是 Gauss-Newton。
    3. 位姿图的 \(H=J^\top\Omega J\) 为什么是稀疏的？
    第 \(k\) 个因子的雅可比只在它涉及的变量的列上非零，\(J_k^\top\Omega_kJ_k\) 因此只在这些变量对应的块上非零。\(n\) 个位姿时只有 \(O(n)\) 个非零块，而不是 \(n^2\)。
    4. 一个只有相对位姿测量（里程计和回环）的位姿图，为什么必须固定一个位姿或加先验？
    相对测量对"全局坐标系放在哪里"毫无感知，所以有 3 个自由度（2 个平移、1 个旋转）的规范自由。固定 0 号位姿（或加先验）就选定了坐标系。
    5. 在鲁棒（Huber）最小二乘里，一个残差特别大的外点最终会得到什么样的权重？
    Huber 的权重 \(w=\min(1,\delta/|r|)\)：残差越大权重越小，但不会降到恰好 0（那是 Tukey 等"再下降"核的性质）。所以 Huber 只削弱而不彻底消除外点的影响。

    8延伸阅读

    • F. Dellaert & M. Kaess, Factor Graphs for Robot Perception, Foundations and Trends in Robotics, 2017 — 因子图、稀疏消元与 SLAM 的权威综述
    • T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017 — 从高斯估计到李群上的批量非线性最小二乘
    • J. Nocedal & S. J. Wright, Numerical Optimization, 2nd ed., Springer, 2006, Ch. 10 — 非线性最小二乘：Gauss-Newton 与 Levenberg-Marquardt
    • G. Grisetti, R. Kümmerle, C. Stachniss & W. Burgard, A Tutorial on Graph-Based SLAM, IEEE Intelligent Transportation Systems Magazine, 2010 — 本节位姿图残差与雅可比的推导来源
    • B. Triggs, P. McLauchlan, R. Hartley & A. Fitzgibbon, Bundle Adjustment: A Modern Synthesis, Vision Algorithms: Theory and Practice, 2000 — 光束法平差、Schur 补与鲁棒核