💻 computer science

Variable Elimination in Hybrid Factor Graphs for Discrete-Continuous Inference & Estimation

This paper introduces a novel framework for Hybrid Factor Graphs featuring a new variable elimination algorithm that enables exact Maximum A Posteriori estimation and marginalization for problems involving both discrete and continuous variables, while employing a tree-structured representation with pruning to ensure tractable inference.

Original authors: Varun Agrawal, Frank Dellaert

Published 2026-04-30

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Varun Agrawal, Frank Dellaert

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to solve a giant, complex puzzle while driving a car. Some pieces of the puzzle are smooth and continuous, like the exact position of your car or the angle of your steering wheel. Other pieces are "on/off" switches or choices, like deciding which road to take at an intersection or whether a traffic light is red or green.

For a long time, computer scientists have been great at solving puzzles with only smooth pieces (like standard GPS navigation) or only switch pieces (like simple logic games). But real-world robotics is messy: it involves both at the same time. This paper introduces a new, smarter way to solve these "hybrid" puzzles all at once, without having to guess or approximate the answers.

Here is a breakdown of how their new system works, using simple analogies:

1. The Problem: The "Two-World" Dilemma

In robotics, you often have to figure out where a robot is (continuous) while also making discrete choices, like "Is this object a cup or a book?" or "Did the robot slip on the floor or stay steady?"

Previous methods tried to solve this by either:

Approximating: Pretending the "choices" were smooth numbers, which leads to errors.
Specialized Solvers: Using different tools for the smooth parts and the choice parts, which is slow and clunky.
Guessing: Trying a few options and hoping one sticks, which can get the robot stuck in a "local minimum" (a wrong solution that looks right).

2. The Solution: A "Hybrid Factor Graph"

The authors built a new mathematical framework called a Hybrid Factor Graph. Think of this as a giant flowchart or a family tree that connects all the robot's data.

The Nodes: These are the variables (where the robot is, what it sees, what choices it made).
The Factors: These are the rules connecting them (e.g., "If the robot turns left, the position changes by X").
The Innovation: They created a special type of "connector" (a factor) that can hold a whole family of possibilities. Imagine a single connector that says, "If the robot is in Mode A, the rule is X. If it's in Mode B, the rule is Y." This allows the system to keep all possible scenarios alive in one neat package.

3. The Engine: "Variable Elimination"

To solve the puzzle, the system uses an algorithm called Variable Elimination. Imagine you are cleaning up a messy room. You pick up one item at a time, figure out how it relates to the rest of the room, and then "eliminate" it from the list of things you need to worry about, leaving behind a simplified summary of its impact.

The Process: The algorithm systematically removes variables (like the robot's position at a specific second) one by one.
The Magic: Because of their new math, when they remove a continuous variable (position), they don't lose the discrete choices (modes). Instead, they pass the "story" of those choices down the line.
The Result: By the end, they have a Hybrid Bayes Network. This is the final, clean map of the most likely scenario, showing exactly where the robot is and what choices it made, with perfect mathematical precision (no guessing).

4. Taming the Explosion: "Pruning the Tree"

There is a catch: If a robot has to make 10 choices, and each choice has 2 options, the number of possible scenarios explodes (2 to the power of 10). If it makes 100 choices, the number of scenarios becomes larger than the number of atoms in the universe. The computer would crash trying to check them all.

The authors added two "gardening" techniques to keep the tree from growing too big:

Hypothesis Pruning: Imagine a gardener looking at a tree with thousands of branches. They cut off the tiny, weak branches that are unlikely to grow, keeping only the top 10 strongest branches. In the robot's mind, this means ignoring the "crazy" scenarios (like the robot flying) and only keeping the top 10 most likely stories.
Dead Mode Removal: If a branch of the tree becomes so unlikely that it has almost zero chance of being true, the system declares it "dead" and locks it into a single, fixed state. This effectively removes that choice from the puzzle entirely, making the math much faster.

5. Real-World Testing

The authors tested this on two big challenges:

The City10000 Dataset: A massive simulation of a robot driving through a city with confusing road signs and ambiguous loop closures (where the robot thinks it's back at a place it's been before). Their system solved it more accurately than previous methods, which often got lost or stuck in wrong answers.
Pose Graph Optimization: A real-world problem of mapping a building where some sensor readings are clearly wrong (outliers). Their system successfully figured out which readings were lies and which were truth, producing a clean map.

The Bottom Line

This paper gives robots a new "brain" that can handle the messy reality of the world. It doesn't just guess; it calculates the exact best answer by keeping track of multiple possibilities simultaneously, and then uses smart pruning to ensure the calculation doesn't take forever. It's like having a detective who can follow every suspect's alibi at once, but knows exactly which ones to drop when the evidence gets too thin.

1. Problem Statement

Robotic estimation tasks (e.g., SLAM, grasping, locomotion) often involve hybrid systems containing both continuous variables (e.g., robot pose, landmark positions) and discrete variables (e.g., data association, contact modes, switching dynamics).

The Challenge: Existing approaches either approximate these interactions (losing accuracy) or rely on specialized solvers limited to specific scenarios. Standard factor graph methods are well-understood for purely continuous (Gaussian) or purely discrete systems, but lack a unified framework for exact inference in hybrid models.
The Goal: To develop a principled framework for exact Maximum A Posteriori (MAP) estimation and marginalization over both discrete and continuous variables within a single factor graph structure, avoiding the local minima issues common in alternating optimization schemes.

2. Methodology

The authors propose a Hybrid Factor Graph framework based on the Conditional Linear Gaussian (CLG) assumption. The core methodology involves three main pillars:

A. Hybrid Factor Graph Definitions

The authors define a new mathematical structure to bridge discrete and continuous domains:

Hybrid Bayes Network (HBN): A directed acyclic graph representing the joint probability $P(X, M, Z)$ , where $X$ are continuous states, $M$ are discrete modes, and $Z$ are measurements.
Hybrid Conditional: A set of conditional components indexed by discrete assignments. For a sensor model, this is a set of Gaussian densities, each corresponding to a specific discrete mode.
Hybrid Factor Graph: Derived by conditioning the HBN on known measurements. It consists of Hybrid Factors, defined as a set of continuous factor components (e.g., Gaussian densities) indexed by discrete modes.
Hybrid Gaussian Factor: A specific factor type where the relationship between continuous variables is linear ( $Ax + \epsilon$ ) with mode-dependent mean and covariance. Crucially, the authors include the normalizer (log-determinant of the covariance) in the factor definition to ensure mathematical correctness during elimination.

B. Hybrid Variable Elimination Algorithm

The paper derives exact inference algorithms by extending the standard Variable Elimination (VE) paradigm to hybrid graphs.

Strong Ordering (CLG Scheme): The algorithm enforces a specific elimination order: all continuous variables are eliminated before discrete variables.
- This ensures the resulting Bayes network has no continuous parents for discrete nodes, maintaining the CLG structure.
- It results in a tree-structured representation of the posterior, facilitating efficient hypothesis management.
Sum-Product Algorithm: Used for computing the full posterior distribution $P(X, M | Z)$ . It involves summing out continuous variables (resulting in new hybrid conditionals) and then summing out discrete variables.
Max-Product Algorithm: Used for MAP estimation. It replaces summation with maximization, yielding a set of functions that allow querying the optimal values for both continuous and discrete variables.
Nonlinear Extension: For nonlinear problems, the framework uses iterative linearization (Taylor expansion) around current estimates, interleaving linearization with variable elimination steps.

C. Managing Tractability (Hypothesis Pruning)

A major bottleneck in hybrid inference is the exponential growth of discrete hypotheses ( $O(K^M)$ ). The authors propose two techniques to ensure tractability:

Hypothesis Pruning: After eliminating variables, the discrete posterior distribution is computed. The algorithm retains only the top $P$ hypotheses (setting others to zero), reducing complexity from exponential to linear ($O(PK)$).
Dead Mode Removal (DMR): If a discrete mode's marginal probability exceeds a high confidence threshold ( $\delta_{DMR}$ ), it is considered "dead" (fixed). The discrete variable is replaced by its fixed assignment, permanently reducing the search space.

3. Key Contributions

Novel Framework: A unified factor graph framework for modeling discrete and continuous variables jointly, supporting arbitrary hybrid relationships (beyond the limited types in previous works like MH-iSAM2).
Exact Inference Algorithms: Rigorous derivations of Hybrid Sum-Product and Hybrid Max-Product algorithms that produce exact posteriors as Hybrid Bayes Networks.
Efficient Representation: A tree-structured representation for hybrid factors that enables $O(\log M)$ access to components and facilitates easy pruning.
Tractability Solutions: The introduction of Hypothesis Pruning and Dead Mode Removal to manage the exponential complexity of discrete variables without sacrificing the global optimum (MAP).
Generalization: The framework subsumes existing approaches (e.g., Switching Linear Dynamical Systems, Mixture-of-Gaussians) and handles nonlinear problems via iterative linearization.

4. Experimental Results

The framework was validated on two large-scale, nonlinear robotics problems with ambiguous measurements:

City10000 Dataset (SLAM):
- Task: 2D SLAM with ambiguous odometry and uncertain loop closures.
- Baselines: Compared against iSAM2 (continuous only), MH-iSAM2 (hybrid), and DC-SAM (alternating optimization).
- Results: The proposed method successfully resolved ambiguities and avoided local minima.
  - MH-iSAM2 showed minor unresolvable errors.
  - DC-SAM fell into a local minimum at 7000 timesteps and failed to recover.
  - Proposed Method: Achieved the most accurate trajectory and was computationally faster than MH-iSAM2, despite lacking iSAM2's incremental design.
Pose Graph Optimization (PGO):
- Task: Optimization with ambiguous edge measurements (inliers vs. outliers) on CSAIL and Intel datasets.
- Baselines: Compared against DCSAM, GNC optimizer, and Levenberg-Marquardt.
- Results: The hybrid approach performed at least as well as the best baselines in terms of rotation and translation errors, demonstrating robustness to outliers and ambiguity.

5. Significance

This work represents a significant advancement in probabilistic robotics estimation by providing a general, exact, and efficient solution for hybrid problems.

Theoretical Impact: It bridges the gap between continuous Gaussian inference and discrete graphical models, offering a unified mathematical foundation (CLG-based) that was previously missing.
Practical Impact: It solves real-world problems where discrete choices (like data association) are critical and cannot be approximated without risk of failure. The ability to perform exact MAP estimation without falling into local minima (a common failure mode in alternating optimization) is a major practical benefit.
Scalability: By introducing effective pruning and mode-fixing strategies, the authors demonstrate that exact hybrid inference is feasible for large-scale, real-world datasets, paving the way for more robust autonomous systems in complex environments.