HybridNonlinearFactorGraph is a HybridFactorGraph specifically for representing nonlinear hybrid optimization problems. It can contain:
gtsam.NonlinearFactor(including derived types likePriorFactor,BetweenFactor)gtsam.DiscreteFactor(includingDecisionTreeFactor,TableFactor)gtsam.HybridNonlinearFactor
This is the graph type you would typically build to model a system with both continuous states (potentially on manifolds) and discrete modes or decisions, where the relationships are nonlinear.
The primary operation performed on a HybridNonlinearFactorGraph is linearize, which converts it into a HybridGaussianFactorGraph suitable for inference using methods like eliminateSequential or eliminateMultifrontal.
import gtsam
import numpy as np
from gtsam import (
HybridNonlinearFactorGraph, HybridNonlinearFactor,
PriorFactorPose2, BetweenFactorPose2, Pose2, Point3,
DecisionTreeFactor, Values, DiscreteValues, HybridValues,
HybridGaussianFactorGraph
)
from gtsam.symbol_shorthand import X, DInitialization and Adding Factors¶
hnfg = HybridNonlinearFactorGraph()
# 1. Add a Nonlinear Factor (Prior)
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, 0.05))
hnfg.add(PriorFactorPose2(X(0), Pose2(0, 0, 0), prior_noise))
# 2. Add a Discrete Factor (Prior on mode)
dk0 = (D(0), 2) # Binary mode
hnfg.add(DecisionTreeFactor([dk0], "0.8 0.2")) # P(D0=0)=0.8
# 3. Add a HybridNonlinearFactor (Mode-dependent odometry)
# Mode 0 (Grippy): Smaller noise
noise0 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.1, 0.1, np.radians(1)))
odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)
# Mode 1 (Slippery): Larger noise
noise1 = gtsam.noiseModel.Diagonal.Sigmas(Point3(0.5, 0.5, np.radians(10)))
odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)
# Assume equal probability for modes within this factor's context
hybrid_odom = HybridNonlinearFactor(dk0, [odom0, odom1])
hnfg.add(hybrid_odom)
print("HybridNonlinearFactorGraph Contents:")
hnfg.print()HybridNonlinearFactorGraph Contents:
HybridNonlinearFactorGraph
size: 3
factor 0: PriorFactor on x0
prior mean: (0, 0, 0)
noise model: diagonal sigmas [0.1; 0.1; 0.05];
factor 1: f[ (d0,2), ]
Choice(d0)
0 Leaf 0.8
1 Leaf 0.2
factor 2: Hybrid [x0 x1; d0]
HybridNonlinearFactor
Choice(d0)
0 Leaf (0) BetweenFactor(x0,x1)
measured: (1, 0, 0)
noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];
1 Leaf (0) BetweenFactor(x0,x1)
measured: (1, 0, 0)
noise model: diagonal sigmas [0.5; 0.5; 0.174532925];
Error Calculation (error, errorTree, discretePosterior)¶
Evaluates the combined error (nonlinear factors) and negative log probability (discrete factors) of the graph.
# Requires HybridValues for evaluation
hybrid_values = HybridValues()
# Continuous/Nonlinear Values
nl_vals = Values()
nl_vals.insert(X(0), Pose2(0.05, 0.05, np.radians(1))) # Near prior mean
nl_vals.insert(X(1), Pose2(1.0, -0.05, np.radians(0))) # Near odom mean
hybrid_values.insert(nl_vals)
# Discrete Values
disc_vals = DiscreteValues()
disc_vals[D(0)] = 0 # Select Grippy mode (lower error)
hybrid_values.insert(disc_vals)
print("\nEvaluating error with HybridValues:")
hybrid_values.print()
# --- error(HybridValues) ---
total_error = hnfg.error(hybrid_values)
print(f"\nTotal graph error: {total_error}")
Evaluating error with HybridValues:
HybridValues:
Continuous: 0 elements
Discrete: (d0, 0)
Nonlinear
Values with 2 values:
Value x0: (gtsam::Pose2)
(0.05, 0.05, 0.0174532925)
Value x1: (gtsam::Pose2)
(1, -0.05, 0)
Total graph error: 1.8480600597872188
# --- errorTree(Values) ---
# Calculates error for Nonlinear & HybridNonlinear factors across all discrete modes
# Ignores purely DiscreteFactors for this calculation.
cont_values = hybrid_values.nonlinear()
error_tree = hnfg.errorTree(cont_values)
print("\nError Tree (Nonlinear errors across modes):")
error_tree.print()
# --- discretePosterior(Values) ---
# Calculates P(M | X=x_cont), normalizing exp(-errorTree)
# Includes contributions from purely discrete factors as well.
discrete_posterior = hnfg.discretePosterior(cont_values)
print("\nDiscrete Posterior P(M | X=x):")
discrete_posterior.print()
Error Tree (Nonlinear errors across modes):
ErrorTree
Choice(d0)
ErrorTree
0 Leaf 1.8480601
ErrorTree
1 Leaf 1.9579211
Discrete Posterior P(M | X=x):
ErrorTree
Choice(d0)
ErrorTree
0 Leaf 0.52743767
ErrorTree
1 Leaf 0.47256233
Linearization¶
The primary operation is linearize, which converts the graph to a HybridGaussianFactorGraph at a given linearization point (continuous Values).
# Linearization point (often the current estimate)
linearization_point = Values()
linearization_point.insert(X(0), Pose2(0, 0, 0))
linearization_point.insert(X(1), Pose2(1, 0, 0))
print("\nLinearizing at:")
linearization_point.print()
# Perform linearization
hgfg = hnfg.linearize(linearization_point)
print("\nResulting HybridGaussianFactorGraph:")
hgfg.print()
# Note: NonlinearFactors become JacobianFactors.
# HybridNonlinearFactors become HybridGaussianFactors.
# DiscreteFactors remain DiscreteFactors.
Linearizing at:
Values with 2 values:
Value x0: (gtsam::Pose2)
(0, 0, 0)
Value x1: (gtsam::Pose2)
(1, 0, 0)
Resulting HybridGaussianFactorGraph:
size: 3
Factor 0
GaussianFactor:
A[x0] = [
10, 0, 0;
0, 10, 0;
0, 0, 20
]
b = [ 0 0 0 ]
No noise model
Factor 1
DiscreteFactor:
f[ (d0,2), ]
Choice(d0)
0 Leaf 0.8
1 Leaf 0.2
Factor 2
HybridGaussianFactor:
Hybrid [x0 x1; d0]{
Choice(d0)
0 Leaf :
A[x0] = [
-10, -0, -0;
0, -10, -10;
-0, -0, -57.2957795
]
A[x1] = [
10, 0, 0;
0, 10, 0;
0, 0, 57.2957795
]
b = [ -0 -0 -0 ]
No noise model
scalar: -5.89658155
1 Leaf :
A[x0] = [
-2, -0, -0;
0, -2, -2;
-0, -0, -5.72957795
]
A[x1] = [
2, 0, 0;
0, 2, 0;
0, 0, 5.72957795
]
b = [ -0 -0 -0 ]
No noise model
scalar: -0.375120634
}
Restriction (restrict)¶
Applies restrict to all factors in the graph for a given discrete assignment.
# Restrict the graph to the 'Grippy' mode (D0=0)
assignment = DiscreteValues()
assignment[D(0)] = 0
restricted_hnfg = hnfg.restrict(assignment)
print("\nRestricted HybridNonlinearFactorGraph (D0=0):")
restricted_hnfg.print()
# Note: The HybridNonlinearFactor is replaced by its D0=0 component (odom0).
# The DiscreteFactor is removed as its variable is assigned.
Restricted HybridNonlinearFactorGraph (D0=0):
HybridNonlinearFactorGraph
size: 2
factor 0: PriorFactor on x0
prior mean: (0, 0, 0)
noise model: diagonal sigmas [0.1; 0.1; 0.05];
factor 1: BetweenFactor(x0,x1)
measured: (1, 0, 0)
noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];