# Timestamped trajectory evaluation in Python

Compare planar robot poses from two CSV files. This exercise performs timestamp association, fits an optional world-frame alignment, and writes absolute position errors and fixed-time relative pose errors. It uses Python 3.12+ and the standard library. No installation is required.

All supplied trajectories are synthetic. They are controlled inputs for learning about evaluation, not recordings from a robot, outputs of a SLAM implementation, or evidence of project performance.

Verified on Python 3.14.3 on macOS ARM64 and Python 3.12.3 on Ubuntu 24.04 ARM64. Both environments passed `check`, generated the nine demo reports, and evaluated the corrected-clock command below. The Linux check ran in an offline container with read-only exercise inputs.

## Run the examples

Unzip the download, open a terminal in its folder, then run:

```sh
python3 trajectory.py check
python3 trajectory.py demo --output run
```

`check` verifies the input file hashes, association details, fitted transforms, and metrics against `expected-results.json`, allowing an absolute numeric difference of 1e-9. It also checks the six-decimal summary in `expected-output.txt`. Each of the nine example directories under `run/` contains:

- `report.json`: input names and SHA-256 hashes, complete protocol, coverage, fitted transform, aggregate metrics, every matched sample, and every RPE pair.
- `matches.csv`: original row indices, original and corrected timestamps, signed time residuals, reference and aligned poses, and position errors.
- `rpe.csv`: endpoint row indices, actual reference and estimate intervals, and translation/rotation errors for every eligible pair.

The writer requires a new or empty output directory. Choose a different directory for each run to retain the earlier results. It refuses to overwrite existing files and does not change the input CSVs.

## Evaluate another CSV

```sh
python3 trajectory.py evaluate \
  --reference data/reference.csv \
  --estimate data/clock-offset.csv \
  --offset -0.4 \
  --max-difference 0.15 \
  --alignment none \
  --lag 1.0 \
  --lag-tolerance 0.01 \
  --output corrected
```

Change the two file paths to evaluate your own data. Input files need exactly this header and numeric rows:

```csv
t,x,y,yaw
0.0,0.0,0.0,0.0
0.5,0.7653668647301796,0.15224093497742652,0.39269908169872414
```

Units are seconds, meters, and radians. Positive yaw is counterclockwise. Each pose maps the same robot body frame into its world frame. Convert camera/base extrinsics before evaluation; a world-frame fit does not generally repair different body-frame definitions. Timestamps must be strictly increasing with no duplicates. Blank cells, extra columns, nonfinite numbers, and fewer than two rows are rejected. The two-row illustration needs a shorter `--lag` than the default to supply an RPE pair.

This is a custom SE(2)/Sim(2) CSV evaluator. It does not read TUM quaternion or KITTI matrix formats, implement SE(3), or claim the same defaults and pair selection as evo, TUM, or RPG tools.

## Timestamp association

The correction is **added to the estimate timestamp**:

```text
corrected estimate time = recorded estimate time + offset
signed residual = corrected estimate time - reference time
```

An estimate whose clock reads 0.4 seconds ahead therefore needs `--offset -0.4`. Choose this correction from timing knowledge or a separate calibration procedure. The evaluator does not infer it from the best trajectory score.

Candidate pairs satisfy `abs(residual) <= max_difference + 1e-12` seconds. The small epsilon handles floating-point boundary roundoff. Candidates are sorted by absolute residual, then reference row index, then estimate row index. Greedy selection accepts a candidate only when neither row has already been used. Finally, the matches are sorted by reference time. Exact equal floating-point residuals use the stated index tie break; approximately equal residuals are not rounded into ties.

This rule is deterministic and one-to-one. It is not a maximum-cardinality or globally minimum-cost matching. A wide tolerance can produce ambiguous or crossed matches. The evaluator rejects matches that reverse estimate time, since those would compare forward reference motion with backward estimate motion. Reduce the tolerance or inspect the clocks when that happens. No interpolation, resampling, or automatic extrapolation occurs.

The implementation uses a timestamp window to enumerate candidate pairs, then sorts the candidates. This is intended for small teaching files; a large tolerance on a dense recording can still create many candidates and consume substantial memory.

Read both row coverage fractions and the unmatched index lists. Row coverage is a fraction of supplied samples, not a claim about continuous time or distance covered. A low score on retained poses says nothing about missing sections.

## Alignment and absolute position error

Use `--alignment none`, `rigid`, or `similarity`. Both fitted modes use **all matched positions with equal weight**, including future poses:

```text
aligned position = scale * R(yaw) * estimate position + [tx, ty]
aligned heading = wrap(estimate heading + yaw)
```

Rigid alignment keeps scale at 1. Similarity fits a positive scale as well. Position RMSE is `sqrt(mean(position_error**2))` over the matches. All subsequent RPE calculations also use the aligned poses. The report names its endpoint statistic `last_matched_position_error_m`: it may not correspond to either file's original final row if association excluded that row.

The position fit rejects repeated or numerically degenerate positions rather than inventing a rotation. Specifically, either centered position spread at or below 1e-24 m², or dot/cross correlation at or below 1e-12 times the geometric mean of the two spreads, is rejected. `none` can still evaluate stationary trajectories when the requested time pairs exist.

A full-trajectory fit measures a different question from an initial-pose alignment. Similarity alignment can erase metric scale error. Keep the fitted scale and timing details with the score.

## Relative pose error at a time lag

The default is **1.0 second**, with a **0.01 second** lag tolerance. It is not a fixed number of samples. For each matched reference time `t_i`, choose the later matched reference time nearest `t_i + lag`. Exact ties choose the earlier time. Keep the pair only if its actual interval is within the lag tolerance plus 1e-12 seconds. Each start contributes at most one pair; endpoints can be reused, and overlapping pairs are included.

The report includes both actual reference and estimate intervals for every pair and their min/max ranges. Selection uses reference times. With approximate timestamp associations, the estimate interval can differ from the reference interval. A constant offset cancels in the estimate interval.

For poses mapping body to world, the error transform is:

```text
reference motion = inverse(Q_i) * Q_j
estimate motion  = inverse(P_aligned_i) * P_aligned_j
error = inverse(reference motion) * estimate motion
```

Translation uses the norm of the error transform's translation. Rotation uses its shortest wrapped angle. Each aggregate is an RMSE; JSON/CSV pair angles are radians and the displayed rotational RMSE is degrees. Scores are not divided by time or distance. No eligible pair raises an error rather than returning a misleading zero.

## What the fixtures show

There are nine reference poses indexed `i = 0..8`. At `t = 0.5*i`, set `theta = i*pi/8`, `x = 2*sin(theta)`, `y = 2*(1-cos(theta))`, and `yaw = theta`. The last x is stored as exactly zero. This is a half-circle of radius 2 m. The first four modifications are the same controlled patterns as the article's interactive lab. CSV values preserve Python float representations of these formulas.

| Estimate file | Change from the reference |
| --- | --- |
| `interior-error.csv` | Add `[0.6,-0.3]*sin(i*pi/8)` m to position, with exactly zero error at the two endpoints. |
| `heading-drift.csv` | Add `i*pi/36` radians to yaw; leave positions unchanged. |
| `frame-offset.csv` | Rotate positions and yaw by +30 degrees, then translate positions by `[1,-0.5]` m. |
| `scale-error.csv` | Multiply positions by 1.25; leave yaw unchanged. |
| `clock-offset.csv` | Add 0.4 seconds to each timestamp; leave poses unchanged. |

Key results, using a one-second RPE lag:

| Case | Matches | Position RMSE (m) | Translation RPE (m) | Rotation RPE (degrees) |
| --- | --- | --- | --- | --- |
| Interior error, no alignment | 9/9 | 0.447214 | 0.336115 | 0 |
| Heading drift, no alignment | 9/9 | 0 | 0.477829 | 10 |
| Frame offset, no alignment | 9/9 | 0.896438 | 0 | 0 |
| Frame offset, rigid | 9/9 | 0 | 0 | 0 |
| Scale error, rigid | 9/9 | 0.414721 | 0.382683 | 0 |
| Scale error, similarity | 9/9 | 0 | 0 | 0 |
| Wrong clock, no alignment | 8/9 | 0.780361 | 0 | 0 |
| Wrong clock, rigid | 8/9 | 0 | 0 | 0 |
| Corrected clock, no alignment | 9/9 | 0 | 0 | 0 |

Zeros are rounded; floating-point fits can leave tiny residuals. Similarity recovers scale 0.8 for the enlarged trajectory. The two wrong-clock cases have six RPE pairs; all other cases have seven.

**A zero aligned score does not validate timing.** With offset 0 and tolerance 0.15 s, estimate rows 0..7 match reference rows 1..8, each 0.1 s apart. These are wrong physical correspondences. The reference's first row and estimate's last row are omitted. Because this fixture moves uniformly around a circle, a single +22.5 degree rigid transform maps those wrong pairs exactly, and their relative motions already agree. Both aligned position RMSE and RPE are zero while the association is wrong. Applying the known -0.4 s correction restores all nine intended matches. This is a deliberately constructed ambiguity, not a timing-calibration method.

Try removing an interior CSV row. Compare match coverage, unmatched indices, actual RPE intervals, and the surviving pair count. Then change `--lag` and read the interval fields again. The default one-second comparison remains a time-based question even when a row disappears.

## Sources and limits

- [Sturm et al., A Benchmark for the Evaluation of RGB-D SLAM Systems](https://cvg.cit.tum.de/_media/spezial/bib/sturm12iros.pdf): ATE and pose-frame RPE definitions.
- [Zhang and Scaramuzza, A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry](https://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf): alignment choices and evaluation interpretation.
- [RPG trajectory evaluation](https://github.com/uzh-rpg/rpg_trajectory_evaluation): timestamped inputs and declared alignment/evaluation settings.

The reference is exact by construction. Real references have calibration, synchronization, and measurement uncertainty. The exercise does not model that uncertainty, estimate confidence intervals, detect tracking failure between samples, or provide a robot-safety assessment. Inspect the trajectory, coverage, frame conventions, timing calibration, and full protocol before interpreting a scalar error.