WnoaInterpFactor - GTSAM Docs

WnoaInterpFactor<POSE> wraps a measurement factor on an interpolated state and expresses its error using the neighboring estimated WNOA states. This avoids adding every measurement time as a full optimization state.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import gtsam
import numpy as np
from gtsam.symbol_shorthand import L, V, X

Creating an interpolation factor¶

Supply the original factor, ordered estimated/interpolated StateData sets, and acceleration PSD. The wrapped Point2 example turns a prior at t=0.5 into a four-key factor on the bordering pose/velocity states.

left = gtsam.StateData(X(0), V(0), 0.0)
middle = gtsam.StateData(X(1), V(1), 0.5)
right = gtsam.StateData(X(2), V(2), 1.0)

model = gtsam.noiseModel.Isotropic.Sigma(2, 0.1)
inner_factor = gtsam.PriorFactorPoint2(X(1), np.array([0.5, 0.0]), model)
factor = gtsam.WnoaInterpFactorPoint2(
    inner_factor,
    {left, right},
    {middle},
    np.array([0.1, 0.1]),
)
print("factor dimension:", factor.dim())
print("border-state keys:", [gtsam.DefaultKeyFormatter(k) for k in factor.keys()])

Noise and interpolation options¶

fixed_noise_model=False augments measurement uncertainty with interpolation uncertainty. Set it to True when the original factor’s noise model should remain fixed. precomp_interp_mats=True caches interpolation matrices for repeated evaluations.

fixed_noise_factor = gtsam.WnoaInterpFactorPoint2(
    inner_factor,
    {left, right},
    {middle},
    np.array([0.1, 0.1]),
    fixed_noise_model=True,
    precomp_interp_mats=True,
)
print("fixed-noise dimension:", fixed_noise_factor.dim())

Other wrapped state spaces¶

WnoaInterpFactorPoint1, WnoaInterpFactorPoint2, WnoaInterpFactorPoint3, WnoaInterpFactorPose2, and WnoaInterpFactorPose3 share the same construction pattern. In most applications, WnoaFactorGraph utilities create these factors automatically from an ordinary measurement graph.

Source¶

WnoaInterpFactor.h

AI assistance caveat¶

AI was used to help draft this documentation, and inaccuracies could be present.