主流多机SLAM框架——分布式状态估计的两种方法

Kimera-Multi

核心状态变量是所有机器人的位姿：

\[X_{\alpha i} = (R_{\alpha i}, t_{\alpha i}) \in SE(3)\]

其中 $\alpha$ 表示机器人编号，$i$ 表示该机器人轨迹中的第 $i$ 个 keyframe。

测量分两类：

第一类是 odometry edges，来自每个机器人自己的 Kimera-VIO。作者认为这些 odometry measurement 基本没有 outlier，所以用普通二次损失。

第二类是 loop closures，包括 intra-robot loop closure 和 inter-robot loop closure。这些可能由于 perceptual aliasing 产生外点，所以需要 robust cost。

因此总优化问题是：

\[\min_{\{X_{\alpha i}\}} \sum_{\alpha \in \mathcal{R}} \sum_{i=1}^{n_{\alpha}-1} r_{\alpha i}(X_{\alpha i}, X_{\alpha,i+1})^2 + \sum_{(\alpha i,\beta j)\in \mathcal{L}} \rho\left( r_{\alpha i}^{\beta j}(X_{\alpha i}, X_{\beta j}) \right)\]

这里 $\mathcal{R}$ 是机器人集合，$\mathcal{L}$ 是所有 loop closures，$\rho$ 是 truncated least squares，TLS robust cost。

loop closure residual 用的是 chordal distance。对一个相对位姿测量：

\[\hat{X}_{\alpha i}^{\beta j} = (\hat{R}_{\alpha i}^{\beta j}, \hat{t}_{\alpha i}^{\beta j})\]

残差写成：

\[r_{\alpha i}^{\beta j} = \left( w_R \left\| R_{\beta j} - R_{\alpha i}\hat{R}_{\alpha i}^{\beta j} \right\|_F^2 + w_t \left\| t_{\beta j} - t_{\alpha i} - R_{\alpha i}\hat{t}_{\alpha i}^{\beta j} \right\|_2^2 \right)^{1/2}\]

这个建模非常典型：多机协同 SLAM 后端本质上就是一个带外点的多机器人相对位姿图优化问题。

多机器人 SLAM = 多机器人局部 odometry + intra/inter-robot loop closures 构成的鲁棒分布式 pose graph optimization。

它的关键难点不是 residual 怎么写，而是：

第一，如何初始化多个机器人之间的坐标系变换。

第二，如何在存在错误 inter-robot loop closures 的情况下鲁棒优化。

第三，如何把 centralized PGO 改造成只依赖局部通信的 distributed PGO。

第四，如何在通信受限下只交换必要的 descriptors、public poses 和 loop closure 信息。

从这个角度看，Kimera-Multi 的真正创新点集中在 robust distributed back-end，尤其是两阶段 D-GNC：先鲁棒初始化机器人坐标系，再用分布式 GNC + RBCD 做鲁棒 PGO。

Kimera-Multi 的 Section V 不是一个“普通 PGO + 分布式通信”的简单组合，而是三层东西叠在一起：

1. 先解决多机器人没有共同坐标系的问题：鲁棒初始化。
2. 再解决回环里有错误匹配的问题：逐步非凸鲁棒优化，也就是 GNC。
3. 最后解决不能集中求解的问题：用 RBCD 做分布式位姿图优化。

论文里把这三件事压得很短，所以读起来会感觉“知道它在做鲁棒分布式 PGO，但不知道它到底怎么动起来”。下面从 Section V 开始，把它拆成一个 3 机器人例子来讲。

---

1. Kimera-Multi 后端到底在优化什么？

假设有 3 个机器人：

\[R_0,\ R_1,\ R_2\]

每个机器人都有自己的局部轨迹：

\[X_0=\{X_{0,1},X_{0,2},...,X_{0,n_0}\}\] \[X_1=\{X_{1,1},X_{1,2},...,X_{1,n_1}\}\] \[X_2=\{X_{2,1},X_{2,2},...,X_{2,n_2}\}\]

每个 $X_{\alpha,i}$ 是一个三维位姿：

\[X_{\alpha,i}=(R_{\alpha,i},t_{\alpha,i})\in SE(3)\]

其中 $\alpha$ 是机器人编号，$i$ 是这个机器人的第 $i$ 个关键帧。

位姿图里有两类边。

第一类是里程计边。例如 $R_0$ 自己的连续关键帧之间有：

\[X_{0,1}\rightarrow X_{0,2}\rightarrow X_{0,3}\]

这些边来自单机 VIO。Kimera-Multi 默认认为这些里程计边没有严重外点，所以用普通二次损失。

第二类是回环边。回环边又分为单机内部回环和机器人之间的回环。比如：

\[X_{0,5} \leftrightarrow X_{1,8}\]

表示 $R_0$ 的第 5 帧和 $R_1$ 的第 8 帧看到了同一个地方，于是前端估计出一个相对位姿测量。这个测量可能是真的，也可能是错的。Kimera-Multi 的鲁棒性主要就是为了处理这些错误回环。

因此，集中式版本的目标函数可以写成：

\[\min_{\{X_{\alpha,i}\}} \sum_{\alpha} \sum_{i=1}^{n_\alpha-1} r_{\alpha,i}(X_{\alpha,i},X_{\alpha,i+1})^2 + \sum_{(\alpha i,\beta j)\in \mathcal{L}} \rho \left( r_{\alpha i}^{\beta j}(X_{\alpha,i},X_{\beta,j}) \right)\]

这里：

\[\mathcal{L}\]

是所有回环边集合，包括单机内部回环和多机之间回环。

普通里程计边用：

\[r^2\]

回环边用鲁棒损失：

\[\rho(r)\]

Kimera-Multi 里选择的鲁棒损失是 截断最小二乘。直观理解就是：

\[\rho(r)= \begin{cases} r^2, & r^2 \leq \bar c^2 \\ \bar c^2, & r^2 > \bar c^2 \end{cases}\]

如果一条回环残差小，认为它可信，正常优化；如果残差大到超过阈值，就不再让它继续拉扯轨迹。

论文中的回环残差不是普通李代数残差，而是 chordal distance。对一条从 $X_{\alpha,i}$ 到 $X_{\beta,j}$ 的相对位姿测量：

\[\hat X_{\alpha i}^{\beta j}=(\hat R_{\alpha i}^{\beta j},\hat t_{\alpha i}^{\beta j})\]

残差是：

\[r_{\alpha i}^{\beta j} = \left( w_R \|R_{\beta j}-R_{\alpha i}\hat R_{\alpha i}^{\beta j}\|_F^2 + w_t \|t_{\beta j}-t_{\alpha i}-R_{\alpha i}\hat t_{\alpha i}^{\beta j}\|_2^2 \right)^{1/2}\]

这个式子的意思很简单：如果当前估计的两个位姿 $X_{\alpha,i},X_{\beta,j}$ 是对的，那么它们之间推出来的相对旋转和平移，就应该接近前端给出的回环测量。Kimera-Multi 对里程计边用普通二次代价，对回环边用截断最小二乘鲁棒代价。

---

2. 为什么多机器人问题必须先初始化？

单机器人位姿图有一个天然初值。你从第一帧开始，把 VIO 的相对运动一段一段乘起来，就能得到所有位姿的大致初值。

但 3 个机器人时，每个机器人只知道自己的局部坐标系：

\[A,\ B,\ C\]

例如：

• $R_0$ 的轨迹在坐标系 $A$ 下；
• $R_1$ 的轨迹在坐标系 $B$ 下；
• $R_2$ 的轨迹在坐标系 $C$ 下。

这三个坐标系之间一开始没有关系。你不能直接把 $R_0$、$R_1$、$R_2$ 的位姿放进同一个全局图里优化。

所以 Section V 的第一步是：

先估计机器人局部坐标系之间的相对变换。

比如要把 $R_1$ 的坐标系 $B$ 对齐到 $R_0$ 的坐标系 $A$，我们需要估计：

\[X_B^A\]

也就是“从 $B$ 到 $A$ 的坐标变换”。

---

3. 一个机器人之间的回环如何推出坐标系变换？

假设 $R_0$ 和 $R_1$ 之间有一条回环：

\[X_{0,i} \leftrightarrow X_{1,j}\]

前端几何验证给出一个相对位姿测量：

\[\hat X_{0i}^{1j}\]

意思是：从 $R_0$ 的第 $i$ 帧看，$R_1$ 的第 $j$ 帧大概在哪里。

同时，两个机器人各自在自己的局部坐标系里有 VIO 初值：

\[\tilde X_{0,i}^{A}\] \[\tilde X_{1,j}^{B}\]

那么这一条回环可以推导出一个 $B$ 到 $A$ 的坐标系对齐候选值：

\[\tilde X_{B,ij}^{A} = \tilde X_{0,i}^{A} \hat X_{0i}^{1j} (\tilde X_{1,j}^{B})^{-1}\]

这个式子可以这样理解：

从 $B$ 坐标系下的 $R_1$ 第 $j$ 帧出发，先变到这个帧本身，再通过回环测量变到 $R_0$ 的第 $i$ 帧，最后再变到 $A$ 坐标系。

所以，每一条机器人之间的回环，都可以给出一个机器人坐标系之间的对齐候选。

如果 $R_0$ 和 $R_1$ 之间有 10 条回环，那么我们就有 10 个候选：

\[\tilde X_{B,1}^{A},\tilde X_{B,2}^{A},...,\tilde X_{B,10}^{A}\]

如果其中 8 条是真回环，它们推出的坐标系变换应该非常接近；如果 2 条是错回环，它们推出的变换会偏得很离谱。

这正是论文 Fig. 4 的核心意思：正确回环会给出彼此一致的坐标系对齐，错误回环会给出离群的对齐结果。

---

4. 鲁棒初始化：先做“坐标系对齐的鲁棒平均”

于是 $R_0$ 和 $R_1$ 的初始化问题变成：

\[X_B^A = \arg\min_{X\in SE(3)} \sum_{(i,j)\in\mathcal{L}_{01}} \rho \left( r_{ij}(X) \right)\]

其中：

\[r_{ij}(X) = \|X \ominus \tilde X_{B,ij}^{A}\|_{\Sigma}\]

也就是说，我们要在所有候选坐标系变换里面，找一个最一致的平均值，同时把离群候选降权或剔除。

这一步本质上不是完整的轨迹优化，而是：

在机器人对之间做一个鲁棒的相对坐标系估计。

以 3 个机器人为例：

• $R_0$ 和 $R_1$ 之间估计 $X_B^A$
• $R_1$ 和 $R_2$ 之间估计 $X_C^B$
• $R_0$ 和 $R_2$ 之间如果有回环，也可以估计 $X_C^A$

然后构建一个机器人级别的图。节点是机器人，边是“这两个机器人之间有回环”。在这个机器人图上选一棵生成树。

比如选择：

\[R_0 \longleftrightarrow R_1 \longleftrightarrow R_2\]

把 $R_0$ 的坐标系 $A$ 当作全局坐标系，那么：

\[X_B^A\]

可以直接把 $R_1$ 初始化到 $A$ 下。

对 $R_2$，先有：

\[X_C^B\]

再组合：

\[X_C^A = X_B^A X_C^B\]

于是 3 个机器人的所有局部轨迹都被放进了同一个全局坐标系。

这就是 Kimera-Multi 的第一阶段。多机器人不像单机器人那样可以简单通过里程计链得到全局初值，所以 D-GNC 第一阶段要用鲁棒、通信高效的方法把所有机器人轨迹初始化到共同坐标系中；第二阶段才真正做完整的分布式鲁棒优化。

---

5. 什么是 GNC？先从普通鲁棒优化说起

现在假设 3 个机器人已经在同一个坐标系下了。接下来我们要优化完整位姿图。

如果所有回环都是对的，直接最小二乘就行：

\[\min_X \sum_i r_i(X)^2\]

但实际中会有错误回环。错误回环的问题是：它的残差很大，如果仍然用 $r^2$，它会强行把两个本来不该对齐的地方拉到一起，导致整张图被扯坏。

所以我们希望用截断最小二乘：

\[\rho(r)=\min(r^2,\bar c^2)\]

这个代价函数的图像是：小残差时是二次函数，大残差时直接封顶。

这就带来一个问题：它是非凸的。

非凸意味着优化很容易陷入错误局部最优。比如刚开始轨迹还没对齐时，某些正确回环的残差也可能很大，如果你一开始就用强鲁棒核，它可能会误判这些正确回环是外点，然后永远用不上它们。

GNC 的思想就是：

不要一开始就使用很硬的截断鲁棒核，而是从一个更平滑、更容易优化的近似问题开始，然后逐渐变成真正的截断最小二乘。

GNC 的核心是先从鲁棒代价函数的凸近似开始，再逐渐引入非凸性，从而避免收敛到错误解；每一步优化一个更容易的替代代价函数，最后逐渐逼近原始鲁棒代价函数。

---

6. GNC 的数学重写：给每条回环加一个权重

GNC 通过 Black–Rangarajan 对偶，把鲁棒优化写成带权重的形式：

\[\min_{X,\ w_i\in[0,1]} \sum_i \left[ w_i r_i(X)^2 + \Phi_{\rho_\mu}(w_i) \right]\]

这里每个回环约束都有一个权重：

\[w_i\in[0,1]\]

它的物理意义非常直观：

• $w_i=1$：这条回环基本可信；
• $w_i=0$：这条回环基本是外点；
• $0<w_i<1$：暂时不确定，先部分相信。

于是原来的“非凸鲁棒优化”被改写成两个子问题交替做：

固定权重，优化轨迹：

\[X \leftarrow \arg\min_X \sum_i w_i r_i(X)^2\]

固定轨迹，更新权重：

\[w_i \leftarrow f(r_i,\mu,\bar c)\]

然后更新控制参数：

\[\mu \leftarrow \text{new }\mu\]

这就是 GNC 的主要结构。

---

7. GNC 权重更新到底怎么判断内点和外点？

Kimera-Multi 对截断最小二乘使用下面的权重更新规则：

\[w_i \leftarrow \begin{cases} 0, & \tilde r_i^2 \in \left[ \frac{\mu+1}{\mu}\bar c^2,\ +\infty \right] \\[4pt] \frac{\bar c}{\tilde r_i}\sqrt{\mu(\mu+1)}-\mu, & \tilde r_i^2 \in \left[ \frac{\mu}{\mu+1}\bar c^2,\ \frac{\mu+1}{\mu}\bar c^2 \right] \\[4pt] 1, & \tilde r_i^2 \in \left[ 0,\ \frac{\mu}{\mu+1}\bar c^2 \right] \end{cases}\]

这个公式看起来复杂，但意思非常简单。它有两个门槛：

低门槛：

\[\frac{\mu}{\mu+1}\bar c^2\]

高门槛：

\[\frac{\mu+1}{\mu}\bar c^2\]

如果残差平方低于低门槛，权重设为 1；如果高于高门槛，权重设为 0；如果在中间，就给一个 0 到 1 之间的软权重。

这个权重更新只依赖当前残差、控制参数 $\mu$ 和截断阈值 $\bar c$，因此特别适合分布式并行计算。

举一个数值例子。设：

\[\bar c=1\]

初期取：

\[\mu=0.1\]

则：

\[\frac{\mu}{\mu+1}\bar c^2 = \frac{0.1}{1.1} \approx 0.091\] \[\frac{\mu+1}{\mu}\bar c^2 = \frac{1.1}{0.1} = 11\]

这时候只有特别离谱的残差才会直接被置零。很多约束即使残差偏大，也只是被部分降权。这对应“先温和一点，不要太早杀死回环”。

后期如果：

\[\mu=10\]

则：

\[\frac{\mu}{\mu+1}\bar c^2 = \frac{10}{11} \approx 0.91\] \[\frac{\mu+1}{\mu}\bar c^2 = \frac{11}{10} \approx 1.1\]

这时候两个门槛非常接近 $\bar c^2=1$。残差小于阈值的基本保留，残差大于阈值的基本剔除。也就是说，GNC 后期才变成真正“硬”的截断最小二乘。

这就是“逐步非凸”的含义：一开始问题比较平滑，后面逐渐接近真正的鲁棒核。

---

8. 什么是 RBCD？先看固定权重后的问题

GNC 的每一轮都有一个“固定权重后优化轨迹”的步骤。

固定所有 $w_i$ 后，问题变成普通加权位姿图优化：

\[\min_{\{X_{\alpha,i}\}} \sum_{\alpha} \sum_{i=1}^{n_\alpha-1} r_{\alpha,i}(X_{\alpha,i},X_{\alpha,i+1})^2 + \sum_{(\alpha i,\beta j)\in\mathcal{L}} w_{\alpha i}^{\beta j} \cdot r_{\alpha i}^{\beta j}(X_{\alpha,i},X_{\beta,j})^2\]

这一步没有鲁棒核了，因为鲁棒性已经通过权重 $w$ 体现出来了。

现在问题是：这个加权 PGO 怎么分布式求？

---

9. RBCD 的核心：每个机器人只优化自己的轨迹块

RBCD 可以翻译成 流形块坐标下降。

“块”的意思是：把变量按机器人分块。

\[X_0 = \{R_0 \text{ 的所有位姿}\}\] \[X_1 = \{R_1 \text{ 的所有位姿}\}\] \[X_2 = \{R_2 \text{ 的所有位姿}\}\]

全局目标函数可以写成：

\[F(X_0,X_1,X_2)\]

它由几部分组成：

\[F(X_0,X_1,X_2) = F_0(X_0) + F_1(X_1) + F_2(X_2) + F_{01}(X_0,X_1) + F_{02}(X_0,X_2) + F_{12}(X_1,X_2)\]

其中：

• $F_0(X_0)$：机器人 0 自己的里程计边和内部回环；
• $F_1(X_1)$：机器人 1 自己的里程计边和内部回环；
• $F_2(X_2)$：机器人 2 自己的里程计边和内部回环；
• $F_{01}(X_0,X_1)$：机器人 0 和机器人 1 之间的回环；
• $F_{02}(X_0,X_2)$：机器人 0 和机器人 2 之间的回环；
• $F_{12}(X_1,X_2)$：机器人 1 和机器人 2 之间的回环。

RBCD 的一次循环可以写成：

\[X_0^{k+1} = \arg\min_{X_0} F(X_0,X_1^k,X_2^k)\] \[X_1^{k+1} = \arg\min_{X_1} F(X_0^{k+1},X_1,X_2^k)\] \[X_2^{k+1} = \arg\min_{X_2} F(X_0^{k+1},X_1^{k+1},X_2)\]

也就是说：

先固定 $R_1,R_2$，让 $R_0$ 优化自己的整条轨迹。

再固定 $R_0,R_2$，让 $R_1$ 优化自己的整条轨迹。

最后固定 $R_0,R_1$，让 $R_2$ 优化自己的整条轨迹。

这就是“块坐标下降”。

为什么叫“流形”？因为 $X_{\alpha,i}$ 不是普通欧氏向量，而是在 $SE(3)$ 上的位姿。更新不能简单做：

\[X \leftarrow X+\Delta X\]

而要在位姿流形上做更新：

\[X \leftarrow X \oplus \delta\]

其中 $\delta$ 是切空间里的小增量。

更严格地说，Kimera-Multi 使用的 RBCD 是在位姿图的秩限制松弛上操作，然后再投影回 $SE(3)$，论文实现里默认松弛秩是 5；但从算法结构上，可以先把它理解成“每次只优化一个机器人自己的轨迹块，其他机器人的轨迹暂时固定”。

---

10. 3 个机器人通信时，RBCD 到底交换什么？

假设有下面几条机器人之间的回环：

\[X_{0,5} \leftrightarrow X_{1,8}\] \[X_{0,12} \leftrightarrow X_{2,3}\] \[X_{1,10} \leftrightarrow X_{2,6}\]

当 $R_0$ 更新自己的轨迹 $X_0$ 时，它需要计算和 $R_1,R_2$ 之间的回环残差。

对于边：

\[X_{0,5} \leftrightarrow X_{1,8}\]

$R_0$ 需要知道 $X_{1,8}$ 当前是多少。

对于边：

\[X_{0,12} \leftrightarrow X_{2,3}\]

$R_0$ 需要知道 $X_{2,3}$ 当前是多少。

但它不需要知道 $R_1$ 和 $R_2$ 的完整轨迹，只需要这些与自己有回环连接的关键帧位姿。

论文把这些叫作 public poses，可以翻译成“公开位姿”或“边界位姿”。RBCD 中每个机器人只负责估计自己的轨迹；当某个机器人更新时，它只需要与存在机器人间回环的邻居通信，并且只需要接收邻居机器人那些参与跨机器人回环的公开位姿。

所以一次 RBCD 循环的通信可以这样理解：

更新 $R_0$：

$R_0$ 向 $R_1$ 请求：

\[X_{1,8}\]

向 $R_2$ 请求：

\[X_{2,3}\]

然后固定它们，优化自己的 $X_0$。

更新 $R_1$：

$R_1$ 向 $R_0$ 请求：

\[X_{0,5}\]

向 $R_2$ 请求：

\[X_{2,6}\]

然后优化自己的 $X_1$。

更新 $R_2$：

$R_2$ 向 $R_0$ 请求：

\[X_{0,12}\]

向 $R_1$ 请求：

\[X_{1,10}\]

然后优化自己的 $X_2$。

这里的关键是：没有任何一个机器人需要拿到全队完整图，也没有中心服务器。

---

11. 把 GNC 和 RBCD 组合起来，就是 Kimera-Multi 的 D-GNC

现在把前面的东西合起来。

D-GNC 的整体流程是：

第 0 步：每个机器人本地跑 VIO

三个机器人各自得到自己的局部轨迹：

\[\tilde X_0^A,\ \tilde X_1^B,\ \tilde X_2^C\]

这些轨迹还不在同一个坐标系下。

第 1 步：鲁棒初始化

用机器人之间的回环，估计：

\[X_B^A,\ X_C^B,\ X_C^A\]

然后选机器人级生成树，把所有轨迹变到一个全局坐标系里。

比如选 $R_0$ 为全局坐标系：

\[X_0^A=\tilde X_0^A\] \[X_1^A=X_B^A\tilde X_1^B\] \[X_2^A=X_C^A\tilde X_2^C\]

这一步解决“多机器人没有共同坐标系”的问题。

第 2 步：所有回环权重初始化为 1

一开始不急着判断谁是外点：

\[w_i=1\]

这表示暂时相信所有回环。

第 3 步：固定权重，用 RBCD 优化轨迹

固定当前 $w_i$，问题变成加权 PGO：

\[\min_X \sum_{\text{里程计}} r^2 + \sum_{\text{回环}} w_i r_i^2\]

然后 3 个机器人按照块坐标下降更新：

\[R_0 \rightarrow R_1 \rightarrow R_2 \rightarrow R_0 \rightarrow ...\]

论文实现中，并不是每次都把这个加权 PGO 解到完全收敛，而是默认执行固定次数的 RBCD 迭代，默认 $T=15$。这样可以降低分布式求解的通信和计算时间，下一轮 GNC 会用当前结果继续热启动。

第 4 步：固定轨迹，更新每条回环权重

现在有了新的轨迹估计，就可以重新计算每条回环的残差。

比如边：

\[X_{0,5} \leftrightarrow X_{1,8}\]

如果当前轨迹下它残差很小：

\[r_{0,5}^{1,8} \approx 0\]

则：

\[w_{0,5}^{1,8}\rightarrow 1\]

如果残差巨大：

\[r_{0,5}^{1,8} \gg \bar c\]

则：

\[w_{0,5}^{1,8}\rightarrow 0\]

这一步可以分布式完成：

• 单机内部回环：机器人自己算；
• 机器人之间回环：两个相关机器人中的一个负责算，然后把新权重发给另一个。

每个回环权重可以独立并行更新；对于机器人之间的回环，由两个相关机器人中的一个计算新权重，然后发给另一个机器人。

第 5 步：更新 $\mu$

让代价函数更加接近真正的截断最小二乘。

也就是从“温和降权”逐渐变成“明确保留或剔除”。

第 6 步：重复 3–5

最终结果是：

• 正确回环权重接近 1；
• 错误回环权重接近 0；
• 每个机器人得到全局一致轨迹；
• 整个过程不需要中心服务器。

---

12. 用一个具体小例子串起来

假设三台机器人 $R_0,R_1,R_2$ 各走了一段轨迹。

前端检测到这些机器人之间的回环：

\[e_1: X_{0,5} \leftrightarrow X_{1,8}\] \[e_2: X_{0,10} \leftrightarrow X_{1,15}\] \[e_3: X_{1,6} \leftrightarrow X_{2,4}\] \[e_4: X_{0,20} \leftrightarrow X_{2,2}\]

其中 $e_4$ 是错误回环。

初始化阶段

$e_1,e_2$ 都连接 $R_0,R_1$，它们各自推出一个 $B$ 到 $A$ 的坐标系变换：

\[\tilde X_{B,1}^{A},\tilde X_{B,2}^{A}\]

如果它们一致，就得到可靠的：

\[X_B^A\]

$e_3$ 连接 $R_1,R_2$，推出：

\[X_C^B\]

于是：

\[X_C^A=X_B^A X_C^B\]

这样三台机器人进入共同坐标系。

$e_4$ 虽然是错误回环，但如果不在生成树上，初始化阶段可能暂时不影响坐标系传播；即使它参与某个 pairwise 对齐，GNC 也倾向于把它作为离群候选降权。

第一次 GNC 外层循环

所有权重先设为：

\[w_1=w_2=w_3=w_4=1\]

RBCD 优化轨迹时，四条回环都会拉动轨迹。但因为鲁棒初始化已经比较好，正确回环 $e_1,e_2,e_3$ 的残差会变小，错误回环 $e_4$ 仍然很大。

权重更新后：

\[w_1\approx 1,\quad w_2\approx 1,\quad w_3\approx 1,\quad w_4<1\]

后续 GNC 外层循环

随着 $\mu$ 增大，判断越来越硬：

\[w_4\rightarrow 0\]

于是 $e_4$ 在后续 RBCD 优化中几乎不再起作用。

最终优化问题近似变成：

\[\min_X \sum_{\text{里程计}} r^2 + r_{e_1}^2 + r_{e_2}^2 + r_{e_3}^2\]

错误回环被自动剔除。

这就是 D-GNC 的本质：用 GNC 判断哪些回环可信，用 RBCD 在分布式网络里优化可信回环约束下的全局轨迹。

---

13. 为什么不能只用 RBCD，不用 GNC？

只用 RBCD 的话，固定目标就是普通最小二乘：

\[\min_X \sum_i r_i^2\]

错误回环残差很大，平方之后影响更大，会强行把轨迹拉坏。

RBCD 只是一个分布式求解器，它不负责判断哪条边是错的。它只能优化你给它的目标函数。

所以：

• RBCD 解决的是“怎么分布式优化”；
• GNC 解决的是“哪些回环该信，哪些不该信”。

没有 GNC，RBCD 会被错误回环污染。

---

14. 为什么不能只用 GNC，不用 RBCD？

只用 GNC 默认是集中式框架。固定权重后的变量更新需要解一个完整加权 PGO：

\[\min_X \sum_i w_i r_i^2\]

如果集中式求解，必须有一个中心节点拿到所有机器人轨迹、所有边、所有回环。

这违反 Kimera-Multi 的分布式设定。

所以：

• GNC 提供鲁棒框架；
• RBCD 让 GNC 的变量更新步骤分布式化。

这就是“分布式 GNC + RBCD”的真实含义。

---

15. 一句话总结 Kimera-Multi Section V

Kimera-Multi 的 Section V 可以总结成：

先用机器人间回环估计各机器人局部坐标系之间的鲁棒初始对齐；然后把多机器人 SLAM 写成带外点回环的位姿图优化问题；再用 GNC 给每条回环引入可更新权重，实现外点剔除；最后在 GNC 的每次固定权重优化中，用 RBCD 让每个机器人只优化自己的轨迹块，并只和邻居交换公开位姿，从而实现鲁棒分布式位姿图优化。

可以把它记成这个公式关系：

\[\text{D-GNC} = \text{鲁棒初始化} + \text{GNC 外层鲁棒权重更新} + \text{RBCD 内层分布式轨迹优化}\]

其中：

\[\text{GNC}:\quad \text{决定每条回环的可信度}\] \[\text{RBCD}:\quad \text{在固定可信度下分布式优化轨迹}\]

这才是 Kimera-Multi 分布式后端的核心。