Detailed Description
class gtsam::WnoaInterpFactor< PoseType >
Wrapper factor that evaluates an inner NoiseModelFactor on states interpolated from neighboring estimated states using a WNOA (white-noise-on-acceleration) Gaussian process interpolator.
Template parameter:
Description: This factor presents an interface identical to a regular factor defined on "interpolated" pose/velocity keys but internally maps those keys to surrounding estimated (outer) states. It:
- builds interpolated Values for requested query times,
- evaluates the inner factor on interpolated states,
- backpropagates inner-factor Jacobians through the interpolation mapping to form Jacobians w.r.t. outer estimated keys,
- optionally augments the measurement noise by adding the interpolation-induced covariance (conditional covariances), resulting in an effective Gaussian noise model used for whitening.
Key features:
- Supports precomputation/reuse via PassedInterpData (interpolated Values, flattened jacobian blocks, and conditional covariances).
- Option to keep the original noise model fixed (no covariance augmentation).
- Optional precomputation of interpolation Lambda/Psi matrices
- Exposes helpers to obtain mappings between interpolated keys and their bordering estimated states.
Typical usage: Construct by providing an inner NoiseModelFactor, ordered sets of estimated and interpolated StateData, and the WNOA PSD vector q_psd_diag. Use as a drop-in factor in a graph that references only estimated keys; call linearize/error/noiseModel as usual. When many factors share interpolation, supply PassedInterpData to avoid redundant work.
Preconditions / Notes:
- Estimated and interpolated StateData must be time-ordered and interpolation query times must lie strictly between bordering estimated times.
- PoseType must be either a Lie-group or vector-space type (checked at compile time).
Public Member Functions | |
| WnoaInterpFactor (const NoiseModelFactor::shared_ptr inner_factor, const std::set< StateData > estimated_states, const std::set< StateData > interp_states, const Eigen::Matrix< double, dim, 1 > q_psd_diag, const bool fixed_noise_model=false, const bool precomp_interp_mats=true) | |
| Construct a WNOA interpolation wrapper factor. | |
| ~WnoaInterpFactor () override | |
| Default destructor. | |
| void | print (const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override |
| Implement functions needed for Testable. | |
| bool | equals (const NonlinearFactor &expected, double tol=1e-9) const override |
| Equality test used by unit tests and debug checks. | |
| Vector | unwhitenedError (const Values &values, OptionalMatrixVecType H=nullptr) const override |
| Compute the unwhitened residual vector for the wrapped inner factor. | |
| std::shared_ptr< GaussianFactor > | linearize (const Values &x) const override |
| Linearize the wrapper factor, computing a JacobianFactor. | |
| std::shared_ptr< GaussianFactor > | linearize (const Values &x, PassedInterpData *passedInterpData) const |
| Linearize using externally provided interpolation data. | |
| double | error (const Values &c) const override |
| Compute the factor error (unwhitened) using the current Values. | |
| double | error (const Values &c, PassedInterpData *passedInterpData) const |
| Error computation accepting precomputed interpolation data. | |
| SharedGaussian | noiseModel (Values &x) const |
| Return an augmented noise model accounting for interpolation. | |
| std::unordered_map< Key, StateData > | getInterpolatedKeys () const |
| Get the mapping from outer Keys to their corresponding interpolated StateData. | |
| std::unordered_map< StateData, std::pair< StateData, StateData > > | getInterpToBorders () const |
| Get the mapping from each interpolated StateData to its bordering states. | |
| Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
| Expose base class overloads for unwhitenedError. | |
| Public Member Functions inherited from gtsam::NoiseModelFactor | |
| NoiseModelFactor () | |
| Default constructor for I/O only. | |
| ~NoiseModelFactor () override | |
| Destructor. | |
| template<typename CONTAINER> | |
| NoiseModelFactor (const SharedNoiseModel &noiseModel, const CONTAINER &keys) | |
| Constructor. | |
| size_t | dim () const override |
| get the dimension of the factor (number of rows on linearization) | |
| const SharedNoiseModel & | noiseModel () const |
| access to the noise model | |
| Vector | unwhitenedError (const Values &x, std::vector< Matrix > &H) const |
| support taking in the actual vector instead of the pointer as well to get access to this version of the function from derived classes one will need to use the "using" keyword and specify that like this: public: using NoiseModelFactor::unwhitenedError; | |
| Vector | whitenedError (const Values &c) const |
| Vector of errors, whitened This is the raw error, i.e., i.e. | |
| Vector | unweightedWhitenedError (const Values &c) const |
| Vector of errors, whitened, but unweighted by any loss function. | |
| double | weight (const Values &c) const |
| Compute the effective weight of the factor from the noise model. | |
| shared_ptr | cloneWithNewNoiseModel (const SharedNoiseModel newNoise) const |
| Creates a shared_ptr clone of the factor with a new noise model. | |
| NonlinearFactor () | |
| Default constructor for I/O only. | |
| template<typename CONTAINER> | |
| NonlinearFactor (const CONTAINER &keys) | |
| Constructor from a collection of the keys involved in this factor. | |
| virtual bool | active (const Values &c) const |
| Checks whether a factor should be used based on a set of values. | |
| virtual void | qcqpFactors (NonlinearFactorGraph *costs, NonlinearEqualityConstraints *constraints, size_t columnDimension=1) const |
| Add this factor's QCQP cost and constraints over matrix-valued QCQP variables with the given column dimension. | |
| virtual shared_ptr | clone () const |
| Creates a shared_ptr clone of the factor - needs to be specialized to allow for subclasses. | |
| virtual shared_ptr | rekey (const std::map< Key, Key > &rekey_mapping) const |
| Creates a shared_ptr clone of the factor with different keys using a map from old->new keys. | |
| virtual shared_ptr | rekey (const KeyVector &new_keys) const |
| Clones a factor and fully replaces its keys. | |
| virtual bool | sendable () const |
| Should the factor be evaluated in the same thread as the caller This is to enable factors that has shared states (like the Python GIL lock). | |
| Public Member Functions inherited from gtsam::Factor | |
| virtual | ~Factor ()=default |
| Default destructor. | |
| bool | empty () const |
| Whether the factor is empty (involves zero variables). | |
| Key | front () const |
| First key. | |
| Key | back () const |
| Last key. | |
| const_iterator | find (Key key) const |
| find | |
| const KeyVector & | keys () const |
| Access the factor's involved variable keys. | |
| const_iterator | begin () const |
| Iterator at beginning of involved variable keys. | |
| const_iterator | end () const |
| Iterator at end of involved variable keys. | |
| size_t | size () const |
| virtual void | printKeys (const std::string &s="Factor", const KeyFormatter &formatter=DefaultKeyFormatter) const |
| print only keys | |
| bool | equals (const This &other, double tol=1e-9) const |
| check equality | |
| KeyVector & | keys () |
| iterator | begin () |
| Iterator at beginning of involved variable keys. | |
| iterator | end () |
| Iterator at end of involved variable keys. | |
Classes | |
| struct | PassedInterpData |
| Container for externally-computed interpolation data. More... | |
Additional Inherited Members | |
| Public Types inherited from gtsam::NoiseModelFactor | |
| typedef std::shared_ptr< This > | shared_ptr |
| Noise model. | |
| Public Types inherited from gtsam::NonlinearFactor | |
| typedef std::shared_ptr< This > | shared_ptr |
| Public Types inherited from gtsam::Factor | |
| typedef KeyVector::iterator | iterator |
| Iterator over keys. | |
| typedef KeyVector::const_iterator | const_iterator |
| Const iterator over keys. | |
| Protected Types inherited from gtsam::NoiseModelFactor | |
| typedef NonlinearFactor | Base |
| typedef NoiseModelFactor | This |
| Protected Types inherited from gtsam::NonlinearFactor | |
| typedef Factor | Base |
| typedef NonlinearFactor | This |
| Protected Member Functions inherited from gtsam::NoiseModelFactor | |
| NoiseModelFactor (const SharedNoiseModel &noiseModel) | |
| Constructor - only for subclasses, as this does not set keys. | |
| Factor () | |
| Default constructor for I/O. | |
| template<typename CONTAINER> | |
| Factor (const CONTAINER &keys) | |
| Construct factor from container of keys. | |
| template<typename ITERATOR> | |
| Factor (ITERATOR first, ITERATOR last) | |
| Construct factor from iterator keys. | |
| template<typename CONTAINER> | |
| static Factor | FromKeys (const CONTAINER &keys) |
| Construct factor from container of keys. | |
| template<typename ITERATOR> | |
| static Factor | FromIterators (ITERATOR first, ITERATOR last) |
| Construct factor from iterator keys. | |
| Protected Attributes inherited from gtsam::NoiseModelFactor | |
| SharedNoiseModel | noiseModel_ |
| Protected Attributes inherited from gtsam::Factor | |
| KeyVector | keys_ |
| The keys involved in this factor. | |
Constructor & Destructor Documentation
◆ WnoaInterpFactor()
|
inline |
Construct a WNOA interpolation wrapper factor.
The wrapper maps an inner factor that references "interpolated" states onto the outer estimated states used by the main solve.
- Parameters
-
inner_factor Factor defined over (possibly) interpolated keys. estimated_states Ordered set of estimated StateData. These are the states that are directly optimized over and serve as borders for interpolation. interp_states Ordered set of states to interpolate (states at query times). q_psd_diag Diagonal PSD vector for the WNOA motion prior. fixed_noise_model If true, do not augment the noise measurement model with GP interpolation process noise. This is faster, but less accurate, as it ignores interpolation uncertainty. precomp_interp_mats If true, precompute Lambda/Psi matrices for each interpolated state.
Member Function Documentation
◆ equals()
|
inlineoverridevirtual |
Equality test used by unit tests and debug checks.
Reimplemented from gtsam::NoiseModelFactor.
◆ error() [1/2]
|
inlineoverridevirtual |
Compute the factor error (unwhitened) using the current Values.
This variant computes the scalar loss by forming the residual vector and evaluating the (possibly augmented) noise model.
Reimplemented from gtsam::NoiseModelFactor.
◆ error() [2/2]
|
inline |
◆ getInterpolatedKeys()
|
inline |
Get the mapping from outer Keys to their corresponding interpolated StateData.
◆ getInterpToBorders()
|
inline |
Get the mapping from each interpolated StateData to its bordering states.
◆ linearize() [1/2]
|
inlineoverridevirtual |
Linearize the wrapper factor, computing a JacobianFactor.
When interpolation increases measurement uncertainty, this function updates the noise model on-the-fly by computing the inner-factor Jacobians with respect to the outer estimated states and the interpolated conditional covariances.
Reimplemented from gtsam::NoiseModelFactor.
◆ linearize() [2/2]
|
inline |
Linearize using externally provided interpolation data.
This overload accepts a PassedInterpData pointer which can contain precomputed interpolated Values, flattened jacobians and conditional covariances. Using externally-computed data avoids duplicate work when many factors share interpolation computations.
◆ noiseModel()
|
inline |
Return an augmented noise model accounting for interpolation.
Computes inner-factor jacobians and conditional covariances, then uses them to form the effective Gaussian noise model that should be used for whitening the residuals. If fixed_noise_model_ is set this returns the base factor's Gaussian noise model unchanged.
◆ print()
|
inlineoverridevirtual |
Implement functions needed for Testable.
Print factor information for debugging.
Prints the outer keys this wrapper connects and forwards a print of the inner factor for detailed inspection.
Reimplemented from gtsam::NoiseModelFactor.
◆ unwhitenedError()
|
inlineoverridevirtual |
Compute the unwhitened residual vector for the wrapped inner factor.
This is the outer-facing residual (before whitening) obtained by evaluating the inner factor on interpolated states and mapping the resulting error back to the outer state ordering.
Implements gtsam::NoiseModelFactor.
The documentation for this class was generated from the following file:
- /tmp/gtsam-4.3.0-doxygen.rsXPUS/source/gtsam/nonlinear/WnoaInterpFactor.h
