Introduction
Robot perception frequently involves solving estimation problems that combine continuous states—such as a vehicle's trajectory or the coordinates of landmarks—with discrete decisions, such as identifying outliers or establishing data associations. These "hybrid" estimation problems are central to Simultaneous Localization and Mapping (SLAM) and semantic perception. However, jointly optimizing discrete and continuous variables is computationally intensive and often NP-hard.
Traditional approaches typically fall into two categories: specialized algorithms for specific tasks (e.g., outlier rejection) or multi-hypothesis trackers that maintain a tree of potential discrete assignments. The former lacks generality, while the latter suffers from exponential growth in complexity, necessitating "pruning" strategies that risk discarding the correct solution. This paper introduces DC-SAM (Discrete-Continuous Smoothing and Mapping), an open-source library designed to solve general hybrid factor graphs efficiently without the need for hypothesis pruning.
The DC-SAM Framework
DC-SAM extends the widely used GTSAM library by providing a unified solver for hybrid factor graphs. The core of the framework is an Alternating Minimization approach that exploits the underlying structure of the problem. While the joint optimization is difficult, the problem becomes tractable when one set of variables is fixed.
Optimization Strategy
The solver iterates between two primary steps:
- The Discrete Step: By fixing the continuous variables (such as robot poses), the problem reduces to finding the optimal discrete assignments. In many robotics applications, these discrete factors are conditionally independent or form small clusters. DC-SAM solves these sub-problems exactly using efficient techniques like the Viterbi algorithm or max-product variable elimination.
- The Continuous Step: With the discrete assignments fixed, the problem becomes a standard nonlinear least-squares optimization. DC-SAM utilizes iSAM2, an incremental smoothing and mapping algorithm that performs sparse updates to the factor graph, ensuring high performance even as the map grows.
A key theoretical contribution of this work is the proof that this alternating approach is a descent method. This guarantees that the objective function—representing the quality of the solution—will improve or remain constant at every iteration, providing a level of mathematical rigor often missing in heuristic hybrid solvers.
Experimental Evaluation
The authors validated DC-SAM across a variety of benchmarks, ranging from synthetic point-cloud registration to real-world autonomous driving datasets.
Robust Pose-Graph Optimization
On the Intel and CSAIL 2D datasets, DC-SAM was tested for its ability to handle synthetic outliers. When provided with a reasonable initialization, DC-SAM achieved accurate trajectory recovery significantly faster than Graduated Non-Convexity (GNC), the current state-of-the-art for robust estimation. Unlike GNC, which uses a graduated approach to ignore outliers, DC-SAM explicitly models the discrete "inlier/outlier" state.
Semantic SLAM on KITTI
The framework's utility in real-world scenarios was demonstrated using the KITTI dataset (Sequences 00, 05, and 08). In a Semantic SLAM context—where the robot must associate detected objects (cars, trucks) across frames—DC-SAM provided substantial improvements over raw odometry. For instance, in Sequence 00, the framework reduced the average translation error from 11.45 meters to 2.88 meters.
Generality and Emergent Behavior
To demonstrate the framework's generality, the authors applied it to point-cloud registration using the Stanford Dragon dataset. Interestingly, the general-purpose DC-SAM solver naturally "re-discovered" the behavior of the well-known Iterative Closest Point (ICP) algorithm. This emergent behavior validates that DC-SAM can subsume specialized algorithms within its broader mathematical framework.
Discussion and Limitations
While DC-SAM offers a powerful and general tool for hybrid estimation, it is fundamentally a local optimizer. Its performance is dependent on the quality of the initial guess. In tests on the CSAIL dataset with poor initialization, the solver was more prone to becoming trapped in local optima compared to global search methods.
Furthermore, while DC-SAM avoids the memory issues of multi-hypothesis solvers by focusing on a Maximum A Posteriori (MAP) estimate, it does not currently track the full posterior distribution. However, it does provide a mechanism to estimate marginal uncertainties for both discrete and continuous variables, which is critical for risk-aware navigation.
Conclusion
DC-SAM bridges the gap between discrete logic and continuous optimization in robotics. By providing a general-purpose, open-source library that is both faster and more scalable than existing multi-hypothesis methods, it enables researchers to tackle complex hybrid problems—from semantic mapping to robust SLAM—within a single, unified framework.
The DC-SAM library is open-source and available for the community at github.com/MarineRoboticsGroup/dcsam.
Summary updated