视觉与抓取 II：ICP 配准与抓取稳定性

1ICP：给位姿「精修」

第 5 章的位姿估计仍可能存在误差，抓取对这些误差的敏感性取决于物体尺寸、接触位置和夹爪策略。可以先理解为：对细长物体或较窄的可抓取区域，较小的平移误差也可能使抓取偏离目标。

敏感性的三个来源：① 机械臂运动误差——特定方向的平移误差可能推翻物体；② 夹爪闭合策略——开合宽度必须与物体尺寸精确匹配；③ 旋转容错性取决于物体与抓取方向。

中文翻译

全部21个物体

纵轴：准确率，刻度0、0.1、0.2、0.3、0.4、0.5、0.6、0.7、0.8、0.9、1。

横轴：平移阈值（米），刻度0、0.02、0.04、0.06、0.08、0.1。

90%；32%；2厘米。

绿色：PoseCNN；红色：PoseCNN＋ICP。

三维平移误差分析

PoseCNN

纵轴：数量，刻度0、1000、2000、3000、4000、5000、6000、7000、8000。

横轴：旋转角误差，刻度0、50、100、150。

三维旋转误差分析

结论：初始估计 + 精修（refinement）是位姿估计的一种常见设计，ICP 就是最经典的「精修」工具。

2ICP 算法详解

迭代最近点（Iterative Closest Point, ICP）：输入两个点云，通过交替匹配与刚体拟合，估计使源点云对齐目标点云的 $(R,t)$；通常只能保证局部结果（课件 p10）：

$$ E(R,t)=\sum_i\min_j\|q_j-(Rp_i+t)\|^2 $$

这里 $p_i$ 为源点、$q_j$ 为目标点，$R,t$把源变到目标，单向目标中每个源点只与一个当前最近目标点配对；点数可不同。对于固定对应，才可写成矩阵平方误差。

ICP 在第 5 章的 Procrustes 拟合外加入最近邻匹配，并交替执行这两个步骤。标准点到点实现可以写成以下四步循环：

步骤 1：在当前位姿下查找对应

把源点云用当前估计变换到目标坐标系，对每个源点 $p_i$ 查找最近目标点 $q_{j_i}$。初值来自已有估计；教学例子的初值可取单位变换。匹配时使用当前变换后的坐标；获得对应点后，再计算配对点集的质心。这样可保留部分重叠点云在当前坐标系中的位置关系。

步骤 2：中心化已经配对的两组点

令 $Q_{corr}$ 的第 $i$ 列为对应目标点 $q_{j_i}$。计算 $\tilde P=P-\bar P$ 与 $\tilde Q_{corr}=Q_{corr}-\bar Q_{corr}$，均值来自当前匹配点对。最近邻查询的暴力复杂度是 $O(|P|\,|Q|)$，KD-tree（KD树）可加速。这里用的是单向最近邻目标；Chamfer距离另有双向定义。

步骤 3：Procrustes 求位姿

第 5 章的内容直接复用：协方差矩阵 $K = \tilde Q_{corr}\tilde P^\top$ → SVD → $\hat{R} = U\,\mathrm{diag}(1,1,\det(UV^\top))\,V^\top$ → $\hat t = \bar Q_{corr} - \hat R\bar P$。

迭代最近点（ICP）

• 问题变为 \(\hat R=\operatorname*{argmin}_{R\in\mathrm{SO}(3)}\|\tilde P_{\mathrm{corr}}-R\tilde P\|_F^2\)。
• 利用带约束的正交 Procrustes 求得 \(\hat R\)。
• 再由残差得到 T。
• 更新 P。

\[U,D,V^{\mathsf T}=\operatorname{SVD}(\tilde P_{\mathrm{corr}}\tilde P^{\mathsf T})\]

\[\hat R=U\operatorname{diag}(1,1,\det(UV^{\mathsf T}))V^{\mathsf T}\]

\[\hat T=\bar Q-\hat R\bar P\]

\[P_{\mathrm{new}}=\hat RP+\hat T\]

对照原课件第13页与完整中文翻译 →

步骤 4：更新与迭代

$P_{new} = \hat{R}P + \hat{t}\mathbf{1}^\top$，作为下一轮输入。终止条件三选一（课件 p15）：相对旋转角与平移增量足够小 / 损失变化小于阈值 / 达到最大迭代次数；迭代停止后，用残差、重叠区域和位姿误差评价配准结果。

PoseCNN：结果可视化

@05:62

对照原课件第18页与完整中文翻译 →

2.1 ICP 不保证全局正确配准

2.2 优缺点与变体

变体（课件 p17）：Point-to-Plane ICP 用源点到目标点局部平面的距离，考虑了局部结构；Plane-to-Plane ICP 同时考虑源点云的局部结构，可改善特定几何与噪声条件下的收敛表现，但仍需检查初值与法向估计。

3类别级位姿估计：NOCS

实例级方法（PoseCNN 等）要求知道每个物体的完整 CAD 模型——经典实例特定设定难以直接用于未见物体。类别级方法（王鹤老师 CVPR 2019 Oral，NOCS）换了个思路：为整个类别建立一个统一的参考系，通过三步归一化定义标准化物体空间（Normalized Object Coordinate Space）：

1. 旋转对齐（Rotation Alignment）：用先验把物体朝向对齐（如茶杯手柄统一朝某轴正方向）；
2. 平移归一化（Translation Normalization）：包围盒中心移到原点；
3. 尺寸归一化（Scale Normalization）：用同一个比例缩放各轴，使包围盒对角线长度为1并装入标准立方体，保留长宽高比例。

步骤1（旋转归一化）

对齐物体朝向。

图中英文与中文释义

Step 1 (rotation normalization): align object orientations＝第一步（旋转归一化）：统一物体朝向。杯子与相机分别按类别的规范方向排列；这定义坐标约定，不表示测试时已知真实朝向。

步骤2（平移归一化）

把物体置于以零为中心的位置。

图中英文与中文释义

Step 2 (translation normalization): zero-center the objects＝第二步（平移归一化）：使物体以零为中心。规范原点与相机空间中的实际位置是两回事。

步骤3（尺度归一化）

统一归一化物体尺度。

顶点坐标：(0.5,0.5,0.5)、(－0.5,0.5,0.5)、(0.5,0.5,－0.5)、(－0.5,0.5,－0.5)、(－0.5,－0.5,－0.5)、(0.5,－0.5,－0.5)。

图中英文与中文释义

Step 3 (scale normalization): uniformly normalize the scales＝第三步（尺度归一化）：使用同一比例缩放各轴。图中坐标以−0.5到0.5示意中心化规范空间；实现若改用[0,1]，需先统一坐标再拟合。

拿茶杯举例：① 手柄方向统一 → 对齐 $R$；② 物体轴对齐包围盒中心放到 $(0,0,0)$ → 对齐 $t$；③ 归一化包围盒大小 → 对齐 size。

3.1 完整推理流程

从图像到 NOCS 图再到位姿

RGB 图像 → 逐像素预测 → NOCS 图 → NOCS 到点的转换 → NOCS 点。

深度 → 反投影 → 深度点。

使用 RANSAC 拟合位姿。

对照原课件第31页与完整中文翻译 →

1. 输入 RGBD，Mask R-CNN 检测分割物体（ROI）；
2. 对物体每个像素预测其 NOCS 坐标 $(x,y,z)$，得到 NOCS Map；
3. 用每个像素的深度与相机内参反投影出相机坐标中的三维点，与该像素预测的NOCS三维坐标配成点对；
4. 用 Umeyama 算法 + RANSAC 拟合 7 DoF 相似变换（缩放 $s$ + 旋转 $R$ + 平移 $t$）：$sR\mathbf{p}_i + t = \mathbf{q}_i$。

中文翻译

RGB图像 → 感兴趣区域对齐（ROI Align）。

• 类别标签。
• 卷积（conv）→ 卷积 → 实例掩码。
• 卷积 → NOCS图。

深度、实例掩码、NOCS图 → 位姿拟合。

• 网络：基于Mask R-CNN的检测器。
• 输出：9自由度的完整物体边界框（包含遮挡部分；amodal bounding boxes）。

中文翻译

RGB图像 —逐像素预测→ NOCS图 —NOCS转为点→ NOCS点。

深度 —反投影→ 深度点。

NOCS点、深度点 → 使用RANSAC进行位姿拟合。

3.2 NOCS 与 ICP 的本质区别

3.3 为什么不直接回归 6D 位姿？

笔记给出三条理由：① 课件的特定消融实验中，NOCS优于该直接回归基线；② 直观上，回归是 3D→6D 的直接映射，而 NOCS 先建 2D→3D 对应、再把位姿变成几何优化问题，更符合直觉；③ NOCS 充分利用形状/几何先验——规范空间强制同类物体共享几何分布，网络学习类别级形状规律时有协同效应（Synergy），可以利用类别内共享规律，泛化能力需按测试实例验证。

消融实验：NOCS 与位姿回归

5 度准确率

类别	回归	NOCS
相机	2	34.2
笔记本电脑	2.3	28.5
马克杯	3.3	4.8
瓶子	24.9	80.4
碗	25	59
罐子	28.9	57.4

10 度准确率

类别	回归	NOCS
相机	21.9	54.9
笔记本电脑	30	74.2
马克杯	20.8	26.3
瓶子	78.3	95
碗	83.8	92.2
罐子	81.7	95.9

越高越好。

对照原课件第46页与完整中文翻译 →

4合成数据：打破标注瓶颈

NOCS 模型的训练需要「逐像素 NOCS 标注」——真实数据标注成本极高、类别泛化不足。解法回到第 1 章的预言：合成数据。但合成与真实之间的分布差异（Sim2Real Gap）会让模型在真实数据上性能暴跌。

王鹤老师的解法是 Mixed Reality Data（CAMERA 混合现实）：背景是真实的，前景是合成的——合成前景自带精确 NOCS 监督信号，轻易获得几十万量级数据：

混合现实数据生成流程

真实桌面场景	检测到的平面	合成RGB图像	真值NOCS图
ShapeNetCore 室内照明	合成物体	真值深度	真值掩码

混合现实数据

对照原课件第38页与完整中文翻译 →

实践中发现简单混合仍有 Gap——合成前景与真实背景的分界太明显，分割网络学到的经验难迁移。解法：Co-Training——用真实分割数据集（COCO）+ 合成数据混合训练 Mask R-CNN（真实数据只监督分割，不参与 NOCS 映射训练）。

课程强调合成数据的重要性：真实数据获取昂贵，合成数据能提供可扩展的几何监督。理解这一思路时，也应比较真实、合成与混合数据在具体任务上的表现。

6D 位姿估计的基础模型

语言辅助的大规模合成数据生成（§3.1）

Objaverse；标签〈保温瓶〉；原模型；噪声纹理；形状；扩散模型；大型语言模型（LLM）；重复；增强后的模型；物理引擎；路径追踪。

“这里有一个〈保温瓶〉。请描述它可能的外观，包括颜色和样式。”

“这个保温瓶是现代的哑光灰色，带有橙色和白色的几何图案。”

神经物体建模（§3.2，无模型设置）

参考图像 → 神经物体场：几何网络 \(f^g\)、外观网络 \(f^c\) → 神经 RGB-D 渲染。

位姿假设生成（§3.3）

全局位姿采样；粗位姿下的渲染；以位姿为条件的输入裁剪；编码器（共享权重）；\(H\times W\times C\)；卷积残差块；图像块；位置嵌入；Transformer 编码器；线性层；旋转更新；平移更新；精化位姿。

位姿选择（§3.4）

K 个假设；假设 0；以位姿为条件的渲染；以位姿为条件的输入裁剪；位姿排序编码器；编码器（共享权重）；\(H\times W\times128\)；卷积残差块；图像块；位置嵌入；多头自注意力；\(P\times512\)；平均池化；\(F\in\mathbb R^{512}\)；多头自注意力；\(K\times512\)；线性层；排序；排名 0、排名 1、排名 \(K-1\)。

FoundationPose 是统一的 6D 物体位姿估计与跟踪基础模型，同时支持基于模型和无模型设置。

Bowen Wen 等，《FoundationPose：新物体的统一 6D 位姿估计与跟踪》，IEEE 计算机视觉与模式识别会议论文集，2024。

对照原课件第56页与完整中文翻译 →

语言辅助的大规模合成数据生成（§3.1）

Objaverse

标签：<保温瓶>。

原始模型 → 形状。

带噪声的纹理。

“这里有一个保温瓶。请描述它可能的外观，包括颜色和风格。” → 大语言模型（LLM）→ 重复。

“这个保温瓶呈现代哑光灰色，带有橙色和白色的几何图案。”

形状、带噪声的纹理、外观描述 → 扩散模型 → 增强后的模型 → 物理引擎 → 路径追踪。

5机器人技术的五代演进

课程笔记将机器人操作技术概括为五个阶段，帮助比较各类系统如何组织感知与动作。这是一种教学分类（核心链条：$\text{pose} \to \text{grasp} \to \text{motion planning} \to \text{control}$）：

判断闭环时，可以检查一点：如果执行过程中根据新估计不断调整动作，整个任务层系统就形成反馈闭环。估计速度描述计算效率，闭环则要求观测参与后续动作修正。

6抓取稳定性：Force 与 Form Closure

位姿知道了，抓取位姿也有了，最后一个问题：这个抓取稳不稳？两个判据（课件 p63-64）：

抓取

• 抓取位姿：抓手的位置与朝向。
  • 4 自由度抓取：3 个位置自由度和 1 个朝向自由度。
  • 6 自由度抓取：3 个位置自由度和 3 个朝向自由度。
• 力封闭：能够抵消作用于物体的任意外部力旋量。

从上向下抓取；6 自由度抓取；灵巧手；平行夹爪。

对照原课件第63页与完整中文翻译 →

物理定义更精确（课件 p64）：力闭合 = 接触力螺旋锥（wrench cones）的正张成（positive span）是整个力螺旋空间；形闭合 = 一组刚性固定装置完全固定刚体。在一阶刚性接触模型下，无摩擦力闭合与一阶形闭合等价；有摩擦时可用较少接触抵抗扰动。力闭合是稳健抓取的常用设计条件。对于有限载荷任务，也可能存在稳定但未达到完整力闭合的抓取；评价时应结合实际外载、关节输出力、摩擦系数和物体承载能力。

力封闭

• 一个简化的二维问题。

具有无摩擦接触的二维抓取

\(f_1\)、\(f_2\)、\(f_3\)；\(f_x\)、\(f_y\)。

• √ 3 个可能的力向量，其中 2 个线性无关。
• × 在每个接触点均施加非零力时，可以产生合力为零的结果。
• × 原点位于可能施加的力所构成的空间内部。

由一个有摩擦接触与一个无摩擦接触组成的二维抓取

\(f_1\)、\(f_{2,1}\)、\(f_{2,2}\)；\(f_x\)、\(f_y\)。

• √ 3 个可能的力向量，其中 2 个线性无关。
• √ 在每个接触点均施加非零力时，可以产生合力为零的结果。
• √ 原点位于可能施加的力所构成的空间内部。

观看：https://www.youtube.com/watch?v=6RWFFMtD5k8

对照原课件第65页与完整中文翻译 →

2D 简化版的理解：若干接触点的可行力向量（受摩擦锥约束）如果正张成整个平面（即原点在力向量的凸包内部），任意外力都能被平衡。这个二维例子只检查平移力；完整平面刚体的判据还需包含转矩。

回归 vs 生成：抓取建模的选择

一种简单的抓取学习设定是单输出回归：预测一个抓取位姿。但遮挡和对称性使一个物体通常有多个可行抓取（多峰分布）——在平方误差监督下，单输出回归可能趋向条件均值，落在两个峰之间。因此，把抓取建模为生成问题（预测一个分布，采样多个候选）更合适。这是第 7 章（GraspNet、条件生成模型）的主线。

7本章小结

8动手实践

作业 2 已在第 5 章介绍。本章另设 ICP 与 Umeyama 练习：亲手实现最近邻匹配、刚体拟合和尺度求解，巩固位姿估计中的几何计算。

练习 1：numpy 实现 ICP（30 分钟）

import numpy as np

def icp(P, Q, max_iter=50, tol=1e-6):
    """点云配准：P (3,n) 源点云 → 对齐 Q (3,m) 目标点云，返回 (R, t, P_aligned)"""
    P = P.copy()
    R_total, t_total = np.eye(3), np.zeros(3)
    for it in range(max_iter):
        # 1. 找最近邻对应（暴力版即可，点少）
        corr = []
        for p in P.T:
            j = np.argmin(np.linalg.norm(Q.T - p, axis=1))
            corr.append(Q[:, j])
        Q_corr = np.array(corr).T

        # 2. 中心化
        cP, cQ = P - P.mean(1, keepdims=True), Q_corr - Q_corr.mean(1, keepdims=True)

        # 3. 带约束 Procrustes（第 5 章公式）
        U, _, Vt = np.linalg.svd(cQ @ cP.T)
        D = np.diag([1, 1, np.linalg.det(U @ Vt)])
        R = U @ D @ Vt
        t = Q_corr.mean(1) - R @ P.mean(1)

        # 4. 更新并检查收敛
        P_new = R @ P + t[:, None]
        movement = np.linalg.norm(P_new - P)
        R_total, t_total = R @ R_total, R @ t_total + t
        P = P_new
        if movement < tol: break
    return R_total, t_total, P

# 生成测试：非对称点云与较小位姿扰动，验证局部精修
np.random.seed(0)
Q = np.random.randn(3, 300) * np.array([[1.0], [0.7], [0.3]])
angle = 0.03
R_true = np.array([[np.cos(angle), -np.sin(angle), 0], [np.sin(angle), np.cos(angle), 0], [0, 0, 1]])
t_true = np.array([0.01, -0.02, 0.01])
P = R_true @ Q + t_true[:, None]
R, t, P_align = icp(P, Q)
print("旋转误差:", np.round(np.linalg.norm(R - R_true.T), 6))
print("平移误差:", np.round(np.linalg.norm(t + R_true.T @ t_true), 6))   # 应接近 0

自测：上面的变换方向是P→Q，因此构造P=R_true Q+t_true后，正确输出应为 $R_{true}^\top$ 与 $-R_{true}^\top t_{true}$。把初始角度改大、删去一半目标点或加入离群点，记录是否失配；把这些结果与完全重合点云的测试对照，可以观察ICP对初值、重叠范围和离群点的敏感性。

练习 2：Umeyama 相似变换（20 分钟）

在练习 1 基础上加一个缩放系数 $s$（NOCS 的 7 DoF 拟合）：

def umeyama(P, Q):
    """7DoF 相似变换：s*R*p + t = q（P、Q 已一一对应）"""
    cP, cQ = P - P.mean(1, keepdims=True), Q - Q.mean(1, keepdims=True)
    # 对应点最小二乘尺度；无噪声时也等于 RMS 比
    U, singular_values, Vt = np.linalg.svd(cQ @ cP.T)
    D = np.diag([1, 1, np.linalg.det(U @ Vt)])
    R = U @ D @ Vt
    s = np.sum(singular_values * np.diag(D)) / np.sum(cP**2)
    t = Q.mean(1) - s * R @ P.mean(1)
    return s, R, t

# 测试：随机 3D 点云 + 随机相似变换
P0 = np.random.rand(3, 100)
s_t, R_t, t_t = 1.5, np.eye(3), np.array([1., -2., 3.])
s, R, t = umeyama(P0, s_t * R_t @ P0 + t_t[:, None])
assert np.allclose(s, s_t, atol=1e-9) and np.allclose(R, R_t, atol=1e-9) and np.allclose(t, t_t, atol=1e-9)
print("✓ Umeyama 恢复的 (s, R, t) 与真值一致")

练习 3：仅二维平移力平衡判断（15 分钟，选做）

给定 3 个接触点的法向力方向（2D），忽略力矩，只判断这些力方向能否正张成整个平面，即原点是否严格位于满维凸包内部：

def force_closure_2d(normals):
    """normals: (C,2) 法向力；仅平移，原点严格在满维凸包内部"""
    from scipy.spatial import ConvexHull
    if len(normals) < 3 or np.linalg.matrix_rank(normals - normals.mean(0)) < 2:
        return False
    hull = ConvexHull(normals)
    # 半空间 n·x+b ≤ 0：x=0 严格在内部要求所有 b < 0
    return bool(np.all(hull.equations[:, -1] < -1e-10))

print(force_closure_2d(np.array([[1,0],[-0.5,0.866],[-0.5,-0.866]])))  # 120°分布 → True
print(force_closure_2d(np.array([[1,0],[1,0.1],[-0.5,0.2]])))    # 集中在同一侧 → False

附A术语表

附B课件 66 页完整索引

原版 Lecture 6 课件共 66 页（GitHub 原文件）。下表逐页列出内容及在本教程中的位置：

附C参考资源

• 课程官网：pku-epic.github.io/Intro2EAI_2025
• 本教程素材来源仓库：zhuozhiyongde/Embodied-Artificial-Intelligence-2025Spring-PKU
• 本章课件：Slides/Lecture06_03_24.pdf（66 页）
• 本章笔记原文：Notes/06-Vision-and-Grasping-II.md

ICP实现和评价参见Open3D官方ICP教程；接触条件参见Modern Robotics：形闭合。

延伸阅读

• Wang et al., NOCS, CVPR 2019 Oral（arXiv）：类别级位姿估计原始论文
• Wen et al., FoundationPose, CVPR 2024：位姿估计基础模型
• Umeyama 算法参考：zpl.fi 的 Kabsch-Umeyama 教程
• 力闭合 2D 示例视频：YouTube 搜索「6RWFFMtD5k8」（课件 p65 推荐）
• Newbury et al., Deep Learning Approaches to Grasp Synthesis（综述）