WnoaStateData - GTSAM Docs

StateData identifies one continuous-time trajectory state by its pose key, velocity key, and timestamp. WNOA factors use it to keep temporal ordering and variable identities consistent.

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 descriptors¶

Pose and velocity keys must be distinct GTSAM keys. Time is a floating-point value in the application’s chosen unit, used consistently with acceleration PSD.

state0 = gtsam.StateData(X(0), V(0), 0.0)
state1 = gtsam.StateData(X(1), V(1), 0.5)
print("pose key:", gtsam.DefaultKeyFormatter(state1.pose))
print("velocity key:", gtsam.DefaultKeyFormatter(state1.velocity))
print("time:", state1.time)

Ordering and sets¶

StateData is hashable, so Python sets can be passed directly to WNOA wrapper functions. The C++ conversion orders those sets by time, with keys providing deterministic tie-breaking; Python code can sort explicitly with a time key when it needs to display that order.

states = {state1, state0}
ordered_states = sorted(states, key=lambda state: state.time)
print("ordered times:", [state.time for state in ordered_states])
assert [state.time for state in ordered_states] == [0.0, 0.5]

Using descriptors in a motion factor¶

The descriptor does not store pose or velocity values. Those live in a Values object under the referenced keys; StateData only tells WNOA factors where and when to find them.

values = gtsam.Values()
values.insert(state0.pose, np.array([0.0, 0.0]))
values.insert(state0.velocity, np.array([1.0, 0.0]))
print("pose value:", values.atPoint2(state0.pose))
print("velocity value:", values.atVector(state0.velocity))

Source¶

WnoaStateData.h

AI assistance caveat¶

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