Continuous-Time Gaussian Process Factors (Factors and Interpolation)

[holmesco]

Summary

This PR introduces the core factors that can be used for continuous-time, Gaussian-process estimation in GTSAM. Note that we restrict our focus to White-Noise-on-Acceleration (WNOA) motion priors, though we have kept the framework as general as possible for expansion to other motion priors in the future.

In particular, this PR introduces the following main features:

• A WNOA motion prior factor
• A WNOA Interpolation class enables (on-manifold) mean and covariance interpolation based on neighboring states.
• A "wrapper class" that allows one to convert standard GTSAM factors defined on interpolated states to an equivalent factor defined on the bordering non-interpolated states.
• A conversion function that transforms an existing factor graph into an equivalent factor graph with a set of interpolated states removed from the main optimization.
• We introduce a simple 1d class called Point1 (used for simplified problems and testing).

Review

Below is a brief file description. The ordering of files also represents our suggested ordering for file review.

File	Description
gtsam/nonlinear/WNOAStateData.h	Lightweight container for timestamped pose/velocity state identifiers
gtsam/nonlinear/WNOAInterpolator.h/.cpp	Core interpolation engine for poses/velocities under WNOA prior
gtsam/nonlinear/WNOAFactor.h	WNOA motion prior factor implementation
gtsam/nonlinear/WNOAInterpFactor.h	Wrapper for interpolating factors at non-estimated states. Also contains functions for converting factor graphs
gtsam/geometry/Point1.h/.cpp	1D point type for testing/examples

Testing

Introduced functionality has been extensively unit tested. Our unit tests were restricted to Vector space and Lie Group variables (Point1, Point2, Point3, Pose2, Pose3)

File	Description
gtsam/nonlinear/tests/testWNOAFactor.cpp	Unit tests for WNOA motion factors
gtsam/nonlinear/tests/testWNOAInterpFactor.cpp	Unit tests for interpolation factors
gtsam/nonlinear/tests/testWNOAInterpolator.cpp	Unit tests for interpolation function
gtsam/geometry/tests/testPoint1.cpp	Unit tests for Point1

Context

This PR is the first (and largest) of two PRs that merge continuous-time gaussian process factors into GTSAM:

1. Factors and Interpolation
2. Factor Graph and LM Optimizer that reduce computation overhead by sharing interpolated states

[Copilot]

Pull request overview

This PR introduces a comprehensive continuous-time Gaussian Process (GP) framework for GTSAM, focusing on White-Noise-on-Acceleration (WNOA) motion priors. The implementation enables GP-based estimation with interpolation support for both vector spaces (Point1/2/3) and Lie groups (Pose2/3).

Changes:

• Introduces Point1 geometry type for 1D testing and examples
• Implements WNOA motion prior factors for continuous-time state estimation
• Adds interpolation infrastructure (Interpolator class) with mean and covariance propagation
• Provides wrapper factors (WNOAInterpFactor) to interpolate measurements at non-estimated states
• Includes comprehensive unit tests covering vector spaces and Lie groups

Reviewed changes

Copilot reviewed 11 out of 11 changed files in this pull request and generated 13 comments.

Show a summary per file

File	Description
gtsam/nonlinear/WNOAStateData.h	Lightweight struct for timestamped pose/velocity state identifiers
gtsam/nonlinear/WNOAInterpolator.h/.cpp	Core interpolation engine implementing WNOA-based pose/velocity interpolation
gtsam/nonlinear/WNOAFactor.h	WNOA motion prior factor connecting successive states
gtsam/nonlinear/WNOAInterpFactor.h	Wrapper factor for measurement interpolation with graph conversion utilities
gtsam/geometry/Point1.h/.cpp	Simple 1D point type for testing and examples
gtsam/nonlinear/tests/testWNOAFactor.cpp	Unit tests for WNOA motion factors
gtsam/nonlinear/tests/testWNOAInterpFactor.cpp	Unit tests for interpolation wrapper factors
gtsam/nonlinear/tests/testWNOAInterpolator.cpp	Unit tests for interpolator functionality
gtsam/geometry/tests/testPoint1.cpp	Unit tests for Point1 geometry

[dellaert]

Awesome! Copilot had some good comments, and I started the CI. I will look at this when both of these things are green :-). I might get to this during the week, otherwise during the weekend. I have a lot of teaching stuff.

[Copilot]

Pull request overview

Copilot reviewed 11 out of 11 changed files in this pull request and generated 12 comments.

[dellaert]

I see CI fails. Please note that your code should only use boost for serialization, and this use should be guarded. Please inspect other headers for how to do this. For everything else we switch to c++17 std::shared etc.

[dellaert]

Same question: ready for my detailed review?

[dellaert]

Seems CI fails on an unused variable. We run with no warnings :-) Do a make check before pushing - saves some CO(2) !

[holmesco]

I don't think make check is catching whatever is causing these errors. It seems to pass even with the commit that was causing errors in the CI. The PR should be ready for your review by EOD. Just checking that our variables are named properly (shouldn't copilot review flag these?)

[holmesco]

Fixed naming conventions in files. @dellaert, should be ready for your review!

[dellaert]

Awesome!
First batch of comments. Many comments apply to all new factors, so do one more round then re-request?

[holmesco]

@dellaert, it seems like our tests are failing when the Cayley map is used for rotations. Specifically, when we define a between factor, the Rot3::CayleyChart::Local function fails because it does not support Jacobians (which we need to properly map the measurement noise).

I can work around this in our tests, but it seems like a fundamental issue. Are there other error functions or factors that do not support Jacobians?

[dellaert]

@dellaert, it seems like our tests are failing when the Cayley map is used for rotations. Specifically, when we define a between factor, the Rot3::CayleyChart::Local function fails because it does not support Jacobians (which we need to properly map the measurement noise).

I can work around this in our tests, but it seems like a fundamental issue. Are there other error functions or factors that do not support Jacobians?

This is probably because you are calling Retract somewhere? You could explicitly call expmap or Expmap. If that’s not possible, tell me where to look :-)

[dellaert]

Would love to move this along- as we are preparing to release 4.3. Was my last comment informative?

[dellaert]

@holmesco I checked out your branch and made a PR against it on your fork. It was mainly naming conventions and some tightening of the APIs, especially when many arguments were passed. So I created this new struct, StateJacobians that makes that a little bit cleaner and eliminates a lot of boilerplate code.

Let me know what you think.

[dellaert]

@holmesco I added one more PR on your fork, eliminating some code duplication. There is two more issues to take care of on your end:

• There is still a CI issue on Windows and Ubuntu. I don't think these have to do with my changes.
• The naming convention for the classes and the files is problematic. We largely follow Google style, and both factors and files should be WnoaXyz not WNOAXyz. See e.g., ImuFactor.

[dellaert]

Almost there :-)

[dellaert]

Awesome. If CI passes I’ll merge.

[dellaert]

Very weird that tests fail on windows.

[holmesco]

Test failure was due to global variable definitions in the test files (bad practice, I know). Added test fixtures and proper scoping. Windows tests are now passing locally.

[dellaert]

@holmesco I noticed you skipped C.I. Could you add a dummy commits to re-run the CI? Maybe format a file with clang-format Google's style. If that is not the case yet for that test file ;-) If you have not formatted with Clang format in general, reformat all the change files in this PR :-).

[holmesco]

@holmesco I noticed you skipped C.I. Could you add a dummy commits to re-run the CI? Maybe format a file with clang-format Google's style. If that is not the case yet for that test file ;-) If you have not formatted with Clang format in general, reformat all the change files in this PR :-).

Woops, should run now. Also we have been formatting with google's style.

[holmesco]

Looks like the windows CI had a network issue when downloading boost. Can we retrigger the CI?

[dellaert]

Done and merged!