Group-wise Consistent Measurement Set Maximization

• The paper introduces GCM to address correlated biases in sonar-based loop closures by imposing group-wise constraints, improving closure selection in multi-robot SLAM.
• GCM augments pairwise consistency tests with spatial and temporal grouping, using cross-robot pose relations and robust aggregation of intra-group cycle residuals.
• The method demonstrates improved accuracy over PCM in bandwidth-limited underwater settings, as evidenced by reductions in ATE and robust performance in challenging environments.

Group-wise Consistent Measurement Set Maximization (GCM) is a loop-closure selection formulation introduced in DRACo-SLAM2 for distributed multi-robot SLAM with multibeam imaging sonar. It is designed for settings in which each robot builds a local map, intermittently detects inter-robot loop closures, and must merge maps under bandwidth-limited underwater acoustic communication. In this setting, nearby sonar-based registrations often exhibit correlated biases, so a set of loop closures can be mutually consistent under pairwise tests while remaining jointly incorrect. GCM addresses that failure mode by augmenting Pairwise Consistent Measurement Set Maximization (PCM) with a group-wise constraint defined over spatially and temporally adjacent closures, using cross-robot pose relations and robust aggregation of intra-group cycle residuals (Huang et al., 31 Jul 2025).

1. Sonar-specific problem setting

In distributed multi-robot SLAM with multibeam imaging sonar, inter-robot loop closures are used to merge local maps and reduce drift. DRACo-SLAM2 considers the case in which loop-closure detection and selection must be both robust and communication-efficient because underwater acoustic communication is bandwidth-limited. The central motivation for GCM is that scan matching on sonar point clouds is often performed on temporally and spatially adjacent scans, especially in regions with repeating structures such as closely spaced pier pilings. In such regions, nearby inter-robot closures often share similar registration biases, including consistent yaw bias or translation bias, because of limited distinct geometric cues, partial overlap, specular returns, and aliasing (Huang et al., 31 Jul 2025).

PCM is insufficient in precisely that regime. PCM checks consistency only between pairs of measurements. If several loop closures are co-located and share a common registration bias, they can mutually reinforce one another under pairwise tests and therefore pass PCM despite being jointly incorrect. GCM is introduced to expose those correlated errors by imposing a constraint on a whole group of related closures rather than on isolated pairs alone (Huang et al., 31 Jul 2025).

A common misconception is that GCM is merely PCM with a stricter threshold. The formulation in DRACo-SLAM2 is structurally different: it adds a group-wise cost over intra-group cycles and composes closures through historical inter-robot map-frame relations, so its rejection mechanism is not reducible to pairwise threshold tightening (Huang et al., 31 Jul 2025).

2. Formal definition and optimization objective

Let $M = \{m_i\}$ denote the set of candidate inter-robot loop closures detected after object graph matching and refined ICP registration. Each $m_i$ is a relative pose measurement between robot frames at specific timestamps. In $SE(2)$ or $SE(3)$, let $T_i \in SE(d)$ denote the estimated rigid transformation associated with $m_i$, with $d \in \{2,3\}$. Each measurement is modeled by a residual $e_i \in \mathbb{R}^p$, with $p = 3$ in $SE(2)$ or $m_i$0 in $m_i$1, and covariance $m_i$2. Using the Lie algebra $m_i$3 mapping, a residual between two poses $m_i$4 is written as $m_i$5 (Huang et al., 31 Jul 2025).

PCM uses a pairwise consistency predicate. For two measurements $m_i$6 and $m_i$7, the cycle-composition error is

$m_i$8

where $m_i$9 and $SE(2)$0 are locally marginalized relative state estimates from each robot’s SLAM and $SE(2)$1 is a weighting matrix. The pairwise predicate is

$SE(2)$2

GCM introduces an additional group-wise constraint. Let $SE(2)$3 denote a group of loop closures that are spatially and temporally adjacent and potentially involve multiple neighbor robots. For $SE(2)$4 and $SE(2)$5, cross-robot composition is defined by

$SE(2)$6

The group-wise cycle residual is

$SE(2)$7

and the group consistency cost is

$SE(2)$8

where $SE(2)$9 is a robust aggregate such as the median or a Huber-weighted mean. DRACo-SLAM2 gives the two explicit forms

$SE(3)$0

and

$SE(3)$1

A group is accepted only if $SE(3)$2 (Huang et al., 31 Jul 2025).

The corresponding selection problem is

$SE(3)$3

DRACo-SLAM2 also states a weighted variant,

$SE(3)$4

In effect, GCM seeks a largest subset that is simultaneously pairwise feasible and group-wise coherent (Huang et al., 31 Jul 2025).

3. Relation to PCM and to broader group-wise consistency formulations

PCM, as summarized in DRACo-SLAM2, constructs a pairwise consistency graph over loop closures, adds edges between mutually consistent pairs, and then selects a largest consistent subset, often by maximum clique or greedy pruning. GCM tightens that feasible set by adding a group-wise cost over intra-group cycles. Consequently, any GCM-feasible set is also PCM-feasible, but not vice versa. In regions without correlated error, GCM often matches PCM; when correlated registration biases are present, GCM is typically a strict subset of the PCM solution and improves precision by removing pairwise-consistent but jointly biased closures (Huang et al., 31 Jul 2025).

Within the broader SLAM literature, Group-$SE(3)$5 Consistency Maximization (GkCM) generalizes the same principle beyond pairwise testing by requiring that every $SE(3)$6-tuple of selected measurements pass a group-$SE(3)$7 consistency test. That problem is formulated as maximum clique on a $SE(3)$8-uniform hypergraph rather than on an ordinary graph, and exact or heuristic branch-and-bound algorithms are adapted to the hypergraph setting (Forsgren et al., 2022). A related treatment for robust multi-robot map merging presents the same equivalence between the largest group-$SE(3)$9 internally consistent subset and the maximum hyperclique, together with hierarchical pruning to reduce the number of required higher-order checks (Forsgren et al., 2023).

GCM in DRACo-SLAM2 is narrower in scope and more domain-specific than those hypergraph formulations. It is not presented as a generic $T_i \in SE(d)$0-uniform hypergraph solver; instead, it is an incremental modification of PCM tailored to underwater sonar scan matching, with groups formed by local spatial and temporal adjacency and with group residuals computed through cross-robot map-frame relations (Huang et al., 31 Jul 2025).

IPC provides a different group-wise perspective. It incrementally validates each loop closure on a minimal independent subgraph, uses a probabilistic $T_i \in SE(d)$1 test on all factors in that subgraph, and gives previously accepted constraints veto power. IPC therefore enforces joint feasibility online, but it does so through local subgraph optimization rather than through the sonar-specific group residual and aggregation used by GCM (Olivastri et al., 2024).

4. Incremental distributed algorithm in DRACo-SLAM2

At robot $T_i \in SE(d)$2, the data used by GCM include object maps $T_i \in SE(d)$3 from neighbor robots together with $T_i \in SE(d)$4’s own object map $T_i \in SE(d)$5, locally marginalized relative states such as $T_i \in SE(d)$6, and historical map-to-map transforms such as $T_i \in SE(d)$7 from previously accepted closures. On receiving $T_i \in SE(d)$8, DRACo-SLAM2 first constructs graphs $T_i \in SE(d)$9 and $m_i$0 and solves QAP/LAP to obtain matched object pairs. If at least three pairs match, an affine transform $m_i$1 is estimated via affine RANSAC and accepted if there are at least four inliers. The system then requests scans for the matched objects and runs ICP with $m_i$2 as the initial guess to produce candidate loop closures $m_i$3 with transforms $m_i$4 and covariances $m_i$5 (Huang et al., 31 Jul 2025).

Group formation is incremental. For each new closure $m_i$6, the robot assigns it to an existing group whose spatial centroid or time window is near $m_i$7—with distance in pose space below $m_i$8—or starts a new group. It then performs PCM pairwise tests against the closures already in that group, computes cross-robot compositions such as $m_i$9, evaluates residuals $d \in \{2,3\}$0 and the aggregate cost $d \in \{2,3\}$1, and rejects $d \in \{2,3\}$2 or prunes offending elements if any pairwise test fails or if $d \in \{2,3\}$3. If accepted, the closure is added both to the group and to the measurement set $d \in \{2,3\}$4, and group summaries such as centroid and error statistics are updated (Huang et al., 31 Jul 2025).

Accepted closures are inserted into the inter-robot factor graph with robust noise modeled by Cauchy. DRACo-SLAM2 then applies a two-step PGO procedure: first a local optimization at $d \in \{2,3\}$5 using $d \in \{2,3\}$6 and $d \in \{2,3\}$7, then optimization of neighboring robots $d \in \{2,3\}$8 involved in $d \in \{2,3\}$9 given $e_i \in \mathbb{R}^p$0’s optimized subset. Communication efficiency is achieved by exchanging object maps containing centers and dimensions, requesting only scans tied to matched objects, and sharing accepted inter-robot closures together with updated map-to-map transforms only when there is significant change (Huang et al., 31 Jul 2025).

The reported per-update complexity reflects that design. Graph matching uses a spectral eigensolve on $e_i \in \mathbb{R}^p$1 plus LAP with Jonker-Volgenant, described as roughly $e_i \in \mathbb{R}^p$2 in the worst case for LAP with $e_i \in \mathbb{R}^p$3, although practical sizes are small after DBSCAN filtering. ICP is near-linear in the number of points in the sliding window and is reported as empirically fast. For a group of size $e_i \in \mathbb{R}^p$4, GCM checks require $e_i \in \mathbb{R}^p$5 pairwise tests per new closure and $e_i \in \mathbb{R}^p$6 residual aggregation; with bounded group sizes, per-update cost is stated as $e_i \in \mathbb{R}^p$7 to $e_i \in \mathbb{R}^p$8, with memory per group $e_i \in \mathbb{R}^p$9 (Huang et al., 31 Jul 2025).

5. Error models, robustification, and integration with object graph matching

In $p = 3$0, DRACo-SLAM2 represents a closure transform as $p = 3$1 and the associated residual vector as

$p = 3$2

In $p = 3$3, the residual lies in $p = 3$4 using twist coordinates. For GCM, the relevant residuals are the cross-robot cycle residuals $p = 3$5, aggregated through the weighted norm $p = 3$6. When approximate covariances are available, the weighting matrix can be set as $p = 3$7, with $p = 3$8 obtained from propagated ICP covariances and pose-graph marginal covariances (Huang et al., 31 Jul 2025).

Robustification appears at two levels. PGO uses a robust Cauchy noise model, whereas GCM’s internal aggregation uses either the median or Huber loss. The Huber function is given explicitly as

$p = 3$9

This separation is technically important: the back-end robustifier mitigates measurement influence during optimization, while GCM attempts to prevent biased closures from entering the optimization in the first place (Huang et al., 31 Jul 2025).

GCM is tightly integrated with object graph matching. Each robot converts its point-cloud map into clustered objects with bounding rectangles via DBSCAN, then builds a complete directed graph whose edge weights are inter-object distances and whose vertices carry dimensions such as length and breadth. Matching is performed by solving a QAP via spectral methods to obtain similarity weights, followed by LAP using Jonker-Volgenant to produce one-to-one vertex matches. With at least three matches, an affine transform is estimated by RANSAC as a prior, and ICP refines it to the candidate closure transform. Candidates are filtered by an overlap ratio before GCM is applied; the registration experiments use a minimum overlap ratio $SE(2)$0 to accept a closure prior to GCM (Huang et al., 31 Jul 2025).

The paper’s worked $SE(2)$1 example clarifies the intended failure mode. Robot $SE(2)$2 receives closures $SE(2)$3 between $SE(2)$4 and $SE(2)$5 and $SE(2)$6 between $SE(2)$7 and $SE(2)$8, all near the same waterfront region, with a small common yaw bias of approximately $SE(2)$9. Because the bias is similar, the pairwise cycle errors can remain below $m_i$00, so PCM accepts $m_i$01. GCM instead computes $m_i$02, aggregates them by the median,

$m_i$03

and rejects at least one closure if the common bias pushes the aggregate above $m_i$04 (Huang et al., 31 Jul 2025).

6. Empirical behavior, trade-offs, and limitations

DRACo-SLAM2 evaluates GCM on two fully simulated 3-robot datasets—one USMMA-like dataset with repeating circular pilings and one airplane wreck site—as well as on a real USMMA dataset with simulated 3-robot communication. In loop-closure detection, object graph matching followed by ICP with a sliding window improves detection throughput dramatically relative to DRACo-SLAM1, and ICP runtime is reported as roughly $m_i$05 faster on average than Go-ICP (Huang et al., 31 Jul 2025).

For pose-graph optimization, the paper reports ATE (RMSE, meters) for the two-step DRACo-SLAM2 system with GCM and PCM:

Dataset / robot	GCM ATE	PCM ATE
USMMA $m_i$06	1.43	1.58
USMMA $m_i$07	1.20	2.43
USMMA $m_i$08	1.23	1.88
Airplane $m_i$09	1.33	1.32
Airplane $m_i$10	0.95	1.36
Airplane $m_i$11	1.31	1.46

These results are described as showing consistent accuracy gains, notably on USMMA where correlated errors are more prevalent. Under full PGO, GCM and PCM are reported as comparable, which localizes GCM’s main benefit to the closure-selection stage rather than to the optimizer itself (Huang et al., 31 Jul 2025).

The method is most helpful when errors are correlated, especially in environments with repeating structures and limited distinct sonar cues. If errors are independent, GCM behaves similarly to PCM. The reported overhead is modest because intra-group aggregation and cross-robot composition add only a small cost relative to ICP and graph matching (Huang et al., 31 Jul 2025).

The paper also gives practical parameter guidance. Group formation can use spatial proximity, for example by clustering closures whose $m_i$12-poses lie within a radius $m_i$13 or within a short time window, and may optionally align with object clusters. Thresholds $m_i$14 and $m_i$15 are tuned by validation; a stated starting point is $m_i$16 so that the group constraint is stricter, with median aggregation recommended for robustness. A sliding-window size of $m_i$17 was used in the sonar registration experiments (Huang et al., 31 Jul 2025).

The stated limitations are correspondingly specific. If correlated biases affect closures spanning many distinct neighbors and regions, aggressive pruning may reduce recall. The method also depends on reasonable availability of historical map-to-map transforms, so very early phases with few accepted closures may rely more heavily on local pairwise tests. Covariance estimation for $m_i$18 may be approximate, and mis-specified weighting can affect sensitivity. Future directions named in the paper are adaptive grouping strategies, principled threshold selection via uncertainty propagation, and integration with active exploration to diversify viewpoints and reduce aliasing (Huang et al., 31 Jul 2025).