点云与 ICP 配准Point Clouds & ICP

一台激光雷达每秒吐出几十万个三维点，深度相机每帧给出几万个。它们没有"这是桌子、那是墙"的标签，只是一堆带噪声的坐标。机器人用这堆点做三件最基本的事，而它们都归结为配准（registration）：找一个刚体运动 \(T=(R,\mathbf t)\in\SE\)，使一堆点 \(P\)（源）经过 \(T\) 之后与另一堆点 \(Q\)（目标）重合。

• 里程计与建图：机器人向前走了一步，前后两帧扫描大部分重叠。把后一帧挪到前一帧上所需的刚体运动，就是这一步的位移；把每一帧按各自的位姿拼起来就是地图（LOAM、KISS-ICP、FAST-LIO 都是这个思路）。
• 物体定位与位姿精修：物体的 CAD 点云（模型）和深度相机看到的局部点云对齐，就得到物体位姿。网络或 PnP 给出一个粗估计，ICP 把它精修到毫米级，然后才交给抓取规划（9.3 节）。
• 标定与多机位拼接：两个传感器同时看同一个场景，让两堆点重合，就得到它们之间的外参。

难点在于对应关系是未知的：两次扫描打到的不是同一批点。如果对应已知，问题有闭式解，一次 SVD 搞定（1.12 节）；对应未知时，问题变成非凸的，得靠迭代，而迭代只能保证走到局部最优。本节的主线：点云长什么样 → 对应已知时的闭式解 → ICP → 点到面 ICP → 什么时候会错（局部极小、退化）→ 机器人里怎么用。

A LiDAR emits hundreds of thousands of 3-D points per second and a depth camera tens of thousands per frame. They come with no "this is a table, that is a wall" labels, just noisy coordinates. A robot does three basic things with them, all of which are registration: find a rigid motion \(T=(R,\mathbf t)\in\SE\) that, applied to one point set \(P\) (the source), makes it coincide with another, \(Q\) (the target).

• Odometry and mapping: the robot moved one step, and consecutive scans largely overlap. The rigid motion that carries the new scan onto the old one is the step's displacement; placing every scan at its own pose gives the map (LOAM, KISS-ICP and FAST-LIO all follow this idea).
• Object localization and pose refinement: align the object's CAD point cloud (the model) with the partial cloud a depth camera sees and you have the object pose. A network or PnP gives a rough estimate, ICP refines it to millimetres, and only then is it handed to grasp planning (Lesson 9.3).
• Calibration and multi-view stitching: two sensors look at the same scene; make their point sets coincide and you have the extrinsics between them.

The difficulty is that correspondences are unknown: two scans never hit the same points. With correspondences, the problem has a closed form, one SVD (Lesson 1.12); without them it is non-convex, needs iteration, and iteration can only promise a local optimum. The thread of this lesson: what a point cloud is → the closed form with known matches → ICP → point-to-plane ICP → how it goes wrong (local minima, degeneracy) → how robots use it.

深度相机的每个像素 \((u,v)\) 存一个深度 \(z\)。用 10.3 节的内参 \(K\) 反投影，再用相机位姿 \({}^{w}T_{c}\) 搬到世界系：

Every pixel \((u,v)\) of a depth camera stores a depth \(z\). Back-project it with the intrinsics \(K\) of Lesson 10.3, then move it to the world frame with the camera pose \({}^{w}T_{c}\):

这样得到的是有序点云（organized）：保留 \((u,v)\) 网格，邻居就是相邻像素，免费。LiDAR 的多圈扫描、多帧拼接、滤波之后，就只剩一个无序点云（unorganized）：一个没有顺序、没有连接关系的集合，找邻居要靠空间索引。它还有几个脾气：深度噪声随距离增大（结构光和 ToF 相机的噪声大致与 \(z^2\) 成正比，下图取 \(\sigma(z)=\sigma\,(z/2)^2\)）；密度随距离变稀；只有朝向传感器的一面被看见。预处理有三件标配：体素滤波（每个体素只留一个质心，让密度均匀、点数变少）、最近邻搜索（k-d 树，平均每次查询 \(O(\log N)\)，暴力是 \(O(N)\)）、法向量（取点的 \(k\) 个近邻，做协方差的特征分解，最小特征值对应的特征向量就是法向，\(\lambda_{\min}/\sum\lambda\) 称为曲率）。

This gives an organized cloud: it keeps the \((u,v)\) grid, so neighbours are neighbouring pixels, for free. After a LiDAR's multiple sweeps, merging and filtering, only an unorganized cloud remains: a set with no order and no connectivity, where finding neighbours takes a spatial index. Clouds have a few more habits: depth noise grows with range (structured-light and ToF noise scales roughly with \(z^2\); the figure uses \(\sigma(z)=\sigma\,(z/2)^2\)); density thins out with distance; and only the side facing the sensor is seen. Three standard preprocessing steps: the voxel filter (keep one centroid per voxel, which evens out density and cuts the count), nearest-neighbor search (a k-d tree answers a query in \(O(\log N)\) on average, brute force takes \(O(N)\)), and normals (take a point's \(k\) neighbours, eigen-decompose their covariance; the eigenvector of the smallest eigenvalue is the normal, and \(\lambda_{\min}/\sum\lambda\) is called the curvature).

先解简单的版本：已知 \(\mathbf a_i\) 对应 \(\mathbf b_i\)，求使 \(\sum_i\|R\mathbf a_i+\mathbf t-\mathbf b_i\|^2\) 最小的 \(R\in\SO,\mathbf t\)。四步：

1. 去中心：\(\bar{\mathbf a},\bar{\mathbf b}\) 是两组点的质心，\(\mathbf a_i'=\mathbf a_i-\bar{\mathbf a}\)，\(\mathbf b_i'=\mathbf b_i-\bar{\mathbf b}\)。平移从问题里消失了。
2. 互协方差矩阵 \(H=\sum_i\mathbf a_i'\mathbf b_i'^{\top}\)（\(3\times3\)）：它记录了"\(\mathbf a\) 的每个方向在多大程度上跟 \(\mathbf b\) 的每个方向同向变化"。
3. SVD：\(H=U\Sigma V^{\top}\)，旋转 \(\hat R=V\,\mathrm{diag}(1,1,\det(VU^{\top}))\,U^{\top}\)。
4. 平移 \(\hat{\mathbf t}=\bar{\mathbf b}-\hat R\bar{\mathbf a}\)。

第 3 步里的 \(\det\) 修正是整节最容易被忽略的一行：SVD 给出的 \(VU^{\top}\) 可能是反射（行列式 \(-1\)），而反射并不是刚体运动。下图让你把 \(B\) 做成镜像，亲眼看到"没有这一行会发生什么"。如果两组点之间还差一个尺度（比如单目 SLAM 的地图和激光地图，或者两个尺度未知的重建），Umeyama（1991）把闭式解推广到相似变换 \(s,R,\mathbf t\)。

Start with the easy version: given that \(\mathbf a_i\) matches \(\mathbf b_i\), find the \(R\in\SO,\mathbf t\) minimizing \(\sum_i\|R\mathbf a_i+\mathbf t-\mathbf b_i\|^2\). Four steps:

1. Center: \(\bar{\mathbf a},\bar{\mathbf b}\) are the centroids, \(\mathbf a_i'=\mathbf a_i-\bar{\mathbf a}\), \(\mathbf b_i'=\mathbf b_i-\bar{\mathbf b}\). Translation drops out of the problem.
2. The cross-covariance \(H=\sum_i\mathbf a_i'\mathbf b_i'^{\top}\) (\(3\times3\)): it records how much each direction of \(\mathbf a\) varies together with each direction of \(\mathbf b\).
3. SVD: \(H=U\Sigma V^{\top}\), and the rotation is \(\hat R=V\,\mathrm{diag}(1,1,\det(VU^{\top}))\,U^{\top}\).
4. Translation \(\hat{\mathbf t}=\bar{\mathbf b}-\hat R\bar{\mathbf a}\).

The \(\det\) correction in step 3 is the easiest line of the whole lesson to forget: the SVD's \(VU^{\top}\) can be a reflection (determinant \(-1\)), and a reflection is not a rigid motion. The figure below lets you mirror \(B\) and see exactly what happens without that line. If the two sets also differ by a scale (a monocular SLAM map against a LiDAR map, say, or two reconstructions of unknown scale), Umeyama (1991) extends the closed form to a similarity \(s,R,\mathbf t\).

ICP（Iterative Closest Point，Besl 与 McKay、Chen 与 Medioni，1992）每一轮做四件事：① 变换——用当前估计 \(T\) 把源点云挪到目标附近；② 匹配——每个源点在目标里找最近邻，得到对应关系；③ 剔除（可选）——丢掉最差的一部分配对；④ 求解——用 Kabsch 闭式解求出使这些配对距离平方和最小的新 \(T\)。重复，直到 \(T\) 不再变化。

下面的图里，琥珀色是源点云（带一个已知的初始错位），紫色是目标。物体是不对称的"玩具"（椭球身体、头、侧瘤、竖尾鳍，没有任何对称性，所以正确的对齐是唯一的）。青色线是当前的对应关系。每点一次单步，就执行一轮；运行会一直迭代到收敛。右上的曲线是 RMSE（匹配距离的均方根）和相对真值的旋转误差，右下是收敛盆：对每个初始角度（绕所选轴）各跑一次 ICP，画出最终的旋转误差。绿色带内（<5°）算对齐成功。

ICP (Iterative Closest Point; Besl and McKay, and Chen and Medioni, 1992) does four things per round: ① transform the source by the current estimate \(T\); ② match every source point to its nearest neighbor in the target; ③ reject (optional) the worst fraction of pairs; ④ solve the Kabsch closed form for the \(T\) that minimizes the squared distances of the remaining pairs. Repeat until \(T\) stops changing.

In the figure, amber is the source (with a known initial misalignment) and purple is the target. The object is an asymmetric "toy" (ellipsoid body, head, side lump, vertical tail fin; no symmetry, so the right alignment is unique). Cyan lines are the current correspondences. Step performs one round, Run iterates to convergence. The upper plot on the right shows the RMSE (root-mean-square matching distance) and the rotation error against the ground truth; the lower plot is the basin of attraction: one ICP run for each initial angle (about the chosen axis), plotting the final rotation error. Inside the green band (<5°) the alignment counts as a success.

点到点 ICP 要求源点落在它匹配到的那个目标点上。可是目标点只是曲面的一次随机采样：源点沿着曲面滑到旁边一点，同样是对的，却要受罚。在大片平坦的地面或墙面上，这使点到点 ICP 的收敛变得很慢，每轮只挪一点点（线性收敛，比例很接近 1）。点到面（Chen 与 Medioni 的原始形式）只罚源点到目标点切平面的垂直距离：

Point-to-point ICP demands that a source point land on its matched target point. But that target point is just a random sample of the surface: the source point sliding to a neighbouring spot on the surface is equally right, yet it is penalized. On large flat floors and walls this makes point-to-point ICP crawl, moving only a little per round (linear convergence with a ratio close to 1). Point-to-plane (the original Chen and Medioni form) only penalizes the distance from the source point to the target point's tangent plane:

它对 \(T\) 不再有 Kabsch 那样的闭式解，但可以用 Gauss-Newton（6.6 节）：在当前估计处线性化，解一个 \(6\times6\) 的法方程，再用指数映射（5.6 节）更新。推导放在"再看数学"一节。下面的图在三个场景里同时跑点到点和点到面，用真值位姿误差 \(\|\mathrm{Log}(T\,T_\ast^{-1})\|\)（旋转以弧度、平移以米，合成一个数）比较。同时算出 \(6\times6\) 矩阵 \(\sum_iJ_i^{\top}J_i\) 的特征值：它们衡量每一个运动方向被数据约束得有多紧。

场景：墙角（地面 + 后墙 + 侧墙，三个互相垂直的面，什么运动都被约束）；隧道（地面 + 两侧墙，三个面都平行于 \(x\) 轴）；隧道 + 立柱（隧道中间放一个方盒，盒子的前后面把 \(x\) 方向约束住了）。

It no longer has a Kabsch-style closed form in \(T\), but Gauss-Newton (Lesson 6.6) works: linearize at the current estimate, solve a \(6\times6\) normal equation, and update with the exponential map (Lesson 5.6). The derivation is in "The math". The figure runs point-to-point and point-to-plane side by side on three scenes and compares them with the ground-truth pose error \(\|\mathrm{Log}(T\,T_\ast^{-1})\|\) (rotation in radians and translation in metres, combined in one number). It also computes the eigenvalues of the \(6\times6\) matrix \(\sum_iJ_i^{\top}J_i\): they measure how tightly the data constrains each direction of motion.

The scenes: a corner (floor + back wall + side wall, three mutually perpendicular planes, everything is constrained); a tunnel (floor + two side walls, all three planes parallel to the \(x\) axis); a tunnel + pillar (a box in the middle, whose front and back faces constrain the \(x\) direction).

已知配对 \((\mathbf a_i,\mathbf b_i)\)，求

Given pairs \((\mathbf a_i,\mathbf b_i)\), find

第一步，平移解耦。对 \(\mathbf t\) 求导令其为 0，得 \(\mathbf t=\bar{\mathbf b}-R\bar{\mathbf a}\)。代回去，代价只剩旋转：\(\sum_i\|R\mathbf a_i'-\mathbf b_i'\|^2\)，其中 \(\mathbf a_i'=\mathbf a_i-\hl{cent}{\bar{\mathbf a}}\)。第二步，展开。\(R\) 是正交矩阵，\(\|R\mathbf a'\|=\|\mathbf a'\|\)，所以

Step 1, translation decouples. Setting the derivative in \(\mathbf t\) to zero gives \(\mathbf t=\bar{\mathbf b}-R\bar{\mathbf a}\). Substituting back leaves a cost in the rotation alone, \(\sum_i\|R\mathbf a_i'-\mathbf b_i'\|^2\), with \(\mathbf a_i'=\mathbf a_i-\hl{cent}{\bar{\mathbf a}}\). Step 2, expand. \(R\) is orthogonal, \(\|R\mathbf a'\|=\|\mathbf a'\|\), so

只需最大化 \(\operatorname{tr}(RH)=\operatorname{tr}(R\,U\Sigma V^{\top})=\operatorname{tr}(\Sigma\,V^{\top}RU)=\sum_k\sigma_k Z_{kk}\)，其中 \(Z=V^{\top}RU\) 是正交矩阵，对角元不超过 1。所以最大值在 \(Z=I\)，即 \(R=VU^{\top}\) 取到（这正是经典的正交 Procrustes 问题的解，用的是 1.12 节的 SVD）。但我们要求 \(\det R=+1\)，而 \(\det Z=\det(V^{\top}RU)=\det R\cdot\det V\det U\)：如果 \(\det(VU^{\top})=-1\)，\(Z=I\) 不被允许，此时最好的行列式为 \(-1\) 的正交矩阵是 \(Z=\mathrm{diag}(1,1,-1)\)（牺牲最小的奇异值 \(\sigma_3\)）。合起来：

We need only maximize \(\operatorname{tr}(RH)=\operatorname{tr}(R\,U\Sigma V^{\top})=\operatorname{tr}(\Sigma\,V^{\top}RU)=\sum_k\sigma_k Z_{kk}\), where \(Z=V^{\top}RU\) is orthogonal and its diagonal entries cannot exceed 1. The maximum is at \(Z=I\), i.e. \(R=VU^{\top}\) (this is exactly the classic orthogonal Procrustes solution, built on the SVD of Lesson 1.12). But we need \(\det R=+1\), and \(\det Z=\det R\cdot\det V\det U\): if \(\det(VU^{\top})=-1\), then \(Z=I\) is not allowed, and the best orthogonal matrix with determinant \(-1\) is \(Z=\mathrm{diag}(1,1,-1)\) (sacrificing the smallest singular value \(\sigma_3\)). Together:

其中 \(D=\mathrm{diag}(1,1,\det(VU^{\top}))\)。唯一性要求 \(\operatorname{rank}H\ge2\)：如果所有点共线（\(\operatorname{rank}H=1\)），绕这条线的旋转是自由的；共面时 \(\sigma_3=0\)，行列式修正选的符号不改变代价，两个候选都是最优，只有 \(\det=+1\) 的是刚体。这是 Kabsch（1976）、Horn（1987，四元数）、Arun 等（1987）、Umeyama（1991）彼此等价的结果。Barfoot 的《State Estimation for Robotics》把它放在李群的框架里，并推广到带权重和不确定性的情形。

with \(D=\mathrm{diag}(1,1,\det(VU^{\top}))\). Uniqueness needs \(\operatorname{rank}H\ge2\): if all points are collinear (\(\operatorname{rank}H=1\)), rotation about the line is free; for coplanar points \(\sigma_3=0\), the determinant fix does not change the cost, both candidates are optimal, and only the one with \(\det=+1\) is a rigid motion. These are the equivalent results of Kabsch (1976), Horn (1987, with quaternions), Arun et al. (1987) and Umeyama (1991). Barfoot's State Estimation for Robotics places it in the Lie-group framework and extends it to weights and uncertainty.

每一步都不增加 \(E\)，\(E\ge0\)，所以单调收敛（Besl 与 McKay 的定理）。但极限只是 \(E\) 的局部极小，且收敛是线性的。剔除最差的一部分配对（修剪 ICP，Chetverikov 等）时，等于对"哪些配对被保留"也做交替最小化，单调性依然成立，而且是处理部分重叠的标准做法。更一般地，用鲁棒核 \(\rho\)（Huber、Cauchy、Geman–McClure）代替平方，经 IRLS 变成每个配对的权重 \(w_i=\rho'(r_i)/r_i\)，Kabsch 变成带权版本；KISS-ICP 里用的是自适应的距离阈值加核。

No step increases \(E\), and \(E\ge0\), so it converges monotonically (the Besl and McKay theorem). But the limit is only a local minimum of \(E\), and convergence is linear. Rejecting the worst fraction of pairs (trimmed ICP, Chetverikov et al.) is alternating minimization over "which pairs are kept" as well, so monotonicity still holds, and it is the standard answer to partial overlap. More generally, replacing the square with a robust kernel \(\rho\) (Huber, Cauchy, Geman-McClure) turns, via IRLS, into per-pair weights \(w_i=\rho'(r_i)/r_i\) and a weighted Kabsch; KISS-ICP uses an adaptive distance threshold with a kernel.

用左扰动 \(T\leftarrow\mathrm{Exp}(\boldsymbol\xi)\,T\)，\(\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)\)（角速度分量在前，与全课程一致）。记 \(\mathbf p_i'=T\mathbf p_i\)。一阶近似下 \(\mathrm{Exp}(\boldsymbol\xi)\mathbf p'\approx\mathbf p'+\boldsymbol\omega\times\mathbf p'+\mathbf v\)，于是残差 \(r_i=\mathbf n_i^{\top}(\mathbf p_i'-\mathbf q_i)\) 变为

Use a left perturbation \(T\leftarrow\mathrm{Exp}(\boldsymbol\xi)\,T\) with \(\boldsymbol\xi=(\boldsymbol\omega,\mathbf v)\) (angular part first, as everywhere in the course). Write \(\mathbf p_i'=T\mathbf p_i\). To first order \(\mathrm{Exp}(\boldsymbol\xi)\mathbf p'\approx\mathbf p'+\boldsymbol\omega\times\mathbf p'+\mathbf v\), so the residual \(r_i=\mathbf n_i^{\top}(\mathbf p_i'-\mathbf q_i)\) becomes

这里用到了标量三重积 \(\mathbf n\cdot(\boldsymbol\omega\times\mathbf p)=\boldsymbol\omega\cdot(\mathbf p\times\mathbf n)\)（1.8 节）。解出 \(\boldsymbol\xi\) 后用 \(T\leftarrow\mathrm{Exp}(\boldsymbol\xi)T\) 更新（用 M.expSE3），这就是流形上的 Gauss-Newton（6.10 节）：每步都在李代数里解线性问题，更新后仍然是合法的 SE(3) 元素，不需要再把 \(R\) 正交化。数值检验：把 \(J_i\) 与 \(r_i(\boldsymbol\xi)=\mathbf n^{\top}(\mathrm{Exp}(\boldsymbol\xi)\mathbf p'-\mathbf q)\) 的中心差分比较，最大偏差在 \(10^{-10}\) 量级。小角度近似 \(R\approx I+[\boldsymbol\omega]_\times\) 是同一阶近似，只是更新不严格落在 SO(3) 上，需要事后投影。为什么更快？点到点把每个目标点当作一个孤立的点，等价于用一个"曲率无穷大"的曲面去逼近；点到面利用了曲面的一阶信息（法向），相当于对距离函数做 Gauss-Newton，实际收敛快得多（Pottmann 等 2006 对收敛性做了几何分析）。

This uses the scalar triple product \(\mathbf n\cdot(\boldsymbol\omega\times\mathbf p)=\boldsymbol\omega\cdot(\mathbf p\times\mathbf n)\) (Lesson 1.8). After solving for \(\boldsymbol\xi\), update with \(T\leftarrow\mathrm{Exp}(\boldsymbol\xi)T\) (M.expSE3); this is Gauss-Newton on a manifold (Lesson 6.10): each step solves a linear problem in the Lie algebra, and the updated pose is again a valid SE(3) element, with no re-orthogonalization of \(R\). A numerical check, comparing \(J_i\) with central differences of \(r_i(\boldsymbol\xi)=\mathbf n^{\top}(\mathrm{Exp}(\boldsymbol\xi)\mathbf p'-\mathbf q)\), agrees to about \(10^{-10}\). The small-angle form \(R\approx I+[\boldsymbol\omega]_\times\) is the same first-order approximation, but the update does not lie exactly on SO(3) and must be projected afterwards. Why is it faster? Point-to-point treats each target point as an isolated point, which is like approximating the surface by one of infinite curvature; point-to-plane uses the surface's first-order information (the normal), which amounts to Gauss-Newton on the distance function and converges much faster in practice (Pottmann et al., 2006, analyze the convergence geometrically).

记 \(A=\sum_iJ_i^{\top}J_i\)。一个平面（法向 \(\mathbf n\)）只对满足 \(\mathbf n\cdot(\mathbf v+\boldsymbol\omega\times\mathbf p)\neq0\) 的运动有"感觉"。\(A\) 的零空间就是所有这些约束都满足的运动，即让所有残差保持不变的螺旋运动：隧道里的 \(\boldsymbol\xi=(\mathbf 0,\hat{\mathbf x})\)，大平地上的"在平面内平移 + 绕法向转动"，圆柱面上的"沿轴平移 + 绕轴转动"，球面上的"绕球心转动"。Gelfand 等（2003）用 \(A\) 的特征分解来选"几何稳定"的采样点；Zhang、Kaess 与 Singh（2016）把它用到激光里程计，检测到退化方向后不再在这个方向上更新状态。注意 \(A\) 里旋转和平移的量纲不同，特征值要在统一尺度后才有意义（FIG 4 把旋转部分乘以场景尺度 \(s\)，并除以点数）。

在高斯噪声假设下，配准的协方差近似为 \(\sigma^2A^{-1}\)，可当作位姿图（Part IV）里这条边的信息矩阵 \(A/\sigma^2\)。但这只是一个下界：它没有考虑对应关系本身的错误和采样间距，实际协方差更大（Censi 2007 给出了更精细的闭式估计）。把 ICP 的 RMSE 当作"置信度"是不安全的（见误区）。

Write \(A=\sum_iJ_i^{\top}J_i\). A plane with normal \(\mathbf n\) only "feels" motions with \(\mathbf n\cdot(\mathbf v+\boldsymbol\omega\times\mathbf p)\neq0\). The null space of \(A\) consists of motions satisfying all these constraints, i.e. the screw motions that leave every residual unchanged: \(\boldsymbol\xi=(\mathbf 0,\hat{\mathbf x})\) in a tunnel, "in-plane translation + rotation about the normal" on a big flat floor, "translation along the axis + rotation about the axis" on a cylinder, "rotation about the center" on a sphere. Gelfand et al. (2003) used the eigendecomposition of \(A\) to pick "geometrically stable" sample points; Zhang, Kaess and Singh (2016) brought it to LiDAR odometry, detecting degenerate directions and not updating the state along them. Note that rotation and translation entries of \(A\) have different units, so eigenvalues only mean something after a common scaling (FIG 4 multiplies the rotation part by the scene scale \(s\) and divides by the point count).

Under Gaussian noise the covariance of the registration is approximately \(\sigma^2A^{-1}\), which can serve as the information matrix \(A/\sigma^2\) of this edge in a pose graph (Part IV). But this is only a lower bound: it ignores errors in the correspondences and the sampling spacing, and the real covariance is larger (Censi 2007 gives a finer closed-form estimate). Treating ICP's RMSE as a "confidence" is unsafe (see the pitfalls).

激光里程计的骨架就是 ICP 的串联：对每一帧新扫描，和上一帧（或局部地图）做配准，得到相对位姿 \(\Delta_k\)，再累乘 \(T_k=T_{k-1}\Delta_k\)。下面是一个二维版本：一个 360° 激光（120 束，最大量程 3.5 m）在"房间—走廊—房间"的平面里走。每帧扫描由光线投射和噪声生成，扫描之间用真实的 ICP（点到点，二维 Kabsch）配准。灰线是墙，紫色点是地图，琥珀色路径是估计，青色虚线是真值。红色的路径段表示这一帧自己的 \(3\times3\) 点到线 Hessian（\(x,y,\theta\)）的最小特征值很小：这里数据几乎没有约束沿某个方向的运动。

"恒等初值"表示 ICP 从"没动"出发；"里程计初值"表示从带有 6% 尺度误差和小噪声的轮式里程计出发（机器人里 ICP 几乎总是这样启动的）；"仅里程计"是完全不用 ICP 的推算。

The skeleton of a LiDAR odometry is a chain of ICPs: register every new scan against the previous scan (or a local map) to get a relative pose \(\Delta_k\), then multiply, \(T_k=T_{k-1}\Delta_k\). Below is a 2-D version: a 360° LiDAR (120 beams, 3.5 m maximum range) travels through a "room, corridor, room" floor plan. Each scan is generated by ray casting plus noise, and consecutive scans are registered with a real ICP (point-to-point, 2-D Kabsch). Gray lines are walls, purple dots are the map, the amber path is the estimate and the cyan dashed one is the ground truth. Red path segments mark scans whose own \(3\times3\) point-to-line Hessian (in \(x,y,\theta\)) has a tiny smallest eigenvalue: the data barely constrain motion in some direction.

"Identity start" means ICP starts from "no motion"; "odometry start" means it starts from wheel odometry with a 6% scale error and small noise (the usual way ICP is launched on a robot); "odometry only" does dead reckoning without ICP.

这些系统都在回答同一个问题：怎么让局部、有噪声、有退化的对齐可靠？答案是几件事的组合：好的初值（IMU、里程计、恒速模型）、局部地图而不只是上一帧、对退化方向的检测、鲁棒核，以及最后把每个相对位姿作为一条边送进位姿图（Part IV）和回环检测，把累积的漂移收回来。

All of these answer the same question: how do you make local, noisy, possibly degenerate alignment reliable? The answer is a combination: a good initial guess (IMU, odometry, constant-velocity model), a local map instead of only the previous frame, detection of degenerate directions, robust kernels, and finally feeding every relative pose as an edge into a pose graph (Part IV) with loop closures to pull accumulated drift back.