FoundationPose 6D 位姿估计

本文最后更新于：2026年7月9日上午

FoundationPose（CVPR 2024 Highlight，NVIDIA）用一个模型实现零样本泛化到新物体——给定 CAD 模型或几张参考照片即可，不重新训练。它的核心是 render-and-compare（渲染对比），把位姿估计从端到端回归转成带监督的图像判别问题。这篇记录它的算法原理和源码架构要点（不含具体代码）。

问题

传统 6D 位姿估计方法的痛点：PCA + ICP 需要好初值、对称物体主轴不唯一、对遮挡反光敏感；关键点 + PnP（如 PVN3D）每个物体要单独训练，换新物体要重新标注训练；直接回归（如 PoseCNN）旋转回归精度差、泛化弱。

FoundationPose 的目标是：一个模型，零样本泛化到从未见过的新物体，不重新训练。同时支持两种输入——model-based（有 3D CAD 模型）和 model-free（只有几张参考照片，无 CAD）。

核心思想

不直接回归位姿，而是：生成多个候选位姿 → 在每个位姿下用网格渲染出"物体应该长什么样" → 和真实观测（RGB-D）对比 → 由网络回归位姿修正量、再打分挑出最优。这套范式叫 analysis-by-synthesis（或 render-and-compare），更接近"人眼认物体"的方式——“从这个角度看它应该长这样”。

非可微渲染

容易误解的一点：FoundationPose 不做可微渲染优化。它不是把 loss 对位姿求梯度反传——渲染本身在 @torch.inference_mode() 下、不反传梯度。它做的是 DeepIM 风格的**“render → compare → 网络直接回归 Δpose”** 的迭代（5 次），每轮重新渲染构成闭环。这样既保留了 render-and-compare 的判别力，又避开了可微渲染梯度不稳、算力昂贵的坑。

与 ICP 对比

维度	ICP（传统几何）	FoundationPose（学习 + 渲染对比）
对比的"东西"	3D 点的空间坐标	2D 图像外观 + 几何（RGB-D）
距离度量	点到点 / 点到面（欧氏几何）	CNN+Transformer 学到的判别距离
搜索方式	局部梯度下降（需好初值）	全局 icosphere 假设采样 + 局部神经细化
初始化	需好初值（PCA / 上一帧）	Register 模式不需要初值
鲁棒性	外点 / 遮挡 / 反光敏感	大规模合成训练见过 → 鲁棒

核心转变：把"位姿估计"从几何优化变成图像判别——度量函数本身是被学出来的（而非手写的点到点距离），所以能学到"哪些像素/通道对位姿敏感、哪些是噪声/遮挡"。

架构

源码里只有两个可训练网络（RefineNet、ScoreNet）加一个不可微的 GPU 光栅化器（nvdiffrast）：

                ┌─────────────────────────────┐
网格 mesh ─────▶│   nvdiffrast 光栅化器          │
位姿 (R,t) ────▶│   (不可微, GPU, 支持光照)      │──▶ 渲染裁剪图 A (RGB + xyz)
                └─────────────────────────────┘
                ┌─────────────────────────────┐
真实 RGB-D ────▶│ warp_perspective 裁剪对齐     │──▶ 观测裁剪图 B (RGB + xyz)
                └─────────────────────────────┘
                              │
                              ▼
          ┌────────────────────────────────────────┐
          │  RefineNet:   回归 Δt, ΔR → 更新位姿     │  (迭代 5 轮)
          │  ScoreNet:    回归标量分数 → 选最优      │  (仅 register 末轮)
          └────────────────────────────────────────┘

位姿参数化

物体位姿 $T=(R,\mathbf{t})\in SE(3)$，表示物体坐标系到相机坐标系的刚体变换。物体的网格预先做一次居中（减去质心），让旋转的枢轴落在物体几何中心，使网络学到的增量在所有物体上对称、可归一化。

RefineNet 输出相对当前位姿的增量 $(\Delta\mathbf{t},\Delta R)$，按"自我中心"方式更新（在相机系下增量）：

$$ \mathbf{t}_{k+1}=\mathbf{t}_k+\Delta\mathbf{t},\qquad R_{k+1}=\Delta R\cdot R_k $$

注意旋转是左乘（相机系增量）。输出做限幅防止大跳变：平移用 $\tanh(\cdot)\odot\text{trans\_normalizer}$（逐通道），旋转 $\Delta\omega=\tanh(\cdot)\cdot\text{rot\_normalizer}$ 再经 so3 指数映射转旋转矩阵 $\Delta R=\exp_{SO(3)}(\Delta\omega^\wedge)$。tanh 把无界回归压到 $[-1,1]$ 乘以 normalizer 得到物理合理的最大步长——这是迭代收敛的工程保证（类似 trust-region），每轮只能做有限修正。

平移估计

平移不需要全局采样，直接从 mask 闭式求：

$$ \mathbf{t}=K^{-1}\begin{bmatrix}u_c\\ v_c\\ 1\end{bmatrix}\cdot z_c,\qquad z_c=\text{median}\big(D[\text{mask}>0]\big),\quad (u_c,v_c)=\text{mask 质心} $$

所有候选位姿共享这一个 $\mathbf{t}$，只需对 $R$ 做全局假设采样。这是一个关键降维：从 $SE(3)$（6 维）降到 $SO(3)$（3 维）。

旋转采样

绕物体的相机方向用正二十面体球面（icosphere）顶点离散采样：逐级细分直到顶点数 ≥ 40（约 42 个），每个顶点构造一个相机到物体的旋转（look-at），每个视角再叠加面内旋转 0°/60°/120°/180°/240°/300°（6 个）。裸候选约 $42\times6\approx252$ 个。

裸候选里大量视角在对称物体下其实等价，需去重。判据：两个位姿 $T_1,T_2$ 同簇当且仅当存在对称变换 $S$ 使

$$ \lVert\mathbf{t}_1-\mathbf{t}_2\rVert其中 $d_{\text{geo}}(R_a,R_b)=\arccos\big(\tfrac{\operatorname{tr}(R_a^\top R_b)-1}{2}\big)$ 是旋转的测地距离，阈值约 30°。聚类后候选数随物体对称性降到几十到上百个。

为什么用 icosphere 而非经纬度网格？icosphere 顶点在球面上近似均匀（每点占据的立体角近似相等），经纬度网格两极过密、赤道过稀。均匀采样保证最坏情况下的视角覆盖，用最少候选达到给定角度分辨率。

Register 模式

首帧定位（不依赖初值）的完整流程：

Step 0  深度预处理: erode → bilateral filter → depth2xyz map
Step 1  guess_translation:   mask 质心 + 深度中位数 → t  (所有候选共享)
Step 2  generate_random_pose_hypo: icosphere×6 → 聚类 → N 个候选旋转 R_i
        候选 T_i = [R_i | t],  i=1..N (典型 N=几十~上百)
Step 3  RefineNet.predict(iteration=5):  对全部 N 候选并行跑 5 轮细化
Step 4  ScoreNet.predict:  对细化后的 N 个位姿打标量分
Step 5  argsort 降序 → 取分数最高者作为最优位姿

N 个候选完全并行（网络前向 batch 可达 1024，渲染 batch 512），这是 register 模式虽慢但可接受的关键。

细化流程

对每个候选位姿 $T_k$ 执行以下流程：

(a) 计算 3D 包围盒裁剪窗口：以位姿中心为圆心、半径取物体直径的某个比例，投影到像素取外接矩形，再 warp 到固定输出尺寸（如 160×160）。这保证渲染图和观测图在同一个 2D 裁剪坐标系下对齐。

(b) 渲染图 A：用 nvdiffrast 对居中网格在位姿 $T_k$ 下做 GPU 光栅化，输出 RGB（带环境光加漫反射）和 3 通道 xyz_map（每像素的相机系 3D 坐标）。

© 观测图 B：把原始 RGB 和 xyz_map（由深度反投影得到）按同一个裁剪变换 warp 到相同尺寸，与 A 像素级对齐。

(d) 几何通道归一化（跨物体泛化的关键）：xyz 不直接喂网络，而是中心化加尺度归一化：

$$ \tilde{\mathbf{x}}_{A/B}(u,v)=\frac{\mathbf{x}_{A/B}(u,v)-\mathbf{t}_k}{\text{diameter}/2} $$

每像素 3D 坐标减去当前位姿平移、除以物体半径，让不同大小、不同距离的物体的几何特征都落在相近量级。这是跨物体泛化的关键归一化。

(e) 网络输入：每张图 6 通道（RGB 3 加归一化 xyz 3）。用 3D xyz 而非单通道深度，是因为 xyz 保留了完整 3D 几何，网络能直接从几何偏移判读位姿误差。

RefineNet

RefineNet 是双分支权重共享的 CNN + Transformer，把"渲染图 A vs 观测图 B"的差异编码成位姿修正量。

关键技术点：

权重共享双分支编码：对 A、B 用同一套卷积权重（实现上把 A、B 沿 batch 维 stack 一次性前向再拆回），保证渲染域和真实域的特征落在同一子空间——这是跨域（合成渲染 vs 真实拍摄）对齐的核心。
拼接后深层融合：早期分别提特征（保留模态特异性），中期 cat 成 256 通道后再深层 ResBlock 融合，让网络学到"A 和 B 在哪里不一致"。
Transformer 聚合：把 2D 特征图展平成 token 序列（加正余弦位置编码），用一层 TransformerEncoder 让 token 间全局交互。自注意力能学到"看哪几个像素位置最判断位姿误差"（遮挡区域 token 的注意力权重自然变低）。
双头输出：一头输出 3 维 $\Delta\mathbf{t}$，一头输出 3 维轴角 $\Delta\omega$，分别 mean-pool 得每个候选的增量。

训练目标是回归位姿增量的 L2 损失（可选 ADD 度量）：

$$ \mathcal{L}_{\text{refine}}=\big\lVert\Delta\mathbf{t}_{\text{pred}}-\Delta\mathbf{t}_{\text{gt}}\big\rVert_2^2+\lambda\big\lVert\Delta\omega_{\text{pred}}-\Delta\omega_{\text{gt}}\big\rVert_2^2 $$

ScoreNet

RefineNet 是逐候选独立回归——它不知道别的候选长什么样。当初始假设离真值太远或物体严重遮挡时，5 轮迭代未必把每个候选都带到真值附近，此时需要一个看全局、做相对排序的网络。

ScoreNet 与 RefineNet 的核心区别：多了一个候选间交叉注意力，让 L 个候选假设互相 attend——每个候选的分数是在"看到其它候选长什么样"之后才给出的。这让网络学到的不是绝对分数，而是相对判别（“这一堆里谁更像真的”）。

对应的损失是对比损失族（pairwise / InfoNCE 风格）：给定一批候选和真值位姿 $T^*$，让最接近真值的那个候选 logit 最大：

$$ \mathcal{L}_{\text{score}}=-\log\frac{\exp\,s_{i^*}}{\sum_i\exp\,s_i},\qquad i^*=\arg\min_i\,d_{\text{ADD}}(T_i,T^*) $$

其中 $d_{\text{ADD}}(T_a,T_b)=\tfrac{1}{M}\sum_m\|T_a x_m - T_b x_m\|$ 是 ADD（Average Distance of model points）位姿误差度量——物体 CAD 的 M 个顶点在两位姿下变换后的平均欧氏距离；$s_i$ 是 ScoreNet 给候选 i 的 logit。

直接对全部 N 个候选做完整交叉注意力代价过高，所以用锦标赛淘汰：逐轮两两比较取胜者、败者淘汰，每轮减半直到只剩一个冠军，冠军给加分确保 argsort 后排第一。整个过程是 $O(\log N)$ 轮、共 $O(N)$ 次成对比较（每轮 $N/2, N/4, \ldots$，求和约 $N$ 次，每次一次网络前向），替代一次性 $O(N^2)$ 的全局注意力。

Track 模式

已知上一帧位姿后，每帧只用 RefineNet 在上一帧位姿附近单假设细化（不用 ScoreNet）。因为只剩 1 个假设，没有候选间比较的需求；帧间运动小，假设已在收敛域内。这就是 register 慢（全局搜索加评分）、track 快（单假设细化）的工程分工。

零样本泛化

FoundationPose 对新物体不微调，由四个机制共同保证：

网络只看"渲染图 vs 真实图"对，不看物体身份：RefineNet / ScoreNet 输入只有 A、B 两个张量，没有任何物体 ID 或嵌入向量。网络学的是通用的图像判别函数，对所有物体共享同一套权重。
几何通道的尺度归一化：xyz 通道用"减当前位姿平移、除物体半径"归一化，让"小物体近拍"和"大物体远拍"在网络上看起来尺度一致。新物体只要能算出直径就自动落入训练时见过的几何尺度分布。
大规模合成训练 + 域随机化：训练不依赖真实位姿标注（极贵），而是用 NVIDIA Isaac Sim/Omniverse 渲染几万种物体（来自 Objaverse 等大模型库），施加随机纹理、随机光照、随机背景（sim-to-real），并用 LLM 辅助扩大物体外观多样性。
Model-free 用 Neural Object Field 统一到 mesh 管线：没有 CAD 时，先用几张参考 RGB-D 训练一个神经隐式表示（NeRF 风格的 Neural Object Field），导出为 mesh；之后 Register/Track 全程用这个 mesh 渲染，与 Model-based 完全共用同一套网络。统一的本质是：下游网络只关心"能不能在任意位姿渲染出物体的 RGB-D"，不关心 mesh 来自 CAD 还是 NeRF 重建。

小结

FoundationPose 的核心是 render-and-compare：icosphere 全局采样 N 个候选旋转（平移由 mask 闭式估出，降维到 SO(3)），对每个候选用 nvdiffrast 渲染 RGB-D 与真实观测裁剪对齐，RefineNet（双分支权重共享 CNN+Transformer）回归位姿增量迭代 5 轮，ScoreNet（带候选间交叉注意力）打分挑最优。关键澄清是它不是可微渲染，而是 DeepIM 风格的网络回归增量；零样本泛化靠"网络不识物体身份 + xyz 尺度归一化 + 大规模合成训练 + model-free 统一 mesh 管线"四机制。代价是秒级、需 GPU，与毫秒级的 ICP 互补——后者快但需初值、对遮挡敏感，前者慢但无需初值、鲁棒。

参考资料

B. Wen et al., “FoundationPose: Unified 6D Pose Estimation and Tracking of Novel Objects,” CVPR, 2024.
Y. Li et al., “DeepIM: Deep Layered Matching for 6D Pose Estimation,” ECCV, 2018.（render-and-compare 姿态细化的先驱）
T. Gleicher et al., “nvdiffrast: Gradient-based primitives,” 2020.
相关笔记：PnP 算法、PnP 的本质、ICP 配准