物体位姿估计 · 2026 · ICRA / IEEE RA-L · CCF B

Simultaneous Extrinsic Contact and In-Hand Pose Estimation via Distributed Tactile Sensing

TacGraph 把双 GelSight 预测的触觉几何、抓内位移与 wrench 写成因子图，并联合约束几何一致性、非穿透、接触运动学和力平衡，同时估计抓内 rest pose、外部接触点与力。触觉单模态接触点误差由 ICP 的 17.53 mm 降至 3.72 mm，3 mm 间隙开环插入成功 23/40；但依赖已知物体 / 环境 mesh、弹性无滑移抓持与单点接触。

Paper Project

研究方向

物体位姿估计

任务层级

Instance-level

物体设定

Unseen objects

输入模态

two GelSight Mini tactile streamsgripper proprioceptionoptional external RGB-D point cloud

关键依赖

known rigid object meshknown static environment meshtrained tactile depth / displacement / wrench modelselastic grasp without slipsingle point contact without torque

完整论文元数据作者、单位、研究方向、数据集、指标、方法标签和阅读状态展开

作者: Mark Van der Merwe, Kei Ota, Dmitry Berenson, Nima Fazeli, Devesh K. Jha
作者单位: University of Michigan / Mitsubishi Electric / Mitsubishi Electric Research Laboratories
会议 / 期刊: 期刊 · ICRA / IEEE RA-L · CCF B
研究方向: 物体位姿估计
开源状态: 未开源 / 未整理
阅读状态: 粗读：已生成精读：已生成
任务设定: Instance-level · Unseen objects
数据集: self-collected GelSight contact-pose benchmark self-collected tactile-only peg-insertion benchmark
评估指标: ADD Rotation Error Translation Error
方法标签: tactile vision-tactile in-hand-pose extrinsic-contact factor-graph contact-physics force-balance non-penetration particle-initialization peg-insertion 未知物体级位姿估计绝对位姿估计
关键依赖: known rigid object meshknown static environment meshtrained tactile depth / displacement / wrench modelselastic grasp without slipsingle point contact without torque

Reading Notes

粗读 / 精读

已生成 src/paper-notes/simultaneous-contact-inhand-pose-icra2026/coarse.md

论文粗读笔记

用于快速判断论文定位、问题设定、核心方法、实验结论和是否值得精读。

论文粗读：TacGraph

一句话概括

TacGraph 用双指触觉预测出的局部几何、抓内位移和 wrench，加上已知物体 / 环境几何，在因子图中同时估计抓内物体位姿、外部接触点与接触力；其价值不只是“触觉辅助位姿”，而是用接触物理让三个相互歧义的未知量彼此约束。

研究问题

机器人做插入、工具使用或装配时，既要知道物体在夹爪中的精确 pose，也要知道物体在哪里、以多大力接触了环境。单个触觉 patch 很局部，同一触觉形状可能对应多个物体位置；视觉虽能提供全局几何，却会受遮挡与深度噪声影响。论文因此问：能否把视觉 / 触觉观测与准静态接触约束统一起来，同时恢复 pose 和 extrinsic contact？

方法主线

系统使用夹爪两侧的 GelSight Mini。三个 object-agnostic 触觉网络分别输出：

初始触觉深度并形成局部点云；
相对初始抓持状态的 in-hand displacement；
夹爪处 6D wrench，并由其幅值判断是否发生外部接触。

因子图的核心变量是单个 object rest pose、各接触时刻的接触点和接触力。当前物体 pose 由 rest pose 与触觉位移组合得到。四类因子分别检查：

触觉与可选视觉点云是否落在物体表面；
物体是否穿透已知环境；
接触点是否同时位于物体和环境表面；
该接触点与力产生的 wrench 是否匹配触觉预测。

TacGraph 触觉模型与物理因子图总览 — 原论文 Figure 2：触觉深度、抓内位移与 wrench 进入由几何一致性、非穿透、接触运动学和力平衡组成的因子图。来源：TacGraph 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

关键结果

Vision + Tactile 下，六个 train / test 几何的 ADD 多数由 TacGraph 最低，分别为 0.68、1.48、1.34、1.05、0.92、1.62 mm。
Tactile-only 下，接触点整体误差为 3.72 ± 3.78 mm；ICP、CHSEL、SCOPE v1 / v2 分别为 17.53、24.41、16.20、10.40 mm。
接触力误差约 0.61 ± 0.36 N。不过各方法共享同一个 learned wrench model，因此这一项不能单独证明因子图的优势。
3 mm 间隙的 tactile-only 开环 peg insertion 中，TacGraph 成功 23/40；ICP 11/40、CHSEL 14/40、SCOPE v1 7/40、v2 3/40。

TacGraph 位姿与接触点定量结果 — 原论文 Tables I–II：TacGraph 在视觉触觉与纯触觉设置中的 pose 和 contact point 误差；当前截图没有接触力 Table III，正文力误差另据原表。来源：TacGraph 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

适用条件与局限

方法需要已知 rigid object mesh 和静态环境 mesh，并假设抓持具有弹性：外力消失后物体回到同一个 rest pose；若发生滑移，该模型失效。外部接触还被简化为单点、无接触 torque。因子图是局部优化，依赖 ICP 等方式生成的初始 pose particles；对高度对称对象仍需更多粒子。quarter cylinder 的插入仅 1/10，也暴露了 GelSight 法向深度不稳定会直接污染结果。

阅读判断

这篇论文最值得借鉴的是“联合变量设计”：pose 决定什么接触可行，contact 又反过来排除错误 pose。它把视觉和触觉从简单特征拼接提升为物理一致性联合推断。若目标是开放世界 / 无 CAD 操作，则还需与在线重建、滑移检测和多接触建模结合。

已生成 src/paper-notes/simultaneous-contact-inhand-pose-icra2026/deep.md

论文精读笔记

用于复现、对比、写 related work 和挖掘后续研究切入点。

论文精读笔记：TacGraph

1. 核心判断

论文：arXiv 2512.23856
项目页：TacGraph
核心问题：局部触觉无法单独消除抓内 pose 与外部接触的歧义，视觉又可能被遮挡；但 pose、接触位置和接触力必须共同满足刚体接触几何与准静态力学。
核心回答：先把双 GelSight 图像转换为局部表面、抓内位移与 wrench，再把几何一致性、非穿透、接触运动学和力平衡写成一个因子图，联合求 rest pose、contact point 与 contact force。
我的判断：真正贡献是把“接触是干扰”改写为“接触是约束”。不过其高精度建立在已知物体 / 环境 mesh、单点接触、无滑移弹性抓持和良好初始化上，不能直接外推到任意在手操作。

2. 任务与状态变量

系统已知：

物体三角 mesh $\mathcal M_o$ ；
静态环境 mesh $\mathcal M_e$ ；
夹爪与相机标定；
两侧 GelSight Mini 图像；
可选外部 RGB-D 点云。

每个时刻希望恢复物体 pose $o_t$ 、外部接触点 $c_t$ 与接触力 $f_t$ 。作者不是为每帧独立优化完整 pose，而是假设夹爪 / 触觉弹性形变引起的相对运动可由网络测得，并引入一个跨时刻固定的 rest pose $o_r$ ：

o_t=g_t\,\delta_t\,o_r,

其中 $g_t$ 是夹爪 pose， $\delta_t$ 是触觉网络给出的 in-hand displacement。外力撤去后，物体应回到同一 $o_r$ ；这个压缩变量的设计减少了时序自由度，但也明确排除了永久滑移。

3. 从触觉图像到可优化观测

论文训练三个 object-agnostic convolutional models。

3.1 Tactile depth

初始时刻两侧 tactile RGB 经 depth model 得到局部高度图，再转换为抓爪坐标系中的 tactile point cloud $P^T$ 。若视觉可用，再与外部点云 $P^V$ 合并为 $P$ 。训练标签由已知几何 fixture 在随机抓持下渲染得到。

3.2 In-hand displacement

网络读取初始与当前的 tactile image pair，回归 $\delta_t\in SE(3)$ 。训练时把已知 fixture 固定后施加小幅 gripper delta motion，因此监督信号是夹爪相对 fixture 的已知变换。该网络承担“触觉变形到抓内刚体位移”的映射，误差会直接进入每个时刻的 pose。

3.3 In-hand wrench

第三个网络从左右 tactile images 预测夹爪处 wrench $w_t\in\mathbb R^6$ ，并用加权范数阈值给出 contact boolean $b_t$ 。训练由环境下方 ATI Gamma F/T sensor 提供 wrench 真值；运行时 ATI 不参与 TacGraph 推断。

TacGraph 从双触觉观测到因子图的完整结构 — 原论文 Figure 2：视觉点云是可选项；三类触觉中间量和已知几何共同约束 rest pose、contact point 与 contact force。来源：TacGraph 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

4. 四类因子

整体 MAP 被写为带 Gaussian factor covariance 的非线性最小二乘，并用 GTSAM / iSAM2 求解。

4.1 几何一致性 $h_1$

将观测点云变换到 object frame，计算其到 $\mathcal M_o$ 的 signed distance。若 pose 正确，初始 tactile / visual points 应位于物体表面附近。该因子只约束 rest pose；当前 pose 再由 $\delta_t$ 传播。

4.2 非穿透 $h_2$

在物体表面采样点，变换到环境坐标后评估 $\operatorname{SDF}(\cdot\mid\mathcal M_e)$ ，只惩罚位于环境内部的负距离：

h_2=\min(0,\operatorname{SDF}(o_t p\mid\mathcal M_e)).

它排除视觉 / 触觉局部上貌似合理、但让物体穿进桌面或 fixture 的 pose。

4.3 接触运动学 $h_3$

接触点必须同时落在 object surface 与 environment surface。因此 $c_t$ 到两个 mesh 的 signed distance 都应接近零。这个约束让 contact 反向限制 pose：若某个 pose 让候选接触点无法同时触及两面，就应被淘汰。

4.4 力平衡 $h_4$

把 contact force 通过接触 Jacobian $J(o_t^{-1}c_t)$ 映射成 grasp frame wrench，并与 tactile network 的 $w_t$ 匹配：

h_4=J(o_t^{-1}c_t)f_t-w_t.

接触位置与力在 torque 项中耦合，因此相似的 force reading 可由 pose / geometry consistency 进一步消歧。 $h_3,h_4$ 只在 $b_t=1$ 时启用。

5. 多粒子初始化

这是局部非线性问题。作者先用 ICP 与 grasp-direction heuristic 生成 $K$ 个 rest-pose particles；每个粒子独立维护 / 优化一张图，再以总 factor cost $H$ 选最优。多数对象 1–2 个初始化已够，但实验统一使用每对象 10 个，以覆盖对称性和局部极小值。

TacGraph 预测 pose 接触以及粒子权重随接触更新的过程 — 原论文 Figure 5：后续接触使初始时几何上都合理的 pose particles 产生不同物理代价，从而保留正确局部解。来源：TacGraph 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

这里需要避免一个误读：TacGraph 没有构造严格的全局 posterior；它是“多初值局部 MAP + 代价择优”。图中的 particle weight 是 $e^{-H}$ 的相对表现，不等同于经过归一化和校准的 pose uncertainty。

6. 实验设置与真值

硬件包含 KUKA LBR iiwa Med R820、WSG-50、每指一枚 GelSight Mini，以及可选 Intel RealSense D435。物体与环境下的 ATI F/T sensor 只用于采集训练 / 评测真值。

pose / contact benchmark 每次固定做 3 个有角度的 poke。pose 真值来自 object-specific fixture，作者给出的 fixture tolerance 约 1 mm；ADD 使用 mesh 上 1000 点。外部接触真值由环境 F/T sensor 推断：在环境表面采样 candidate points，寻找使 $c\times f$ 与测得 torque 最匹配者。这意味着 contact-point “真值”本身也是由单点力矩模型反演，而不是独立高精度触点追踪。

7. Pose、接触点与力

TacGraph 与 ICP CHSEL SCOPE 的 pose 和 contact 定量比较 — 原论文 Tables I–II：位姿与接触点误差，灰色标出各感知设置中的最优结果。当前截图没有接触力 Table III；正文力误差另据原表。来源：TacGraph 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

7.1 Vision + Tactile pose

六个几何上，TacGraph ADD 均在 0.68–1.62 mm。对应 ICP 为 2.19–4.86 mm，CHSEL 在 slanted rectangle 上达到 12.28 mm。视觉存在时 ICP 已不差，但接触约束仍能把解拉回物理可行区域。

7.2 Tactile-only pose

TacGraph 在 cylinder / rectangle / quarter cylinder / slanted rectangle / wrench / screwdriver 上为 2.96 / 1.29 / 8.45 / 7.58 / 0.78 / 1.54 mm。它不是所有对象都达到毫米级：曲面和弱法向敏感性仍使 quarter cylinder、slanted rectangle 明显更难。

7.3 Contact point

整体 contact-point error：

方法	Vision + Tactile (mm)	Tactile-only (mm)
ICP	4.14 ± 2.92	17.53 ± 9.75
CHSEL	4.33 ± 5.62	24.41 ± 14.52
SCOPE v1	12.28 ± 13.28	16.20 ± 14.49
SCOPE v2	9.83 ± 11.59	10.40 ± 10.93
TacGraph	3.68 ± 3.72	3.72 ± 3.78

这组结果比“pose 平均更低”更能支持联合估计：去掉视觉后，几何基线的 contact 会随错误 pose 漂移，而 $h_3+h_4$ 仍把接触锚定到环境 / 物体表面与 wrench。

7.4 Contact force

TacGraph 在两种感知设置中都是 0.61 ± 0.36 N；其他方法约 0.61–0.68 N。因为它们使用相同 learned force model，差异主要来自估计 contact position 后 torque consistency 的修正，不能把 0.61 N 解释为独立 force estimator 的全面胜出。

8. 开环 peg insertion

每个对象有专用 3 mm clearance fixture。系统完成探索接触、估计最终 rest pose 后进行开环插入；环境力超过 5 N 时停止并释放。每种方法、每对象 10 次，共 40 次。

TacGraph 纯触觉开环插入成功率 — 原论文 Table IV：TacGraph 总成功 23/40，明显高于四个比较系统；quarter cylinder 仍只有 1/10。来源：TacGraph 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

TacGraph 分对象为 cylinder 7/10、rectangle 6/10、quarter cylinder 1/10、wrench 9/10，总计 23/40。ICP 11/40、CHSEL 14/40、SCOPE v1 7/40、v2 3/40。该实验说明估计误差确实影响下游任务，但仍是最终估计后的 open-loop execution；论文没有展示 estimator 与控制器闭环协同，也没有让机器人主动选择最有信息的 poke。

9. 失败模式与假设

已知几何： $\mathcal M_o,\mathcal M_e$ 都必须准确；CAD 偏差会同时污染四类因子。
弹性 rest pose：外力撤去后必须回到同一 pose；grasp slip 会让所有历史观测无法由单个 $o_r$ 解释。
单点、无 torque contact：面接触、边接触或多点并发会破坏 $c_t,f_t$ 参数化。
准静态：快速冲击和显著惯性未纳入 force balance。
局部优化：对称性依赖粒子覆盖；更多粒子提升鲁棒性但增加计算。
触觉法向质量：quarter cylinder 的 noisy depth 导致 pose 和插入失败。
人工交互序列：实验接触由人工挑选，尚未解决 action selection。
代码可用性：截至本站整理时，官方项目页公开视频和论文，未提供实现仓库。

10. 可复现清单

明确所有坐标系：sensor、gripper、object、environment、robot base。
复现三个触觉网络各自的训练 fixture、数据规模、loss 和 calibration。
保存每个 factor covariance；这相当于不同观测 / 物理约束的相对权重。
对每个对象报告初始化粒子数、ICP seed 和失败粒子比例。
区分 learned wrench error、joint contact localization error 与最终 task success。
增加 mesh perturbation、slip、双接触、面接触和动态冲击消融。
在 closed-loop insertion 中用 contact uncertainty 触发纠偏，而非只执行最终 pose。

11. 方法启发

TacGraph 最可迁移的部分是把中间感知量设计成可进入物理因子的对象：point cloud 对应几何一致性，位移对应状态压缩，wrench 对应力平衡。比起直接把 RGB、depth、tactile feature 拼接进一个大网络，这种设计更容易诊断“哪条约束冲突”。下一步若与 model-free online reconstruction、slip detector 和 multi-contact factor 联合，才可能从精密 fixture 场景走向开放世界操作。

Simultaneous Extrinsic Contact and In-Hand Pose Estimation via Distributed Tactile Sensing