Learning odometry corrections with PyTorch

A sensor that consistently overestimates a robot’s displacement can make its estimated path drift away from its true path. In this tutorial, a small neural network learns to correct those measurements. GTSAM estimates the robot’s positions using the corrected measurements, and we train the network to reduce the difference between those estimates and known reference positions.

Training requires the derivative of position error with respect to the network’s weights. Gradients computes the part of this derivative that passes through GTSAM’s position estimates, and PyTorch computes the network’s derivatives. The scale example introduces this calculation with one adjustable number instead of a neural network.

The example assumes basic Python, matrix multiplication, and the chain rule for derivatives. It requires NumPy, Plotly, PyTorch, and a GTSAM development build with the parameter-gradient API. Run it only where PyTorch is already installed; the installation cell below adds GTSAM and Plotly when running in Colab.

A C++ version using LibTorch implements the same training workflow without plots.

import importlib.util

if importlib.util.find_spec("torch") is None:
    raise ImportError("This optional notebook requires an environment with PyTorch.")

import numpy as np
import torch
from torch import nn
from torch.nn.utils import parameters_to_vector, vector_to_parameters
import plotly.graph_objects as go
from plotly.subplots import make_subplots
import plotly.io as pio
import gtsam
from gtsam.symbol_shorthand import X

print("PyTorch:", torch.__version__)

PyTorch: 2.13.0

1. Generating measurements with a sensor bias¶

Each example path contains nine two-dimensional positions and eight measured displacements between them. A sensor feature $f_i$ , such as temperature scaled to lie between -1 and 1, determines a systematic measurement error called the bias. Write $x_i^{\mathrm{true}}$ for the true position at step $i$ , $b(f_i)$ for the two-coordinate bias, and $\epsilon_i$ for small random noise. The measured displacement $m_i$ is

m_i=(x_{i+1}^{\mathrm{true}}-x_i^{\mathrm{true}})+b(f_i)+\epsilon_i.

(1)

Noisy measurements of the first and last positions provide anchors for each path. GTSAM will combine these anchors with the displacement measurements to estimate all nine positions. The exact reference positions are saved in truth so that we can measure the estimation error afterward.

Four paths will be used for training, and two additional paths will be reserved for evaluating the trained network. The code calls these reserved paths validation_data. Varying phase changes the paths and the sensor features encountered along them.

rng = np.random.default_rng(7)
steps_per_trajectory = 8


def sensor_bias(features):
    return np.column_stack((
        0.12 + 0.18 * np.sin(1.4 * features),
        -0.08 + 0.14 * features**2,
    ))


def make_trajectory(phase):
    angles = 0.8 * np.arange(steps_per_trajectory) + phase
    features = np.sin(angles)
    true_steps = np.column_stack((0.45 + 0.08 * np.cos(angles), 0.16 * np.sin(angles)))
    truth = np.vstack((np.zeros(2), np.cumsum(true_steps, axis=0)))
    measurements = true_steps + sensor_bias(features) + rng.normal(0, 0.005, true_steps.shape)
    anchors = truth[[0, -1]] + rng.normal(0, 0.01, (2, 2))
    return dict(features=features, measurements=measurements, truth=truth, anchors=anchors)


training_data = [make_trajectory(phase) for phase in (0.0, 0.8, 1.6, 2.4)]
validation_data = [make_trajectory(phase) for phase in (0.4, 2.0)]
print(f"Training trajectories: {len(training_data)}; held-out: {len(validation_data)}")

Training trajectories: 4; held-out: 2

2. Predicting the sensor bias¶

The neural network takes one sensor feature and predicts a horizontal and a vertical bias. It first computes eight weighted sums of the input, adds eight adjustable offsets, and applies Tanh to each result. Tanh is a smooth nonlinear function with outputs between -1 and 1. A second weighted sum produces the two bias coordinates. This type of network is called a multilayer perceptron, or MLP.

The network has 34 adjustable numbers: 8 input weights, 8 hidden-layer offsets, 16 output weights, and 2 output offsets. PyTorch calls the adjustable offsets biases too; they are network parameters, whereas the sensor bias is the quantity we want the network to predict. We initialize the output weights near zero so that the first predictions make only small measurement corrections.

PyTorch uses double precision here to match GTSAM’s numerical calculations. Setting the random seed makes the initial weights repeatable. The torch.no_grad() block lets us set those starting weights without recording derivatives of the initialization.

torch.manual_seed(7)
dtype = torch.float64
model = nn.Sequential(nn.Linear(1, 8), nn.Tanh(), nn.Linear(8, 2)).to(dtype=dtype)
with torch.no_grad():
    model[-1].weight.mul_(0.1)
    model[-1].bias.zero_()
print(model)
print("Trainable coordinates:", sum(parameter.numel() for parameter in model.parameters()))

Sequential(
  (0): Linear(in_features=1, out_features=8, bias=True)
  (1): Tanh()
  (2): Linear(in_features=8, out_features=2, bias=True)
)
Trainable coordinates: 34

3. Using the correction in an odometry factor¶

An odometry factor compares the displacement between two estimated positions with a corrected sensor measurement. Let $\theta$ collect the network’s 34 adjustable numbers, and write its prediction for feature $f_i$ as $b_\theta(f_i)$ . Subtracting that prediction from the measurement gives $m_i-b_\theta(f_i)$ . The prediction-minus-measurement difference is the raw factor residual:

e_i(x_i,x_{i+1},\theta) =x_{i+1}-x_i-m_i+b_\theta(f_i).

(2)

The factor’s error function must also tell GTSAM how this difference changes when either position changes. These derivatives are two-by-two matrices, called Jacobians. Increasing the first position decreases the difference, so its Jacobian is $-I$ , where $I$ is the identity matrix. Increasing the second position increases the difference, giving Jacobian $I$ .

The noise model determines how strongly GTSAM weights each coordinate error. We assign standard deviations of 0.08 and 0.12 to horizontal and vertical odometry errors, respectively. A smaller standard deviation gives an error more weight. The error function itself returns the raw difference; GTSAM applies these weights.

odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.08, 0.12]))

A second function supplies derivatives of the raw factor residual with respect to the network weights. We pass this function to CustomFactor for GTSAM to call when it needs derivatives; such a function is called a callback. Let $L$ denote the training loss, which will measure position error. GTSAM passes this function a two-coordinate vector $c$ that describes how the loss changes when the factor residual changes, including the effect of re-estimating the positions. The API calls $c$ a factor residual cotangent.

The chain rule converts $c$ into this factor’s contribution to each weight’s loss derivative. For one weight $p$ and the horizontal and vertical factor residual coordinates $e_x,e_y$ , the contribution is

c_x\frac{\partial e_x}{\partial p} +c_y\frac{\partial e_y}{\partial p}.

(3)

torch.autograd.grad computes these sums for all the network parameters at once. The 34 resulting derivatives form this factor’s parameter gradient. This operation is called a vector–Jacobian product, or VJP, and it is the job of the parameter_vjp function below. grad_outputs=cotangent supplies the two coefficients $c_x,c_y$ . The positions and these coefficients are held fixed while PyTorch differentiates the network. GTSAM has already accounted for the noise weights, so the function uses the raw factor residual.

The returned parameter derivatives must follow one consistent order. PyTorch’s model.parameters() supplies the network’s weight matrices and offset vectors; we save that order in parameters. parameters_to_vector joins their derivatives into one vector of 34 numbers, and detach().numpy() returns those numbers to GTSAM as a NumPy array.

A ParameterTag lets the function recognize which group of parameters is being requested. Here the group contains all 34 network parameters. Every odometry factor and network gradient query uses network_tag to identify that group. The callback returns None for a different tag.

parameters = tuple(model.parameters())
network_tag = gtsam.ParameterTag("odometry MLP")


def odometry_factor(first, second, measurement, feature):
    feature_tensor = torch.tensor([feature], dtype=dtype)
    measurement_tensor = torch.tensor(measurement, dtype=dtype)
    with torch.no_grad():
        corrected_measurement = measurement - model(feature_tensor).numpy()

    def error(this, values, jacobians):
        if jacobians is not None:
            jacobians[0], jacobians[1] = -np.eye(2), np.eye(2)
        return values.atPoint2(second) - values.atPoint2(first) - corrected_measurement

    def parameter_vjp(this, values, requested, factor_residual_cotangent):
        if requested != network_tag:
            return None
        # Differentiate the network while holding these two inputs fixed.
        displacement = torch.tensor(
            values.atPoint2(second) - values.atPoint2(first), dtype=dtype,
        )
        cotangent = torch.tensor(factor_residual_cotangent, dtype=dtype).detach()
        raw_factor_residual = displacement - measurement_tensor + model(feature_tensor)
        parts = torch.autograd.grad(raw_factor_residual, parameters, grad_outputs=cotangent)
        return parameters_to_vector(parts).detach().numpy()

    return gtsam.CustomFactor(odometry_noise, [first, second], error, parameter_vjp)

The factor stores a corrected measurement when it is constructed, so GTSAM can estimate positions without repeatedly evaluating the network. Its parameter callback evaluates the network again when a derivative is requested. The weights must therefore stay fixed between constructing the factor and finishing its derivative queries.

Odometry factors connect neighboring positions, and anchor factors constrain the two ends of each path. The diagram shows a shortened path with three positions. Round nodes are variables, and square nodes are factors.

Figure 1. A shortened path. Each D factor uses the same network parameters $\theta$ to correct its measurement. Each A factor uses an endpoint measurement. The dashed arrows show which factors use the network.

4. Estimating positions for several paths¶

GTSAM estimates positions by minimizing the sum of squared factor residuals after applying the noise weights. The network parameters stay fixed during this position estimation.

We can put all the training paths in one graph by giving each position a distinct key, its identifier in the graph. X(i) creates such a key from an integer. Values stores a two-coordinate position under each key. solve_batch starts with guesses obtained by adding up the measured displacements from the first anchor.

The anchor factors keep the estimated path close to its measured endpoints. We give the start anchor a standard deviation of 0.03 and the end anchor a standard deviation of 0.25, so the start is more strongly constrained. The reference positions are returned separately in targets for the loss calculation; they are not used as additional position measurements.

start_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.03)
end_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.25)


def solve_batch(trajectories):
    graph, initial, targets = gtsam.NonlinearFactorGraph(), gtsam.Values(), {}
    for trajectory_index, trajectory in enumerate(trajectories):
        keys = [X(trajectory_index * (steps_per_trajectory + 1) + i)
                for i in range(steps_per_trajectory + 1)]
        integrated = np.vstack((np.zeros(2), np.cumsum(trajectory["measurements"], axis=0)))
        for i, key in enumerate(keys):
            initial.insert(key, trajectory["anchors"][0] + integrated[i])
            if i > 0:
                targets[key] = trajectory["truth"][i]
        graph.add(gtsam.PriorFactorPoint2(keys[0], trajectory["anchors"][0], start_noise))
        for i, measurement in enumerate(trajectory["measurements"]):
            graph.add(odometry_factor(keys[i], keys[i + 1], measurement, trajectory["features"][i]))
        graph.add(gtsam.PriorFactorPoint2(keys[-1], trajectory["anchors"][1], end_noise))
    solution = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
    return graph, solution, targets

5. Computing the training loss and its gradient¶

The position loss measures the average squared distance between the estimated positions and their reference positions. Write $x_k^\star(\theta)$ for an estimated position, $y_k$ for its reference, and $N$ for the number of positions being compared. For training, $N=32$ : eight positions after the start of each of the four paths. We also add a small penalty on the squared network parameters, with coefficient $\alpha$ , to discourage large parameter values. The total training loss is

L(\theta)=\frac{1}{2N}\sum_k\|x_k^\star(\theta)-y_k\|^2 +\frac{\alpha}{2}\|\theta\|^2.

(4)

The derivative of the position loss with respect to position $x_k^\star$ is $(x_k^\star-y_k)/N$ . The factor $1/2$ in the loss cancels the factor 2 from differentiating the square. GTSAM calls these derivatives state cotangents. A VectorValues associates each derivative vector with its position’s key.

parameterGradient turns the position derivatives into derivatives with respect to the network weights. It uses the solved graph and calls the factors’ parameter functions with network_tag, adding their returned vectors. Ordinary anchor factors have no network parameters, but their influence on the estimated positions is included in this calculation.

The weight penalty contributes a second part of the total gradient because it depends directly on the weights. torch.autograd.grad computes this contribution from weight_penalty(), and we add it to the vector returned by parameterGradient. This call returns derivatives without changing the network’s weights.

def position_loss(solution, targets):
    loss, state_cotangents = 0.0, gtsam.VectorValues()
    for key, target in targets.items():
        error = solution.atPoint2(key) - target
        loss += 0.5 * (error @ error) / len(targets)
        state_cotangents.insert(key, error / len(targets))
    return loss, state_cotangents


penalty_weight = 1e-4


def weight_penalty():
    return 0.5 * penalty_weight * sum(parameter.square().sum() for parameter in parameters)


graph, solution, targets = solve_batch(training_data)
initial_training_loss, state_cotangents = position_loss(solution, targets)
gradients = gtsam.Gradients(graph, solution)
through_estimates = gradients.parameterGradient(state_cotangents, network_tag)
direct_parts = torch.autograd.grad(weight_penalty(), parameters)
total_gradient = through_estimates + parameters_to_vector(direct_parts).detach().numpy()
print(f"Initial position loss: {initial_training_loss:.6f}")
print(f"Total gradient norm: {np.linalg.norm(total_gradient):.6f}")

Initial position loss: 0.122021
Total gradient norm: 1.796164

6. Comparing the gradient with finite differences¶

Small changes in the weights reveal how the loss responds in a chosen parameter direction. Choose a vector $v$ with length one in the 34-dimensional parameter space. For a small number $h$ , we solve the graph at weights $\theta+hv$ and again at $\theta-hv$ . The slope estimated from those two losses, called a central finite difference, is

\frac{L(\theta+hv)-L(\theta-hv)}{2h}.

(5)

The dot product $v^T\nabla_\theta L$ predicts the slope in the same direction. The code compares this prediction with the finite difference at three step sizes, including the weight penalty in each loss. The vector $v$ changes all 34 parameters together, so the displayed slopes describe one direction through parameter space.

Each changed set of weights needs a newly constructed graph and newly estimated positions. This keeps the stored corrected measurements consistent with the network used by the derivative callback. parameters_to_vector saves the current weights, and vector_to_parameters assigns the perturbed weights before each solve. The finally block restores the saved weights afterward.

def total_loss(trajectories):
    _, estimate, reference = solve_batch(trajectories)
    loss, _ = position_loss(estimate, reference)
    return loss + weight_penalty().item()


original_weights = parameters_to_vector(parameters).detach().clone()
direction = torch.randn_like(original_weights)
direction /= direction.norm()
analytic = total_gradient @ direction.numpy()
try:
    for step in (1e-4, 1e-5, 1e-6):
        vector_to_parameters(original_weights + step * direction, parameters)
        plus = total_loss(training_data)
        vector_to_parameters(original_weights - step * direction, parameters)
        minus = total_loss(training_data)
        numerical = (plus - minus) / (2 * step)
        print(f"Step {step:.0e}: analytic {analytic:.9f}, finite difference {numerical:.9f}")
finally:
    vector_to_parameters(original_weights, parameters)

Step 1e-04: analytic -0.030936929, finite difference -0.030936927
Step 1e-05: analytic -0.030936929, finite difference -0.030936929
Step 1e-06: analytic -0.030936929, finite difference -0.030936929

7. Updating the network weights¶

Adam updates network weights using their loss derivatives and a running record of previous derivatives. We use PyTorch’s implementation with a learning rate of 0.03, which controls the size of the updates. Each iteration estimates positions with the current weights, computes the total gradient, and takes one Adam step.

PyTorch’s optimizer reads each parameter tensor’s derivative from its .grad field. weight_penalty().backward() fills these fields with the direct penalty derivatives. We then split GTSAM’s returned vector according to the sizes of the parameter tensors and add each part to the matching .grad field. zero_grad clears the fields before the next iteration so old derivatives are not added again.

The two reserved paths measure how well the current network works on examples that do not update its weights. evaluate solves their positions and reports the same position loss used for training. We save their initial estimates before the first update for comparison with the trained estimates, and record both training and validation losses every five updates.

def evaluate(trajectories):
    _, estimate, reference = solve_batch(trajectories)
    loss, _ = position_loss(estimate, reference)
    return loss, estimate


initial_validation_loss, before_training = evaluate(validation_data)
parameter_sizes = [parameter.numel() for parameter in parameters]
optimizer = torch.optim.Adam(parameters, lr=0.03)
epochs = 120
history = {"epoch": [0], "train": [initial_training_loss], "validation": [initial_validation_loss]}
for epoch in range(1, epochs + 1):
    optimizer.zero_grad(set_to_none=True)
    graph, solution, targets = solve_batch(training_data)
    _, state_cotangents = position_loss(solution, targets)
    gradients = gtsam.Gradients(graph, solution)
    flat_gradient = torch.from_numpy(gradients.parameterGradient(state_cotangents, network_tag))
    weight_penalty().backward()
    with torch.no_grad():
        for parameter, block in zip(parameters, flat_gradient.split(parameter_sizes)):
            parameter.grad.add_(block.reshape_as(parameter))
    optimizer.step()
    if epoch % 5 == 0:
        train_loss, _ = evaluate(training_data)
        validation_loss, _ = evaluate(validation_data)
        history["epoch"].append(epoch)
        history["train"].append(train_loss)
        history["validation"].append(validation_loss)

final_validation_loss, after_training = evaluate(validation_data)
print(f"Training position loss: {initial_training_loss:.6f} -> {history['train'][-1]:.6f}")
print(f"Held-out position loss: {initial_validation_loss:.6f} -> {final_validation_loss:.6f}")

Training position loss: 0.122021 -> 0.000286
Held-out position loss: 0.126805 -> 0.000165

8. Comparing the results¶

After 120 weight updates, both training and validation position losses are less than one percent of their initial values. The curves below report position error alone, leaving out the weight penalty. Their vertical axis uses a logarithmic scale: moving from 0.01 to 0.001 means a tenfold reduction in loss. Hover over a point to read its value and the number of Adam updates.

pio.renderers.default = "plotly_mimetype+notebook_connected"
loss_figure = go.Figure()
for label, field in (("Training", "train"), ("Held-out", "validation")):
    loss_figure.add_scatter(x=history["epoch"], y=history[field], mode="lines+markers", name=label)
loss_figure.update_layout(
    title="Position loss through training", xaxis_title="Adam updates",
    yaxis_title="Mean squared position error / 2",
    yaxis=dict(type="log", tickvals=[1e-4, 1e-3, 1e-2, 1e-1],
               ticktext=["0.0001", "0.001", "0.01", "0.1"]),
    template="plotly_white", height=360,
)
loss_figure.show()

The trained network approximates the sensor bias that generated the measurements. Because we chose that bias when creating the data, we can compare the network’s predictions with it at 100 feature values between -1 and 1. Training used position errors rather than these bias values as its loss.

The first validation path also moves closer to its reference positions after training. The final plot compares the estimated positions before and after the weight updates with the reference path. Equal horizontal and vertical scales preserve the shape of the paths.

feature_grid = np.linspace(-1.0, 1.0, 100)
with torch.no_grad():
    learned_bias = model(torch.tensor(feature_grid[:, None], dtype=dtype)).numpy()

bias_figure = make_subplots(rows=1, cols=2, subplot_titles=("Horizontal bias", "Vertical bias"))
for coordinate in range(2):
    for label, values, dash, color in (
        ("True bias", sensor_bias(feature_grid), "solid", "#245785"),
        ("Trained MLP", learned_bias, "dash", "#25834c"),
    ):
        bias_figure.add_scatter(
            x=feature_grid, y=values[:, coordinate], name=label, legendgroup=label,
            showlegend=coordinate == 0, line=dict(dash=dash, color=color), row=1, col=coordinate + 1,
        )
bias_figure.update_xaxes(title_text="Sensor feature")
bias_figure.update_yaxes(title_text="Displacement bias")
bias_figure.update_layout(title="Bias learned from position supervision", template="plotly_white", height=380)
bias_figure.show()

trajectory_figure = go.Figure()
for label, points in (
    ("Reference", validation_data[0]["truth"]),
    ("Before training", np.array([before_training.atPoint2(X(i)) for i in range(steps_per_trajectory + 1)])),
    ("After training", np.array([after_training.atPoint2(X(i)) for i in range(steps_per_trajectory + 1)])),
):
    trajectory_figure.add_scatter(x=points[:, 0], y=points[:, 1], mode="lines+markers", name=label)
trajectory_figure.update_layout(
    title="First held-out trajectory", xaxis_title="Horizontal position",
    yaxis_title="Vertical position", yaxis=dict(scaleanchor="x", scaleratio=1),
    template="plotly_white", height=380,
)
trajectory_figure.show()

Advanced: why this gradient agrees with re-estimation¶

Changing a network weight changes a factor’s corrected measurement but does not change its position Jacobians, $-I$ and $I$ . The factor residual is therefore affine in the positions: a linear function plus a constant. This property makes the derivative computed by Gradients exact for this example, even though the network is nonlinear in its weights.

The two second-derivative terms omitted by the Gauss–Newton approximation both vanish for these factors. To see this, collect the positions into a vector $X$ and write $B$ for the matrix that forms neighboring position differences. Let $m$ collect the measurements and $b_\theta(f)$ collect the predicted biases. Let $W$ be the fixed matrix of noise weights. Applying these weights, called whitening, gives the weighted factor residual vector

r=W\bigl(BX-m+b_\theta(f)\bigr),

(6)

The position Jacobian is the constant matrix $WB$ . It changes neither with the positions nor with the network weights, so $\partial^2_{XX}r=0$ and $\partial^2_{X\theta}r=0$ . The anchor factors have constant position Jacobians as well.

Other factor models can have position Jacobians that depend on the positions or the learned parameters. Their omitted second-derivative terms can be nonzero, so a gradient from Gradients can differ from a finite difference that re-solves the graph. The class guide derives those terms and explains the Gauss–Newton approximation.