explainer
Trajectory error metrics: compare full paths, frames, and drift
Compare ATE, RPE, alignment, and timing with an interactive trajectory lab and downloadable Python evaluator. Inspect timestamp matches, missing coverage, and a misleading zero-error fit.
What you will learn
- Distinguish full-trajectory position RMSE from endpoint error.
- Define the frame, units, timestamp associations, and alignment used by a comparison.
- Calculate relative pose errors with translation and rotation reported separately.
- Explain why similarity alignment can hide a metric-scale error.
- Read the lag and matched-pair count alongside an error score.
- Evaluate timestamped CSVs and identify a clock error concealed by alignment.
Before you start
A trajectory can finish at the right point and still be wrong along the way. Position RMSE measures error across matched samples. Relative pose error measures disagreement between motions over a stated interval.
Both scores depend on the comparison setup. This lesson uses nine synthetic poses so you can inspect every contribution, change alignment, and check the calculations independently. The downloadable Python exercise adds timestamped CSVs and a clock error that rigid alignment can conceal.
Decide what the trajectory score should measure
A reference trajectory gives poses to compare against an estimate. In this lesson, the reference is known exactly because we generate it. Real reference systems also have calibration, timing, and measurement uncertainty.
The TUM RGB-D benchmark paper by Sturm and colleagues defines absolute trajectory error and relative pose error for this comparison. ATE describes disagreement across aligned poses; fixed-lag RPE compares relative motions. State the exact convention when reporting either name.
The lab evaluates supplied trajectories. It contains no camera, wheel sensor, or SLAM estimator. Use it to learn what a metric responds to before applying that metric to odometry or a SLAM result.
Match timestamps, frames, and units first
Let Qᵢ be a reference pose and Pᵢ the estimated pose at the same timestamp. Both map coordinates from the robot body into their world frame. In 2D, a pose contains x, y, and counterclockwise yaw θ:
Positions use meters, timestamps use seconds, and internal angles use radians. The readouts convert rotational errors to degrees. P and Q must describe the same body frame, such as the camera optical frame or the robot base.
A camera pose and a base pose differ by a fixed body-frame transform. Convert that extrinsic relationship before comparing them. A fitted world-frame alignment does not generally correct a body-frame mismatch throughout a turning route.
The RPG trajectory-evaluation toolbox uses timestamps to establish correspondences. Real data need a declared matching tolerance or interpolation rule and any known clock offset. The interactive lab and inline Python example use nine pre-associated pairs at identical times: 0, 0.5, …, 4 s.
A timestamp error can create a false motion error. At 1 m/s, a 0.2 s offset along a straight segment creates a 0.2 m position discrepancy. On constant-speed straight motion, free translation alignment can even absorb that timing mistake.
Compare all positions with RMSE
After the chosen alignment, call the estimated position p̃ᵢ and reference position qᵢ. Each translational absolute pose error is their Euclidean distance:
This is the translational APE RMSE, often reported as translational ATE RMSE when the alignment protocol is specified. Endpoint position error uses only eₙ₋₁. It says nothing about the other N − 1 samples.
The default example adds position error [0.6, −0.3] sin(iπ/8) m to samples i = 0 through 8. Its first and final positions match exactly. The squared error at each sample is 0.45 sin²(iπ/8) m².
Across all nine samples, those squared errors sum to 1.8 m². Position RMSE is therefore √(1.8/9) = 0.447214 m, while endpoint error is 0 m. Averaging distances would produce a different statistic; RMSE squares errors before averaging.
Fit one rigid frame transformation
Two correct trajectories can use different world origins and headings. A rigid SE(2) alignment fits one planar rotation R and translation t from the estimate into the reference:
The lab gives every matched position equal weight and fits all nine. It rotates each estimated yaw by the same fitted angle. It does not fit a separate transform at every timestamp.
For a direct 2D solution, subtract each trajectory's position mean. Call the centered estimates uᵢ and centered references vᵢ. Compute:
The Global frame offset case applies +30° rotation and translation [1, −0.5] m. Without alignment, its position RMSE is 0.896438 m. Rigid alignment recovers −30° and translation approximately [−0.616025, 0.933013] m, reducing the score to zero apart from rounding.
Zhang and Scaramuzza's evaluation tutorial explains why alignment must reflect the system's unobservable frame freedoms. Fitting all positions uses future samples and can lower the reported error. An alignment chosen from the initial pose answers a different evaluation question.
Decide whether scale alignment is allowed
A rigid transform preserves distance. If an estimate enlarges every position by 1.25, rotation and translation cannot fully repair it. The lab's scale case retains 0.414721 m position RMSE after its best rigid fit.
A similarity Sim(2) alignment also fits one positive scale s:
The fitted scale is 0.8 for the 1.25× example, and its aligned position error becomes zero. Apply s to positions, while yaw receives only φ. A scale does not multiply an angle.
Similarity alignment can be appropriate for monocular motion estimates with unknown global scale. For a system meant to recover metric distances, report the scale error explicitly. A favorable similarity-aligned score can conceal that error. The RPG toolbox's alignment options distinguish similarity, rigid, and yaw-constrained fits for different sensing assumptions.
These closed-form fits require position variation that determines a rotation. Repeated identical positions cannot determine that rotation from positions alone. The helper rejects an undetermined fit.
Calculate relative pose error in pose frames
For matched samples i and j, form each relative motion using pose composition. The inverse puts the later pose in the earlier body's coordinates:
This follows the TUM benchmark's RPE definition, here in SE(2). Report the norm of E's translation separately from the magnitude of its wrapped rotation angle. Take an RMSE over the chosen pairs for each quantity.
The order of multiplication matters. Subtracting two world-coordinate displacement vectors can miss heading errors because it omits the pose frames. In the Heading drift case, every estimated world position is exact, yet translational RPE is nonzero.
A concrete two-pose check makes this visible. Let the reference move from (0, 0, 0°) to (1, 0, 0°), while the estimate uses the same positions with heading 90° at both samples. Reference motion is [1, 0] in its starting frame; estimated motion is [0, −1] in its starting frame, yielding translation error √2 m and relative rotation error 0°.
A constant world-frame rigid transform cancels inside P̃ᵢ⁻¹P̃ⱼ. That explains why the global frame-offset case has zero RPE even before alignment. Constant heading error with unchanged positions, as in the two-pose example, is a different change.
Report the lag and rotation error separately
Set j = i + ℓ, where ℓ is a lag in samples. Nine poses provide 9 − ℓ pairs. Our fixed 0.5 s spacing gives these choices:
| Lag | Time interval | Number of pairs |
|---|---|---|
| 1 sample | 0.5 s | 8 |
| 2 samples | 1 s | 7 |
| 4 samples | 2 s | 5 |
Use every eligible pair, including overlapping pairs. The default interior-error case has translational RPE RMSE 0.185079 m at lag 1, 0.336115 m at lag 2, and 0.482411 m at lag 4. Those values measure different intervals.
The heading-drift example adds 5° of error per sample. Its rotational RPE RMSE is 5°, 10°, or 20° for the three lags. Angles wrap to the shortest signed difference before taking their magnitude, so a crossing from +179° to −179° represents 2°, not 358°.
The lab reports meters and degrees over the chosen interval. It does not divide by seconds or distance traveled. A meters-per-second, degrees-per-meter, or percentage-drift score needs its own stated normalization.
Compare four controlled error patterns
The reference follows a half-circle of radius 2 m. At sample i, its heading is θᵢ = iπ/8 and position is [2 sin θᵢ, 2(1 − cos θᵢ)] m. The four cases modify this same reference in controlled ways.
Try the cases in this order:
- Interior error: the endpoint agrees, but the position-error plot exposes the middle of the route.
- Global frame offset: rigid alignment removes the coordinate discrepancy; RPE stays zero throughout.
- Scale error: compare the remaining error after rigid alignment with the zero score after similarity alignment.
- Heading drift: positions overlap exactly, while the heading ticks and RPE readouts reveal disagreement.
Every score uses the same displayed, aligned estimate. Rigid alignment leaves both RPE measures unchanged. Similarity alignment changes translation scale and can therefore change translational RPE.
A fit can improve position RMSE while worsening endpoint error. In the interior case, rigid alignment reduces RMSE to 0.239624 m but moves the previously matching endpoint by 0.413776 m. The optimizer minimizes error across all nine positions.
Report enough detail to reproduce the score
A useful evaluation report includes the comparison protocol alongside the numbers:
- Data: sequence, duration, reference source, estimated trajectory, and reference uncertainty.
- Coordinates: body frame, world-frame convention, position units, and angle representation.
- Timing: clock offsets, association or interpolation rule, matched count, and excluded samples.
- Alignment: allowed transform, fitted scale, fitting subset, and whether all metrics use the aligned poses.
- Metrics: error definition, lag, pair selection, normalization, and aggregation statistic.
- Availability: tracking failures, missing coverage, and variation across repeated runs.
A low score on the surviving matches can hide tracking loss elsewhere. Report coverage so two methods cannot look comparable merely because one omitted difficult sections.
Also separate the live pose stream from a final optimized trajectory. Pose-graph optimization can revise earlier positions and headings after a loop constraint arrives. The ICP failure experiments demonstrate a zero correspondence residual at the wrong transform; a fit score alone cannot establish full-trajectory accuracy.
Reproduce the evaluator in Python
This standard-library example constructs the same trajectories and fits their positions. Relative errors use full 3×3 homogeneous-matrix multiplication, making the pose convention explicit. It assumes the timestamp associations are already correct.
from math import atan2, cos, hypot, pi, sin, sqrt
reference = [(i*.5, 2*sin(i*pi/8), 2*(1-cos(i*pi/8)), i*pi/8)
for i in range(9)]
def estimate(name):
result = []
for i, (time, x, y, yaw) in enumerate(reference):
if name == "interior":
error = 0 if i in (0, 8) else sin(i*pi/8)
x, y = x + .6*error, y - .3*error
elif name == "frame":
c, s = cos(pi/6), sin(pi/6)
x, y, yaw = c*x - s*y + 1, s*x + c*y - .5, yaw + pi/6
elif name == "scale":
x, y = 1.25*x, 1.25*y
elif name == "heading":
yaw += i*pi/36
result.append((time, x, y, yaw))
return result
def align(poses, mode):
if mode == "none":
return poses
px, py = [sum(p[k] for p in poses)/9 for k in (1, 2)]
qx, qy = [sum(q[k] for q in reference)/9 for k in (1, 2)]
dot = cross = spread = 0.0
for p, q in zip(poses, reference):
ux, uy, vx, vy = p[1]-px, p[2]-py, q[1]-qx, q[2]-qy
dot += ux*vx + uy*vy
cross += ux*vy - uy*vx
spread += ux*ux + uy*uy
angle = atan2(cross, dot)
c, s = cos(angle), sin(angle)
scale = hypot(dot, cross)/spread if mode == "similarity" else 1
tx, ty = qx-scale*(c*px-s*py), qy-scale*(s*px+c*py)
return [(t, scale*(c*x-s*y)+tx, scale*(s*x+c*y)+ty, a+angle)
for t, x, y, a in poses]
def matrix(pose):
_, x, y, angle = pose
c, s = cos(angle), sin(angle)
return [[c, -s, x], [s, c, y], [0, 0, 1]]
def inverse(a):
return [[a[0][0], a[1][0], -a[0][0]*a[0][2]-a[1][0]*a[1][2]],
[a[0][1], a[1][1], -a[0][1]*a[0][2]-a[1][1]*a[1][2]],
[0, 0, 1]]
def multiply(a, b):
return [[sum(a[i][k]*b[k][j] for k in range(3))
for j in range(3)] for i in range(3)]
def rms(values):
return sqrt(sum(value*value for value in values)/len(values))
def evaluate(name, mode, lag=1):
poses = align(estimate(name), mode)
errors = [hypot(p[1]-q[1], p[2]-q[2]) for p, q in zip(poses, reference)]
translations, rotations = [], []
for i in range(9-lag):
j = i + lag
dq = multiply(inverse(matrix(reference[i])), matrix(reference[j]))
dp = multiply(inverse(matrix(poses[i])), matrix(poses[j]))
error = multiply(inverse(dq), dp)
translations.append(hypot(error[0][2], error[1][2]))
rotations.append(abs(atan2(error[1][0], error[0][0]))*180/pi)
return rms(errors), errors[-1], rms(translations), rms(rotations)
for name, mode in [("interior", "none"), ("frame", "none"),
("frame", "rigid"), ("scale", "rigid"),
("scale", "similarity"), ("heading", "none")]:
values = evaluate(name, mode)
print(name, mode, " ".join(f"{value:.6f}" for value in values))
Each output row lists position RMSE in meters, endpoint error in meters, translational RPE RMSE in meters, and rotational RPE RMSE in degrees. All rows use lag 1, or 0.5 s:
interior none 0.447214 0.000000 0.185079 0.000000
frame none 0.896438 1.439821 0.000000 0.000000
frame rigid 0.000000 0.000000 0.000000 0.000000
scale rigid 0.414721 0.572719 0.195090 0.000000
scale similarity 0.000000 0.000000 0.000000 0.000000
heading none 0.000000 0.000000 0.281875 5.000000
Trajectory evaluation Python exercise: match timestamped CSVs
A zero aligned error can hide a clock error. This download demonstrates that failure on the same circular path used above. It adds timestamp association and file-based reporting to the earlier evaluator, which assumes its poses already match in time.
Download the trajectory-evaluation exercise ZIP. It contains the Python evaluator, README, expected output, and six CSV files. Use Python 3.12 or later; no additional packages are required.
The reference CSV and supplied estimates contain synthetic poses. They reproduce the lesson's controlled geometry; they are not output from a SLAM estimator or a physical robot. This is a small planar evaluator with its own CSV format, not a reader for TUM's 3D quaternion files.
Run the saved trajectories
Extract the archive, open a terminal in that directory, and run:
python3 trajectory.py demo --output run
python3 trajectory.py check
Use a fresh output directory for each run. The demo reads the supplied CSVs and writes reports for nine comparisons. They cover the original interior, frame, scale, and heading examples, plus three ways of evaluating the clock-offset file. The check verifies input hashes and the nine numerical reference results; the README explains each output file and the tested environments.
Every input has four columns: t,x,y,yaw. Time is in seconds, x and y are in meters, and yaw is a counterclockwise angle in radians. Each pose maps the same robot body frame into its trajectory's world frame. Timestamps must be finite, strictly increasing, and unique; frame and unit consistency remain the reader's responsibility.
Follow the timestamp association rule
The evaluator adds --offset to each estimated timestamp before matching:
time residual = t_estimate + offset − t_reference
It considers pairs within --max-difference, sorts them by absolute time residual, and accepts a pair only while both samples remain unused. Exact ties follow reference row, then estimate row. This is a deterministic greedy rule; it does not find a globally optimal assignment. The program rejects matches that reverse estimated time when ordered by reference time.
Time comparisons allow a fixed 10⁻¹²-second margin for floating-point roundoff, recorded in each report. There is no interpolation. A pose that lacks an accepted partner contributes no position error, so each report includes unmatched rows and coverage for both inputs. A narrow tolerance can leave too few matches to evaluate; widening it changes which physical moments the score compares.
The clock-offset CSV copies every reference pose but labels its timestamp 0.4 seconds later. With offset zero and a 0.15-second tolerance, the estimated pose labeled 0.4 s matches the reference at 0.5 s. That estimate actually contains the reference's 0.0-second pose. Eight such shifted matches survive; one row from each input remains unmatched.
See how alignment conceals the wrong match
| Protocol | Matches | Position RMSE (m) |
|---|---|---|
| Wrong clock, no alignment | 8/9 | 0.780361 |
| Wrong clock, rigid alignment | 8/9 | 0.000000 |
| Corrected clock, no alignment | 9/9 | 0.000000 |
Here both inputs have nine poses, so the match fractions describe both coverages. The zeros show six-decimal rounding. Inspect the full-precision report before treating a displayed zero as exact.
The robot advances around the circle at a constant angular rate. Moving every match ahead by one sample is therefore equivalent to rotating the path by 22.5° around its center. The fitted world-frame transform removes that discrepancy even though the timestamp pairs are wrong. The relative motions also repeat, so RPE alone cannot identify this clock mistake.
Use the known fixture offset to repair the association:
python3 trajectory.py evaluate \
--reference data/reference.csv \
--estimate data/clock-offset.csv \
--offset -0.4 --max-difference 0.15 \
--alignment none --lag 1 --lag-tolerance 0.01 \
--output corrected
The offset is −0.4 because it subtracts the estimate's known clock delay. It comes from the fixture's construction, not from searching for the smallest trajectory error. The corrected run matches all nine original pairs without fitting a frame correction. For measured data, establish clock relationships from the recording system or a separately justified synchronization procedure.
Read the protocol beside the score
Each report preserves input hashes, matching settings, the fitted transform, and separate position and relative-pose errors. The matched-row output lets you inspect both timestamps and their signed residual. The RPE output records the actual time interval in each trajectory for every pair.
The download defaults to a one-second RPE lag, while the earlier inline Python example uses 0.5 seconds. For each matched reference time, it chooses the nearest later matched reference sample to the requested lag, within the stated lag tolerance. It includes overlapping pairs; nine regularly spaced poses give seven one-second pairs. Missing or irregular samples can change that count, and estimated intervals can differ from reference intervals.
Choose alignment to match the question:
none: compare poses in their supplied world frames.rigid: fit one rotation and translation using all matched positions.similarity: also fit a positive position scale, while rotating yaw without scaling it.
The scale-error CSV reproduces the 1.25× path. Its fitted similarity scale is 0.8, which removes the position error. If metric distance is part of the system's promise, keep that fitted scale visible. Fitting the complete route also uses future positions, so the resulting score describes an offline comparison rather than a live localization guarantee.
The endpoint field describes the last matched pose. If matching drops the end of a run, that field cannot describe its final recorded pose. Likewise, a file with too few matches or no eligible RPE interval should fail with an explanation, not silently report zero error.
Try a coverage change
Make a copy of one estimate CSV and remove a middle row. Evaluate the copy using the same offset, alignment, and one-second lag. Check which associations disappear, how many RPE pairs remain, and whether the reported intervals still satisfy the declared tolerance.
Use the ROS 2 timestamped-transform example to connect this file-level timing question to a received sensor observation. Its supplied transforms form a separate experiment; they are not the input data for this download.
Check two evaluation claims
Exercise 1: Test whether a correct endpoint proves a correct route. Nine matched samples have squared position errors totaling 1.8 m². Their first and final positions match exactly. What are the full-route position RMSE and endpoint error, and which statement does the endpoint support?
Show the full-route and endpoint calculation
Solution: Position RMSE is √(1.8/9) = 0.447214 m, while endpoint error is 0 m. The endpoint supports agreement at that final timestamp. It cannot establish agreement at the seven interior timestamps.
Exercise 2: Recover the interval behind a rotational RPE score. A nine-pose record uses 0.5 s sampling. Its heading error increases by 5° each sample, and evaluation uses lag 4. How many pairs contribute, what interval does each span, and what is rotational RPE RMSE? Does that number have degrees-per-second units?
Show the pair count, interval, and rotation error
Solution: There are 9 − 4 = 5 pairs, each spanning 2 s. Every pair gains 4 × 5° = 20° of heading error, so the rotational RPE RMSE is 20°. The displayed metric uses degrees over that interval; dividing by 2 s would define a separate normalized value of 10°/s.
Sources and further study
- Jürgen Sturm and colleagues, A Benchmark for the Evaluation of RGB-D SLAM Systems, Section VII: the original benchmark's pose convention, fixed-lag RPE, and aligned translational ATE.
- Zichao Zhang and Davide Scaramuzza, A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry: alignment freedoms, least-squares fitting, metric choices, and reproducible evaluation.
- RPG trajectory-evaluation toolbox: the authors' implementation and documentation for timestamped inputs, alignment configuration, and error reporting.
Compare visual odometry and SLAM next. The distinction helps explain why local motion error and final corrected trajectory error can tell different stories.