Part I · 第 6 章优化 · 6.9Part I · Ch. 6 Optimization · 6.9

非线性最小二乘与因子图Nonlinear Least Squares & Factor Graphs

机器人从不直接"看到"自己的状态：它只有测距、里程计、像素这些间接的、带噪声的测量。把每条测量写成"预测与实际之差"的残差，再让所有残差的平方和最小，就是非线性最小二乘。把"哪条测量涉及哪些变量"画成图，就得到因子图：SLAM、光束法平差、相机标定背后共同的结构，而它的稀疏性正是能解大问题的关键。 A robot never "sees" its state directly. It only has indirect, noisy measurements: ranges, odometry, pixels. Write each measurement as a residual, "prediction minus observation", and minimize the sum of squared residuals: that is nonlinear least squares. Draw which measurement touches which variables and you get a factor graph, the structure shared by SLAM, bundle adjustment and calibration, whose sparsity is what lets us solve huge problems.

约 30 分钟~30 min 前置：1.13 最小二乘 · 3.3 雅可比 · 6.6 Newton、Gauss-Newton 与 LMPrereqs: 1.13 Least squares · 3.3 Jacobian · 6.6 Newton, Gauss-Newton & LM 4 个交互4 interactives

1为什么机器人需要它Why robots need this

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

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

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

To know where it is, a robot has only indirect clues: distances to a few beacons, how many times the wheels turned, which pixel row and column a corner lands on. Between the measurements $\mathbf z$ and the state $\x$ we want sits a nonlinear measurement function $h(\x)$: ranges need square roots, pixels need a division by depth, rotations need trigonometry. And every measurement is noisy and they need not agree with each other.

Under Gaussian noise (Lesson 4.6) the most reasonable estimate is the $\x$ that makes the gap between prediction $h(\x)$ and observation $\mathbf z$ (the residual) small overall: the maximum-likelihood estimate, which is exactly the minimizer of the sum of squared residuals. The least squares of Lesson 1.13 is the special case where $h$ is linear; once $h$ is nonlinear we iterate: linearize $h$ at the current point, solve a linear least-squares problem (the Gauss-Newton of Lesson 6.6), and repeat.

A robot-mapping SLAM problem can have hundreds of thousands of pose and landmark variables and millions of measurements. Treated as one dense matrix it would never finish. Happily each measurement involves only two or three variables; a factor graph draws that fact and shows how to exploit it: the matrices are almost all zeros.

2直觉：残差、线性化、再来一次Intuition: residuals, linearize, repeat

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

The simplest example is range-based localization: a few beacons at known positions $\mathbf b_i$ in the plane, a robot that measures its distance $r_i$ to each, and an unknown position $\x\in\R^2$. The residual of measurement $i$ is

$$ \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 三种方法从同一个起点同时出发。

Geometrically each measurement is a circle of radius $r_i$ around its beacon, and the robot should be on it. Without noise, three circles meet at a point; with noise they do not, and the minimum of $f$ is the spot "closest to all circles at once". The derivative of a residual with respect to position is a unit vector (pointing from the beacon to the robot): it is row $i$ of the Jacobian $J$.

One Gauss-Newton step: expand each $e_i$ to first order, $e_i(\x+\boldsymbol\delta)\approx e_i+J_i\boldsymbol\delta$, solve that linear least-squares problem for $\boldsymbol\delta$, update, repeat. Below, gradient descent, Gauss-Newton and Levenberg-Marquardt all start from the same point.

FIG 1测距定位：三种求解器的比赛Range localization: three solvers race

$\log_{10}(f-f_{\rm opt})$ 与迭代次数$\log_{10}(f-f_{\rm opt})$ vs. iteration

三角形是基站，细圆是测得的距离（带噪声），背景是代价 $f$ 的等值线（越偏青色越低）。拖动空心圆设定三种方法共同的起点。红：梯度下降，黄：Gauss-Newton，蓝：LM（带自适应阻尼）。右图是各自离最优代价的距离（对数），Gauss-Newton 在最优点附近几乎是"二次收敛"。切到"两个基站"会出现镜像的两个最小值；起点落在两个基站的连线上时 $J^\top J$ 奇异。 Triangles are beacons, thin circles the measured ranges (noisy), and the background shows contours of the cost $f$ (more cyan = lower). Drag the hollow circle to set the common start of all three. Red: gradient descent, amber: Gauss-Newton, blue: LM (with adaptive damping). The right plot is each one's distance from the optimal cost (log scale); Gauss-Newton is close to "quadratically convergent" near the optimum. Switch to "2 beacons" for two mirror-image minima; starting on the line through both beacons makes $J^\top J$ singular.

试一试Try this

按"运行"：让 Gauss-Newton 在 6 步内收敛到最优，而梯度下降还没到。Press "Run": see Gauss-Newton converge within 6 steps while gradient descent has not arrived.
切到"两个基站"，把起点拖到两个基站的连线上（会自动吸附）：两个 $J$ 行平行，$J^\top J$ 奇异，Gauss-Newton 要么动不了、要么被甩到远处，而 LM 的阻尼让它仍然收敛。Switch to "2 beacons" and drag the start onto the line through both beacons (it snaps): the two $J$ rows become parallel, $J^\top J$ is singular, and Gauss-Newton either cannot move or is flung far away, while LM's damping lets it converge anyway.

核心洞见Key insight

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

Gauss-Newton turns a nonlinear least-squares problem into a sequence of linear least-squares problems; each step needs only the Jacobian $J$, no second derivatives. Near the optimum (small residuals) it is as fast as Newton's method; far away the linearization cannot be trusted, and LM's damping pulls it back to the safe mode of "small steps, roughly gradient descent".

3从许多残差到因子图From many residuals to a factor graph

现实问题的变量很多：一串位姿 $\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)。消元顺序决定了填充多少：下面先看一遍。

Real problems have many variables: a trail of poses $\x_0,\dots,\x_n$, a set of landmarks. Each measurement "knows" only two or three of them: odometry links two consecutive poses, a loop closure links two far-apart poses, an observation links a pose and a landmark. Draw variables as dots, measurements (factors) as little squares, and connect them: that is a factor graph.

Its mathematical meaning is plain: the total cost is a sum of factor costs, $f(\x)=\tfrac12\sum_k\|\mathbf r_k(\x_{S_k})\|^2_{\Sigma_k}$, each factor $k$ depending on a small set $S_k$ of variables. So block row $k$ of the Jacobian $J$ has only $|S_k|$ nonzero blocks. The normal-equation matrix $H=J^\top J$ (the information matrix) is nonzero at $(a,b)$ exactly when variables $a$ and $b$ appear together in some factor: the sparsity pattern of $H$ is the variable adjacency induced by the factor graph (two variables are linked when they share a factor).

To solve $H\boldsymbol\delta=-J^\top\mathbf r$ one usually takes a Cholesky factorization $H=LL^\top$. Eliminating a variable makes all its neighbours mutually adjacent, which is called fill-in. The elimination order decides how much fill-in there is. Look first.

FIG 2因子图、稀疏矩阵与填充Factor graph, sparse matrices and fill-in

消元顺序Elimination order

上：圆点是变量，小方块是因子，点击两个圆点可添加或删除它们之间的因子（0 号变量带一个先验因子，固定整体"锚点"）。下：从左到右依次是 $J$（每行一个因子）、$H=J^\top J$ 的非零图案、按所选顺序做 Cholesky 后的 $L$；红色格子是填充。"路标"预设里 0 号变量是被所有位姿看到的路标，先验因子改挂在 1 号位姿上。 Top: dots are variables, small squares are factors; click two dots to add or remove a factor between them (variable 0 carries a prior factor that pins down the global "anchor"). Bottom, left to right: $J$ (one row per factor), the nonzero pattern of $H=J^\top J$, and $L$ after Cholesky in the chosen order; red cells are fill-in. In the "Landmark" preset variable 0 is a landmark seen by every pose, and the prior sits on pose 1 instead.

选"路标"预设，顺序用"自然"：消去 0 号路标产生满填充。换成"最小度"，填充降为 0。Pick the "Landmark" preset with the "Natural" order: eliminating landmark 0 first creates dense fill-in. Switch to "Min-degree" and the fill-in drops to 0.
选"链"预设，点击 0 号和 9 号变量添加一个回环因子，看 $H$ 和 $L$ 的变化。Pick the "Chain" preset and click variables 0 and 9 to add a loop-closure factor; watch $H$ and $L$ change.

真实系统里In real systems

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

Bundle adjustment is the opposite case: there are millions of landmarks, each seen by only a few cameras, and landmarks never share a factor with each other. So eliminating the landmarks first (the Schur complement trick) is cheap and leaves a small, fairly dense system in the camera poses only. FIG 2's single landmark seen by every pose is the worst case for that order. Libraries such as GTSAM, g2o and Ceres find low-fill orders automatically with algorithms like AMD and COLAMD; iSAM2 updates only the small part affected when a new measurement arrives, which is why SLAM can run incrementally in real time.

4再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 1 中对应的元素会被高亮（残差、雅可比方向）。Tip: hover the highlighted symbols in the formulas; the matching elements in FIG 1 light up (the residuals, the Jacobian directions).

问题与线性化The problem and its linearization

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

Stack all residuals into one vector $\mathbf r(\x)\in\R^m$ ($m$ measurements, each first "whitened" by its noise covariance as $\Sigma_k^{-1/2}\mathbf r_k$ so every component has unit-variance noise). The problem is:

$$ \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 在左边加了阻尼项：

Substituting, $\tfrac12\|\mathbf r+J\boldsymbol\delta\|^2$ is quadratic in $\boldsymbol\delta$; setting its gradient to zero gives the normal equations (Lesson 1.13), which is one Gauss-Newton step. LM adds a damping term on the left:

$$ \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 里的蓝线就是这样。

$\lambda\to 0$ is Gauss-Newton; as $\lambda\to\infty$, $\boldsymbol\delta\approx -D^{-1}J^\top\mathbf r/\lambda$, a short step along the negative gradient $-\nabla f=-J^\top\mathbf r$, scaled per coordinate by $D$: gradient descent (Lesson 6.3). This lesson uses Marquardt's original scaling $D=\operatorname{diag}(J^\top J)$ mentioned in Lesson 6.6, not $\lambda I$. The LM strategy: if the new point lowers the cost, accept and decrease $\lambda$ (trust the linearization more); otherwise reject and increase $\lambda$ (shrink the step). That is what the blue curve in FIG 1 does.

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

Why can we drop second-order terms? The true Hessian of $f$ is $J^\top J+\sum_i e_i\nabla^2 e_i$. Gauss-Newton omits the second term. When residuals $e_i$ are small (the model fits well) or $h$ is nearly linear, it is negligible; so on good problems Gauss-Newton is nearly as fast as Newton without ever forming a second derivative.

位姿图的残差Residuals of a pose graph

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

Planar poses $\x_i=(\mathbf t_i,\theta_i)$. An edge $(i,j)$ carries a measurement $\mathbf z_{ij}=(\Delta\mathbf t,\Delta\theta)$: "seen from the frame of $i$, pose $j$ is at $\Delta\mathbf t$ with heading difference $\Delta\theta$". The residual is the predicted relative pose minus the measured one:

$$ \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$）：

It depends only on $\x_i$ and $\x_j$, so the Jacobian has just two $3\times3$ blocks (these are the analytic expressions used in FIG 3, with $\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 节的主题。

Two things to notice. First, the whole graph's cost is invariant under a global translation and rotation, so $H$ is singular (a 3-dimensional null space). In practice one pose is held fixed (pose 0 in FIG 3) or given a prior factor; this is called gauge fixing. Second, in 3D poses live on $\SE$, where $\x\leftarrow\x+\boldsymbol\delta$ is not allowed and one maps back with $\Exp$; that is the subject of Lesson 6.10.

鲁棒核Robust kernels

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

The squared loss is brutally sensitive to wrong measurements (outliers): an error $r$ times larger costs $r^2$ times more. Replace $\tfrac12 r^2$ with a slower-growing $\rho(r)$ (e.g. Huber: $\tfrac12 r^2$ for $|r|\le\delta$, else $\delta(|r|-\tfrac12\delta)$) to blunt their influence. Solve by iteratively reweighted least squares (IRLS): each round gives residual $k$ a weight

$$ 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 都用了它。

An outlier has a large residual and so a small weight, and counts for less in the weighted normal equations. FIG 3 and FIG 4 both use it.

5机器人中的它：位姿图 SLAM 与回环In robotics: pose-graph SLAM and loop closure

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

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

A robot drives around a loop and returns to its start. Odometry (wheel or visual) has a small error each step; accumulated, the dead-reckoned trajectory is an unclosed "spiral": drift. When the robot recognizes "I have been here before" (loop detection), it gains an extra measurement linking the start and the end. Pose-graph optimization treats every odometry and loop-closure measurement as a factor and adjusts all poses together so the sum of squared residuals is minimal; the drift gets spread evenly along the whole trajectory.

Below the robot visits 24 poses along a circle. Odometry has noise and a systematic heading drift, and the initial estimate (blue) is plain odometry accumulation. Click two poses to add a loop constraint between them; "Iterate" runs genuine Gauss-Newton (with a tiny damping); on the right is the block-sparse pattern of $H$: each blue cell is an odometry factor, purple cells are loop closures (red for the false closure), which add "long-range" nonzero blocks outside the diagonal band.

FIG 3位姿图 SLAM：回环把漂移拉回来Pose-graph SLAM: a loop closure pulls the drift back

$H=J^\top\Omega J$ 的块图案（24×24 个块）Block pattern of $H=J^\top\Omega J$ (24×24 blocks)

虚线灰圆是真实轨迹（仅玩具问题里才知道）。蓝线是当前估计。回环约束画成直线，颜色随其白化残差变化（绿：满足，黄：中等，红：残差大），错误回环用虚线。"加入回环"用真实相对位姿加小噪声；"加入错误回环"模拟误识别：它声称第 5 号和第 17 号位姿几乎在同一个位置。切换 Huber 核后重新迭代，看错误回环的影响如何被压低。 The dashed grey circle is the true trajectory (known only in this toy problem). The blue line is the current estimate. Loop constraints are drawn as lines colored by their whitened residual (green: satisfied, amber: moderate, red: large); a false closure is dashed. "Add loop closure" uses the true relative pose plus small noise; "Add false closure" simulates a misrecognition: it claims poses 5 and 17 are at nearly the same place. Switch on the Huber kernel and iterate again to see the false closure's influence suppressed.

自己加一条回环：先点击一个位姿，再点击另一个相隔较远的位姿。Add a loop yourself: click one pose, then another far from it.
加入真实回环并迭代，让估计的均方根误差降到 0.35 以下。Add a true loop closure and iterate until the RMS error drops below 0.35.
先加入真实回环，再加入错误回环，迭代：轨迹被扭曲。打开 Huber 核后重新开始并迭代，把均方根误差压到 0.8 以下。Add the true loop closure, then the false one, and iterate: the trajectory warps. Re-start with the Huber kernel on and iterate to bring the RMS error below 0.8.

另一个小实验：外点与鲁棒核A small experiment: outliers and robust kernels

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

The effect of a robust kernel is easiest to see when fitting a line. Below are 10 points, one of them an outlier. Drag any point. The red line is ordinary least squares; the blue line is IRLS (the thickness of the vertical segment at each point shows the weight it received).

FIG 4直线拟合：L2 与鲁棒核Line fit: L2 vs. a robust kernel

真实直线是 $y=0.5x+1$（灰色虚线）。IRLS 先从 L2 解出发，然后做 30 轮重加权。阈值 $\delta$（Cauchy 里是尺度 $c$）越小，对外点越不敏感，但对正常点的噪声也越"偏心"。 The true line is $y=0.5x+1$ (grey dashed). IRLS starts from the L2 solution and does 30 reweighting rounds. The smaller the threshold $\delta$ (the scale $c$ for Cauchy), the less sensitive it is to outliers, but the more it also discounts ordinary noise.

把靠近一端的点拖到离直线很远的地方（例如把最右边的点拖到底部）：红线被明显拽歪，蓝线几乎不动。Drag a point near one end far from the line (e.g. the rightmost point down to the bottom): the red line is clearly pulled away while the blue one barely moves.

真实系统里In real systems

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

The back ends of visual SLAM (local bundle adjustment in ORB-SLAM, the sliding-window optimization of VINS-Fusion) are exactly this kind of problem: variables are camera poses and 3D landmarks, residuals are reprojection errors (predicted pixel minus observed pixel). ORB-SLAM solves them with g2o's Levenberg-Marquardt and a Huber kernel against mismatches; VINS-Fusion solves its sliding-window problem with Ceres and a robust loss. The pose graphs of lidar SLAM (the loop-closure optimization of Cartographer or LIO-SAM) are nearly identical to this toy, only on $\SE$. The toolbox: GTSAM, g2o, Ceres Solver and SymForce.

6常见误区Common pitfalls

误区 1：初值不好，却以为是算法不行Pitfall 1: blaming the algorithm for a bad initial guess

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

The cost of a nonlinear least-squares problem is usually not convex: with "2 beacons" in FIG 1 there are two mirror minima, and which one GN lands in depends on the start. SLAM initializes from accumulated odometry, which is usually adequate; bundle adjustment needs initial values from geometric methods such as triangulation and PnP (Part III). Both Gauss-Newton and LM only promise a local minimum.

误区 2：忘了白化，把不同单位的残差直接相加Pitfall 2: forgetting to whiten, adding residuals of different units

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

In a pose graph, position residuals are in meters, angle residuals in radians, reprojection errors in pixels. Squaring and adding them raw lets the numerically large one drown the rest. The right way is to divide by each noise standard deviation (multiply by $\Sigma^{-1/2}$), making every residual dimensionless with unit variance. Then "1" means "one $\sigma$", and the threshold $\delta$ of a robust kernel has a common meaning.

误区 3：角度残差没有"卷绕"Pitfall 3: angle residuals without wrapping

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

If $\theta_j-\theta_i-\Delta\theta$ comes out as $2\pi-0.01$, the true discrepancy is only $-0.01$ rad. Forget to wrap the angle residual into $(-\pi,\pi]$ and the optimizer sees a huge residual and desperately spins some pose a full turn. More generally, on manifolds ($\SO$, $\SE$) both residuals and updates should be defined through $\Log$ and $\Exp$ (Lessons 5.6 and 6.10).

误区 4：一个错误回环毁掉整张地图Pitfall 4: one false loop closure ruins the whole map

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

FIG 3 showed it: a false loop closure drags the trajectory together by force. A robust kernel can weaken but not eliminate the damage. Real systems verify geometry before admitting a closure (RANSAC, pairwise-consistency checks) or use methods that can "switch off" factors (switchable constraints, graduated non-convexity). Treat the robust kernel as the last line of defense, not the first.

7小练习Check yourself

1. 机器人估计位置在原点 $(0,0)$，基站在 $(3,4)$，测得距离 4.6。残差 $e=\|\x-\mathbf b\|-r$ 与雅可比行 $J=\partial e/\partial\x$ 是？1. The robot's estimate is the origin $(0,0)$, a beacon sits at $(3,4)$, and the measured range is 4.6. What are the residual $e=\|\x-\mathbf b\|-r$ and the Jacobian row $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，正好消掉残差。$\|\x-\mathbf b\|=5$, so $e=5-4.6=0.4$; $J=(\x-\mathbf b)/\|\x-\mathbf b\|=(-3,-4)/5=(-0.6,-0.8)$, a unit vector. A Gauss-Newton step $\boldsymbol\delta=-J^\top e/(JJ^\top)=(0.24,0.32)$ moves 0.4 toward the beacon and removes the residual exactly.

2. Levenberg-Marquardt 的阻尼 $\lambda$ 取得很大时，一步的行为接近？2. When the Levenberg-Marquardt damping $\lambda$ is very large, a step behaves like?

$(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。With $(J^\top J+\lambda D)\boldsymbol\delta=-J^\top\mathbf r$ and large $\lambda$, the left side is about $\lambda D\boldsymbol\delta$, so $\boldsymbol\delta\approx-D^{-1}J^\top\mathbf r/\lambda$: a small step along the gradient (scaled by $D$). Gauss-Newton is the $\lambda\to0$ end.

3. 位姿图的 $H=J^\top\Omega J$ 为什么是稀疏的？3. Why is a pose graph's $H=J^\top\Omega J$ sparse?

第 $k$ 个因子的雅可比只在它涉及的变量的列上非零，$J_k^\top\Omega_kJ_k$ 因此只在这些变量对应的块上非零。$n$ 个位姿时只有 $O(n)$ 个非零块，而不是 $n^2$。The Jacobian of factor $k$ is nonzero only in the columns of its own variables, so $J_k^\top\Omega_kJ_k$ is nonzero only on the blocks of those variables. With $n$ poses there are $O(n)$ nonzero blocks, not $n^2$.

4. 一个只有相对位姿测量（里程计和回环）的位姿图，为什么必须固定一个位姿或加先验？4. A pose graph with only relative-pose measurements (odometry and loops) must fix one pose or add a prior. Why?

相对测量对"全局坐标系放在哪里"毫无感知，所以有 3 个自由度（2 个平移、1 个旋转）的规范自由。固定 0 号位姿（或加先验）就选定了坐标系。Relative measurements know nothing about where the global frame is placed, so there are 3 degrees of gauge freedom (two translations, one rotation). Fixing pose 0 (or adding a prior) chooses the frame.

5. 在鲁棒（Huber）最小二乘里，一个残差特别大的外点最终会得到什么样的权重？5. In robust (Huber) least squares, what weight does a gross outlier with a huge residual end up with?

Huber 的权重 $w=\min(1,\delta/|r|)$：残差越大权重越小，但不会降到恰好 0（那是 Tukey 等"再下降"核的性质）。所以 Huber 只削弱而不彻底消除外点的影响。Huber's weight is $w=\min(1,\delta/|r|)$: the larger the residual the smaller the weight, but never exactly 0 (that is a property of redescending kernels like Tukey's). So Huber blunts rather than removes an outlier's influence.

8延伸阅读Further reading

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