SLAM 与位姿图SLAM & Pose Graphs

把一台扫地机器人放进一套从没见过的公寓。它要规划清扫路线，就需要一张地图；它要把每一帧激光扫描拼进地图，又必须知道这一帧是在哪里扫的。4.8 节的 EKF 定位假设地图已知，12.1 节的 ICP 和 15.1 节的视觉惯性里程计只估计相邻两帧之间的相对运动。前者需要地图，后者没有地图也能工作，可是误差会一直累积：每一步的小误差叠加起来，走得越远偏得越多，而且永远不会自己变小。

SLAM（Simultaneous Localization And Mapping，同时定位与建图）的做法是把机器人的整条轨迹和地图当作同一组未知数一起估计。关键在于回环：机器人回到以前到过的地方并认出来时，就得到一条连接“现在”和“很久以前”的测量。这一条测量足以把一路积累的漂移拉回来，并把修正分摊到整条轨迹上。

现代 SLAM 几乎都写成6.9 节的因子图最小二乘：变量是一串机器人位姿，因子是里程计和回环。6.9 节的 FIG 3 已经在 24 个位姿的圆上演示过回环怎样修正漂移。本节再往前走四步：(1) 一个完整的系统，前端从真实的激光扫描里自己找回环（地点识别加 ICP 验证），后端在 \(\mathrm{SE}(2)\) 上优化位姿图；(2) 两种更早的方法 EKF-SLAM 和 FastSLAM，以及它们各自卡在哪里；(3) 错误回环和鲁棒核；(4) 增量求解：机器人每走一步，求解器到底要重算多少。

Put a robot vacuum into an apartment it has never seen. To plan a cleaning route it needs a map; to stitch each lidar scan into that map it must know where the scan was taken. The EKF localization of Lesson 4.8 assumes the map is known. The ICP of Lesson 12.1 and the visual-inertial odometry of Lesson 15.1 only estimate the motion between neighbouring frames. The first needs a map; the second works without one, but its errors keep adding up: small errors at every step accumulate, the further the robot drives the further off it is, and the error never shrinks on its own.

SLAM (simultaneous localization and mapping) estimates the robot's whole trajectory and the map together, as one set of unknowns. The key is the loop closure: when the robot comes back to a place it has visited and recognizes it, it gains a measurement linking "now" with "long ago". That single measurement is enough to pull back the drift accumulated on the way and to spread the correction over the whole trajectory.

Almost all modern SLAM is written as the factor-graph least squares of Lesson 6.9: the variables are a chain of robot poses, the factors are odometry and loop closures. FIG 3 of Lesson 6.9 already showed on a circle of 24 poses how a loop closure fixes drift. This lesson goes four steps further: (1) a complete system, whose front end finds loop closures by itself in real lidar scans (place recognition plus ICP verification) and whose back end optimizes the pose graph on \(\mathrm{SE}(2)\); (2) two older methods, EKF-SLAM and FastSLAM, and where each one gets stuck; (3) false loop closures and robust kernels; (4) incremental solving: how much the solver really has to recompute at every step.

下面是一个 24 m × 14 m 的室内环境：外墙、两个大的方块障碍，以及墙边一些小箱子。机器人沿“8”字形路线走一圈，共约 77 m，每走 0.8 m 记一帧 360° 激光扫描（180 束，测距噪声 2 cm），一共 92 帧。扫描本身很准，问题全出在“把扫描放到哪里”。如果用真实位姿拼，得到的地图和真实墙壁严丝合缝；如果只能用轮式里程计算出的位姿拼，每一帧都放在一个略有偏差的位置上。

这里的里程计有两类误差：随机噪声（每一步都不同），以及航向偏差：每走一米，航向多算一个固定的小角度，比如两个轮子的半径差了一点点就会这样（9.1 节）。航向误差最致命，因为它会被距离放大：航向偏了 \(\delta\theta\)，之后每走一米就多出约 \(\delta\theta\) 米的横向误差。

Below is a 24 m × 14 m indoor environment: outer walls, two large blocks, and a few small boxes along the walls. The robot drives a figure-eight route of about 77 m and records a 360° lidar scan (180 beams, 2 cm range noise) every 0.8 m, 92 scans in all. The scans themselves are accurate; the whole problem is where to put them. Stitched at the true poses, the map matches the real walls exactly; stitched at the poses computed from wheel odometry, every scan lands in a slightly wrong place.

The odometry here has two kinds of error: random noise (different at every step) and a heading bias: every metre driven adds a small fixed angle to the heading, as happens when the two wheel radii differ slightly (Lesson 9.1). Heading error is the most damaging, because distance amplifies it: a heading off by \(\delta\theta\) adds about \(\delta\theta\) metres of lateral error for every metre driven afterwards.

几个值得试的设置：把航向偏差和随机噪声都调成 0，地图和真实一模一样；只去掉噪声、保留 0.2°/m 的偏差，终点误差仍有 1.4 m：系统误差和随机误差一样有害。偏差调成负值，地图向另一侧弯。还要注意：误差并不是单调增长的，在“8”字路线上，往一个方向转弯造成的误差，有一部分会被反方向的转弯抵消，所以中途的最大误差比终点大。

Settings worth trying: set both the heading bias and the random noise to 0 and the map is perfect; remove only the noise but keep the 0.2°/m bias and the end error is still 1.4 m: systematic error hurts as much as random error. Make the bias negative and the map bends the other way. Notice too that the error does not grow monotonically: on a figure-eight, part of the error caused by turning one way is undone by turning the other way, which is why the worst error on the way is larger than the error at the end.

下图用的是 FIG 1 的同一个世界、同一条路线、同样的扫描，跑的是一个完整的、简化但不作弊的位姿图 SLAM。系统分两部分：

前端（从传感器数据里造出约束）：每 0.8 m 建一个关键帧，即一个位姿变量 \(X_k\)，并附上这一帧的扫描。相邻关键帧之间加一条里程计边，测量值是轮式里程计给出的相对位姿。然后做地点识别：在当前估计里，找出离新关键帧不到“搜索半径”、且至少早 20 帧的旧关键帧（最近的 2 个），把两帧扫描用 ICP 对齐。初值来自当前估计，并且再从 ±10° 的航向各试一次。对齐之后，用点到线距离检查：至少 75% 的点落在对方扫描 10 cm 以内、且这些点的 RMS 距离小于 5 cm，才接受为一条回环边，测量值就是 ICP 的结果。

后端（把所有约束一起满足）：每加入一条回环，就从当前估计出发做 Gauss-Newton（最多 10 次迭代，代价上升时加一点 LM 阻尼），把第一个位姿固定为规范（6.9 节）。新关键帧的初值，是把里程计接在上一个关键帧的当前估计后面得到的。地图不单独存储：每次都由“各关键帧的扫描放在各自的估计位姿上”重新画出来，所以位姿一被修正，地图也跟着修正。

The figure below uses the same world, route and scans as FIG 1 and runs a complete pose-graph SLAM, simplified but with no shortcuts. It has two parts:

The front end (turning sensor data into constraints): every 0.8 m it creates a keyframe, a pose variable \(X_k\) with that moment's scan attached. Consecutive keyframes get an odometry edge whose measurement is the relative pose reported by wheel odometry. Then comes place recognition: in the current estimate it looks for older keyframes (at least 20 frames back, the 2 nearest) within the "search radius" of the new keyframe and aligns the two scans with ICP, starting from the current estimate and also from headings turned by ±10°. After alignment a point-to-line check decides: only if at least 75% of the points lie within 10 cm of the other scan and their RMS distance is below 5 cm is the result accepted as a loop-closure edge, with the ICP result as its measurement.

The back end (satisfying all constraints together): every new loop closure triggers Gauss-Newton from the current estimate (at most 10 iterations, with a little LM damping if the cost rises), with the first pose held fixed as the gauge (Lesson 6.9). A new keyframe is initialized by attaching its odometry to the current estimate of the previous keyframe. The map is not stored separately: it is redrawn every time from "each keyframe's scan placed at that keyframe's estimated pose", so when the poses are corrected, the map is corrected with them.

位姿图是 2000 年代后期才成为主流的。在那之前，SLAM 主要按 4.7 节的贝叶斯滤波来做：只估计当前位姿和地图，旧位姿一到就“边缘化”掉。下面两个实验用同一个场景：18 个点状路标，机器人沿 14 m × 9 m 的椭圆以 1 m/s 逆时针走两圈（约 76 s）。传感器测量前方 ±90°、4.5 m 以内路标的距离和方位（噪声 0.1 m 和 2°），并且已知每次看到的是哪个路标（数据关联已知，和 4.8 节一样）；里程计速度噪声 0.1 m/s，角速度噪声 0.05 rad/s。

Pose graphs became mainstream only in the late 2000s. Before that, SLAM was mostly done as the Bayes filter of Lesson 4.7: estimate only the current pose and the map, and "marginalize" every old pose as soon as it is passed. The two experiments below share one scene: 18 point landmarks, and a robot driving counter-clockwise twice around a 14 m × 9 m ellipse at 1 m/s (about 76 s). The sensor measures range and bearing to landmarks within ±90° of the heading and 4.5 m (noise 0.1 m and 2°), and it knows which landmark it is seeing (known data association, as in Lesson 4.8); odometry has 0.1 m/s speed noise and 0.05 rad/s turn-rate noise.

EKF-SLAM 把 4.8 节的 EKF 定位直接推广：状态不只是机器人的 \((x,y,\theta)\)，而是把所有已看到的路标坐标都拼进去，\(\mathbf y=[\x;\mathbf m_1;\dots;\mathbf m_n]\in\R^{3+2n}\)，用一个均值和一个 \((3+2n)\times(3+2n)\) 的协方差 \(P\) 表示全部信念。第一次看到一个路标时，用测量反推它的位置，把状态扩维；以后每次再看到它，就做一次普通的 EKF 更新。精髓在协方差的非对角块 \(\hl{pxm}{P_{xm}}\)：路标是从一个有误差的位姿看到的，所以它的误差和机器人的误差相关，路标之间也因此相关。正因为有这些相关，机器人重新看到很早以前的路标时，修正会顺着 \(P\) 传给所有路标，包括此刻根本看不见的那些。

EKF-SLAM generalizes the EKF localization of Lesson 4.8 directly: the state is not just the robot's \((x,y,\theta)\) but also the coordinates of every landmark seen so far, \(\mathbf y=[\x;\mathbf m_1;\dots;\mathbf m_n]\in\R^{3+2n}\), and the whole belief is one mean and one \((3+2n)\times(3+2n)\) covariance \(P\). When a landmark is seen for the first time, its position is computed back from the measurement and the state is augmented; every later sighting is an ordinary EKF update. The essence is in the off-diagonal blocks \(\hl{pxm}{P_{xm}}\) of the covariance: a landmark is seen from an uncertain pose, so its error is correlated with the robot's, and through the robot the landmarks become correlated with each other. Because of these correlations, when the robot sees a landmark from long ago, the correction flows through \(P\) to every landmark, including those it cannot see right now.

保留相关性时，两圈后的路标 RMS 误差约 0.07 m，NEES 约 3：略高于理想的 2，这是 EKF-SLAM 已知的线性化不一致（见第 8 节）。代价是：每次更新都要改写整个 \(P\)，状态维数 \(N=3+2n\)，每次更新约 \(2N^2\) 次乘加，内存也是 \(N^2\)。18 个路标时 \(N=39\)，毫无压力；一张一万个路标的地图，每次更新就是 \(8\times10^8\) 次运算。这就是 EKF-SLAM 只能用于小地图的原因。

With the correlations kept, the landmark RMS error after two laps is about 0.07 m and the NEES about 3: a little above the ideal 2, the known linearization inconsistency of EKF-SLAM (see Section 8). The price: every update rewrites all of \(P\). With state dimension \(N=3+2n\), each update costs about \(2N^2\) multiply-adds and the memory is \(N^2\) too. With 18 landmarks \(N=39\), no problem at all; for a map of ten thousand landmarks each update is \(8\times10^8\) operations. That is why EKF-SLAM is limited to small maps.

FastSLAM（Montemerlo 等，2002）注意到一件事：如果整条路径已知，各路标就彼此独立了，因为它们之间的相关全是通过机器人位姿传过来的。于是用 4.10 节的粒子滤波表示路径，每个粒子是一条路径假设 \(\hl{path}{\x^{[i]}_{1:t}}\)，并且带着 \(n\) 个各自独立的 2×2 小 EKF，每个路标一个。每一步：按运动模型给每个粒子采样一个新位姿，用它自己的地图更新它看到的路标，并按测量和它的地图吻合的程度更新权重；有效粒子数 \(N_\text{eff}\) 低于 \(M/2\) 时做系统重采样。每次更新只碰被看到的那个路标，代价约为 \(M\) 乘以每步测量数，与地图大小无关（用树形结构共享地图时是 \(M\log n\)）。

FastSLAM (Montemerlo et al., 2002) notices one thing: if the whole path were known, the landmarks would be independent of each other, because all their correlation comes through the robot pose. So it represents the path with the particle filter of Lesson 4.10: each particle is a path hypothesis \(\hl{path}{\x^{[i]}_{1:t}}\) carrying \(n\) independent 2×2 EKFs, one per landmark. At each step: sample a new pose for every particle from the motion model, update the landmarks it sees with its own map, and reweight it by how well the measurements agree with its map; resample systematically when the effective sample size \(N_\text{eff}\) drops below \(M/2\). An update touches only the landmark that was seen, so the cost is about \(M\) times the number of measurements per step, independent of the map size (\(M\log n\) when the maps are shared in a tree).

地点识别会认错：两段一样的走廊、一排相同的货架、换了季节的街道。一条错误回环说“这两个位姿在同一个地方”，在平方损失下它的拉力与偏差成正比，而且它的信息矩阵和真回环一样大，于是整张图被硬拽到一起。补救办法是 6.9 节、11.5 节讲过的鲁棒核：把回环边的代价换成增长更慢的 \(\hl{kern}{\rho}\)，用 IRLS 求解，每条边按当前的归一化误差 \(\chi=\sqrt{\mathbf e^\top\Omega\mathbf e}\) 得到一个权重 \(w(\chi)\)。下面是一个 120 个位姿的“曼哈顿世界”位姿图（机器人在 1 m 网格上随机游走，回到同一个网格点就有一定概率产生真回环，共 47 条），再随机加入若干条错误回环；所有优化都从同一个里程计初值出发。鲁棒核只加在回环边上。

Place recognition makes mistakes: two identical corridors, a row of identical shelves, a street in another season. A false closure says "these two poses are at the same place"; under the squared loss its pull grows with the discrepancy, and its information matrix is as large as a true closure's, so the whole graph is dragged together by force. The remedy is the robust kernel of Lessons 6.9 and 11.5: replace the cost of the loop-closure edges with a slower-growing \(\hl{kern}{\rho}\) and solve by IRLS, giving every edge a weight \(w(\chi)\) from its current normalized error \(\chi=\sqrt{\mathbf e^\top\Omega\mathbf e}\). Below is a 120-pose "Manhattan world" pose graph (a robot random-walking on a 1 m grid; returning to the same grid point produces a true closure with some probability, 47 in all), with a number of false closures added at random. Every optimization starts from the same odometry initialization. The robust kernel is applied to loop-closure edges only.

再试几种组合（默认图、6 条错误回环）：Huber（尺度 3）把误差降到约 0.42 m，比平方损失好得多，但做不到参照水平：它的影响函数有界却不为零，错误回环还在用常数大小的力拉。错误回环加到 20 条时，Huber 的误差升到约 1.5 m，Cauchy 和 DCS 仍是 0.08 m。尺度太小也不行：把 DCS 的尺度降到 1，47 条真回环里有 40 条被一起关掉，误差 0.67 m；降到 0.5，47 条真回环有 46 条被关掉，结果基本停在里程计初值。原因是从里程计初值看，真回环的误差也很大（漂移有 1 m，而回环的 σ 是 5 cm，\(\chi\) 可达 20），一个太“狠”的核分不清谁真谁假。点“换一张图”可以看到，DCS 在某些图上尺度 3 也会误杀真回环，而 Cauchy 更宽容。

Try a few more combinations (default graph, 6 false closures): Huber (scale 3) brings the error down to about 0.42 m, far better than the squared loss but not the reference: its influence function is bounded but not zero, so the false closures still pull with a constant force. With 20 false closures Huber rises to about 1.5 m, while Cauchy and DCS stay at 0.08 m. Too small a scale fails as well: at DCS scale 1, 40 of the 47 true closures are switched off along with the false ones and the error is 0.67 m; at 0.5, 46 of the 47 are switched off and the result essentially stays at the odometry initialization. The reason: seen from the odometry start, true closures have large errors too (the drift is 1 m while a closure's σ is 5 cm, so \(\chi\) reaches 20), and too aggressive a kernel cannot tell true from false. "New graph" shows that on some graphs DCS at scale 3 also kills true closures, whereas Cauchy is more forgiving.

机器人每秒都在加新位姿。每来一个关键帧就从头解一遍整张图，代价随地图增大而增大，迟早跟不上。增量平滑（iSAM，Kaess 等 2008；iSAM2，2012）的想法是：保留上一步的分解结果，只改动受新因子影响的那一部分。用 6.9 节的语言：正规方程矩阵 \(H=LL^\top\)，Cholesky 因子 \(\hl{fill}{L}\) 的第 \(c\) 列只依赖排在它前面的变量。加入一个新因子，只会改写消元树上从这个因子最早的变量到根的那一条路径上的列，其余的列一个数也不变。下图用 FIG 2 那条“8”字路线的位姿图（按真实几何建：里程计链，加上相距不到 1 m、相隔至少 20 帧的回环，共 92 个位姿、19 条回环），逐步计算 \(L\) 的非零块（每块 3×3），并标出每一步必须重算的块。

A robot adds new poses every second. Re-solving the whole graph from scratch at every keyframe costs more as the map grows, and sooner or later it cannot keep up. The idea of incremental smoothing (iSAM, Kaess et al. 2008; iSAM2, 2012) is to keep the previous factorization and change only the part affected by the new factors. In the language of Lesson 6.9: the normal-equation matrix is \(H=LL^\top\), and column \(c\) of the Cholesky factor \(\hl{fill}{L}\) depends only on the variables ordered before it. A new factor rewrites only the columns on the path of the elimination tree from the factor's earliest variable to the root; every other column stays exactly the same. The figure uses the pose graph of FIG 2's figure-eight route (built from the true geometry: the odometry chain plus closures between poses less than 1 m apart and at least 20 frames apart, 92 poses and 19 closures), computes the nonzero blocks of \(L\) (each 3×3) step by step, and marks the blocks that must be recomputed at each step.

记位姿 \(\x_{0:t}\)、地图 \(\mathbf m\)、控制（里程计）\(\mathbf u_{1:t}\)、测量 \(\mathbf z_{1:t}\)。在马尔可夫假设下（4.7 节），整个后验分解成一串小因子的乘积：

Write the poses \(\x_{0:t}\), the map \(\mathbf m\), the controls (odometry) \(\mathbf u_{1:t}\) and the measurements \(\mathbf z_{1:t}\). Under the Markov assumptions (Lesson 4.7) the whole posterior factors into a product of small pieces:

估计整个左边叫完整 SLAM（平滑），就是位姿图和 BA 做的事。只要当前状态 \(p(\x_t,\mathbf m\mid\cdot)=\int p(\x_{0:t},\mathbf m\mid\cdot)\,d\x_{0:t-1}\) 叫在线 SLAM（滤波），就是 EKF-SLAM 做的事。在高斯近似下，“积分掉”一个变量就是对信息矩阵做 Schur 补（11.6 节）：被消去的位姿的所有邻居两两连起来。每个路标都是从某个位姿上看到的，消掉一串位姿之后，所有路标都互相连上，这就是 FIG 3 那个越来越满的矩阵的来历。不做边缘化，信息矩阵保持稀疏：每个块只对应一个因子。

Estimating the whole left-hand side is full SLAM (smoothing), which is what pose graphs and BA do. Keeping only the current state, \(p(\x_t,\mathbf m\mid\cdot)=\int p(\x_{0:t},\mathbf m\mid\cdot)\,d\x_{0:t-1}\), is online SLAM (filtering), which is what EKF-SLAM does. Under a Gaussian approximation, "integrating out" a variable is a Schur complement on the information matrix (Lesson 11.6): all neighbours of the eliminated pose become mutually linked. Every landmark was seen from some pose, so once a chain of poses has been eliminated all landmarks are linked to each other: that is where the ever-fuller matrix of FIG 3 comes from. Without marginalization the information matrix stays sparse, with one block per factor.

状态 \(\mathbf y=[\x;\mathbf m_1;\dots;\mathbf m_n]\)，协方差按块写成下式。对路标 \(j\) 的距离-方位测量 \(\mathbf z=(r,\phi)\)，记 \(\boldsymbol\delta=\mathbf m_j-\mathbf t\)，\(q=\|\boldsymbol\delta\|^2\)，预测为 \(\hat r=\sqrt q\)，\(\hat\phi=\operatorname{atan2}(\delta_y,\delta_x)-\theta\)。雅可比 \(H\) 只在机器人的 3 列和路标 \(j\) 的 2 列上非零：

The state is \(\mathbf y=[\x;\mathbf m_1;\dots;\mathbf m_n]\) with the block covariance below. For a range-bearing measurement \(\mathbf z=(r,\phi)\) of landmark \(j\), let \(\boldsymbol\delta=\mathbf m_j-\mathbf t\) and \(q=\|\boldsymbol\delta\|^2\); the prediction is \(\hat r=\sqrt q\), \(\hat\phi=\operatorname{atan2}(\delta_y,\delta_x)-\theta\). The Jacobian \(H\) is nonzero only in the robot's 3 columns and landmark \(j\)'s 2 columns:

虽然 \(H\) 很稀疏，\(PH^\top\) 却是一整列稠密的块：只要 \(P_{xm}\) 和 \(P_{mm}\) 非零，增益 \(K\) 在每个路标上都有分量，\(KSK^\top\) 改写 \(P\) 的全部 \((3+2n)^2\) 个元素。这就是 \(O(n^2)\) 的来源，也是回环时“看不见的路标也被修正”的来源。新路标 \(\mathbf m=\mathbf t+r\,(\cos(\theta+\phi),\sin(\theta+\phi))\) 的扩维要用 \(G_x=\partial\mathbf m/\partial\x\) 和 \(G_z=\partial\mathbf m/\partial\mathbf z\)：新的协方差块是 \(G_xP_{xx}G_x^\top+G_zRG_z^\top\)，与已有状态的交叉块是 \(G_xP_{x\,\cdot}\)。FIG 3 的“全部丢掉”就是每一步之后把 \(P_{xm}\) 和 \(P_{mm}\) 的非对角块置零。

Sparse as \(H\) is, \(PH^\top\) is a full dense block column: as long as \(P_{xm}\) and \(P_{mm}\) are nonzero, the gain \(K\) has a component on every landmark, and \(KSK^\top\) rewrites all \((3+2n)^2\) entries of \(P\). That is where the \(O(n^2)\) comes from, and also why "landmarks out of view get corrected" at a loop closure. Augmenting with a new landmark \(\mathbf m=\mathbf t+r\,(\cos(\theta+\phi),\sin(\theta+\phi))\) uses \(G_x=\partial\mathbf m/\partial\x\) and \(G_z=\partial\mathbf m/\partial\mathbf z\): the new covariance block is \(G_xP_{xx}G_x^\top+G_zRG_z^\top\) and its cross block with the existing state is \(G_xP_{x\,\cdot}\). The "cut them all" option of FIG 3 sets the off-diagonal blocks of \(P_{xm}\) and \(P_{mm}\) to zero after every step.

这一步是精确的（不是近似），因为给定整条路径，各路标的测量互不相干：这叫 Rao-Blackwell 化，能解析处理的部分（路标）用高斯解析处理，只对剩下的部分（路径）采样。权重按 4.10 节的重要性采样推出：提议分布是运动模型，所以权重就是测量似然，\(H_m\) 是测量对路标的 2×2 雅可比。FastSLAM 2.0 把最新的测量也放进提议分布，粒子更集中，所需粒子数少得多。

This step is exact, not an approximation: given the whole path, the measurements of different landmarks are independent. It is called Rao-Blackwellization: the part that can be handled analytically (the landmarks) is handled with Gaussians, and only the rest (the path) is sampled. The weight follows from the importance sampling of Lesson 4.10: the proposal is the motion model, so the weight is the measurement likelihood, with \(H_m\) the 2×2 Jacobian of the measurement with respect to the landmark. FastSLAM 2.0 puts the latest measurement into the proposal as well, which concentrates the particles and needs far fewer of them.

位姿 \(X_i=(\mathbf t_i,\theta_i)\)。一条边 \((i,j)\) 带一个相对位姿测量 \(Z_{ij}=(\Delta\mathbf t,\Delta\theta)\) 和信息矩阵 \(\Omega_{ij}\)（协方差的逆）。误差是“测量的逆乘以预测”这个变换 \(E_{ij}=Z_{ij}^{-1}X_i^{-1}X_j\) 写成向量（预测与测量完全一致时 \(E_{ij}=I\)，误差为零）：

Poses are \(X_i=(\mathbf t_i,\theta_i)\). An edge \((i,j)\) carries a relative-pose measurement \(Z_{ij}=(\Delta\mathbf t,\Delta\theta)\) and an information matrix \(\Omega_{ij}\) (the inverse covariance). The error is the transform "inverse measurement times prediction", \(E_{ij}=Z_{ij}^{-1}X_i^{-1}X_j\), written as a vector (when prediction and measurement agree, \(E_{ij}=I\) and the error is zero):

这和 6.9 节的残差几乎一样，只是平移误差再转到测量 \(Z_{ij}\) 的坐标系里：这是 g2o 的约定；GTSAM 用 \(\Log(E_{ij})\)，两者零点相同、在零附近一阶相同（6.10 节：\(V(\varphi)\approx I\)）。在二维里，旋转只是一个角度，\(\theta\leftarrow\operatorname{wrap}(\theta+\delta\theta)\) 本身就是 SO(2) 上的指数映射更新，平移在世界坐标里直接相加，所以 FIG 2 直接用 \((\mathbf t,\theta)\) 加增量；三维时必须用 6.10 节的 \(\Exp\)。令 \(\mathbf a=R_i^\top(\mathbf t_j-\mathbf t_i)\)，\(R=R(\Delta\theta)^\top R(\theta_i)^\top\)，两个 3×3 雅可比块是

This is almost the residual of Lesson 6.9, except that the translation error is rotated once more into the frame of the measurement \(Z_{ij}\): that is g2o's convention; GTSAM uses \(\Log(E_{ij})\). The two have the same zero and agree to first order near it (Lesson 6.10: \(V(\varphi)\approx I\)). In 2D a rotation is a single angle, so \(\theta\leftarrow\operatorname{wrap}(\theta+\delta\theta)\) is itself the exponential-map update on SO(2), and translations add in world coordinates; FIG 2 therefore adds increments to \((\mathbf t,\theta)\) directly. In 3D you need the \(\Exp\) of Lesson 6.10. With \(\mathbf a=R_i^\top(\mathbf t_j-\mathbf t_i)\) and \(R=R(\Delta\theta)^\top R(\theta_i)^\top\), the two 3×3 Jacobian blocks are

上面这行是页面加载时现算的：把解析雅可比和中心差分逐元素比较。\(H\) 的块 \((i,j)\) 非零当且仅当 \(i,j\) 之间有边，所以它是“对角带 + 回环处的几块”。全图在整体平移和旋转下不变，\(H\) 有 3 维零空间，固定 \(X_0\)（规范，6.9 节）后才可逆。\(w_{ij}\) 是 IRLS 权重：里程计边取 1，回环边按下面的核函数取值。

The line above is computed when the page loads: it compares the analytic Jacobians entry by entry with central differences. Block \((i,j)\) of \(H\) is nonzero exactly when there is an edge between \(i\) and \(j\), so \(H\) is "a diagonal band plus a few blocks at the loop closures". The whole graph is invariant to a global translation and rotation, so \(H\) has a 3-dimensional null space and becomes invertible only once \(X_0\) is fixed (the gauge, Lesson 6.9). \(w_{ij}\) is the IRLS weight: 1 for odometry edges, and for loop closures the value given by the kernels below.

Huber 对大误差只给 \(k/\chi\) 的权重，力的大小 \(w\chi=k\) 有界但不为零。Cauchy 和 DCS 是“回落”型：\(\chi\) 很大时权重趋于零，错误回环几乎被关掉，但代价不再是凸的，结果依赖初值。DCS（dynamic covariance scaling，Agarwal 等 2013）是从 switchable constraints（Sünderhauf 与 Protzel 2012）推出来的：后者给每条回环配一个要和位姿一起优化的“开关” \(s\in[0,1]\)，回环的信息矩阵乘以 \(s^2\)，再加一项先验 \(\Phi(1-s)^2\)（\(\Phi\) 是开关先验方差的倒数）。于是这条边的代价是 \(s^2\chi^2+\Phi(1-s)^2\)，完全关掉（\(s=0\)）时等于 \(\Phi\)。Agarwal 等分析收敛点时要求这个代价不超过 \(\Phi\)，得到 \(0\le s\le2\Phi/(\Phi+\chi^2)\)，取其中不超过 1 的最大值，就是上面的闭式 \(s\)：开关不再是要优化的变量，信息矩阵直接按 \(s^2\) 缩放。注意 \(\Phi\) 是和 \(\chi^2\) 比较的，不是和 \(\chi\)：\(\chi^2\le\Phi\) 时权重为 1。

Huber gives a large error only the weight \(k/\chi\): the force \(w\chi=k\) is bounded but not zero. Cauchy and DCS "redescend": as \(\chi\) grows the weight goes to zero and a false closure is all but switched off, at the price of a non-convex cost whose result depends on the start. DCS (dynamic covariance scaling, Agarwal et al. 2013) was derived from the switchable constraints of Sünderhauf and Protzel (2012). Those give every closure a "switch" \(s\in[0,1]\) that is optimized along with the poses; the closure's information matrix is multiplied by \(s^2\), and a prior term \(\Phi(1-s)^2\) is added (\(\Phi\) is the inverse of the switch prior's variance). The edge's cost is then \(s^2\chi^2+\Phi(1-s)^2\), which equals \(\Phi\) when the closure is fully switched off (\(s=0\)). Analysing the converged solution, Agarwal et al. require this cost never to exceed \(\Phi\), which gives \(0\le s\le2\Phi/(\Phi+\chi^2)\); taking the largest such value not above 1 yields the closed-form \(s\) above. The switch is no longer a variable to optimize: the information matrix is simply scaled by \(s^2\). Note that \(\Phi\) is compared with \(\chi^2\), not \(\chi\): the weight is 1 while \(\chi^2\le\Phi\).

在最优点附近，位姿的协方差约为 \(H^{-1}\)（和 11.6 节 BA 的协方差同理）。FIG 2 的椭圆用的是它的 3×3 对角块：先分解 \(H=LL^\top\)，再对每个位姿解两次三角方程，不需要求出整个 \(H^{-1}\)。增量更新也建立在这个分解上：加入一个新因子相当于 \(H\leftarrow H+J^\top\Omega J\)，其中 \(J\) 只在几个变量上非零；Cholesky 因子 \(\hl{fill}{L}\) 中改变的列，恰好是消元树上从这几个变量中最早的那个到根的路径（Davis 与 Hager 的秩更新理论）。所以按时间排序时，一条里程计边只改动最后两列，一条连回很早位姿的回环会改动从那一位姿以后的几乎所有列。

Near the optimum the pose covariance is approximately \(H^{-1}\) (as for the BA covariance of Lesson 11.6). FIG 2's ellipses use its 3×3 diagonal blocks: factor \(H=LL^\top\) once, then solve two triangular systems per pose, without ever forming all of \(H^{-1}\). Incremental updates build on the same factorization: a new factor means \(H\leftarrow H+J^\top\Omega J\) with \(J\) nonzero on only a few variables, and the columns of the Cholesky factor \(\hl{fill}{L}\) that change are exactly those on the elimination-tree path from the earliest of these variables to the root (the rank-update theory of Davis and Hager). So in time order an odometry edge changes only the last two columns, and a closure back to a very old pose changes nearly every column after that pose.

真实系统的结构和 FIG 2 一样分成前端和后端，差别在于每一块都做得更结实。前端：连续帧之间不只靠轮式里程计，而是用扫描匹配或视觉/视觉惯性里程计（12.1、15.1 节）给出相对位姿；地点识别不依赖当前估计的位置（那正是漂移之后最不可信的东西），而是比较外观或几何描述子；候选回环要经过几何验证。后端：稀疏求解器、好的消元顺序、鲁棒核，以及增量更新。SLAM 给出的是位姿和挂在位姿上的测量；把它们做成哪一种地图（占据栅格、代价地图、距离场、拓扑图……），取决于谁来读地图，这是 15.3 节的内容。

Real systems are split into a front end and a back end just like FIG 2; the difference is that every part is made sturdier. Front end: consecutive frames are linked not only by wheel odometry but by scan matching or visual / visual-inertial odometry (Lessons 12.1 and 15.1); place recognition does not rely on the position in the current estimate (exactly what is least trustworthy after drift) but compares appearance or geometric descriptors; candidate closures go through geometric verification. Back end: sparse solvers, good elimination orders, robust kernels and incremental updates. SLAM delivers poses with measurements attached to them; which kind of map to build from them (an occupancy grid, a costmap, a distance field, a topological graph…) depends on who reads the map, and that is the subject of Lesson 15.3.

还有一个 12.1 节留下的问题：位姿图里每条边的信息矩阵 \(\Omega\) 从哪来？对 ICP 边，常用的是高斯牛顿近似 \(\Omega\approx A/\sigma^2\)（\(A=\sum J_i^\top J_i\)），但 12.1 节已经指出它只是下界，没有算上错误对应和采样间距，真实的不确定度更大；直接用它，回环边会显得过于自信。FIG 2 采用的是固定的保守值（里程计边 σ = 5 cm、0.015 rad，回环边 3 cm、0.008 rad）。退化场景（长走廊）里 \(A\) 在沿走廊方向接近奇异，这时把它当信息矩阵反而是对的：它告诉后端“这个方向我不知道”。

One question left open by Lesson 12.1: where does the information matrix \(\Omega\) of each edge come from? For an ICP edge the usual choice is the Gauss-Newton approximation \(\Omega\approx A/\sigma^2\) (with \(A=\sum J_i^\top J_i\)), but Lesson 12.1 already warned that it is only a lower bound on the uncertainty: it ignores wrong correspondences and sampling spacing, the real uncertainty is larger, and used as-is it makes loop-closure edges look overconfident. FIG 2 uses fixed, conservative values (odometry edges σ = 5 cm and 0.015 rad, closures 3 cm and 0.008 rad). In a degenerate scene (a long corridor) \(A\) is nearly singular along the corridor, and there using it as the information matrix is exactly right: it tells the back end "I don't know this direction".