SLAM文献之KernelGPA: A Globally Optimal Solution to Deformable SLAM in Closed-form

KernelGPA: A Globally Optimal Solution to Deformable SLAM in Closed-form 提出了一种在非刚性变形环境下求解 SLAM 问题的闭式全局最优解方法。下面是对其算法原理和核心推导过程的系统解析。

---

一、算法背景与目标

问题描述：

传统 SLAM 主要假设环境为刚性，而在一些应用场景（如医疗、软体机器人）中，环境本身具有可变形性。KernelGPA 针对该问题，提出了一种既考虑传感器位姿、又能建模环境变形的非刚性 SLAM 方法，并提供了 闭式解。

---

二、核心建模思想：从 Procrustes 到 KernelGPA

1. Generalized Procrustes Analysis (GPA)

GPA 是一种将多个形状对齐到共同参考框架的统计方法。对于输入点集 { X i } i = 1 n \{\mathbf{X}_i\}_{i=1}^n {Xi​}i=1n​，它寻找刚性变换 ${\mathbf{R}}_i,{\mathbf{t}}_i$ 使得对齐误差最小化：

$$\underset{{\mathbf{R}}_i,{\mathbf{t}}_i,\mathbf{M}}{\min }\sum _{i=1}^n\Vert {\mathbf{R}}_i{\mathbf{X}}_i+{\mathbf{t}}_i-\mathbf{M}{\Vert }_F^2$$

其中 $\mathbf{M}$ 是对齐后形状的平均。

2. KernelGPA 的扩展

GPA 假设刚性变换，KernelGPA 拓展为可变形变换模型：

每一帧传感器的点云 ${\mathbf{X}}_i\in {\mathbb{R}}^{N\times 3}$ 经变换后对齐到全局模板 $\mathbf{M}$，目标是估计：

• 每一帧的变换 ${\Phi }_i(\cdot )$：考虑环境变形与相机运动的组合
• 全局地图 $\mathbf{M}$

---

三、核回归变形模型（Kernel-based deformation model）

设变形变换为：

$${\Phi }_i(\mathbf{x})=\mathbf{x}+{\mathbf{K}}_{\mathbf{x}}{\mathbf{W}}_i$$

其中：

• ${\mathbf{K}}_{\mathbf{x}}\in {\mathbb{R}}^{1\times M}$：核矩阵（如高斯核）；
• ${\mathbf{W}}_i\in {\mathbb{R}}^{M\times 3}$：每一帧的权重参数（表示非刚性变形）；
• 核函数如 $k(\mathbf{x},{\mathbf{c}}_j)=\exp (-\Vert \mathbf{x}-{\mathbf{c}}_j{\Vert }^2/2{\sigma }^2)$

将所有点堆叠成矩阵形式 ${\mathbf{X}}_i$，则有：

$${\Phi }_i({\mathbf{X}}_i)={\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i$$

---

四、闭式解推导过程

Step 1: 构建损失函数

目标是将所有帧经过变形变换后的点云对齐到一个模板 $\mathbf{M}$：

min ⁡ M , { W i } ∑ i = 1 n ∥ Φ i ( X i ) − M ∥ F 2 = ∑ i = 1 n ∥ X i + K i W i − M ∥ F 2 \min_{\mathbf{M}, \{\mathbf{W}_i\}} \sum_{i=1}^n \| \Phi_i(\mathbf{X}_i) - \mathbf{M} \|_F^2 = \sum_{i=1}^n \| \mathbf{X}_i + \mathbf{K}_i \mathbf{W}_i - \mathbf{M} \|_F^2 M,{Wi​}min​i=1∑n​∥Φi​(Xi​)−M∥F2​=i=1∑n​∥Xi​+Ki​Wi​−M∥F2​

对 ${\mathbf{W}}_i$ 和 $\mathbf{M}$ 求导并令导数为 0，得到一组闭式最小二乘方程。

Step 2: 闭式解 ${\mathbf{W}}_i$

固定 $\mathbf{M}$，对每个 ${\mathbf{W}}_i$ 有：

$${\mathbf{W}}_i=({\mathbf{K}}_i^{\top }{\mathbf{K}}_i{)}^{-1}{\mathbf{K}}_i^{\top }(\mathbf{M}-{\mathbf{X}}_i)$$

将其代入总损失函数中，再对 $\mathbf{M}$ 求闭式解：

$$\mathbf{M}=\frac{1}{n}\sum _{i=1}^n({\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i)$$

此过程迭代一次即可收敛（线性系统）。

详细推导过程如下：

原始优化目标

考虑 $n$ 帧点云 { X i } i = 1 n \{\mathbf{X}_i\}_{i=1}^n {Xi​}i=1n​，每帧 $N$ 个点，每个点三维坐标。引入核变形模型：

$${\Phi }_i({\mathbf{X}}_i)={\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i$$

• ${\mathbf{X}}_i\in {\mathbb{R}}^{N\times 3}$：第 $i$ 帧原始点云
• ${\mathbf{K}}_i\in {\mathbb{R}}^{N\times M}$：第 $i$ 帧的核矩阵（如高斯核）
• ${\mathbf{W}}_i\in {\mathbb{R}}^{M\times 3}$：待优化的变形系数
• $\mathbf{M}\in {\mathbb{R}}^{N\times 3}$：全局模板

最小化目标函数：

$$\mathcal{L}=\sum _{i=1}^n{\Vert {\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i-\mathbf{M}\Vert }_F^2$$

---

对 ${\mathbf{W}}_i$ 的导数与闭式解推导

我们先固定 $\mathbf{M}$，对每帧的 ${\mathbf{W}}_i$ 求解：

记：

$${\mathcal{L}}_i={\Vert {\mathbf{K}}_i{\mathbf{W}}_i-(\mathbf{M}-{\mathbf{X}}_i)\Vert }_F^2$$

展开 Frobenius 范数：

$${\mathcal{L}}_i=\mathrm{tr}\left[({\mathbf{K}}_i{\mathbf{W}}_i-{\mathbf{Y}}_i{)}^{\top }({\mathbf{K}}_i{\mathbf{W}}_i-{\mathbf{Y}}_i)\right]\text{其中 }{\mathbf{Y}}_i:=\mathbf{M}-{\mathbf{X}}_i$$

求导：

$$\frac{\partial {\mathcal{L}}_i}{\partial {\mathbf{W}}_i}=2{\mathbf{K}}_i^{\top }({\mathbf{K}}_i{\mathbf{W}}_i-{\mathbf{Y}}_i)$$

令导数为零：

$${\mathbf{K}}_i^{\top }{\mathbf{K}}_i{\mathbf{W}}_i={\mathbf{K}}_i^{\top }{\mathbf{Y}}_i\Rightarrow \boxed{{\mathbf{W}}_i=({\mathbf{K}}_i^{\top }{\mathbf{K}}_i{)}^{-1}{\mathbf{K}}_i^{\top }(\mathbf{M}-{\mathbf{X}}_i)}$$

这是典型的多变量线性最小二乘解。

---

对 $\mathbf{M}$ 的导数与闭式解推导

将上面得到的 ${\mathbf{W}}_i$ 代入损失函数，对 $\mathbf{M}$ 求导：

目标函数重新写成：

$$\mathcal{L}(\mathbf{M})=\sum _{i=1}^n{\Vert {\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i-\mathbf{M}\Vert }_F^2$$

记 ${\mathbf{Z}}_i={\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i$，有：

$$\mathcal{L}=\sum _{i=1}^n\Vert {\mathbf{Z}}_i-\mathbf{M}{\Vert }_F^2=\sum _{i=1}^n\mathrm{tr}\left[({\mathbf{Z}}_i-\mathbf{M}{)}^{\top }({\mathbf{Z}}_i-\mathbf{M})\right]$$

对 $\mathbf{M}$ 求导：

$$\frac{\partial \mathcal{L}}{\partial \mathbf{M}}=-2\sum _{i=1}^n({\mathbf{Z}}_i-\mathbf{M})\Rightarrow n\mathbf{M}=\sum _{i=1}^n{\mathbf{Z}}_i\Rightarrow \boxed{\mathbf{M}=\frac{1}{n}\sum _{i=1}^n({\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i)}$$

---

小结：闭式解流程

1. 初始化 $\mathbf{M}$（例如平均点云）

2. 对每帧计算：

   $${\mathbf{W}}_i=({\mathbf{K}}_i^{\top }{\mathbf{K}}_i{)}^{-1}{\mathbf{K}}_i^{\top }(\mathbf{M}-{\mathbf{X}}_i)$$

3. 更新全局模板：

   $$\mathbf{M}=\frac{1}{n}\sum _{i=1}^n({\mathbf{X}}_i+{\mathbf{K}}_i{\mathbf{W}}_i)$$

4. 若需要可继续迭代一次或两次，实际收敛很快。

---

五、尺度模糊与刚性优化

由于所有变形都是相对的，闭式解存在全局尺度不确定性。KernelGPA 引入一个后处理优化：

刚性最大化约束（Rigidity Maximization）：

通过优化使每帧变形尽可能刚性（例如保持局部距离），如下：

$$\underset{s}{\max }\sum _{i=1}^n\Vert J_{{\Phi }_i}(\mathbf{x}{)}^{\top }{J}_{{\Phi }_i}(\mathbf{x})-\mathbf{I}{\Vert }_F^2$$

此步骤用于解析尺度并加强物理约束。

---

六、实验与性能

在多个非刚性数据集（肝脏、面部、肺部 CT）上实验，结果表明：

• KernelGPA 在 精度、鲁棒性 和 收敛速度 上优于现有非刚性 SLAM 方法；
• 闭式解显著减少了迭代优化的计算开销。

---

七、总结

特性	描述
目标	解决可变形环境下的 SLAM 问题
方法	基于核回归的变形建模 + Procrustes 闭式对齐
优势	闭式解、无初值依赖、全局最优、低计算量
应用	医学成像、软体机器人、动态环境建图