CumulativeSplineTrajectory: A smooth Pose2 example

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

GTSAM supports differentiable cumulative Lie-group splines with gtsam::CumulativeSplineTrajectory<T> from gtsam/basis/CumulativeSplineTrajectory.h. In this example, the trajectory controls are planar gtsam::Pose2 objects from gtsam/geometry/Pose2.h. The example turns sparse control poses into a smooth trajectory and visualizes analytic tangent derivatives.

A mobile robot may be estimated at only a few times while a camera, controller, or visualization needs poses in between. In this example we build a smooth planar trajectory from six control poses and inspect how cumulative cubic blending works.

No spline background is required. We will sample the trajectory, see why pose interpolation uses Pose2.Logmap and Pose2.Expmap, and inspect the resulting curve and tangent derivatives. For ordinary scalar or vector basis weights, use the separate CardinalSplineBasis example.

Primary contributor: Brett Downing.

Contents¶

What we are building
1. Sample the Pose2 trajectory
2. Why poses use tangent-space increments
3. Inspect the smooth trajectory
4. Ask the trajectory for analytic derivatives
Python and expression-valued C++ APIs

import gtsam
import numpy as np
import plotly.graph_objects as go
from plotly.subplots import make_subplots

What we are building¶

The six poses below are control poses: they shape the curve, but a B-spline is not required to pass through each one. A cumulative spline works with the change from one pose to the next. It turns each change on gradually and overlaps neighboring changes to avoid sharp corners.

CumulativeSplineTrajectoryPose2 is the Pose2 instantiation of CumulativeSplineTrajectory<T>. This notebook stays with the Lie-group trajectory API; it does not use linear basis weights to blend poses.

control_poses = [
    gtsam.Pose2(0.0, 0.0, 0.00),
    gtsam.Pose2(1.3, 0.2, 0.12),
    gtsam.Pose2(2.6, 0.9, 0.35),
    gtsam.Pose2(3.5, 2.0, 0.70),
    gtsam.Pose2(3.7, 3.3, 1.10),
    gtsam.Pose2(3.0, 4.3, 1.45),
]

trajectory_model = gtsam.CumulativeSplineTrajectoryPose2()
for pose in control_poses:
    trajectory_model.addControlPoint(pose)

print("index    x      y    theta (rad)")
for index, pose in enumerate(control_poses):
    print(f"{index:>3}   {pose.x():5.2f}  {pose.y():5.2f}    {pose.theta():5.2f}")

index    x      y    theta (rad)
  0    0.00   0.00     0.00
  1    1.30   0.20     0.12
  2    2.60   0.90     0.35
  3    3.50   2.00     0.70
  4    3.70   3.30     1.10
  5    3.00   4.30     1.45

1. Sample the `Pose2` trajectory¶

A CumulativeSplineTrajectoryPose2 works with relative pose increments, not dense weights on the poses themselves. We ask the trajectory for each sample and collect the returned Pose2 values for plotting.

end_time = len(control_poses) + 2.0
sample_times = np.linspace(0.0, end_time, 501)
trajectory = [
    trajectory_model.sampleTrajectory(float(time))
    for time in sample_times
]

2. Why poses use tangent-space increments¶

Ordinary subtraction does not make sense for poses. Internally, CumulativeSplineTrajectoryPose2 computes a relative pose and applies Pose2.Logmap to obtain a three-vector $\xi_i=[v_x,v_y,\omega]$ . The class blends those vectors and uses Pose2.Expmap to return a valid pose:

\xi_i=\operatorname{Log}(T_{i-1}^{-1}T_i), \qquad T(t)=T_0\operatorname{Exp}\left(\sum_i c_i(t)\xi_i\right).

(1)

Read this from left to right: measure each step, smoothly scale the steps, add them in a local coordinate system, then apply the accumulated motion to the first pose.

assert all(isinstance(pose, gtsam.Pose2) for pose in trajectory)

3. Inspect the smooth trajectory¶

The circles are the control poses and the blue curve is the cumulative spline. Short line segments show sampled headings. Hover over the curve to inspect time and orientation. Notice that the controls guide the trajectory without acting as interpolation constraints.

trajectory_x = np.array([pose.x() for pose in trajectory])
trajectory_y = np.array([pose.y() for pose in trajectory])
trajectory_theta = np.array([pose.theta() for pose in trajectory])

path_figure = go.Figure()
path_figure.add_scatter(
    x=trajectory_x,
    y=trajectory_y,
    mode="lines",
    name="cumulative spline",
    customdata=np.column_stack((sample_times, trajectory_theta)),
    hovertemplate="t=%{customdata[0]:.2f}<br>x=%{x:.2f}<br>y=%{y:.2f}<br>theta=%{customdata[1]:.2f}<extra></extra>",
)
path_figure.add_scatter(
    x=[pose.x() for pose in control_poses],
    y=[pose.y() for pose in control_poses],
    mode="markers+text",
    text=[f"T{i}" for i in range(len(control_poses))],
    textposition="top center",
    marker={"size": 11, "symbol": "circle-open", "line": {"width": 2}},
    name="control poses",
)

heading_x, heading_y = [], []
for pose in trajectory[::25]:
    heading_x.extend([pose.x(), pose.x() + 0.25 * np.cos(pose.theta()), None])
    heading_y.extend([pose.y(), pose.y() + 0.25 * np.sin(pose.theta()), None])
path_figure.add_scatter(
    x=heading_x, y=heading_y, mode="lines", name="sampled headings",
    line={"color": "rgba(31, 119, 180, 0.55)", "width": 1},
    hoverinfo="skip",
)
path_figure.update_layout(
    title="A smooth planar pose trajectory",
    xaxis_title="x",
    yaxis_title="y",
    yaxis={"scaleanchor": "x", "scaleratio": 1},
    template="plotly_white",
)
path_figure.show()

4. Ask the trajectory for analytic derivatives¶

The kernel stores exact piecewise polynomials, so the trajectory can evaluate the derivative of the accumulated tangent coordinate analytically:

\dot{\xi}(t)=\sum_i \dot c_i(t)\xi_i.

(2)

The translation-coordinate rates are shown separately from the angular-coordinate rate because they have different units. They start and finish at zero and change smoothly through the interior. This tangent-coordinate derivative should not be confused with a body-frame velocity without the appropriate Lie-group Jacobian conversion.

tangent_rates = np.vstack([
    trajectory_model.sampleTrajectoryDerivative(float(time))
    for time in sample_times
])

rate_figure = make_subplots(
    rows=2, cols=1, shared_xaxes=True,
    subplot_titles=("translation tangent rates", "angular tangent rate"),
)
rate_figure.add_scatter(x=sample_times, y=tangent_rates[:, 0], name="vx", row=1, col=1)
rate_figure.add_scatter(x=sample_times, y=tangent_rates[:, 1], name="vy", row=1, col=1)
rate_figure.add_scatter(x=sample_times, y=tangent_rates[:, 2], name="omega", row=2, col=1)
rate_figure.update_xaxes(title_text="kernel time t", row=2, col=1)
rate_figure.update_yaxes(title_text="distance / time", row=1, col=1)
rate_figure.update_yaxes(title_text="radians / time", row=2, col=1)
rate_figure.update_layout(
    title="Analytic derivative of the accumulated tangent coordinate",
    template="plotly_white",
    hovermode="x unified",
    height=650,
)
rate_figure.show()

Python and expression-valued C++ APIs¶

This notebook uses CumulativeSplineTrajectoryPose2 for constant Pose2 controls and numeric timestamps. Other available instantiations include CumulativeSplineTrajectoryRot2, CumulativeSplineTrajectoryRot3, and CumulativeSplineTrajectoryPose3.

In C++, the class additionally accepts expressions. Control poses and time can therefore be constants, variables, or computed expressions in a factor graph:

CumulativeSplineTrajectory<Pose2> trajectory(1.0);
for (size_t i = 0; i < poseCount; ++i) {
  trajectory.addControlPoint(Pose2_(Symbol('x', i)));
}

Double_ time(Symbol('t', 0));
Pose2_ pose = trajectory.sampleTrajectory(time);
Vector3_ tangentRate = trajectory.sampleTrajectoryDerivative(time);

Increase the density when control points occur more than once per unit of physical time. If the plausible time is known, pass a narrow sample window so the resulting expression includes only nearby controls. For the component-level details, continue with the cumulative spline concepts notebook.