物体位姿估计 · 2026 · ICRA · CCF B

InvariantCloud: A Globally Invariant, Uniquely Indexed Point Cloud Framework for Robust 6-DoF Tactile Pose Tracking

InvariantCloud 预建带唯一 ID 的 475 点触觉全局网格，逐帧无需 LK / 最近邻搜索即可一一匹配：Kabsch 解 roll / pitch，接触轮廓 PCA 解 yaw，质心与高度解平移。一分钟静止测试的平移 MAE 为 0.22 / 0.14 / 0.11 mm，yaw 0.84°；对蛋等弱轮廓也明显优于 NormalFlow，但完全球形接触的 yaw 不可观。

Paper

研究方向

物体位姿估计

任务层级

Instance-level

物体设定

Unseen objects

输入模态

GelSight Mini RGB tactile streamintrinsic marker constellationcontact height map

关键依赖

vision-based tactile sensor with stable markersknown-sphere indentation calibrationno-contact global marker referencecontinuous contact for frame trackingnon-circular contact silhouette for observable yaw

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

作者: Pengfei Ye, Yuxiang Ma, Yi Zhou, Wei Chen, Wenzhen Dong, Molong Duan
作者单位: Hong Kong University of Science and Technology / Massachusetts Institute of Technology / Chinese University of Hong Kong
会议 / 期刊: 会议 · ICRA · CCF B
研究方向: 物体位姿估计
开源状态: 未开源 / 未整理
阅读状态: 粗读：已生成精读：已生成
任务设定: Instance-level · Unseen objects
数据集: self-collected GelSight Mini household-object sequences self-collected multi-contact tactile SLAM sequence
评估指标: Rotation Error Translation Error
方法标签: tactile-only vision-based-tactile marker-point-cloud unique-index kabsch pca yaw-tracking drift-free-registration tactile-slam model-free 未知物体级位姿估计绝对位姿估计
关键依赖: vision-based tactile sensor with stable markersknown-sphere indentation calibrationno-contact global marker referencecontinuous contact for frame trackingnon-circular contact silhouette for observable yaw

Reading Notes

粗读 / 精读

已生成 src/paper-notes/invariantcloud-icra2026/coarse.md

论文粗读笔记

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

论文粗读笔记：InvariantCloud

1. 基本信息

论文：arXiv 2605.25216
作者 / 机构：Pengfei Ye、Yuxiang Ma、Yi Zhou 等；HKUST、MIT、CUHK。
任务：只用 GelSight Mini 的视觉触觉观测，在没有对象 CAD 和外部相机的条件下跟踪接触物体相对传感器的 6D pose，并支持跨接触 tactile SLAM。

2. 一句话定位

InvariantCloud 把传感器表面的 marker lattice 预先变成带永久 ID 的稠密 3D 网格；每帧只激活接触轮廓覆盖的 ID，从而绕过 LK / nearest-neighbor 搜索，以 Kabsch、PCA 和质心直接解 6D 运动。

3. 方法概览

用已知球体在多个位置压入传感器，训练 MLP 从 RGB、像素位置与 marker 信息预测 surface normal，再以 DCT Poisson integration 得到 height map。
无接触时检测原始 marker，双线性插值成带唯一 ID 的稠密参考网格；实验用 19×25，共 475 点。
每帧融合颜色差与高度凹陷得到 contact mask，并选择当前激活的全局 ID 子集。
相同 ID 直接建立前后帧 3D 对应，单次 Kabsch SVD 解 roll / pitch。
对 contact subset 的 2D 分布做 PCA，以主轴恢复绕传感器法向的 yaw。
接触区域 3D 质心变化给出 XYZ translation。

InvariantCloud 全局唯一索引网格和跨帧 ID 一一对应 — 原论文 Figure 2–3：无接触 marker 被插值成稠密永久网格；前后帧按 ID 直接配对，不再执行 nearest-neighbor ICP 或光流搜索。来源：InvariantCloud 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

4. 实验速览

GelSight Mini：320×240，约 25 Hz。
对象：scissors、utility knife、egg、sensor box。
一分钟静止接触的累计 MAE：
- translation：0.22 / 0.14 / 0.11 mm；
- rotation：0.62° / 0.69° / 0.84°。
LK+ICP 对应数值为 2.52 / 3.14 / 2.29 mm 和 10.89° / 8.15° / 7.19°，长时漂移明显。
return-pose 实验每对象 / 方法 5 次；完整方法在 6 个自由度总体最低，NormalFlow 在 egg 的 yaw return error 可接近 20°。
0→−90° yaw tracking 中，NormalFlow 在 egg 接触上近乎不动，在 utility knife 接触上明显滞后且末段停滞；InvariantCloud 在两者上仍随旋转推进。

四个对象在移动后返回初始接触位置的 6D repeatability error — 原论文 Figure 6：每对象 / 方法五次 return test；InvariantCloud（绿）在各自由度总体最低，ICP（红）即使静态也积累明显误差。来源：InvariantCloud 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

5. 快速判断

优点：不用对象 CAD、外部 RGB 或 motion capture；全局 ID 从匹配机制上消除逐帧搜索漂移，PCA 又专门解决 tactile yaw。
关键边界：yaw 来自接触轮廓主轴，不是 marker 随物体刚性转动；完全球形 / 圆形接触轮廓的 yaw 本质不可观。
主要限制：依赖带稳定 marker 的特定视觉触觉传感器、球压标定与连续接触；实验对象和序列规模较小，未报告完整 runtime 分解或公开代码。

已生成 src/paper-notes/invariantcloud-icra2026/deep.md

论文精读笔记

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

论文精读笔记：InvariantCloud

1. 核心判断

论文：arXiv 2605.25216
核心问题：视觉触觉 sensor 的局部接触面积小，marker 会受剪切和滑移；逐帧 LK / ICP correspondence 会误匹配并积累漂移，尤其难恢复绕 sensor normal 的 yaw。
核心回答：先在 sensor frame 固化一个带唯一 ID 的稠密 marker-derived point cloud；每次接触只改变哪些全局点被激活以及它们的高度。直接按 ID 配对解 roll / pitch，以接触轮廓 PCA 解绝对 yaw，以质心解平移。
我的判断：论文最好的思想是把 correspondence 从“每帧估计”改成“初始化时定义”。它显著改善 return repeatability 与弱轮廓 yaw，但所谓 6D pose 是对象相对局部触觉面的接触 pose；可观性受接触轮廓、连续接触和传感器标定强约束。

2. 任务边界

系统不需要：

对象 CAD；
外部 RGB / depth camera；
在线 motion capture；
object-specific learning。

系统需要：

GelSight 类、带稳定 intrinsic marker 的 vision-based tactile sensor；
已知球压入标定；
无接触时的全局 marker reference；
接触期间可提取稳定轮廓和高度；
frame tracking 时保持连续接触。

输出的坐标系是 sensor frame。它适合夹持器内局部接触跟踪，并不直接给出对象在机器人 base / 世界坐标中的全局 pose；要用于操作还需已知 sensor–gripper kinematics。

3. Height map 与全局参考点云

3.1 球压标定

已知直径的球在多个位置压入 elastomer。对每个像素提取：

RGB；
归一化像素坐标；
intrinsic marker 的嵌入信息。

MLP 预测局部 surface normal 的倾角；得到 gradient field 后，用 DCT 求 Poisson equation，积分成 height map $H$ 。像素 $(x,y)$ 经 image center 与 pixel-to-mm scale $s$ 映射到

P=((x-c_x)s,(c_y-y)s,h(x,y)s).

这一步决定 Z translation 与 Kabsch 3D 点的精度。对象无需训练，但传感器必须先训练 / 标定，所以它是 object-agnostic，不是 calibration-free。

3.2 永久 ID

无接触图像中检测 $N_{\mathrm{markers}}$ 个 marker，并按其矩形行列结构排序。作者在相邻 marker 之间做双线性插值，把稀疏网格扩为更密的 lattice；每个网格位置以

g=r(n+1)+c+1

获得全局唯一 ID。实验常用 19×25，即 475 点；跨接触表面重建时增至 31×41。

全局 marker 网格构建与跨帧唯一 ID matching — 原论文 Figure 2–3：绿色稠密 lattice 的坐标和 ID 在 sensor frame 中一次定义；当前 contact mask 只选择其子集，同 ID 直接构成 Kabsch 对应。来源：InvariantCloud 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

关键不是点数更密，而是 identity 不再由 frame-to-frame nearest neighbor 决定。只要接触区域重新覆盖同一全局位置，系统能回到相同 ID 集合，因此 return-to-start 不累积此前匹配误差。

4. Contact subset

每帧融合 color difference 与 height depression 的双阈值，再经 dilation / erosion 得到 contact mask $C_t$ 。在全局 lattice 的整数像素位置采样 mask，筛出当前激活点，并用当前 height map 将其提升为 3D。

这意味着全局 grid 是 sensor-fixed reference，真正随对象 motion 变化的是：

contact silhouette 覆盖哪些 IDs；
对应 grid 点的高度；
contact centroid 与主轴。

marker 本身在 elastomer 上可能发生剪切 / 滑移，算法不把局部 marker displacement 当作对象 yaw 的唯一证据，这正是其区别于 LK+ICP 的地方。

5. 6D 分解求解

5.1 Kabsch 解 roll / pitch

前后帧 contact subcloud 取公共 IDs，得到一一对应的 3D 点集 $\{p_i\},\{q_i\}$ 。去质心后构造

H=\sum_i\tilde p_i\tilde q_i^\top=U\Sigma V^\top,

并以标准 Kabsch

R^\star=V\,\operatorname{diag}(1,1,\operatorname{sign}\det(VU^\top))\,U^\top

求最优刚体旋转。

论文把这里称为 X / Y rotation estimation：它主要利用接触 surface height / normal 变化恢复相对 sensor plane 的 roll、pitch。yaw 另行处理，因为 elastomer 上的 torsional slip 会让局部 marker-based registration 对绕 Z 旋转失效。

5.2 PCA 解 yaw

对当前接触内的 2D 全局点集

S_t=\{p_i\in\mathcal P\mid C_t(p_i)=1\}

计算 covariance $M_t$ ，取第一主轴 $a_t$ ，并定义

\theta_t=\operatorname{atan2}(a_{t,y},a_{t,x}).

除理想圆形外，物体 yaw 会改变 contact silhouette 及其覆盖的全局 IDs；PCA 主轴因此成为 sensor-frame 中的绝对方向，而不是把相邻帧小旋转不断相乘。

NormalFlow 与 InvariantCloud 在 yaw 旋转和返回过程中的漂移对比 — 原论文 Figure 4：marker 会相对对象剪切 / 滑移，NormalFlow 的局部方向逐步漂移；InvariantCloud 每帧都从全局 ID 子集的轮廓主轴重新计算 yaw，返回时恢复初始方向。来源：InvariantCloud 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

PCA 也有明确几何边界：

完全圆形 / 球形接触的 covariance 两个特征值相同，yaw 不可观；
近圆轮廓的主轴会对 mask noise 高度敏感；
主轴天然有 180° 符号歧义，连续实现需要用上一帧保持方向符号一致；
压力改变 contact patch 形状时，主轴变化可能混入“变形”而非刚体 yaw。

5.3 质心解 XYZ

对 contact mask 做 morphological closing，取最大 contour 的 area-moment centroid；利用 height map 提升为 3D。前后帧 centroid 的 XY 差给平面平移，contact region height 给 Z。

这个设计低成本且不漂移，但隐含“contact centroid 的变化等价于对象 translation”。若对象在曲面上滚动、接触 patch 因压力扩张或发生非刚性变形，centroid motion 不完全等于刚体平移。

6. 跨接触 tactile SLAM

连续 tracking 中断、重新接触时，不能直接按相邻 frame IDs 延续。作者利用每个 contact patch 的 PCA absolute yaw：

计算两个 contact clouds 的 Z rotation difference；
做 XY alignment 与 geometric centering；
再用 ICP refinement；
以 overlap ratio 判断 registration 是否可信；
成功后融合点云。

这里 ICP 没有消失，而是从“每帧 pose 主求解器”降级为“有绝对 yaw 初值后的跨接触 refinement”。这解释了论文标题中的 globally invariant 并不意味着所有 registration 都完全不用 ICP。

7. 一分钟静止漂移

实验使用 320×240、约 25 Hz 的 GelSight Mini。静止接触一分钟，五段序列平均：

方法	Δx / Δy / Δz (mm)	Δθx / Δθy / Δθz (°)
InvariantCloud	0.22 / 0.14 / 0.11	0.62 / 0.69 / 0.84
NormalFlow	0.41 / 0.38 / 0.28	1.42 / 0.97 / 0.78
LK + ICP	2.52 / 3.14 / 2.29	10.89 / 8.15 / 7.19

完整方法不是每一个单元格都最好：静止 yaw MAE 0.84° 略高于 NormalFlow 0.78°。它的优势主要是 XYZ、roll / pitch 以及后续动态 / return yaw 的稳定性，而不是把 Table I 说成六项全胜。

8. Return repeatability

四个对象在保持接触时随机平移 / 旋转，再返回与初始轮廓高度重合的位置；每对象、每方法重复 5 次。

InvariantCloud NormalFlow ICP 的六自由度返回误差 — 原论文 Figure 6：四种家庭物体各进行五次 return test；InvariantCloud（绿）在全部对象的总体六自由度误差最低。NormalFlow 对 egg 的 Z rotation return error 接近 20°，ICP 在平移和旋转上均明显漂移。来源：InvariantCloud 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

scissors 和 sensor box 有明显非对称轮廓，NormalFlow 尚能估 yaw；egg 与 utility knife 的椭圆 / 弱差异 contact patch 更难，局部 normal optimization 无法从滑移 marker 恢复绝对方向。全局 ID + silhouette PCA 的 return property 在这里最有说服力。

9. 0→−90° 动态 yaw

实验在对象上贴三个荧光 marker，以 motion-capture 保证实际旋转达到约 −90°。论文将 NormalFlow 与 InvariantCloud 的 yaw 曲线对比：

四个对象从零到负九十度的触觉 yaw tracking 曲线 — 原论文 Figure 8：scissors / sensor box 上两法都能跟随；NormalFlow 在 egg 上近乎停滞，在 utility knife 上明显滞后且末段停滞，InvariantCloud 在两者上仍向 −90°推进。来源：InvariantCloud 原论文。图片版权归论文作者所有，本站仅作学术解读展示。

图中没有逐对象数值表，因此不宜从曲线人工读出高精度小数。可可靠得出的结论是：

明显轮廓对象：两种方法都能追踪；
弱轮廓对象：NormalFlow 的局部法向 / marker cue 不足；
InvariantCloud 对非圆 contact silhouette 更稳；
完全球形仍不可解，作者在结论中明确承认。

10. Tactile SLAM 演示

作者仅用 GelSight Mini 多次离散扫描 scissors，不借助外部 motion capture，将多个局部 patch 注册为较大的表面轮廓。实验说明 absolute yaw 可作为跨接触 registration 初值，但它仍是单对象、定性 surface reconstruction demo，没有 Chamfer distance、闭环误差或大规模 CAD 对比。

11. 局限与复现清单

完全球 / 圆接触的 yaw 不可观，近圆时也应监控 PCA eigenvalue ratio。
连续 frame pose 依赖持续接触；脱离后只能进入跨接触 registration，而非无观测传播。
height-map MLP、球压数据、marker 排序和 pixel-to-mm scale 都要单独校准并版本化。
contact mask 的阈值、morphology 与 pressure 变化会同时影响 PCA 和 centroid，应做压力分层消融。
31×41 grid 可改善 reconstruction fidelity，但论文没有系统报告 grid density 对 latency / noise 的曲线。
实验只有四个家庭物体与少量 hand-operated trials；需要机器人控制、不同材料 / 曲率和长序列 benchmark。
sensor marker 脱落、老化或 elastomer creep 会破坏“全局稳定 reference”，应设计在线 reference health check。
截至本站整理时未发现官方实现仓库，论文仅公开预印本。

12. 方法启发

InvariantCloud 的可迁移价值在于重新设计 correspondence 的来源：当传感器自身存在稳定、可编号的结构时，与其每帧重新搜索局部特征，不如把 identity 固化在 sensor coordinate system，再让当前观测选择 active subset。它把长期漂移从一个优化调参问题变成一个表示问题；但可观性没有被魔法消除，yaw 最终仍依赖非圆接触几何，这个边界同样重要。