Fixed-Lag Smoother Example

Most factor-graph examples keep every variable in the problem forever. That doesn’t scale to a robot that runs for hours: the graph, and the cost of solving it, would grow without bound.

A fixed-lag smoother fixes this by only keeping variables whose timestamp falls within a trailing window (the “lag”). As new measurements arrive, anything older than the lag is automatically marginalized out -- summarized into the remaining variables and then dropped -- so the problem size stays bounded no matter how long the robot runs.

The scenario: a robot drives in a straight line at 2 m/s. Two independent odometry-like sensors each measure the motion between consecutive poses (simulating sensor fusion), sampled every 0.25 seconds for 3 seconds, with a 2-second lag.

Authors: Frank Dellaert (C++), Jeremy Aguilon (Python), et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

import numpy as np
import gtsam

1. Configure the smoother¶

BatchFixedLagSmoother re-solves with Levenberg-Marquardt on every update() call (GTSAM also has an IncrementalFixedLagSmoother, based on iSAM2, which is more efficient but not covered in this notebook). lag=2.0 means only the most recent 2 seconds of poses are kept.

Each update needs three staging containers: new factors, new variable values, and a mapping from key to timestamp -- the timestamps are how the smoother knows which variables have aged out of the window.

lag = 2.0
smoother_batch = gtsam.BatchFixedLagSmoother(lag)

new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values()
new_timestamps = {}

2. Prior on the first pose¶

As in the odometry example, a prior anchors pose key 0 at the origin. We also record its timestamp as 0.0 seconds.

prior_mean = gtsam.Pose2(0, 0, 0)
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
X1 = 0
new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean)
new_timestamps[X1] = 0.0

3. Simulate two odometry sensors over time¶

Every 0.25 seconds we add a new pose key, derived directly from the timestamp (int(1000 * time)), and a constant-velocity guess (time * 2 meters along x). Two BetweenFactorPose2s connect it to the previous pose -- one per “sensor”, each with its own measurement and noise model, simulating fusion of two independently noisy odometry sources.

update() is only called once every two steps (time >= 0.50), so the very first call batches two hops of factors at once, while later calls process one hop at a time. The staging containers are cleared after each update(), but the smoother’s own internal window keeps sliding forward regardless.

delta_time = 0.25
time = 0.25

while time <= 3.0:
    previous_key = int(1000 * (time - delta_time))
    current_key = int(1000 * time)

    # assign current key to the current timestamp
    new_timestamps[current_key] = time

    # Add a guess for this pose to the new values
    # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]
    current_pose = gtsam.Pose2(time * 2, 0, 0)
    new_values.insert(current_key, current_pose)

    # Add odometry factors from two different sources with different error
    # stats
    odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
    odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.1, 0.1, 0.05]))
    new_factors.push_back(gtsam.BetweenFactorPose2(
        previous_key, current_key, odometry_measurement_1, odometry_noise_1
    ))

    odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
    odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(
        np.array([0.05, 0.05, 0.05]))
    new_factors.push_back(gtsam.BetweenFactorPose2(
        previous_key, current_key, odometry_measurement_2, odometry_noise_2
    ))

    # Update the smoothers with the new factors. In this case,
    # one iteration must pass for Levenberg-Marquardt to accurately
    # estimate
    if time >= 0.50:
        smoother_batch.update(new_factors, new_values, new_timestamps)
        print("Timestamp = " + str(time) + ", Key = " + str(current_key))
        print(smoother_batch.calculateEstimatePose2(current_key))

        new_timestamps.clear()
        new_values.clear()
        new_factors.resize(0)

    time += delta_time

Timestamp = 0.5, Key = 500
(0.995821, 0.0231012, 0.0300001)

Timestamp = 0.75, Key = 750
(1.49284, 0.0457247, 0.045)

Timestamp = 1.0, Key = 1000
(1.98981, 0.0758879, 0.06)

Timestamp = 1.25, Key = 1250
(2.48627, 0.113502, 0.075)

Timestamp = 1.5, Key = 1500
(2.98211, 0.158558, 0.09)

Timestamp = 1.75, Key = 1750
(3.47722, 0.211047, 0.105)

Timestamp = 2.0, Key = 2000
(3.97149, 0.270956, 0.12)

Timestamp = 2.25, Key = 2250
(4.4648, 0.338272, 0.135)

Timestamp = 2.5, Key = 2500
(4.95705, 0.41298, 0.15)

Timestamp = 2.75, Key = 2750
(5.44812, 0.495063, 0.165)

Timestamp = 3.0, Key = 3000
(5.9379, 0.584503, 0.18)

4. What’s kept in the window¶

smoother_batch.timestamps() returns the key-to-timestamp map for every variable currently in the smoother. After 3 seconds have passed with a 2-second lag, pose 0 (timestamp 0.0) should be long gone -- marginalized out, not just cosmetically hidden.

remaining = smoother_batch.timestamps()
for key, t in sorted(remaining.items(), key=lambda kv: kv[1]):
    print(f"Key: {key}  Time: {t}")

Key: 1000  Time: 1.0
Key: 1250  Time: 1.25
Key: 1500  Time: 1.5
Key: 1750  Time: 1.75
Key: 2000  Time: 2.0
Key: 2250  Time: 2.25
Key: 2500  Time: 2.5
Key: 2750  Time: 2.75
Key: 3000  Time: 3.0

Only the poses from the last ~2 seconds remain; pose 0 and the earliest handful of poses are no longer part of the problem at all. This bounded-memory property is exactly why fixed-lag smoothing -- and its incremental, iSAM2-based cousin -- is the tool of choice for real-time or long-duration SLAM, where a robot cannot afford to keep re-optimizing its entire history forever.