主流多机SLAM框架——多机SLAM问题思考与总结

这几周的时间里读了不少关于多机协同SLAM系统以及consensus理论的论文、各种书以及blog等等，内容很广很杂，感觉有必要在此进行系统性梳理以更好地进入下一阶段的深入研究（毕竟能讲出来才是真得学会了๐•ᴗ•๐）。

本文针对主流多机协同SLAM系统涉及到的Methodology以及实验进行梳理总结，包括了我自己对一些基础知识的学习和理解、各个开源框架的个性与共性设计思路还有一些方法的推导。（由于本人水平有限，如有错误，请联系我修改！）

本文主要涉及以下论文：

[1] Tian Y, Chang Y, Arias F H, et al. Kimera-multi: Robust, distributed, dense metric-semantic slam for multi-robot systems[J]. IEEE transactions on robotics, 2022, 38(4).

[2] Zhu F, Ren Y, Yin L, et al. Swarm-LIO2: Decentralized efficient LiDAR-inertial odometry for aerial swarm systems[J]. IEEE Transactions on Robotics, 2024, 41: 960-981.

[3] Liu X, Lei J, Prabhu A, et al. Slideslam: Sparse, lightweight, decentralized metric-semantic slam for multirobot navigation[J]. IEEE Transactions on Robotics, 2025, 41: 6529-6548.

当然这三篇论文都涉及到很多其他论文，比如单机视觉里程计Kimera、单机激光里程计Fast-LIO1/2、Mesh的构建方法、ESIKF、时间同步等。三篇论文中我觉得kimera是最难理解的，理解了kimera就能把其他两篇论文理解到位。

1. 分布式多机系统涉及的核心问题

读过多篇论文后会发现，虽然分布式多机SLAM涉及到的理论方法和工程细节极其繁杂，但在系统设计层面解决的问题在大方向上是具有绝对共性的，这其中主要涉及到以下几个方面的内容：

• 底层单一机器人里程计（多机前端）：这一点很好理解，由于我们讨论的系统都不是集中式的，而是分布式的，所以每一个机器人都作为一个独立节点有自己独立的状态估计能力。这就使得底层的单机VIO/LIO等里程计算法变得非常重要，我们当然希望每个机器人本身就有很好的SLAM能力，能够基于自身的传感器数据稳定且高效地解算出高精度的位姿和地图数据。比如kimera-multi的底层就是kimera-vio，Swarm-lio2的底层就是fast-lio1/2。当然，如果一个多机系统能不受传感器的限制，做好单机和多机的接口，不管单一机器人用的是lidar还是camera都能与系统集成兼容，就具备了更好的泛化能力，比如上面提到的论文[3]Slideslam和Swarm-SLAM，这也是分布式多机SLAM的一个重要方向。个人认为，单机SLAM是多机SLAM前端的一环。

• 回环检测（多机前端）：在已有单机里程计的基础上，多机之间的数据关联是进行分布式计算的一个前提，无论系统建模成什么问题，也不管采取的是基于滤波还是优化的方法，检查并构建机器人之间的共视关系是非常重要的一环。总结来看，回环之间的匹配大多采取现成的方法，比如视觉可以基于DBow2词袋、Supor Point&Glue等方法来实现匹配，只需要传递描述子信息即可，也可以像SlideSLAM一样做物体级的匹配，不需要传递原始的keypoint数据。这里值得一提的是基于词袋模型的匹配并不需要匹配上很多个点，阈值设定的经验值可以很小（比如10-15），lidar则也有自己现成的匹配方法(我对激光SLAM的回环检测没那么熟悉)。这里的难点主要是如何设计压缩数据，降低回环检测交换数据的大小和占用的带宽，减少通信压力。

• 系统methodology（多机后端）：系统层面的方法主要是将多机SLAM建模成一个什么样的问题，并选择一种求解方法。在系统建模上，对于基于优化的方法目前看下来的这些文章在loss function的设计上大同小异，唯一的区别就是到底是纯轨迹的位姿图优化问题还是带有landmark的因子图优化问题

  比如Kimera-Multi求解的核心问题：

  \[\min_{\mathbf{X}_{\alpha_i} \in SE(3), \forall\alpha\in\mathcal{R}, \forall i} \sum_{\alpha\in\mathcal{R}} \sum_{i=1}^{n_\alpha-1} r_{\alpha_i}(\mathbf{X}_{\alpha_i}, \mathbf{X}_{\alpha_{i+1}})^2 + \sum_{(\alpha_i,\beta_j)\in \mathcal{L}} \rho\left(r_{\beta_j}^{\alpha_i}(\mathbf{X}_{\alpha_i}, \mathbf{X}_{\beta_j})\right)\]

  包含里程计误差（标准最小二乘形式）和回环误差（包含自身回环以及机器人间的回环，用核函数分段降低大误差权值）可以看到Kimera-Multi系统的核心loss function是不包含landmark的，只有位姿的优化。

  而SlideSLAM则不同，其求解的核心问题为：

  \[\mathbf{x}_{1:t}^{(j)}, \hat{\mathbf{x}}_{1:t}^{(k)}, \mathcal{M}^{(k)} = \arg\max_{\mathbf{x}_{1:t}^{(j)}, \mathbf{x}_{1:t}^{(k)}, \mathcal{M}} P(\mathbf{x}_{1:t}^{(j)}, \mathbf{x}_{1:t}^{(k)}, \mathcal{M} | \mathcal{I}_t^{(k)})\]

  这个贝叶斯形式的公式实际上是包含了里程计误差、轨迹误差、landmark误差的，因此SlideSLAM同时优化位姿和landmark。

  以上两个系统都是基于优化方法的，对于Swarm-LIO2这样基于滤波方法的系统，是在ESIKF递推框架下，每个机器人分散地估计自身状态和相对于所有队友的全局外参变换，其中状态向量为：

  \[\mathbf{x}_{i} = \left[{}^{G_i}R_{b_i}^T, {}^{G_i}p_{b_i}^T, {}^{G_i}v_{b_i}^T, b_{g_i}^T, b_{a_i}^T, {}^{G_i}g^T, \cdots, {}^{G_i}R_{G_j}^T, {}^{G_i}p_{G_j}^T, \cdots \right]^T\]

  Prediction:

  \[\hat{\mathbf{x}}_{i,k|k-1}, \mathbf{P}_{k|k-1} = \mathcal{PREDICT}(\hat{\mathbf{x}}_{i,k-1|k-1}, \mathbf{u}_{i,k}, \mathbf{P}_{k-1|k-1})\]

  Update:

  \[\hat{\mathbf{x}}_{i,k|k}, \mathbf{P}_{k|k} = \mathcal{UPDATE}(\hat{\mathbf{x}}_{i,k|k-1}, \mathcal{Y}_{i,k}, \mathbf{P}_{k|k-1})\]

  可以看出Swarm-LIO2无显式landmark变量，每个机器人独立运行ESIKF，仅通过共享相互观测和位姿估计进行松散协作，在线递推的点云+相互观测融合，而非离线批量全局优化。

• 初始化计算：绕不开的一个核心问题，必须计算出多机器人之间的相对位姿变换。

• 通信策略：目前看到的多机SLAM基本上都在陆地上基于wifi通信，这类技术实际上已经相对成熟。像阿木实验室也有现成的模块来组网，根据之前的使用经验，由于多机SLAM系统基本上不传输原始的图像数据，因此数传范围可以稳定在几公里。因此通信模块是组成多机系统的重要一环，但基本上所有的工作都不会把创新点放在这一部分。

• 实验方案：多机的数据集目前还是比较缺少的，一个思路是把单机数据集切成几段来模拟过程，MIT之前也开源了kimera的数据，但现在关闭了申请通道。根据不同的任务，实验的设置也不一样，比如swarm-lio2更偏向于多机目标跟踪这样的编队任务，验证了多个无人机穿越森林、和跟踪移动目标的能力，而kimera则面向全局的语义mesh地图构建。总的来说，多机协同SLAM除了需要验证轨迹的精度、一致性以外也要度量数据通信带来的延迟、数据量等指标。

2. 核心基础概念

2.1 Landmark路标

下面这张图表示多个机器人的位姿图和landmark：

这里衍生出几个问题：

1. 什么是landmark？
2. landmark在单机和多机系统中的作用分别是什么，对应的formulate的问题又是什么？
3. 对于包含全局地图（Mesh或点云）来说，是否需要优化landmark，以及如何保证不同机器人看到的landmark的一致性以及构建的全局地图的一致性(最后这个问题我们可以先行搁置，在对每个系统建模后再来寻找答案)。

2.1.1 什么是 Landmark？

在 SLAM 系统中，理解 Landmark（路标/特征点）需要将其在物理前端与数学后端剥离开来看。

• 物理视角（前端锚点）： Landmark 是现实环境中的静止、显著且能被重复观测的物理特征。不考虑线特征或语义信息的时候，landmark就是前端提取的特征点对应的3D空间点。在视觉 SLAM 中，它们是被提取出的角点、边缘（如 ORB、FAST 特征点），甚至可以是高阶的线、面或语义物体。前端的核心任务是通过描述符匹配和几何校验，完成数据关联（Data Association），即认出“不同视角下看到的是同一个物理锚点”。
• 数学视角（后端变量）： 一旦进入优化器，物理特征就变成了纯粹的数学变量 $l_j \in \mathbb{R}^3$。它们在因子图中作为独立的节点，与机器人的位姿节点 $x_i$ 相连，构成了约束网络的基础。

2.1.2 Landmark 在单机与多机系统中的作用及问题建模 (Formulation)

Landmark 在单机和多机系统中的处理方式不同，这直接反映在优化问题的数学建模上：

单机系统：联合优化 (Joint Optimization)：

在以 ORB-SLAM 或 VIO（如 VINS-Fusion 的局部滑动窗口）为代表的高精度单机模块中，位姿和 Landmark 通常会被联合优化（Bundle Adjustment, BA）。

• 作用： 显式地参与地图构建，通过重投影误差不断修正自身的 3D 坐标，同时拉扯相机的位姿。

• 数学建模： 这是一个标准的无约束非线性最小二乘问题（假设不考虑零空间固定）：

  \[\min_{\mathcal{X}, \mathcal{L}} \sum_{(i,j)} \| z_{ij} - h(x_i, l_j) \|_{\Sigma}^2\]

  其中，$\mathcal{X}$ 是所有位姿节点，$ \mathcal{L}$ 是所有 Landmark 节点，$z_{ij}$ 是观测值（如像素坐标），$h(\cdot)$ 是投影函数。这种模型精度极高，但矩阵维度随 Landmark 数量呈爆炸式增长。

• ORB-SLAM2 的做法：无论是局部建图线程（Local BA），还是全局回环线程（Global BA），都在同时优化相机位姿（Pose）和 3D 地图点（Landmark）。这就导致它精度极高，但也使得它的计算复杂度非常庞大。

• VINS-Fusion 的做法：在它的前端和 VIO 滑动窗口阶段（Estimator）：VINS-Fusion 是一个紧耦合框架。它在滑动窗口内，不仅优化位姿、速度、IMU零偏，而且显式地优化了Landmark。 只不过它为了数值稳定性，没有直接优化 Landmark 的 (X,Y,Z)，而是优化了 Landmark 在相机坐标系下的逆深度。在它的全局回环阶段（Pose Graph Thread）为了保证实时的计算效率，VINS-Fusion 在检测到回环后，丢弃了所有的 Landmark，仅仅构建了一个只包含位姿的4自由度的位姿图进行优化。

多机系统：边缘化与物体级

在多机协同系统（如分布式后端）中，为了满足严苛的通信带宽限制和分布式计算要求，系统通常采取降维策略。

• 策略1-边缘化Landmark ：Landmark 不再是后端优化的变量。两个机器人在前端匹配到共享的 Landmark 集合后，会在本地利用这些点计算出一个相对位姿变换（Relative Pose, $T_{AB}$）。一旦 $T_{AB}$ 计算完成，这些 Landmark 的历史使命就结束了，直接被边缘化，不进入全局分布式优化器。

• 策略2-物体级Landmark ：当landmark不再是稀疏的特征点，而是场景中的一个个物体，那此时的landmark的维度的数量就大幅减小，可以作为优化变量进行求解。

2.2 因子图 & 位姿图

在明确了 Landmark 在系统中的不同存在形式后，我们需要理解后端优化的核心数据结构。在 SLAM 中，因子图（Factor Graph）、位姿图（Pose Graph）以及非线性最小二乘（Non-linear Least Squares）问题是构成后端优化框架的三大支柱。

2.2.1 因子图与位姿图的区别与联系

简而言之：包含的变量种类和层级不同。位姿图优化（PGO）是因子图优化（FGO）的一种特例（或者说降维简化版）。

• 因子图 (Factor Graph)：

  • 结构： 包含两种节点。一是变量节点，用圆圈表示（需要被估计的状态，包括机器人的位姿、环境中的地图点/Landmark、IMU零偏、相机内外参等）；二是因子节点，用方块表示（代表测量模型或约束，如相机的重投影误差、IMU预积分误差等）。

  • 特点： 它是 SLAM 问题最通用、最细粒度的数学图模型表达，什么都能往里装。

在多机语义 SLAM（如 SlideSLAM）中，由于将环境物体（如汽车、树木等语义级别 Landmark）作为了优化变量，其状态空间小且具有高度的复用和协同价值，因此系统会显式保留这些物体节点，执行完整的因子图优化 (FGO)。

下面这张图表示因子图：

• 位姿图 (Pose Graph)：

  • 结构： 只有机器人的位姿节点（如 $SE(3)$），边只有位姿之间的相对运动变换（由里程计或闭环检测生成的 $SE(3)$ 测量值）。

  • 特点： 这是一种高度抽象的图结构。环境中的所有点云或网格级别的 Landmark 都被剥离或隐藏了，常用于纯粹的轨迹优化。

在大规模稠密分布式多机系统（如 Kimera-Multi）中，为满足严苛的通信带宽和计算资源限制，系统绝对不传递也绝不联合优化数十万个三维点。它只传输相对位姿约束，其分布式后端执行的是纯粹的位姿图优化 (PGO)。

2.2.2 因子图如何与非线性最小二乘问题对应？

在 SLAM 中，我们希望根据所有的传感器测量值 $Z$，推断出系统最可能的状态 $X$（位姿、地图点等）。在概率论中，这被称为最大后验概率估计（MAP, Maximum A Posteriori）。

因子图与非线性最小二乘就是同一问题的两种角度：因子图是通过高斯噪声假设和负对数似然推导出来的。

概率图表达：

因子图表示了状态 $X$ 和测量值 $Z$ 之间的联合概率分布。假设各个测量相互独立，联合概率可以分解为各个因子的乘积：

其中 $X_i$ 是第 $i$ 个因子相关的变量，$z_i$ 是具体的测量值。

高斯噪声假设：

我们假设传感器的测量误差服从高斯分布，那么第 $i$ 个测量的概率密度可以写成：

这里 $h_i(X_i)$ 是观测模型（比如重投影方程），$\Sigma_i$ 是测量的协方差矩阵（表示传感器的精度）。

定义误差项（Residual/Error）为 $e_i(X_i) = h_i(X_i) - z_i$。

从求最大概率到求最小误差（最小二乘）：

为了最大化后验概率

我们对整个概率取负对数（负对数将乘积变成求和，且单调性不变）：

这正是一个标准的非线性最小二乘的目标函数。

其中 $\Sigma_i^{-1}$ 称做信息矩阵（权重），代表我们对这个测量项的信任程度。

二者如何实现相互转换？：

从因子图 → 最小二乘问题（建图过程）：

当算法需要求解时，必须把因子图”翻译”成数学多项式语方程式：

1. 遍历变量节点（State Variables）：把所有需要优化的状态拼接成一个大的状态向量 $x$。

2. 遍历因子节点（Factor/Measurement）：针对每一个因子，写出它的误差函数 $e_i(x)$ 和信息矩阵 $\Omega_i$。

3. 构建代价函数：将所有因子的误差项加起来：

1. 线性化与求解：对非线性的误差函数在当前状态处进行一阶泰勒展开（线性化），构建线性方程组（法方程 $H \Delta x = b$，其中 $H = J^T \Omega J$）。然后用高斯-牛顿法 (G-N) 或列文伯格-马夸尔特法 (L-M) 求解更新量 $\Delta x$。

从最小二乘问题 → 因子图（可视化与结构分析过程）：

反过来，如果你有一个最小二乘标准形式 $F(x) = \sum_k |r_k(x)|^2$，你可以按照以下规则构建因子图：

1. 识别变量节点：看代价函数中有哪些独立的变量块（比如位置 $T_1, T_2$，点 $P_1$）。每一个变量画一个圆圈（变量节点）。

2. 识别因子节点：看代价函数有几个求和项（几个误差来源 $r_k$）。每一个求和项 $r_k$ 画一个方块（因子节点）。

3. 建立连接（Edges）：观察误差 $r_k$ 依赖于哪些变量。例如，重投影误差 $r_k(T_1, P_1)$ 同时依赖于机器人位姿 $T_1$ 和地图点 $P_1$，那就把方块 $k$ 与圆圈 $T_1$ 和圆圈 $P_1$ 相连。

二者的对应关系非常直观，相当于同一问题的几何视角与代数视角：

• 因子图中的一个变量节点 $\longleftrightarrow$ 最小二乘中的一个待优化自变量。

• 因子图中的一个因子节点 $\longleftrightarrow$ 代价函数中的一个误差平方项 $e_i^T \Sigma_i^{-1} e_i$。

• 因子图的拓扑结构 $\longleftrightarrow$ 决定了最小二乘问题中雅可比矩阵（或海塞矩阵 $\mathbf{H}$）的稀疏结构。如果两个变量在图中没有因子相连，矩阵对应位置的元素必定为 0。

2.2.3 从因子图到位姿图的数学降维：边缘化（Marginalization）

对于 Kimera-Multi 这类将稠密 Landmark 丢弃的系统，其实 Landmark 并不是凭空消失的，而是通过严格的数学推导被“折叠”进了相对位姿中。这个将庞大因子图坍缩成纯位姿图的过程，在数学上称为 舒尔补（Schur Complement），在 SLAM 领域俗称 边缘化。注意这个基于舒尔补的边缘化过程是发生在单机VIO中的。

严谨的推导过程如下：

假设当前系统状态包含所有位姿集合 $\mathbf{x}$ 和所有路标点集合 $\mathbf{l}$。对重投影误差进行一阶泰勒展开，并令目标函数导数为 0，得到高斯-牛顿法的高斯正规方程（Normal Equations）：

由于在视觉 SLAM 中路标点相互之间没有直接观测（点看不到点），矩阵 $\mathbf{H}_{ll}$ 必然是一个块对角矩阵（Block-diagonal matrix），极易求逆。我们可以从上述方程组的第二行解出 $\delta \mathbf{l}$：

将其代回第一行方程中，即可彻底消去 $\delta \mathbf{l}$，得到边缘化后的纯位姿正规方程：

此时，我们定义新的纯位姿信息矩阵 \(\mathbf{H}_{xx}^*\) 和残差项 \(\mathbf{g}_x^*\)：

• 数学上的 Schur Complement：

  \[\mathbf{H}_{xx}^* = \mathbf{H}_{xx} - \mathbf{H}_{xl} \mathbf{H}_{ll}^{-1} \mathbf{H}_{lx}\]
• \[\mathbf{g}_x^* = \mathbf{g}_x - \mathbf{H}_{xl} \mathbf{H}_{ll}^{-1} \mathbf{g}_l\]

物理意义与结论：

1. 纯粹的位姿图形成： 待求解方程变成了 $\mathbf{H}_{xx}^* \delta \mathbf{x} = \mathbf{g}_x^*$。数以万计的路标点变量完全退出求解方程，只留下规模极小的机器人位姿增量 $\delta \mathbf{x}$ 进行优化。

2. 信息无损压缩： 路标点被丢弃但信息未丢。路标点提供的约束信息（\(\mathbf{H}_{xl} \mathbf{H}_{ll}^{-1} \mathbf{H}_{lx}\)）作为附加权重，死死地叠加到了位姿的信息矩阵上。

3. 生成新边（约束）： 原本因子图中两个位姿是通过共视 Landmark 建立弱联系（$x_1 \rightarrow l_1 \leftarrow x_2$），经过舒尔补的矩阵乘法运算后，这部分区域被非零元素填补。这在图拓扑上的物理表现就是：这两个位姿之间生成了一条直接相连的新边。Kimera-Multi 等分布式系统正是利用这一条条新边，仅交换位姿图信息即可完成多机协同优化。

2.3 一致性

机器人各自的位姿是如何保证在全局坐标系下是一致的呢？只要有两个机器人之间回环的相对位姿变换就可以保证吗？到底如何理解位姿在全局上达成了一致？

核心问题：机器人位姿在全局上是如何达成一致的？

仅仅有两个机器人之间的相对位姿变换是不够的。 相对位姿只是一个测量值（带有噪声），真正保证它们在全局上达成一致的，是背后的全局图优化机制。

为了透彻理解“全局一致”到底是什么意思，我们可以用弹簧质点模型来建立物理直觉，并结合位姿图的初始化和优化的过程来分析。单次相对位姿只是提供了一条“弹簧约束”，它能把独立的图连起来，但不能保证全局准确。

2.3.1 全局坐标系的初始化

在机器人刚启动时，Robot A 和 Robot B 都认为自己的起点是宇宙的中心 $(0,0,0)$。此时，它们生活在两个平行的局部坐标系中。

当它们第一次相遇（第一次产生共视），前端算出了一个相对位姿变换 $T_{AB}$。系统会利用这第一个 $T_{AB}$，执行一次鲁棒分布式初始化：

1. 系统指定 Robot A 的原点作为全局坐标系的绝对原点。

2. Robot B 把自己所有的历史轨迹，直接乘以 $T_{AB}^{-1}$，强行搬到 Robot A 的坐标系下。

此时，它们在同一个坐标系下了，但这叫“一致”吗？不叫。 因为这只是基于单次测量的粗略对齐，伴随着里程计的漂移，如果它们继续走下去，轨迹依然是错位的。

假设 Robot A 和 Robot B 继续各自探索，由于各自的里程计（视觉或惯性）都在累积误差，过了十分钟，它们第二次相遇了。

• 前端通过第二次共视，又算出了一个新的相对位姿 $T’_{AB}$。
• 但是，如果此时你拿 Robot A 当前在全局坐标系下的绝对位姿 $X_A$，乘以这个新的 $T’_{AB}$，算出来的 Robot B 的位姿，绝对不可能等于 Robot B 自己在全局坐标系下的当前位姿 $X_B$。
• 数学表达就是：$X_A \cdot T’_{AB} \neq X_B$。

这就叫“全局不一致”（Inconsistency）。 测量数据（$T’_{AB}$）和系统当前的状态（$X_A, X_B$）产生了严重的自相矛盾。

2.3.2 位姿图优化：以弹簧质点模型类比

那么，如何消除这种矛盾，达成真正的“全局一致”？这就需要进行位姿图优化（Pose Graph Optimization，PGO）。

假设一个物理系统：

1. 质点（Node）： 机器人的每一个位姿 $X_i$，都是一个漂浮在 3D 空间中的铁球。

2. 里程计弹簧（Odometry Edge）： 机器人自己的前后两个位姿之间，连着一根蓝色的弹簧。这根弹簧的“自然长度”是里程计测出来的相对运动。

3. 回环弹簧（Loop Closure Edge）： 当两个机器人相遇时，Robot A 的铁球和 Robot B 的铁球之间，被硬拉上了一根红色的弹簧。这根弹簧的“自然长度和扭转角度”就是前端算出来的相对位姿 $T_{AB}$。

一开始，因为累积误差的存在，Robot A 和 Robot B 的铁球位置不对，这根红色的回环弹簧会被严重拉扯或挤压（产生了残差）。

全局优化？就是松开手，让这个由成千上万个铁球和弹簧组成的巨大系统，在三维空间中自由释放内部的弹力。有的铁球被往前拉一点，有的被往后拽一点。系统的总势能（所有弹簧被拉伸/压缩的能量总和）在这个过程中不断下降。

数学上的等价表达，就是最小化所有残差的平方和：

什么是全局达成了一致？

当整个弹簧系统彻底静止，达到了势能最低点（Minimum Energy State）时，我们就说所有的位姿在全局上达成了一致（Consensus）。

全局一致性是建立在大量的里程计弹簧和多条回环弹簧相互交织之上的，通过非线性优化（比如 ADMM 或 D-GNC 算法）将所有矛盾的张力释放到最低点时，这些漂浮的坐标轴才算真正对齐在了一个逻辑严密的全局地图中。

在这个静止状态下：

• 没有任何一个测量（相对位姿）是被完美满足的（每根弹簧都还有一点点微小的形变）。
• 但是，所有测量之间的矛盾被平均、最优地化解了。Robot A 和 Robot B 的绝对坐标 $(X_A, X_B)$ 妥协到了一个让全网误差最小的黄金位置。

实际论文中的多机系统一致性在下面进行展开分析。

3. 系统建模与一致性

系统建模的一个核心依据可以认为是多机系统面向的任务到底需不需要追求高度的全局一致性，通俗点说就是，如果机器人更多只是关心自己跑的怎么样，拿队友的数据作为辅助，也不关心landmark，那他就是不追求全局一致的，面向的任务可能是目标跟踪或者集群导航，比如Swarm-LIO2的实验中，对移动目标跟踪时，无人机之间不需要绝对的全局一致性，只要能够维护自身轨迹以及与其他相邻无人机之间的外参变换就可以了。而像Kimera/Slide这类系统，最后建立了稠密的地图或语义目标级别的地图，则更看重全局的一致性，所以在建模的时候一定会综合考虑自身位姿、队友位姿以及路标，只不过对路标的处理方法各异。

3.1 Kimera-Multi

Kimera-Multi的流程可以总结为几个核心模块：回环检测-全局外参鲁棒初始化-分布式位姿图优化-Mesh优化。即机器人相遇后首先交换关键帧特征点的描述子信息检测回环，如果回环存在则恢复相对位姿。随后系统采用层级初始化策略建立全局参考系：编号最小的机器人作为Leader定义全局坐标系原点和方向，其他机器人通过inter-robot loop closure测量计算自身相对于全局坐标系的对齐变换。所有机器人的位姿被投影到全局坐标系后，形成一个包含局部和全局约束的统一优化问题。优化阶段，Leader按顺序调度各个机器人执行本地DPGO优化步骤。优化收敛后，通过Mesh融合模块将各个机器人的局部mesh结果投影到全局坐标系生成统一的3D模型。

(1)鲁棒分布式初始化

多机器人系统在启动时各自处于独立的局部坐标系中。在进行全局 DPGO 联合优化前，必须将所有机器人的轨迹统一到一个全局参考系下。然而，由于视觉感知混淆，机器人间的直接回环（inter-robot loop closures）往往包含大量错误（Outliers），如果直接使用会导致初始化的地图骨架严重扭曲。因此，Kimera-Multi 提出了一种分为两步的鲁棒分布式初始化机制打破这一僵局：

第一步：成对坐标系估计 (Pairwise Coordinate Frame Estimation)

假设机器人 $\alpha$ 和 $\beta$（分别对应局部坐标系 $A$ 和 $B$）在物理世界相遇并产生了回环，记 $\alpha$ 的第 $i$ 个位姿与 $\beta$ 的第 $j$ 个位姿间的相对测量为 $\tilde{X}{\beta_j}^{\alpha_i} \in SE(3)$。结合它们各自局部的里程计位姿估计 $\hat{X}{\alpha_i}^A$ 和 $\hat{X}_{\beta_j}^B$，可以推导出坐标系 $B$ 原点相对于坐标系 $A$ 原点的一个候选变换：

这个公式(4)本质上是一个坐标系变换链：先将 $\beta$ 倒退回其局部原点，通过回环跨越到 $\alpha$ 视角，再回到 $\alpha$ 的局部原点。

由于两车相遇时可能产生多个回环，且其中往往掺杂着错误的感知匹配，系统不会简单地求平均，而是构建了一个鲁棒位姿平均 (Robust Pose Averaging) 优化问题：

其中，$\rho$ 是截断最小二乘（TLS）鲁棒代价函数，$L_{\alpha,\beta}$ 是两车间的回环集合。$r_{ij}(X)$ 衡量了待求的平均变换与单次测量推导出的候选变换之间的测地线距离残差：

通过使用 渐进非凸性 (GNC) 算法求解公式(5)，系统能够像“投票”一样，自动识别出相互一致的正确回环（内点），并将偏离过大的错误回环（外点）权重归零。这一步实现了两人之间排除干扰、极其精准的相对坐标系“对表”。

第二步：多机器人坐标系树 (Multi-robot Coordinate Frame Estimation)

完成两两之间的鲁棒“对表”后，系统在整个机器人团队中建立一棵生成树 (Spanning Tree)。选定 Leader（如编号最小的 Robot 1）的局部坐标系作为宇宙中心的全局坐标系。

对于其他没有与 Leader 直接碰面的机器人，只需沿着生成树的唯一路径，将沿途高度可靠的鲁棒成对变换进行矩阵复合（相乘），即可推导计算出各自相对于全局坐标系原点的初始变换 $\hat{X}_B^A$。拿到该变换矩阵后，各机器人只需在本地将其局部轨迹投影（相乘）到该全局坐标系中，即可完成高质量的全局初始化，为后续的 DPGO 奠定无畸变的基础，而且全程无需交换完整的轨迹数据。

投影到全局坐标系下的代码如下：

auto T = TLocalInit.value();  // 获取本地坐标系中的轨迹
  for (size_t i = 0; i < num_poses(); ++i) {
    Pose T_robot_frame(T.pose(i));              // 第i个pose，在本地robot坐标系中
    Pose T_world_frame = T_world_robot * T_robot_frame;  // ← 矩阵乘法投影
    T.pose(i) = T_world_frame.pose();           // 更新为全局坐标系中的pose
  }

(2)分布式位姿图优化

求解的核心问题：

包含里程计误差（标准最小二乘形式）和回环误差（包含自身回环以及机器人间的回环，用核函数分段降低大误差权值）可以看到Kimera-Multi系统的核心loss function是不包含landmark的，只有位姿的优化。

其中 \(r_{\beta_i}^\alpha(\mathbf{X}_{\alpha_i}, \mathbf{X}_{\beta_j})\) 定义为：

轨迹的一致性分析：

为什么最后多个robots能达到全局一致：它们始终在同一个坐标系中求解同一个优化问题的不同部分。这个函数包含了所有的局部里程计误差和所有的跨机器人回环误差 。只不过为了避免把所有数据传给一台中央服务器，算法（RBCD 黎曼块坐标下降法）被设计成了“切块”执行：Robot 1 负责更新这个大方程中属于自己的那几个全局坐标变量，算完后把更新后的全局坐标发给 Robot 2；Robot 2 接着更新自己的变量，以此类推 。

所有机器人都将位姿投影到了一个全局坐标系下，求解的问题也是关于全局坐标系的，求救过程中两个机器人会异步地不停互相发送数据进行迭代，直到两个位姿收敛到一个平衡点使得回环的误差最小，就是说两边不断互相交换回环点的位姿迭代优化结果，最后在global坐标系下，回环处的位姿误差达到了最小，轨迹实现了全局一致

就是说他们实际求解的是同一个大的优化问题，整个问题包含每个自身的里程计误差和所有的回环点误差，假如两个机器人检测出5个回环帧，就会把这5个回环帧之间的误差用计算的相对位姿加和算出来放进优化问题里，由于他们都是投影到一个绝对的参考系也就是global下，只要通信能够保证所有机器人都覆盖到，就能够实现全局一致的轨迹，即使这些轨迹是分段的，在单一机器人上的，但只要从各自机器人中提取出最后的轨迹，一定能够在全局上完美融合在一起。

这在代码中体现如下：

// dpgo/src/PGOAgent.cpp
// 局部项（odometry + local loop closures）
for (const auto &edge : mPoseGraph->localEdges()) {
  cost += edge->residual(X[edge.src], X[edge.dst]);  // f_k
}

// inter-robot项（inter-robot measurements）
for (const auto &edge : mPoseGraph->sharedEdges()) {
  cost += edge->weight * edge->residual(X[edge.src], X[edge.dst]);  // ρ_w
}

(3)Mesh优化

这里有必要先理解Mesh、Voxblox、TSDF及变形图的概念，理解Mesh与体素的核心对应关系：

TSDF：TSDF (Truncated Signed Distance Function, 截断符号距离函数)：像素（Pixel）是二维图像的基本单位，而体素（Voxel）是三维空间的基本单位。空间被划分为 3D 的体素网格。每个体素里存的不是颜色，而是一个距离值——该体素到最近物体表面的距离。现在我们有了一个由无数个“体素”填满的房间。假设房间中间放着一个真实的苹果。我们怎么在这些体素里记录这个苹果的形状呢？这就是 TSDF 要干的活。

• Distance（距离）：我们在每一个体素里存一个数字，这个数字代表当前这个体素的中心点，距离苹果表面有多远。
• Signed（符号）：为了区分物体的里面和外面，我们人为规定：如果体素在苹果外面（空气中），距离记为正数（比如 +2 厘米）。如果体素在苹果里面（果肉里），距离记为负数（比如 -1 厘米）。苹果的表面（表皮），就是数值正好等于 0 的地方！ 这在数学上叫零交叉点（Zero-crossing）。
• Truncated（截断）：房间很大，远离苹果的体素（比如离了 3 米远）记录精确距离既浪费内存又没意义。所以我们设定一个“截断范围”（比如贴近苹果表面的前后 5 厘米）。只要超出这个范围的体素，我们统统记成一个最大值（比如统统记为 +5 厘米），不再精细计算。这就极大地节省了算力。

体素是容器（一个个小方块），TSDF 是装在容器里的数据（带有正负号的距离值）。我们通过看哪些体素里存的数值是 0，就能完美地勾勒出物体的表面轮廓。

Mesh：虽然 TSDF 很好地用数学方式描述了物体的表面，但它对于渲染（给人类看）和物理碰撞（让机器人避障）来说不够直观。这时候就需要 Mesh。Mesh 是由顶点（Vertices）、边（Edges）和面（Faces，通常是三角形）组成的纯表面几何结构。它内部是空的。

从装满 TSDF 数据的体素阵列中变换成为Mesh用到了图形学中非常著名的算法：移动立方体（Marching Cubes）。Kimera 论文中也明确提到，它使用 Marching Cubes 算法从 TSDF 提取出网格。这个算法的工作原理是：遍历所有的体素，找到那些存着正数和负数交界的体素（说明这里穿过了表面，数值从正变负，一定经过了 0），然后在这些数值为 0 的位置上“打点”（生成顶点），并用三角形把这些点连起来。连完之后，一层 3D Mesh 外壳就被提取出来了。

变形图（Deformation Graph）：计算机图形学中的形状变形模型，通过”稀疏锚点+局部刚性变换”实现3D网格的全局调整，在保持局部形状不变的前提下修正全局几何误差。它是本文Mesh优化模块的核心技术。

Kimera-Multi将Mesh的优化与轨迹的优化完全解耦：先通过分布式鲁棒位姿图优化（D-GNC）得到全局一致的轨迹，再在每个机器人本地独立进行基于变形图的Mesh优化，无需机器人之间交换任何Mesh数据，极大降低了通信开销。变形图优化的目标是：用优化后的全局一致轨迹修正初始Mesh的几何误差，同时保持Mesh的局部刚性和语义标签不变。整个过程分为三步：构建变形图、求解变形图优化问题、插值得到完整Mesh的变形结果。

其求解的核心问题如下（论文公式11-13）：

① 符号定义（统一论文符号体系）

② 变形图优化目标函数（论文公式11）

变形图优化是一个带约束的最小二乘问题，同时优化关键帧位姿和简化Mesh顶点的变换：

三项约束的物理意义及与全局一致性的关联：

1. 锚定约束（第一项）：强制变形图中的关键帧位姿完全匹配D-GNC输出的全局一致位姿。当优化收敛时，$X_i = \overline{X}_i$，这是Mesh全局一致的核心前提。

2. 局部刚性约束（第二项）：保证Mesh的局部形状不被扭曲。它要求相邻Mesh顶点的相对位置在变形前后保持不变，避免出现Mesh破面、拉伸或断层。

3. 观测一致性约束（第三项）：保证”关键帧看到的Mesh顶点”与”优化后的Mesh顶点”位置一致。对于任意被关键帧i观测到的Mesh顶点l，收敛时满足：

   \[t_l^M = R_i^X \cdot \tilde{g}_{il} + t_i^X = \overline{R}_i \cdot \tilde{g}_{il} + \overline{t}_i\]

   这正好等于用全局一致的关键帧位姿重新计算得到的顶点坐标。

③ 完整Mesh的变形插值（论文公式12-13）

为了降低计算量，变形图仅使用简化后的Mesh顶点。优化完成后，通过加权插值将变形结果传播到原始Mesh的所有顶点：

其中权重\(s_j(v_i)\)采用二次衰减的距离权重函数，并归一化到和为1：

这里$d_{\max}$是原始顶点$v_i$到其第$k+1$个最近变形图节点的距离（本文取$k=4$）。由于所有变形图节点的变换都是全局一致的，插值得到的原始Mesh顶点也必然是全局一致的。

这两个公式解决了”如何把几千个稀疏控制点的优化结果，无损传播到几十万原始Mesh顶点”的核心问题，在计算效率和几何精度之间实现了平衡。

公式13：权重函数（插值的核心）

逐符号物理意义： | 符号 | 物理意义 | |——|———-| | $v_i$ | 待变形的原始Mesh顶点（窗帘的一根丝线） | | $g_j$ | 第$j$个变形图控制点的原始坐标（挂钩的初始位置） | | \(\left\|v_{i}-g_{j}\right\|\) | 原始顶点$v_i$到控制点$g_j$的欧氏距离 | | $d_{\max}$ | 原始顶点$v_i$到其第$k+1$个最近控制点的距离（$k=4$时为第5个最近点） | | \(s_j(v_i)\) | 控制点$g_j$对原始顶点$v_i$的影响权重 | | \(\sum_{j=1}^{m} s_{j}\left(v_{i}\right)=1\) | 所有权重归一化，保证变形后顶点不会出现整体偏移 |

核心设计逻辑：

这是计算机图形学中变形插值的标准函数，具备三个不可替代的优势：

1. 近大远小的物理直觉：距离越近的控制点对原始顶点的影响越大，距离越远影响越小，符合空间变形的自然规律；

2. 紧支撑性（计算效率的关键）：当$\left|v_{i}-g_{j}\right| \geq d_{\max}$时，$s_j(v_i)=0$，即只有最近的4个控制点会影响该原始顶点，更远的控制点完全不参与计算。这将原本$O(N \times m)$的计算量（$N$为原始顶点数，$m$为控制点总数）降至$O(N \times 4)$，是变形图能实时运行的核心原因；

3. 光滑性保证：二次函数是连续可导的，变形后的Mesh表面会保持光滑，不会出现棱角、断层或锯齿状失真。

代码实现细节：

论文中采用上述二次衰减函数，而Kimera-PGMO开源代码中默认使用计算更简便的高斯权重函数

二者变形效果几乎完全一致。论文默认取$k=4$是工程最优值：$k=3$时变形不够光滑，$k=5$时计算量增加25%但精度提升不足0.1%。

公式12：插值变形公式

逐符号物理意义：

先计算原始顶点相对于控制点的相对位置，再用控制点的新变换作用于该相对位置，最后加权平均所有控制点的贡献。

这是保持Mesh局部刚性的唯一方法：

• 如果直接加权平均控制点坐标，当控制点相互靠近时，中间的原始顶点会被压缩；当控制点相互远离时，原始顶点会被拉伸，最终导致Mesh严重扭曲、破面；
• 而”相对位置+变换”的方式，保证了原始顶点与控制点之间的相对位置永远不变，只是整体跟着控制点一起移动，完美保留了Mesh的原始几何形状。

Kimera-Multi的工程简化： 论文和代码中，变形图的所有控制点仅保留平移自由度，无旋转自由度，即$R_j^M = I$（单位矩阵）。因此公式12可简化为： \(\tilde{v}_{i}=\sum_{j=1}^{m} s_{j}\left(v_{i}\right)\left(v_{i}-g_{j}+t_{j}^{M}\right)\) 这是一个非常高效的工程取舍：SLAM中Mesh的漂移99%都是平移漂移，旋转漂移几乎可以忽略；引入旋转自由度会让每个控制点的优化变量从3个增至6个，计算量翻倍，但Mesh精度提升不足0.01m，得不偿失。

插值不会丢失Mesh细节。插值仅修改顶点的坐标，完全不改变Mesh的拓扑结构（顶点-边-面的连接关系）和语义标签，所有原始几何细节（墙角、台阶、凹凸）都会被完整保留。且由于公式13的紧支撑性，每个原始顶点仅受最近的4个控制点影响，更远的控制点权重为0，不参与计算。

核心总结：公式(12)和(13)通过”稀疏控制点优化+稠密加权插值”的范式，将百万级的Mesh全局优化问题降维到千级，在保持Mesh局部刚性和几何精度的前提下，实现了实时的全局一致性修正，是Kimera-Multi多机器人稠密语义SLAM系统的关键技术之一。

Mesh的一致性分析： Mesh的几何完全由生成它的关键帧位姿决定。只要所有关键帧的位姿是全局一致的，那么由这些关键帧生成的Mesh必然是全局一致的，其数学推导分为两步：

第一步：D-GNC优化后轨迹必然全局一致

1. 鲁棒初始化保证坐标系统一：通过GNC求解鲁棒的机器人间相对变换（论文公式4-5），并构建机器人级生成树，将所有机器人的局部轨迹统一到同一个全局坐标系下，且该过程不受70%以下外点回环的影响。

2. 全局目标函数保证收敛到一致解：所有机器人都在最小化同一个全局PGO目标函数（论文公式7），且RBCD求解器具有可证明的收敛性。每次迭代中，有回环的机器人会交换公共位姿信息，确保它们对公共位姿的估计完全一致；没有回环的机器人通过生成树的传递保持全局一致。

3. 鲁棒性保证外点不破坏一致性：D-GNC通过交替权重-变量更新，将所有外点回环的权重降至0，相当于将其从目标函数中完全移除，避免了错误回环导致的轨迹扭曲。

第二步：轨迹全局一致→Mesh必然全局一致

1. 初始Mesh的每个顶点坐标都是生成它的关键帧位姿的线性函数：$g_v = X_i \cdot c_v$（$c_v$是顶点在相机坐标系下的坐标）。若$X_i$全局一致，则理论上$g_v$也全局一致。

2. 变形图的锚定约束强制关键帧位姿完全匹配全局一致的轨迹，观测一致性约束强制Mesh顶点匹配关键帧位姿，局部刚性约束保证未观测顶点通过插值保持全局一致。

3. 变形图仅改变Mesh顶点的坐标，完全不改变Mesh的拓扑结构和语义标签属性，因此几何的全局一致性自然传递给语义的全局一致性。

总结：鲁棒初始化→所有机器人同坐标系→D-GNC 最小化全局目标函数→轨迹全局一致→变形图把 Mesh 顶点锚定到全局一致的关键帧上→Mesh 全局一致。

唯一例外情况：当机器人之间几乎没有回环（如论文Stata数据集，机器人2与其他机器人仅1个回环）、外点回环比例超过70%或关键帧过于稀疏时，DPGO轨迹可能无法完全全局一致，进而导致Mesh出现局部错位。此时可通过增加RBCD迭代次数提升轨迹精度，从而改善Mesh一致性。

通信策略的一点说明：DPGO的执行是根据leader是否接收到新measurement数据决定的，但同时也有一定的周期性，leader会轮番查询是否建立通信，以及是否有新回环，假如有4个机器人，1和2开始的时候在同一个cluster中，1为leader，3和4开始的时候在同一个cluster中，3为leader，那么1和2运行一段时间后后相遇，系统检测出回环，执行一次DPGO，34同理，如果1和2继续运动，并在一段时间后二次建立通信，此时虽然未检测出回环但是有了新观测也会开始执行新一轮的DPGO优化，如果系统运行时间足够长，每个机器人都能够通信一次并建立回环，那么最后每个机器人中都能维护一个全局坐标系下包含所有机器人轨迹的独立轨迹结果

这一点可以在代码上体现：

// dpgo_ros/src/PGOAgentROS.cpp:L108-125 (timerCallback)
if (mState == PGOAgentState::WAIT_FOR_DATA) {
  // 定期更新cluster
  updateCluster();
  
  // Leader约每10秒检查一次
  if (isLeader() && elapsed_time_since_reset > 10) {
    publishRequestPoseGraphCommand();  // ← 周期性查询
  }
}

但这里值得一提的是，kimera的实验章节有这样一句话：

In our experiments in this section and Section VII-C, we run Kimera-Multi in a setting, where robots are constantly in communication range

所以其实实验设置上并没有为难自己，机器人始终处于通信状态，也就没有我刚刚顾虑的中间加入机器人要动态初始化的问题了，但说实话这样的实验条件设置还是相对理想了。

3.2 SlideSLAM

在分布式优化问题建模上，SlideSLAM和Kimera-Multi的共同特点是二者都考虑了机器人位姿以及landmark位置，追求高度的全局一致性。所不同的是Kimera-Multi将位姿图优化和地图优化解耦成了两个阶段分别进行计算，而SlideSLAM则将机器人位姿与landmark作为状态变量放进了一个因子图中实现共同优化。

SlideSLAM的创新点在于抛弃了低维且庞大的landmark点特征，而是基于语义信息将landmark上升为更高维度的物体级特征，这就大大减少了通信所需要交互的信息，以及能够提高计算效率。毕竟不论是lidar还是RGBD相机，一帧里面的物体都是有限个数的，再多也不可能多过点特征的数量。并且物体级的特征只需要知道其形状label、尺寸和质心坐标，大概是一个7维向量，也不会带来过多的计算。

SlideSLAM一直在强调度量语义（Metric-Semantic）信息，实际上就是指他们为语义目标赋予了度量信息，并利用带有度量信息的物体级landmark构建了factor graph因子，系统框架以及因子图如下图所示：

其求解的核心问题为：

这个贝叶斯形式的公式（MAP，最大后验估计）实际上包含了相对里程计测量误差和语义 landmark 观测误差 。假设系统传感器噪声服从零均值高斯分布，该 MAP 问题即可等价转化为在因子图（Factor Graph）上的非线性最小二乘问题。

多机器人去中心化通信与信息状态更新：

SlideSLAM的关键创新在于通过时变通信拓扑和信息状态递推机制实现真正的去中心化协作。根据论文定义，通信拓扑由时变邻接矩阵 $C_t \in {0,1}^{K \times K}$ 描述，其中 $C_t$ 的第 $(i,j)$ 元素为1当且仅当时刻 $t$ 机器人 $i$ 和 $j$ 在通信范围内。

1. 通信拓扑与邻接关系

任意时刻 $t$，机器人 $k$ 的邻接集合定义为（公式5）：

这意味着机器人 $k$ 只能与其邻接集合 $ N_t^{(k)}$ 中的机器人进行信息交换。与依赖全局通信的中央式系统不同，SLIDE SLAM能够在间断性通信和动态拓扑条件下工作。对于任意两个机器人对 $(i,j)$，系统追踪其上次见面时间（公式6）：

这个时间戳对于增量式处理多机融合数据至关重要，确保不会重复融合相同的信息。

2. 信息状态的递推更新

每个机器人 $k$ 维护一个信息状态 $I_t^{(k)}$，这是该机器人所有已获取信息（既包括自己的、也包括从其他机器人收到的）的完整记录。信息状态通过以下递推关系更新（公式7）：

其中：

• 第一项 $\bigcup_{j \in [K] \setminus {k}} I_{L_t^{(k,j)}}$：包含机器人 $k$ 从其他所有机器人接收到的最新信息，时间戳为上次见面时间
• 第二项 $\bigcup_{s \in [t]} {h_s^{(k)}}$：机器人 $k$ 自身的所有相对里程计观测 $h_s^{(k)} = x_s^{(k)} \ominus x_{s-1}^{(k)}$
• 第三项 $\bigcup_{s \in [t]} {m_s^{(k)}}$：机器人 $k$ 自身的所有语义观测 $m_s^{(k)} = h(x_s^{(k)}, \mathcal{M}) + v_s^{(k)}$

这个递推关系确保了即使在网络分割的情况下，每个机器人仍能最大化利用已知的所有信息。

3. 去中心化消息交换机制

当通信链接建立时（即 $j \in N_t^{(k)}$），机器人 $i$ 向机器人 $j$ 发送的消息定义为（公式11）：

这个消息包含两个关键部分：

（A）信息状态 $I_t^{(i)}$（对应代码中的 poseMstPair[]）：

• 机器人 $i$ 的所有历史关键位姿序列：$x_i(0), x_i(1), \ldots, x_i(T)$（在 $\mathcal{F}_i$ 坐标系下）
• 相应的相对里程计序列：$h_i(0), h_i(1), \ldots, h_i(T)$，每项为SE(3)变换，大小56字节
• 每个关键位姿处的语义观测：立方体、圆柱体、椭球体等物体的检测，包含：
  • 物体的位置和姿态（通过Pose3表示）
  • 尺寸参数（Cuboid: 3维; Cylinder: 1维; Ellipsoid: 2维）
  • 语义标签（类别信息如car, tree, etc.）

（B）地图估计 $\hat{\mathcal{M}}_t^{(i)}$（对应代码中的 map_of_labelXYZ[] + interRobotTFs[]）：

• 地图库：所有识别出的物体的汇总，表示为7维向量 $[\text{label}, x, y, z, q_x, q_y, q_z]$，在机器人 $i$ 的坐标系 $\mathcal{F}_i$ 下
• 已知变换矩阵：机器人 $i$ 之前与其他机器人建立的所有变换关系 ${T_m^i : m \in \text{通过Loop Closure已关联的机器人}}$

4. 通信-融合的完整流程

当机器人1和机器人2相遇时，融合过程如下：

结合系统底层的工程实现，当两个机器人1和2融合数据时，仅在Robot 1（HOST机器人）中构建的联合因子图求解如下目标函数：

其中 $X_2’ = T_{2→1} \circ X_2$ 表示Robot 2的位姿已变换到Robot 1坐标系。

HOST机制下的因子图优化流程：

1. 初始化阶段：Robot 1和2各自在独立的本地坐标系 $\mathcal{F}_1$ 和 $\mathcal{F}_2$ 中建立本地因子图
2. 回环检测阶段：Place Recognition线程（独立运行）检测两个机器人间的回环，计算坐标系变换 $T_{2→1}$，存入共享数据库
3. 数据收集与变换阶段：Robot 1从数据库读取Robot 2的新数据（关键位姿、观测、相对里程计），通过 $T_{2→1}$ 变换到 $\mathcal{F}_1$
4. 图融合与优化阶段：仅在Robot 1中执行：
   • 将变换后的Robot 2状态变量（$X_2’$）、因子和观测添加到Robot 1的因子图
   • 调用 factorGraph_.solve() 进行联合优化

关键性质：联合因子图中的所有状态变量（Robot 1的位姿 $X_1$、Robot 2的变换后位姿 $X_2’$、共享地标 $\mathcal{M}$）都在Robot 1坐标系 $\mathcal{F}_1$ 中表示，优化产生的所有估计值都是在此坐标系下的最优解。

• Robot 1 的所有变量集合：

• Robot 2 的所有变量集合：

各项残差定义为：

（1）Robot 1的里程计残差：

这约束Robot 1相邻两个位姿的相对变换与里程计测量 $h_1(i)$ 的一致性。

（2）Robot 2的里程计残差：

关键点：这里的所有量都在Robot 1的坐标系 $\mathcal{F}_1$ 中：

• $X_2’(j)$ 是Robot 2在时刻 $j$ 的变换后位姿（通过 $T_{2→1}$ 从Robot 2坐标系变换到 $\mathcal{F}_1$）
• $h_2(j)$ 是Robot 1从Robot 2接收到的相对里程计测量（以se(3)欧拉角或四元数表示的相对变换）
• 这个残差在Robot 1的因子图中约束Robot 2相邻两个变换后位姿的相对变换与里程计测量的一致性
• 重要：Robot 2的所有里程计数据被直接视为Robot 1因子图中的观测约束，参与联合优化

（3）观测因子残差：

其中：

• $\hat{z}_{obj}$ 是检测到的物体观测（在各自传感器框架或变换后的坐标系中）
• $h_{obs}$ 是观测函数（predictive measurement model）
• 关键点：$\mathcal{M}$ 是共享的地标变量集合，存在于Robot 1的因子图中
  • 当两个机器人观测到同一个地标时，它们的观测都会对这个共享变量 $\mathcal{M}$ 添加约束
  • Robot 2的观测 $e_{obs}^{(2)}$ 使用变换后的位姿 $X_2’(j)$（在 $\mathcal{F}_1$ 中）
  • Cholesky求解器在Robot 1中自动通过信息加权融合（Schur补）计算共享地标的最优估计

（4）Loop Closure残差（关键部分，仅在HOST因子图中）：

其中：

• $X_1(i)$：Robot 1在时刻 $i$ 的优化估计位姿（在 $\mathcal{F}_1$ 坐标系中）
• $X_2’(j)$：Robot 2在时刻 $j$ 的优化估计位姿（已通过 $T_{2→1}$ 变换到 $\mathcal{F}_1$ 坐标系）
• $T_{measured}$：Place Recognition获得的 $T_{2→1}$ 变换（从Robot 2发现位置到Robot 1对应位置）
• $e_{LC}^{(k)}$ 是 $\mathbb{SE}(3)$ 切空间中的6维向量

物理含义：约束当前估计的两个机器人位姿变换（按Place Recognition的几何观测应该满足的关系）与实际Place Recognition测量值的一致性。

地标级约束：

这表示Robot 1和Robot 2对同一个地标的估计在变换后应该一致：

• $L_A^{(1)*}$ 是Robot 1对地标A位置的估计（在 $\mathcal{F}_1$ 坐标系）
• $T_{2→1} \cdot L_A^{(2)}$ 是Robot 2观测的同一地标（变换到 $\mathcal{F}_1$ 后）
• 这些约束确保了多机器人观测的地标在全局坐标系中的一致性

回环约束的 100 倍权重（HOST因子图中的强力耦合机制）：

在 SlideSLAM 的代码实现中，Loop Closure 并不是一种全新的数学残差形式，而是通过极大地缩小回环观测因子的协方差矩阵来在HOST机器人的因子图中实现强力耦合。代码中强行设置 $\Sigma_{LC} = 0.01 \Sigma_{odom}$。

在马氏距离（Mahalanobis distance） $|e|_{\Sigma}^2 = e^T \Sigma^{-1} e$ 的计算中，协方差缩小 100 倍，等价于在目标函数中为该残差项赋予了 100 倍的巨大权重。

Vector6 noise_model_pose_vec = Vector6::Ones() * 0.00001;  // 10^-5
noise_model_pose = noiseModel::Diagonal::Sigmas(noise_model_pose_vec);

// Loop Closure: 100倍更紧的约束（只在HOST因子图中）
noise_model_closure = noiseModel::Diagonal::Sigmas(noise_model_pose_vec * 0.01);
// = Diagonal::Sigmas([10^-7, 10^-7, 10^-7, 10^-7, 10^-7, 10^-7])

数学对应：

• 里程计噪声：$\Sigma_{odom} = diag(10^{-5}, …)$
• Loop Closure噪声（HOST图）：$\Sigma_{LC} = diag(10^{-7}, …) = 0.01 \times \Sigma_{odom}$

这意味着在HOST机器人的联合因子图损失函数中：

在HOST因子图中的约束权重 = 100倍

结论：

由于这个 100 倍的强力耦合权重，当两台机器人的局部地图发生语义关联（Place Recognition识别到同一个 landmark）时，HOST机器人的因子图求解器中的 Jacobian 矩阵会产生强烈的非对角块耦合。这相当于用一根”极其坚固的钢丝”（Loop Closure因子）把两个原本独立的”弹簧系统”（各自的里程计）连在了一起。求解器被强行打破原本独立的坐标系 $\mathcal{F}_1$ 和 $\mathcal{F}_2$ 之间的预期差异，将Robot 1的轨迹与Robot 2变换后的轨迹、以及共享地标 $\mathcal{M}$ “拉扯”并收敛到完全一致的几何构型——这一切都发生在HOST因子图的单次solve()调用中。

其实我个人觉得这个算法本身并不复杂，只不过他的loss function残差有三项都是语义级别的，即论文中的公式(13)(14)(15)，分别代表了长方体、圆柱体和椭球体的参数信息：

具体来说，这三种几何形体的误差项定义如下：

长方体因子 (Cuboid Factors)

对于长方体目标（例如车辆），其状态向量定义为 $l^g = [\mathbf{r}; \mathbf{t}; \mathbf{d}]$，其中 $\mathbf{r}$ 是旋转向量，$\mathbf{t}$ 是平移向量，$\mathbf{d}$ 是表示长、宽、高的尺寸向量。

在因子图优化中，我们需要计算长方体预期位姿和实际观测位姿之间的差异。长方体的误差函数定义如下：

公式解析：

• 这部分误差分为两块：位姿误差和尺寸误差。
• 位姿误差（上部）：$H_{cub}^w$ 是长方体在世界坐标系下的位姿，$H_w^s$ 是世界坐标系到传感器坐标系的变换（也就是机器人的位姿估计的逆），两者相乘得到长方体在传感器坐标系下的预期位姿。$H_{cub}^s(\mathbf{z})$ 则是传感器实际测量到的长方体位姿。通过求逆并相乘，计算出它们之间的相对变换。接着使用对数映射 ($\log$) 将 $\mathbb{SE}(3)$ 流形上的误差映射到李代数 $\mathfrak{se}(3)$ 空间，最后使用 vee 算子 ($^\vee$) 将其展开为 $6 \times 1$ 的向量。
• 尺寸误差（下部）：非常直观，就是地图中估计的长方体尺寸 $\mathbf{d}$ 与实际测量到的尺寸 $\mathbf{d}(\mathbf{z})$ 之间的向量差。

圆柱体因子 (Cylinder Factors)

对于圆柱体目标（例如树干或灯柱），由于在有限视野内通常无法观测到其完整高度，因此不估计高度。其状态向量定义为 $l^g = [\mathbf{b}; \mathbf{n}; r]$，其中 $\mathbf{b}$ 是轴线的原点，$\mathbf{n}$ 是轴线的方向向量，$r$ 是半径。

圆柱体的误差函数定义如下：

公式解析：

• 这里的误差分为三项：位置误差、方向误差和半径误差。
• 位置误差：将世界坐标系下的轴线原点 $\mathbf{b}$ 转换到传感器坐标系下（乘以旋转 $R_w^s$ 并加上平移 $\mathbf{t}_w^s$），然后与观测到的原点 $\mathbf{b}(\mathbf{z})$ 求差。
• 方向误差：将世界坐标系下的轴线方向 $\mathbf{n}$ 转换到传感器坐标系下，与观测到的方向 $\mathbf{n}(\mathbf{z})$ 求差。
• 半径误差：地图中估计的半径 $r$ 与观测到的半径 $r(\mathbf{z})$ 之间的标量差。

椭球体因子 (Ellipsoid Factors)

所有不能归类为长方体或圆柱体的不规则物体都用椭球体建模（例如椅子、桌子）。为了简化，模型固定了朝向并只保留两个形状参数。其状态向量为 $l^g = [\mathbf{c}, \mathbf{d}_e]^T$，其中 $\mathbf{c}$ 是三维质心坐标，$\mathbf{d}_e = [d_r, d_h]^T$ 代表半径和高度。

值得注意的是，椭球体的尺寸估计在后端的计算方式上有些不同：尺寸参数 $\mathbf{d}_e$ 是通过简单的滑动平均（moving average）来更新的，而没有放入全局最小二乘优化中。因此，因子图中的椭球体残差仅考虑其质心的观测误差。

将世界坐标系下的质心转换到机体坐标系后得到 $[c’_x, c’_y, c’_z]^T$。系统的观测模型采用了距离-方位角（range-bearing）的形式，质心测量的误差函数如下：

公式解析：

• $rg_{exp} = \sqrt{c_x’^2 + c_y’^2 + c_z’^2}$ 是预期的距离观测值。

• $\theta_{exp} = \tan^{-1}\left(\frac{c’y}{c’_x}\right)$ 和 $\phi{exp} = \tan^{-1}\left(\frac{c’_x}{\sqrt{c_x’^2 + c_y’^2}}\right)$ 分别是预期的两个方位角（仰角和偏航角）。
• 这三项分别减去实际测量到的距离 $rg(\mathbf{z})$ 和角度 $\theta(\mathbf{z}), \phi(\mathbf{z})$，构成了椭球体质心的误差向量。该模型直接复用了 GTSAM 中现成的 range-bearing 测量模型。

总结来说，SlideSLAM 通过这三个精巧的误差函数，成功地将复杂的语义物体转化为因子图优化中可求解的数学约束。这种做法极大地压缩了地图表示的维度，是其能够实现实时、轻量级、去中心化多机器人协作的关键所在。

Robot 1和Robot 2相遇时，持续增量地互相发送地标信息。两者各自执行Place Recognition/Loop Closure检测，调用相同的回环检测算法，使用网格搜索+数据关联检测地标对应关系，基于SVD计算变换矩阵 $T_{2→1}$（从Robot 2坐标系到Robot 1坐标系）。

关键：联合因子图仅在HOST机器人（Robot 1）中构建和优化

Place Recognition检测到回环后，系统采用集中式优化机制：

1. 数据变换：Robot 2的所有关键位姿、相对里程计、观测数据通过 $T_{2→1}$ 变换到Robot 1坐标系 $\mathcal{F}_1$
2. 图融合：Robot 2变换后的所有状态变量（位姿 $X_2’$、观测）和因子被添加到Robot 1的因子图中
3. 联合优化：仅在Robot 1执行一次 factorGraph_.solve()，进行所有状态变量（$X_1, X_2’, \mathcal{M}$）的联合优化
4. 结果：100×权重的Loop Closure约束强制所有地标满足一致性：

其中 $L_A^{1*}$ 是Robot 1的优化后地标估计，$L_A^{(2)}$ 是Robot 2观测的同一地标。

关于增量式优化（iSAM2）：

当Robot 1和Robot 2第一次相遇并完成联合优化后，ISAM2求解器在Robot 1中产生联合最优解 ${X_1^, X_2’^, \mathcal{M}^*}$。当两个机器人第二次相遇时，新增的观测数据只需被添加到Robot 1的因子图中。ISAM2增量式求解的本质在于：自动识别Bayes树中受新因子影响的部分，有选择性地重新优化，而不是修改历史的信息。因此不需要从头重新优化之前的所有数据，只在新数据周围进行局部重线性化和改进。

3.3 Swarm-LIO2

Swarm-LIO2的系统建模则相对来说有所不同，因为其更多追求的是自身状态估计跑的怎么样，而不追求全局的一致性，也不构建全局的地图。

Swarm-LIO2是一个基于滤波的框架，其状态向量为：

去中心化集群状态估计 (DECENTRALIZED SWARM STATE ESTIMATION)

Swarm-LIO2 的核心任务是在每个 AAV（无人机）上实现全去中心化的状态估计，包括互状态估计、基于 ESIKF 的自身状态估计以及全局外参的在线细化。

互状态估计 (Mutual State Estimation)

集群中每个 AAV $i$ 的任务之一是估计其他队友 $j$ 在 $i$ 的全局坐标系中的状态 $G_i x_{b_j}$。为了降低计算复杂度，Swarm-LIO2 仅在每个 AAV 上估计自身状态 $G_i \bar{x}_{b_i}$ 并通过网络交换。

无人机 $i$ 通过接收到的无人机 $j$ 的自身估计状态 \(G_j \tilde{T}_{b_j}\)，利用全局外参 \(^{G_i}T_{G_j}\) 投影得到队友的状态估计值：

其中，$^{G_j}\tilde{T}_{b_j}$ 是无人机 $j$ 发送给 $i$ 的自身位姿估计。由于初始外参可能存在误差，系统会在状态向量中追加外参状态并在 ESIKF 中不断细化。

状态与协方差预测 (State and Covariance Prediction)

在第 $k$ 个 LiDAR 帧期间，系统利用 IMU 测量进行预测。

离散状态转移模型

状态预测遵循以下离散转移模型： \(x_{i,\tau+1} = x_{i,\tau} \oplus \left( \Delta t_{\tau} f_i(x_{i,\tau}, u_{i,\tau}, w_{i,\tau}) \right) \tag{4}\)

状态向量定义

对于无人机 $i$，状态向量 $x_i$、过程噪声 $w_i$ 和输入 $u_i$ 定义如下：

其中，$\mathcal{M}$ 是维度为 $18 + 6(N-1)$ 的流形空间。

转移函数 $f_i$：

离散状态转移函数 $f_i$ 具体展开为：

\(f_{i} \triangleq \begin{bmatrix} \omega_{m}-b_{g}-n_{g} \\ ^{G_{i}}v_{b_{i}} + \frac{1}{2}(^{G_{i}}R_{b_{i}}(a_{m_{i}}-b_{a_{i}}-n_{a_{i}})+^{G_{i}}g)\Delta t_{\tau} \\ ^{G_{i}}R_{b_{i}}(a_{m_{i}}-b_{a_{i}}-n_{a_{i}})+^{G_{i}}g \\ n_{b_{gi}} \\ n_{b_{ai}} \\ 0_{3\times1} \\ \vdots \\ 0_{3\times1} \\ 0_{3\times1} \end{bmatrix} \tag{6}\) 注：公式中最后三项 $0$ 对应外参状态的导数，表示预测阶段外参保持不变。

误差状态迭代状态更新 (Error State Iterative State Update)

当接收到新的 LiDAR 扫描或互观测数据时，在扫描结束时刻 $t_{i,k}$ 执行更新。

测量建模 (Modeling of Measurements)

通用测量模型为： \(y_k = h(x_k, v_k) \tag{8}\)

(a) LiDAR 点云测量

对于第 $n$ 个去畸变的点 $^{b_i}p_n$，其到平面的投影距离模型为：

(b) 主动互观测 (Active Mutual Observation)

无人机 $i$ 观测到队友 $j$ 的质心点 $^{b_i}\tilde{p}{b_j}$。利用 $j$ 传来的位置估计值 $^{G_j}\tilde{p}{b_j}$，建模如下：

(c) 被动互观测 (Passive Mutual Observation)

无人机 $i$ 接收到来自队友 $j$ 对自己的观测数据 $^{b_j}\tilde{p}_{b_i}$： \(b_j \tilde{p}_{b_i} = \underbrace{((^{G_{j}}\tilde{T}_{b_{j}} \oplus n_{T_{j}})^{-1}{}^{G_{i}}T_{G_{j}}^{-1}) \circ ^{G_{i}}p_{b_{i}} + n_{po,ij}}_{h_{po,ij}(x_i, n_{T_j}, n_{po,ij})} \tag{13}\)

(d) 统一测量方程

综合所有观测，整体测量向量 $y$、函数 $h$ 和噪声 $v$ 为： \(\begin{aligned} y &= [0, \dots, ^{b_i}\tilde{p}_{b_j}^T, \dots, ^{b_j}p_{b_i}^T, \dots]^T \\ h &= [\dots, h_{p,n}^T, \dots, h_{ao,ij}^T, \dots, h_{po,ij}^T, \dots]^T \\ v &= [\dots, n_{p,n}^T, \dots, n_{p_j}^T, n_{ao,ij}^T, \dots, n_{T_j}^T, n_{po,ij}^T, \dots]^T \end{aligned} \tag{14}\)

互观测的时间补偿 (Temporal Compensation)

由于异步和传输延迟，必须补偿时间偏差 $^i\tau_j$。

• 主动观测补偿：将 $j$ 的位置补偿到 $i$ 的系统时间 $t_{i,k}$： \(^{G_{j}}\tilde{p}_{b_{j}}^{comp} = ^{G_{j}}\tilde{p}_{b_{j}} + ^{G_{j}}\tilde{v}_{b_{j}}(t_{i,k} - t_{j,k} + ^{i}\tau_{j}) \tag{15}\) 代入 (11) 得到补偿后的模型 (16)。

• 被动观测补偿：将自身位置 $G_i p_{b_i}$ 补偿到队友 $j$ 的系统时间 $t_{j,k}$： \(^{G_{i}}p_{b_{i}}^{comp} = ^{G_{i}}p_{b_{i}} + ^{G_{i}}v_{b_{i}}(t_{j,k} - t_{i,k} - ^{i}\tau_{j}) \tag{17}\) 代入 (13) 得到补偿后的模型 (18)。

状态更新

最后，利用迭代卡尔曼滤波器（ESIKF）反复计算增益并更新状态，直到收敛。此过程耦合了 IMU、点云和互观测，保证了在 LiDAR 退化场景下的厘米级精度。

ESIKF 预测步与更新步公式推导细节

在 Swarm-LIO2 的紧耦合框架中，误差状态迭代卡尔曼滤波（ESIKF）是核心的状态估计算法。以下结合手写推导笔记，对流形上的误差状态运动学、雅可比矩阵构建以及基于最大后验估计（MAP）的迭代更新进行详细解析。

1. 预测步：误差状态与运动学传播

• 误差状态定义：系统定义了包含自身状态以及多机全局外参的误差状态向量 $\tilde{x}$。
• 旋转误差的推导：对于 $SO(3)$ 流形上的旋转，系统采用了右乘扰动 (Right Perturbation) 模型，即 $R = \hat{R}Exp(\delta\theta)$。
• 状态转移方程线性化：由于非线性函数的存在，需要对误差状态的传递进行线性化。在推导离散时间下的旋转误差 $\delta\theta_{k+1}$ 时，引入了李群的伴随性质 $Exp(\phi)R = R Exp(R^T \phi)$，并利用 BCH (Baker-Campbell-Hausdorff) 公式的线性近似将其展开。
• 位置与速度的传播：在得到旋转误差模型后，将含有扰动的状态代入原始运动学方程中，通过忽略二阶极小项（如 $\delta\theta \times \delta\theta$），推导出位置 $\delta p$ 的一阶误差传播关系。

2. 预测步雅可比与协方差传播

• 雅可比矩阵构建：同理推导出速度 $\delta v$、零偏 $\delta b_g, \delta b_a$ 及重力 $\delta g$ 的误差传播后，将所有状态变量对误差变量的偏导数进行汇总，组装成高维的状态转移雅可比矩阵 $F_x$ 与噪声雅可比矩阵 $F_w$。
• 协方差预测：基于线性化后的误差传递模型推导出的经典先验协方差预测公式为：$P_{k+1} = F_x P_k F_x^T + F_w Q F_w^T$。

3. 更新步：基于 MAP 的目标函数构造

• 最大后验估计：更新步并未直接套用经典的卡尔曼增益公式，而是从 MAP (Maximum A Posteriori) 视角出发。系统通过构建一个联合代价函数 $C(\delta x^k)$，将基于运动学方程预测得来的先验残差，与基于 LiDAR 和多机通讯获取的测量残差进行联合最小化优化。

4. 更新步：状态求解与等价性证明

• 观测模型：代价函数中的观测模型对应三个部分：LiDAR 点对面的几何约束（公式 9）、系统预测自身位置与主动观测点云的残差（公式 11），以及系统预测队友位置与被动观测的残差（公式 13）。

• 二次型求导：将观测方程在当前迭代点处进行一阶泰勒展开，将非线性优化问题转化为线性最小二乘问题。对该二次型代价函数关于误差状态求导并令其等于零

，解出高斯-牛顿法中的最优状态增量：

• 卡尔曼增益等价性：通过应用矩阵求逆引理 (Matrix Inversion Lemma) 对上述优化求解结果进行恒等变换，可以严格证明：基于迭代优化的解法在数学上与经典的卡尔曼增益 \(K = P H^T (H P H^T + R)^{-1}\) 形式完全等价。即

这种等价性证明了 ESIKF 在融合复杂非线性观测时的严谨性与有效性。