HybridNonlinearFactor

A HybridNonlinearFactor represents a factor in a nonlinear hybrid optimization problem. Similar to HybridGaussianFactor, it represents a factor whose behavior depends on the assignment of discrete variables, but here the underlying components are gtsam.NonlinearFactors (specifically, NoiseModelFactors). It inherits from HybridFactor.

Mathematically, it represents a factor $\phi(X, M)$ whose value for a specific discrete assignment $M=m$ is proportional to the likelihood of the continuous variables $X$ under that mode, potentially weighted:

\phi(X, M=m) \propto \exp\left(-E_m - \frac{1}{2} ||h_m(X) - z_m||^2_{\Sigma_m}\right)

(1)

This corresponds to an error (negative log-likelihood) for mode $m$ of:

\text{error}(X, M=m) = E_m + \frac{1}{2} ||h_m(X) - z_m||^2_{\Sigma_m}

(2)

where for each discrete assignment $m$ , we have:

A nonlinear measurement function $h_m(X)$ .
A measurement $z_m$ .
A noise model $\Sigma_m$ defining the squared Mahalanobis distance $|| \cdot ||^2_{\Sigma_m}$ .
An optional scalar term $E_m$ . This term is added to the negative log-likelihood derived from the NoiseModelFactor. It can be used, for example, to incorporate prior probabilities $P(m)$ for different modes by setting $E_m = -\log P(m)$ (plus any constant, as only differences in $E_m$ matter for optimization). A smaller $E_m$ makes mode $m$ relatively more likely, all else being equal.

Internally, it uses a DecisionTree<Key, NonlinearFactorValuePair> to store the different nonlinear factor components (NoiseModelFactor::shared_ptr) and their associated scalar terms (double), indexed by the DiscreteKeys.

import gtsam
import numpy as np

from gtsam import (
    HybridNonlinearFactor,
    BetweenFactorPose2,
    Values,
    DiscreteValues,
    HybridValues,
    Pose2)
from gtsam.symbol_shorthand import X, D

Initialization¶

Initialize with discrete keys and corresponding NoiseModelFactor components, optionally with scalar energies.

# --- Example: Mode-Dependent Odometry ---
dk0 = (D(0), 2) # Binary mode: Slippery (0) or Grippy (1)

# Nonlinear factors (BetweenFactorPose2) for each mode
# Mode 0 (Slippery): Larger noise
noise0 = gtsam.noiseModel.Diagonal.Sigmas([0.5, 0.5, np.radians(10)])
odom0 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise0)

# Mode 1 (Grippy): Smaller noise
noise1 = gtsam.noiseModel.Diagonal.Sigmas([0.1, 0.1, np.radians(1)])
odom1 = BetweenFactorPose2(X(0), X(1), Pose2(1.0, 0, 0), noise1)

# Option A: Just factors (scalar energy E_m = 0)
hnlf_a = HybridNonlinearFactor(dk0, [odom0, odom1])
print("HybridNonlinearFactor (factors only):")
hnlf_a.print()

HybridNonlinearFactor (factors only):
HybridNonlinearFactor
 Hybrid [x0 x1; d0]
HybridNonlinearFactor
 Choice(d0) 
 0 Leaf (0) BetweenFactor(x0,x1)
  measured:  (1, 0, 0)
  noise model: diagonal sigmas [0.5; 0.5; 0.174532925];

 1 Leaf (0) BetweenFactor(x0,x1)
  measured:  (1, 0, 0)
  noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];

# Option B: Factors + scalar energies (e.g., prior on mode)
# P(Slippery)=0.2 -> E0 = -log(0.2)
# P(Grippy)=0.8  -> E1 = -log(0.8)
scalar0 = -np.log(0.2)
scalar1 = -np.log(0.8)
hnlf_b = HybridNonlinearFactor(dk0, [(odom0, scalar0), (odom1, scalar1)])
print("\nHybridNonlinearFactor (factors + scalars):")
hnlf_b.print()


HybridNonlinearFactor (factors + scalars):
HybridNonlinearFactor
 Hybrid [x0 x1; d0]
HybridNonlinearFactor
 Choice(d0) 
 0 Leaf (1.60943791) BetweenFactor(x0,x1)
  measured:  (1, 0, 0)
  noise model: diagonal sigmas [0.5; 0.5; 0.174532925];

 1 Leaf (0.223143551) BetweenFactor(x0,x1)
  measured:  (1, 0, 0)
  noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];

Accessing Components and Error Calculation¶

Similar to HybridGaussianFactor, you can access the underlying decision tree and calculate errors.

# --- Error Calculation ---
# Requires continuous Values and discrete DiscreteValues (or HybridValues)

# Continuous values
cont_vals = Values()
cont_vals.insert(X(0), Pose2(0, 0, 0))
cont_vals.insert(X(1), Pose2(1.1, 0.1, np.radians(2))) # Slightly off

# Discrete assignment
assignment0 = DiscreteValues()
assignment0[D(0)] = 0  # Slip mode

# Method 1: error(Values, DiscreteValues)
err0 = hnlf_b.error(cont_vals, assignment0)
# Expected: scalar0 + 0.5 * || odom0.evaluateError(x0, x1) ||^2
print(f"\nError for D0=0 (Slip): {err0}")

# Method 2: error(HybridValues)
hybrid_vals0 = HybridValues(cv=gtsam.VectorValues(), dv=assignment0, v=cont_vals)
err0_hv = hnlf_b.error(hybrid_vals0)
print(f"Error using HybridValues (Slip): {err0_hv}")

# Check Grippy mode
assignment1 = DiscreteValues()
assignment1[D(0)] = 1  # Grippy mode
err1 = hnlf_b.error(cont_vals, assignment1)
print(f"\nError for D0=1 (Grippy): {err1}\n") # Should be much lower due to smaller noise

# --- errorTree(Values) ---
# Calculates nonlinear error for *all* discrete modes given continuous values
adt = hnlf_b.errorTree(cont_vals)
adt.print("Error Tree")


Error for D0=0 (Slip): 1.6694379124341003
Error using HybridValues (Slip): 1.6694379124341003

Error for D0=1 (Grippy): 3.2231435513142106

Error Tree Choice(d0) 
Error Tree 0 Leaf 1.6694379
Error Tree 1 Leaf 3.2231436

Linearization¶

A key function is linearize, which converts the HybridNonlinearFactor into a HybridGaussianFactor at a given linearization point (continuous Values).

# Using hnlf_b
linearization_point = Values()
linearization_point.insert(X(0), Pose2(0, 0, 0))
linearization_point.insert(X(1), Pose2(1.0, 0, 0)) # Linearize at the expected mean

# Linearize the whole factor (all modes)
hybrid_gaussian_factor = hnlf_b.linearize(linearization_point)
print("\nLinearized HybridGaussianFactor:")
hybrid_gaussian_factor.print()
# Note: The Gaussian components will be JacobianFactors evaluated at the linearization point.
# The scalar energies E_m are carried over.

# Linearize only for a specific mode (useful internally, maybe not direct API)
# assignment = gtsam.DiscreteValues([(D(0), 0)]) # Slippery
# gaussian_factor_mode0 = hnlf_b.linearize(linearization_point, assignment) # Hypothetical API
# print("\nLinearized GaussianFactor for Mode 0:")
# gaussian_factor_mode0.print()


Linearized HybridGaussianFactor:
HybridGaussianFactor

Hybrid [x0 x1; d0]{
 Choice(d0) 
 0 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: 1.23431728

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

}

Restriction (`restrict`)¶

Fixes the discrete variables, returning the corresponding NoiseModelFactor component.

# Using hnlf_b
# Create an empty DiscreteValues and assign D(0) = 1 (Grippy mode)
assignment = DiscreteValues()
assignment[D(0)] = 1
restricted_factor_ptr = hnlf_b.restrict(assignment)

restricted_factor_ptr.print("\nRestricted Factor for D0=1 (Grippy):")


Restricted Factor for D0=1 (Grippy):BetweenFactor(x0,x1)
  measured:  (1, 0, 0)
  noise model: diagonal sigmas [0.1; 0.1; 0.0174532925];

HybridNonlinearFactor

Initialization¶

Accessing Components and Error Calculation¶

Linearization¶

Restriction (restrict)¶

Restriction (`restrict`)¶