WnoaFactorGraph<POSE> is the expression-factor graph produced when factors on interpolated states are rewritten in terms of neighboring estimated WNOA states. Python exposes one specialization for each wrapped point or pose trajectory type.
import gtsam
import numpy as np
from gtsam.symbol_shorthand import L, V, XEstimated and interpolated states¶
The estimated states are optimization variables. An interpolated state lies between them in time and is mapped to its left and right borders.
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)
q_psd_diag = np.array([0.1, 0.1])
interp_map = {middle: (left, right)}
empty_wnoa_graph = gtsam.WnoaFactorGraphPoint2(interp_map, q_psd_diag)
print("initial factors:", empty_wnoa_graph.size())Rewriting a factor graph¶
interpolateWnoaFactorGraphPoint2() replaces factors involving interpolated poses with WnoaInterpFactorPoint2 objects and adds the required WNOA motion prior. The returned specialized graph remains a NonlinearFactorGraph.
measurement_graph = gtsam.NonlinearFactorGraph()
model = gtsam.noiseModel.Isotropic.Sigma(2, 0.1)
measurement_graph.add(
gtsam.PriorFactorPoint2(X(1), np.array([0.5, 0.0]), model)
)
wnoa_graph = gtsam.interpolateWnoaFactorGraphPoint2(
measurement_graph, {left, right}, {middle}, q_psd_diag
)
print("rewritten factors:", wnoa_graph.size())
assert wnoa_graph.size() == 2Related utilities and specializations¶
interpolateFactorGraph* returns a plain nonlinear graph, while updateInterpValues* reconstructs interpolated values after solving and updateInterpValuesWithCovariance* also returns conditional covariances. The graph classes are WnoaFactorGraphPoint1, WnoaFactorGraphPoint2, WnoaFactorGraphPoint3, WnoaFactorGraphPose2, and WnoaFactorGraphPose3.
Source¶
AI assistance caveat¶
AI was used to help draft this documentation, and inaccuracies could be present.