CumulativeSplineTrajectory

Note: AI was used in the creation of this example.

CumulativeSplineTrajectory<T> represents a smooth trajectory whose control points are poses, rotations, or other Lie-group values. It can return ordinary values for numeric timestamps or build differentiable expressions when controls or time are variables in a factor graph.

For a runnable planar example, see CumulativeSplineTrajectoryExample.

Primary contributor: Brett Downing.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

# Install GTSAM from pip if running in Google Colab
try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass  # Not in Colab

import gtsam
import numpy as np

When to use it¶

Use CumulativeSplineTrajectory<T> when:

the control points are Rot2, Rot3, Pose2, Pose3, or another Lie group;
the trajectory must respect the geometry of those values;
a control point or timestamp must remain a GTSAM expression; or
a bounded time window should keep the expression graph sparse.

The class works with relative tangent-space increments rather than weighted sums of the control points themselves.

The cumulative construction¶

For an ordinary scalar sequence, define consecutive changes $\Delta_i=x_i-x_{i-1}$ . A cumulative curve turns each change on smoothly:

x(t)=x_0+\sum_{i=1}^{N-1}c_i(t)\Delta_i,

(1)

where each $c_i(t)$ is a shifted smooth step. Before its support, a step is zero; after its support, it is one. Overlapping steps make the curve and its derivatives smooth.

Applying the construction to a Lie group¶

Poses and rotations cannot be subtracted or averaged as ordinary vectors. The trajectory therefore maps each relative change into a tangent vector,

\xi_i=\operatorname{Log}(T_{i-1}^{-1}T_i),

(2)

accumulates the weighted tangent increments, and maps the result back to the group:

T(t)=T_0\operatorname{Exp}\left(\sum_{i=1}^{N-1}c_i(t)\xi_i\right).

(3)

Differentiating the smooth steps gives tangent-coordinate derivatives without finite differences.

Kernels, density, and windows¶

A KernelBase object defines the smooth step $c_i$ and its analytic derivatives. The default kernels::IrwinHallCDF2 kernel produces a cubic cardinal spline. PiecewisePolynomial stores the exact formulas on each interval.

The trajectory density is the number of control points per unit of the timestamp coordinate. Time derivatives are scaled by the corresponding power of that density.

For expression-valued time, windowStart and windowEnd bound the plausible coordinate. The trajectory then includes only control points whose kernel support overlaps that window, preserving sparsity.

Python usage¶

Python provides the four supported specializations as gtsam.CumulativeSplineTrajectoryRot2, gtsam.CumulativeSplineTrajectoryRot3, gtsam.CumulativeSplineTrajectoryPose2, and gtsam.CumulativeSplineTrajectoryPose3. Construct the specialization matching the control-point type, add the controls in timestamp order, and then sample a numeric timestamp.

This Pose2 trajectory moves smoothly from $y=0$ to $y=2$ and samples its midpoint and tangent rate:

trajectory = gtsam.CumulativeSplineTrajectoryPose2()
for y in (0.0, 0.0, 2.0, 2.0):
    trajectory.addControlPoint(gtsam.Pose2(0.0, y, 0.0))

pose = trajectory.sampleTrajectory(3.5)
tangent_rate = trajectory.sampleTrajectoryDerivative(3.5)

assert pose.equals(gtsam.Pose2(0.0, 1.0, 0.0), 1e-9)
np.testing.assert_allclose(tangent_rate, [0.0, 1.5, 0.0], atol=1e-9)
pose, tangent_rate

((0, 1, 0), array([0. , 1.5, 0. ]))

Pass density to the constructor when control points are not unit-spaced; pass a second padFront boolean when the first control should extend over the kernel’s leading support. sampleTrajectoryDerivative returns tangent coordinates, with the derivative order as its fourth argument after the optional window bounds. For a complete plotted workflow, see the Pose2 example.

C++ usage¶

This trajectory uses pose variables as controls and an expression-valued timestamp:

CumulativeSplineTrajectory<Pose3> trajectory(20.0);
for (size_t i = 0; i < poseCount; ++i) {
  trajectory.addControlPoint(Pose3_(Symbol('p', i)));
}

Double_ time(Symbol('t', 0));
Pose3_ pose = trajectory.sampleTrajectory(time, 4.5, 5.5);
Vector6_ tangentRate =
    trajectory.sampleTrajectoryDerivative(time, 4.5, 5.5, 1);

The window from 4.5 to 5.5 excludes unrelated controls from the resulting expression. A custom kernel must outlive the trajectory; the exported Irwin–Hall kernels have static lifetime.

CumulativeSplineTrajectory Pose2 example
CardinalSplineBasis for scalar or vector coefficients
AsVectorSpace for deliberately adapting a manifold-only component
ProductLieGroup for combining compatible trajectory components