Discrete-Continuous Smoothing and Mapping

Kevin J. Doherty, Ziqi Lu, Kurran Singh, John J. Leonard

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:

  1. 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.
  2. 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

Abstract

We describe a general approach for maximum <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">a posteriori</i> (MAP) inference in a class of <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">discrete-continuous factor graphs</i> commonly encountered in robotics applications. While there are openly available tools providing flexible and easy-to-use interfaces for specifying and solving inference problems formulated in terms of <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">either</i> discrete <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">or</i> continuous graphical models, at present, no similarly general tools exist enabling the same functionality for <italic xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">hybrid</i> discrete-continuous problems. We aim to address this problem. In particular, we provide a library, DC-SAM, extending existing tools for inference problems defined in terms of factor graphs to the setting of discrete-continuous models. A key contribution of our work is a novel solver for efficiently recovering approximate solutions to discrete-continuous inference problems. The key insight to our approach is that while joint inference over continuous and discrete state spaces is often hard, many commonly encountered discrete-continuous problems can naturally be split into a “discrete part” and a “continuous part” that can individually be solved easily. Leveraging this structure, we optimize discrete and continuous variables in an alternating fashion. In consequence, our proposed work enables straightforward representation of and approximate inference in discrete-continuous graphical models. We also provide a method to approximate the uncertainty in estimates of both discrete and continuous variables. We demonstrate the versatility of our approach through its application to distinct robot perception applications, including robust pose graph optimization, and object-based mapping and localization.
BibTeX
@article{doherty2022discrete,
  author = {Kevin J. Doherty and Ziqi Lu and Kurran Singh and John J. Leonard},
  title = {{Discrete-Continuous Smoothing and Mapping}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2022},
  url = {https://openreview.net/forum?id=Hd9_2mJqWvm},
}
Figure