WnoaFactor - GTSAM Docs

WnoaMotionFactor<POSE> imposes a white-noise-on-acceleration prior between two time-stamped pose/velocity states. Python provides concrete factors for Point1, Point2, Point3, Pose2, and Pose3 trajectories.

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 state metadata¶

Each factor connects pose and velocity keys at two times. StateData keeps those keys and timestamps together, while q_psd_diag specifies acceleration power spectral density per tangent dimension.

state0 = gtsam.StateData(X(0), V(0), 0.0)
state1 = gtsam.StateData(X(1), V(1), 1.0)
q_psd_diag = np.array([0.1, 0.1])

factor = gtsam.WnoaMotionFactorPoint2(state0, state1, q_psd_diag)
print("connected keys:", [gtsam.DefaultKeyFormatter(k) for k in factor.keys()])

Motion residual¶

evaluateError(p0, v0, p1, v1) compares endpoint states with the constant-velocity trajectory implied by WNOA. A one-second unit-x motion with equal endpoint velocities has zero residual.

error = factor.evaluateError(
    np.array([0.0, 0.0]),
    np.array([1.0, 0.0]),
    np.array([1.0, 0.0]),
    np.array([1.0, 0.0]),
)
print("motion error:", error)
np.testing.assert_allclose(error, np.zeros(4), atol=1e-12)

Other wrapped state spaces¶

Choose WnoaMotionFactorPoint1, WnoaMotionFactorPoint2, WnoaMotionFactorPoint3, WnoaMotionFactorPose2, or WnoaMotionFactorPose3 to match the stored pose type. The velocity dimension matches that pose’s tangent dimension. Insert the factor into a NonlinearFactorGraph like any other noise-model factor.

Source¶

WnoaFactor.h

AI assistance caveat¶

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