静态刚体场景中6D姿态估计的物理合理性

### 静态刚体场景中6D姿态估计的物理合理性 #### 1. 引言 在6D物体姿态估计中，物理合理性本身存在固有的模糊性。例如，一个苹果放在平面上有几乎无限种合理的姿态。我们的目标是扩展姿态误差函数，同时考虑表面对齐和物理合理性，以克服单独使用任何一种评估方法时出现的姿态模糊问题。在表面对齐模糊的情况下，应优先选择物理上更合理的姿态；在物理合理性模糊的情况下，应优先选择表面对齐更接近的姿态。 #### 2. 相关工作 现有的6D物体姿态估计中的姿态误差函数大致可分为与模型无关和基于表面对齐的方法： - **与模型无关的方法**：对6D变换本身进行操作，例如确定估计平移的欧几里得距离或估计旋转与真实姿态的角度距离。但同时考虑两者并非易事。不同的研究者提出了不同的解决方案，如分别对平移和旋转设置阈值、添加加权因子等。 - **基于表面对齐的方法**：在评估6D物体姿态估计质量时更受青睐。常见的姿态误差函数包括平均模型点距离（ADD）、具有不可区分视图的平均模型点距离（ADI）等。为克服模糊性，也有研究者提出了一些模糊不变的姿态误差函数。 | 方法类型 | 具体方法 | 特点 | | ---- | ---- | ---- | | 与模型无关 | 分别阈值法 | 分别对平移和旋转设置阈值判断估计是否正确 | | 与模型无关 | 加权因子法 | 为旋转误差添加加权因子 | | 基于表面对齐 | ADD | 考虑估计姿态和真实姿态下模型对应点的平均距离 | | 基于表面对齐 | ADI | 对最近邻距离求平均 | | 基于表面对齐 | ACPD、MCPD、VSD | 模糊不变的姿态误差函数 | mermaid流程图如下： ```mermaid graph LR A[姿态误差函数] --> B[与模型无关方法] A --> C[基于表面对齐方法] B --> B1[分别阈值法] B --> B2[加权因子法] C --> C1[ADD] C --> C2[ADI] C --> C3[ACPD、MCPD、VSD] ``` #### 3. 6D物体姿态估计的物理合理性 6D物体姿态估计器的预测通常存在误差。现有的姿态误差函数如ADD或VSD主要考虑目标物体在估计姿态和真实姿态下的表面对齐情况，但漂浮或相交物体的估计可能与物理上合理的估计具有相同的表面对齐误差。我们认为，对于人类观察者来说，遵循物理原理的姿态估计更易于理解。在机器人抓取中，不合理的姿态估计可能导致机器人与环境发生碰撞。因此，物理上合理的姿态估计应优先考虑，并在姿态估计器的评估中得到体现。 ##### 3.1 可行的物体姿态 假设除目标物体外的所有场景物体都处于真实姿态。我们通过目标物体表面点与其他物体表面的距离来描述目标物体与场景的交互。 - 表面距离公式：$d(x_i, S) = \min_{y_i \in bdS} \|x_i - y_i\|_2$，其中$x_i \in bdO$是目标物体$O$表面的点，$d$是其到场景物体$S$表面最近点的欧几里得距离。 - 接触点集：$C(O) = \bigcup_{S \in S} \{x_i \in bdO | d(x_i, S) < \varepsilon\}$ - 相交点集：$I(O) = \bigcup_{S \in S} \{x_i \in (bdO \cap intS) | d(x_i, S) > \varepsilon\}$ 如果物体与至少一个其他物体接触（不漂浮）且不与任何其他物体相交（不相交），则该姿态为可行姿态。即需要满足$|C(O)| > 0$（不漂浮）和$|I(O)| = 0$（不相交）。 ##### 3.2 稳定的物体姿态 如果目标物体在其接触点$C(O)$处得到支撑时处于静态平衡（SE），则其姿态为稳定姿态。在给定场景中，物体处于SE的条件如下： - 力平衡：$\sum_{i} f_i + f_{ext} = \sum_{i} f_i + mg = 0$ - 扭矩平衡：$\sum_{i} (cm - x