FoundationPose 推理全流程解读：从一帧 RGB-D 到 6D 位姿

文章目录

- [1. FoundationPose 在做什么](#1. FoundationPose 在做什么)
- [2. 全流程一览](#2. 全流程一览)
- [3. M0 物体预处理：把物体整理成「标准形态」](#3. M0 物体预处理：把物体整理成「标准形态」)
- - [3.1 mesh 居中](#3.1 mesh 居中)
  - [3.2 物体直径：全流程统一的「尺度」](#3.2 物体直径：全流程统一的「尺度」)
  - [3.3 预生成 252 个旋转假设](#3.3 预生成 252 个旋转假设)
- [4. M1 深度预处理：让真实深度更接近渲染深度](#4. M1 深度预处理：让真实深度更接近渲染深度)
- [5. M2 位姿假设生成：一个平移 × 252 个旋转](#5. M2 位姿假设生成：一个平移 × 252 个旋转)
- - [5.1 初始平移只用一个点](#5.1 初始平移只用一个点)
  - [5.2 与旋转组合](#5.2 与旋转组合)
- [6. M3 位姿条件裁剪与渲染：为每个假设准备一对「可比」的图](#6. M3 位姿条件裁剪与渲染：为每个假设准备一对「可比」的图)
- - [6.1 裁剪窗口：假设下物体应该出现的区域](#6.1 裁剪窗口：假设下物体应该出现的区域)
  - [6.2 渲染 A 与抠取 B](#6.2 渲染 A 与抠取 B)
  - [6.3 以物体为中心的 XYZ 归一化](#6.3 以物体为中心的 XYZ 归一化)
- [7. M4 RefineNet：迭代 5 轮的位姿精修](#7. M4 RefineNet：迭代 5 轮的位姿精修)
- - [7.1 网络结构](#7.1 网络结构)
  - [7.2 解耦的位姿更新](#7.2 解耦的位姿更新)
  - [7.3 实测：5 轮之后发生了什么](#7.3 实测：5 轮之后发生了什么)
- [8. M5 ScoreNet：在比较中选出最好的那个](#8. M5 ScoreNet：在比较中选出最好的那个)
- - [实测：scorer 用纹理解决了对称歧义](#实测：scorer 用纹理解决了对称歧义)
- [9. 总结：七个关键设计](#9. 总结：七个关键设计)
- 附：代码阅读路线

本文结合论文 FoundationPose: Unified 6D Pose Estimation and Tracking of Novel Objects （Bowen Wen 等，CVPR 2024 Highlight，arXiv:2312.08344）与官方代码，把 model-based（给定 CAD 模型）场景下单帧位姿估计的推理流程从输入到输出走一遍：每一步做什么、数据长什么样（shape）、为什么这样设计。

代码基于官方仓库 commit a1b694b；示例为官方 demo mustard0（芥末瓶）首帧；文中的图片和数字均来自在 RTX 4090 上的实际运行。

1. FoundationPose 在做什么

输入：一个从未见过的物体的 CAD 模型（带纹理的 mesh，不带纹理默认灰度图）、一帧 RGB-D 图像、相机内参 K K K，以及该物体在图像中的 2D mask（来自任意检测/分割模型）。

输出：物体在相机坐标系下的 6D 位姿 T T T（4 × 4 矩阵），满足 x c a m = T ⋅ x m e s h x_{cam} = T \cdot x_{mesh} xcam=T⋅xmesh。

不需要针对新物体做任何训练或微调。 核心思路是经典的 render-and-compare：

先在整个旋转空间里撒下一批粗略的位姿假设 → 对每个假设，渲染「物体如果在这个位姿下应该长什么样」，和真实观测对比，由网络预测位姿修正量，反复迭代 → 再用另一个网络给所有精修后的假设打分，选出最好的一个。

图 1-1 mustard0 首帧的估计结果。左：3D 包围盒与坐标轴；右：按估计位姿渲染的物体叠加在真实图像上（绿线为渲染轮廓）。

2. 全流程一览

根据代码逻辑，将整个过程分为以下几个部分。

图2-1 全流程示意图

模块	做什么	输入 → 输出（N = 252）
M0 物体预处理	mesh 居中、算直径、预生成旋转假设	mesh → `rot_grid (252,4,4)`
M1 深度预处理	深度去噪，反投影为点云图	depth `(480,640)` → `xyz_map (480,640,3)`
M2 位姿假设生成	估一个初始平移，与 252 个旋转组合	mask + depth → `poses (252,4,4)`
M3 位姿条件裁剪 + 渲染	按每个假设裁剪观测、渲染物体	`poses` → A、B 各 `(252,6,160,160)`
M4 RefineNet	预测位姿修正量，迭代 5 轮	A、B → 精修后的 `poses (252,4,4)`
M5 ScoreNet	给所有假设打分，选最优	A、B → `scores (252,)` → `pose (4,4)`

在 RTX 4090 上，整个 register() 耗时约 0.82 s：refiner 5 轮 0.63 s，scorer 0.17 s，其余 0.02 s。

论文中另外两部分不在这条推理路径上：§3.1 用 LLM 辅助生成合成数据，只用于训练；§3.2 神经物体建模只在没有 CAD 模型（model-free）时用来重建物体，重建完成后同样走上表的流程。

3. M0 物体预处理：把物体整理成「标准形态」

这一步只与物体有关、与图像无关，每个物体只做一次 （estimater.py 中的 reset_object 与 make_rotation_grid）。如果把FoundationPose写成服务的话，每个sku可以在初始化的时候做掉这步，不占用实际调用时的推理时间。

3.1 mesh 居中

python 复制代码

self.model_center = (mesh.vertices.max(0) + mesh.vertices.min(0)) / 2   # 包围盒中心
mesh.vertices = mesh.vertices - self.model_center

此后的渲染、裁剪、精修、打分全部基于居中后的 mesh。原因是后面几个环节都默认「物体原点 ≈ 几何中心」：平移初始化估计的是物体中心；旋转假设绕原点旋转；裁剪窗口以原点的投影为中心。如果 CAD 的原点在物体底面甚至更远处，这些环节都会出问题。

输出时再用补偿矩阵转回原始坐标系： T o r i = T c e n t e r e d ⋅ $I - c 0 1$ T_{ori} = T_{centered} \cdot \begin{bmatrix} I & -c \\ 0 & 1 \end{bmatrix} Tori=Tcentered⋅ $I0-c1$ 。

3.2 物体直径：全流程统一的「尺度」

对顶点两两求距离取最大值，mustard 为 0.1965 m。直径会在后面两处起关键作用：

裁剪尺寸（M3）：裁剪窗口大小由直径决定，与物体朝向无关；
尺度归一化（M3 / M4）：输入的 XYZ 按物体半径归一化，网络输出的平移也以半径为单位。因此网络与物体的绝对大小无关，大大小小的物体共用一套权重。

同时，mesh 会被转换成 nvdiffrast 所需的 GPU 张量（顶点、面、法向、UV、纹理），后面每轮批量渲染 252 个假设时直接复用。可以认为mesh描述了我们需要的CAD模型。

3.3 预生成 252 个旋转假设

论文 §3.3：在以物体为中心的 icosphere 上均匀采样 N s N_s Ns 个视角，每个视角再配 N i N_i Ni 个平面内旋转。

图3-1 视角和旋转示意图

蓝点为 42 个 icosphere 视角（相机位置），箭头为指向物体中心的相机光轴；在其中一个视角上，红/橙箭头画出了 6 个平面内旋转（每隔 60°）。

icosphere 采视角：第 1 次细分的 icosphere 有 42 个顶点，近似均匀地分布在球面上（不会像经纬网格那样在两极扎堆）。每个顶点作为一个相机位置，光轴指向物体中心。
× 6 个平面内旋转 ：每个视角绕自身光轴再转 0°、60°、...、300°。「从哪个方向看」×「画面里怎么转」覆盖了整个 SO(3)，共 42 × 6 = 252 个旋转。
按对称性去重：乘上物体的对称变换后旋转差 < 30° 的假设视为重复。demo 没有提供对称信息，252 个全部保留；对称物体（如罐头）可以借此大幅减少假设数。

这组旋转与物体形状无关：任何物体（无对称时）拿到的都是同一组 252 个旋转。

💡 为什么这么粗？ 相邻假设最多相差约 30°，只要有一个落在真值附近，后面的 refiner 就能把它拉过去。网格越密计算量越大，越稀则可能全都离真值太远。这里是覆盖率与速度的折中。（论文附录写的是 N i = 12 N_i = 12 Ni=12，开源代码为 6。）

4. M1 深度预处理：让真实深度更接近渲染深度

python 复制代码

depth = erode_depth(depth, radius=2)             # 删除与邻域不一致的像素
depth = bilateral_filter_depth(depth, radius=2)  # 门限 + 空间高斯平均：降噪、补洞
xyz_map = depth2xyzmap(depth, K)                 # (480,640,3) 相机系点云图

erode_depth：5 × 5 窗口内与中心深度相差 > 1 mm（或无效）的邻居超过 80% 时，把中心置 0。用于去掉飞点和不一致的像素。
bilateral_filter_depth ：名为双边滤波，但值域参数 sigmaR = 100000 使值域项完全失效；真正起作用的是「只平均与窗口均值相差 < 1 cm 的像素」这道门限。它会平滑表面，也会把无效像素用邻居的值填上。

图4-1 物体深度之预处理示意图

物体附近的放大。变化图中，红色为 erode 删除的像素，绿色为双边滤波填充的像素。

实测（整幅图）：erode 删除 11,973 个像素，双边滤波填充 17,202 个、只删除 30 个，保留像素的深度平均只改变 0.59 mm。erode 在倾斜的桌面上删出了成行的横条（1 mm 阈值加上深度按毫米量化，过于严格），随后几乎全被双边滤波补回；物体轮廓外原本就有一圈传感器无效带（黑环），有效区域被向外扩了 1~2 px。净效果是深度更完整、离群点更少。

💡 为什么网络吃的是 XYZ 而不是深度？ 图像在 M3 中会被裁剪、缩放到 160 × 160，此后单个像素的深度值已无法说明「这条射线朝哪个方向」；而 XYZ 直接给出 3D 位置，与相机内参、裁剪比例无关，换相机也不需要重训。另外，XYZ 便于做 M3 中「以物体为中心」的归一化。

5. M2 位姿假设生成：一个平移 × 252 个旋转

5.1 初始平移只用一个点

python 复制代码

uc, vc = mask 外接框中心
zc = median(depth[mask & 有效])                  # mask 内深度的中值
t0 = zc * K⁻¹ @ [uc, vc, 1]                     # (3,)，相机系

mustard 首帧：mask 外接框中心 (136.5, 306.0)，深度中值 0.782 m， t 0 t_0 t0 = (−0.450, 0.116, 0.782) m。用中值是为了抵抗 mask 边缘混入的背景深度。

⚠️ t 0 t_0 t0 在深度方向系统性偏近 ：相机只能看到物体朝向自己的那一面， z c z_c zc 是可见表面的深度，而物体中心在表面之后。实测最终位姿比 t 0 t_0 t0 远了 16.5 mm，x、y 也各差约 1 cm。这里不修正，交给 refiner。

5.2 与旋转组合

把 M0 的 252 个旋转的平移部分统一设为 t 0 t_0 t0，得到 252 个粗假设 T i = $R i ∣ t 0$ T_i = $R_i \\mid t_0$ Ti= $Ri∣t0$ ，shape (252,4,4)：

图5-1 252 个假设中的一部分

左上为观测；第 1 行是同一视角的 6 个平面内旋转，第 2、3 行是 12 个不同视角。平移都相同，所以物体大小、位置一致，只有朝向不同。

💡 为什么只枚举旋转？ 平移几乎可以从深度直接读出，误差只有几厘米且是单峰的，一个初值加局部精修就够；旋转则无法直接读出，是多峰的（存在对称、遮挡带来的歧义），必须全局撒点。搜索空间从 6D 降到 3D------如果平移也要枚举，比如每个方向 5 个值，假设数会变成 252 × 125 ≈ 3 万。

6. M3 位姿条件裁剪与渲染：为每个假设准备一对「可比」的图

这是 refiner 和 scorer 每次前向之前 都要做的数据准备：对每个假设，得到一张渲染图 A 和一张观测图 B。refiner 每轮迭代都基于最新位姿重做一次。

6.1 裁剪窗口：假设下物体应该出现的区域

裁剪窗口是原图上的一块正方形区域：如果这个假设是对的，物体应该出现在画面的哪里（外加一圈余量）。 它的中心是假设平移 t t t 的投影，半边长是 1.2 × d / 2 1.2 \times d/2 1.2×d/2（ d d d 为直径）投影到图像上的长度。

图6-1 裁剪窗口

红框：refiner 第 1 轮的窗口（中心为 t 0 t_0 t0 的投影，边长约 125 px）；青框：scorer 对最终位姿的窗口（crop_ratio 1.1）。

这个窗口有两个用途：从真实图像中抠出这块区域，得到 B；告诉渲染器只渲染 这块区域，得到 A。两者透过同一个窗口生成，因此像素对齐。代码中用一个 3 × 3 矩阵 tf_to_crops（原图像素 → 裁剪图像素）表示它，相当于定义了一台「对准窗口、放大到 160 × 160 的虚拟相机」，内参为 K c r o p = tf_to_crops ⋅ K K_{crop} = \texttt{tf\_to\_crops} \cdot K Kcrop=tf_to_crops⋅K。

💡 为什么用「平移 + 直径」而不是检测框来定窗口？

为平移修正提供反馈：若假设平移在 x/y 上有偏差，观测中的物体就不在裁剪中心；在 z 上有偏差，观测中的物体就比渲染的大或小。窗口随位姿移动，网络只需学会「对齐到中心、对齐到标准大小」。检测框是固定的，做不到这一点。

尺度归一化：不管物体远近大小，裁剪后物体始终约占画面的 1/1.2。

与旋转无关：窗口只由平移和直径决定。因此第 1 轮时 252 个假设的观测图 B 完全相同。

6.2 渲染 A 与抠取 B

A：nvdiffrast 渲染。技巧是在裁剪空间中对投影坐标做一次 2D 仿射，把原图中的窗口拉伸到整个视口，再直接以 160 × 160 光栅化。这样不用渲染 252 张整图再裁剪，计算量只与 160 × 160 有关。同时插值得到每个像素对应表面点的相机系坐标，即渲染 XYZ。
B ：用 tf_to_crops 对真实 RGB（双线性插值）和 xyz_map（最近邻，避免在前后景交界处插出空中的假点）做透视变换。

6.3 以物体为中心的 XYZ 归一化

python 复制代码

xyz = (xyz - t_hypo) / (d / 2)      # 减去当前假设的平移，除以物体半径
xyz[|xyz| >= 2 或无效] = 0

A 和 B 做完全相同的变换，被放进同一个「以假设物体中心为原点、以物体半径为单位」的坐标系。A 中物体坐标落在 $- 1 , 1$ $-1, 1$ $-1,1$ 附近；若假设平移有误差 Δ t \Delta t Δt，B 中物体的坐标相对 A 就整体偏移 Δ t / r \Delta t / r Δt/r------这正是 refiner 要预测的量。离假设中心超过 2 倍半径的点（远处背景、近处遮挡物）被置零。

最终 A、B 各为 (252,6,160,160)：RGB 3 通道 + XYZ 3 通道。

图6-2 第 1 轮的 A/B 输入

第 1 轮的 4 个假设。每行依次为：渲染 RGB、观测 RGB、渲染 XYZ、观测 XYZ、渲染叠加在观测上。四行的 B 完全相同，只有 A 不同；A 只有物体，B 则保留了桌面等背景，网络要自己学会区分。

7. M4 RefineNet：迭代 5 轮的位姿精修

python 复制代码

for _ in range(5):
    A, B = make_crop_data_batch(poses, ...)          # M3：按当前位姿裁剪 + 渲染
    out = RefineNet(A, B)                            # trans (252,3), rot (252,3)
    Δt = out['trans'] * (d / 2)                      # 半径单位 → 米
    ΔR = so3_exp_map(tanh(out['rot']) * 20°)         # 轴角 → 旋转矩阵，单步每个分量 ≤ 20°
    t, R = t + Δt, ΔR @ R                            # 解耦更新

252 个假设一次前向完成，各自独立，没有提前终止，也不做淘汰------挑选是 M5 的事。

7.1 网络结构

图7-1 RefineNet 网络结构

共享权重的孪生编码器：A、B 用同一套 CNN 提取特征，二者处于同一特征空间才便于比较。
在 40 × 40 处融合：先各自提取低层特征（消化渲染图与真实图的成像差异），再在空间分辨率仍较高时沿通道拼接，让后续卷积逐位置比较 A、B 的错位。
Transformer 头：经过 3 次 stride-2 下采样，160 × 160 变为 20 × 20 = 400 个 token。位姿误差是全局量，self-attention 让所有 token 互相交互，适合做这种全局推理。
平移、旋转分两个头：Δt 和 ΔR 属于不同的空间，分开学习更利于训练。

7.2 解耦的位姿更新

t + = t + Δ t , R + = Δ R ⋅ R （论文式 8、9） t^+ = t + \Delta t,\qquad R^+ = \Delta R \cdot R \qquad \text{（论文式 8、9）} t+=t+Δt,R+=ΔR⋅R（论文式 8、9）

💡 为什么不用常规的 T + = Δ T ⋅ T T^+ = \Delta T \cdot T T+=ΔT⋅T？ 展开后 t + = Δ R ⋅ t + Δ t t^+ = \Delta R \cdot t + \Delta t t+=ΔR⋅t+Δt：旋转会绕相机原点 转，把物体整个「甩」到别处，平移被旋转牵连。解耦写法让旋转绕物体自身中心进行（这也依赖 M0 的居中），Δt 与 ΔR 互不影响。

💡 为什么 ΔR 左乘？ 左乘时 ΔR 的旋转轴在相机坐标系下表达。网络只能从画面上看出「往左转一点」「绕视线转一点」这类与画面对齐的修正，而 CAD 的坐标轴定义是任意的、网络无从得知。

另外，旋转更新经过 tanh × 20° 限幅，20° 正是训练时施加的旋转扰动幅度，保证单步更新落在训练分布之内。训练时只迭代 1 次，推理时迭代 5 次：每轮误差缩小后，下一轮的输入仍在训练分布之内。

7.3 实测：5 轮之后发生了什么

图7-2 精修过程

每行一个假设，每列为该轮开始前（最后一列为 5 轮后）的渲染叠加观测图。

第 1 行（最终被选中的假设）：初始朝向明显不对，5 轮后轮廓和纹理都对齐了。
第 2 行：从另一个初始旋转出发，收敛到几乎同一位姿（相差 1.9°）。
第 3 行 ：轮廓同样完美对齐，但露出的是瓶子背面的蓝色标签------它收敛到了相差 178.6° 的「翻转」位姿。芥末瓶前后外形几乎对称，refiner 落入了局部最优。

图7-3 收敛分布

各假设与最终选中位姿的旋转距离。蓝：精修前；橙：精修后。

精修前，252 个假设在 SO(3) 上大致均匀分布；精修后，它们集中到了两处：35 个 落在正确位姿 15° 以内，151 个 落在相差 150° 以上的翻转位姿。平移更新的中位数从第 1 轮的 31 mm 降到第 5 轮的 6 mm（第 1 轮的修正最大，其中有一部分来自 t 0 t_0 t0 的深度偏差）；被选中的假设连续几轮旋转更新都接近 20° 的上限，大角度误差只能分几步纠正。

这说明 refiner 是一个局部优化器，只要有一部分假设落进了正确的「盆地」就够了，剩下的交给 M5。

8. M5 ScoreNet：在比较中选出最好的那个

精修后的 252 个位姿再做一次 M3（crop_ratio 降为 1.1，裁剪更紧，物体占比更大，更利于比较细节），然后送入 ScoreNet。

图8-1 ScoreNet 两级比较

第一级：单个假设内部的比较 。backbone 与 RefineNet 结构相同（权重独立），A、B 编码融合后，在 400 个 patch 之间做 self-attention，平均池化成一个 512 维特征 F i \mathcal{F}_i Fi，描述第 i 个假设与观测的吻合程度。
第二级：假设之间的比较 。把 252 个 F i \mathcal{F}_i Fi 视作一个序列，在它们之间做 self-attention，再经线性层得到每个假设的分数，shape (252,)。最后取分数最高者，乘上 M0 的补偿矩阵转回原始 CAD 坐标系输出。

💡 为什么要第二级？ 论文指出，直接把 F i \mathcal{F}_i Fi 映射成分数，等于要求网络给出「绝对分数」，这很难学：无纹理、遮挡、反光时，即使位姿正确吻合度也可能很差，不同场景之间的分数更没有可比性。而「这一批候选里谁最好」是相对判断，容易得多。第二级 attention 让每个假设都能「看到」其他所有假设。

💡 不加位置编码：第二级把假设当作一个集合，结果与排列顺序无关；attention 又天然支持变长输入，所以训练时每组 K = 5 个假设，推理时 K = 252 照样能用。

训练使用「位姿条件」的三元组损失：要求离真值更近的假设比更远的分数至少高一个间隔 α \alpha α，且只在较好的那个假设足够接近真值（旋转误差 < 10°）时才比较------两个都离真值很远的假设，谁好谁坏本就说不清。

实测：scorer 用纹理解决了对称歧义

图8-2 按分数排序的假设

排名前 6 与最后 3 的假设。上排为渲染叠加观测，下排为渲染 XYZ。

前 31 名全部是正确位姿 ；151 个翻转位姿在形状上与观测几乎完全吻合，但最好的一个只排第 32。能区分它们的只有纹理：正面红色标签 vs 背面蓝色标签。
差距却很小：正确与翻转只差 0.19 个 logit，252 个假设的分数跨度也不到 2。可见这个分数只有相对意义，适合排序，但不宜跨帧、跨物体直接设阈值来判断估计是否可信。

9. 总结：七个关键设计

生成 → 精修 → 选择。252 个粗假设负责覆盖 SO(3)，refiner 负责把附近的假设拉到真值，scorer 负责挑出最好的。三者各司其职，每一步的难度都降低了。
只枚举旋转，平移只估一个。平移可以从深度直接读出，旋转多峰、有歧义，搜索空间因此从 6D 降到 3D。
以物体为中心的统一约定。原点 = 几何中心，尺度 = 直径。裁剪、XYZ 归一化、平移输出都以此为基准，网络与物体的绝对大小、距离、CAD 原点都无关。
位姿条件裁剪。窗口由假设的平移和直径决定、随位姿更新而移动，为平移修正提供反馈；渲染图与观测图共用同一窗口，像素对齐。
RGB + XYZ 作为输入。XYZ 与内参、裁剪解耦；减去假设平移后，平移误差直接体现为观测相对渲染的坐标偏移。
解耦的位姿更新。旋转绕物体中心、在相机系下表达，平移与旋转互不牵连，也与 CAD 坐标轴的定义无关。
相对打分。scorer 在所有假设之间做 self-attention，把难以标定的「绝对好坏」变成容易学习的「相对排序」------实测中，正是它靠纹理从形状几乎相同的两类假设里选对了答案。

附：代码阅读路线

复制代码

run_demo.py
├── FoundationPose.__init__                   estimater.py
│   ├── reset_object                          ← M0 居中、直径、渲染张量
│   └── make_rotation_grid                    ← M0 252 个旋转
└── FoundationPose.register                   estimater.py
    ├── erode_depth / bilateral_filter_depth  ← M1（Utils.py）
    ├── generate_random_pose_hypo             ← M2
    │   └── guess_translation
    ├── PoseRefinePredictor.predict           ← M4（learning/training/predict_pose_refine.py）
    │   └── for 5 轮:
    │       ├── make_crop_data_batch          ← M3
    │       │   ├── compute_crop_window_tf_batch / nvdiffrast_render（Utils.py）
    │       │   └── kornia warp_perspective / transform_batch
    │       ├── RefineNet.forward             （learning/models/refine_network.py）
    │       └── egocentric_delta_pose_to_pose
    ├── ScorePredictor.predict                ← M5（learning/training/predict_score.py）
    │   └── ScoreNetMultiPair.forward         （learning/models/score_network.py）
    └── argsort → poses[0] @ get_tf_to_centered_mesh()

本文的图由 docs/scripts/make_figures.py 生成：它在首帧上调用一次 register，通过包装数据准备函数、给网络挂 forward hook 记录中间结果，不修改任何源码。流程图由 docs/scripts/diagrams/ 下的源文件渲染。