CLAUDE: fopen_pose_compare.py — accept --xml (INTERFRAME.corr-xml) as pose source

--ego (egomotion.csv) was required but is not always present (it is an
optional output file).  --xml (*-INTERFRAME.corr-xml) is always written
by the pipeline and contains the same x/y/z/a/t/r values.

Changes:
- add parse_interframe_xml(): sorts timestamps → same frame order as COLMAP
- --xml is the new preferred input; --ego is optional and overrides --xml
  (it additionally provides IMS/PIMU columns)
- at least one of --xml or --ego is required
- IMS/PIMU [SKIP] messages suppressed when using XML (NaN is expected)
- verified: XML and egomotion.csv produce identical results
Co-authored-by: Claude <claude@elphel.com>

scripts/fopen_pose_compare.py

@@ -4,7 +4,9 @@ fopen_pose_compare.py — Compare COLMAP camera poses with imagej-elphel ERS/GPS
 
 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)
+  - imagej-elphel *-INTERFRAME.corr-xml  (--xml, always present)
+    OR  *-egomotion.csv                  (--ego, optional output file)
+    --ego takes precedence if both are given (it has IMS/PIMU columns too).
 
 Computes a 7-DOF Sim(3) alignment between the two POSITION sets (Umeyama 1991):
   COLMAP_center ≈ s * R_align * ego_pos + t_align
@@ -22,10 +24,11 @@ convention errors immediately visible.
 Usage:
   python3 scripts/fopen_pose_compare.py \\
       --images  /path/to/sparse/0_txt/images.txt \\
-      --ego     /path/to/*-egomotion.csv \\
+      --xml     /path/to/*-INTERFRAME.corr-xml \\
+      [--ego    /path/to/*-egomotion.csv] \\
       [--out    pose_compare.csv] \\
       [--plot]  \\
-      [--skip   3 17 ...]  # egomotion row indices to skip (0-based among valid rows)
+      [--skip   3 17 ...]  # pose row indices to skip (0-based)
 
 Output:
   - Console: Sim(3) parameters, position RMSE (COLMAP units + physical metres),
@@ -37,6 +40,7 @@ Output:
 import argparse
 import sys
 import re
+import xml.etree.ElementTree as ET
 import numpy as np
 import csv
 
@@ -240,6 +244,61 @@ def parse_egomotion_csv(path, skip_rows=None):
     return rows
 
 
+def parse_interframe_xml(path, skip_rows=None):
+    """
+    Parse imagej-elphel *-INTERFRAME.corr-xml.
+    Returns list of dicts sorted by timestamp — same order as COLMAP frame_NNNN
+    (elphel_to_colmap.py assigns frame numbers by sorting timestamps).
+
+    Fields match parse_egomotion_csv() for compatibility:
+      scene_idx  — 0-based sequential index
+      timestamp  — float seconds
+      x/y/z      — position (m) relative to reference scene
+      az/tilt/roll — orientation (rad)
+      imsX/Y/Z, pimuX_C/Y_C/Z_C — NaN (not in XML)
+    """
+    skip_set = set(skip_rows or [])
+    PREFIX    = "EYESIS_DCT_AUX."
+    SCENE_KEY = PREFIX + "scenes_"
+
+    tree    = ET.parse(path)
+    root    = tree.getroot()
+    entries = {e.get("key"): e.text for e in root.findall("entry")}
+
+    raw = {}
+    for key, value in entries.items():
+        if not key.startswith(SCENE_KEY):
+            continue
+        ts_str = key[len(SCENE_KEY):]
+        if ts_str.endswith("_dt") or ts_str.endswith("_d2t"):
+            continue
+        try:
+            vals = [float(v) for v in value.split(",")]
+        except (TypeError, ValueError):
+            continue
+        if len(vals) != 6:
+            continue
+        # "1763233715_106222"  →  1763233715.106222
+        ts_float = float(ts_str.replace("_", ".", 1))
+        raw[ts_float] = vals
+
+    nan = float('nan')
+    rows = []
+    for i, (ts, vals) in enumerate(sorted(raw.items())):
+        if i in skip_set:
+            continue
+        x, y, z, az, tilt, roll = vals
+        rows.append({
+            'scene_idx': i,
+            'timestamp': ts,
+            'x': x,   'y': y,   'z': z,
+            'az': az, 'tilt': tilt, 'roll': roll,
+            'imsX': nan, 'imsY': nan, 'imsZ': nan,
+            'pimuX_C': nan, 'pimuY_C': nan, 'pimuZ_C': nan,
+        })
+    return rows
+
+
 # ──────────────────────────── matching ───────────────────────────────────── #
 
 def match_sequences(colmap_entries, ego_rows):
@@ -439,22 +498,31 @@ 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('--xml',    default=None,  help='imagej-elphel *-INTERFRAME.corr-xml (preferred)')
+    ap.add_argument('--ego',    default=None,  help='imagej-elphel *-egomotion.csv (optional; overrides --xml if given)')
     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 rows to skip')
+                    metavar='ROW', help='0-based pose rows to skip')
     args = ap.parse_args()
 
+    if not args.xml and not args.ego:
+        ap.error("provide at least one of --xml or --ego")
+
     # ── Load ───────────────────────────────────────────────────────────────
     print(f"Loading COLMAP images: {args.images}")
     colmap_entries = parse_images_txt(args.images)
     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  (scene {ego_rows[0]['scene_idx']} … {ego_rows[-1]['scene_idx']})")
+    if args.ego:
+        print(f"Loading egomotion:     {args.ego}")
+        ego_rows = parse_egomotion_csv(args.ego, skip_rows=args.skip)
+        print(f"  {len(ego_rows)} rows  (scene {ego_rows[0]['scene_idx']} … {ego_rows[-1]['scene_idx']})")
+    else:
+        print(f"Loading INTERFRAME XML: {args.xml}")
+        ego_rows = parse_interframe_xml(args.xml, skip_rows=args.skip)
+        print(f"  {len(ego_rows)} rows  (ts {ego_rows[0]['timestamp']:.3f} … {ego_rows[-1]['timestamp']:.3f})")
 
     pairs = match_sequences(colmap_entries, ego_rows)
     print(f"  {len(pairs)} matched pairs")
@@ -472,6 +540,8 @@ def main():
     for label, kx, ky, kz in col_specs:
         s, R_align, t, res = compute_pos_alignment(pairs, kx, ky, kz)
         if s is None:
+            if not args.ego:
+                continue   # IMS/PIMU are always NaN when using XML — no need to warn
             print(f"\n[SKIP] {label}: insufficient valid data")
             continue
         print_pos_alignment_report(label, s, R_align, t, res)