Evaluating map quality and localization accuracy

1. Engineering problem

SLAM estimates are in an arbitrary frame; a reference trajectory is in another. Comparing them without alignment measures the frame difference, not the SLAM quality. Absolute and relative errors answer different questions.

2. When is this needed?

• Comparing SLAM packages on the same data.
• Verifying an upgrade.
• Setting an acceptance criterion.

3. Selection and design decisions

Common metrics: absolute trajectory error (ATE): RMS position difference after the best rigid alignment of the estimate to the reference; relative pose error (RPE): error over a fixed distance or time step, which shows local drift; and for maps, comparison to a surveyed map or measured distances between features. A reference can come from a motion-capture system, a total station, RTK GNSS outdoors, or known landmark positions.

4. Advantages and disadvantages

• ATE hides local jumps; RPE hides slow global drift; report both.
• Reference quality limits what you can measure: the reference must be several times better than the SLAM.
• More runs give better statistics and cost time.

5. Requirements and constraints

• The reference is at least several times more accurate than the target error.
• Time stamps of estimate and reference are matched.
• Alignment type (rigid, or with scale for monocular) is stated.

6. Architecture

estimate + reference ─▶ time match ─▶ rigid alignment ─▶ ATE (RMS) and RPE ─▶ report with route and conditions

7. Implementation

1. Record a run with a reference trajectory.
2. Match samples by time.
3. Align the estimate to the reference (below).
4. Compute ATE and RPE.
5. Report with the route, speed, environment and software version.

import numpy as np
def align(A, B):                                   # rigid alignment of estimate A onto ground truth B (Kabsch)
    ca, cb = A.mean(0), B.mean(0)
    U, _, Vt = np.linalg.svd((A - ca).T @ (B - cb))
    d = np.sign(np.linalg.det(Vt.T @ U.T))
    R = Vt.T @ np.diag([1, 1, d]) @ U.T
    return A @ R.T + (cb - R @ ca)
t = np.linspace(0, 2 * np.pi, 50)
gt = np.stack([5 * np.cos(t), 5 * np.sin(t), 0 * t], 1)
a = np.radians(10)
Rz = np.array([[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]])
est = gt @ Rz.T + [3.0, -2.0, 0.0] + np.stack([0.02 * np.sin(7 * t), 0.02 * np.cos(5 * t), 0 * t], 1)
raw = np.sqrt(((est - gt) ** 2).sum(1).mean())
ate = np.sqrt(((align(est, gt) - gt) ** 2).sum(1).mean())
al = align(est, gt)
rpe = np.sqrt(((np.diff(al, axis=0) - np.diff(gt, axis=0)) ** 2).sum(1).mean())
print(f"RMSE without alignment {raw:.3f} m, absolute trajectory error after alignment {ate:.3f} m, relative error per step {rpe:.4f} m")

Printed (checked by the site’s tests):

RMSE without alignment 3.699 m, absolute trajectory error after alignment 0.020 m, relative error per step 0.0152 m

Reading it: an estimate that is rotated 10° and shifted by (3, −2) m has an unaligned RMSE of 3.7 m, which is just the frame difference; after rigid alignment the absolute trajectory error is 0.020 m and the relative error per step is 0.015 m. The data are synthetic.

8. Real examples

• Mocap lab: ATE and RPE of three SLAM packages on the same recorded run.

9. Failure scenarios

• Reporting unaligned RMSE.
• Using a reference that is no better than the SLAM.
• Comparing packages on different routes.

10. Troubleshooting

1. If ATE is large and RPE is small, there is slow drift or a scale error.
2. If both are large, check time matching and calibration.
3. If ATE changes with the start frame, check the alignment.

11. Validation

• The example prints the line shown (executed by the site tests).
• Results are reproducible on a second run of the same data.

12. Engineering checklist

• Reference quality known
• Time matched
• Alignment stated
• ATE and RPE reported
• Route and conditions recorded
• Software version recorded

13. Further references

• ATE and RPE are standard SLAM evaluation metrics; the exact definitions in benchmark tools (for example the TUM RGB-D benchmark tools or evo) have options that you should read in their documentation.

Related on FixTheRobot