Score localization estimates against truth at their capture time

app/autonomy/AutonomyVisualizationModel.js

@@ -1,3 +1,4 @@
+import { Euler, Quaternion, Vector3 } from "three";
 /**
  * Renderer-neutral primitives for candidate/oracle perception and localization overlays.
  * Capture-time metadata travels with each snapshot so replay does not need ROS re-decode.
@@ -468,6 +469,48 @@ export function localizationError(estimate, truth) {
         dz,
         horizontalM: Math.hypot(dx, dy),
         positionM: Math.hypot(dx, dy, dz),
+        // Yaw (the ZYX Euler angle about +Z) of the rotation from truth to estimate, in (-pi, pi].
+        headingRad: new Euler().setFromQuaternion(
+            new Quaternion().copy(truth.orientation).invert().multiply(new Quaternion().copy(estimate.orientation)),
+            "ZYX",
+        ).z,
+        truthCaptureTimeNs: truth.captureTimeNs ?? null,
+    };
+}
+
+/**
+ * Truth pose at `captureTimeNs`, interpolated between the two nearest samples.
+ * `history` holds normalized truth odometry sorted by ascending `captureTimeNs`.
+ * Returns null when the time is outside the history.
+ */
+export function truthAtCaptureTime(history, captureTimeNs) {
+    if (!Array.isArray(history) || history.length === 0 || !Number.isFinite(captureTimeNs)) return null;
+    let high = history.length - 1;
+    if (captureTimeNs > history[high].captureTimeNs || captureTimeNs < history[0].captureTimeNs) return null;
+    let low = 0;
+    while (high - low > 1) {
+        const middle = (low + high) >> 1;
+        if (history[middle].captureTimeNs <= captureTimeNs) low = middle;
+        else high = middle;
+    }
+    const before = history[low];
+    const after = history[high];
+    if (before.captureTimeNs === captureTimeNs) return before;
+    if (after.captureTimeNs === captureTimeNs) return after;
+    const alpha = (captureTimeNs - before.captureTimeNs) / (after.captureTimeNs - before.captureTimeNs);
+    const interpolated = new Vector3().lerpVectors(
+        new Vector3().copy(before.position),
+        new Vector3().copy(after.position),
+        alpha,
+    );
+    const position = { x: interpolated.x, y: interpolated.y, z: interpolated.z };
+    const rotation = new Quaternion().copy(before.orientation).slerp(new Quaternion().copy(after.orientation), alpha);
+    return {
+        ...before,
+        captureTimeNs,
+        position,
+        orientation: { x: rotation.x, y: rotation.y, z: rotation.z, w: rotation.w },
+        threePosition: rep103ToThreeVector(position),
     };
 }
 

app/autonomy/CandidateOutputRuntime.js

@@ -13,6 +13,7 @@ import {
     normalizeLegacyBoxes,
     normalizeOdometry,
     normalizeSemanticImage,
+    truthAtCaptureTime,
 } from "./AutonomyVisualizationModel.js";
 import {
     oracleTopicSignalPath,
@@ -31,6 +32,9 @@ const LOCALIZATION_CONTRACTS = new Set([
     "localization-estimate",
 ]);
 
+// Estimates older than this have no truth to compare against.
+const TRUTH_HISTORY_NS = 10_000_000_000;
+
 function byteLengthOf(data) {
     if (!data) return 0;
     if (typeof data.byteLength === "number") return data.byteLength;
@@ -47,6 +51,7 @@ export class CandidateOutputRuntime {
         this.lastLocalization = emptyLocalizationSnapshot();
         this.lastGoodPerception = null;
         this.lastGoodLocalization = null;
+        this.truthHistory = [];
         this._defineSignals();
     }
 
@@ -55,6 +60,7 @@ export class CandidateOutputRuntime {
         this.lastLocalization = emptyLocalizationSnapshot();
         this.lastGoodPerception = null;
         this.lastGoodLocalization = null;
+        this.truthHistory = [];
         this._publishPerception(this.lastPerception, 0, 0);
         this._publishLocalization(this.lastLocalization, 0, 0);
     }
@@ -103,11 +109,12 @@ export class CandidateOutputRuntime {
         this._publishPerception(nextPerception, applyStep, applyTimeNs);
 
         if (truth) {
-            const truthNorm = normalizeOdometry(truth, { source: "oracle" });
+            this._recordTruth(normalizeOdometry(truth, { source: "oracle" }));
+            const alignedTruth = this._truthFor(this.lastLocalization.estimate);
             const nextLoc = {
                 ...this.lastLocalization,
-                truth: truthNorm,
-                error: localizationError(this.lastLocalization.estimate, truthNorm),
+                truth: alignedTruth,
+                error: localizationError(this.lastLocalization.estimate, alignedTruth),
             };
             this.lastLocalization = nextLoc;
             this._publishLocalization(nextLoc, applyStep, applyTimeNs);
@@ -161,7 +168,7 @@ export class CandidateOutputRuntime {
                         : null,
                 }
                 : emptyLocalizationSnapshot(meta);
-            ghost.truth = this.lastLocalization.truth;
+            ghost.truth = this._truthFor(ghost.estimate);
             ghost.error = localizationError(ghost.estimate, ghost.truth);
             this.lastLocalization = ghost;
             this._publishLocalization(ghost, applyStep, applyTimeNs);
@@ -213,8 +220,8 @@ export class CandidateOutputRuntime {
     _ingestLocalization(envelope, { applyStep, applyTimeNs, status }) {
         const estimate = normalizeOdometry(envelope.value, { source: "candidate", status });
         const truthRaw = this.telemetry?.read?.(oracleTopicSignalPath("truth-odometry"))?.value;
-        const truth = this.lastLocalization.truth
-            || normalizeOdometry(truthRaw, { source: "oracle" });
+        if (truthRaw) this._recordTruth(normalizeOdometry(truthRaw, { source: "oracle" }));
+        const truth = this._truthFor(estimate);
         const next = {
             captureTimeNs: envelope.captureTimeNs ?? null,
             arrivalTimeNs: envelope.arrivalTimeNs ?? null,
@@ -232,6 +239,27 @@ export class CandidateOutputRuntime {
         return next;
     }
 
+    _recordTruth(truth) {
+        if (!truth || !Number.isFinite(truth.captureTimeNs)) return;
+        const last = this.truthHistory[this.truthHistory.length - 1];
+        if (last && truth.captureTimeNs <= last.captureTimeNs) return;
+        this.truthHistory.push(truth);
+        const oldest = truth.captureTimeNs - TRUTH_HISTORY_NS;
+        let drop = 0;
+        while (drop < this.truthHistory.length && this.truthHistory[drop].captureTimeNs < oldest) drop += 1;
+        if (drop > 0) this.truthHistory.splice(0, drop);
+    }
+
+    // Score an estimate against truth at the estimate's own capture time, so
+    // transport latency does not count as position error.
+    _truthFor(estimate) {
+        if (!estimate) return null;
+        if (!Number.isFinite(estimate.captureTimeNs)) {
+            return this.truthHistory[this.truthHistory.length - 1] ?? null;
+        }
+        return truthAtCaptureTime(this.truthHistory, estimate.captureTimeNs);
+    }
+
     _transformToMapBinder(captureTimeNs) {
         const runtime = this.transformRuntime;
         if (!runtime?.lookupTransformChain) return null;