rpx_benchmark.pose_metrics

  1"""RPX-RCPE evaluation metrics — novel + standard.
  2
  3Consumes per-pair results (from the runner) enriched with the
  4metadata fields that :class:`PosePairGenerator` attaches
  5(``pair_type``, ``rotation_bin``, ``chain_id``, ``chain_position``).
  6
  7Standard
  8--------
  9- ``rotation_error_deg``, ``translation_error_m`` — already computed
 10  per-sample by ``rpx_benchmark.metrics.pose``.
 11- ``translation_angular_deg`` — scale-invariant direction error.
 12- ``pose_error_max_deg`` — max(rot, trans_angular).
 13- ``AUC@5° / @10° / @20°`` — standard pose AUC.
 14
 15Novel (unique to RPX-RCPE)
 16--------------------------
 17- **Metric AUC@(θ°, d cm)** — joint threshold on rotation AND metric
 18  translation L2. Only meaningful with metric-scale GT (MegaDepth
 19  can't do this).
 20- **Cross-phase Δ** — performance drop from intra-phase to cross-phase
 21  pairs at matched rotation bins. Measures scene-change robustness.
 22- **Temporal drift** — accumulated rotation/translation error over a
 23  chain. Measures stability for SLAM / trajectory integration.
 24- **Per-bin / per-type breakdowns** — full disaggregation by pair type
 25  and rotation difficulty.
 26"""
 27
 28from __future__ import annotations
 29
 30from typing import Any, Dict, List, Sequence
 31
 32import numpy as np
 33
 34CANONICAL_POSE_METRICS = ("auc_5deg", "auc_10deg", "auc_20deg")
 35PHASE_LABELS = {0: "clutter", 1: "interaction", 2: "clean"}
 36_trapezoid = getattr(np, "trapezoid", None) or np.trapz  # type: ignore[attr-defined]
 37
 38# ─────────────────────────────────────────────────────────────────────────────
 39# Per-pair error functions (pure numpy, no framework deps)
 40# ─────────────────────────────────────────────────────────────────────────────
 41
 42def rotation_error_deg(R_pred: np.ndarray, R_gt: np.ndarray) -> float:
 43    R_rel = R_pred @ R_gt.T
 44    cos_t = np.clip((np.trace(R_rel) - 1.0) / 2.0, -1.0, 1.0)
 45    return float(np.degrees(np.arccos(cos_t)))
 46
 47
 48def translation_l2(t_pred: np.ndarray, t_gt: np.ndarray) -> float:
 49    return float(np.linalg.norm(t_pred - t_gt))
 50
 51
 52def translation_angular_deg(t_pred: np.ndarray, t_gt: np.ndarray) -> float:
 53    n_pred = np.linalg.norm(t_pred)
 54    n_gt = np.linalg.norm(t_gt)
 55    if n_pred < 1e-9 or n_gt < 1e-9:
 56        return 180.0
 57    cos_a = np.clip(np.dot(t_pred, t_gt) / (n_pred * n_gt), -1.0, 1.0)
 58    return float(np.degrees(np.arccos(cos_a)))
 59
 60
 61# ─────────────────────────────────────────────────────────────────────────────
 62# AUC computation
 63# ─────────────────────────────────────────────────────────────────────────────
 64
 65def auc_at_thresholds(
 66    errors: np.ndarray,
 67    thresholds: Sequence[float] = (5.0, 10.0, 20.0),
 68) -> Dict[str, float]:
 69    """Fraction-of-pairs ≤ threshold, integrated (AUC) up to each threshold.
 70
 71    Returns ``{"auc@5": ..., "auc@10": ..., "auc@20": ...}`` etc.
 72    """
 73    errors = np.sort(errors)
 74    n = len(errors)
 75    out: Dict[str, float] = {}
 76    for th in thresholds:
 77        # Trapezoidal AUC of the recall curve from 0 to th
 78        bins = np.linspace(0, th, num=100)
 79        recall = np.array([(errors <= b).sum() / max(n, 1) for b in bins])
 80        auc = float(_trapezoid(recall, bins) / th)
 81        out[f"auc@{th:.0f}"] = auc
 82    return out
 83
 84
 85def metric_auc(
 86    rot_errors: np.ndarray,
 87    trans_errors_m: np.ndarray,
 88    rot_thresholds: Sequence[float] = (5.0, 10.0, 20.0),
 89    trans_thresholds_cm: Sequence[float] = (5.0, 10.0, 20.0),
 90) -> Dict[str, float]:
 91    """Joint AUC: fraction of pairs with rot ≤ θ AND trans ≤ d.
 92
 93    Novel metric — only possible with metric-scale GT translation.
 94    Returns e.g. ``{"metric_auc@(5deg,5cm)": 0.32, ...}``.
 95    """
 96    out: Dict[str, float] = {}
 97    for th_r in rot_thresholds:
 98        for th_t in trans_thresholds_cm:
 99            th_t_m = th_t / 100.0
100            frac = float(np.mean(
101                (rot_errors <= th_r) & (trans_errors_m <= th_t_m)
102            ))
103            out[f"metric_auc@({th_r:.0f}deg,{th_t:.0f}cm)"] = frac
104    return out
105
106
107def metric_auc_curve(
108    rot_errors_deg: np.ndarray,
109    trans_errors_m: np.ndarray,
110    max_rotation_deg: float = 20.0,
111    max_translation_cm: float = 20.0,
112) -> float:
113    """Joint rotation/metric-translation AUC used as the paper's M-AUC.
114
115    At curve position ``u`` a pair succeeds only when both
116    ``rotation <= u*max_rotation_deg`` and
117    ``translation <= u*max_translation_cm``. Integrating joint recall over
118    ``u in [0, 1]`` produces one bounded higher-is-better number.
119    """
120    rot = np.asarray(rot_errors_deg, dtype=np.float64)
121    trans_cm = np.asarray(trans_errors_m, dtype=np.float64) * 100.0
122    valid = np.isfinite(rot) & np.isfinite(trans_cm)
123    if not np.any(valid):
124        return 0.0
125    u = np.linspace(0.0, 1.0, 101)
126    recall = np.array([
127        np.mean(
128            (rot[valid] <= value * max_rotation_deg)
129            & (trans_cm[valid] <= value * max_translation_cm)
130        )
131        for value in u
132    ])
133    return float(_trapezoid(recall, u))
134
135
136def canonical_metric_vector(
137    per_pair: List[Dict[str, Any]],
138    *,
139    metric_translation_available: bool = True,
140) -> Dict[str, float | None]:
141    """Return the active D6 K=3 angular vector for one pose cell.
142
143    The three angular AUCs use ``max(rotation error, translation-direction
144    error)``. Metric-translation M-AUC is intentionally excluded from the
145    active protocol so metric- and up-to-scale adapters remain comparable.
146    """
147    valid = [
148        row for row in per_pair
149        if np.isfinite(row.get("pose_error_max_deg", np.nan))
150    ]
151    if not valid:
152        return {key: None for key in CANONICAL_POSE_METRICS}
153    pose_error = np.asarray(
154        [row["pose_error_max_deg"] for row in valid], dtype=np.float64
155    )
156    auc = auc_at_thresholds(pose_error, (5.0, 10.0, 20.0))
157    values: Dict[str, float | None] = {
158        "auc_5deg": auc["auc@5"],
159        "auc_10deg": auc["auc@10"],
160        "auc_20deg": auc["auc@20"],
161    }
162    return values
163
164
165def build_rcpe_cells(
166    per_pair: List[Dict[str, Any]],
167    *,
168    model_name: str,
169    difficulty: str,
170    metric_translation_available: bool,
171) -> tuple[List[Dict[str, Any]], List[Dict[str, Any]]]:
172    """Build canonical per-(scene, phase) D6 cells from intra-phase pairs.
173
174    Cross-phase pairs and temporal chains are diagnostics and intentionally
175    do not enter the phase MANOVA/JEDI cells. Their results remain in
176    ``cross_phase_delta`` and ``temporal_drift`` respectively.
177    """
178    from .cell_log import cell_from_metrics
179
180    buckets: Dict[tuple[str, int], List[Dict[str, Any]]] = {}
181    for row in per_pair:
182        if row.get("pair_type") != "intra_phase":
183            continue
184        try:
185            phase = int(row.get("phase"))
186        except (TypeError, ValueError):
187            continue
188        if phase not in PHASE_LABELS:
189            continue
190        scene = str(row.get("scene_id") or "")
191        if not scene:
192            continue
193        buckets.setdefault((scene, phase), []).append(row)
194
195    cells: List[Dict[str, Any]] = []
196    analysis_rows: List[Dict[str, Any]] = []
197    for (scene, phase), rows in sorted(buckets.items()):
198        metrics = canonical_metric_vector(
199            rows,
200            metric_translation_available=metric_translation_available,
201        )
202        phase_label = PHASE_LABELS[phase]
203        cells.append(cell_from_metrics(
204            metrics,
205            model_name=model_name,
206            task="relative_camera_pose",
207            scene_id=scene,
208            phase=phase_label,
209            n_samples=len(rows),
210            difficulty=difficulty,
211            metric_keys=CANONICAL_POSE_METRICS,
212        ))
213        analysis_rows.append({
214            "id": f"{scene}_{phase_label}",
215            "scene": scene,
216            "phase": phase_label,
217            "difficulty": difficulty,
218            **metrics,
219        })
220    return cells, analysis_rows
221
222
223# ─────────────────────────────────────────────────────────────────────────────
224# Disaggregated breakdowns
225# ─────────────────────────────────────────────────────────────────────────────
226
227def _group_mean(
228    per_pair: List[Dict[str, Any]],
229    group_key: str,
230    metric_keys: Sequence[str],
231) -> Dict[str, Dict[str, float]]:
232    """Group per-pair results by ``group_key`` and compute means."""
233    groups: Dict[str, List[Dict]] = {}
234    for p in per_pair:
235        g = str(p.get(group_key, "unknown"))
236        groups.setdefault(g, []).append(p)
237
238    out: Dict[str, Dict[str, float]] = {}
239    for g, items in sorted(groups.items()):
240        means: Dict[str, float] = {"n_pairs": float(len(items))}
241        for k in metric_keys:
242            vals = [item[k] for item in items if k in item]
243            if vals:
244                means[k] = float(np.mean(vals))
245        out[g] = means
246    return out
247
248
249def per_bin_breakdown(per_pair: List[Dict[str, Any]]) -> Dict[str, Dict[str, float]]:
250    """Aggregate by rotation_bin (easy / medium / hard / extreme)."""
251    return _group_mean(per_pair, "rotation_bin", [
252        "rotation_error_deg", "translation_error_m",
253        "translation_angular_deg", "pose_error_max_deg",
254    ])
255
256
257def per_type_breakdown(per_pair: List[Dict[str, Any]]) -> Dict[str, Dict[str, float]]:
258    """Aggregate by pair_type (intra_phase / cross_phase / temporal_chain)."""
259    return _group_mean(per_pair, "pair_type", [
260        "rotation_error_deg", "translation_error_m",
261        "translation_angular_deg", "pose_error_max_deg",
262    ])
263
264
265# ─────────────────────────────────────────────────────────────────────────────
266# Cross-phase Δ
267# ─────────────────────────────────────────────────────────────────────────────
268
269def cross_phase_delta(
270    per_pair: List[Dict[str, Any]],
271    metric_key: str = "rotation_error_deg",
272) -> Dict[str, float]:
273    """Performance drop from intra-phase to cross-phase at matched bins.
274
275    Returns ``{"delta_easy": ..., "delta_medium": ..., ...}`` where
276    delta = mean(cross) - mean(intra) for the same rotation bin.
277    Positive = cross-phase is harder (expected).
278    """
279    intra: Dict[str, List[float]] = {}
280    cross: Dict[str, List[float]] = {}
281    for p in per_pair:
282        pt = p.get("pair_type")
283        rb = p.get("rotation_bin")
284        if rb is None or metric_key not in p:
285            continue
286        val = p[metric_key]
287        if pt == "intra_phase":
288            intra.setdefault(rb, []).append(val)
289        elif pt == "cross_phase":
290            cross.setdefault(rb, []).append(val)
291
292    out: Dict[str, float] = {}
293    for bin_name in ("easy", "medium", "hard", "extreme"):
294        if bin_name in intra and bin_name in cross:
295            delta = float(np.mean(cross[bin_name]) - np.mean(intra[bin_name]))
296            out[f"delta_{bin_name}"] = delta
297    if intra and cross:
298        all_intra = [v for vs in intra.values() for v in vs]
299        all_cross = [v for vs in cross.values() for v in vs]
300        out["delta_overall"] = float(np.mean(all_cross) - np.mean(all_intra))
301    return out
302
303
304def cross_phase_auc_delta(
305    per_pair: List[Dict[str, Any]],
306    thresholds: Sequence[float] = (5.0, 10.0, 20.0),
307) -> Dict[str, float]:
308    """Matched-bin AUC loss from intra-phase to Clutter/Clean pairs.
309
310    Positive means cross-phase is worse. The overall value is the mean of
311    available bin/threshold deltas, so lower is better as in the paper table.
312    """
313    deltas: Dict[str, float] = {}
314    all_values = []
315    for bin_name in ("easy", "medium", "hard", "extreme"):
316        intra = [
317            float(row["pose_error_max_deg"])
318            for row in per_pair
319            if row.get("pair_type") == "intra_phase"
320            and row.get("rotation_bin") == bin_name
321            and np.isfinite(row.get("pose_error_max_deg", np.nan))
322        ]
323        cross = [
324            float(row["pose_error_max_deg"])
325            for row in per_pair
326            if row.get("pair_type") == "cross_phase"
327            and row.get("rotation_bin") == bin_name
328            and np.isfinite(row.get("pose_error_max_deg", np.nan))
329        ]
330        if not intra or not cross:
331            continue
332        intra_auc = auc_at_thresholds(np.asarray(intra), thresholds)
333        cross_auc = auc_at_thresholds(np.asarray(cross), thresholds)
334        for threshold in thresholds:
335            key = f"auc@{threshold:.0f}"
336            value = float(intra_auc[key] - cross_auc[key])
337            deltas[f"delta_{bin_name}_{key}"] = value
338            all_values.append(value)
339    if all_values:
340        deltas["delta_overall"] = float(np.mean(all_values))
341    return deltas
342
343
344# ─────────────────────────────────────────────────────────────────────────────
345# Temporal drift
346# ─────────────────────────────────────────────────────────────────────────────
347
348def temporal_drift(
349    per_pair: List[Dict[str, Any]], *, metric_translation_available: bool = True
350) -> Dict[str, Any]:
351    """Accumulated error along temporal chains.
352
353    Groups chain pairs by ``chain_id``, orders by ``chain_position``,
354    and computes cumulative rotation + translation error.
355
356    Returns ``{"chains": [...], "mean_drift_rot_deg": ...,
357    "mean_drift_trans_m": ...}``.
358    """
359    chains: Dict[str, List[Dict]] = {}
360    for p in per_pair:
361        if p.get("pair_type") != "temporal_chain":
362            continue
363        cid = (p.get("metadata") or p).get("chain_id")
364        if cid is None:
365            continue
366        chains.setdefault(cid, []).append(p)
367
368    chain_results = []
369    for cid, pairs in sorted(chains.items()):
370        pairs.sort(key=lambda x: (x.get("metadata") or x).get("chain_position", 0))
371        pred_total = np.eye(4, dtype=np.float64)
372        gt_total = np.eye(4, dtype=np.float64)
373        legacy_cum_rot = 0.0
374        legacy_cum_trans = 0.0
375        steps = []
376        for p in pairs:
377            if all(
378                key in p
379                for key in (
380                    "pred_rotation", "pred_translation",
381                    "gt_rotation", "gt_translation",
382                )
383            ):
384                pred_step = np.eye(4, dtype=np.float64)
385                gt_step = np.eye(4, dtype=np.float64)
386                pred_step[:3, :3] = np.asarray(p["pred_rotation"])
387                pred_step[:3, 3] = np.asarray(p["pred_translation"])
388                gt_step[:3, :3] = np.asarray(p["gt_rotation"])
389                gt_step[:3, 3] = np.asarray(p["gt_translation"])
390                pred_total = pred_total @ pred_step
391                gt_total = gt_total @ gt_step
392                cum_rot = rotation_error_deg(pred_total[:3, :3], gt_total[:3, :3])
393                cum_trans = translation_l2(pred_total[:3, 3], gt_total[:3, 3])
394            else:
395                legacy_cum_rot += p.get("rotation_error_deg", 0.0)
396                legacy_cum_trans += p.get("translation_error_m", 0.0)
397                cum_rot = legacy_cum_rot
398                cum_trans = legacy_cum_trans
399            steps.append({
400                "position": (p.get("metadata") or p).get("chain_position", 0),
401                "rotation_error_deg": p.get("rotation_error_deg", 0.0),
402                "translation_error_m": (
403                    p.get("translation_error_m", 0.0)
404                    if metric_translation_available else None
405                ),
406                "cumulative_rotation_error": cum_rot,
407                "cumulative_translation_error": (
408                    cum_trans if metric_translation_available else None
409                ),
410            })
411        chain_results.append({
412            "chain_id": cid,
413            "length": len(pairs),
414            "total_drift_rot_deg": cum_rot,
415            "total_drift_trans_m": cum_trans if metric_translation_available else None,
416            "steps": steps,
417        })
418
419    if not chain_results:
420        return {
421            "chains": [],
422            "mean_drift_rot_deg": 0.0,
423            "mean_drift_trans_m": 0.0 if metric_translation_available else None,
424        }
425
426    return {
427        "chains": chain_results,
428        "mean_drift_rot_deg": float(np.mean([c["total_drift_rot_deg"] for c in chain_results])),
429        "mean_drift_trans_m": (
430            float(np.mean([c["total_drift_trans_m"] for c in chain_results]))
431            if metric_translation_available else None
432        ),
433        "n_chains": len(chain_results),
434    }
435
436
437# ─────────────────────────────────────────────────────────────────────────────
438# Full evaluation — the one function users call
439# ─────────────────────────────────────────────────────────────────────────────
440
441def evaluate_rcpe(
442    per_pair: List[Dict[str, Any]],
443    rot_auc_thresholds: Sequence[float] = (5.0, 10.0, 20.0),
444    metric_rot_thresholds: Sequence[float] = (5.0, 10.0, 20.0),
445    metric_trans_thresholds_cm: Sequence[float] = (5.0, 10.0, 20.0),
446    metric_translation_available: bool = True,
447) -> Dict[str, Any]:
448    """Compute the full RPX-RCPE metric basket from per-pair results.
449
450    Parameters
451    ----------
452    per_pair : list of dict
453        Each dict must contain at minimum ``rotation_error_deg`` and
454        ``translation_error_m``.  Should also carry ``pair_type``,
455        ``rotation_bin``, ``chain_id``, ``chain_position`` from the
456        PosePairGenerator metadata for the novel metrics to fire.
457
458    Returns
459    -------
460    dict with keys:
461        ``aggregated`` — overall means.
462        ``standard_auc`` — AUC@5°/10°/20° on pose_error_max_deg.
463        ``metric_auc`` — null with status ``skipped_by_protocol``.
464        ``per_bin`` — breakdown by rotation difficulty.
465        ``per_type`` — breakdown by pair type (intra/cross/temporal).
466        ``cross_phase_delta`` — perf drop intra→cross (novel).
467        ``temporal_drift`` — accumulated error over chains (novel).
468    """
469    if not per_pair:
470        return {}
471
472    # Ensure derived per-pair fields exist.  The runner now emits
473    # translation_angular_deg and pose_error_max_deg directly; this
474    # fallback handles legacy per-pair dicts that only carry the
475    # two original scalar errors.
476    for p in per_pair:
477        if "translation_angular_deg" not in p:
478            p["translation_angular_deg"] = float("nan")
479        if "pose_error_max_deg" not in p:
480            ta = p.get("translation_angular_deg", 0.0)
481            if np.isnan(ta):
482                ta = 0.0
483            p["pose_error_max_deg"] = max(
484                p.get("rotation_error_deg", 0.0), ta,
485            )
486
487    # Headline angular AUC values are defined on exact-gap intra-phase pairs.
488    # Cross-phase pairs and temporal chains have separate diagnostics.
489    headline_rows = [p for p in per_pair if p.get("pair_type") == "intra_phase"]
490    if not headline_rows:  # backwards compatibility for legacy manifests
491        headline_rows = per_pair
492    rot_errs = np.array([p["rotation_error_deg"] for p in headline_rows])
493    trans_errs = np.array([p["translation_error_m"] for p in headline_rows])
494    pose_max = np.array([p["pose_error_max_deg"] for p in headline_rows])
495    canonical = canonical_metric_vector(
496        headline_rows,
497        metric_translation_available=metric_translation_available,
498    )
499
500    per_bin = per_bin_breakdown(per_pair)
501    per_type = per_type_breakdown(per_pair)
502    if not metric_translation_available:
503        for breakdown in (per_bin, per_type):
504            for values in breakdown.values():
505                values.pop("translation_error_m", None)
506
507    return {
508        "n_pairs": len(per_pair),
509        "n_headline_intra_pairs": len(headline_rows),
510        "metric_translation_available": metric_translation_available,
511        "aggregated": {
512            "rotation_error_deg": float(np.mean(rot_errs)),
513            "rotation_error_deg_median": float(np.median(rot_errs)),
514            "translation_error_m": (
515                float(np.mean(trans_errs)) if metric_translation_available else None
516            ),
517            "translation_error_m_median": (
518                float(np.median(trans_errs)) if metric_translation_available else None
519            ),
520            "pose_error_max_deg": float(np.mean(pose_max)),
521        },
522        "standard_auc": auc_at_thresholds(pose_max, rot_auc_thresholds),
523        "canonical": canonical,
524        "m_auc": None,
525        "metric_auc": None,
526        "metric_auc_status": "skipped_by_protocol",
527        "per_bin": per_bin,
528        "per_type": per_type,
529        "cross_phase_delta": cross_phase_auc_delta(per_pair, rot_auc_thresholds),
530        "cross_phase_error_delta": cross_phase_delta(per_pair, "rotation_error_deg"),
531        "cross_phase_delta_trans": (
532            cross_phase_delta(per_pair, "translation_error_m")
533            if metric_translation_available else {}
534        ),
535        "temporal_drift": temporal_drift(
536            per_pair, metric_translation_available=metric_translation_available
537        ),
538    }