HybridGaussianFactor

A HybridGaussianFactor represents a factor where the underlying probability distribution is Gaussian, but which Gaussian distribution applies depends on the assignment of a set of discrete variables. It inherits from HybridFactor.

Mathematically, it represents a factor $\phi(X, M)$ over continuous variables $X$ and discrete variables $M$ , defined as:

\phi(X, M=m) \propto \exp(-E_m - \frac12 ||A_m X - b_m||^2)

(1)

where for each discrete assignment $m$ , we have a standard Gaussian factor defined by $A_m$ , $b_m$ (and an associated noise model, implicitly defining the Mahalanobis distance), plus an optional scalar energy term $E_m$ .

Internally, it uses a DecisionTree<Key, GaussianFactorValuePair> to store the different Gaussian factor components (GaussianFactor::shared_ptr) and their associated scalar energies (double), indexed by the DiscreteKeys.

These factors typically arise from:

Linearizing a HybridNonlinearFactor.
Representing sensor models with discrete operating modes (e.g., a sensor that works differently in rain vs. sun).
As intermediate results during hybrid elimination.

import gtsam
import numpy as np

from gtsam import HybridGaussianFactor, JacobianFactor, DiscreteKeys, DiscreteValues
from gtsam.symbol_shorthand import X, D

Initialization¶

Can be initialized with one or more discrete keys and the corresponding Gaussian factor components.

# --- Example 1: Single Discrete Key ---
dk0 = (D(0), 2) # Binary key D0

# Gaussian factor for mode D0=0: N(X(0); mean=0, sigma=1) -> ||x0 - 0||^2 / 2
gf0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Isotropic.Sigma(1, 1.0))
# Gaussian factor for mode D0=1: N(X(0); mean=5, sigma=2) -> ||x0 - 5||^2 / (2*4)
gf1 = JacobianFactor(X(0), np.eye(1), np.array([5.0]), gtsam.noiseModel.Isotropic.Sigma(1, 2.0))

# Option A: Just factors (scalar energy E_m defaults to 0)
hgf_a = HybridGaussianFactor(dk0, [gf0, gf1])
print("HybridGaussianFactor (factors only):")
hgf_a.print()

HybridGaussianFactor (factors only):
HybridGaussianFactor

Hybrid [x0; d0]{
 Choice(d0) 
 0 Leaf :
  A[x0] = [
	1
]
  b = [ 0 ]
  Noise model: unit (1) 
scalar: 0

 1 Leaf :
  A[x0] = [
	1
]
  b = [ 5 ]
isotropic dim=1 sigma=2
scalar: 0

}

# Option B: Factors with scalar energies E_m
# Example: Add -log P(D0=0)= -log(0.7) and -log P(D0=1)=-log(0.3)
scalar0 = -np.log(0.7)
scalar1 = -np.log(0.3)
hgf_b = HybridGaussianFactor(dk0, [(gf0, scalar0), (gf1, scalar1)])
print("\nHybridGaussianFactor (factors + scalars):")
hgf_b.print()


HybridGaussianFactor (factors + scalars):
HybridGaussianFactor

Hybrid [x0; d0]{
 Choice(d0) 
 0 Leaf :
  A[x0] = [
	1
]
  b = [ 0 ]
  Noise model: unit (1) 
scalar: 0.356675

 1 Leaf :
  A[x0] = [
	1
]
  b = [ 5 ]
isotropic dim=1 sigma=2
scalar: 1.20397

}

In C++ you can create HybridGaussianFactors with multiple discrete keys as well, but that capability is not yet exposed in the wrapper.

Accessing Components¶

You can retrieve the underlying decision tree structure and select a specific Gaussian factor component based on a discrete assignment.

# Using hgf_b from Example 1B

# Get the factor/scalar pair for a specific assignment
assignment0 = gtsam.DiscreteValues()
assignment0[D(0)] = 0
factor_pair0 = hgf_b(assignment0) # Calls operator()
print("\nFactor/Scalar pair for D0=0:")
factor_pair0[0].print("Factor:") # GaussianFactor component
print(f"Scalar: {factor_pair0[1]}") # Scalar energy term

assignment1 = gtsam.DiscreteValues()
assignment1[D(0)] = 1
factor_pair1 = hgf_b(assignment1)
print("\nFactor/Scalar pair for D0=1:")
factor_pair1[0].print("Factor:")
print(f"Scalar: {factor_pair1[1]}")


Factor/Scalar pair for D0=0:
Factor:
  A[x0] = [
	1
]
  b = [ 0 ]
  Noise model: unit (1) 
Scalar: 0.35667494393873245

Factor/Scalar pair for D0=1:
Factor:
  A[x0] = [
	1
]
  b = [ 5 ]
isotropic dim=1 sigma=2
Scalar: 1.2039728043259361

Error Calculation (`error`, `errorTree`)¶

error(HybridValues): Calculates the total error $E_m + \frac{1}{2} ||A_m X - b_m||^2$ for the specified continuous values $X$ and discrete assignment $M=m$ .
errorTree(VectorValues): Calculates the Gaussian error term $\frac{1}{2} ||A_m X - b_m||^2$ for the given continuous values $X$ across all discrete assignments $m$ , returning the results as an AlgebraicDecisionTree indexed by the discrete keys $M$ . It also adds the scalar energy $E_m$ to each leaf.

# Using hgf_b = HybridGaussianFactor(dk0, [(gf0, scalar0), (gf1, scalar1)])
# gf0: N(X(0); 0, 1), scalar0 = -log(0.7)
# gf1: N(X(0); 5, 2), scalar1 = -log(0.3)

# --- error(HybridValues) ---
hybrid_vals0 = gtsam.HybridValues()
hybrid_vals0.insert(X(0), np.array([0.1])) # Continuous value
hybrid_vals0.insert(D(0), 0)              # Discrete assignment

err0 = hgf_b.error(hybrid_vals0)
# Expected: scalar0 + 0.5 * ||0.1 - 0||^2 / 1^2
print(f"\nError for X0=0.1, D0=0: {err0} (Expected ~ {-np.log(0.7) + 0.5*0.1**2})")

hybrid_vals1 = gtsam.HybridValues()
hybrid_vals1.insert(X(0), np.array([4.0])) # Continuous value
hybrid_vals1.insert(D(0), 1)              # Discrete assignment

err1 = hgf_b.error(hybrid_vals1)
# Expected: scalar1 + 0.5 * ||4.0 - 5.0||^2 / 2^2
print(f"Error for X0=4.0, D0=1: {err1} (Expected ~ {-np.log(0.3) + 0.5*(-1.0)**2 / 4.0})")

# --- errorTree(VectorValues) ---
cont_vals = gtsam.VectorValues()
cont_vals.insert(X(0), np.array([1.0])) # Evaluate Gaussian part at X0=1.0

adt = hgf_b.errorTree(cont_vals)
# Leaf 0 (D0=0): scalar0 + 0.5 * ||1.0 - 0||^2 / 1^2 = scalar0 + 0.5
# Leaf 1 (D0=1): scalar1 + 0.5 * ||1.0 - 5.0||^2 / 2^2 = scalar1 + 0.5 * 16 / 4 = scalar1 + 2.0
print("\nError Tree for X0=1.0:")
adt.print()
print(f"(Expected Leaf 0 ~ {-np.log(0.7) + 0.5})")
print(f"(Expected Leaf 1 ~ {-np.log(0.3) + 2.0})")


Error for X0=0.1, D0=0: 0.36167494393873245 (Expected ~ 0.36167494393873245)
Error for X0=4.0, D0=1: 1.3289728043259361 (Expected ~ 1.3289728043259361)

Error Tree for X0=1.0:
ErrorTree
 Choice(d0) 
ErrorTree
 0 Leaf 0.85667494
ErrorTree
 1 Leaf 3.2039728
(Expected Leaf 0 ~ 0.8566749439387324)
(Expected Leaf 1 ~ 3.203972804325936)

Restriction (`restrict`)¶

Fixes the discrete variables according to a given assignment, returning the corresponding GaussianFactor component (as a gtsam.Factor::shared_ptr).

# Using hgf_b
restricted_factor_ptr = hgf_b.restrict(assignment1)# Choose mode 1

if restricted_factor_ptr:
    restricted_factor_ptr.print("\nRestricted Factor for D0=1:")
    # Should be equivalent to gf1: N(X(0); 5, 2)
else:
    print("\nRestriction failed.")


Restricted Factor for D0=1:
Continuous [x0]{
 Leaf :
  A[x0] = [
	1
]
  b = [ 5 ]
isotropic dim=1 sigma=2
scalar: 1.20397

}

HybridGaussianFactor

Initialization¶

Accessing Components¶

Error Calculation (error, errorTree)¶

Restriction (restrict)¶

Error Calculation (`error`, `errorTree`)¶

Restriction (`restrict`)¶