Adds a Lie-Group aware differentiable spline curve implementation #2212

BrettRD · 2025-08-08T15:31:50Z

This PR follows a discussion on the mailing list about clock recovery for continuous-time trajectories.
I got the ok from my team to push the abstract features upstream, and I've re-written the implementation to be more versatile and line up better with existing literature.

I'm not ready to merge just yet (the header file preambles are conspicuously missing and the comments are still a bit of a mess), but I'd like some feedback for where the files should live, name-spacing, etc.
I'd also like some guidance on how best to expose Expression based features to the python bindings.

Basic usage:

  // instantiate the expression factory (uses cubic splines by default)
  TrajectoryModel<Pose3> model();

  // add some control points
  for(int i=0;i<10;i++){
    model.add_control_point(Pose3_(Key('p', i)));
  }

  size_t n; 
  std::vector<Pose3> measurements;

  // construct an Expression that samples the trajectory at the Nth timestamp
  Pose3_ sample = model.sample_trajectory(Double_(Key('t', n)));

  // construct the factor
  f = ExpressionFactor(
    /*noise model*/
    measurement[n],
    sample,
  );
 // add f to the factor graph

Features:

Differentiable cardinal splines up to 7th order (higher orders are easy to add).
Nth derivatives of splines exposed as Expressions (necessarily including Jacobians)
spline abstraction also covers FIR filters
computational complexity can use lower bound of O(spline_order+1)
unknown timestamps represented with computational complexity of O(spline_order + timestamp_search_window)
Template type for CartesianProducts of existing manifolds (like a std::pair but inheriting from LieGroup).
Template type AsVectorSpace for extrapolating VectorSpace features from Manifold traits
N-dimensional cardinal spline meshes (surface patches are a happy accidental feature)

Miscellany:

Expression template for expmap (probably needs a quick test)

unused features that can be be removed from the PR:

Irwin Hall Probability Density Function (only the CDF is required)
Expression templates extended for operators on Double_ types

Implementation

Spline implementation follows a CVPR paper where the weights are pre-computed, and the weighted sum happens in the Lie Algebra using the first point (of the points in the window) as the datum.
This results in an execution that can act on a minimal number of control points, considers each point once, and has a trivial derivative propagation rule.
The weights for (uniformly sampled) splines are captured by the Irwin Hall Cumulative Probability Density Function.
The (uniformly sampled) weighted sum happens to be structurally equivalent to a Finite Impulse Response (FIR) Filter.

Questions:

Do the navigation and IMU features set down a type convention for angular velocity? and does it differ from traits<Rot3>::TangentVector?
traits<T>::TangentVector is a sensible type for first derivatives of T, but is it an adequate type for second and Nth derivatives?
The spline curve system is implemented under a type called TrajectoryModel, but it's capable of defining surface-patches. what's a better name for it?
What wrapping API would be required to use this from Python?
- add keys to serve as control points?
- generate factors?

Missing Tests:

cross-check against the fixed-order implementation by @koide3
~~Analytic derivatives of Rot3 spline curves match numerical derivatives to Nth order~~
Let me know if there's anything that you notice

Missing Examples:

Factor graph associating raw IMU data directly to derivatives of the trajectory in order to recover the time offset between non-monotonic, asynchronous, Pose3 and IMU data-sources Furgale 2013

Missing Features:

Cal3 doesn't have an expmap, so powered zoom lenses can't be built into a coordinated motion model using TrajectoryModel<CartesianProduct<Pose3, Cal3> > A Riemannian embedding using the localCoordinates call would be (barely) enough like a logmap for this spline to function. See AsVectorSpace

dellaert · 2025-08-09T18:09:54Z

@BrettRD this is amazing!

Here are my overall (Codex-assisted!) comments without yet trying to understand the code in detail. The first two comments are obvious/easy as they relate to style/comment currently requested for GTSAM PRs. The third one is way less obvious and might need a face-to-face meeting so I can help think through it with you:

Code style

the files can go straight in the basis folder, not a big fan of further cascading the folder structure
Please use PascalCase for class names and camelCase for methods and member variables to match the rest of the library. It’s basically Google but no snake_case
Ensure all files pass clang-format with the repository’s configuration .clang-format (based on Google style). Running clang-format -i on changed files before committing will keep the style consistent.

Documentation

Please add Doxygen comments to new public classes, methods, and major typedefs in header files. Briefly describe the intent, parameters, and return values; this improves discoverability and helps future contributors.

The Basis framework

Primer on the gtsam/basis framework

Basis.h defines a CRTP base class Basis that supplies utility functors:

CalculateWeights / DerivativeWeights (static in each derived class) compute the basis-function values and derivatives for a given order N at point x.
EvaluationFunctor and VectorEvaluationFunctor evaluate a basis at runtime and return optional Jacobians.
WeightMatrix builds matrices of basis weights for multiple evaluation points.
Specific bases (e.g., Chebyshev1Basis, Chebyshev2Basis, FourierBasis) implement these static methods and typedef Parameters to encode their coefficient structure.
BasisFactors.h wraps these functors into factor graph factors (EvaluationFactor, VectorEvaluationFactor, etc.) that enforce measurements on scalar or vector-valued parameterizations.

Integrating your spline work

If the spline uses a new polynomial or kernel basis, create a MySplineBasis class inheriting from Basis and implement CalculateWeights and DerivativeWeights.
Expose evaluation through the provided functors so you can reuse BasisFactors or FitBasis for optimization and interpolation.
Reuse existing naming and spacing conventions (e.g., MySplineBasis::Parameters) and keep the interface consistent with current basis classes—this will allow existing tooling (factors, tests, potential Python bindings) to work with minimal adjustments.

That being said, we should have a chat about what that might look like here, and whether it fits your code.

dellaert · 2025-08-09T18:16:34Z

PS calibrations objects satisfy the manifold traits, but not (yet) Lie group. It does not make obvious sense to “compose” 2 calibration objects, and reading the CVPR paper the cumulative spline representation seems to demand composition and inverse.

You could, in an application, “hack” this by deriving from a Calibration type, adding the composition / addition, and make that a Lie group / vector space, with corresponding traits.

BrettRD · 2025-08-11T05:12:26Z

I'll have another read of the basis framework. On a first read, it looked like a lot of machinery to handle butterfly calculations in bounded domains and it wasn't clear if any of it could be adapted to single evaluations of FIR taps.

For Cal3, I suspect every choice of manifold embedding is going to be equally arbitrary, so I'm tempted to make a simple CRTP adaptor to hack Vector-space traits onto the side of Manifold, based on a naive Reimannian embedding.
That way, Manifold classes can remain only Manifold, and users who to need to pretend it's a more sophisticated manifold can apply a wrapping of Reimannain<Cal3>, making the hackyness an explicit user choice, while still being a well-behaved and easy-to-use type.

dellaert · 2025-08-11T10:29:00Z

I'll have another read of the basis framework. On a first read, it looked like a lot of machinery to handle butterfly calculations in bounded domains and it wasn't clear if any of it could be adapted to single evaluations of FIR taps.

Indeed, Basis is designed to create functors (in BasisFactors.h) at single "taps" in bounded domains. But, if you have a trajectory with $t\in[a,b]$, you do have a bounded domain. I think the issue is that CalculateWeights should yield a sparse matrix in the case of splines.

For Cal3, I suspect every choice of manifold embedding is going to be equally arbitrary, so I'm tempted to make a simple CRTP adaptor to hack Vector-space traits onto the side of Manifold, based on a naive Riemannian embedding. That way, Manifold classes can remain only Manifold, and users who to need to pretend it's a more sophisticated manifold can apply a wrapping of Riemannian<Cal3>, making the hackyness an explicit user choice, while still being a well-behaved and easy-to-use type.

I like it. Maybe AsVectorSpace ? It definitely "works" - I think we had addition at one point - but it felt wrong. They are not even groups. For the simplest $K$ calibration, they are all affine transforms, but already $K^{-1}$ is not a valid calibration.

BrettRD · 2025-08-13T03:59:00Z

I like it. Maybe AsVectorSpace ? It definitely "works" - I think we had addition at one point - but it felt wrong. They are not even groups. For the simplest K calibration, they are all affine transforms, but already K − 1 is not a valid calibration.

Something like that. I really ought to split this PR

The third one is way less obvious and might need a face-to-face meeting so I can help think through it with you

I sent an email to your gatech address, is that the best way to reach you?

…tesianProduct between multiplicative and additive groups.

Copilot

Pull Request Overview

This PR introduces a Lie-Group aware differentiable spline curve implementation to enable clock recovery for continuous-time trajectories. The implementation follows CVPR 2020 research on efficient derivative computation for cumulative B-splines on Lie Groups.

Key changes:

Adds TrajectoryModel template class for cardinal spline interpolation up to 7th order with Expression-based API
Implements CartesianProduct and AsVectorSpace template utilities for combining manifolds
Provides Irwin Hall polynomial kernels for spline basis functions
Extends Expression templates with expmap function and scalar multiplication operators

Reviewed Changes

Copilot reviewed 16 out of 16 changed files in this pull request and generated 10 comments.

Show a summary per file

File	Description
gtsam/slam/expressions.h	Adds expmap Expression template and scalar multiplication operators
gtsam/geometry/CartesianProduct.h	Template for combining unrelated manifolds into single data structure
gtsam/geometry/AsVectorSpace.h	Adapter to add VectorSpace traits to Manifold-only classes
gtsam/basis/polynomial/TrajectoryModel.h	Main spline interpolation Expression factory for continuous trajectories
gtsam/basis/polynomial/PiecewisePolynomial.h	Base class for piecewise polynomial kernel functions
gtsam/basis/polynomial/IrwinHall.h	Header declaring Irwin Hall polynomial coefficients
gtsam/basis/polynomial/IrwinHall.cpp	Irwin Hall PDF coefficient implementations
gtsam/basis/polynomial/IrwinHallCDF.cpp	Irwin Hall CDF coefficient implementations
gtsam/basis/polynomial/kernels.h	Abstract base class for convolutional kernel functions
examples/ClockRecoveryContinuousTrajectory.cpp	Example demonstrating clock recovery with asynchronous sensors
Test files	Comprehensive unit tests for all new functionality

Comments suppressed due to low confidence (2)

gtsam/basis/polynomial/TrajectoryModel.h:134

Accessing 'kernel' as a public member when it's declared as 'const KernelBase& kernel_;' - should use 'trajectory_model.kernel_' instead.

gtsam/basis/polynomial/TrajectoryModel.h:143

Accessing 'kernel' as a public member when it should be 'accel_clock_model.kernel_' based on the class definition.

    Double_ timestamp,

_{Tip: Customize your code reviews with copilot-instructions.md. Create the file or learn how to get started.}

Copilot · 2025-08-21T12:00:00Z

gtsam/geometry/tests/testCartesianProduct.cpp

@@ -0,0 +1,148 @@
+/**
+ * @file    testCartesinProduct.cpp


Filename in comment has a typo: 'testCartesinProduct.cpp' should be 'testCartesianProduct.cpp'

Suggested change

* @file testCartesinProduct.cpp

* @file testCartesianProduct.cpp

Copilot · 2025-08-21T12:00:01Z

gtsam/geometry/tests/testAsVectorSpace.cpp

@@ -0,0 +1,157 @@
+/**
+ * @file    testCartesinProduct.cpp


Wrong filename in comment: should be 'testAsVectorSpace.cpp' not 'testCartesinProduct.cpp'

Suggested change

* @file testCartesinProduct.cpp

* @file testAsVectorSpace.cpp

Copilot · 2025-08-21T12:00:01Z