HybridFactor is the abstract base class for all factors involved in hybrid inference problems within GTSAM. It extends the standard gtsam.Factor interface to accommodate factors that may involve both discrete and continuous variables.
Key features:
Manages separate sets of continuous (
gtsam.Key) and discrete (gtsam.DiscreteKey) variable keys.Provides methods to check the type of factor (discrete-only, continuous-only, or truly hybrid).
Defines virtual methods like
errorTreeandrestrictthat derived classes must implement based on their specific nature.gtsam.HybridGaussianConditional(inherits from HybridGaussianFactor)gtsam.HybridConditional(wrapper class)
import gtsam
import numpy as np
from gtsam import HybridGaussianFactor, JacobianFactor, DecisionTreeFactor
from gtsam.symbol_shorthand import X, DBasic Interface¶
All hybrid factors provide methods to access their continuous and discrete keys and check their category.
# Example using a derived class (HybridGaussianFactor)
dk = (D(0), 2) # Discrete key D0 with cardinality 2
gf0 = JacobianFactor(X(0), np.eye(1), np.zeros(1), gtsam.noiseModel.Unit.Create(1))
gf1 = JacobianFactor(X(0), np.eye(1), np.ones(1), gtsam.noiseModel.Unit.Create(1))
hybrid_factor = HybridGaussianFactor(dk, [gf0, gf1])
# Access methods inherited from HybridFactor
print(f"Is Discrete? {hybrid_factor.isDiscrete()}")
print(f"Is Continuous? {hybrid_factor.isContinuous()}")
print(f"Is Hybrid? {hybrid_factor.isHybrid()}")
print(f"Number of Continuous Keys: {hybrid_factor.nrContinuous()}")
print(f"Continuous Keys: {hybrid_factor.continuousKeys()}")
print(f"Discrete Keys: {hybrid_factor.discreteKeys()}") # List of (key, card) pairs
print(f"All Keys (Factor base): {hybrid_factor.keys()}") # Combined keys
hybrid_factor.print("\nHybrid Factor Example (HybridGaussianFactor):")Is Discrete? False
Is Continuous? False
Is Hybrid? True
Number of Continuous Keys: 1
Continuous Keys: [8646911284551352320]
Discrete Keys:
d0 2
All Keys (Factor base): [8646911284551352320, 7205759403792793600]
Hybrid Factor Example (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 = [ 1 ]
Noise model: unit (1)
scalar: 0
}
Virtual Methods (errorTree, restrict)¶
Derived classes implement methods to handle the interaction between discrete and continuous parts.
errorTree(VectorValues): Calculates the error for the continuous part, returning anAlgebraicDecisionTreekeyed by the discrete variables.restrict(DiscreteValues): Creates a new factor (typically continuous-only) by fixing the discrete variables to a specific assignment.
# --- errorTree ---
# Needs continuous values
continuous_values = gtsam.VectorValues()
continuous_values.insert(X(0), np.array([0.5]))
continuous_values.insert(X(1), np.array([1.5]))
adt = hybrid_factor.errorTree(continuous_values)
print("\nError Tree (AlgebraicDecisionTree):")
adt.print()
Error Tree (AlgebraicDecisionTree):
ErrorTree
Leaf 0.125
# --- restrict ---
# Needs a discrete assignment
assignment = gtsam.DiscreteValues()
assignment[D(0)] = 0 # Choose the first Gaussian factor component
restricted_factor = hybrid_factor.restrict(assignment)
# The result is typically a shared_ptr[Factor], often a GaussianFactor
restricted_factor.print("\nRestricted Factor (Assignment D0=0):")
# Try restricting with the other assignment
assignment[D(0)] = 1
restricted_factor_1 = hybrid_factor.restrict(assignment)
restricted_factor_1.print("\nRestricted Factor (Assignment D0=1):")
Restricted Factor (Assignment D0=0):
Continuous [x0]{
Leaf :
A[x0] = [
1
]
b = [ 0 ]
Noise model: unit (1)
scalar: 0
}
Restricted Factor (Assignment D0=1):
Continuous [x0]{
Leaf :
A[x0] = [
1
]
b = [ 1 ]
Noise model: unit (1)
scalar: 0
}