rpx_benchmark.ar_pose_audit

  1"""AR-board consistency audit for RPX single-object camera poses.
  2
  3The single-object captures contain an 18-marker FewSOL board in the D435 RGB
  4images and a synchronized camera-to-world pose saved from the T265.  The RPX
  5capture rig is documented by the dataset owners as already camera-frame
  6aligned.  This module deliberately does *not* estimate a D435-to-T265
  7extrinsic.  Instead it compares relative SE(3) motions, which also cancels the
  8unrelated board and T265 world origins.
  9
 10The ALVAR ``MarkerData`` codes below are the canonical IDs 0--17 distributed
 11with ``ros-perception/ar_track_alvar``.  They are represented as an OpenCV
 12custom dictionary (5x5 payload, two-bit black border), allowing an offline
 13audit directly from RPX ``rgb.tar`` files without ROS.
 14"""
 15
 16from __future__ import annotations
 17
 18import csv
 19import io
 20import json
 21import tarfile
 22from dataclasses import dataclass
 23from pathlib import Path
 24from typing import Any, Iterable, Mapping, Sequence
 25
 26import numpy as np
 27from scipy.spatial.transform import Rotation
 28
 29from .exceptions import ConfigError
 30
 31# ALVAR MarkerData IDs 0--17, sampled from the canonical marker generator.
 32_ALVAR_5X5_BITS = (
 33    "1101111011101011111111111",
 34    "1101111011101010011011101",
 35    "1101111011101011011010110",
 36    "1101111011101010111110100",
 37    "1101111011101010111001110",
 38    "1101111011101011011101100",
 39    "1101111011101010011100111",
 40    "1101111011101011111000101",
 41    "1101111011101010010111110",
 42    "1101111011101011110011100",
 43    "1101111011101010110010111",
 44    "1101111011101011010110101",
 45    "1101111011101011010001111",
 46    "1101111011101010110101101",
 47    "1101111011101011110100110",
 48    "1101111011101010010000100",
 49    "0001011001101011111111111",
 50    "0001011001101010011011101",
 51)
 52
 53# FewSOL marker board geometry, metres.  Marker coordinates are measured from
 54# the upper-left of the 70.4 x 50.4 cm board; +Y below is converted to +Y up.
 55BOARD_WIDTH_M = 0.704
 56BOARD_HEIGHT_M = 0.504
 57MARKER_SIZE_M = 0.059
 58_MARKER_CENTRES_CM = (
 59    (2.95, 2.95),
 60    (15.85, 2.95),
 61    (28.75, 2.95),
 62    (41.55, 2.95),
 63    (54.45, 2.95),
 64    (67.45, 2.95),
 65    (67.45, 13.75),
 66    (67.45, 25.00),
 67    (67.45, 36.20),
 68    (67.45, 47.45),
 69    (54.45, 47.45),
 70    (41.55, 47.45),
 71    (28.70, 47.45),
 72    (15.80, 47.45),
 73    (2.95, 47.45),
 74    (2.95, 36.75),
 75    (2.95, 25.55),
 76    (2.95, 14.25),
 77)
 78
 79# FewSOL's published D435 calibration.  CLI callers can override every value.
 80DEFAULT_CAMERA_MATRIX = np.array(
 81    [[611.10888672, 0.0, 315.51083374], [0.0, 610.02844238, 237.73669434], [0.0, 0.0, 1.0]],
 82    dtype=np.float64,
 83)
 84
 85# librealsense pose coordinates are X right, Y up, Z back; OpenCV RGB camera
 86# coordinates are X right, Y down, Z forward.  This fixed basis change is not a
 87# fitted camera extrinsic.  save_device_data.py writes the raw T265 quaternion
 88# and translation, so it must be applied unless poses were converted later.
 89_T265_TO_OPENCV = np.diag([1.0, -1.0, -1.0, 1.0])
 90
 91
 92@dataclass(frozen=True)
 93class BoardObservation:
 94    """One D435 observation of the static marker board."""
 95
 96    camera_from_board: np.ndarray
 97    marker_ids: tuple[int, ...]
 98    reprojection_rmse_px: float
 99    inlier_corners: int
100
101
102def _cv2() -> Any:
103    try:
104        import cv2
105    except ImportError as exc:  # pragma: no cover - environment-dependent
106        raise ConfigError(
107            "AR pose auditing requires OpenCV contrib; install "
108            "`pip install 'rpx-benchmark[ar-pose-audit]'`."
109        ) from exc
110    if not hasattr(cv2, "aruco"):
111        raise ConfigError(
112            "OpenCV was installed without the aruco module; use opencv-contrib-python-headless."
113        )
114    return cv2
115
116
117def alvar_dictionary() -> Any:
118    """Build the canonical ALVAR 0--17 dictionary for OpenCV detection."""
119
120    cv2 = _cv2()
121    rows = []
122    for code in _ALVAR_5X5_BITS:
123        bits = np.fromiter((int(bit) for bit in code), dtype=np.uint8).reshape(5, 5)
124        rows.append(cv2.aruco.Dictionary_getByteListFromBits(bits))
125    byte_list = np.concatenate(rows, axis=0)
126    return cv2.aruco.Dictionary(byte_list, 5, 2)
127
128
129def make_detector() -> Any:
130    """Create an ALVAR detector tuned for the 640x480 RPX RGB frames."""
131
132    cv2 = _cv2()
133    params = cv2.aruco.DetectorParameters()
134    params.markerBorderBits = 2
135    params.minMarkerPerimeterRate = 0.01
136    params.maxMarkerPerimeterRate = 1.0
137    params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
138    params.errorCorrectionRate = 0.8
139    return cv2.aruco.ArucoDetector(alvar_dictionary(), params)
140
141
142def marker_object_corners(marker_id: int) -> np.ndarray:
143    """Return TL, TR, BR, BL corners in the board-centred coordinate frame."""
144
145    if not 0 <= marker_id < len(_MARKER_CENTRES_CM):
146        raise ConfigError(f"unknown FewSOL marker id: {marker_id}")
147    x_cm, y_cm = _MARKER_CENTRES_CM[marker_id]
148    cx = x_cm * 0.01 - BOARD_WIDTH_M / 2.0
149    cy = BOARD_HEIGHT_M / 2.0 - y_cm * 0.01
150    half = MARKER_SIZE_M / 2.0
151    return np.array(
152        [
153            [cx - half, cy + half, 0.0],
154            [cx + half, cy + half, 0.0],
155            [cx + half, cy - half, 0.0],
156            [cx - half, cy - half, 0.0],
157        ],
158        dtype=np.float64,
159    )
160
161
162def estimate_board_pose(
163    image: np.ndarray,
164    detector: Any,
165    camera_matrix: np.ndarray = DEFAULT_CAMERA_MATRIX,
166    distortion: np.ndarray | None = None,
167    *,
168    reprojection_threshold_px: float = 4.0,
169) -> BoardObservation | None:
170    """Detect visible ALVAR tags and estimate ``T_camera_from_board``."""
171
172    cv2 = _cv2()
173    corners, ids, _rejected = detector.detectMarkers(image)
174    if ids is None or not len(ids):
175        return None
176
177    # A duplicated ID is geometrically ambiguous; keep the largest detection.
178    by_id: dict[int, np.ndarray] = {}
179    for marker_id, image_corners in zip(ids.ravel().tolist(), corners, strict=True):
180        marker_id = int(marker_id)
181        candidate = np.asarray(image_corners, dtype=np.float64).reshape(4, 2)
182        previous = by_id.get(marker_id)
183        if previous is None or abs(cv2.contourArea(candidate.astype(np.float32))) > abs(
184            cv2.contourArea(previous.astype(np.float32))
185        ):
186            by_id[marker_id] = candidate
187
188    candidate_ids = tuple(sorted(by_id))
189    object_points = np.concatenate([marker_object_corners(i) for i in candidate_ids])
190    image_points = np.concatenate([by_id[i] for i in candidate_ids])
191    distortion = (
192        np.zeros(5, dtype=np.float64)
193        if distortion is None
194        else np.asarray(distortion, dtype=np.float64)
195    )
196    K = np.asarray(camera_matrix, dtype=np.float64)
197
198    if len(candidate_ids) == 1:
199        ok, rvec, tvec = cv2.solvePnP(
200            object_points,
201            image_points,
202            K,
203            distortion,
204            flags=cv2.SOLVEPNP_IPPE,
205        )
206        inliers = np.arange(4, dtype=np.int32).reshape(-1, 1)
207    else:
208        ok, rvec, tvec, inliers = cv2.solvePnPRansac(
209            object_points,
210            image_points,
211            K,
212            distortion,
213            reprojectionError=float(reprojection_threshold_px),
214            confidence=0.999,
215            iterationsCount=200,
216            flags=cv2.SOLVEPNP_ITERATIVE,
217        )
218    if not ok or inliers is None or len(inliers) < 4:
219        return None
220
221    keep = inliers.ravel()
222    marker_ids = tuple(sorted({candidate_ids[int(index) // 4] for index in keep}))
223    ok, rvec, tvec = cv2.solvePnP(
224        object_points[keep],
225        image_points[keep],
226        K,
227        distortion,
228        rvec,
229        tvec,
230        True,
231        flags=cv2.SOLVEPNP_ITERATIVE,
232    )
233    if not ok:
234        return None
235    projected, _ = cv2.projectPoints(object_points[keep], rvec, tvec, K, distortion)
236    residual = projected.reshape(-1, 2) - image_points[keep]
237    rmse = float(np.sqrt(np.mean(np.sum(residual * residual, axis=1))))
238    rotation, _ = cv2.Rodrigues(rvec)
239    transform = np.eye(4, dtype=np.float64)
240    transform[:3, :3] = rotation
241    transform[:3, 3] = np.asarray(tvec).reshape(3)
242    return BoardObservation(transform, marker_ids, rmse, int(len(keep)))
243
244
245def pose_from_npz_bytes(payload: bytes) -> np.ndarray:
246    """Load an RPX T265 camera-to-world pose from an NPZ payload."""
247
248    with np.load(io.BytesIO(payload)) as data:
249        position = np.asarray(data["position"], dtype=np.float64).reshape(3)
250        quaternion = np.asarray(data["orientation"], dtype=np.float64).reshape(4)
251    transform = np.eye(4, dtype=np.float64)
252    transform[:3, :3] = Rotation.from_quat(quaternion).as_matrix()
253    transform[:3, 3] = position
254    return transform
255
256
257def rotation_error_deg(a: np.ndarray, b: np.ndarray) -> float:
258    relative = a[:3, :3].T @ b[:3, :3]
259    cosine = np.clip((np.trace(relative) - 1.0) / 2.0, -1.0, 1.0)
260    return float(np.degrees(np.arccos(cosine)))
261
262
263def relative_pose(reference: np.ndarray, current: np.ndarray) -> np.ndarray:
264    return np.linalg.inv(reference) @ current
265
266
267def t265_to_opencv_pose(pose: np.ndarray) -> np.ndarray:
268    """Express a raw librealsense T265 pose in the OpenCV axis convention."""
269
270    return _T265_TO_OPENCV @ np.asarray(pose, dtype=np.float64) @ _T265_TO_OPENCV
271
272
273def _pearson(a: np.ndarray, b: np.ndarray) -> float | None:
274    a = np.asarray(a, dtype=np.float64).ravel()
275    b = np.asarray(b, dtype=np.float64).ravel()
276    if len(a) < 2 or np.std(a) < 1e-12 or np.std(b) < 1e-12:
277        return None
278    return float(np.corrcoef(a, b)[0, 1])
279
280
281def _distribution(values: Sequence[float]) -> dict[str, float | None]:
282    """Return robust, publication-friendly statistics for one error series."""
283
284    array = np.asarray(values, dtype=np.float64)
285    if not len(array):
286        return {key: None for key in ("rmse", "mean", "median", "std", "p95", "max")}
287    return {
288        "rmse": float(np.sqrt(np.mean(np.square(array)))),
289        "mean": float(np.mean(array)),
290        "median": float(np.median(array)),
291        "std": float(np.std(array)),
292        "p95": float(np.percentile(array, 95)),
293        "max": float(np.max(array)),
294    }
295
296
297def _robust_outliers(values: Sequence[float], *, z_threshold: float = 3.5) -> np.ndarray:
298    """Flag high residuals using a median/MAD threshold without assuming Gaussian noise."""
299
300    array = np.asarray(values, dtype=np.float64)
301    if not len(array):
302        return np.zeros(0, dtype=bool)
303    median = float(np.median(array))
304    mad = float(np.median(np.abs(array - median)))
305    if float(np.ptp(array)) < 1e-9:
306        return np.zeros(len(array), dtype=bool)
307    if mad < 1e-12:
308        return array > median + 1e-9
309    robust_z = (array - median) / (1.4826 * mad)
310    return robust_z > z_threshold
311
312
313def _slope_norm(frame_ids: Sequence[int], vectors: np.ndarray) -> float | None:
314    """Magnitude of a least-squares vector trend, expressed per source frame."""
315
316    if len(frame_ids) < 2:
317        return None
318    x = np.asarray(frame_ids, dtype=np.float64)
319    x -= x.mean()
320    denominator = float(x @ x)
321    if denominator < 1e-12:
322        return None
323    centred = np.asarray(vectors, dtype=np.float64) - np.mean(vectors, axis=0)
324    slope = (x[:, None] * centred).sum(axis=0) / denominator
325    return float(np.linalg.norm(slope))
326
327
328def _rotation_medoid(rotations: Rotation) -> Rotation:
329    """Choose the observed orientation with minimum median geodesic distance."""
330
331    quaternions = rotations.as_quat()
332    cosine_half_angles = np.clip(np.abs(quaternions @ quaternions.T), 0.0, 1.0)
333    distances = 2.0 * np.arccos(cosine_half_angles)
334    return rotations[int(np.argmin(np.median(distances, axis=1)))]
335
336
337def compare_pose_tracks(
338    frame_ids: Sequence[int],
339    cam_poses: Sequence[np.ndarray],
340    board_observations: Sequence[BoardObservation],
341    *,
342    pose_axis_convention: str = "t265",
343) -> tuple[dict[str, Any], list[dict[str, Any]]]:
344    """Compare AR and saved motion without estimating a sensor transform."""
345
346    if not (len(frame_ids) == len(cam_poses) == len(board_observations)):
347        raise ConfigError("frame, camera-pose and board-observation counts differ")
348    if not frame_ids:
349        return {"matched_frames": 0}, []
350
351    if pose_axis_convention == "t265":
352        cam_poses = [t265_to_opencv_pose(pose) for pose in cam_poses]
353    elif pose_axis_convention != "opencv":
354        raise ConfigError("pose_axis_convention must be 't265' or 'opencv'")
355
356    ar_poses = [np.linalg.inv(observation.camera_from_board) for observation in board_observations]
357    cam_anchor, ar_anchor = cam_poses[0], ar_poses[0]
358    cam_relative = [relative_pose(cam_anchor, pose) for pose in cam_poses]
359    ar_relative = [relative_pose(ar_anchor, pose) for pose in ar_poses]
360
361    frame_rows: list[dict[str, Any]] = []
362    translation_errors = []
363    rotation_errors = []
364    cam_translations = []
365    ar_translations = []
366    cam_rotvecs = []
367    ar_rotvecs = []
368    for frame_id, cam_rel, ar_rel, observation in zip(
369        frame_ids, cam_relative, ar_relative, board_observations, strict=True
370    ):
371        trans_error = float(np.linalg.norm(cam_rel[:3, 3] - ar_rel[:3, 3]))
372        rot_error = rotation_error_deg(cam_rel, ar_rel)
373        translation_errors.append(trans_error)
374        rotation_errors.append(rot_error)
375        cam_translations.append(cam_rel[:3, 3])
376        ar_translations.append(ar_rel[:3, 3])
377        cam_rotvecs.append(Rotation.from_matrix(cam_rel[:3, :3]).as_rotvec())
378        ar_rotvecs.append(Rotation.from_matrix(ar_rel[:3, :3]).as_rotvec())
379        frame_rows.append(
380            {
381                "frame_idx": int(frame_id),
382                "detected": True,
383                "marker_count": len(observation.marker_ids),
384                "marker_ids": ";".join(str(i) for i in observation.marker_ids),
385                "reprojection_rmse_px": observation.reprojection_rmse_px,
386                "translation_error_m": trans_error,
387                "rotation_error_deg": rot_error,
388            }
389        )
390
391    cam_t = np.asarray(cam_translations)
392    ar_t = np.asarray(ar_translations)
393    cam_r = np.asarray(cam_rotvecs)
394    ar_r = np.asarray(ar_rotvecs)
395
396    step_translation_errors = []
397    step_rotation_errors = []
398    step_translation_errors_per_frame = []
399    step_rotation_errors_per_frame = []
400    step_gaps = []
401    for index in range(1, len(frame_ids)):
402        cam_step = relative_pose(cam_poses[index - 1], cam_poses[index])
403        ar_step = relative_pose(ar_poses[index - 1], ar_poses[index])
404        step_translation_errors.append(float(np.linalg.norm(cam_step[:3, 3] - ar_step[:3, 3])))
405        step_rotation_errors.append(rotation_error_deg(cam_step, ar_step))
406        gap = frame_ids[index] - frame_ids[index - 1]
407        step_gaps.append(gap)
408        step_translation_errors_per_frame.append(step_translation_errors[-1] / max(gap, 1))
409        step_rotation_errors_per_frame.append(step_rotation_errors[-1] / max(gap, 1))
410
411    # If the frames are aligned and the board is static, this composition must
412    # be constant.  No world-origin or hand-eye fit is involved.
413    world_from_boards = [
414        cam @ obs.camera_from_board for cam, obs in zip(cam_poses, board_observations, strict=True)
415    ]
416    board_anchor = world_from_boards[0]
417    board_translation_drift = [
418        float(np.linalg.norm(pose[:3, 3] - board_anchor[:3, 3])) for pose in world_from_boards
419    ]
420    board_rotation_drift = [rotation_error_deg(board_anchor, pose) for pose in world_from_boards]
421
422    # Jitter is measured around a sequence-level central pose rather than the
423    # first observation, which may itself be noisy.  The translation median is
424    # robust to isolated PnP failures; the geodesic rotation medoid avoids
425    # quaternion sign ambiguity and is not pulled toward planar-PnP flips.
426    board_positions = np.asarray([pose[:3, 3] for pose in world_from_boards])
427    board_rotations = Rotation.from_matrix([pose[:3, :3] for pose in world_from_boards])
428    board_position_centre = np.median(board_positions, axis=0)
429    board_rotation_centre = _rotation_medoid(board_rotations)
430    board_translation_jitter = np.linalg.norm(
431        board_positions - board_position_centre[None, :], axis=1
432    )
433    board_rotation_jitter = np.degrees(
434        (board_rotation_centre.inv() * board_rotations).magnitude()
435    )
436    translation_outliers = _robust_outliers(board_translation_jitter)
437    rotation_outliers = _robust_outliers(board_rotation_jitter)
438    combined_outliers = translation_outliers | rotation_outliers
439    rotation_residual_vectors = (
440        board_rotation_centre.inv() * board_rotations
441    ).as_rotvec()
442
443    for row, position, pose, translation_jitter, rotation_jitter, is_outlier in zip(
444        frame_rows,
445        board_positions,
446        world_from_boards,
447        board_translation_jitter,
448        board_rotation_jitter,
449        combined_outliers,
450        strict=True,
451    ):
452        row.update(
453            {
454                "world_board_x_m": float(position[0]),
455                "world_board_y_m": float(position[1]),
456                "world_board_z_m": float(position[2]),
457                "world_board_qx": float(Rotation.from_matrix(pose[:3, :3]).as_quat()[0]),
458                "world_board_qy": float(Rotation.from_matrix(pose[:3, :3]).as_quat()[1]),
459                "world_board_qz": float(Rotation.from_matrix(pose[:3, :3]).as_quat()[2]),
460                "world_board_qw": float(Rotation.from_matrix(pose[:3, :3]).as_quat()[3]),
461                "static_board_translation_jitter_m": float(translation_jitter),
462                "static_board_rotation_jitter_deg": float(rotation_jitter),
463                "static_board_robust_outlier": bool(is_outlier),
464            }
465        )
466
467    def mean(values: Sequence[float]) -> float | None:
468        return float(np.mean(values)) if values else None
469
470    def rmse(values: Sequence[float]) -> float | None:
471        return float(np.sqrt(np.mean(np.square(values)))) if values else None
472
473    summary: dict[str, Any] = {
474        "matched_frames": len(frame_ids),
475        "first_matched_frame": int(frame_ids[0]),
476        "last_matched_frame": int(frame_ids[-1]),
477        "translation_correlation": {
478            "x": _pearson(cam_t[:, 0], ar_t[:, 0]),
479            "y": _pearson(cam_t[:, 1], ar_t[:, 1]),
480            "z": _pearson(cam_t[:, 2], ar_t[:, 2]),
481            "all_components": _pearson(cam_t, ar_t),
482            "magnitude": _pearson(np.linalg.norm(cam_t, axis=1), np.linalg.norm(ar_t, axis=1)),
483        },
484        "rotation_vector_correlation": {
485            "x": _pearson(cam_r[:, 0], ar_r[:, 0]),
486            "y": _pearson(cam_r[:, 1], ar_r[:, 1]),
487            "z": _pearson(cam_r[:, 2], ar_r[:, 2]),
488            "all_components": _pearson(cam_r, ar_r),
489            "magnitude": _pearson(np.linalg.norm(cam_r, axis=1), np.linalg.norm(ar_r, axis=1)),
490        },
491        "anchor_relative_translation_rmse_m": rmse(translation_errors),
492        "anchor_relative_translation_mean_m": mean(translation_errors),
493        "anchor_relative_rotation_mean_deg": mean(rotation_errors),
494        "anchor_relative_rotation_rmse_deg": rmse(rotation_errors),
495        "step_translation_rmse_m": rmse(step_translation_errors),
496        "step_rotation_rmse_deg": rmse(step_rotation_errors),
497        "step_translation_rmse_m_per_frame": rmse(step_translation_errors_per_frame),
498        "step_rotation_rmse_deg_per_frame": rmse(step_rotation_errors_per_frame),
499        "mean_detected_frame_gap": mean(step_gaps),
500        "static_board_translation_drift_rmse_m": rmse(board_translation_drift),
501        "static_board_rotation_drift_rmse_deg": rmse(board_rotation_drift),
502        "static_board_translation_jitter": _distribution(board_translation_jitter),
503        "static_board_rotation_jitter": _distribution(board_rotation_jitter),
504        "static_board_translation_jitter_robust_inliers": _distribution(
505            board_translation_jitter[~combined_outliers]
506        ),
507        "static_board_rotation_jitter_robust_inliers": _distribution(
508            board_rotation_jitter[~combined_outliers]
509        ),
510        "static_board_translation_drift_slope_m_per_frame": _slope_norm(
511            frame_ids, board_positions
512        ),
513        "static_board_rotation_drift_slope_deg_per_frame": (
514            None
515            if (slope := _slope_norm(frame_ids, rotation_residual_vectors)) is None
516            else float(np.degrees(slope))
517        ),
518        "static_board_robust_outlier_rate": float(np.mean(combined_outliers)),
519        "mean_marker_count": mean([len(obs.marker_ids) for obs in board_observations]),
520        "mean_reprojection_rmse_px": mean([obs.reprojection_rmse_px for obs in board_observations]),
521    }
522    return summary, frame_rows
523
524
525def _member_map(archive: tarfile.TarFile, suffix: str) -> dict[int, tarfile.TarInfo]:
526    result = {}
527    for member in archive.getmembers():
528        if member.isfile() and member.name.endswith(suffix):
529            try:
530                result[int(Path(member.name).stem)] = member
531            except ValueError:
532                continue
533    return result
534
535
536def audit_sequence(
537    sequence_dir: str | Path,
538    camera_matrix: np.ndarray = DEFAULT_CAMERA_MATRIX,
539    distortion: np.ndarray | None = None,
540    *,
541    max_frames: int | None = None,
542    reprojection_threshold_px: float = 4.0,
543    pose_axis_convention: str = "t265",
544    min_markers: int = 2,
545    max_reprojection_rmse_px: float = 2.0,
546) -> tuple[dict[str, Any], list[dict[str, Any]]]:
547    """Audit one ``objects/<id>/0`` directory directly from tar shards."""
548
549    sequence_dir = Path(sequence_dir)
550    rgb_path = sequence_dir / "rgb.tar"
551    pose_path = sequence_dir / "labels" / "cam_pose" / "v1.tar"
552    if not rgb_path.is_file() or not pose_path.is_file():
553        raise FileNotFoundError(f"missing RGB or camera-pose archive under {sequence_dir}")
554
555    detector = make_detector()
556    frame_ids: list[int] = []
557    cam_poses: list[np.ndarray] = []
558    observations: list[BoardObservation] = []
559    all_rows: dict[int, dict[str, Any]] = {}
560    raw_detections = 0
561    cv2 = _cv2()
562    with tarfile.open(rgb_path) as rgb_tar, tarfile.open(pose_path) as pose_tar:
563        rgb_members = _member_map(rgb_tar, ".png")
564        pose_members = _member_map(pose_tar, ".npz")
565        candidates = sorted(set(rgb_members) & set(pose_members))
566        if max_frames is not None:
567            candidates = candidates[:max_frames]
568        for frame_id in candidates:
569            rgb_file = rgb_tar.extractfile(rgb_members[frame_id])
570            pose_file = pose_tar.extractfile(pose_members[frame_id])
571            if rgb_file is None or pose_file is None:
572                continue
573            encoded = np.frombuffer(rgb_file.read(), dtype=np.uint8)
574            image = cv2.imdecode(encoded, cv2.IMREAD_COLOR)
575            observation = estimate_board_pose(
576                image,
577                detector,
578                camera_matrix,
579                distortion,
580                reprojection_threshold_px=reprojection_threshold_px,
581            )
582            accepted = observation is not None
583            reject_reason = ""
584            if observation is not None:
585                raw_detections += 1
586                if len(observation.marker_ids) < min_markers:
587                    accepted = False
588                    reject_reason = "insufficient_inlier_markers"
589                elif observation.reprojection_rmse_px > max_reprojection_rmse_px:
590                    accepted = False
591                    reject_reason = "high_reprojection_error"
592            all_rows[frame_id] = {
593                "frame_idx": frame_id,
594                "raw_detected": observation is not None,
595                "detected": accepted,
596                "reject_reason": reject_reason,
597            }
598            if observation is not None:
599                all_rows[frame_id].update(
600                    {
601                        "marker_count": len(observation.marker_ids),
602                        "marker_ids": ";".join(str(i) for i in observation.marker_ids),
603                        "reprojection_rmse_px": observation.reprojection_rmse_px,
604                    }
605                )
606            if accepted and observation is not None:
607                frame_ids.append(frame_id)
608                cam_poses.append(pose_from_npz_bytes(pose_file.read()))
609                observations.append(observation)
610
611    metrics, matched_rows = compare_pose_tracks(
612        frame_ids,
613        cam_poses,
614        observations,
615        pose_axis_convention=pose_axis_convention,
616    )
617    for row in matched_rows:
618        all_rows[int(row["frame_idx"])].update(row)
619    metrics.update(
620        {
621            "object_id": sequence_dir.parent.name,
622            "sequence": str(sequence_dir),
623            "total_paired_frames": len(all_rows),
624            "detection_coverage": len(frame_ids) / max(len(all_rows), 1),
625            "raw_detected_frames": raw_detections,
626            "raw_detection_coverage": raw_detections / max(len(all_rows), 1),
627        }
628    )
629    rows = [dict(object_id=sequence_dir.parent.name, **all_rows[i]) for i in sorted(all_rows)]
630    return metrics, rows
631
632
633def aggregate_sequence_metrics(sequences: Sequence[Mapping[str, Any]]) -> dict[str, Any]:
634    """Produce frame-weighted aggregate values from per-sequence summaries."""
635
636    total = sum(int(item.get("total_paired_frames", 0)) for item in sequences)
637    matched = sum(int(item.get("matched_frames", 0)) for item in sequences)
638    raw_matched = sum(int(item.get("raw_detected_frames", 0)) for item in sequences)
639    scalar_keys = (
640        "anchor_relative_translation_rmse_m",
641        "anchor_relative_rotation_rmse_deg",
642        "step_translation_rmse_m",
643        "step_rotation_rmse_deg",
644        "step_translation_rmse_m_per_frame",
645        "step_rotation_rmse_deg_per_frame",
646        "static_board_translation_drift_rmse_m",
647        "static_board_rotation_drift_rmse_deg",
648        "static_board_translation_drift_slope_m_per_frame",
649        "static_board_rotation_drift_slope_deg_per_frame",
650        "static_board_robust_outlier_rate",
651        "mean_marker_count",
652        "mean_reprojection_rmse_px",
653    )
654    result: dict[str, Any] = {
655        "sequences": len(sequences),
656        "sequences_with_detections": sum(
657            int(item.get("matched_frames", 0)) > 0 for item in sequences
658        ),
659        "total_frames": total,
660        "matched_frames": matched,
661        "detection_coverage": matched / max(total, 1),
662        "raw_detected_frames": raw_matched,
663        "raw_detection_coverage": raw_matched / max(total, 1),
664    }
665    for key in scalar_keys:
666        values = [
667            (float(item[key]), int(item.get("matched_frames", 0)))
668            for item in sequences
669            if item.get(key) is not None and int(item.get("matched_frames", 0)) > 0
670        ]
671        result[key] = (
672            float(
673                np.average([value for value, _ in values], weights=[weight for _, weight in values])
674            )
675            if values
676            else None
677        )
678    for family in ("translation_correlation", "rotation_vector_correlation"):
679        result[family] = {}
680        for component in ("x", "y", "z", "all_components", "magnitude"):
681            values = [
682                (float(item[family][component]), int(item["matched_frames"]))
683                for item in sequences
684                if item.get(family, {}).get(component) is not None
685            ]
686            result[family][component] = (
687                float(
688                    np.average(
689                        [value for value, _ in values], weights=[weight for _, weight in values]
690                    )
691                )
692                if values
693                else None
694            )
695    return result
696
697
698def write_results(
699    output_dir: str | Path,
700    sequence_metrics: Sequence[Mapping[str, Any]],
701    frame_rows: Iterable[Mapping[str, Any]],
702    metadata: Mapping[str, Any],
703) -> dict[str, Any]:
704    """Write JSON, CSV and a concise Markdown report."""
705
706    output_dir = Path(output_dir)
707    output_dir.mkdir(parents=True, exist_ok=True)
708    frame_rows = list(frame_rows)
709    aggregate = aggregate_sequence_metrics(sequence_metrics)
710    valid_rows = [row for row in frame_rows if row.get("detected")]
711    for family, column in (
712        ("static_board_translation_jitter", "static_board_translation_jitter_m"),
713        ("static_board_rotation_jitter", "static_board_rotation_jitter_deg"),
714    ):
715        aggregate[family] = _distribution(
716            [float(row[column]) for row in valid_rows if row.get(column) is not None]
717        )
718    robust_inlier_rows = [
719        row for row in valid_rows if not bool(row.get("static_board_robust_outlier"))
720    ]
721    for family, column in (
722        (
723            "static_board_translation_jitter_robust_inliers",
724            "static_board_translation_jitter_m",
725        ),
726        (
727            "static_board_rotation_jitter_robust_inliers",
728            "static_board_rotation_jitter_deg",
729        ),
730    ):
731        aggregate[family] = _distribution(
732            [float(row[column]) for row in robust_inlier_rows if row.get(column) is not None]
733        )
734    aggregate["static_board_robust_outlier_rate"] = (
735        float(np.mean([bool(row.get("static_board_robust_outlier")) for row in valid_rows]))
736        if valid_rows
737        else None
738    )
739    payload = {
740        "protocol": dict(metadata),
741        "aggregate": aggregate,
742        "per_sequence": list(sequence_metrics),
743    }
744    (output_dir / "summary.json").write_text(json.dumps(payload, indent=2, sort_keys=True) + "\n")
745
746    flattened = []
747    nested_families = (
748        "translation_correlation",
749        "rotation_vector_correlation",
750        "static_board_translation_jitter",
751        "static_board_rotation_jitter",
752        "static_board_translation_jitter_robust_inliers",
753        "static_board_rotation_jitter_robust_inliers",
754    )
755    for item in sequence_metrics:
756        row = {key: value for key, value in item.items() if not isinstance(value, Mapping)}
757        for family in nested_families:
758            for key, value in item.get(family, {}).items():
759                row[f"{family}.{key}"] = value
760        flattened.append(row)
761    _write_csv(output_dir / "per_sequence.csv", flattened)
762    _write_csv(output_dir / "per_frame.csv", frame_rows)
763
764    def fmt(value: Any, digits: int = 4) -> str:
765        return "n/a" if value is None else f"{float(value):.{digits}f}"
766
767    report = f"""# RPX SOS AR-board / camera-pose audit
768
769## Protocol
770
771- Dataset: `{metadata.get("dataset_repo", "unknown")}` at revision `{metadata.get("dataset_revision", "unknown")}`
772- Sequences: {aggregate["sequences"]} ({aggregate["sequences_with_detections"]} with detections)
773- Frames inspected: {aggregate["total_frames"]}
774- Frames with a valid AR-board pose: {aggregate["matched_frames"]} ({aggregate["detection_coverage"]:.1%})
775- Frames with any raw tag pose: {aggregate["raw_detected_frames"]} ({aggregate["raw_detection_coverage"]:.1%})
776- Camera matrix: `{metadata["camera_matrix"]}`
777- Sensor-frame assumption: D435 RGB and saved T265 camera poses are already aligned.
778- Sensor transform fitting: **none**.
779- World-origin handling: relative poses from the first detected frame in each sequence.
780- Primary quality gate: at least {metadata["min_markers"]} RANSAC-consistent markers and reprojection RMSE <= {metadata["max_reprojection_rmse_px"]} px.
781
782## Aggregate results
783
784| Measure | Value |
785|---|---:|
786| Translation correlation, all components | {fmt(aggregate["translation_correlation"]["all_components"])} |
787| Translation-magnitude correlation | {fmt(aggregate["translation_correlation"]["magnitude"])} |
788| Rotation-vector correlation, all components | {fmt(aggregate["rotation_vector_correlation"]["all_components"])} |
789| Rotation-magnitude correlation | {fmt(aggregate["rotation_vector_correlation"]["magnitude"])} |
790| Anchor-relative translation RMSE | {fmt(aggregate["anchor_relative_translation_rmse_m"])} m |
791| Anchor-relative rotation RMSE | {fmt(aggregate["anchor_relative_rotation_rmse_deg"])} deg |
792| Step translation RMSE | {fmt(aggregate["step_translation_rmse_m"])} m |
793| Step rotation RMSE | {fmt(aggregate["step_rotation_rmse_deg"])} deg |
794| Gap-normalized step translation RMSE | {fmt(aggregate["step_translation_rmse_m_per_frame"])} m/frame |
795| Gap-normalized step rotation RMSE | {fmt(aggregate["step_rotation_rmse_deg_per_frame"])} deg/frame |
796| Static-board translation drift RMSE | {fmt(aggregate["static_board_translation_drift_rmse_m"])} m |
797| Static-board rotation drift RMSE | {fmt(aggregate["static_board_rotation_drift_rmse_deg"])} deg |
798| Static-board translation jitter, median / P95 | {fmt(aggregate["static_board_translation_jitter"]["median"])} / {fmt(aggregate["static_board_translation_jitter"]["p95"])} m |
799| Static-board rotation jitter, median / P95 | {fmt(aggregate["static_board_rotation_jitter"]["median"])} / {fmt(aggregate["static_board_rotation_jitter"]["p95"])} deg |
800| Robust-inlier translation jitter, median / P95 | {fmt(aggregate["static_board_translation_jitter_robust_inliers"]["median"])} / {fmt(aggregate["static_board_translation_jitter_robust_inliers"]["p95"])} m |
801| Robust-inlier rotation jitter, median / P95 | {fmt(aggregate["static_board_rotation_jitter_robust_inliers"]["median"])} / {fmt(aggregate["static_board_rotation_jitter_robust_inliers"]["p95"])} deg |
802| Translation drift slope | {fmt(aggregate["static_board_translation_drift_slope_m_per_frame"], 6)} m/frame |
803| Rotation drift slope | {fmt(aggregate["static_board_rotation_drift_slope_deg_per_frame"], 6)} deg/frame |
804| Robust residual outlier rate | {fmt(aggregate["static_board_robust_outlier_rate"])} |
805| Mean marker count | {fmt(aggregate["mean_marker_count"], 2)} |
806| Mean marker reprojection RMSE | {fmt(aggregate["mean_reprojection_rmse_px"])} px |
807
808## Automated finding
809
810The AR-derived and stored motion tracks are strongly correlated (translation
811`{fmt(aggregate["translation_correlation"]["all_components"])}`, rotation
812`{fmt(aggregate["rotation_vector_correlation"]["all_components"])}`). Across accepted frames,
813the fixed-board residual has median/P95 translation
814`{fmt(aggregate["static_board_translation_jitter"]["median"])} / {fmt(aggregate["static_board_translation_jitter"]["p95"])} m`
815and median/P95 rotation
816`{fmt(aggregate["static_board_rotation_jitter"]["median"])} / {fmt(aggregate["static_board_rotation_jitter"]["p95"])} deg`.
817After removing the automatically identified median/MAD residual outliers, those P95 values are
818`{fmt(aggregate["static_board_translation_jitter_robust_inliers"]["p95"])} m` and
819`{fmt(aggregate["static_board_rotation_jitter_robust_inliers"]["p95"])} deg`.
820
821These values show measurable disagreement, but they are an upper bound on T265 pose error rather
822than a pure T265 accuracy measurement. The released files do not contain the capture device's
823factory distortion coefficients or a numerical D435-to-T265 extrinsic. The residual therefore also
824contains tag-corner/PnP uncertainty, approximate-intrinsics error, synchronization error and any
825unmodelled sensor-frame offset. Large orientation branches in the per-sequence PDF should be
826treated as planar-PnP/reference failures until independently verified.
827
828## Interpretation
829
830Relative-motion metrics remove only the arbitrary world origin; they do not learn or apply a
831D435-to-T265 transform. The static-board residual is an independent consistency check: with
832correct synchronization, conventions and the stated camera-frame alignment, the composed board
833pose should be constant. Review per-sequence and per-frame files before drawing conclusions from
834the aggregate correlations, especially where marker coverage or reprojection quality is low.
835
836The first-frame drift metrics are directly comparable to anchor-based ATE. The central-pose jitter
837metrics use the sequence median position and rotation medoid, making them less sensitive to one
838bad anchor. Gap-normalized step errors are the RPE-style local-motion measurements. A robust
839outlier is a translation or rotation residual more than 3.5 scaled MAD above its sequence median.
840"""
841    (output_dir / "report.md").write_text(report)
842    return payload
843
844
845def _write_csv(path: Path, rows: Sequence[Mapping[str, Any]]) -> None:
846    keys = sorted({key for row in rows for key in row})
847    with path.open("w", newline="") as handle:
848        writer = csv.DictWriter(handle, fieldnames=keys)
849        writer.writeheader()
850        writer.writerows(rows)