Skip to the content
FixTheRobot Explore Robotics / UAV / Module 4 / M4-L5

UAV Systems Engineering · M4-L5 · Hands-on lab (runs on your machine)

Analyze trajectory drift

Implement the two standard trajectory metrics yourself, then trust the tool because you understand the alignment.

  • SLAM and Autonomous Localization
  • Established engineering
  • About 75 minutes

What you will do

Every SLAM and odometry result in this track is judged by ATE and RPE. You will implement both in NumPy, test them on a synthetic drifting trajectory with a known answer, and compare with evo on a real result.

Before you start

  • Lessons M2-T8 (drift), M3-T15 (consistency) and a trajectory from the ORB-SLAM3, VINS-Fusion, OpenVINS or FAST-LIO2 labs (or the synthetic one below).
  • Python 3 with NumPy; optionally pip install evo.

Reference implementation and self-test

Alignment first (Umeyama, SE(3) without scale, or Sim(3) for monocular), then ATE as the RMS of position differences, and RPE over a fixed distance.

import numpy as np
rng = np.random.default_rng(0)

def umeyama(est, gt, with_scale=False):
    mu_e, mu_g = est.mean(0), gt.mean(0); E, G = est - mu_e, gt - mu_g
    U, S, Vt = np.linalg.svd(G.T @ E / len(est)); D = np.diag([1, 1, np.linalg.det(U) * np.linalg.det(Vt)])
    R = U @ D @ Vt; s = np.trace(np.diag(S) @ D) / E.var(0).sum() if with_scale else 1.0
    return R, mu_g - s * R @ mu_e, s

def ate_rmse(est, gt, with_scale=False):
    R, t, s = umeyama(est, gt, with_scale); aligned = (s * (R @ est.T)).T + t
    return np.sqrt(np.mean(np.sum((aligned - gt) ** 2, axis=1)))

def rpe_trans(est, gt, delta=10):                                    # relative translation error over 'delta' frames, in the local frames (rotation ignored here: positions only)
    de = est[delta:] - est[:-delta]; dg = gt[delta:] - gt[:-delta]
    return np.sqrt(np.mean(np.sum((de - dg) ** 2, axis=1)))

# synthetic ground truth: a 100 m figure-of-eight at constant altitude
t = np.linspace(0, 2 * np.pi, 1000); gt = np.stack([30 * np.sin(t), 15 * np.sin(2 * t), 5 + 0 * t], axis=1)
step = np.diff(gt, axis=0)
# estimate = integrated steps with 1% scale error, a small heading drift and noise: classic odometry drift
th = np.cumsum(np.full(len(step), 1e-4)); c, s_ = np.cos(th), np.sin(th)
est_steps = np.stack([c * step[:, 0] - s_ * step[:, 1], s_ * step[:, 0] + c * step[:, 1], step[:, 2]], axis=1) * 1.01 + 0.002 * rng.standard_normal(step.shape)
est = np.vstack([gt[0], gt[0] + np.cumsum(est_steps, axis=0)])

# the metrics are invariant to a rigid transform of the estimate (that is what the alignment is for)
Rz = np.array([[np.cos(0.7), -np.sin(0.7), 0], [np.sin(0.7), np.cos(0.7), 0], [0, 0, 1]]); est_moved = (Rz @ est.T).T + np.array([10.0, -5.0, 2.0])
a1, a2 = ate_rmse(est, gt), ate_rmse(est_moved, gt)
print(f"ATE {a1:.3f} m (after alignment), same after moving the estimate: {a2:.3f} m; RPE over 10 frames {rpe_trans(est, gt):.4f} m; path length {np.sum(np.linalg.norm(step, axis=1)):.0f} m")
assert abs(a1 - a2) < 1e-9 and 0.05 < a1 < 3.0
assert ate_rmse(gt, gt) < 1e-9 and ate_rmse(gt * 1.0 + np.array([1, 2, 3.0]), gt) < 1e-9          # perfect trajectories, up to a rigid transform, have zero ATE
est_scaled = est * 0.5
assert ate_rmse(est_scaled, gt, with_scale=True) < 0.6 * ate_rmse(est_scaled, gt, with_scale=False)   # Sim(3) alignment removes monocular scale error
ATE with Umeyama alignment and RPE, tested on synthetic drift.

Steps

  1. Export your estimated trajectory from an earlier lab in TUM format (timestamp tx ty tz qx qy qz qw) and obtain the ground truth in the same format, with matching timestamps.
  2. Associate the two by timestamp (nearest within 10 ms) before computing anything.
  3. Compute ATE with SE(3) alignment (and Sim(3) if the system is monocular), and RPE over 10 m or 10 s windows.
  4. Compare with evo_ape tum gt.txt est.txt -va --plot --plot_mode xy and evo_rpe tum gt.txt est.txt -va --delta 10 --delta_unit m; the numbers must agree within the association tolerance.
  5. Plot error against distance travelled and fit the drift ratio (mm per mm). State it as a percentage.

Questions to answer

  • Why does ATE depend on the alignment and RPE barely does?
  • What is the effect of aligning only the first pose rather than the whole trajectory?
  • Which metric would you use to compare two odometries on a long route, and why?

How to know it worked

  • The self-test above passes with your own copy of the code.
  • Your ATE matches evo within the timestamp-association tolerance.
  • You report drift as a percentage of path length with the alignment method named.

If it does not work

SymptomLikely causeWhat to check
ATE hugeTimestamps not associated or trajectories in different framesPrint the first few matched pairs; check frame conventions
evo and your number disagreeDifferent alignment or associationUse the same alignment option and tolerance
Negative or NaN scaleDegenerate trajectory (straight line)Use more of the path or fix scale

References

Record your progress

Saved in this browser. This lab runs on your own machine, so the site cannot check it for you.