HAMMER: Heterogeneous, Multi-Robot Semantic Gaussian Splatting

In the first stage of the alignment process, HAMMER detects potential candidate correspondences between the data of the unaligned robots and that of the aligned robots. Regardless of alignment status, a place-recognition feature vector is extracted for every color image received from the collaborating robots using NETVLAD [24]. Additionally, the features from the unaligned robots are periodically compared to those of the aligned robots using cosine similarity. The similarities are thresholded with $\gamma_{ij}$ where $i,j$ indicates the index of the aligned and unaligned robot, respectively. For pairs of images with similarities exceeding $\gamma_{ij}$, the alignment module performs an additional feature-matching verification step where SuperPoint local features [25] are extracted for each image, and then matched using LightGlue [26]. If the fraction of features matched during verification exceeds a fixed threshold (0.25 in our experiments) then the image pair is accepted as a potential inter-robot correspondence.

When no image pairs pass the threshold, then $\gamma_{ij}$ is increased to match the mean of all place recognition similarities greater than the previous value. This thresholding scheme is used because multiple instances of the same camera (e.g. two robots with ZED cameras) tend to have higher similarity regardless of visual content, resulting in spurious matches. Dynamic thresholding allows initial values of $\gamma_{ij}$ to be set to a low value for all robot pairs, circumventing any hyper-parameter tuning that would otherwise be necessary. While this procedure for determining candidate alignment image pairs grows quadratically in the number of images per robot, many of these operations can be cached, forming a relatively small computational footprint.

For every potential correspondence between an aligned and unaligned robot, a small set of color images within some fixed temporal window size $W=16$ is selected from each robot’s data cache on the server. Subsets of the pair’s data streams are used to perform localized SfM, as opposed to bulk SfM where the whole data stream is used. We use the COLMAP backend [15] with SuperPoint features and the SuperGlue matcher [27], which demonstrates robustness in aligning images from different device types. Importantly, the localized SfM does not use any pose information from the robot’s SLAM systems, producing an independent relative pose estimate between robots.

The output of the localized SfM procedure lies in an arbitrary coordinate frame, $\mathcal{T}^{s}$. The final stage of alignment determines the scaled transformation mapping poses from the local frame of the unaligned robot, $\mathcal{T}^{j}$, to the global frame of the aligned robot, $\mathcal{T}^{g}$. The relative scaled transform, rotation $R^{jg}\in SO(3)$, translation $t^{jg}\in\mathbb{R}^{3}$, and scaling $s^{jg}\in\mathbb{R}^{+}$, transforms a pose $(R^{j},t^{j})$ in $\mathcal{T}^{j}$ to $\mathcal{T}^{g}$ through

$\displaystyle t^{g}$

$\displaystyle=s^{jg}R^{jg}t^{j}+t^{jg},\;\;R^{g}=R^{jg}R^{j},$

(1)

where $(R^{g},t^{g})$ is the transformed pose. Scaled transformations account for non-metric localization software used by any robot and the SfM intermediary frame. So long as the origin frame is set using a robot with a metric localization system, the map will retain the metric scale.

Computing the relative transformation for unaligned robots requires solving two instances of a modified absolute orientation problem [28]. One instance computes the scaled transformation from the unaligned $\mathcal{T}^{j}$ to the intermediary SfM frame $\mathcal{T}^{s}$. The second instance transforms poses from $\mathcal{T}^{s}$ to the global frame $\mathcal{T}^{g}$. The first instance ingests an ordered sequence of poses $\{(R_{k}^{j},t_{k}^{j})\}_{k=1}^{W}$ from the unaligned local frame and $\{(R_{k}^{s},t_{k}^{s})\}_{k=1}^{W}$ from the corresponding poses in the localized SfM solution.

To compute these transformations we solve the following rotation-aware absolute orientation problem

$$\min_{s,R,t}\sum_{k=1}^{W}\left[||sR\,t_{k}^{j}-t_{k}^{s}+t||^{2}+\epsilon||RR_{k}^{j}-R_{k}^{s}||_{F}\right],$$

(2)

where $||\cdot||_{F}$ denotes the Frobenius norm. Equation 2 optimizes the scaling, rotation, and translation $(s,R,t)$ between the two frames with a small regularization term on the rotation to break ties. For example, pose sequences where both datasets progress linearly (like for ground robots) may not have a unique rotation that minimizes the first term of (2). Therefore, the second term serves to optimize over the remaining degrees of freedom of $R$ subject to some small $\epsilon=0.01$. While no closed form solution exists for (2), each individual term has a closed form solution dependent on an expression of the form $SVD(H)$. Therefore, we can compute an approximately optimal solution dependent on $SVD(H_{1}+\epsilon H_{2})$.

The result of approximately solving (2) is the scaled transformation from the unaligned $\mathcal{T}^{j}$ frame to the localized SfM $\mathcal{T}^{s}$ frame. Repeating the same procedure between poses in the global frame set by the aligned robots and their corresponding poses in the SfM solution produces the transformation from $\mathcal{T}^{s}$ to $\mathcal{T}^{g}$. Finally, the composition of both transformations yields $(s^{jg},R^{jg},t^{jg})$ the relative frame transformation from the unaligned robot’s local coordinate frame to the global coordinate frame.

The proposed inter-robot alignment module is robust and fails gracefully by terminating early or rejecting spurious alignments. These events can occur during image-correspondence verification, after localized SfM if the solution fails to converge, and during the absolute orientation procedures if the solution residuals are high. Rejection at any of these stages avoids spurious alignments due to on-board localization errors or failures during the localized SfM solve.

Figure 2 illustrates a high-level overview of the entire alignment process. Importantly, the process treats the on-board localization algorithms as black-boxes and only uses the resulting camera poses and color images as input. This attribute of HAMMER dramatically simplifies the integration of new devices due to the ubiquity and variety of on-board localization frameworks. In contrast, alignment methods that require access to lower-level information from each individual robot’s localization system (e.g. pose graphs or sparse maps) will fail if the software is closed-source.

3DGS models the opacity and color of the environment using explicit Gaussian primitives, which are optimized based on a differentiable, tile-based rasterization process into a photometric loss. The optimizable parameters of each Gaussian include a mean $\mu\in\mathbb{R}^{3}$, a covariance $\Sigma\in S^{3}_{++}$ (the geometric extent of the Gaussian), an opacity $\alpha\in[0,1]$, and an RGB color $c\in[0,1]^{3}$. The color is further represented as a set of learned spherical harmonics coefficients, yielding view-dependent color to model realistic visual appearance.

Furthermore, we seek to model semantics in the environment. Specifically, HAMMER encodes the per-pixel semantic embedding, extracted from 2D color images using CLIP [30, 31], into the $3$DGS. Some works [32] augment the parameters of each Gaussian with a semantic channel and regress it directly from a image reconstruction loss. However, these semantic embeddings can range from 500 to 1000 dimensions, which is prohibitively expensive to inference and store. Therefore, an additional encoding network is used to compress the CLIP embedding. This encoding network is learned in conjunction with the 3DGS model, but can be difficult to optimize while streaming data.

Instead, HAMMER optimizes a continuous feature field, parameterized by a neural network with hash-grid positional encodings. This feature field $\phi_{s}(x):\mathbb{R}^{3}\rightarrow\mathbb{R}^{d_{s}}$ maps positions in the $3$DGS frame $\mathcal{T}^{g}$ to a feature vector of dimension $d_{s}$. To query the semantic embedding of a Gaussian, we evaluate $\phi_{s}$ at the mean. During training, we re-project the $3$DGS rendered depth and ground-truth CLIP images into $3$D, producing a labeled point cloud. The point cloud is then directly used to supervise the feature field.

A key challenge when mapping with data streams from heterogeneous robots is compensation for each sensor’s differing image signal processing (ISP) pipelines (e.g. exposure and white-balancing). These pipelines can have a significant impact on the appearance of the resulting color images. Consequently, these appearance shifts can produce view inconsistency in the color between devices, despite their content being the same. The view inconsistency can cause erroneous, non-existent structures in the 3DGS map, called floaters, due to the optimizer reconciling contradicting signals from images captured from devices with different ISP pipelines. To avoid this undesirable behavior, we utilize the bilateral grid-based ISP compensation method proposed in [33]. Specifically, for each image, we create additional bilateral grid parameters that compensate for the appearance differences between devices by matching the color of the rendered images to the ground-truth images through affine transformations.

Once the poses from a given robot’s data stream have been aligned to the map frame we begin the online optimization process to actively incorporate the data stream into the 3DGS map. For each message (i.e. color image, pose, geometric data) sent by the aligned robots, HAMMER integrates this data into the $3$DGS through the following initialization process. First, new Gaussian primitives are spawned, with the means located according to a sparse point cloud. If the geometric data is a point cloud, then a fixed number of points are sampled from the point cloud to produce the sparse point cloud. If the data is a depth image, then the camera intrinsics are used to project randomly selected pixels into the scene to create a sparse cloud. In both instances, the Gaussians are initialized with isotropic covariances such that when projected onto their source cameras they occupy a single pixel. These covariances are computed as $\Sigma=\frac{d}{f}\*I_{3}$ where $d$ is the distance of the Gaussian from the source camera, $f$ is the focal length of the camera, and $\*I_{3}$ is the identity matrix in $3$D. The color is initialized using the pixel value of the corresponding projected pixel. Finally, the opacity is initialized to a fixed value $\sigma=0.3$ for all Gaussians.

After the initialization phase, the data from each aligned robot stream is aggregated into a pool, which is continuously sampled and used for $3$DGS optimization. We weight the probability of a particular data tuple being sampled based on the number of times it has already been sampled to avoid over-weighting information from frames received early in the runtime. When depth images are available, HAMMER adds depth-supervision to the existing color photometric loss. The depth loss helps improve the geometry of the map in regions of visual ambiguity (e.g. monochromatic flat surfaces).

During online map optimization, HAMMER refines not only the individual camera poses, but also the relative transformation between each robot’s local frame and the global frame. Specifically, we optimize an $SE(3)$ offset from the aligned camera pose to the refined camera pose. Regularization on both the rotation and translation is used to avoid catastrophic drift in the poses. We use a similar approach for fine-tuning the alignment of each of the robot’s local frame $\mathcal{T}^{j}$ to the global frame $\mathcal{T}^{g}$.

HAMMER uses ROS2 [34] to stream data from all robots to a server. Software for HAMMER is based off of a modified version of NerfBridge [35]. The server is a desktop equipped with an AMD Ryzen 7 5800x CPU, a NVIDIA RTX 4090 GPU, and 32 GBs of RAM.

The heterogeneous teams are composed of Turtle Bots (non-holonomic ground robots equipped with stereo cameras) and humans wearing Aria glasses [1] that are equipped with a broad range of onboard sensors. The ground robots are outfitted with a forward facing ZED Mini stereo camera roughly 0.5m above the ground. The ZED camera is connected to a NVIDIA Jetson Orin Nano that handles on-board sensor data processing. The camera uses its proprietary SLAM system integrated with the ZED SDK for pose estimation. The ground robot is connected to the server using 5GHz WiFi, and streams the estimated pose, 1280x720 MJPEG compressed color images, and 1280x720 depth images at 10Hz.

Aria glasses provide their own onboard compute and pose estimate using the Aria Project’s proprietary SLAM system. The glasses produce 1440x1440 fisheye color images and semi-dense point clouds at 10Hz. Unlike the ZED, the Aria devices do not have a ROS interface, so we convert data recorded from these devices into ROS bags and play these bags back during experiments to simulate streaming. For all robots, we assume access to intrinsic camera calibration information.

We test HAMMER in two real-world environments, which we label Home and Basement. Home is a multi-room apartment and contains regions of high feature richness (open rooms with plants/furniture) and low richness (hallways and rooms with blank walls). The scene also includes regions of drastic lighting changes due to open windows with outside natural light, resulting in wide variations in color image appearances due to camera exposure compensation. In Home, we deploy two Aria glasses and one ground robot.

Basement is composed of a large lab space, kitchenette, long hallways, and a conference room. Lighting conditions in this environment are more uniform due to the presence of continuous overhead lighting, but also includes several regions with sparse visual features like hallways and reflective floor-to-ceiling glass windows. Two Aria glasses and two ground robots are deployed in Basement.

Evaluation data is produced by holding out some of the data streamed from each device during the trials. To assess the accuracy of HAMMER’s pose alignment and refinement, we process all data with COLMAP (i.e. bulk SfM) to get accurate pose estimates. This dataset serves as an upper bound on the quality of the aligned streamed dataset. The bulk SfM procedure takes about 2-5 hours for each trial, illustrating the need for HAMMER’s real-time alignment module.

We compare HAMMER against three baselines. The first is Oracle, which uses the output of the bulk SfM procedure to optimize a $3$DGS map. Again, Oracle provides an upper bound on the performance of HAMMER because the SfM poses are treated as ground truth in our evaluation set. Next, Individuals optimizes independent $3$DGS maps using each robot’s own data, which forms a set of baselines. Importantly, to ensure a fair comparison, Individuals uses the same data collected by executing HAMMER, but it has an extra advantage in that the 3DGS map is trained in bulk with all data available at once (from a single robot). Individuals shows the best quality possible from a single robot, thereby highlighting HAMMER’s added value in using multi-device collaboration to improve map quality.

To our knowledge, the only comparable work to HAMMER is Di-NeRF [12], which proposes collaboratively optimizing a NeRF while using the same gradient-based alignment correction scheme discussed in Section IV-B3. However, rather than solving for initial relative frame transformations like HAMMER, Di-NeRF initializes translation and rotation to zero and identity, respectively. HAMMER has additional differences to Di-NeRF, which we modify to provide a fair comparison. First, Di-NeRF is a distributed optimization algorithm, while HAMMER is centralized. Therefore, we solve the centralized problem proposed by Di-NeRF which forms an upper bound on the performance of Di-NeRF. Second, Di-NeRF does not perform online training, so we use cached data from HAMMER trials for map optimization. Again, this modification gives Di-NeRF an advantage compared to HAMMER because it has access to all of the data at the start of training. Finally, Di-NeRF uses a NeRF representation, which is typically less performant than $3$DGS. Therefore, we replace the NeRF with a $3$DGS, retaining the hyper-parameters of HAMMER when possible. We refer to this baseline as Di-NeRF*.

Figure 3 visualizes meshes extracted from the $3$DGS maps constructed by HAMMER as they evolve with time during the online map optimization process. Embedded in the meshes are trajectories taken by each robot during deployment, highlighting where each robot contributed data and the overall spatial distribution of data. Qualitatively, HAMMER demonstrates superior photo-realistic renders on the evaluation set (Figure 5) compared to Di-NeRF*, which fails to resolve robot alignments and therefore cannot accurately match the ground-truth images. HAMMER also approaches the quality of the Oracle baseline.

We quantify reconstruction quality based on the average peak signal to noise ratio (PSNR) of each method on the held-out evaluation data set (Figure 4). HAMMER dramatically outperforms Di-NeRF*, suggesting that the gradient-based alignment of Di-NeRF fails to converge to accurate inter-robot alignments. The set of Individuals baselines underperforms HAMMER as expected, highlighting the benefit of robot collaboration in map building. Note that the performance of some individual maps is relatively high due to good performance on their respective evaluation sets, but degrade drastically when benchmarked on another robot’s evaluation set. Finally, as expected, Oracle is an upper bound on the performance of HAMMER but is significantly slower.

HAMMER runtime is dominated by the $3$DGS map optimizer and the inter-robot alignment module. Processing a single data-frame in the map optimizer ranges between 15 ms (66 Hz) when the map is sparsely populated to 38 ms (26 Hz). Slower iterations typically occur immediately after a robot has been aligned and all of its cached data is consolidated into the map. The runtime of the alignment module is dominated (90% - 99.5% of the runtime) by the localized SfM procedure. Each SfM alignment requires about 33.6s to solve. The remaining runtime is primarily the feature-matching based verification for proposed image correspondences (approximately 2.6s).

We demonstrate the utility of HAMMER for collaborative downstream tasks by completing a multi-robot motion planning task with language-specified goals in Basement using a HAMMER $3$DGS map. To set a goal location, we query an object’s location using natural language. Then, using CLIP, we extract the semantic embedding for the goal query and search the HAMMER $3$DGS map for the most semantically similar Gaussian using cosine similarity. Next, we sample the surrounding space for a nearby location in free space, which we set as the goal location. Once a goal is specified, we use Splat-Nav [7] to plan a collision-free trajectory from the initial position of each robot to the goal location. Figure 6 shows optimized trajectories for each of the four robots in Basement with 9 semantically-specified goals. Searching the map for each semantic goal location required about 20ms, and the subsequent trajectories were computed within 0.2s to 0.75s depending on the distance to the goal. All robots successfully completed the task.