文章概括

引用：

@article{huang2026simulation,
  title={Simulation-Ready Cluttered Scene Estimation via Physics-aware Joint Shape and Pose Optimization},
  author={Huang, Wei-Cheng and Han, Jiaheng and Ye, Xiaohan and Pan, Zherong and Hauser, Kris},
  journal={arXiv preprint arXiv:2602.20150},
  year={2026}
}

Huang, W.C., Han, J., Ye, X., Pan, Z. and Hauser, K., 2026. Simulation-Ready Cluttered Scene Estimation via Physics-aware Joint Shape and Pose Optimization. arXiv preprint arXiv:2602.20150.

主页：
原文：
代码、数据和视频：

系列文章：
请在 $《$文章$》$ 专栏中查找

---

宇宙声明！

引用解析部分属于自我理解补充，如有错误可以评论讨论然后改正！

---

ABSTRACT

从真实世界观测中估计出可直接用于仿真的场景，对于后续的规划任务和策略学习任务至关重要。 遗憾的是，现有方法在复杂杂乱的环境中表现不佳，当扩展到多个相互作用的物体时，往往会出现计算成本过高、鲁棒性不足以及泛化能力受限等问题。 本文提出了一种基于优化的统一框架来进行 real-to-sim 场景估计，该方法在物理约束条件下联合恢复多个刚体物体的形状（shape）和位姿（pose）。 我们的方法建立在两个关键技术创新之上。 首先，我们利用了最近提出的可形状求导接触模型（shape-differentiable contact model）。 该模型具有全局可微性，因此可以在建模物体之间接触关系的同时，对物体几何形状与位姿进行联合优化。 其次，我们利用了增广拉格朗日（augmented Lagrangian）Hessian 矩阵的结构化稀疏性，从而设计出一种高效的线性系统求解器，其计算成本能够随着场景复杂度的增加而保持良好的扩展性。 基于该优化框架，我们构建了一条端到端的 real-to-sim 场景估计流程，该流程整合了：

• 基于学习的物体初始化

• 受物理约束的形状—位姿联合优化

• 可微分的纹理细化（texture refinement）

在包含最多 5 个物体和 22 个凸包（convex hulls）的复杂场景实验中，我们的方法能够稳定地重建符合物理规律且可用于仿真的物体形状与位姿。

I. INTRODUCTION

场景估计（scene estimation）是机器人学和具身智能（embodied AI）中的一个基础问题，尤其在 real-to-sim 转换 中尤为重要。 一个理想的场景估计器应该能够从稀疏观测（如图像）中重建一个可直接用于仿真的环境（simulation-ready environment）。 除了感知层面的准确性之外，估计得到的物体形状、位姿以及物理属性必须满足物理一致性，并且能够直接在物理模拟器中使用，这对于后续任务（例如运动规划、模型预测控制以及策略学习）至关重要。 在 复杂杂乱的场景（cluttered scenes）中，这一问题尤为困难，因为多个物体会通过接触（contact）发生相互作用，而像机器人操作（robotic manipulation） 这样的任务需要精确的物理推理能力。 多年来，人们提出了多种不同的场景估计方法，包括：

• 贝叶斯推断（Bayesian inference） [12]
• 深度学习（deep learning） [38]
• 数值优化（numerical optimization） [44]

在这些方法中，基于优化的场景估计方法 [44] 在 real-to-sim 应用 中具有独特优势： 它们可以在估计过程中显式地引入物理定律和物理约束。 通过施加以下物理约束：

• 非穿透约束（non-penetration）

• 接触一致性（contact consistency）

• 力平衡条件（equilibrium conditions）

物理约束可以显著地正则化解空间（solution space） 并减少歧义。

尽管具有这些优势，但基于优化的状态估计方法面临的一个主要挑战是： 物理约束的建模会引入大量辅助变量，例如：

• 法向接触力

• 摩擦接触力

• 拉格朗日乘子（Lagrange multipliers）

大多数现有方法使用现成的求解器 [13, 34]，在一个大规模非线性规划问题（NLP）中同时优化所有变量。 这种单体式（monolithic）策略会导致计算开销极高，并且在包含大量相互作用物体的复杂场景中扩展性很差。 为了缓解这一复杂性，一些实际方法（如 [44]）依赖于启发式接触选择 oracle。 然而这种方法本质上是脆弱的，当接触被遗漏时很容易失败。 更根本的问题是，为了控制计算成本，现有方法通常假设物体几何形状已知，并且只优化物体位姿（pose）。 然而，从稀疏观测进行场景估计，本质上需要同时推断物体形状和位姿，这会显著增加决策空间的维度。 随之而来的大量形状参数使得现有的优化方法在计算上变得极其昂贵，在实际中往往难以求解（intractable）。

我们认为这些限制的共同根源在于： 现有的优化状态估计方法没有利用问题结构（structure-aware） [44, 45]。 由于所有变量都被放在一个单体式 NLP 问题中进行优化，这些方法没有利用物理约束优化问题本身的结构特性。 因此，我们提出了一种结构感知（structure-aware）的 real-to-sim 场景估计框架，用于复杂环境。 这是第一个在形状—位姿联合空间中进行数值优化的实用算法。 我们的方法基于 separating-plane-based shape-differentiable contact model（SDRS） [41]。 该模型通过将法向接触力表示为物体位姿的函数，从而消除了法向接触力作为显式变量的需求。 我们将 SDRS 方法扩展到准静态配置优化（quasistatic configuration optimization）中，从而降低问题维度。 同时我们证明，该问题的 增广拉格朗日 Hessian 矩阵具有高度结构化的稀疏模式，可以通过 Woodbury 公式和 Schur 补实现高效求解。 所提出的优化形式在形状和位姿两个变量上都是全局可微的， 因此能够在任意接触条件下进行联合优化，其中物体形状表示为多个凸包（convex hulls）的并集。 为了提高鲁棒性，我们在优化中考虑所有潜在的接触对，而不是使用启发式接触选择 [44]。 同时通过我们提出的降维表示保持计算成本可控。 此外，我们引入了一个全局支持的接触激活函数 [42]， 进一步缓解了梯度消失问题和约束资格问题（constraint qualification issues）。

基于这一联合优化框架，我们构建了一条端到端 real-to-sim 场景估计流程， 该流程可以直接处理单张 RGBD 图像观测（如图1所示）。 我们在一系列复杂场景基准上评估了该方法，这些场景包含最多 5 个物体和 22 个凸包。 实验结果表明，我们的方法能够稳定地产生符合物理规律且可用于仿真的场景重建结果。

图1：给定一张复杂杂乱场景（cluttered scene）的单张 RGBD 图像观测，我们使用 SAM3D 和 FoundationPose 来获得物体形状（shape）和位姿（pose） 的初始估计。 然而，这些初始估计可能会违反物理约束，因此无法直接用于物理仿真（图中以红色表示）。 我们的方法通过联合调整物体形状参数和位姿参数，在施加物理约束的同时最小化感知损失（perceptual loss）。 最终得到可直接用于仿真的结果（图中以绿色表示）。

II. RELATED WORK

在本节中，我们回顾了有关状态估计（state estimation）和场景理解（scene understanding）的相关研究工作，并重点讨论它们与real-to-sim 转换、复杂杂乱环境以及可用于仿真的场景重建之间的关系。

a) Scene Estimation: 早期的场景估计方法通常假设物体几何形状已知，并主要关注从部分观测数据中恢复物体的位姿（pose）。 经典方法 [11,30] 将刚体配准（rigid-body registration）建模为几何对齐问题，后来又扩展到了非刚体模型 [3] 和 关节结构模型（articulated models） [9]。 尽管这些方法在几何理论上具有良好基础，但在存在遮挡（occlusion）和数据缺失的情况下表现脆弱，而这些问题在真实复杂场景中非常普遍。 基于学习的方法，例如 PoseCNN [38] 和 FoundationPose [36]，通过利用学习到的先验知识提高了鲁棒性，但它们仍然是纯粹基于感知的（perception-driven）方法。 由于缺乏显式的物理约束，这些方法并不适合用于可直接用于仿真的场景估计。 一些较新的研究 [32,25,8,40,37] 通过基于采样的优化方法或物理违背损失（physics-violation loss） 引入物理推理，但仍然存在局限性： 例如假设少量固定的物体形状候选 [32]； 或者只优化单个物体的形状 [8]； 或者需要密集观测数据，例如 RGB 视频序列 [25,37]； 或者只建模碰撞约束，而没有保证完整的物理一致性 [40]。

b) Physics-aware Numerical Optimization: 为了提高物理有效性，已有研究将物理约束引入数值优化过程。 例如 PhysPose [23] 和 Verefine [6] 通过惩罚项（penalties）或事后模拟检查（post hoc simulation checks）来鼓励物理合理的配置，但它们并没有提供完全物理一致的优化过程。 一种更为系统的方法 [44] 直接施加非穿透约束和力平衡约束， 但该方法依赖于启发式接触选择 oracle，在复杂场景中会降低系统的鲁棒性。 对可变形物体（deformable objects） 的扩展 [15] 进一步拓展了物理感知估计的应用范围。 然而，大多数现有方法仍然假设物体几何形状已知，并只关注位姿估计。 这使得它们不适用于 real-to-sim 转换任务，因为在这种任务中必须同时恢复物体的形状。 据我们所知，仅有一项工作 [8] 同时考虑了形状与位姿推理，但该方法只适用于单个物体。

c) Differentiable Simulation & Rendering: 可微模拟器（differentiable simulators）和可微渲染器（differentiable renderers） [17,24] 使得对具有物理基础的场景进行基于梯度的优化成为可能。 它们已经被应用于大规模状态估计任务 [39,22,46]。 然而，大多数可微模拟器主要关注动态轨迹（dynamic trajectories），而不是复杂场景中常见的准静态、力平衡配置（quasistatic force-balanced configurations）。 此外，非光滑的接触事件和可见性变化会违反非线性规划（NLP）求解器的平滑性假设。 本文的方法建立在全局可微的 SDRS 接触模型 [41] 之上。 我们将该模型重新表述为适用于准静态平衡问题，从而实现对物体形状与位姿的平滑且可扩展的联合优化。 这一设计可以直接支持复杂真实场景的可仿真重建，从而实现 real-to-sim 转换。

III. METHODS

在本节中，我们介绍用于 可仿真复杂场景重建（simulation-ready cluttered scene reconstruction） 的完整方法流程。 随后，我们将重点介绍一种基于优化的场景估计器，该方法在形状与位姿的联合空间（joint pose- and shape-space） 中对一组刚体进行估计。

把一个复杂场景中的多个物体，同时当成“有形状、有位置、有朝向、会接触”的刚体，然后通过优化，把它们调到一个既符合观测、又符合物理的状态。

联合地估计 shape（形状）+ pose（位姿）+ contact（接触关系）

一边调整物体摆放，一边调整物体外形，直到既符合观测又符合物理。

A. Problem Statement

参考文献 [44]，我们将该问题表示为如下形式的带等式约束的非线性规划（NLP）问题：

$$\underset{q,x}{\mathrm{argmin}}O(q,x)\text{s.t.}C(q,x)=0,(1)$$

我们要找一组最好的 $q$ 和 $x$， 让目标函数 $O(q,x)$ 尽量小， 同时还必须满足约束 $C(q,x)=0$。

• 先把形状和位姿写成变量
• 再把物理关系写成约束
• 然后统一求解

/ 所以它是一个优化驱动的方法，不是一个“黑盒神经网络直接吐答案”的方法。

我们提出一种结构化的增广拉格朗日方法（Augmented Lagrangian Method, ALM）变体来寻找该问题的局部最优解。

把“约束必须满足”这件事，通过拉格朗日项和惩罚项，逐渐融进优化里。

在以下内容中，如果不会引起歧义，我们将省略函数参数的表示。 其中，

• $q$ 表示所有物体位姿的向量
• $x$ 表示所有物体形状的向量

物理约束被建模为全局可微的等式约束 $C(q,x)=0$. 目标函数 $O(q,x)$ 被假设为一个任意的全局可微函数，从而使该框架能够适用于不同的状态估计任务。 例如， $O$ 可以表示：

• 可微渲染图像空间损失 [21]
• 点云配准损失 [2]

即使形状在变、凸包几何在变，整个接触模型仍然要尽量保持可微。

我们的模型建立在最近提出的基于凸包的接触模型（convex-hull-based contact model） [41] 之上。 该模型在理论上被证明是二阶可微的（second-order differentiable），能够在凸包几何结构任意变化的情况下保持全局二阶可微性。

说到二阶可微，也就是 Hessian 层面也有良好性质。

如图3所示，我们将场景表示为： N 个刚体（rigid bodies），每个刚体由： M 个凸包（convex hulls）的并集 构成，而每个凸包包含： V 个顶点（vertices）。

具体来说， $x_{ijk}\in {\mathbb{R}}^3$ 表示： 第 $i$ 个刚体中第 $j$ 个凸包的第 $k$ 个顶点， 并且该顶点坐标是在刚体局部坐标系（body frame） 中表示的。 将这些顶点拼接起来，可以得到 $x\in {\mathbb{R}}^{N\times M\times V\times 3}$ 该变量描述了所有刚体的形状信息。 为了表示刚体的位姿，我们定义第 $i$ 个刚体从局部坐标系到全局坐标系的变换。 其旋转矩阵为 $R_i({\theta }_i)=\exp [{\theta }_i{]}_{\times }$ 并且包含一个平移向量 $t_i\in {\mathbb{R}}^3$ ，其中旋转通过 Rodrigues 公式参数化： ${\theta }_i\in {\mathbb{R}}^3$ 并且符号 $[\cdot {]}_{\times }$ 表示叉乘矩阵（cross-product matrix）。 将所有 ${\theta }_i$ 和 $t_i$ 拼接起来得到向量 $q$ ， 该向量编码了所有刚体的位姿信息。 已知映射 $R_i({\theta }_i)$ 是光滑函数（smooth） [14]。 局部到全局坐标的转换定义为 $X_{ijk}=R_i{x}_{ijk}+t_i$ 当上下文明确时，我们将省略函数参数的表示。

$x_{ijk}$ 在这个物体自己的身体坐标系里，这个顶点在哪里。

$X_{ijk}$：放到全局世界坐标后的顶点位置。

${\theta }_i:$

文章用一个更适合优化的参数：

$${\theta }_i\in {\mathbb{R}}^3$$

再通过 Rodrigues 公式把它变成旋转矩阵：

$$R_i({\theta }_i)=\exp [{\theta }_i{]}_{\times }$$

它用 3 个数来参数化 3D 旋转，然后再转换成真正的旋转矩阵。

$[{\theta }_i{]}_{\times }$： 把向量变成一个能参与旋转运算的矩阵形式
$\exp [{\theta }_i{]}_{\times }$： 把旋转向量转换成真正的旋转矩阵。用一个光滑的方法，把 3 维旋转参数变成合法的旋转矩阵。

---

$(n,d)$：通常一个平面可以写成：

$$n^{\top }X+d=0$$

其中：

• $n$：平面的法向量
• $d$：平面的位置偏置

所以 $(n,d)$ 就唯一确定了一张平面。 图中蓝色虚线就是这张平面。

---

• Hull${}_{ij}$：第 $i$ 个物体的第 $j$ 个凸包
• Hull${}_{i^{\prime }{j}^{\prime }}$：第 $i^{\prime }$ 个物体的第 $j^{\prime }$ 个凸包

图3：假设我们希望建模一个放在椅子上的盒子，其中盒子（浅棕色）放置在椅子（深棕色）上。 盒子和椅子分别表示为第 $i$ 个刚体和第 $i^{\prime }$ 个刚体。 其中，盒子被建模为一个单独的凸包（convex hull），而椅子被建模为四个凸包的并集。 每一个凸包都是由一组顶点张成的多面体（polytope）。 这些顶点表示为 $X_{ijk}$ （图中红色点）。 在 盒子上的第 $ij$ 个凸包 与 椅子上的第 $i^{\prime }{j}^{\prime }$ 个凸包（用于表示椅背支撑部分）之间， 我们引入一个分离平面（separating plane） ： $(n,d{)}_{ij{i}^{\prime }{j}^{\prime }}$ （图中蓝色），该平面作为接触模型（contact model）的代理表示。

B. Scene Estimation Pipeline

这节其实在讲两件事：
第一件事： 怎么得到一个好的初始场景

因为后面的优化器是局部优化器， 不是从零开始就能神奇找到全局最优。 所以必须先有一个差不多靠谱的起点。

第二件事： 有了初始场景后，怎么定义一个“可以优化”的目标函数
也就是要让优化器知道，什么叫“这个物体形状对了、位置对了”。作者没有只用一种误差，而是用了三种误差项一起约束。

---

所以这节可以浓缩成一句话： 先用学习方法给出 shape 和 pose 的初值，再用一个由三类距离项组成的可微目标函数去联合优化 shape 和 pose。

我们的联合优化器采用一种基于局部梯度的优化方法，如果没有高质量的初始化，很容易陷入局部最小值（local minima）。 为了获得高质量的初始估计，我们首先提取点云数据，然后利用基于学习的模型 SAM3D [10] 从点云中预测物体形状的初始估计。 SAM3D 使我们能够提取 mesh 模型并将点云按照每个 mesh 进行分割。 最终得到：

• N 个点云 ${\mathcal{P}}_1,\dots ,{\mathcal{P}}_N$
• N 个对应的 mesh ${\mathcal{M}}_1,\dots ,{\mathcal{M}}_N$

虽然 SAM3D 也会预测物体位姿，但我们观察到这些估计通常不够准确。 因此我们使用 FoundationPose [36] 对物体位姿进行进一步优化，得到位姿向量 $q$ 的初始估计。 接下来，对于每个 mesh ${\mathcal{M}}_i$，我们使用凸分解（convex decomposition） [35] 来生成凸包顶点，从而得到形状向量 $x$ 的初始估计。 随后，这些初始估计会进行调整，以确保不同物体之间没有穿透（penetration-free），然后输入到我们的联合优化器中以施加物理约束（详见附录）。 尽管本文描述时假设每个刚体由 $M$ 个凸包（每个凸包包含 $V$ 个顶点）表示，但我们的实现是通用的，可以根据凸分解 [35] 自动适应任意数量的凸包和顶点。

什么是凸分解（convex decomposition）？为什么做它？

这一步是把 SAM3D 给的 mesh ${\mathcal{M}}_i$ 变成后面优化器真正要吃的那种表示：

多个凸包的并集

凸分解在做什么？
就是把一个复杂形状拆成多个“简单凸块”。 比如椅子可以拆成：

• 椅面一个凸包
• 靠背一个凸包
• 每条腿一个凸包

这样后面就能对这些凸包之间定义分离平面和接触关系。

如何进行凸分解？

Xinyue Wei, Minghua Liu, Zhan Ling, and Hao Su. Approximate convex decomposition for 3d meshes with collision-aware concavity and tree search. ACM Transactions on Graphics (TOG), 41(4):1–18, 2022.

---

“没有穿透（penetration-free）” 主要是指 “不同刚体之间不要互相穿透” ，而不是要求 “同一个刚体内部的多个凸包彼此绝对不能重叠”。

Objective Function Construction

利用分割后的点云 ${\mathcal{P}}_i$、提取的 mesh ${\mathcal{M}}_i$ 以及凸包，我们可以采用类似 trimmed ICP [11] 的方法定义一个可微目标函数 $O$。 在该方法中，我们选择 ICP 项以确保目标函数在迭代过程中单调下降，从而保证算法收敛。

ICP 你可以先理解成“找最近对应点，再最小化对应距离”的经典配准思路。

1. 当前两个形状大致摆在那
2. 给源点找目标中的最近点
3. 固定这些对应关系
4. 调整变换，使距离变小
5. 再重新找最近点
6. 重复

具体来说，对于世界坐标中的每一个凸包顶点 $X_{ijk}$，我们已知它属于第 $i$ 个刚体。 因此我们在 mesh ${\mathcal{M}}_i$ 上寻找与 $X_{ijk}$ 距离最近的点：

$$p(X_{ijk})=\underset{x\in {\mathcal{M}}_i}{\mathrm{argmin}}\Vert x-X_{ijk}\Vert .(2)$$

对每一个当前凸包顶点 $X_{ijk}$， 去同一个物体对应的 mesh ${\mathcal{M}}_i$ 上找最近点（一个点 $p(X_{ijk})$ 而不是距离值本身。）。 也就是说：

• 当前优化中的几何表示：凸包顶点
• 参考形状：SAM3D 给出的 mesh

让凸包顶点不要偏离 mesh 太远。

随后固定 $p(X_{ijk})$，并引入项 $\Vert X_{ijk}(q,x)-p(X_{ijk}){\Vert }^2$ 用于正则化凸包形状。

优化变量里的 shape 实际上是凸包顶点。 如果不加约束，这些顶点可以乱跑。 所以需要有一股力量把它们拉向一个合理的表面。 这个合理表面就是初始 mesh。 于是有了项：

$$\Vert X_{ijk}(q,x)-p(X_{ijk}){\Vert }^2$$

它的意思就是：

当前这个凸包顶点，别离自己在 mesh 上的最近点太远。

• $q$ 表示所有物体位姿的向量
• $x$ 表示所有物体形状的向量
• $X_{ijk}(q,x)$：当前顶点的位置
• $p(X_{ijk})$：这个顶点在参考 mesh 上对应的最近点

相反地，对于属于第 $i$ 个点云的每个点 $p_{il}$，我们惩罚它到第 $i$ 个刚体凸包并集表面的距离。 该表面表示为 $\partial {\cup }_j\text{CH}(X_{ij\bullet })$ 我们在凸包并集表面上寻找与 $p_{il}$ 最近的点：

$$X(p_{il})=\underset{X\in \partial {\cup }_j\text{CH}(X_{ij\bullet })}{\mathrm{argmin}}\Vert X-p_{il}\Vert .(3)$$

它表面上是在讲“最近点”，实际上它在做三件事：

• 把点云和当前物体表面对应起来

• 把“最近点”写成凸包顶点的函数

• 这样后面就能对 shape 和 pose 做可微优化

---

公式：
在当前物体表面上所有可能的点里面，找一个和点云点 $p_{il}$ 距离最短的点，这个点就叫 $X(p_{il})$。

在实际实现中，我们使用 Manifold3d Library [19] 计算凸包并集，并将其表面提取为三角网格。 从而将 $X(p_{il})$ 的计算转化为点到 mesh 的距离计算问题。 计算得到的 $X(p_{il})$ 必须位于某个凸包的表面。 不失一般性，我们假设它位于第 $j(p_{il})$ 个凸包上：$X(p_{il})={\sum }_k{X}_{ij(p_{il})k}{w}_k$ ，其中 $w_k$ 为凸组合权重。 随后按照 ICP 思想，固定凸组合权重 $w_k$。 这样 $X(p_{il})$ 仅成为 $x$ 与 $q$ 的函数。 因此可以引入正则项 $\Vert X(p_{il})-p_{il}{\Vert }^2$ ，以进一步对凸包形状进行正则化。

这一段要做的事情非常简单：

对于点云里的每个点 $p_{il}$， 找到它在“当前这个物体表面”上最接近的位置， 然后逼着这个表面去贴近点云。

也就是说：

• 点云点 $p_{il}$：来自观测，比较可信
• 当前物体表面：是你现在优化中的 convex hull union surface
• 他们之间如果有距离，说明当前形状还没贴好观测

所以作者加一个损失：$\Vert X(p_{il})-p_{il}{\Vert }^2$
意思就是： 让当前物体表面上的最近点 $X(p_{il})$ 尽量靠近点云点 $p_{il}$。

---

并集 / 边界 的区别：
二维类比：两个矩形

假设有两个矩形，部分重叠。 它们的并集是两个矩形覆盖到的整块区域。包括：外轮廓里面的所有面积，重叠的部分，内部全部都算。它们的边界只是这一整块区域最外圈那条线。不是整个面积， 只是包住它的轮廓线。

---

1. $p_{il}$ 是什么？

这是第 $i$ 个物体的点云中的第 $l$ 个点。 你可以理解成： 相机真的看到的一个 3D 点，这个点属于物体 $i$。比如盒子表面上某个被相机看到的点。来自照相机的真实点云。

注：真实点云经过SAM3D之后物体会被分割，并且得到对应物体的mesh（model），得到model之后送到FoundationPose中可以估计6D pose。这个pose反向投影到点云之后和观测到的点云是不一样的。

2. $\partial {\cup }_j\mathrm{CH}(X_{ij\bullet })$ 是什么？
第 $i$ 个物体当前的外表面。更准确说是： 第 $i$ 个物体由多个凸包组成，先把这些凸包取并集，再取这个并集的边界表面。所以它表示的就是：

当前优化中的这个物体的整体表面

你就把它想成： “当前物体长出来的壳” 就行。

注：凸包顶点经过 pose 变换，得到世界坐标下的位置。 $X_{ijk}=R_i{x}_{ijk}+t_i$

3. $X(p_{il})$ 是什么？
它表示： 对于点云点 $p_{il}$， 在当前物体表面上离它最近的那个点。 所以它不是随便一个点， 而是一个最近点投影。

---

$X(p_{il})={\sum }_k{X}_{ij(p_{il})k}{w}_k$：

本质上是在说：点云点 $p_{il}$ 在当前物体表面上的最近点 $X(p_{il})$，落在某一个凸包上；而这个凸包上的任意一点，都可以写成该凸包顶点的加权平均。基本的凸几何事实：

凸包里的点，能写成顶点的凸组合。

$j(p_{il})$：这个最近点 $X(p_{il})$ 落在第 $j$ 个凸包上。
$X_{ij(p_{il})k}$：第 $i$ 个物体中，第 $j(p_{il})$ 个凸包的第 $k$ 个顶点在世界坐标中的位置。
$w_k$：最近点 $X(p_{il})$ 是由这些顶点按什么比例加权平均出来的。

---

一个凸包就是：

给定一堆顶点，把它们所有可能的凸组合都收集起来，形成的集合

数学上你可以把“凸包”理解成：

$$\mathrm{CH}(v_1,\dots ,v_m)=\left\{\sum _k{w}_k{v}_k|w_k\ge 0,\sum _k{w}_k=1\right\}$$

这句话意思是：

只要一个点能写成这些顶点的非负加权平均，而且权重和为 1，那它就在这个凸包里。

---

固定 $w_k$ 后，$X(p_{il})$ 就变成了：

只由当前形状 $x$ 和位姿 $q$ 决定的连续函数

Mesh Prior

最后我们注意到：点云只能约束可见区域的物体形状。 为了约束不可见区域的形状，我们利用 SAM3D 重建的 mesh ${\mathcal{M}}_i$。 对于每个 mesh 顶点 $p_{il}\in {\mathcal{M}}_i$，同样计算最近点 $X(p_{il})$ 并引入项 $\Vert X(p_{il})-p_{il}{\Vert }^2$。

对于这个 mesh 顶点 $p_{il}$， 在当前优化中的 convex hull union surface 上，离它最近的那个点。 也就是说：

• $p_{il}$：SAM3D mesh 上的点。

• $X(p_{il})$：当前模型表面上的对应点，“当前优化中的凸包表面”。凸包初始上确实是从 mesh 做 convex decomposition 得来的，所以它们一开始来源相关。但一旦进入优化，它们就不再是同一个表面了。

Final Objective

总之，我们的目标函数被表述为三类项的加权和，其权重系数分别为$w_{1,2,3}$：

$$O(q,x)=\{\begin{array}{ll}{w}_1{\sum }_{ijk}\Vert X_{ijk}-p(X_{ijk}){\Vert }^2+& \text{Type I}\\ w_2{\sum }_{p_{il}\in {\mathcal{P}}_i}\Vert X(p_{il})-p_{il}{\Vert }^2+& \text{Type II}(4)\\ w_3{\sum }_{p_{il}\in {\mathcal{M}}_i}\Vert X(p_{il})-p_{il}{\Vert }^2& \text{Type III}\end{array}$$

该目标函数遵循 Hausdorff 距离 的思想，即对称惩罚距离。

• Type I：凸包顶点 $X_{ijk}$ 到 mesh ${\mathcal{M}}_i$ 的距离

• Type II：点云 到 convex hull surface $\partial {\cup }_j\text{CH}(X_{ij\bullet })$ 的距离

• Type III：mesh 到 convex hull surface $\partial {\cup }_j\text{CH}(X_{ij\bullet })$ 的距离

我们认为（assume）点云 ${\mathcal{P}}_i$ 是真实观测数据，因此对形状估计具有强约束作用。 因此赋予较大的权重 $w_2$。 另一方面，mesh ${\mathcal{M}}_i$ 可能是 SAM3D [10] 推断（hallucinated）得到的。 因此仅作为弱形状先验，并赋予较小权重 $w_3$。

上述过程的一个关键缺点是：它违反了非线性规划（NLP）的收敛保证。 需要注意的是，我们的 Type I 项 $\Vert X_{ijk}-p(X_{ijk}){\Vert }^2$ 采用了 ICP（Iterative Closest Point，迭代最近点） 的标准处理方式： 我们首先 固定 $p(X_{ijk})$ 来求解 ALM （增广拉格朗日方法：一边想让目标函数 $O(q,x)$ 尽量小，一边又必须满足约束 $C(q,x)=0$。）优化问题，然后再通过 公式 (2) 更新 $p(X_{ijk})$。 由于每一项在迭代过程中都单调减小，因此该过程可以保证收敛 [11]。

$X_{ijk}$：当前凸包顶点
$p(X_{ijk})$：这个顶点在固定 mesh ${\mathcal{M}}_i$ 上的最近点
$$p(X_{ijk})=\underset{x\in {\mathcal{M}}_i}{\mathrm{argmin}}\Vert x-X_{ijk}\Vert .(2)$$
Type I 的参考对象 mesh ${\mathcal{M}}_i$ 是固定不变的。也就是说，Type I 里：

• 当前点 $X_{ijk}$ 会动
• 但参考表面 mesh 不动

所以每次“重新找最近点”这件事是在一个固定表面上做的。因为对于任何当前点 $X_{ijk}$，在固定表面上找最近点，得到的那个点一定不会比你随便选一个旧对应点更差。在固定表面上重新投影到最近点，只会让该项误差更小或不变，不会更大。

然而，我们的 Type II 和 Type III 项 $\Vert X(p_{il})-p_{il}{\Vert }^2$ 并不能保证在迭代过程中单调下降，因为 公式 (3) 中的更新可能会增加函数值。 其根本原因在于： 参考 mesh ${\mathcal{M}}_i$ 的几何形状是固定的，而凸包并集的形状是可以变化的。 为了严格保证收敛，我们借鉴 [11] 引入了一种启发式方法： 选择性删除可能导致函数值增加的 Type II 和 Type III 项。 具体而言，我们根据 公式 (3) 更新 $X(p_{il})$ 后的函数值增长量对所有 Type II 和 Type III 项进行排序：

$${\Delta }_{il}=\Vert X(p_{il})-p_{il}{\Vert }^2-\Vert X(p_{il}{)}^{prev}-p_{il}{\Vert }^2,(5)$$

其中 $X(p_{il}{)}^{prev}={\sum }_k{X}_{ij(p_{il})k}{w}_k^{prev}$ 表示使用上一轮迭代的凸组合权重计算得到的点。

$$X(p_{il})=\underset{X\in \partial {\cup }_j\text{CH}(X_{ij\bullet })}{\mathrm{argmin}}\Vert X-p_{il}\Vert .(3)$$

• $p_{il}$ 是参考点（点云点或者 mesh 点）
• $X(p_{il})$ 是当前 convex hull union surface 上的最近点

你这次找到的最近点，对应的是“旧的表面”； 下一次优化后，表面变形了，最近点可能突然跳到完全不同的位置。

随后，我们不断删除函数值增长最大的 Type II 或 Type III 项（我们反复删除那种在被删除后会使目标函数值增加最多的 Type II 项或 Type III 类项，直到目标函数$O$不再增加。），直到目标函数 $O$ 不再增加。 该过程（最近点更新 + 选择性删除项）会在每次 ALM 子问题求解之后执行。 完整的算法流程在附录中给出，并在图2中进行了示意说明。

图2：我们展示了公式 (4) 中的三类目标函数项。 这些目标项用于正则化以下距离： 凸包顶点 $X_{ijk}$（红色）；SAM3D 识别得到的 mesh 顶点 $p_{il}\in {\mathcal{M}}_i$（蓝色） 以及 点云中的点 $p_{il}\in {\mathcal{P}}_i$（黄色）。 此外，我们还展示了一个目标函数值可能增加的情况（a）。 假设一个刚体（浅棕色）由两个互不相交的凸包组成（b）。 此时，蓝色顶点的最近点位于上方凸包的底部表面。 当凸包顶点更新之后（c），两个凸包发生合并，最近点移动到了右侧边界。

Texture Generation

作为方法的可选最后一步，我们可以通过可微光栅化（differentiable rasterization）为每个物体生成颜色纹理。 具体来说，在 ALM 优化完成后，我们固定物体形状和位姿。 随后使用 Manifold3d Library [19] 将凸包并集转换为三角网格（triangle mesh）。 接着使用 xatlas [43] 为每个 mesh 生成 UV 坐标。 最后使用 可微渲染器 [18] 来最小化： SAM3D 预测图像 与 mesh 渲染图像 之间的差异。 在这个优化过程中，纹理贴图（texture map）是优化变量。

核心的目标是：让当前重建出来的 mesh 渲染出来以后，在颜色上尽量接近 SAM3D 提供的参考图像。

---

光栅化是图形学里把三角网格投影到图像平面、确定每个像素属于哪个三角形的过程。 普通光栅化很多步骤是离散的，不太容易直接求导。 可微光栅化就是： 用一种可求导的方式近似或实现这个过程， 让图像误差能对上游参数产生梯度。 所以这里的“可微光栅化”本质上是在说： 我们可以把渲染结果和目标图像做比较，然后自动优化纹理。

C. Optimization in Joint Shape-Pose Space

前面所有东西都准备好了，现在我们要真正解这个“形状 + 位姿 + 物理约束”的优化问题。
前面你已经知道：

• 变量有 $q$（位姿）和 $x$（形状）
• 有目标函数 $O(q,x)$
• 有约束 $C(q,x)=0$

所以这是“求解器层面”的核心。

• $q$：所有物体的位姿
• $x$：所有物体的形状参数（凸包顶点）
• $O(q,x)$：前面那三个 Type I / II / III 组成的几何目标函数
• $C(q,x)=0$：物理约束，比如接触、平衡等

众所周知 [31]，为了找到问题1的局部最优且可行解，一个关键要求是约束函数 $C$ 必须满足线性无关约束资格条件（Linear Independent Constraint Qualification，LICQ）。即：${\lambda }_{min}(\nabla C\nabla C^T)$ 对于任意的 $q$ 和 $x$，都有 ${\lambda }_{min}(\nabla C\nabla C^T)$ 不会接近于零（即具有正下界）。 在实际实现中，我们使用一种定制版本的增广拉格朗日方法（ALM），通过迭代最小化以下增广拉格朗日函数：

$$\underset{q,x}{\mathrm{argmin}}O(q,x)+{\lambda }^{T}C(q,x)+\frac{\rho }{2}\Vert C(q,x){\Vert }^2(6)$$

其中，每个子问题使用 Levenberg–Marquardt (LM) 算法 进行求解。 并按照文献 [26] 的方法，在迭代过程中逐步增加：

• 拉格朗日乘子 $\lambda$
• 惩罚系数 $\rho$

可行解（feasible solution）：它真的满足约束并且$O(q,x)$较小。
局部最优且可行解：在满足物理约束的前提下，找到一个附近最好的解。
线性无关约束资格条件（Linear Independent Constraint Qualification，LICQ）：所有约束在局部都得“有独立的信息量”，不能互相重合、互相打架、或者退化成几乎同一条约束。你可以把每个约束想成一根“拉绳”， 每根绳子都在告诉优化器一个方向上的限制。 如果这些绳子： 方向独立， 不是重复的，
不是互相塌到一起，那优化器就比较容易判断“怎么在满足约束的同时往下降”。

---

${\lambda }_{min}(\nabla C\nabla C^T)$ 不接近 0：其实是在检查约束梯度之间是不是还保持足够独立。

1. $\nabla C$ 是什么？ 就是约束对变量的导数矩阵， 也可以粗略理解成： 每条约束在当前点“朝哪个方向变化”的信息。
2. $\nabla C\nabla C^T$ 是什么？ 这是把这些约束梯度拿来做一个矩阵， 看看它们是不是独立、是不是退化。
3. 最小特征值 ${\lambda }_{min}$ 为什么重要？ 如果这个最小特征值非常接近 0，通常意味着： 有些约束方向变得几乎线性相关了，或者说约束系统变得接近退化， 这时数值优化会很难。他们希望约束系统始终保持“非退化、方向独立、数值健康”。

如果最小特征值 ${\lambda }_{min}$ 很接近 0，通常意味着： 存在某个非零方向，在这个方向上，约束几乎不给信息。

---

$$\underset{q,x}{\mathrm{argmin}}O(q,x)+{\lambda }^{T}C(q,x)+\frac{\rho }{2}\Vert C(q,x){\Vert }^2(6)$$
$O(q,x)$： 这是原来的目标函数。 也就是前面你已经学过的三类误差项： Type I， Type II，Type III。它负责： 几何和观测拟合
${\lambda }^{T}C(q,x)$： 这是拉格朗日乘子项。 你可以把 $\lambda$ 想成： 约束监督员的当前“执法力度”。 如果某条约束总是满足不好， 这个监督员就会在后续迭代里更强地提醒优化器： 你别只顾着降目标函数，约束也要管！
$\frac{\rho }{2}\Vert C(q,x){\Vert }^2$： 如果约束违反得很严重， 这个项就会很大，逼着优化器去减小约束残差。 你可以把 $\rho$ 想成： 罚款力度越大，违反约束越疼。

---

这篇文章里 ALM 每一轮实际在做什么？
在当前最近点和当前活跃项已经固定后， 解一个“增广拉格朗日子问题”， 同时更新 $q$ 和 $x$， 让目标函数变小、约束更接近满足。 所以这里的 ALM 是内层求解器。 外层还有：最近点更新， 删坏项， 更新乘子/罚参数， 这些是更大的迭代框架。

---

LM 可以粗略看成是： 高斯牛顿法 + 一点阻尼的折中版本

它特别适合这类： 非线性最小二乘；有二阶近似；希望比纯梯度下降更快，但又更稳一点的问题。
最朴素理解： 如果你有一个非线性目标函数， LM 会在当前点附近做局部近似， 然后求一个更新步长。 它比普通梯度下降更“看曲率”， 通常收敛更快； 同时又比纯牛顿法更稳一些， 不那么容易一步迈太猛。
为什么这篇文章适合 LM？ 因为当前 ALM 子问题可以写成一种适合局部二阶近似处理的形式， 而且他们后面非常重视： Hessian 结构；线性系统求解；结构化加速。这些都和 LM / 高斯牛顿类方法很搭。

---

为什么要逐步增加 $\lambda$ 和 $\rho$？
如果一上来就： $\rho$ 特别大，乘子也特别强，那优化器会很难动，问题数值也会变得很僵硬。 这就像老师一开始就拿最严标准罚你， 你连基本答案都还没写出来，过程会很难。
更合理的做法是什么？
先允许系统： 找到一个大致合理的方向，先兼顾目标和约束，然后随着迭代慢慢推进，再逐渐加强： 约束执法力度 $\lambda$，罚款力度 $\rho$，这样最后就能逼近一个更可行的解。
所以这一步的核心逻辑是什么？
就是： 先松一点，后严一点。 这在约束优化里非常常见。

与以往研究相比，我们在问题1的建模方式上存在几个关键区别。 首先，不同于基于互补约束（complementarity constraints）的建模方法 [44,27]，我们消除了法向接触力的辅助变量。 这显著降低了优化问题的维度以及子问题的计算成本。 其次，我们的优化问题只包含等式约束。 因此，每个子问题只需要求解线性方程组，而不是求解一般的二次规划问题（quadratic program）。 从而带来显著的计算加速。 接下来，我们首先分析无摩擦（frictionless）情况。 随后将摩擦力作为额外的决策变量引入模型。 并提出一种结构化线性求解器来高效处理这些变量。

互补约束（complementarity constraints）：会显式引入：接触间隙；接触力；互补关系。这类建模往往比较重， 变量多、约束复杂、数值求解不轻松。 所以作者强调自己“不同于基于互补约束的方法”， 本质上是在说： 我们走了一条更轻、更适合结构化求解的路。

1) Physics Constraints Without Friction

我们首先考虑一种情况：所有物体之间只存在法向接触力（normal contact forces），而不存在摩擦力（friction）。 使用上一节介绍的表示方法，物理约束可以简化为：在静止姿态（resting pose）下最小化系统的内外势能（potential energy）。 系统的势能 $\Psi (q,x)$ 由两部分组成：

• 重力势能 ${\Psi }_g$
• 碰撞势能 ${\Psi }_c$

因此：$\Psi ={\Psi }_g+{\Psi }_c$

Gravitational Potential

作者要把“重力”写进优化里。为了让这个重力项对形状和位姿都好求导，他们不采用“体积均匀分布质量”的传统写法，而是假设质量集中在凸包顶点上。这样重力势能就可以直接写成顶点位置的简单求和，而且对 shape 和 pose 都是光滑、二阶可微的。

重力势能的建模基于一个假设： 物体的质量集中在凸包顶点上 [41]。 这是因为如果假设质量均匀分布，则势能函数对形状变量 $x$ 不可微。 在该假设下，定义：

$${\Psi }_g(q,x)=-\rho \sum _{i,j,k}⟨X_{ijk},g⟩(7)$$

其中：

• $\rho$ 表示密度（density）
• $g$ 表示重力加速度（gravitational acceleration）

显然，${\Psi }_g$ 对所有变量都是二阶可微的，这使得用户可以优化物体密度与质量分布。 随后将约束定义为：$C={\nabla }_q\Psi$。如果 $\Psi$ 是全局二阶可微函数，那么约束函数 $C$ 也是可微的，并满足优化问题所需的平滑性条件。

${\Psi }_g(q,x)$：重力势能。 下标 $g$ 表示 gravitational。 它依赖于： $q$：位姿，$x$：形状。为什么依赖这两个？ 因为顶点位置 $X_{ijk}$ 会随着 shape 和 pose 改变。
$\rho$： 密度 density。 你可以把它理解成一个整体比例因子。 它控制： 这些顶点“总共代表多重的质量”。因为他们把质量离散到顶点上，所以密度/质量规模自然会作为系数出现。
$g$：这是重力加速度向量。
$X_{ijk}$：第 $i$ 个刚体，第 $j$ 个凸包，第 $k$ 个顶点在世界坐标中的位置。
$⟨X_{ijk},g⟩$： 这是内积。 如果你把 $g$ 理解成竖直向下的向量， 那这个内积本质上就在测： 这个顶点沿重力方向的位置高低，也就是“高度”的一种等价表达。

Collision Potential

作者不直接用“两个物体绝对不能碰”的硬约束去建模碰撞，而是引入一个“分离平面”的中间变量，用一个对数障碍型势函数去保证两个凸包始终被一张平面分开。这样碰撞约束就被变成了一个光滑、鲁棒、全局二阶可微的碰撞势能。

第二个势能项是碰撞势能（collision potential）。 与使用硬约束（hard constraints）建模碰撞的方法 [27,44] 不同，我们采用一种仅原始变量（primal-only）的内点法建模方式 [20]，该方法被证明更加鲁棒。 这是因为内点方法能够始终保持碰撞约束被满足，从而避免在深度穿透（deep penetration）情况下出现梯度消失问题。

primal-only： 只在原始变量空间里建模，不额外引入一大堆对偶/互补结构来处理碰撞。通过障碍势函数直接约束原始几何变量，使碰撞约束始终保持满足。

在轻微接触时，你通常还能比较自然地说：

• 这两个物体是沿哪个法向碰上的

• 最近点是哪两个

• 应该沿哪个方向把它们推开

但一旦深度穿透：

• 最近点可能不唯一

• 法向可能不稳定

• 接触面可能一下子变成大片区域

• 哪个对应点代表“应该分开的方向”变得很模糊

于是即便你定义了一个损失，它的导数也可能：

• 来回跳

• 方向混乱

• 变得很小

• 甚至在某些区域几乎不变

所以优化器拿不到稳定信号。

Ye 等人 [41] 将文献 [20] 的思想推广到任意两个凸包之间的势函数定义。 具体而言，两个凸包不发生相交当且仅当它们之间存在一个分离平面（separating plane） [29]。 因此，我们引入分离平面变量： $(n,d{)}_{ij{i}^{\prime }{j}^{\prime }}\in {\mathbb{R}}^4$，该平面用于分离第 $ij$ 个凸包与第 $i^{\prime }{j}^{\prime }$ 个凸包。 其中：

• $n_{ij{i}^{\prime }{j}^{\prime }}$ 表示平面法向量
• $d_{ij{i}^{\prime }{j}^{\prime }}$ 表示平面偏移量

碰撞势函数定义为 ${\bar{\Psi }}_{ij{i}^{\prime }{j}^{\prime }}(q,x,n_{ij{i}^{\prime }{j}^{\prime }},d_{ij{i}^{\prime }{j}^{\prime }})$：

$${\bar{\Psi }}_{ij{i}^{\prime }{j}^{\prime }}=\{\begin{array}{l}-\log (1-\Vert n_{ij{i}^{\prime }{j}^{\prime }}\Vert )\\ {\sum }_k-\log (⟨n_{ij{i}^{\prime }{j}^{\prime }},X_{ijk}⟩+d_{ij{i}^{\prime }{j}^{\prime }})(8)\\ {\sum }_{k^{\prime }}-\log (-⟨n_{ij{i}^{\prime }{j}^{\prime }},X_{i^{\prime }{j}^{\prime }{k}^{\prime }}⟩-d_{ij{i}^{\prime }{j}^{\prime }})\end{array}$$

第一项保证平面法向量的模长不超过1。

为什么要限制 $|n|<1$？ 因为如果不限制， 平面表示有尺度自由度问题。 比如同一张平面： 用 $(n,d)$ 表示，也可以用 $(2n,2d)$ 表示，也可以用 $(100n,100d)$ 表示。那优化会很不稳，因为同一几何对象有无穷种缩放等价表示。 所以作者加这一项，相当于给平面参数做一个规范化约束： 别让 $n$ 无限制变大。

第二项与第三项确保两个凸包位于分离平面的两侧。 从而保证无碰撞约束（collision-free constraints）。 研究表明 ${\bar{\Psi }}_{ij{i}^{\prime }{j}^{\prime }}$ 是良定义且全局二阶可微的函数。

第二项： 保证第一个凸包的所有顶点都在平面的一侧。第一个凸包所有顶点都必须在平面正侧，而且不能贴到边界太近。 因为如果某个顶点逼近平面：这项就会爆炸到 $+\infty$。
第三项： 保证第二个凸包的所有顶点都在平面的另一侧。

把第二项和第三项合起来看就是： 第一个凸包所有点在平面正侧，第二个凸包所有点在平面负侧，于是这张平面把两个凸包严格分开了。

在合法区域内，$\log$ 很光滑， 这正适合这篇文章的二阶优化框架。 所以 log 障碍是一个非常经典、非常适合这里的选择。

尽管定义中引入了分离平面变量 $(n,d)$，但该函数对这些变量是严格凸的。 因此可以隐式消除这些变量，并定义：

$${\Psi }_{ij{i}^{\prime }{j}^{\prime }}(q,x)=\underset{n_{ij{i}^{\prime }{j}^{\prime }},d_{ij{i}^{\prime }{j}^{\prime }}}{\min }{\bar{\Psi }}_{ij{i}^{\prime }{j}^{\prime }}(q,x,n_{ij{i}^{\prime }{j}^{\prime }},d_{ij{i}^{\prime }{j}^{\prime }})(9)$$

根据隐函数定理（implicit function theorem），${\Psi }_{ij{i}^{\prime }{j}^{\prime }}$ 仍然保持全局二阶可微性。

如果你固定当前的 $q,x$， 把这个势函数只看成关于分离平面变量 $(n,d)$ 的函数， 那它是一个严格凸函数。 严格凸是什么意思？ 最朴素讲就是： 它只有一个唯一最优点，不会有很多同样好的坑。 这意味着： 对于固定的两个凸包，“最好的分离平面”是唯一且好求的，不会有一大堆乱七八糟的局部极小，这非常有利于后面把 $(n,d)$ 消掉。 给定当前凸包几何后，我们总是选那个“最优分离平面”对应的最小障碍势能值。

公式（9）： 对于这对凸包，在所有可能分离平面里，挑那个最合适的平面，使碰撞势函数最小；这个最小值就定义成这对凸包的最终碰撞势能。所以最终你不用再管具体平面长啥样， 只要知道： 当前这两个凸包之间，有一个由最优分离平面诱导出来的碰撞势能值。

Full Collision Potential

作者把每一对凸包之间的无碰撞障碍势能加起来，得到整个场景的碰撞势能；这个势能既能逼近“绝对不能碰撞”的硬约束，又因为来自同一个分离平面势函数的最优解，所以天然满足作用力与反作用力配对；同时，由于一旦违反碰撞约束势能就变成无穷大，优化器会自动拒绝非法解，从而在整个迭代过程中都保持无碰撞。

完整碰撞势能定义为：${\Psi }_c=\mu {\sum }_{i\ne i^{\prime }}{\sum }_{j,j^{\prime }}{\Psi }_{ij{i}^{\prime }{j}^{\prime }}$，其中 $\mu$ 为互补间隙（complementarity gap）。 文献 [20] 表明，当 $\mu \to 0$ 时，${\Psi }_c$ 可以任意精确地逼近硬碰撞约束。

${\Psi }_{ij{i}^{\prime }{j}^{\prime }}$：是一对凸包之间的碰撞势能，也就是“第 $i$ 个物体的第 $j$ 个凸包”和“第 $i^{\prime }$ 个物体的第 $j^{\prime }$ 个凸包”之间的无碰撞势函数。这句话的意思就是： 把场景里所有可能的凸包对的碰撞势能都加起来。

$\mu$ 是一个控制障碍势函数“逼近硬约束程度”的参数。它像一个“松紧旋钮”。 $\mu$ 大一点：障碍势函数相对更“软”，$\mu$
趋近 0：障碍势函数越来越像硬约束。 你可以把 $\mu$ 理解成“软约束逼近硬约束的尺度参数”。

此外，函数 ${\bar{\Psi }}_{ij{i}^{\prime }{j}^{\prime }}$ 在分离平面变量 $(n,d{)}_{ij{i}^{\prime }{j}^{\prime }}$ 上的最优性条件保证了： 两个凸包之间施加的力是大小相等、方向相反的。这在本质上满足了牛顿第三定律（Newton’s third law）。 最后，当碰撞约束（collision constraints）被违反时，势函数的值为 $\Psi =\infty$ 。在这种情况下，该解会被 ALM 子问题求解器（ALM subproblem solver）拒绝，并改用更小的搜索步长（search step size）。从而保证在每一次迭代中碰撞约束都得到满足。 为了提高效率，可以使用截断对数函数（clamped log function）使势函数具有局部支持性。在实际应用中，由于凸包表示（convex-hull representation）具有紧凑性，因此计算所有物体对之间的碰撞关系在计算上是足够高效的。这使得我们能够使用具有全局支持（global support）的对数函数（log function）。全局支持具有一个重要优势： 即使物体之间距离较远，仍然能够保证梯度不为零（non-zero gradients）。这有助于满足 LICQ（Linear Independent Constraint Qualification，线性无关约束资格条件）。我们在下文中总结了该物理约束所具有的良好性质（well-behaved properties）。 这些性质已经在文献 [41] 中得到了严格证明。

性质 III.1

在无摩擦（frictionless）的情况下，约束函数 $C(q,x)$ 对于变量 $q$ 和 $x$ 都是全局可微（globally differentiable） 的。 当 $C(q,x)=0$时，所有物体都处于：

• 力平衡（force balance）

• 力矩平衡（torque balance）

并且同时满足牛顿第三定律（Newton’s third law）。

2) Incorporating Frictional Contacts

在无摩擦情况下，法向接触力可以通过碰撞势能对顶点位置求导直接得到；但摩擦力不是保守力，不能从势能函数直接导出来，所以必须把摩擦力当作新的优化变量显式加入。为了保持整个接触结构自洽，作者仍然用分离平面作为中介，并要求所有作用在这个“零质量平面”上的摩擦力保持平衡。

摩擦接触更加复杂，因为摩擦力总是耗散动能，因此无法像保守力那样对应一个势能函数形式。 在本节中，我们首先建立摩擦接触模型，随后提出一种结构感知算法（structure-aware algorithm） 来求解 ALM 子问题（问题6）。 如图4所示，我们考虑一对凸包（convex hulls）。 对于每一个顶点 $x_{ijk}$（对于 $x_{i^{\prime }{j}^{\prime }{k}^{\prime }}$ 的情况是对称的），其法向接触力定义为：

$$f_{ijk,i^{\prime }{j}^{\prime }}^{\perp }=\frac{\partial {\Psi }_{ij{i}^{\prime }{j}^{\prime }}}{\partial X_{ijk}},(10)$$

左边这个顶点所受到的法向接触力，等于“这对凸包的碰撞势能”对这个顶点位置的导数。也就是： 如果你移动这个顶点，碰撞势能会怎么变，那个变化趋势就对应一个法向力。
为什么这就是法向力？ 因为前面那套碰撞势能 ${\Psi }_{ij{i}^{\prime }{j}^{\prime }}$ 本质上是在做： 用分离平面障碍函数，阻止两个凸包互相靠近和穿透。所以这个势能对顶点位置的梯度， 自然会给出一个“把顶点往远离碰撞方向推开”的力。 这个方向就是法向相关方向。 因为碰撞障碍本身就在描述“沿分离方向的排斥”。 所以作者直接把：$\frac{\partial {\Psi }_{}}{\partial X_{}}$ 解释成法向接触力。

图4：展示了我们的摩擦模型示意图，该模型作用在第 $ij$ 个凸包与第 $i^{\prime }{j}^{\prime }$ 个凸包之间。每一个顶点（例如 $X_{ijk}$）处，法向力 $f_{ijk,i^{\prime }{j}^{\prime }}^{\perp }$ 是 $x$ 与 $q$ 的函数（见公式10）。而摩擦力 $f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }$ 则是需要优化的额外决策变量。我们遵循 SDRS 接触模型的思想，将分离平面作为接触建模的代理结构。每一个作用在第 $ij$ 个凸包上的摩擦力 $f^{\parallel }ijk,i^{\prime }{j}^{\prime }$，都会在分离平面 $(n,d)ij{i}^{\prime }{j}^{\prime }$ 上产生一个相反的力 $-f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }$。对于第 $i^{\prime }{j}^{\prime }$ 个凸包也是同样的情况。随后我们将分离平面建模为一个质量为零的物理对象。因此所有施加在该平面上的力必须保持平衡。

1. 法向力 $f^{\perp }$：垂直于接触面的方向 负责“顶住、不穿透”。

2. 切向力/摩擦力 $f^{\parallel }$：沿着接触面的方向 负责“阻碍滑动”。

此外，根据摩擦锥约束（friction cone constraint），该顶点还可能受到切向摩擦力 $f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }$，并满足：

$$⟨f_{ijk,i^{\prime }{j}^{\prime }}^{\perp },f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }⟩=0\Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }\Vert \le \eta \Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\perp }\Vert ,(11)$$

其中 $\eta$ 是摩擦锥的摩擦系数。

法向力 $f^{\perp }$

• 方向：沿接触面法向
• 作用：顶住，不让物体互相穿透

摩擦力 $f^{\parallel }$

• 方向：沿接触面切向
• 作用：阻止相对滑动

所以一个接触点处的总接触力，通常可以拆成：

• 法向分量
• 切向分量

这就是这里的 $f^{\perp }$ 和 $f^{\parallel }$。

---

$\Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }\Vert \le \eta \Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\perp }\Vert$的意思是摩擦力的最大大小，不能超过法向力乘摩擦系数。
为什么摩擦力上限要和法向力有关？ 因为现实中摩擦能力取决于“压得有多紧”。 例如： 你轻轻把手放在桌上，摩擦不大。你用力压住一个物体，再推它，摩擦会更大。所以一般静摩擦上限写成：$\Vert f^{\parallel }\Vert \le \eta \Vert f^{\perp }\Vert$

摩擦系数 $\eta$ 是这个接触面“有多涩”或者“有多滑”的参数。

然而，仅有摩擦锥约束仍然是不够的。 我们还必须确保施加在两个凸包上的摩擦力集合同时满足：

• 力平衡
• 力矩平衡

力平衡就是：所有力加起来，合力为 0。
力矩平衡就是：所有力对某个参考点产生的总转动效应为 0。

为实现这一点，我们采用文献 [41] 的思想，将分离平面（separating plane）视为一个质量为零的虚拟物体。 尽管该分离平面对系统没有直接物理影响，但它可以作为一种代理结构（proxy） 来建立并施加摩擦力的平衡条件。 具体来说，当一个摩擦力作用在 $x_{ijk}$ 上时，会在该平面上施加一个相反方向的力。 因此，在平面内产生一个力：$-f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }$ 并产生一个平面内力矩： $-T_{ij{i}^{\prime }{j}^{\prime }}{X}_{ijk}\times f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }$， 其中：

• $T_{ij{i}^{\prime }{j}^{\prime }}$ 表示投影算子，用于投影到平面法向空间
• $n_{ij{i}^{\prime }{j}^{\prime }}$ 是未归一化的分离平面法向量

这些量在法向碰撞势能（公式8） 中已经定义。

作者为了把很多个顶点上的摩擦力组织成一个整体平衡系统，引入了一个“分离平面”作为中介。这个平面本身不是现实物体，也不参与真实动力学，但它像一个零质量的代理对象：每个顶点上的摩擦力都会在这个平面上留下一个反向作用力和相应的力矩。然后作者要求这个虚拟平面上的总力和总力矩必须平衡，这样就把原本分散的摩擦力统一组织起来了。

---

$X_{ijk}\times f^{\parallel }$：这个是“位置向量叉乘力”，也就是：力矩。所以这一部分很好理解：力作用点的位置是 $X_{ijk}$，力是 $f^{\parallel }$，它们叉乘得到转动效应。前面的负号 $-$：是因为这是记在平面上的“反作用”。顶点上受到了一个摩擦力，平面上就承受相反的作用与相应反向力矩。
为什么前面还乘一个 $T_{ij{i}^{\prime }{j}^{\prime }}$？ 这是因为作者不是关心任意三维力矩，而是关心和这个分离平面相关的那部分“平面内”力矩结构。所以要先做一个投影。
原文说： $T_{ij{i}^{\prime }{j}^{\prime }}$ 表示投影算子，用于投影到平面法向空间。投影算子就是： 把一个向量只保留某个方向/某个子空间里的分量，其他分量去掉。 比如你有一个 3D 向量， 你只想看它在某个平面里的分量， 或者只想看它沿某个法向的分量， 那就用投影。
这里为什么要投影？ 因为分离平面定义了一个特殊的几何结构：

• 有法向方向
• 有切向平面

而摩擦本来就是与这个平面几何强相关的。 所以作者不想让所有三维方向的力矩都混在一起， 而是要把它们整理成：

• 和分离平面几何结构一致的那部分分量

这就是投影的作用。 $T$ 的作用是把“由顶点摩擦力产生的力矩”整理到与分离平面相关的正确几何子空间里。

最后，由于分离平面的质量为零，它必须保持力学平衡，否则会产生无限大的加速度。 因此可以得到以下平面内力与力矩平衡条件：

因为分离平面被建模成零质量代理，所以所有作用到它上的摩擦力和由这些摩擦力产生的平面内力矩，都必须相互抵消。接着，作者把这些摩擦力对物体所做的“广义功/力学作用”写进一个增广势能表达式中，再对位姿求梯度，得到带摩擦的增广约束函数 $\bar{C}$。当这个约束等于 0 时，整个系统就在有摩擦接触下达到了力平衡、平面内力矩平衡，并满足牛顿第三定律。

$$\sum _k{f}_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }+\sum _{k^{\prime }}{f}_{i^{\prime }{j}^{\prime }{k}^{\prime },ij}^{\parallel }=0$$

来自第一个凸包所有接触顶点的摩擦力，和来自第二个凸包所有接触顶点的摩擦力，加起来必须为 0。平面内总切向力平衡。

$$T_{ij{i}^{\prime }{j}^{\prime }}\left[\sum _k{X}_{ijk}\times f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }+\sum _{k^{\prime }}{X}_{i^{\prime }{j}^{\prime }{k}^{\prime }}\times f_{i^{\prime }{j}^{\prime }{k}^{\prime },ij}^{\parallel }\right]=0(12)$$

这些摩擦力不光要合力为 0，还不能留下净转动趋势。$T$ 的作用是把总力矩投影到和分离平面建模相关的子空间里，作者只要求这一部分平衡。
所以公式 (12) 总共在表达两件事：

• 第一条：总摩擦力平衡
• 第二条：总平面内力矩平衡

这两条加起来就是说： 分离平面这个零质量代理对象，既不能被平移推动，也不能被扭转。 这就把所有局部摩擦变量“拧”成了一个整体自洽系统。

最后，我们通过定义增广势能与约束函数 $\bar{C}$ 来引入摩擦接触力：

$$\bar{C}(q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel })={\nabla }_q\left[\Psi (q,x)-\sum _{i\ne i^{\prime }}\sum _{j,j^{\prime }}\sum _k⟨X_{ijk},f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }⟩\right].(13)$$

公式(13) 是在把“原来的势能系统”和“摩擦力对位姿做的功”合在一起，然后对位姿求梯度，形成新的增广约束函数。
$\Psi (q,x)$： 这是前面已经有的总势能。 它包含前面无摩擦部分的物理能量，比如： 重力势能， 碰撞势能，其他相关势能项。它代表的是： 原来的保守力系统。

$-\sum ⟨X_{ijk},f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }⟩$：这个项是新加的，和摩擦有关。先粗略理解成： 力对位置变量的线性作用项。 它和虚功/广义力建模有关系。 这里作者不是说“摩擦变成真正势能了”， 而是在做一种很常见的优化写法： 把外加力（这里是摩擦力）通过对广义坐标的作用写进一个增广表达式里， 然后对 $q$ 求梯度，得到平衡条件。 现在先不用深究拉格朗日力学最严格形式， 先抓住这层意思： 这个内积项是在把摩擦力如何作用到位姿自由度上写出来。

为什么对 $q$ 求梯度？ 作者一直在用一个核心思想： 平衡条件可以通过“对广义位姿变量求导”来表达。如果系统对某个 $q$ 方向还有净驱动力， 那说明它还没平衡。 如果梯度为 0，说明在这些位姿自由度上已经没有继续“往前走”的趋势了。 所以现在加入摩擦之后， 作者就把原来的势能项和摩擦作用项一起考虑， 再对 $q$ 求导，得到新的约束函数： $\bar{C}$，这就是所谓的增广物理约束。
为什么叫“增广约束函数” $\bar{C}$？ 因为它比原来的 $C$ 多了摩擦这一块。 你可以理解成： 原来的 $C$ 只管无摩擦情况的物理平衡， 现在的 $\bar{C}$ 在原来的基础上，再把摩擦接触力也纳入进来， 所以它是“增强版 / 扩展版”的约束函数。

公式 (13) 最白话的理解是什么？ 把原来的物理能量和摩擦力对系统的作用合在一起，然后看它们对位姿变量的总驱动力。如果这个总驱动力为 0，就说明系统在有摩擦条件下也平衡了。 这就是 $\bar{C}$ 的物理意义。

接下来，我们总结该增广物理约束的良好性质。

性质 III.2

当 $\mu >0$ 时， $\bar{C}(q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel })$ 对 $q$ 与 $x$ 都是全局可微的。当 $\bar{C}(q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel })=0$ 时，所有物体都满足：

• 力平衡
• 平面内力矩平衡

并同时满足牛顿第三定律。

为什么先强调 $\mu >0$？ 因为只要障碍参数 $\mu$ 是正的， 前面碰撞势函数就还是那个平滑的内点障碍形式。 于是整套函数： 《碰撞部分，重力部分，摩擦项，增广约束》 都还能保持良好的光滑性。 这对 ALM / LM 很重要。

为什么“对 $q,x$ 全局可微”很重要？ 因为作者一直在坚持同一件事： 我要用二阶优化方法，所以整个约束系统必须尽量光滑。 性质 III.2 告诉你： 即使加了摩擦，这个扩展后的约束函数也没有把可微性弄坏。 这非常重要。

当 $\bar{C}=0$ 时，为什么就代表平衡？ 因为 $\bar{C}$ 本身就是按“广义平衡条件”构造的。 所以： $\bar{C}=0$ 就表示这些位姿自由度上的净驱动力为 0。再结合前面平面上的力和力矩平衡条件， 就得到： 力平衡， 平面内力矩平衡，牛顿第三定律。也就是说，系统在有摩擦情况下也形成了一个物理自洽平衡态。

我们强调，与无摩擦情况相比的一个重要区别在于：我们只能保证垂直于平面法向量方向的力矩分量达到平衡 [41]。 遗憾的是，沿着平面法向方向的力矩分量通常无法保持平衡。 但是，这一方向上的力矩不平衡程度由互补间隙（complementarity gap） $\mu$ 控制。 通过选择一个足够小的 $\mu$ 值，这种不平衡在实际中可以忽略不计。

虽然摩擦接触力在形式上很容易建模，但将这些约束加入优化问题会显著增加决策空间的维度。 具体来说，对于每一对刚体之间的每一个顶点，我们都需要优化一对切向摩擦力变量 $f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel }$ ， 因此得到如下带约束优化问题：

$$\underset{q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel }}{\mathrm{argmin}}O(q,x)(14)$$

找 $q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel }$ 使得 O(q,x)最小。

约束为

$$\text{s.t.}\{\begin{array}{rl}\bar{C}(q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel })& =0\\ ⟨f_{ijk,i^{\prime }{j}^{\prime }}^{\perp },f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }⟩& =0\\ \Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }\Vert & \le \eta \Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\perp }\Vert \\ \sum _k{f}_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }+\sum _{k^{\prime }}{f}_{i^{\prime }{j}^{\prime }{k}^{\prime },ij}^{\parallel }& =0,\\ T_{ij{i}^{\prime }{j}^{\prime }}\left[\sum _k{X}_{ijk}\times f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }+\sum _{k^{\prime }}{X}_{i^{\prime }{j}^{\prime }{k}^{\prime }}\times f_{i^{\prime }{j}^{\prime }{k}^{\prime },ij}^{\parallel }\right]& =0.\end{array}$$
上述优化问题通常使用 ALM 算法求解，而该算法在内部需要 LM 算法求解大规模线性系统，这成为主要的计算瓶颈。

1. $\bar{C}(q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel })=0$
它把： 原来的势能系统，摩擦力对系统的作用。 统一写进了一个“广义平衡条件”。 最白话说，它在要求： 整个系统在有摩擦的情况下，位姿方向上的物理平衡要成立。 也就是： 该平衡的力要平衡；该平衡的力矩要平衡；系统别还想继续动。这是总体平衡条件。
2. $⟨f_{ijk,i^{\prime }{j}^{\prime }}^{\perp },f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }⟩=0$
摩擦力必须和法向力正交。 也就是： 法向力是垂直接触面的，摩擦力必须真正在切向平面里。这是一条局部几何约束。
3. $\Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }\Vert \le \eta \Vert f_{ijk,i^{\prime }{j}^{\prime }}^{\perp }\Vert$
这就是摩擦锥约束。 表示： 每个接触顶点的摩擦力大小不能超过法向力允许的上限。 这也是一条局部物理约束。
4. ${\sum }_k{f}_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }+{\sum }_{k^{\prime }}{f}_{i^{\prime }{j}^{\prime }{k}^{\prime },ij}^{\parallel }=0$
这表示： 分离平面上的总切向力平衡。 也就是说： 来自一侧凸包所有顶点的摩擦力， 加上另一侧凸包所有顶点的摩擦力，总和必须为 0。 这是全局/集合层面的平衡约束， 不是单个顶点的局部约束。
5. $T_{ij{i}^{\prime }{j}^{\prime }}\left[{\sum }_k{X}_{ijk}\times f_{ijk,i^{\prime }{j}^{\prime }}^{\parallel }+{\sum }_{k^{\prime }}{X}_{i^{\prime }{j}^{\prime }{k}^{\prime }}\times f_{i^{\prime }{j}^{\prime }{k}^{\prime },ij}^{\parallel }\right]=0$
这表示： 分离平面上的总平面内力矩平衡。 也就是说，所有这些摩擦力相对于平面产生的转动效应，在平面相关子空间里要相互抵消。 这也是全局平衡约束。

这五条约束可以分成两层

• 第一层：总体平衡
  $\bar{C}(q,x,f^{\parallel })=0$
  这是整个系统级别的增广物理平衡。

• 第二层：局部接触合法性 + 平面代理平衡

  • 正交约束
  • 摩擦锥约束
  • 总摩擦力平衡
  • 总平面内力矩平衡

  这四类是在细化每一组摩擦接触变量到底能不能算“合法”。

为了解决这一问题，我们注意到，由摩擦力带来的额外复杂度可以通过利用 Gauss–Newton Hessian 矩阵中的特殊稀疏结构大幅降低。首先，ALM 子问题可以写成如下形式：

$$\underset{q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{}}{\mathrm{argmin}}O+{\lambda }^T\bar{C}+\frac{\rho }{2}\Vert \bar{C}{\Vert }^2+\sum _{ij{i}^{\prime }{j}^{\prime }}\Phi (q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{})(15)$$

其中，每一项$\Phi (q,x,f_{ij{i}^{\prime }{j}^{\prime }})$都包含两部分内容： 一部分是约束条件（即公式(11)和公式(12)中的约束）； 另一部分是由于每一对凸包$ij$和$i^{\prime }{j}^{\prime }$之间存在摩擦而引入的增广拉格朗日项。 具体定义在附录中给出。

虽然加入摩擦以后，ALM 子问题变得很大，但这个“大”并不是乱成一团的大。摩擦变量和摩擦约束天然是按“每一对凸包”局部组织的，所以对应的 Gauss–Newton Hessian 会出现很强的块稀疏结构。作者正是要利用这个结构，把原本很贵的大规模求解问题拆得更高效。
Hessian 是什么？ 可以粗略理解成： 目标函数在当前点附近的“二阶弯曲信息”矩阵。 它告诉你： 哪些变量之间耦合强，往哪个方向改参数更有效，局部曲率长什么样。 在 LM、Gauss–Newton、牛顿类方法里， 都会和 Hessian 或近似 Hessian 打交道。
为什么这里叫 Gauss–Newton Hessian，而不是“真正 Hessian”？ 因为在很多最小二乘/残差型问题里， 作者不会用全精确 Hessian， 而是用一个更方便、也通常更稳定的近似： Gauss–Newton 近似 Hessian。 最典型的形式你可以想成是： $J^{T}J$。其中 $J$ 是残差的 Jacobian。 不用现在死记公式， 只要知道： 它是 LM / Gauss–Newton 这类方法里自然出现的二阶近似矩阵

---

$$\underset{q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{}}{\mathrm{argmin}}O+{\lambda }^T\bar{C}+\frac{\rho }{2}\Vert \bar{C}{\Vert }^2+\sum _{ij{i}^{\prime }{j}^{\prime }}\Phi (q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{})(15)$$

第一项：$O$。就是原来的几何目标函数。 也就是： Type I，Type II，Type III，这些观测拟合项。
第二项：${\lambda }^T\bar{C}$。 这是增广拉格朗日里的乘子项。 这里的 $\bar{C}$ 已经是带摩擦的增广物理约束。
第三项：$\frac{\rho }{2}|\bar{C}{|}^2$。这是增广拉格朗日里的罚项。 目的是逼 $\bar{C}$ 更接近 0。
前三项合起来就是： “原来的带约束 ALM 主体”，只不过现在约束已经是有摩擦版本了。

第四项：${\sum }_{ij{i}^{\prime }{j}^{\prime }}\Phi (q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{})$。作者把所有“每对凸包相关的摩擦附加项”收拢成了一个总和。 也就是说： 每一对凸包 $ij$ 和 $i^{\prime }{j}^{\prime }$，都有一个属于它自己的局部附加块 $\Phi$。 这个 $\Phi$ 里面包括两类东西： 公式 (11) 和公式 (12) 里的那些摩擦约束，与这些约束对应的增广拉格朗日项。

不同凸包对之间的耦合只出现在项 $\bar{C}(q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{})$ 中。 因此 Gauss-Newton Hessian 矩阵可以写为：

$$H\triangleq {\nabla }^{2}O+\sum _{ij{i}^{\prime }{j}^{\prime }}{\nabla }^2\Phi +\nabla {\bar{C}}^T\nabla \bar{C}(16)$$

我们首先注意到 $\nabla \bar{C}$ 的秩最多为 $|q|$，即配置空间的维度。 因此我们可以利用 Woodbury 矩阵恒等式通过算法1高效求解该线性系统。

$$\underset{q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{}}{\mathrm{argmin}}O+{\lambda }^T\bar{C}+\frac{\rho }{2}\Vert \bar{C}{\Vert }^2+\sum _{ij{i}^{\prime }{j}^{\prime }}\Phi (q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{})(15)$$

那为什么 $\bar{C}$ 会耦合？ 因为 $\bar{C}$ 是一个全局增广物理约束。 它对 $q$ 求梯度，而且里面把： 整个势能，所有摩擦力对系统位姿的作用。统一合在一起了。 所以从系统级别看，$\bar{C}$ 会同时“看到”很多不同凸包对。 因此： 它是那个把局部块串起来的全局主干。 这就是作者说“耦合只出现在 $\bar{C}$ 中”的意思。

$$H\triangleq {\nabla }^{2}O+\sum _{ij{i}^{\prime }{j}^{\prime }}{\nabla }^2\Phi +\nabla {\bar{C}}^T\nabla \bar{C}(16)$$ 是 Gauss–Newton / LM 思路下，当前子问题二阶近似的三部分来源。

第一项：${\nabla }^{2}O$ 在干什么？ 这是原始几何目标函数 $O(q,x)$ 的 Hessian。 它代表： 当前几何拟合目标（Type I/II/III）对变量的二阶曲率信息。 这部分主要和： shape，pose 有关。 它是全局主目标的一部分。
第二项：$\sum {\nabla }^2\Phi$ 在干什么？ 这是所有局部摩擦块的二阶信息加起来。 因为每个 $\Phi$ 只对应一对凸包， 所以这部分天然是： 按凸包对局部求和的块结构。 这是后面能快的关键之一。
第三项：$\nabla {\bar{C}}^T\nabla \bar{C}$ 在干什么？ 这是最关键的一项。 它来自全局约束 $\bar{C}$ 的 Gauss–Newton 近似。 你可以把它理解成： 全局约束对变量的“耦合贡献”。

图5：我们展示了矩阵$H$（左）和矩阵$A$（右）的结构。矩阵$A$是块对角形式，并且每个块都很小。矩阵$H$的分解使用了Woodbury矩阵恒等式。

相关矩阵结构如图5所示。 然而，算法1仍然需要求解左侧矩阵 $A$ 的线性系统，该矩阵仍然涉及所有决策变量。 幸运的是，由于矩阵 $\nabla \bar{C}$ 已经从矩阵 $A$ 中分离出来，不同凸包对之间的摩擦力已经解耦。 因此可以使用 Schur complement（舒尔补）求解器高效求解该线性系统。 具体而言，对于任意右侧向量 $b$，可以将其分解为：

$$b=(b_{qx}^T,{\cdots }^T,b_{ij{i}^{\prime }{j}^{\prime }}^T,{\cdots }^T{)}^T(17)$$

其中第一个块对应矩阵中前 $|q|+|x|$ 行。 后续每个块对应一个凸包对 $ij{i}^{\prime }{j}^{\prime }$ 的行。

类似地，${\nabla }^2\Phi (q,x,f_{ij{i}^{\prime }{j}^{\prime }}^{\parallel })$ 的 Hessian 具有如下分块结构：

$${\nabla }^2\Phi =\left(\begin{array}{cc}{\nabla }_{qx}^2\Phi & {\nabla }_{qx,ij{i}^{\prime }{j}^{\prime }}^2\Phi \\ {\nabla }_{ij{i}^{\prime }{j}^{\prime },qx}^2\Phi & {\nabla }_{ij{i}^{\prime }{j}^{\prime }}^2\Phi \end{array}\right)(18)$$

利用上述记号，我们可以按照 算法2 所示的方法求解该线性系统。