Incremental Mapping with Measurement Synchronization & Compression

TL;DR

The paper tackles the challenge of asynchronous, multi-sensor SLAM by proposing an incremental method to construct connected factor graphs that guarantee data fusion across sensors with varying rates. It introduces sub-optimal graph generation via clustering of measurements and selection of topology using an external evaluation criterion, Mutually Orthogonal Metric (MOM). The approach achieves substantial graph compression—approximately 30% on average—while preserving map quality, validated on KAIST Urban sequences with multiple sensor modalities. This work enables scalable, globally consistent mapping under asynchronous measurements and lays groundwork for learning-based strategies to automate graph topology selection.

Abstract

Modern autonomous vehicles and robots utilize versatile sensors for localization and mapping. The fidelity of these maps is paramount, as an accurate environmental representation is a prerequisite for stable and precise localization. Factor graphs provide a powerful approach for sensor fusion, enabling the estimation of the maximum a posteriori solution. However, the discrete nature of graph-based representations, combined with asynchronous sensor measurements, complicates consistent state estimation. The design of an optimal factor graph topology remains an open challenge, especially in multi-sensor systems with asynchronous data. Conventional approaches rely on a rigid graph structure, which becomes inefficient with sensors of disparate rates. Although preintegration techniques can mitigate this for high-rate sensors, their applicability is limited. To address this problem, this work introduces a novel approach that incrementally constructs connected factor graphs, ensuring the incorporation of all available sensor data by choosing the optimal graph topology based on the external evaluation criteria. The proposed methodology facilitates graph compression, reducing the number of nodes (optimized variables) by ~30% on average while maintaining map quality at a level comparable to conventional approaches.

Figures (5)

• Figure 1: Base
• Figure 2: Ours
• Figure 4: A measurement sequence with 4 scans acquired from 2 LiDARs, GPS and IMU data, and the factor graph with poses, velocities, and IMU biases to be estimated. $G_{t-1}$ is a factor graph for the time history $[t_0, ..., t \text{-} 1]$, with prior factors connecting the poses derived from LiDAR scans. The vertex $X_2$ is not connected with others, and the associated GPS factor does not influence the estimation of all variables.
• Figure 5: Different factor graph candidates generated by the proposed algorithm for the same sequence of measurements. The resulting graph $G_{t}$ is a combination $\oplus$ of the previous graph $G_{t-1}$ and the factors form the chosen candidate $G_{\text{best}}$. The 1-st candidate clusters the GPS measurement and the scan from LiDAR 2, while the 2-nd one combines it with two scans from both LiDARs.
• Figure 6: A connected factor graph with LiDAR 1 (red) and LiDAR 2 (purple) scan poses illustrates a challenge in the Minimal Time Shift and Minimal Solver Error scenarios. The variables are loosely coupled, and the GPS factors do not compensate for the drift in odometry measurements obtained from scans of LiDAR 2.