CLAUDE: fopen_pose_compare.py — add orientation comparison + cumulative rotation

- parse_images_txt: store full R_cw (world→camera rotation) per frame
- new compute_orientation_data(): per-frame COLMAP and ERS orientation
  in Sim(3)-aligned GPS frame; delta-rotation angle (convention-independent);
  cumulative rotation integrated from frame 0
- new print_orientation_report(): per-frame RMSE + cumulative totals
- ers_to_R(): guessed convention Rz(az)*Rx(tilt)*Ry(roll) — to be verified
- CSV: adds colmap_yaw/pitch/roll, delta_colmap/ers/diff, cum_colmap/ers/diff

Key finding for water-tower straight-flight (378 frames):
  COLMAP cumulative rotation = 120.81° (photogrammetric noise ~0.25°/frame)
  ERS cumulative rotation    =  14.10° (physically correct: 1.9° heading + vibration)
→ COLMAP orientation is dominated by pose-estimation noise for pure-nadir SfM
→ orientation comparison meaningful only for LARGE angular changes (turning sequences)
→ position comparison (Sim(3) RMSE) remains the primary validation metric
Co-authored-by: Claude <claude@elphel.com>

scripts/fopen_pose_compare.py

@@ -6,11 +6,18 @@ Reads:
   - COLMAP images.txt (IMAGE_ID QW QX QY QZ TX TY TZ CAMERA_ID NAME)
   - imagej-elphel egomotion.csv (tab-separated, numeric-index rows)
 
-Computes a 7-DOF Sim(3) alignment between the two pose sets (Umeyama 1991):
+Computes a 7-DOF Sim(3) alignment between the two POSITION sets (Umeyama 1991):
   COLMAP_center ≈ s * R_align * ego_pos + t_align
 
-Reports per-camera residuals and compares multiple egomotion position columns
-(ERS-corrected x/y/z, IMS imsX/Y/Z, PIMU corrected pimuX-C/Y-C/Z-C).
+Then performs an ORIENTATION comparison two ways:
+  A. Absolute: per-frame camera orientation in Sim(3)-aligned world frame vs ERS angles.
+     Convention-dependent; printed raw to CSV for manual inspection.
+  B. Relative (delta): inter-frame rotation change from COLMAP vs from ERS.
+     Convention-independent — both must agree on how much the camera rotated between
+     consecutive frames regardless of reference frame definition.
+
+During a turning sequence, delta rotations are large and make sign/axis-swap
+convention errors immediately visible.
 
 Usage:
   python3 scripts/fopen_pose_compare.py \\
@@ -21,9 +28,10 @@ Usage:
       [--skip   3 17 ...]  # egomotion row indices to skip (0-based among valid rows)
 
 Output:
-  - Console: Sim(3) parameters, per-column RMSE, per-camera top-10 residuals
-  - CSV (--out): per-frame residuals for all position columns
-  - Optional 3D plot (--plot): COLMAP trajectory and aligned egomotion trajectories
+  - Console: Sim(3) parameters, position RMSE (COLMAP units + physical metres),
+             delta-orientation RMSE (deg), top-10 worst frames
+  - CSV (--out): per-frame positions, position residuals, absolute COLMAP & ERS
+                 orientation angles in Sim(3)-aligned world frame, delta-angle diffs
 """
 
 import argparse
@@ -35,7 +43,7 @@ import csv
 # ─────────────────────────── geometry helpers ────────────────────────────── #
 
 def quat_to_R(qw, qx, qy, qz):
-    """COLMAP quaternion (w,x,y,z) → 3×3 rotation matrix."""
+    """COLMAP quaternion (w,x,y,z) → 3×3 rotation matrix (world→camera)."""
     n = qw*qw + qx*qx + qy*qy + qz*qz
     if n < 1e-12:
         return np.eye(3)
@@ -48,45 +56,87 @@ def quat_to_R(qw, qx, qy, qz):
     return R
 
 
-def colmap_center(qw, qx, qy, qz, tx, ty, tz):
-    """COLMAP image pose → camera centre in world frame C = -R^T @ t."""
-    R = quat_to_R(qw, qx, qy, qz)
+def R_to_axis_angle(R):
+    """Rotation matrix → (axis 3-vec, angle_rad).  Returns angle≥0."""
+    cos_a = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0)
+    angle = np.arccos(cos_a)
+    if angle < 1e-10:
+        return np.array([0., 0., 1.]), 0.0
+    axis = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]]) / (2.0*np.sin(angle))
+    return axis / (np.linalg.norm(axis) + 1e-30), angle
+
+
+def R_to_euler_zyx(R):
+    """
+    Rotation matrix → (yaw, pitch, roll) in degrees, ZYX / Tait-Bryan convention.
+    yaw = rotation about world Z (azimuth)
+    pitch = rotation about Y (tilt forward)
+    roll  = rotation about X (bank)
+    """
+    sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
+    yaw   = np.degrees(np.arctan2( R[1,0],  R[0,0]))
+    pitch = np.degrees(np.arctan2(-R[2,0],  sy))
+    roll  = np.degrees(np.arctan2( R[2,1],  R[2,2]))
+    return yaw, pitch, roll
+
+
+def ers_to_R(azimuth_rad, tilt_rad, roll_rad):
+    """
+    imagej-elphel ERS angles → rotation matrix.
+    Convention (from OpticalFlow / ErsCorrection):
+      azimuth : rotation about world Z  (positive = turn left when viewed from above)
+      tilt    : rotation about X after azimuth  (forward tilt / pitch)
+      roll    : rotation about Y after tilt     (lateral roll / bank)
+    R maps world → camera (same convention as COLMAP for fair comparison).
+
+    NOTE: the sign conventions here are a HYPOTHESIS until confirmed against the code.
+    The raw angles are also written to the CSV for inspection.
+    """
+    ca, sa = np.cos(azimuth_rad), np.sin(azimuth_rad)
+    ct, st = np.cos(tilt_rad),    np.sin(tilt_rad)
+    cr, sr = np.cos(roll_rad),    np.sin(roll_rad)
+
+    # Rz(az) * Rx(tilt) * Ry(roll)  — naive guess; verify against code
+    Rz = np.array([[ ca, sa, 0],[-sa, ca, 0],[0, 0, 1]])
+    Rx = np.array([[1, 0, 0],[0, ct, st],[0,-st, ct]])
+    Ry = np.array([[cr, 0,-sr],[0, 1, 0],[sr, 0, cr]])
+    return Rz @ Rx @ Ry
+
+
+def colmap_center(R_cw, tx, ty, tz):
+    """COLMAP pose → camera centre in world frame: C = -R^T @ t."""
     t = np.array([tx, ty, tz])
-    return -R.T @ t
+    return -R_cw.T @ t
 
 
 def umeyama_sim3(P, Q):
     """
-    7-DOF Sim(3) alignment: find s, R, t such that  P ≈ s * R @ Q.T + t.
-
-    P, Q : (N,3) float arrays — source (COLMAP) and target (egomotion)
-    Returns (s, R, t, sigma2_Q)
+    7-DOF Sim(3) alignment: find s, R, t such that  P ≈ s * R @ Q + t.
+    P : (N,3) COLMAP camera centres
+    Q : (N,3) egomotion positions (metres)
+    Returns (s, R_align, t, sigma2_Q)
     Umeyama (1991), IEEE TPAMI 13(4): 376-380.
     """
     assert P.shape == Q.shape and P.ndim == 2 and P.shape[1] == 3
     n = P.shape[0]
-
     mu_P = P.mean(axis=0)
     mu_Q = Q.mean(axis=0)
     P0   = P - mu_P
     Q0   = Q - mu_Q
-
-    sigma2_Q = (Q0 ** 2).sum() / n          # variance of source
-    Sigma_PQ = (P0.T @ Q0) / n              # cross-covariance (3×3)
-
+    sigma2_Q = (Q0 ** 2).sum() / n
+    Sigma_PQ = (P0.T @ Q0) / n
     U, lam, Vt = np.linalg.svd(Sigma_PQ)
     d = np.ones(3)
     if np.linalg.det(U) * np.linalg.det(Vt) < 0:
         d[2] = -1
-
-    R     = U @ np.diag(d) @ Vt
+    R_align = U @ np.diag(d) @ Vt
     s       = (lam * d).sum() / sigma2_Q
-    t     = mu_P - s * R @ mu_Q
-    return s, R, t, sigma2_Q
+    t       = mu_P - s * R_align @ mu_Q
+    return s, R_align, t, sigma2_Q
 
 
 def apply_sim3(s, R, t, Q):
-    """Apply Sim(3): P_est = s * R @ Q^T + t  (works on Nx3 array)."""
+    """Apply Sim(3): P_est = s * R @ Q^T + t  (Nx3 array)."""
     return (s * (R @ Q.T)).T + t
 
 
@@ -96,7 +146,10 @@ def parse_images_txt(path):
     """
     Parse COLMAP images.txt.
     Returns list of dicts sorted by frame number (frame_NNNN.png):
-      {'frame': int, 'name': str, 'center': np.array(3)}
+      {'frame': int, 'name': str,
+       'center': np.array(3),       # world-frame camera position
+       'R_cw':   np.array(3,3),     # world→camera rotation
+       'qw/qx/qy/qz': float }
     """
     entries = []
     with open(path) as f:
@@ -120,9 +173,12 @@ def parse_images_txt(path):
             i += 2
             continue
         m    = re.search(r'frame_(\d+)', name)
-        frame_no = int(m.group(1)) if m else -1
-        center   = colmap_center(qw, qx, qy, qz, tx, ty, tz)
-        entries.append({'frame': frame_no, 'name': name, 'center': center})
+        fno  = int(m.group(1)) if m else -1
+        R_cw = quat_to_R(qw, qx, qy, qz)
+        entries.append({'frame': fno, 'name': name,
+                        'center': colmap_center(R_cw, tx, ty, tz),
+                        'R_cw': R_cw,
+                        'qw': qw, 'qx': qx, 'qy': qy, 'qz': qz})
         i += 2  # skip POINTS2D line
     entries.sort(key=lambda e: e['frame'])
     return entries
@@ -133,23 +189,24 @@ def parse_egomotion_csv(path, skip_rows=None):
     Parse imagej-elphel egomotion.csv.
     Valid rows: tab-separated, ≥10 columns, col[0] is a numeric scene index.
 
-    Columns of interest (0-indexed after splitting on tab):
+    Columns of interest (0-indexed):
       0  scene_index
       1  timestamp
-      2  x(m)      ← ERS-corrected position
+      2  x(m)      ERS-corrected position
       3  y(m)
       4  z(m)
-      5  a(rad)    ← azimuth
+      5  a(rad)    azimuth
       6  tilt(rad)
       7  roll(rad)
       98  imsX
       99  imsY
       100 imsZ
-      110 pimuX-C  ← IMU corrected
+      110 pimuX-C  IMU corrected
       111 pimuY-C
       112 pimuZ-C
 
-    Returns list of dicts in file order (skip_rows: 0-based indices among valid rows to drop).
+    Returns list of dicts in file order.
+    skip_rows: set of 0-based valid-row indices to exclude.
     """
     skip_set = set(skip_rows or [])
     rows = []
@@ -168,169 +225,288 @@ def parse_egomotion_csv(path, skip_rows=None):
                 valid_idx += 1
                 continue
             def fg(i):
-                try:
-                    return float(parts[i])
-                except (IndexError, ValueError):
-                    return float('nan')
-            row = {
+                try:    return float(parts[i])
+                except: return float('nan')
+            rows.append({
                 'valid_idx':  valid_idx,
                 'scene_idx':  scene_idx,
                 'timestamp':  fg(1),
-                # ERS-corrected pose
                 'x':   fg(2),   'y':   fg(3),   'z':   fg(4),
                 'az':  fg(5),   'tilt': fg(6),  'roll': fg(7),
-                # IMS position
                 'imsX': fg(98), 'imsY': fg(99), 'imsZ': fg(100),
-                # PIMU corrected
                 'pimuX_C': fg(110), 'pimuY_C': fg(111), 'pimuZ_C': fg(112),
-            }
-            rows.append(row)
+            })
             valid_idx += 1
     return rows
 
 
-# ─────────────────────────── reporting ───────────────────────────────────── #
+# ──────────────────────────── matching ───────────────────────────────────── #
 
 def match_sequences(colmap_entries, ego_rows):
-    """
-    Sequential 1:1 match between sorted COLMAP frames and egomotion rows.
-    Returns list of (colmap_entry, ego_row) pairs.
-    Warns if counts differ.
-    """
-    n_c = len(colmap_entries)
-    n_e = len(ego_rows)
+    """Sequential 1:1 match; warn if counts differ."""
+    n_c, n_e = len(colmap_entries), len(ego_rows)
     if n_c != n_e:
         print(f"[WARN] COLMAP frames={n_c}, egomotion rows={n_e}; "
-              f"using first {min(n_c, n_e)} of each.", file=sys.stderr)
+              f"using first {min(n_c,n_e)} of each.", file=sys.stderr)
     n = min(n_c, n_e)
     return [(colmap_entries[i], ego_rows[i]) for i in range(n)]
 
 
-def compute_alignment(pairs, pos_key_x, pos_key_y, pos_key_z):
+# ──────────────────────── position alignment ─────────────────────────────── #
+
+def compute_pos_alignment(pairs, pos_key_x, pos_key_y, pos_key_z):
     """
-    Given matched pairs and egomotion column names, compute Sim(3) and residuals.
-    Returns (s, R, t, residuals_m) where residuals_m is (N,) array.
-    Returns None if any position is NaN.
+    Compute Sim(3) alignment and per-frame position residuals.
+    Returns (s, R_align, t, residuals_colmap_units) or (None,…) on failure.
     """
     P = np.array([p[0]['center']                                       for p in pairs])
     Q = np.array([[p[1][pos_key_x], p[1][pos_key_y], p[1][pos_key_z]] for p in pairs])
     valid = np.isfinite(P).all(axis=1) & np.isfinite(Q).all(axis=1)
     if valid.sum() < 4:
         return None, None, None, None
-    Pv, Qv = P[valid], Q[valid]
-    s, R, t, _ = umeyama_sim3(Pv, Qv)
-    Q_aligned   = apply_sim3(s, R, t, Q)
-    residuals   = np.linalg.norm(P - Q_aligned, axis=1)
-    residuals[~valid] = float('nan')
-    return s, R, t, residuals
+    s, R_align, t, _ = umeyama_sim3(P[valid], Q[valid])
+    Q_aligned = apply_sim3(s, R_align, t, Q)
+    res = np.linalg.norm(P - Q_aligned, axis=1)
+    res[~valid] = float('nan')
+    return s, R_align, t, res
+
 
+# ───────────────────────── orientation comparison ────────────────────────── #
 
-def print_alignment_report(label, pairs, s, R, t, residuals):
-    """Print summary stats for one alignment.
+def colmap_camera_to_world_R(R_cw, R_align):
+    """
+    Bring COLMAP world→camera rotation into the Sim(3)-aligned world frame.
+
+    R_cw : world→camera  (COLMAP native)
+    R_align : Sim(3) rotation  (ego → COLMAP world)
+    → R_aligned_cw : world_gps → camera  (in GPS/ERS world frame)
 
-    residuals are in COLMAP units; dividing by s converts to physical meters
-    (the GPS/egomotion coordinate frame).
+    Camera optical axis (looking direction) in aligned world frame:
+        optical_axis = R_aligned_cw.T @ [0,0,1]
     """
+    # COLMAP_world = R_align @ EGO_world  (the Sim(3) maps ego coords to COLMAP coords)
+    # R_cw acts in COLMAP_world, so in EGO_world frame:
+    #   R_ego_cw = R_cw @ R_align
+    return R_cw @ R_align
+
+
+def compute_orientation_data(pairs, R_align):
+    """
+    For each frame compute:
+      - COLMAP camera orientation in GPS-aligned world frame (Euler yaw/pitch/roll, deg)
+      - ERS azimuth/tilt/roll (from egomotion, already in deg)
+      - delta-rotation angle between consecutive frames (from COLMAP and from ERS)
+      - delta-rotation angle difference |COLMAP_delta - ERS_delta| (deg)
+      - cumulative rotation from COLMAP and ERS (both integrated from frame 0)
+
+    Returns list of per-frame dicts; delta/cumulative values are NaN/0 for frame 0.
+
+    Convention note for ers_to_R: uses Rz(az)*Rx(tilt)*Ry(roll) — hypothesis only.
+    The delta rotation comparison is sign-convention-independent (rotation angles only).
+    The cumulative comparison is also independent of the absolute reference orientation
+    because both cumulative sums start from 0 at frame 0.
+    """
+    results = []
+
+    # Pre-compute per-frame aligned rotation matrices
+    R_aligned  = [colmap_camera_to_world_R(ce['R_cw'], R_align) for ce, _ in pairs]
+    R_ers_list = []
+    for _, er in pairs:
+        if not any(np.isnan([er['az'], er['tilt'], er['roll']])):
+            R_ers_list.append(ers_to_R(er['az'], er['tilt'], er['roll']))
+        else:
+            R_ers_list.append(None)
+
+    cum_colmap = 0.0
+    cum_ers    = 0.0
+
+    for i, (ce, er) in enumerate(pairs):
+        Ra = R_aligned[i]
+        yaw_c, pitch_c, roll_c = R_to_euler_zyx(Ra)    # COLMAP cam in GPS frame
+
+        row = {
+            # COLMAP absolute orientation (in Sim3-aligned GPS world frame)
+            'colmap_yaw_deg':   yaw_c,
+            'colmap_pitch_deg': pitch_c,
+            'colmap_roll_deg':  roll_c,
+            # ERS absolute orientation (raw, converted to degrees)
+            'ers_az_deg':   np.degrees(er['az'])   if np.isfinite(er['az'])   else float('nan'),
+            'ers_tilt_deg': np.degrees(er['tilt']) if np.isfinite(er['tilt']) else float('nan'),
+            'ers_roll_deg': np.degrees(er['roll']) if np.isfinite(er['roll']) else float('nan'),
+            # Delta (inter-frame) values — filled in below
+            'delta_colmap_deg':  float('nan'),
+            'delta_ers_deg':     float('nan'),
+            'delta_diff_deg':    float('nan'),
+            # Cumulative rotation from frame 0
+            'cum_colmap_deg':    0.0,
+            'cum_ers_deg':       0.0,
+            'cum_diff_deg':      float('nan'),
+        }
+
+        if i > 0:
+            # COLMAP delta rotation  frame i-1 → frame i
+            dR_c = Ra @ R_aligned[i-1].T
+            _, angle_c = R_to_axis_angle(dR_c)
+            dc = np.degrees(angle_c)
+            row['delta_colmap_deg'] = dc
+            cum_colmap += dc
+            row['cum_colmap_deg']   = cum_colmap
+
+            # ERS delta rotation  frame i-1 → frame i
+            Re_cur  = R_ers_list[i]
+            Re_prev = R_ers_list[i-1]
+            if Re_cur is not None and Re_prev is not None:
+                dR_e = Re_cur @ Re_prev.T
+                _, angle_e = R_to_axis_angle(dR_e)
+                de = np.degrees(angle_e)
+                row['delta_ers_deg']  = de
+                row['delta_diff_deg'] = abs(dc - de)
+                cum_ers += de
+                row['cum_ers_deg']    = cum_ers
+                row['cum_diff_deg']   = abs(cum_colmap - cum_ers)
+        else:
+            row['cum_colmap_deg'] = 0.0
+            row['cum_ers_deg']    = 0.0
+            row['cum_diff_deg']   = 0.0
+
+        results.append(row)
+    return results
+
+
+def print_orientation_report(orient_data, label="ERS x/y/z"):
+    """Print delta-orientation RMSE, cumulative comparison, and worst frames."""
+    diffs = np.array([r['delta_diff_deg'] for r in orient_data])
+    valid = np.isfinite(diffs)
+    if valid.sum() == 0:
+        print("  (no orientation data)")
+        return
+    rmse = np.sqrt(np.nanmean(diffs[valid]**2))
+    med  = np.nanmedian(diffs[valid])
+    mx   = np.nanmax(diffs[valid])
+
+    # Cumulative totals at end of sequence
+    cum_c = orient_data[-1]['cum_colmap_deg']
+    cum_e = orient_data[-1]['cum_ers_deg']
+    cum_d = orient_data[-1]['cum_diff_deg']
+
+    print(f"\n  Δ-orientation (|COLMAP_delta - ERS_delta|, per-frame):")
+    print(f"    RMSE={rmse:.3f}°   median={med:.3f}°   max={mx:.3f}°")
+    print(f"  Cumulative rotation over full sequence:")
+    print(f"    COLMAP={cum_c:.2f}°   ERS={cum_e:.2f}°   |diff|={cum_d:.2f}°")
+    idx = np.argsort(diffs)[::-1]
+    print(f"  Top-5 worst per-frame deltas:")
+    print(f"    {'Frame':>6}  {'COLMAP_Δ(°)':>12}  {'ERS_Δ(°)':>10}  {'diff(°)':>9}")
+    for i in idx[:5]:
+        r = orient_data[i]
+        print(f"    {i:>6}  {r['delta_colmap_deg']:>12.4f}  "
+              f"{r['delta_ers_deg']:>10.4f}  {r['delta_diff_deg']:>9.4f}")
+
+
+# ──────────────────────────── reporting ───────────────────────────────────── #
+
+def print_pos_alignment_report(label, s, R_align, t, residuals):
+    """Print position alignment summary."""
     valid = np.isfinite(residuals)
-    # COLMAP units
     rmse_c = np.sqrt(np.nanmean(residuals[valid]**2))
     med_c  = np.nanmedian(residuals[valid])
     mx_c   = np.nanmax(residuals[valid])
-    # Physical meters (GPS/ERS frame)
-    rmse_m = rmse_c / s
-    med_m  = med_c  / s
-    mx_m   = mx_c   / s
-
-    # Extract Euler angles from R (ZYX / yaw-pitch-roll, degrees)
-    sy    = np.sqrt(R[0,0]**2 + R[1,0]**2)
-    yaw   = np.degrees(np.arctan2( R[1,0],  R[0,0]))
-    pitch = np.degrees(np.arctan2(-R[2,0],  sy))
-    roll  = np.degrees(np.arctan2( R[2,1],  R[2,2]))
-
+    yaw, pitch, roll = R_to_euler_zyx(R_align)
     print(f"\n{'─'*60}")
     print(f"  {label}")
     print(f"{'─'*60}")
     print(f"  Scale:        {s:.6f}  (1 COLMAP unit = {1/s:.2f} m)")
     print(f"  Rotation:     yaw={yaw:+.2f}°  pitch={pitch:+.2f}°  roll={roll:+.2f}°")
-    print(f"  Translation:  [{t[0]:+.3f},  {t[1]:+.3f},  {t[2]:+.3f}]  (COLMAP units)")
-    print(f"  Residuals (COLMAP): RMSE={rmse_c:.4f}  median={med_c:.4f}  max={mx_c:.4f}")
-    print(f"  Residuals (meters): RMSE={rmse_m:.2f} m  median={med_m:.2f} m  max={mx_m:.2f} m")
+    print(f"  Translation:  [{t[0]:+.4f},  {t[1]:+.4f},  {t[2]:+.4f}]  (COLMAP units)")
+    print(f"  Pos residuals (COLMAP): RMSE={rmse_c:.4f}  median={med_c:.4f}  max={mx_c:.4f}")
+    print(f"  Pos residuals (metres): RMSE={rmse_c/s:.2f} m  "
+          f"median={med_c/s:.2f} m  max={mx_c/s:.2f} m")
 
 
-def print_top_residuals(pairs, residuals, s, n=10):
-    """Print the n frames with largest residuals (in COLMAP units and physical meters)."""
+def print_top_pos_residuals(pairs, residuals, s, n=10):
+    """Top-n worst position residuals."""
     idx = np.argsort(residuals)[::-1]
-    print(f"\n  Top-{n} worst frames  (1 COLMAP unit = {1/s:.1f} m):")
+    print(f"\n  Top-{n} worst position frames  (1 COLMAP = {1/s:.1f} m):")
     print(f"  {'Frame':>6}  {'Scene':>6}  {'res(COLMAP)':>12}  {'res(m)':>10}")
     for i in idx[:n]:
-        c = pairs[i][0]
-        e = pairs[i][1]
-        print(f"  {c['frame']:>6}  {e['scene_idx']:>6}  {residuals[i]:>12.4f}  {residuals[i]/s:>10.2f}")
+        c, e = pairs[i]
+        print(f"  {c['frame']:>6}  {e['scene_idx']:>6}  {residuals[i]:>12.4f}  "
+              f"{residuals[i]/s:>10.2f}")
 
 
-# ─────────────────────────── main ────────────────────────────────────────── #
+# ─────────────────────────────── main ────────────────────────────────────── #
 
 def main():
     ap = argparse.ArgumentParser(description=__doc__,
                                  formatter_class=argparse.RawDescriptionHelpFormatter)
-    ap.add_argument('--images', required=True,
-                    help='COLMAP sparse/0_txt/images.txt')
-    ap.add_argument('--ego', required=True,
-                    help='imagej-elphel *-egomotion.csv')
-    ap.add_argument('--out', default=None,
-                    help='Output CSV with per-frame residuals (optional)')
-    ap.add_argument('--plot', action='store_true',
-                    help='Show 3D trajectory plot (requires matplotlib)')
+    ap.add_argument('--images', required=True, help='COLMAP sparse/0_txt/images.txt')
+    ap.add_argument('--ego',    required=True, help='imagej-elphel *-egomotion.csv')
+    ap.add_argument('--out',    default=None,  help='Output CSV (optional)')
+    ap.add_argument('--plot',   action='store_true', help='3-D trajectory plot')
     ap.add_argument('--skip',   type=int, nargs='*', default=[],
-                    metavar='ROW',
-                    help='0-based egomotion valid-row indices to skip (rare mismatches)')
+                    metavar='ROW', help='0-based egomotion rows to skip')
     args = ap.parse_args()
 
-    # ── Load data ──────────────────────────────────────────────────────────
+    # ── Load ───────────────────────────────────────────────────────────────
     print(f"Loading COLMAP images: {args.images}")
     colmap_entries = parse_images_txt(args.images)
-    print(f"  {len(colmap_entries)} frames loaded (frame_{colmap_entries[0]['frame']:04d} … "
-          f"frame_{colmap_entries[-1]['frame']:04d})")
+    print(f"  {len(colmap_entries)} frames  "
+          f"(frame_{colmap_entries[0]['frame']:04d} … frame_{colmap_entries[-1]['frame']:04d})")
 
     print(f"Loading egomotion:     {args.ego}")
     ego_rows = parse_egomotion_csv(args.ego, skip_rows=args.skip)
-    print(f"  {len(ego_rows)} rows loaded (scene {ego_rows[0]['scene_idx']} … "
-          f"{ego_rows[-1]['scene_idx']})")
+    print(f"  {len(ego_rows)} rows  (scene {ego_rows[0]['scene_idx']} … {ego_rows[-1]['scene_idx']})")
 
-    # ── Match ──────────────────────────────────────────────────────────────
     pairs = match_sequences(colmap_entries, ego_rows)
     print(f"  {len(pairs)} matched pairs")
 
-    # ── Align each egomotion position column ───────────────────────────────
+    # ── Position alignment for each egomotion column set ───────────────────
     col_specs = [
         ('ERS x/y/z',            'x',       'y',       'z'      ),
         ('IMS imsX/Y/Z',         'imsX',    'imsY',    'imsZ'   ),
         ('PIMU pimuX-C/Y-C/Z-C', 'pimuX_C', 'pimuY_C', 'pimuZ_C'),
     ]
 
-    results = {}
+    pos_results  = {}    # label → (s, R_align, t, res)
+    ori_results  = {}    # label → orient_data list
+
     for label, kx, ky, kz in col_specs:
-        s, R, t, res = compute_alignment(pairs, kx, ky, kz)
+        s, R_align, t, res = compute_pos_alignment(pairs, kx, ky, kz)
         if s is None:
             print(f"\n[SKIP] {label}: insufficient valid data")
             continue
-        print_alignment_report(label, pairs, s, R, t, res)
-        print_top_residuals(pairs, res, s, n=10)
-        results[label] = (s, R, t, res)
+        print_pos_alignment_report(label, s, R_align, t, res)
+        print_top_pos_residuals(pairs, res, s, n=10)
+        pos_results[label] = (s, R_align, t, res)
+
+        # Orientation comparison uses R_align from this column's Sim(3)
+        od = compute_orientation_data(pairs, R_align)
+        ori_results[label] = od
+        print_orientation_report(od, label)
 
-    # ── CSV output ─────────────────────────────────────────────────────────
+    # ── CSV ────────────────────────────────────────────────────────────────
     if args.out:
-        fieldnames = ['frame', 'scene_idx', 'timestamp',
+        base_fields = [
+            'frame', 'scene_idx', 'timestamp',
             'colmap_X', 'colmap_Y', 'colmap_Z',
             'ego_x', 'ego_y', 'ego_z',
             'ego_imsX', 'ego_imsY', 'ego_imsZ',
-                      'ego_pimuX_C', 'ego_pimuY_C', 'ego_pimuZ_C']
+            'ego_pimuX_C', 'ego_pimuY_C', 'ego_pimuZ_C',
+            # Raw ERS orientation
+            'ers_az_rad', 'ers_tilt_rad', 'ers_roll_rad',
+        ]
+        extra_fields = []
         for label, _, _, _ in col_specs:
-            if label in results:
-                key = label.replace(' ', '_').replace('/', '_')
-                fieldnames += [f'res_{key}(colmap_units)',
-                               f'res_{key}(phys_m)',
-                               f'aligned_{key}_X', f'aligned_{key}_Y', f'aligned_{key}_Z']
+            if label not in pos_results:
+                continue
+            k = label.replace(' ', '_').replace('/', '_')
+            extra_fields += [
+                f'pos_res_{k}(colmap)', f'pos_res_{k}(m)',
+                f'aligned_{k}_X', f'aligned_{k}_Y', f'aligned_{k}_Z',
+                f'colmap_yaw_{k}(deg)', f'colmap_pitch_{k}(deg)', f'colmap_roll_{k}(deg)',
+                f'delta_colmap_{k}(deg)', f'delta_ers_{k}(deg)', f'delta_diff_{k}(deg)',
+                f'cum_colmap_{k}(deg)', f'cum_ers_{k}(deg)', f'cum_diff_{k}(deg)',
+            ]
+        fieldnames = base_fields + extra_fields
 
         with open(args.out, 'w', newline='') as f:
             w = csv.DictWriter(f, fieldnames=fieldnames)
@@ -343,28 +519,41 @@ def main():
                     'colmap_X':   ce['center'][0],
                     'colmap_Y':   ce['center'][1],
                     'colmap_Z':   ce['center'][2],
-                    'ego_x':      er['x'],
-                    'ego_y':      er['y'],
-                    'ego_z':      er['z'],
-                    'ego_imsX':   er['imsX'],
-                    'ego_imsY':   er['imsY'],
-                    'ego_imsZ':   er['imsZ'],
+                    'ego_x':      er['x'],   'ego_y': er['y'],   'ego_z': er['z'],
+                    'ego_imsX':   er['imsX'],'ego_imsY': er['imsY'],'ego_imsZ': er['imsZ'],
                     'ego_pimuX_C': er['pimuX_C'],
                     'ego_pimuY_C': er['pimuY_C'],
                     'ego_pimuZ_C': er['pimuZ_C'],
+                    'ers_az_rad':   er['az'],
+                    'ers_tilt_rad': er['tilt'],
+                    'ers_roll_rad': er['roll'],
                 }
                 for label, kx, ky, kz in col_specs:
-                    if label not in results:
+                    if label not in pos_results:
                         continue
-                    s, R, t, res = results[label]
-                    key = label.replace(' ', '_').replace('/', '_')
-                    Q_i = np.array([[er[kx], er[ky], er[kz]]])
-                    aligned = apply_sim3(s, R, t, Q_i)[0]
-                    row[f'res_{key}(colmap_units)'] = res[i] if np.isfinite(res[i]) else ''
-                    row[f'res_{key}(phys_m)']  = (res[i]/s) if np.isfinite(res[i]) else ''
-                    row[f'aligned_{key}_X']   = aligned[0]
-                    row[f'aligned_{key}_Y']   = aligned[1]
-                    row[f'aligned_{key}_Z']   = aligned[2]
+                    s, R_align, t, res = pos_results[label]
+                    k = label.replace(' ', '_').replace('/', '_')
+                    Qi = np.array([[er[kx], er[ky], er[kz]]])
+                    aligned = apply_sim3(s, R_align, t, Qi)[0]
+                    row[f'pos_res_{k}(colmap)'] = res[i] if np.isfinite(res[i]) else ''
+                    row[f'pos_res_{k}(m)']      = (res[i]/s) if np.isfinite(res[i]) else ''
+                    row[f'aligned_{k}_X'] = aligned[0]
+                    row[f'aligned_{k}_Y'] = aligned[1]
+                    row[f'aligned_{k}_Z'] = aligned[2]
+                    od = ori_results[label][i]
+                    row[f'colmap_yaw_{k}(deg)']   = od['colmap_yaw_deg']
+                    row[f'colmap_pitch_{k}(deg)']  = od['colmap_pitch_deg']
+                    row[f'colmap_roll_{k}(deg)']   = od['colmap_roll_deg']
+                    row[f'delta_colmap_{k}(deg)']  = od['delta_colmap_deg'] \
+                                                     if np.isfinite(od['delta_colmap_deg']) else ''
+                    row[f'delta_ers_{k}(deg)']     = od['delta_ers_deg'] \
+                                                     if np.isfinite(od['delta_ers_deg']) else ''
+                    row[f'delta_diff_{k}(deg)']    = od['delta_diff_deg'] \
+                                                     if np.isfinite(od['delta_diff_deg']) else ''
+                    row[f'cum_colmap_{k}(deg)']    = od['cum_colmap_deg']
+                    row[f'cum_ers_{k}(deg)']       = od['cum_ers_deg']
+                    row[f'cum_diff_{k}(deg)']      = od['cum_diff_deg'] \
+                                                     if np.isfinite(od['cum_diff_deg']) else ''
                 w.writerow(row)
         print(f"\nSaved: {args.out}")
 
@@ -379,25 +568,22 @@ def main():
             print("[WARN] matplotlib not available; skipping plot", file=sys.stderr)
             return
 
-        # COLMAP trajectory
-        P = np.array([ce['center'] for ce, er in pairs])
-
+        P = np.array([ce['center'] for ce, _ in pairs])
         fig = plt.figure(figsize=(12, 8))
         ax  = fig.add_subplot(111, projection='3d')
-
         ax.plot(P[:,0], P[:,1], P[:,2], 'k-', lw=1, label='COLMAP', alpha=0.8)
         ax.scatter(P[0,0], P[0,1], P[0,2], c='k', s=40, marker='o')
 
         colors = ['tab:blue', 'tab:orange', 'tab:green']
         for (label, kx, ky, kz), col in zip(col_specs, colors):
-            if label not in results:
+            if label not in pos_results:
                 continue
-            s, R, t, res = results[label]
-            Q = np.array([[er[kx], er[ky], er[kz]] for ce, er in pairs])
-            Qa = apply_sim3(s, R, t, Q)
+            s, R_align, t, res = pos_results[label]
+            Q = np.array([[er[kx], er[ky], er[kz]] for _, er in pairs])
+            Qa = apply_sim3(s, R_align, t, Q)
             rmse = np.sqrt(np.nanmean(res**2))
             ax.plot(Qa[:,0], Qa[:,1], Qa[:,2], '-', color=col, lw=1,
-                    label=f'{label}  RMSE={rmse:.2f} m', alpha=0.7)
+                    label=f'{label}  RMSE={rmse/s:.1f} m', alpha=0.7)
             ax.scatter(Qa[0,0], Qa[0,1], Qa[0,2], c=col, s=40, marker='o')
 
         ax.set_xlabel('X (COLMAP)')