Part III · 第 12 章 三维表示与配准 · 12.1

点云与 ICP 配准

激光雷达和深度相机交给机器人的不是图像，而是一堆三维点。要把两次扫描拼起来、算出机器人走了多远、判断物体摆在哪里，都要回答同一个问题：哪一个刚体运动能让这堆点和那堆点重合？如果知道点与点的对应关系，答案是一次 SVD 的闭式解（Kabsch）；不知道时，就先用最近邻去猜对应，再求解，再猜，如此反复，这就是 ICP。本节从深度图反投影、体素滤波和法向量出发，交互地演示闭式解与反射陷阱，ICP 的收敛盆与局部极小，点到面 ICP 为什么更快，走廊里为什么会"退化"，最后用一个 2D 激光里程计看漂移是怎样出现的。

约 35 分钟 前置：1.9 投影 · 1.12 SVD · 5.3 SE(3) · 5.6 指数映射 · 6.6 Gauss-Newton · 10.3 内外参 5 个交互

    1为什么机器人需要它

    一台激光雷达每秒吐出几十万个三维点，深度相机每帧给出几万个。它们没有"这是桌子、那是墙"的标签，只是一堆带噪声的坐标。机器人用这堆点做三件最基本的事，而它们都归结为配准（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 → 什么时候会错（局部极小、退化）→ 机器人里怎么用。

    2直觉：点云是一个集合，配准是找一个刚体运动

    点云从哪来

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

    $$ \mathbf p^{c}=z\,K^{-1}\begin{bmatrix}u\\ v\\ 1\end{bmatrix},\qquad \mathbf p^{w}={}^{w}T_{c}\,\mathbf p^{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\) 称为曲率）。

    FIG 1从深度图到点云：噪声、体素滤波与法向量
    有序：64×48 深度图
    无序：同一批点，拖动旋转
    场景是地面加球、方盒和圆柱，用光线投射得到深度图，再按上式反投影（左图越暖越近）。右图是同一批点（橙色），青色短线是朝向传感器的 PCA 法向量；"曲率"模式下蓝、橙、红表示曲率低、中、高。"法向量误差"是与解析法向量夹角的平均值（方盒取最近的面）。"k-d 树访问"是每次最近邻查询平均访问的节点数，对比暴力搜索要比较的点数。随机数固定，滑块带来的变化是真实的。
    试一试
    • 把深度噪声 σ 调到 2 cm 以上，切到"法向量"：近处的平面还平吗？法向量误差涨到多少？再把 \(k\) 调大，误差降下来了吗？代价是什么（看方盒的棱）？
    • 把体素边长调到 8 cm 以上：点数剩下多少？远处稀疏的点和近处稠密的点，密度变得更均匀了吗？

    已知对应关系：闭式解

    先解简单的版本：已知 \(\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\)。

    FIG 2Kabsch / Umeyama：已知对应时的闭式配准
    2D：拖动紫色点云的圆圈
    3D：同一算法，拖动旋转
    琥珀色是 \(A\)，紫色是 \(B\)（编号相同的点互相对应，虚线相连）。拖动紫色云中心的圆圈改变平移，拖动外圈改变旋转。绿色空心圆是用估计的 \((\hat s,\hat R,\hat{\mathbf t})\) 变换 \(A\) 的结果；"镜像"打开时，红色虚线圆是漏掉 det 修正的 \(VU^{\top}\)（一个反射）作用在 \(A\) 上的结果。十字是质心。右图是 3D 版本：滑块给出真实的旋转和平移，读数给出恢复误差，以及对 2000 个随机 \((R,\mathbf t)\) 扰动的检验——没有任何一个扰动的代价低于解的代价。
    试一试
    • 把 \(B\) 切成"镜像"：不带修正的 \(VU^{\top}\) 行列式为 \(-1\)，而且在无噪声时残差正好为 0（镜像像总能被反射完美对上）。真正的刚体解残差是多少？为什么说"残差为 0"在这里是个陷阱？
    • 把"真实尺度" \(s\) 拖到 1.3 以上，用"刚体拟合"得到一个很差的残差；切到"相似变换拟合"（Umeyama）：残差回到 0，\(\hat s\) 等于几？
    核心洞见

    对应关系已知时，刚体配准是一个有闭式解的最小二乘：质心对齐，再对互协方差做一次 SVD。对应关系未知时，一个自然的想法是"先猜对应，再用闭式解"：猜对应用最近邻，每猜一次就用闭式解把源点云挪近一点，再猜。这就是 ICP，下一节让它动起来。

    3动手玩：ICP 实验室

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

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

    FIG 3ICP 逐步演示与收敛盆
    琥珀：源 · 紫：目标 · 青线：对应
    初始错位 = 绕所选轴转 \(\theta_0\) 再平移 \(\tau\)；ICP 从恒等变换出发，要把它转回去。被"保留比例"剔除的配对和杂点画成红色。上曲线纵轴是 \(\log_{10}\)：源和目标是同一物体的两次独立采样，RMSE 降到约 0.05 m（采样间距）就不再下降，所以要看紫色虚线的旋转误差才知道对不对。"多起点"从以质心对齐的 24 个等间隔旋转各跑一次，挑最终 RMSE 最小的（ICP 自己看得到的量，不是真值误差）。收敛盆对 37 个起始角度各跑一次（用一半的源点加速），改变轴、平移、数据或保留比例时重新计算，竖线是当前角度。
    试一试
    • 把初始角度拖到 150° 以上，点"运行"：ICP 平稳收敛，RMSE 也降得不错，但旋转误差仍有几十度甚至上百度。ICP 收敛到了一个局部极小（头尾对调的对齐）。观察收敛盆曲线：成功区间大约有多宽？
    • 数据选"部分重叠 + 杂点"，初始角度取 20° 左右，保留比例为 100%：运行后旋转误差仍有十几度，因为源里有目标没有的东西。把保留比例降到 80% 左右再运行：误差降到几度以内。
    • 回到"完整"数据，初始角度 180°，点"多起点"：24 个起点里最低 RMSE 的那个给出了正确的对齐。这是"ICP 需要好的初值"的一个最朴素的补救办法。
    一个想法，两种看法

    交替最小化：ICP 在 \(E(T,c)=\sum_i\|T\mathbf p_i-\mathbf q_{c(i)}\|^2\) 上交替优化：固定 \(T\) 时，最近邻 \(c\) 使 \(E\) 最小；固定 \(c\) 时，Kabsch 使 \(E\) 最小。每一步都不会让 \(E\) 变大，所以 \(E\) 单调下降（FIG 3 的 RMSE 曲线不会上升），但只能收敛到局部极小。这和 \(k\)-means 的"分配—更新"是同一种结构。距离场上的下降：把 \(f(T)=\sum_i d(T\mathbf p_i,Q)^2\)（\(d\) 是到点集的距离）当作要最小化的函数，最近邻对应相当于只用 \(d^2\) 的一阶信息，对每个点把目标曲面近似成"一个点"，点到面 ICP（下一节）则把曲面近似成"一个平面"，信息更多，收敛更快。

    4第二、第三视角：点到面与退化

    点到面：沿着曲面滑动是不受罚的

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

    $$ E_{\text{pl}}(T)=\sum_i\Big(\mathbf n_i^{\top}\big(T\mathbf p_i-\mathbf q_i\big)\Big)^2 . $$

    它对 \(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\) 方向约束住了）。

    FIG 4点到点 vs 点到面，以及退化方向
    琥珀：第 k 轮的源 · 紫：目标
    左：目标（紫）与第 \(k\) 轮的源（琥珀，用所选方法的位姿）。右上：两种方法的位姿误差（\(\log_{10}\)，竖线是当前 \(k\)）；右下：\(6\times6\) 矩阵（旋转部分乘了场景尺度，使量纲一致，再除以点数）在当前位姿处的特征值，横轴是 \(\log_{10}\lambda\)，最小的一个若比最大的小 1000 倍以上就标成红色，说明有一个无约束方向：红色箭头画出这个方向的平移分量。目标法向量用的是场景的精确法向量，源和目标是两次独立采样（各带 3 mm 噪声）。点到面在隧道里的 \(x\) 方向误差纹丝不动：它没有信息可用；点到点在慢慢往前爬，只是因为隧道两端有边界（那是边缘效应，不是真正的可观测性）。
    试一试
    • 选"墙角"和"点到面"，把 \(k\) 拖到 4：位姿误差已经降到 0.01 以下；再选"点到点"，同样的 \(k\) 误差还在 0.1 以上。读数里给出各自达到 0.02 所需的轮数。
    • 选"隧道"：最小特征值掉到 0（红色），红色箭头沿着隧道，位姿误差曲线停在初始错位的 \(x\) 分量处。再选"隧道 + 立柱"：最小特征值恢复到 \(10^{-2}\) 量级，点到面又能在几轮内收敛。
    三种说法，一件事

    几何：平面只约束沿它法向的运动，隧道的三个面的法向都垂直于 \(x\) 轴，所以沿 \(x\) 的平移没有任何面能"感觉到"。代数：\(\sum J_i^{\top}J_i\) 在该方向上特征值为 0，法方程奇异。统计：估计的协方差大致是 \(\sigma^2(\sum J_i^{\top}J_i)^{-1}\)，特征值趋于 0 意味着该方向上的方差趋于无穷。三种说法都解释了同一件事：走廊里的激光里程计在沿走廊方向上会漂。

    5再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 2、FIG 3 或 FIG 4 中对应的元素会被高亮。

    问题与 Kabsch 推导

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

    $$ \min_{R\in\SO,\ \mathbf t\in\R^3}\ \sum_{i=1}^{n}\big\|R\mathbf a_i+\mathbf t-\mathbf b_i\big\|^2 . $$

    第一步，平移解耦。对 \(\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'\|\)，所以

    $$ \sum_i\|R\mathbf a_i'-\mathbf b_i'\|^2=\sum_i\|\mathbf a_i'\|^2+\sum_i\|\mathbf b_i'\|^2-2\,\operatorname{tr}\!\big(R\,\hl{H}{H}\big),\qquad H=\sum_i\mathbf a_i'\mathbf b_i'^{\top}=U\Sigma V^{\top}. $$

    只需最大化 \(\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\)）。合起来：

    $$ \hl{R}{\hat R}=V\,\mathrm{diag}\big(1,1,\det(VU^{\top})\big)\,U^{\top},\qquad \hat{\mathbf t}=\bar{\mathbf b}-\hat R\bar{\mathbf a},\qquad \text{(Umeyama)}\ \ \hat s=\frac{\operatorname{tr}(\Sigma D)}{\sum_i\|\mathbf a_i'\|^2},\ \ \hat{\mathbf t}=\bar{\mathbf b}-\hat s\hat R\bar{\mathbf a}, $$

    其中 \(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》把它放在李群的框架里，并推广到带权重和不确定性的情形。

    ICP 是交替最小化，所以单调但只是局部

    $$ \underbrace{\hl{pairs}{c_{k+1}(i)=\arg\min_j\|T_k\mathbf p_i-\mathbf q_j\|}}_{\text{match}},\qquad \underbrace{T_{k+1}=\arg\min_{T\in\SE}\sum_i\|T\mathbf p_i-\mathbf q_{c_{k+1}(i)}\|^2}_{\text{Kabsch}},\qquad E(T_{k+1},c_{k+1})\le E(T_k,c_{k+1})\le E(T_k,c_k). $$

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

    点到面的线性化

    用左扰动 \(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)\) 变为

    $$ r_i(\boldsymbol\xi)\approx r_i+\mathbf n_i^{\top}(\boldsymbol\omega\times\mathbf p_i')+\mathbf n_i^{\top}\mathbf v=r_i+\underbrace{\big[(\mathbf p_i'\times\mathbf n_i)^{\top}\ \ \mathbf n_i^{\top}\big]}_{J_i\ (1\times6)}\boldsymbol\xi ,\qquad \Big(\sum_i J_i^{\top}J_i\Big)\hl{null}{\boldsymbol\xi}=-\sum_i J_i^{\top}r_i . $$

    这里用到了标量三重积 \(\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 对收敛性做了几何分析）。

    退化与不确定性

    记 \(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 当作"置信度"是不安全的（见误区）。

    变体一览

    方法 残差 / 模型 特点
    点到点\(\|T\mathbf p-\mathbf q\|^2\)闭式解（Kabsch）；不需要法向；平坦处收敛慢（Besl 与 McKay 1992）
    点到面\((\mathbf n^{\top}(T\mathbf p-\mathbf q))^2\)Gauss-Newton；需要目标法向；快得多，走廊里退化（Chen 与 Medioni 1992）
    Generalized-ICP\(\mathbf d^{\top}(C_q+RC_pR^{\top})^{-1}\mathbf d\)每个点配一个"扁平"协方差（沿法向很窄），源和目标的曲面一起考虑，相当于"面到面"；对初值和采样更稳（Segal 等 2009）
    NDT目标 = 网格里的高斯混合不需要显式对应，目标函数光滑可微（Biber 与 Straßer 2003）
    Colored ICP点到面 + 颜色项颜色约束沿曲面滑动的方向，缓解几何退化（Park 等 2017）
    全局配准特征匹配 + RANSAC / 分支定界不需要初值：FPFH 描述子配对，每次用 3 对点做一次 Kabsch（FIG 2 的算法），RANSAC 选内点最多的；TEASER++、Go-ICP 给出鲁棒或全局最优版本，再交给 ICP 精修

    6机器人中的它：激光里程计与它的漂移

    激光里程计的骨架就是 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 的推算。

    FIG 5二维激光里程计：走廊里的漂移
    灰线是墙，紫色点是用估计位姿把前 \(k\) 帧扫描拼成的地图；机器人用箭头表示（琥珀：估计；青：真值）。走廊长 10 m、宽 1 m，量程只有 3.5 m，所以走廊中段两端的房间都看不到，扫描只有两条平行直线：沿走廊的运动没有任何数据约束。读数里的"最小特征值"来自当前帧，低于 0.04 就标红。所有噪声种子固定。
    试一试
    • 选"恒等初值"，把 \(k\) 拖到最后：估计的机器人远远落在后面，位置误差超过 3 m（把 \(k\) 停在走廊里，读数里 ICP 的 RMSE 只有 1 到 2 cm），第二个房间的扫描被堆在走廊和第一个房间的位置上。
    • 切到"里程计初值"，再拖到最后：误差降到 2 m 以内。走廊中段 ICP 对沿走廊方向没有信息，只能原样接受初值（带着 6% 的尺度误差）；在有特征的地方它又把误差拉回来。再切到"仅里程计"，对比一下 ICP 的贡献。
    真实系统里

    激光里程计与建图：LOAM（Zhang 与 Singh，RSS 2014）从每圈扫描里按局部平滑度提取边缘点和平面点，分别做点到线、点到面匹配，里程计 10 Hz、建图 1 Hz 两个线程；LeGO-LOAM（Shan 与 Englot，IROS 2018）先分割地面并分两步优化，更轻量；FAST-LIO / FAST-LIO2（Xu 与 Zhang，RA-L 2021；Xu 等，T-RO 2022）把原始点的点到面残差放进迭代扩展卡尔曼滤波，和 IMU 紧耦合，地图用增量 k-d 树；KISS-ICP（Vizzo 等，RA-L 2023）用点到点 ICP，加上自适应距离阈值、恒速运动补偿和体素哈希地图，一套参数适配多种激光雷达。稠密建图：KinectFusion（Newcombe 等，ISMAR 2011）每帧深度图对"从当前模型光线投射出的深度和法向"做投影数据关联的点到面 ICP，在 GPU 上实时运行，模型是 TSDF 体素（12.2 节）。物体位姿精修：网络、PnP（11.4 节）或特征匹配给出粗位姿，再用点到面 ICP 对齐模型点云与深度点云，是 6D 位姿估计流程常见的最后一步。工具箱：Open3D（registration_icp、彩色 ICP、FPFH + RANSAC）、PCL（ICP、GICP、NDT）、small_gicp、libpointmatcher、TEASER++、KISS-ICP。传感器端：机械旋转式（Velodyne、Ouster）和固态（Livox）激光雷达，结构光或 ToF 深度相机（RealSense、Azure Kinect）。

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

    7常见误区

    误区 1：以为 ICP 总能把两堆点对齐

    ICP 是局部方法：它只能收敛到离初值最近的局部极小。FIG 3 的收敛盆（绕 z 轴）只有大约 ±60° 宽，初始角度再大就会收敛到"头尾对调"的对齐，而且迭代过程光滑、RMSE 也在单调下降，看不出任何异常。机器人里的补救办法是好的初值（里程计、IMU、恒速模型、上一帧的位姿）、多起点或全局配准（特征匹配 + RANSAC）、以及用独立于 ICP 的指标检查结果（重叠率、内点比例、与里程计是否一致）。

    误区 2：忽略部分重叠、动态物体和杂点

    最近邻匹配总会给每个源点一个目标点，哪怕那个源点在目标里根本没有对应物（另一半房间、路过的行人和车）。这些错误配对用平方距离放大，把解拉偏（FIG 3："部分重叠 + 杂点"下保留 100% 时旋转误差十几度）。用修剪 ICP、距离阈值、鲁棒核，或先去掉动态物体（语义分割、多帧一致性）。注意修剪比例是对重叠率的假设：设得比真实重叠率高，坏点仍然混进来；设得太低，约束又不够。

    误区 3：残差很小就等于位姿对

    FIG 5 里"恒等初值"的 ICP 在走廊里每一帧都收敛得很好：两条直线的重合度极高，RMSE 很小，位姿却完全错了。残差只度量"点离曲面有多远"，不度量"沿着曲面滑了多远"。对称的物体（圆柱、球、有对称性的零件）、平坦的地面、长走廊、隧道、空旷的大厅都有这个问题。要检查 \(A=\sum J^{\top}J\) 的最小特征值（FIG 4），或者让另一个传感器（IMU、轮速、相机）提供这个方向上的信息。

    误区 4：忘记 det 修正，也忘记 ICP 的"小细节"

    没有 \(\det\) 修正的 Kabsch 在数据含噪声、近似平面或近似对称时会偶尔输出反射（FIG 2：镜像时 \(\det=-1\)，残差看起来更小）。其它常犯的细节：单位（毫米和米混用，距离阈值差三个数量级；点到面的旋转与平移量纲不同，不做尺度的法方程病态）；降采样过狠（体素大于薄结构，如电线、杆子，点云里直接消失，且体素边界会产生混叠）；多对一（最近邻会让很多源点挤到同一个目标点上，这是边缘和稀疏处的典型现象，用双向检验或修剪缓解）；法向量（PCA 法向在棱边会被抹平，噪声大时要增大 \(k\)；FIG 1）。

    误区 5：把 ICP 的 RMSE 当成不确定性

    RMSE 小，是因为同一批点被反复挪近；它里面混着采样间距、对应错误和模型误差，不是位姿误差的方差。把它直接当成位姿图边的权重，会让这些边过于自信，一两条带有偏差的激光边就能拖歪整条轨迹。至少要用 \(A/\sigma^2\)（并按经验放大），或者用 Censi 一类的方法估协方差，并且在检测到退化方向时把这个方向的信息置零，而不是让 \(A^{-1}\) 爆炸。

    8小练习

    1. Kabsch 算法里 \(H=U\Sigma V^{\top}\)，算出 \(VU^{\top}\) 的行列式为 \(-1\)。正确的做法是？
    \(VU^{\top}\) 是反射，残差也许更小，但不是刚体运动。在 \(\det=+1\) 的约束下，最好的解是牺牲代价最小的方向：把最小奇异值对应的奇异向量翻号。\(-VU^{\top}\)（三维里）行列式 \(+1\) 但不是最优：它翻了所有三个方向。
    2. 关于 ICP 的收敛，哪一句是对的？
    交替最小化的两步都不增加代价，下有界所以收敛（Besl 与 McKay），但没有全局保证（FIG 3 的收敛盆）。带噪声的两次采样 RMSE 到不了 0；而退化场景里错误的位姿也能有很小的 RMSE（FIG 5）。
    3. 已知两组点的质心 \(\bar{\mathbf a}=(1,2,3)\)，\(\bar{\mathbf b}=(4,6,8)\)，估计的旋转是绕 \(z\) 轴转 \(90^\circ\)（\((x,y,z)\mapsto(-y,x,z)\)）。\(\hat{\mathbf t}\) 是多少？
    \(\hat{\mathbf t}=\bar{\mathbf b}-\hat R\bar{\mathbf a}\)，\(\hat R\bar{\mathbf a}=(-2,1,3)\)，所以 \(\hat{\mathbf t}=(4+2,\ 6-1,\ 8-3)=(6,5,5)\)。直接用 \(\bar{\mathbf b}-\bar{\mathbf a}=(3,4,5)\) 是忘了旋转。
    4. 一台激光雷达在一条笔直、墙面光滑的长隧道里做点到面 ICP，隧道里没有别的东西。哪个量最可能出问题？
    三个面的法向都垂直于隧道轴，沿轴的平移不会改变任何点到面的距离，\(\sum J^{\top}J\) 在该方向上奇异。高度被地面约束，左右偏移被两侧墙约束，偏航由两侧墙的位置随纵向的变化约束。
    5. ICP 每轮要为 \(N\) 个源点各找一个最近邻，目标有 \(M\) 个点。用 k-d 树，相对暴力搜索，平均每轮的匹配代价大约从什么变成什么？
    k-d 树建一次 \(O(M\log M)\)，每次最近邻查询平均 \(O(\log M)\)（最坏情况和高维下会退化）。FIG 1 的读数里能看到每次查询访问的节点数远小于 \(M\)。真实系统还会用增量 k-d 树或体素哈希，避免每帧重建。

    9延伸阅读

    • P. J. Besl & N. D. McKay, A Method for Registration of 3-D Shapes, IEEE TPAMI 14(2), 1992 — ICP 的原始论文，含单调收敛定理
    • Y. Chen & G. Medioni, Object Modelling by Registration of Multiple Range Images, Image and Vision Computing 10(3), 1992 — 同年独立提出，使用点到面距离
    • K. S. Arun, T. S. Huang & S. D. Blostein, Least-Squares Fitting of Two 3-D Point Sets, IEEE TPAMI 9(5), 1987; S. Umeyama, Least-Squares Estimation of Transformation Parameters Between Two Point Patterns, IEEE TPAMI 13(4), 1991 — 闭式解的 SVD 推导，以及带尺度、处理反射的版本
    • B. K. P. Horn, Closed-Form Solution of Absolute Orientation Using Unit Quaternions, JOSA A 4(4), 1987 — 四元数版本的闭式解
    • S. Rusinkiewicz & M. Levoy, Efficient Variants of the ICP Algorithm, 3DIM 2001 — ICP 各环节（采样、匹配、加权、剔除、误差度量）的系统比较
    • A. Segal, D. Haehnel & S. Thrun, Generalized-ICP, RSS 2009 — 面到面的概率化 ICP
    • F. Pomerleau, F. Colas & R. Siegwart, A Review of Point Cloud Registration Algorithms for Mobile Robotics, Foundations and Trends in Robotics 4(1), 2015 — 面向移动机器人的 ICP 变体综述
    • N. Gelfand, L. Ikemoto, S. Rusinkiewicz & M. Levoy, Geometrically Stable Sampling for the ICP Algorithm, 3DIM 2003; J. Zhang, M. Kaess & S. Singh, On Degeneracy of Optimization-based State Estimation Problems, ICRA 2016 — 退化方向的特征值分析，及其在激光里程计中的应用
    • J. Zhang & S. Singh, LOAM: Lidar Odometry and Mapping in Real-time, RSS 2014; I. Vizzo et al., KISS-ICP: In Defense of Point-to-Point ICP, IEEE RA-L 2023 — 两种激光里程计思路：特征 + 点到线/面，与调好的点到点
    • R. A. Newcombe et al., KinectFusion: Real-Time Dense Surface Mapping and Tracking, ISMAR 2011 — 投影数据关联的点到面 ICP 与 TSDF 融合
    • T. D. Barfoot, State Estimation for Robotics (Cambridge University Press, 2017) — 在李群框架下推导点云对齐，并讨论不确定性