Continuous Time GP Factor Graph

[holmesco]

This introduces a derived factor graph class that is designed to work with the Continuous-Time Gaussian Process factors introduced in #2411. In particular, factor graph optimization leverages shared memory for values and Jacobians of interpolated states that are shared between different factors.

Our custom factor graph, WnoaFactorGraph, defines it's own version of linearize and error that use a cache for common interpolation variables. For these methods to be used by the GTSAM optimization routines (instead of the base class definitions), we implemented the following changes:

• store NonlinearOptimizer graphs via shared_ptr to preserve derived graph dispatch
• Updated optimization routines accordingly (use graph() to recover graph rather than referencing stored property, graph_
• make NonlinearFactorGraph::error/linearize virtual and override them in our new factor graph

[Copilot]

Pull request overview

Adds continuous-time WNOA interpolation-aware infrastructure to reduce redundant computation across factors by sharing interpolated state values/Jacobians during error evaluation and linearization.

Changes:

• Introduces WNOAFactorGraph<PoseType> with interpolation metadata, batching, and optimized error() / linearize().
• Introduces WNOALevenbergMarquardtOptimizer<PoseType> specialized for WNOAFactorGraph.
• Updates interpolation utilities and tests to support returning either NonlinearFactorGraph or WNOAFactorGraph.

Reviewed changes

Copilot reviewed 8 out of 8 changed files in this pull request and generated 7 comments.

Show a summary per file

File	Description
gtsam/nonlinear/tests/testWnoaInterpFactor.cpp	Adds coverage for interpolating into WNOAFactorGraph and optimizing with the new WNOA LM optimizer; updates template call site.
gtsam/nonlinear/WnoaLevenbergMarquardtOptimizer.h	Declares the WNOA-specialized LM optimizer.
gtsam/nonlinear/WnoaLevenbergMarquardtOptimizer.cpp	Implements the WNOA-specialized LM optimizer and explicit instantiations.
gtsam/nonlinear/WnoaInterpolator.h	Renames PSD parameter/member to q_psd_diag_ for clarity/consistency.
gtsam/nonlinear/WnoaInterpolator.cpp	Updates implementation to match the renamed PSD parameter/member.
gtsam/nonlinear/WnoaInterpFactor.h	Generalizes interpolateFactorGraph to return either NonlinearFactorGraph or WNOAFactorGraph.
gtsam/nonlinear/WnoaFactorGraph.h	Declares the new interpolation-aware factor graph class.
gtsam/nonlinear/WnoaFactorGraph.cpp	Implements interpolation caching/batching plus optimized error() / linearize() and explicit instantiations.

[Copilot]

Pull request overview

Copilot reviewed 20 out of 20 changed files in this pull request and generated 4 comments.

[dellaert]

Awesome! LGTM!
Proof in pudding will be a cool Python notebook that runs on colab as well :-)