Kimera-Multi (T-RO)

Posted on Edited on In Waline:

对论文 Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems (IEEE T-OR, 2022) 的阅读整理。

Kimera-Multi (T-RO)

论文情况

  • 标题：Kimera-Multi: Robust, Distributed, Dense Metric-Semantic SLAM for Multi-Robot Systems
  • 作者：Yulun Tian , Yun Chang , Fernando Herrera Arias , Carlos Nieto-Granda , Jonathan P. How and Luca Carlone
  • 期刊：IEEE Transaction on Robotics, 2022
  • 源码：未开源

1 Introduction

多机器人 SLAM 的前景：

  • 工厂自动化、搜救、智能运输、行星探索、军用活动和民用活动的监视。

metric-semantic：

  • 在估计场景三维几何结构的同时，预测目标和场景结构的语义标签。

本文工作：

  • 所开发的系统完全分布式，能够在通信带宽的约束下实时估计密集 metric-semantic 3D 网格模型。3D 网格捕捉了机器人操作环境的完整而密集的几何图形。
  • 通过使用人类可理解的语义标签（如建筑、道路和物体）对网格进行注释，系统提供了环境的高级抽象。

单机器人 V.S. 多机器人：

  • 单机器人：SLAM++、SemanticFusion、Kimera、Voxblox++。
  • 多机器人：
    • 已有的一些系统依赖于稀疏 landmark。虽然这些系统适用于协同定位，但是无法提供完全的稠密建图方法。
    • 一些系统适用语义信息改进协同 SLAM，但结果仍然很稀疏。使用稠密的语义信息，但是方法局限在局部地图的匹配。
    • 由于多机器人缺乏共同参考坐标系和里程计框架，容易导致数据关联错误。现有的一些系统严重依赖于初始化或启发式搜索。

本文贡献：

  • 提出 Kimera-Multi，多个机器人可以实时协同构建环境的 3D 网格图：

    • 每个机器人执行 Kimera 处理视觉惯性传感器数据，获取局部轨迹和 3D 网格估计；
    • 当机器人之间可以通信时，触发分布式处理，执行机器人间位置识别，相对位姿估计，分布轨迹估计。
    • 根据联合优化的轨迹估计，每个机器人实时进行局部网格修正以纠正局部地图漂移。

    Fig.1 为 3 个机器人使用 Kimera-Multi 协同 SLAM 的结果。

  • 提出了新的对离群值具有鲁棒性的两阶段分布式 PGO 方法：

    • 第一阶段：使用 GNC[1] 在全局坐标系初始机器人的局部轨迹，以此估计机器人之间的坐标系相对变换。
    • 第二阶段：使用构建在 Riemannian block-coodinate descent（RBCD）[2] 求解器的 GNC 的分布式拓展，求解 PGO 问题。

  • 拓展的实验评估：

    • 在大规模图像数据集和 SLAM benchmark 数据集上进行定量评估。演示了 Kimera-Multi 在自动地面机器人收集的具有挑战性的真实环境数据集上的应用。

2 System Overview

对于 Kimera-Muliti，每个机器人执行完全区中心的 metric-semantic SLAM，系统结构如 Fig.2，其一共包含 4 个主要模块：

  • （1）局部（单机器人）Kimera：

    每个机器人本地运行 Kimera 进行实时局部轨迹和网格计算：

    • Kimera-VIO[3] 作为 VIO 模块。
    • Kimera-Semantics[3] 处理深度图、2D 语义分割并使用 VIO 的姿态估计生成密集 metric-semantic 3D 网格。
    • Kimera-VIO 使用 ORB 和 DBoW 计算每个关键帧的 BoW 表示，用于分布回环检测。

  • （2）分布式回环检测：

    当机器人 α\alpha 和 β\beta 在通信范围内时，执行回环检测，机器人交换关键帧的 BoW 描述子。当检测到匹配的描述子时，使用标准 GV（Geometry Vision）技术估计相对姿态。

  • （3）使用 PGO 的鲁棒分布式轨迹估计：

    使用 Kimera-VIO 的里程测量和所有检测到的回环检测，通过 PGO 计算所有机器人在全局坐标系的最优轨迹。

    • 先用鲁棒的初始化计算机器人坐标系间粗略的相对变换；
    • 然后使用鲁棒的 GNC 分布式拓展选择回环检测并计算最优轨迹估计。

  • （4）局部网格优化（LMO）：

    执行局部处理，对每个机器人的网格进行变换，确保与分布式 PGO 得到的轨迹估计一致。

模块（2）和（3）需要使用机器人通信，Fig.3 为模块的数据流。

实现细节：

  • C++，ROS（机器人间、模块间的信息交换）

3 Distributed Loop Closure Detection

如 Fig.3，当能够进行交互时，机器人 α\alpha 通过向机器人 β\beta 发送自上次交会以来的新关键帧的全局描述子，初始化分布式回环检测。使用 DBoW2 库将这些描述子表示为 BoW 向量。

当接收到 BoW 向量，β\beta 在本地关键帧里寻找视觉相似度分数超过阈值（≥0.1\ge 0.1）的候选匹配帧。当检测到回环时，使用标准 GV 估计两个匹配关键帧的相对变换。实验中：

  • β\beta 先向 α\alpha 请求匹配关键帧的 3D 特征点和对应描述子；
  • β\beta 使用 OpenCV 的最近邻搜索，通过匹配两个特征描述子集合，计算假定（putative）关系。通过假定关系，β\beta 使用 RANSAC 结合五点法[4] 和三点法[5] 计算相对变换。在 OpenGV 中提供了两个方法的实现。
  • 如果 GV 收到 5 个以上的假定关系，则接受回环并且发给分布式轨迹估计模块。

4 Robust Distributed Trajectory Estimation

机器人计算轨迹时，通过使用整个机器人团队的里程测量、机器人内、机器人间的回环，协同解决 PGO 问题得到。一些回环检测可能是 outliers（感知偏差导致），因此需要 PGO 对 outliers 鲁棒：

  • 前期版本的方法使用 PCM 结合最大团计算实现轨迹估计，然而即使是并行计算也要较长的计算时间，因此提出了改进方法。

本文提出了基于 GNC 的鲁棒轨迹估计算法。GNC 的主要思想是从损失函数的凸近似开始，逐步引入非凸性，防止收敛到局部解。

一般来说，GNC 可以不需要初始猜测，但据观察，在出现异常值时，3D SLAM 的全局求解会变得缓慢。因此，后续一些工作中，GNC 的每次迭代都执行局部优化（从无 outlier 的初始猜测开始），此方法被证明是非常有效的。

在单机器人 SLAM 中，可以通过将里程测量结果连接在一起，获得无 outlier 的初始猜测。在多机器人的情况下，不同机器人的姿态之间不存在里程测量，因此挑战变成了建立一个对异常值不敏感的初始猜测。

本文提出的 D-GNC 包含了两个阶段：

  • 第一阶段：使用对异常值鲁棒以及有效通信的方法在全局坐标系中初始化机器人的轨迹。
  • 第二阶段：开发完全分布的算法来执行 GNC，使用 RBCD 作为分布求解的子程序块。

4.1 Background: GNC

传统 M 估计面临的一个挑战是：

  • 最小二乘所使用到的鲁棒损失函数 ρ\rho 可能高度非凸，使得局部搜索变得对初始值敏感。

GNC 的思想是优化一系列更简单的（非凸性较小的）替代损失函数，这些函数能逐步收敛到原始的鲁棒损失函数。每个替代函数使用与传统的 M 估计相同的表示形式：

min⁡x∈X∑iρμ(ri(x))(1)\min_{x \in \mathcal{X}} \sum_i \rho_{\mu} (r_i (x)) \tag{1}

其中 ri:X→Rr_i: \mathcal{X} \rightarrow \mathbb{R} 为与第 ii 个测量值有关的残差。替代函数 ρμ\rho_{\mu} 的序列，被控制参数 μ\mu 参数化，使得满足对于给定的常量 μ0,μ1\mu_0, \mu_1：

  • （1）当 μ→μ0\mu \rightarrow \mu_0，ρμ\rho_{\mu} 为凸函数；
  • （2）当 μ→μ1\mu \rightarrow \mu_1，ρμ\rho_{\mu} 收敛到原始（非凸）鲁棒损失函数 ρ\rho。

实际中，在 μ0\mu_0 附近初始化 μ\mu，并在后续迭代过程中将其值更新至 μ1\mu_1 附近。

对于式 (1) 中的每一个函数，GNC 使用 Black-Rangarajan 二元性[6] 进行重构，即在确定的技术条件下（满足所有常见的损失函数），(1) 等价于如下的优化问题：

min⁡x∈X,wi∈[0,1]∑i[wiri2(x)+Φρ u(wi)](2)\min_{x \in \mathcal{X}, w_i \in [0, 1]} \sum_i [w_i r_i^2 (x) + \Phi_{\rho_{\,u}}(w_i)] \tag{2}

wi∈[0,1]w_i \in [0, 1] 是与第 ii 个测量相关联的权重。在 (2) 中，离群处理 Φρ u(wi)\Phi_{\rho_{\,u}}(w_i) 对 wiw_i 进行惩罚，其表达依赖于损失函数 ρ\rho 和控制参数 μ\mu 的选择。类似于传统 IRLS 问题，GNC 优化 (2) 中 x,wix, w_i 的同时更新控制参数 μ\mu：

  1. 变量 xx 更新：用固定权重 wiw_i 更新变量 xx。相当于求解标准的加权最小二乘问题：

    x∗=argmin⁡x∈X∑iwiri2(x)(3)x^* = \underset{x \in \mathcal{X}}{\operatorname{argmin}} \sum_i w_i r_i^2 (x) \tag{3}

  2. 权重 wiw_i 更新：用固定变量 xx 更新权重 wiw_i。每个 wiw_i 对应的更新都有一个闭合表达式，该表达式依赖于当前的鲁棒替代函数 ρμ\rho_{\mu}。

  3. 参数 μ\mu 更新：用一个常数因子更新 μ\mu 使其接近 μ1\mu_1。

μ\mu 初始化为接近于 μ0\mu_0。在没有更好的猜测的情况下，所有的权重都初始化为 1。然后，重复上述步骤，直到 μ\mu 接近 μ1\mu_1。

4.2 Robust Distributed Initialization

为优化位姿图，首先需要初始化所有机器人在共享坐标系（全局坐标系）的位姿。每个机器人可以通过连接里程测量在其局部参考系中初始化轨迹。然而，为了在全局参照系中表示这些局部初始猜测，必须估计局部参考系之间的相对位姿。

4.2.1 Pairwise Coordinate Frame Estimation

考虑机器人 α\alpha 和 β\beta 以及对应的局部坐标系 AA 和 BB。考虑 α\alpha 的第 ii 个位姿和 β\beta 的第 jj 个位姿构成的回环，记为 X~βjαi∈SE(3)\tilde{\bold{X}}_{\beta_j}^{\alpha_i} \in SE(3)。记在机器人的局部坐标系下，前述两个位姿的里程测量为 X^αiA,X^βiB∈SE(3)\hat{\bold{X}}_{\alpha_i}^A, \hat{\bold{X}}_{\beta_i}^B \in SE(3)。结合回环，可以得到带噪声的从 AA 到 BB 的相对位姿变换：

X^BijA≜X^αiAX~βjαi(X^βiB)−1(4)\hat{\bold{X}}_{B_{ij}}^A \triangleq \hat{\bold{X}}_{\alpha_i}^A \tilde{\bold{X}}_{\beta_j}^{\alpha_i} (\hat{\bold{X}}_{\beta_i}^B)^{-1} \tag{4}

其中 X^BijA\hat{\bold{X}}_{B_{ij}}^A 表示这个估计是使用回环 (i,j)(i,j) 得到。

从 (4) 会发现，每一个机器人间回环都提供了一个参考系 AA 和 BB 的可选对齐。此外，由 inlier 回环提供的可选对齐应该是一致的（无偏性），如 Fig.4 所示。

为获取真实相对变换的可靠估计，构建并求解下鲁棒的位姿平均问题：

X^BA=argmin⁡X∈SE(3)∑(i,j)∈Lα,βρ(rij(X))(5)\hat{\bold{X}}_B^A = \underset{\bold{X} \in SE(3)}{\operatorname{argmin}} \sum_{(i,j) \in L_{\alpha, \beta}} \rho(r_{ij}(\bold{X})) \tag{5}

其中 ρ:R→R\rho: \mathbb{R} \rightarrow \mathbb{R} 为截断最小二乘（TLS）损失函数，Lα,βL_{\alpha, \beta} 为机器人 α\alpha 和 β\beta 之间的回环集。每个残差测量了要计算的平均位姿 X\bold{X} 和测量 X^BijA\hat{\bold{X}}_{B_{ij}}^A 之间的测地距离（Geodesic Distance）：

rij(X)≜∥X⊟X^BijA∥Σ(6)r_{ij}(\bold{X}) \triangleq \left\| \bold{X} \boxminus \hat{\bold{X}}_{B_{ij}}^A \right\|_{\Sigma} \tag{6}

其中 Σ∈S++6\Sigma \in \mathbb{S}^6_{++} 为固定的协方差。实验中，使用旋转标准差为 0.1rad，平移标准差为 0.5m 的对角协方差。对于给定的一对机器人，一个机器人可以在本地使用 GNC 完成计算而不需要额外的通信，并将解发给另一个机器人。使用 GTSAM 中提供的 GNC，其在求解 (5) 时使用 LM 对每个 GNC 变量进行更新。

4.2.2 Multi-robot Coordinate Frame Estimation

上述程序可以在机器人之间重复执行，在一个全局坐标系中表达所有局部参考帧（和轨迹估计）。

为此，如 Fig.4，首先在机器人依赖图中选择任意生成树，顶点对应机器人，边表示两个对应机器人之间至少存在一个机器人间回环。生成树产生了任意两个机器人之间的唯一路径。

不失一般性，选择任意机器人 α\alpha 并以其局部坐标系 AA 作为全局参考系。对于每一个非 α\alpha 机器人 β\beta，需要获取其相对全局坐标系的相对变换 X^BA∈SE(3)\hat{\bold{X}}^A_B \in SE(3)。这可以通过遍历生成树上从 α\alpha 到 β\beta 的唯一路径，组合路径上的相对变化即可得到。实际中，这可以通过局部通信增长以 α\alpha 所构建的生成树，达到完全分布式的方式。

4.3 Robust Distributed PGO

初始化完成后，机器人执行鲁棒分布式 PGO 来获取优化的轨迹估计和需要拒绝的回环 outliers。设 Xαi=(Rαi,tαi)∈SE(3)\bold{X}_{\alpha_i} = (\bold{R}_{\alpha_i}, \bold{t}_{\alpha_i}) \in SE(3) 表示在全局坐标系下 αi\alpha_i 的第 ii 个位姿。目标是使用所有的里程测量和回环优化所有的位姿变量：

min⁡Xαi∈SE(3)∀α∈R,∀i∑α∈R∑i=1nα−1rαi(Xαi,Xαi+1)2⏟odometry+∑(αi,βj)∈Lρ(rβjαi(Xαi,Xβj))⏟loop clousures(7)\begin{aligned} \underset{\forall \alpha \in \mathcal{R}, \forall i}{\min_{\bold{X}_{\alpha_i} \in SE(3)}} \underbrace{\sum_{\alpha \in \mathcal{R}} \sum_{i=1}^{n_{\alpha} - 1} r_{\alpha_i} (\bold{X}_{\alpha_i}, \bold{X}_{\alpha_{i+1}})^2}_{\text{odometry}} + \underbrace{\sum_{(\alpha_i, \beta_j) \in L} \rho \left( r_{\beta_j}^{\alpha_i} (\bold{X}_{\alpha_i}, \bold{X}_{\beta_j}) \right)}_{\text{loop clousures}} \end{aligned} \tag{7}

其中 R={α,β,...}\mathcal{R} = \{ \alpha, \beta, ... \} 为机器人集合，nαn_\alpha 为 α\alpha 的总位姿数，LL 包含机器人内和机器人间的回环。(7) 中的每个残差对应于全局位姿图中的单个相对位姿测量，其中残差使用弦距（chordal distance）。例如回环残差定义为：

rβjαi(Xαi,Xβj)≜(wR∥Rβj−RαiR~βjαi∥F2+wt∥tβj−tαi−Rαit~βjαi∥22)12(8)r_{\beta_j}^{\alpha_i} (\bold{X}_{\alpha_i}, \bold{X}_{\beta_j}) \triangleq \left( w_R \left\| \bold{R}_{\beta_j} - \bold{R}_{\alpha_i} \tilde{\bold{R}}_{\beta_j}^{\alpha_i} \right\|^2_F + w_t \left\| \bold{t}_{\beta_j} - \bold{t}_{\alpha_i} - \bold{R}_{\alpha_i} \tilde{\bold{t}}_{\beta_j}^{\alpha_i} \right\|^2_2 \right)^{\frac{1}{2}} \tag{8}

其中 X~βjαi=(R~βjαi,t~βjαi)∈SE(3)\tilde{\bold{X}}_{\beta_j}^{\alpha_i} = (\tilde{\bold{R}}_{\beta_j}^{\alpha_i}, \tilde{\bold{t}}_{\beta_j}^{\alpha_i}) \in SE(3) 为带噪声的位姿变换，wR,wt>0w_R, w_t > 0。对于里程测量使用标准二次代价函数（quadratic cost）。回环部分的 ρ\rho 选用与 4.2 中相同的 TLS 函数。

为求解 (7)，使用了分布式 GNC。4.1 中介绍了 GNC 的变量更新和权重更新，这里将详细介绍更新过程。

4.3.1 Variable Update

变量更新时，步骤成为标准（加权）PGO：

min⁡Xαi∈SE(3)∀α∈R,∀i∑α∈R∑i=1nα−1rαi(Xαi,Xαi+1)2+∑(αi,βj)∈Lwβjαi⋅rβjαi(Xαi,Xβj)2(9)\begin{aligned} \underset{\forall \alpha \in \mathcal{R}, \forall i}{\min_{\bold{X}_{\alpha_i} \in SE(3)}} \sum_{\alpha \in \mathcal{R}} \sum_{i=1}^{n_{\alpha} - 1} r_{\alpha_i} (\bold{X}_{\alpha_i}, \bold{X}_{\alpha_{i+1}})^2 + \sum_{(\alpha_i, \beta_j) \in L} w_{\beta_j}^{\alpha_i} \cdot r_{\beta_j}^{\alpha_i} (\bold{X}_{\alpha_i}, \bold{X}_{\beta_j})^2 \end{aligned} \tag{9}

对比 (7)，损失函数 ρ\rho 被加权平方误差替代，参考 (3)。使用 RBCD 作为分布式优化的求解器（Algorithm 1, Line 4）。

RBCD 作用于 (9) 的秩限制松弛（rank-restricted relaxation），随后将解投射到特殊的特殊欧式群。在实现中，将默认的秩松弛设置为 5。RBCD 为完全分布式的算法，每个机器人 α∈R\alpha \in \mathcal{R} 负责估计自己的轨迹 Xα≜{Xαi,i=1,...,nα}\bold{X}_\alpha \triangleq \{ \bold{X}_{\alpha_i}, i=1,...,n_\alpha \}。在执行过程中，机器人通过与邻居的部分信息交互来更新轨迹：

  • α\alpha 更新轨迹的每次迭代时，需要与邻居交换信息（回环）。
  • α\alpha 只需要接收邻居机器人的“public poses”。

在原始（中心化）GNC 算法中，每个变量更新步骤都使用全局求解器或局部搜索技术求解至完全收敛。在分布式方法中，使 (9) 完全收敛很慢，为此，放松收敛要求并允许更新过程使用近似解。具体地说，只在固定数量（15）的迭代过程基于当前权重使用 RBCD 增强轨迹估计。

4.3.2 Weight Update

原始 GNC 中，每个使用 TLS 的残差函数权重更新仅依赖于当前残差 r^i\hat{r}_i、控制参数 μ\mu 和 TLS 损失的阈值 cˉ\bar{c}：

wi←{0,if r^i2∈[μ+1μcˉ2,+∞]cˉr^iμ(μ+1)−μ,if r^i2∈[μμ+1cˉ2,μ+1μcˉ2]1,if r^i2∈[0,μμ+1cˉ2](10)w_i \leftarrow \begin{cases} 0, &\text{if } \hat{r}_i^2 \in [\frac{\mu+1}{\mu} \bar{c}^2, +\infin] \\ \frac{\bar{c}}{\hat{r}_i} \sqrt{\mu(\mu+1)} - \mu, &\text{if } \hat{r}_i^2 \in [\frac{\mu}{\mu+1} \bar{c}^2, \frac{\mu+1}{\mu} \bar{c}^2] \\ 1, &\text{if } \hat{r}_i^2 \in [0, \frac{\mu}{\mu+1} \bar{c}^2] \end{cases} \tag{10}

(10) 可以独立且并行的计算（Algorithm 1, Line 5）。

具体地说，每个机器人首先并行地更新与机器人内回环相关的权重。然后，对于每个机器人间回环，两个机器人中的一个计算更新的权重，然后将新的权重传递给另一个机器人。在权重更新阶段之后，每个机器人也更新其控制参数 μ\mu 的本地副本（Algorithm 1, Line 6）。

5 Local Mesh Optimization

Kimera-Semantic 从 Kimera-VIO 估计中构建 3D 网格。前述分布式 PGO 可以提供更精确的位姿，因此可以用于网格构建。此处提出一种基于变形图（deformation graoh）的网格优化方法。

  • 变形图为计算机图形学的一个模型，其变形给定网格，以便将网格中的点锚定到用户定义的位置，同时确保网格保持局部刚性。

5.1 Creating the Deformation Graph

如 Fig.5，创建一个统一的变形图，包括一个简化的网格和一个轨迹关键帧的姿态图。Kimera-Semantics 使用 Kimera-VIO 的姿态估计为每个机器人 α\alpha 构建局部 3D 网格，跟踪 Kimera-VIO 的每个关键帧中得到的 3D 网格顶点子集。为了构建变形图：

  • 首先从 Kimera-Semantics 对网格进行子采样，得到一个简化的网格。将网格顶点存储在八叉树中，使用在线顶点聚类方法简化网格；随着网格的增长，八叉树的同一体素中的顶点被合并，退化的面和边被删除。体素大小根据环境或数据集进行调优。
  • 然后，将该简化网格的顶点和对应的关键帧位姿作为顶点添加到变形图中；把变形图中相应的顶点称为网格顶点和关键帧顶点。
  • 此外，在变形图中添加了两种类型的边：网格边（对应于简化网格中共享一个面的网格顶点对）和关键帧边（将关键帧与它所观察的网格顶点集连接起来）。

对于变形图的每个顶点 kk，设变换 Mk=(RkM,tkM)\bold{M}_k = (\bold{R}_k^M, \bold{t}_k^M) 定义了一个局部坐标系，RkM\bold{R}_k^M 初始化为单位阵，tkM\bold{t}_k^M 初始化为来自 Kimera-Semantics 的网格顶点的位置 gk\bold{g}_k（不考虑回环）。设关键帧 ii 的位姿为 Xi=(Rkx,tkx)\bold{X}_i = (\bold{R}_k^x, \bold{t}_k^x)，初始化为 Kimera-VIO 估计的位姿。

5.2 Optimizing the Deformation Graph

目标是修正每个机器人上的网格，以关联关键帧姿势的变化。为此，需要调整姿态（和网格顶点位置），以“锚定”关键帧姿态到分布式 PGO 的最新估计，如 Fig.5 所示。设分布式 PGO 的最优位姿为 Xˉi\bar{\bold{X}}_i，nn 为轨迹上的关键帧数，mm 为变形图中的网格顶点数。通过如下的每个机器人上的局部优化求解 Xi,Mi\bold{X}_i, \bold{M}_i：

argmin⁡X1,...,Xn∈SE(3)M1,...,Mm∈SE(3)∑i=0n∥Xi⊟Xˉi∥Σx2+∑k=0m∑l∈NM(k)∥RkM(gl−gk)+tkM−tlM∥Σ2+∑i=0n∑l∈NM(i)∥Rixg~il+tix−tiM∥Σ2(11)\begin{aligned} \underset{\bold{M}_1, ..., \bold{M}_m \in SE(3)}{\underset{\bold{X}_1, ..., \bold{X}_n \in SE(3)}{\operatorname{argmin}}} &\quad \sum_{i=0}^n \left\| \bold{X}_i \boxminus \bar{\bold{X}}_i \right\|^2_{\Sigma_x} \\ &+ \sum_{k=0}^m \sum_{l \in \mathcal{N}^M(k)} \left\| \bold{R}_k^M (\bold{g}_l - \bold{g}_k) + \bold{t}_k^M - \bold{t}_l^M \right\|^2_{\Sigma} \\ &+ \sum_{i=0}^n \sum_{l \in \mathcal{N}^M(i)} \left\| \bold{R}_i^x \tilde{\bold{g}}_{il} + \bold{t}_i^x - \bold{t}_i^M \right\|_{\Sigma}^2 \end{aligned} \tag{11}

其中 gk\bold{g}_k 为顶点 kk 在变形图中的不变位置（nondeformed position），g~il\tilde{\bold{g}}_{il} 为顶点 ll 在关键帧 ii 的坐标系下的不变位置，NM(k)\mathcal{N}^M(k) 为变形图上和 kk 邻接的所有顶点，⊟\boxminus 表示位姿 Xi\bold{X}_i 和 Xˉi\bar{\bold{X}}_i 的正切空间表示。

  • (11) 的第 1 项使得关键帧位姿 Xi\bold{X}_i 去匹配 PGO 优化后的位姿 Xˉi\bar{\bold{X}}_i；
  • (11) 的第 2 项最小化与 gk\bold{g}_k 的错误匹配来维护网格刚性；
  • (11) 的第 3 项确保关键帧和网格顶点之间的局部刚性。
  • 使用 GTSAM 结合 LM 求解 (11)。

由于变形图包含原始网格的降采样版本，在优化后，检索剩余顶点的位置。特别地，完整网格顶点的位置通过变形图中节点的仿射变换得到：

v~i=∑j=1msj(vi)[RjM(vi−gj)+tjM](12)\tilde{\bold{v}}_i = \sum_{j=1}^m s_j (\bold{v}_i) [\bold{R}_j^M (\bold{v}_i - \bold{g}_j) + \bold{t}_j^M] \tag{12}

其中 vi\bold{v}_i 表示顶点的原始位置，v~i\tilde{\bold{v}}_i 为变形后的位置。权重 sjs_j 为：

sj(vi)=(1−∥vi−gj∥dmax)2(13)s_j(\bold{v}_i) = \left( 1 - \frac{\| \bold{v}_i - \bold{g}_j \|}{d_{max}} \right)^2 \tag{13}

然后进行归一化。其中 dmaxd_{max} 表示到 k+1k+1 个顶点的距离（k=4k=4）。

6 Experiments

6.1 PGO Robustness Analysis

  • 单机器人实验：
  • 多机器人实验：

6.2 Evaluation in Simulation and Benchmarking Datasets

  • 轨迹估计结果：
  • 通信开销：
  • Metric-Semantic 网格质量：

6.3 Evaluation in Large-Scale Outdoor Datasets

参考

  • [1] H. Yang, P. Antonante, V. Tzoumas, and L. Carlone, “Graduated non- convexity for robust spatial perception: From non-minimal solvers to global outlier rejection,” IEEE Robot. Autom. Lett., vol. 5, no. 2, pp. 1127–1134, Apr. 2020.

  • [2] Y. Tian, K. Khosoussi, D. M. Rosen, and J. P. How, “Distributed certifiably correct pose-graph optimization,” IEEE Trans. Robot., vol. 37, no. 6, pp. 2137–2156, Dec. 2021.

  • [3] A. Rosinol, M. Abate, Y. Chang, and L. Carlone, “Kimera: An open-source library for real-time metric-semantic localization and mapping,” in Proc. IEEE Int. Conf. Robot. Autom., 2020, pp. 1689–1696.

  • [4] D. Nistér, “An efficient solution to the five-point relative pose problem,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 26, no. 6, pp. 756–770,

    Jun. 2004.

  • [5] K. Arun, T. Huang, and S. Blostein, “Least-squares fitting of two 3-D point sets,” IEEE Trans. Pattern Anal. Mach. Intell., vol. PAMI-9, no. 5, pp. 698–700, Sep. 1987.

  • [6] M. J. Black and A. Rangarajan, “On the unification of line processes, outlier rejection, and robust statistics with applications in early vision,” Int. J. Comput. Vis., vol. 19, no. 1, pp. 57–91, 1996.