Skip to content

I
imagej-elphel
Commit cd4e13ef authored by Andrey Filippov's avatar Andrey Filippov
Browse files
Options

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: 's avatarClaude <claude@elphel.com>
parent e24bde9f
Hide whitespace changes
Inline Side-by-side
Showing with 342 additions and 156 deletions
scripts/fopen_pose_compare.py
View file @ cd4e13ef
...@@ -6,11 +6,18 @@ Reads: ...@@ -6,11 +6,18 @@ Reads:
- COLMAP images.txt (IMAGE_ID QW QX QY QZ TX TY TZ CAMERA_ID NAME) - 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 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 COLMAP_center ≈ s * R_align * ego_pos + t_align
Reports per-camera residuals and compares multiple egomotion position columns Then performs an ORIENTATION comparison two ways:
(ERS-corrected x/y/z, IMS imsX/Y/Z, PIMU corrected pimuX-C/Y-C/Z-C). 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: Usage:
python3 scripts/fopen_pose_compare.py \\ python3 scripts/fopen_pose_compare.py \\
...@@ -21,9 +28,10 @@ Usage: ...@@ -21,9 +28,10 @@ Usage:
[--skip 3 17 ...] # egomotion row indices to skip (0-based among valid rows) [--skip 3 17 ...] # egomotion row indices to skip (0-based among valid rows)
Output: Output:
- Console: Sim(3) parameters, per-column RMSE, per-camera top-10 residuals - Console: Sim(3) parameters, position RMSE (COLMAP units + physical metres),
- CSV (--out): per-frame residuals for all position columns delta-orientation RMSE (deg), top-10 worst frames
- Optional 3D plot (--plot): COLMAP trajectory and aligned egomotion trajectories - CSV (--out): per-frame positions, position residuals, absolute COLMAP & ERS
orientation angles in Sim(3)-aligned world frame, delta-angle diffs
""" """
import argparse import argparse
...@@ -35,7 +43,7 @@ import csv ...@@ -35,7 +43,7 @@ import csv
# ─────────────────────────── geometry helpers ────────────────────────────── # # ─────────────────────────── geometry helpers ────────────────────────────── #
def quat_to_R(qw, qx, qy, qz): 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 n = qw*qw + qx*qx + qy*qy + qz*qz
if n < 1e-12: if n < 1e-12:
return np.eye(3) return np.eye(3)
...@@ -48,45 +56,87 @@ def quat_to_R(qw, qx, qy, qz): ...@@ -48,45 +56,87 @@ def quat_to_R(qw, qx, qy, qz):
return R return R
def colmap_center(qw, qx, qy, qz, tx, ty, tz): def R_to_axis_angle(R):
"""COLMAP image pose → camera centre in world frame C = -R^T @ t.""" """Rotation matrix → (axis 3-vec, angle_rad). Returns angle≥0."""
R = quat_to_R(qw, qx, qy, qz) 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]) t = np.array([tx, ty, tz])
return -R.T @ t return -R_cw.T @ t
def umeyama_sim3(P, Q): def umeyama_sim3(P, Q):
""" """
7-DOF Sim(3) alignment: find s, R, t such that P ≈ s * R @ Q.T + t. 7-DOF Sim(3) alignment: find s, R, t such that P ≈ s * R @ Q + t.
P : (N,3) COLMAP camera centres
P, Q : (N,3) float arrays — source (COLMAP) and target (egomotion) Q : (N,3) egomotion positions (metres)
Returns (s, R, t, sigma2_Q) Returns (s, R_align, t, sigma2_Q)
Umeyama (1991), IEEE TPAMI 13(4): 376-380. Umeyama (1991), IEEE TPAMI 13(4): 376-380.
""" """
assert P.shape == Q.shape and P.ndim == 2 and P.shape[1] == 3 assert P.shape == Q.shape and P.ndim == 2 and P.shape[1] == 3
n = P.shape[0] n = P.shape[0]
mu_P = P.mean(axis=0) mu_P = P.mean(axis=0)
mu_Q = Q.mean(axis=0) mu_Q = Q.mean(axis=0)
P0 = P - mu_P P0 = P - mu_P
Q0 = Q - mu_Q Q0 = Q - mu_Q
sigma2_Q = (Q0 ** 2).sum() / n
sigma2_Q = (Q0 ** 2).sum() / n # variance of source Sigma_PQ = (P0.T @ Q0) / n
Sigma_PQ = (P0.T @ Q0) / n # cross-covariance (3×3)
U, lam, Vt = np.linalg.svd(Sigma_PQ) U, lam, Vt = np.linalg.svd(Sigma_PQ)
d = np.ones(3) d = np.ones(3)
if np.linalg.det(U) * np.linalg.det(Vt) < 0: if np.linalg.det(U) * np.linalg.det(Vt) < 0:
d[2] = -1 d[2] = -1
R_align = U @ np.diag(d) @ Vt
R = U @ np.diag(d) @ Vt s = (lam * d).sum() / sigma2_Q
s = (lam * d).sum() / sigma2_Q t = mu_P - s * R_align @ mu_Q
t = mu_P - s * R @ mu_Q return s, R_align, t, sigma2_Q
return s, R, t, sigma2_Q
def apply_sim3(s, R, t, 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 return (s * (R @ Q.T)).T + t
...@@ -96,7 +146,10 @@ def parse_images_txt(path): ...@@ -96,7 +146,10 @@ def parse_images_txt(path):
""" """
Parse COLMAP images.txt. Parse COLMAP images.txt.
Returns list of dicts sorted by frame number (frame_NNNN.png): 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 = [] entries = []
with open(path) as f: with open(path) as f:
...@@ -119,10 +172,13 @@ def parse_images_txt(path): ...@@ -119,10 +172,13 @@ def parse_images_txt(path):
except (ValueError, IndexError): except (ValueError, IndexError):
i += 2 i += 2
continue continue
m = re.search(r'frame_(\d+)', name) m = re.search(r'frame_(\d+)', name)
frame_no = int(m.group(1)) if m else -1 fno = int(m.group(1)) if m else -1
center = colmap_center(qw, qx, qy, qz, tx, ty, tz) R_cw = quat_to_R(qw, qx, qy, qz)
entries.append({'frame': frame_no, 'name': name, 'center': center}) 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 i += 2 # skip POINTS2D line
entries.sort(key=lambda e: e['frame']) entries.sort(key=lambda e: e['frame'])
return entries return entries
...@@ -133,23 +189,24 @@ def parse_egomotion_csv(path, skip_rows=None): ...@@ -133,23 +189,24 @@ def parse_egomotion_csv(path, skip_rows=None):
Parse imagej-elphel egomotion.csv. Parse imagej-elphel egomotion.csv.
Valid rows: tab-separated, ≥10 columns, col[0] is a numeric scene index. 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 0 scene_index
1 timestamp 1 timestamp
2 x(m) ← ERS-corrected position 2 x(m) ERS-corrected position
3 y(m) 3 y(m)
4 z(m) 4 z(m)
5 a(rad) ← azimuth 5 a(rad) azimuth
6 tilt(rad) 6 tilt(rad)
7 roll(rad) 7 roll(rad)
98 imsX 98 imsX
99 imsY 99 imsY
100 imsZ 100 imsZ
110 pimuX-C ← IMU corrected 110 pimuX-C IMU corrected
111 pimuY-C 111 pimuY-C
112 pimuZ-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 []) skip_set = set(skip_rows or [])
rows = [] rows = []
...@@ -168,169 +225,288 @@ def parse_egomotion_csv(path, skip_rows=None): ...@@ -168,169 +225,288 @@ def parse_egomotion_csv(path, skip_rows=None):
valid_idx += 1 valid_idx += 1
continue continue
def fg(i): def fg(i):
try: try: return float(parts[i])
return float(parts[i]) except: return float('nan')
except (IndexError, ValueError): rows.append({
return float('nan')
row = {
'valid_idx': valid_idx, 'valid_idx': valid_idx,
'scene_idx': scene_idx, 'scene_idx': scene_idx,
'timestamp': fg(1), 'timestamp': fg(1),
# ERS-corrected pose
'x': fg(2), 'y': fg(3), 'z': fg(4), 'x': fg(2), 'y': fg(3), 'z': fg(4),
'az': fg(5), 'tilt': fg(6), 'roll': fg(7), 'az': fg(5), 'tilt': fg(6), 'roll': fg(7),
# IMS position 'imsX': fg(98), 'imsY': fg(99), 'imsZ': fg(100),
'imsX': fg(98), 'imsY': fg(99), 'imsZ': fg(100),
# PIMU corrected
'pimuX_C': fg(110), 'pimuY_C': fg(111), 'pimuZ_C': fg(112), 'pimuX_C': fg(110), 'pimuY_C': fg(111), 'pimuZ_C': fg(112),
} })
rows.append(row)
valid_idx += 1 valid_idx += 1
return rows return rows
# ─────────────────────────── reporting ───────────────────────────────────── # # ──────────────────────────── matching ───────────────────────────────────── #
def match_sequences(colmap_entries, ego_rows): def match_sequences(colmap_entries, ego_rows):
""" """Sequential 1:1 match; warn if counts differ."""
Sequential 1:1 match between sorted COLMAP frames and egomotion rows. n_c, n_e = len(colmap_entries), len(ego_rows)
Returns list of (colmap_entry, ego_row) pairs.
Warns if counts differ.
"""
n_c = len(colmap_entries)
n_e = len(ego_rows)
if n_c != n_e: if n_c != n_e:
print(f"[WARN] COLMAP frames={n_c}, egomotion rows={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) n = min(n_c, n_e)
return [(colmap_entries[i], ego_rows[i]) for i in range(n)] 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. Compute Sim(3) alignment and per-frame position residuals.
Returns (s, R, t, residuals_m) where residuals_m is (N,) array. Returns (s, R_align, t, residuals_colmap_units) or (None,…) on failure.
Returns None if any position is NaN.
""" """
P = np.array([p[0]['center'] for p in pairs]) 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]) 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) valid = np.isfinite(P).all(axis=1) & np.isfinite(Q).all(axis=1)
if valid.sum() < 4: if valid.sum() < 4:
return None, None, None, None return None, None, None, None
Pv, Qv = P[valid], Q[valid] s, R_align, t, _ = umeyama_sim3(P[valid], Q[valid])
s, R, t, _ = umeyama_sim3(Pv, Qv) Q_aligned = apply_sim3(s, R_align, t, Q)
Q_aligned = apply_sim3(s, R, t, Q) res = np.linalg.norm(P - Q_aligned, axis=1)
residuals = np.linalg.norm(P - Q_aligned, axis=1) res[~valid] = float('nan')
residuals[~valid] = float('nan') return s, R_align, t, res
return s, R, t, residuals
# ───────────────────────── orientation comparison ────────────────────────── #
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)
def print_alignment_report(label, pairs, s, R, t, residuals): Camera optical axis (looking direction) in aligned world frame:
"""Print summary stats for one alignment. 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
residuals are in COLMAP units; dividing by s converts to physical meters
(the GPS/egomotion coordinate frame). 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) valid = np.isfinite(residuals)
# COLMAP units
rmse_c = np.sqrt(np.nanmean(residuals[valid]**2)) rmse_c = np.sqrt(np.nanmean(residuals[valid]**2))
med_c = np.nanmedian(residuals[valid]) med_c = np.nanmedian(residuals[valid])
mx_c = np.nanmax(residuals[valid]) mx_c = np.nanmax(residuals[valid])
# Physical meters (GPS/ERS frame) yaw, pitch, roll = R_to_euler_zyx(R_align)
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]))
print(f"\n{'─'*60}") print(f"\n{'─'*60}")
print(f" {label}") print(f" {label}")
print(f"{'─'*60}") print(f"{'─'*60}")
print(f" Scale: {s:.6f} (1 COLMAP unit = {1/s:.2f} m)") 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" 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" Translation: [{t[0]:+.4f}, {t[1]:+.4f}, {t[2]:+.4f}] (COLMAP units)")
print(f" Residuals (COLMAP): RMSE={rmse_c:.4f} median={med_c:.4f} max={mx_c:.4f}") print(f" Pos 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" 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): def print_top_pos_residuals(pairs, residuals, s, n=10):
"""Print the n frames with largest residuals (in COLMAP units and physical meters).""" """Top-n worst position residuals."""
idx = np.argsort(residuals)[::-1] 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}") print(f" {'Frame':>6} {'Scene':>6} {'res(COLMAP)':>12} {'res(m)':>10}")
for i in idx[:n]: for i in idx[:n]:
c = pairs[i][0] c, e = pairs[i]
e = pairs[i][1] print(f" {c['frame']:>6} {e['scene_idx']:>6} {residuals[i]:>12.4f} "
print(f" {c['frame']:>6} {e['scene_idx']:>6} {residuals[i]:>12.4f} {residuals[i]/s:>10.2f}") f"{residuals[i]/s:>10.2f}")
# ─────────────────────────── main ────────────────────────────────────────── # # ─────────────────────────────── main ────────────────────────────────────── #
def main(): def main():
ap = argparse.ArgumentParser(description=__doc__, ap = argparse.ArgumentParser(description=__doc__,
formatter_class=argparse.RawDescriptionHelpFormatter) formatter_class=argparse.RawDescriptionHelpFormatter)
ap.add_argument('--images', required=True, ap.add_argument('--images', required=True, help='COLMAP sparse/0_txt/images.txt')
help='COLMAP sparse/0_txt/images.txt') ap.add_argument('--ego', required=True, help='imagej-elphel *-egomotion.csv')
ap.add_argument('--ego', required=True, ap.add_argument('--out', default=None, help='Output CSV (optional)')
help='imagej-elphel *-egomotion.csv') ap.add_argument('--plot', action='store_true', help='3-D trajectory plot')
ap.add_argument('--out', default=None, ap.add_argument('--skip', type=int, nargs='*', default=[],
help='Output CSV with per-frame residuals (optional)') metavar='ROW', help='0-based egomotion rows to skip')
ap.add_argument('--plot', action='store_true',
help='Show 3D trajectory plot (requires matplotlib)')
ap.add_argument('--skip', type=int, nargs='*', default=[],
metavar='ROW',
help='0-based egomotion valid-row indices to skip (rare mismatches)')
args = ap.parse_args() args = ap.parse_args()
# ── Load data ────────────────────────────────────────────────────────── # ── Load ───────────────────────────────────────────────────────────────
print(f"Loading COLMAP images: {args.images}") print(f"Loading COLMAP images: {args.images}")
colmap_entries = parse_images_txt(args.images) colmap_entries = parse_images_txt(args.images)
print(f" {len(colmap_entries)} frames loaded (frame_{colmap_entries[0]['frame']:04d} … " print(f" {len(colmap_entries)} frames "
f"frame_{colmap_entries[-1]['frame']:04d})") f"(frame_{colmap_entries[0]['frame']:04d} … frame_{colmap_entries[-1]['frame']:04d})")
print(f"Loading egomotion: {args.ego}") print(f"Loading egomotion: {args.ego}")
ego_rows = parse_egomotion_csv(args.ego, skip_rows=args.skip) ego_rows = parse_egomotion_csv(args.ego, skip_rows=args.skip)
print(f" {len(ego_rows)} rows loaded (scene {ego_rows[0]['scene_idx']} … " print(f" {len(ego_rows)} rows (scene {ego_rows[0]['scene_idx']} … {ego_rows[-1]['scene_idx']})")
f"{ego_rows[-1]['scene_idx']})")
# ── Match ──────────────────────────────────────────────────────────────
pairs = match_sequences(colmap_entries, ego_rows) pairs = match_sequences(colmap_entries, ego_rows)
print(f" {len(pairs)} matched pairs") print(f" {len(pairs)} matched pairs")
# ── Align each egomotion position column ─────────────────────────────── # ── Position alignment for each egomotion column set ───────────────────
col_specs = [ col_specs = [
('ERS x/y/z', 'x', 'y', 'z' ), ('ERS x/y/z', 'x', 'y', 'z' ),
('IMS imsX/Y/Z', 'imsX', 'imsY', 'imsZ' ), ('IMS imsX/Y/Z', 'imsX', 'imsY', 'imsZ' ),
('PIMU pimuX-C/Y-C/Z-C', 'pimuX_C', 'pimuY_C', 'pimuZ_C'), ('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: 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: if s is None:
print(f"\n[SKIP] {label}: insufficient valid data") print(f"\n[SKIP] {label}: insufficient valid data")
continue continue
print_alignment_report(label, pairs, s, R, t, res) print_pos_alignment_report(label, s, R_align, t, res)
print_top_residuals(pairs, res, s, n=10) print_top_pos_residuals(pairs, res, s, n=10)
results[label] = (s, R, t, res) 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: if args.out:
fieldnames = ['frame', 'scene_idx', 'timestamp', base_fields = [
'colmap_X', 'colmap_Y', 'colmap_Z', 'frame', 'scene_idx', 'timestamp',
'ego_x', 'ego_y', 'ego_z', 'colmap_X', 'colmap_Y', 'colmap_Z',
'ego_imsX', 'ego_imsY', 'ego_imsZ', 'ego_x', 'ego_y', 'ego_z',
'ego_pimuX_C', 'ego_pimuY_C', 'ego_pimuZ_C'] 'ego_imsX', 'ego_imsY', 'ego_imsZ',
'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: for label, _, _, _ in col_specs:
if label in results: if label not in pos_results:
key = label.replace(' ', '_').replace('/', '_') continue
fieldnames += [f'res_{key}(colmap_units)', k = label.replace(' ', '_').replace('/', '_')
f'res_{key}(phys_m)', extra_fields += [
f'aligned_{key}_X', f'aligned_{key}_Y', f'aligned_{key}_Z'] 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: with open(args.out, 'w', newline='') as f:
w = csv.DictWriter(f, fieldnames=fieldnames) w = csv.DictWriter(f, fieldnames=fieldnames)
...@@ -343,28 +519,41 @@ def main(): ...@@ -343,28 +519,41 @@ def main():
'colmap_X': ce['center'][0], 'colmap_X': ce['center'][0],
'colmap_Y': ce['center'][1], 'colmap_Y': ce['center'][1],
'colmap_Z': ce['center'][2], 'colmap_Z': ce['center'][2],
'ego_x': er['x'], 'ego_x': er['x'], 'ego_y': er['y'], 'ego_z': er['z'],
'ego_y': er['y'], 'ego_imsX': er['imsX'],'ego_imsY': er['imsY'],'ego_imsZ': er['imsZ'],
'ego_z': er['z'],
'ego_imsX': er['imsX'],
'ego_imsY': er['imsY'],
'ego_imsZ': er['imsZ'],
'ego_pimuX_C': er['pimuX_C'], 'ego_pimuX_C': er['pimuX_C'],
'ego_pimuY_C': er['pimuY_C'], 'ego_pimuY_C': er['pimuY_C'],
'ego_pimuZ_C': er['pimuZ_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: for label, kx, ky, kz in col_specs:
if label not in results: if label not in pos_results:
continue continue
s, R, t, res = results[label] s, R_align, t, res = pos_results[label]
key = label.replace(' ', '_').replace('/', '_') k = label.replace(' ', '_').replace('/', '_')
Q_i = np.array([[er[kx], er[ky], er[kz]]]) Qi = np.array([[er[kx], er[ky], er[kz]]])
aligned = apply_sim3(s, R, t, Q_i)[0] aligned = apply_sim3(s, R_align, t, Qi)[0]
row[f'res_{key}(colmap_units)'] = res[i] if np.isfinite(res[i]) else '' row[f'pos_res_{k}(colmap)'] = 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'pos_res_{k}(m)'] = (res[i]/s) if np.isfinite(res[i]) else ''
row[f'aligned_{key}_X'] = aligned[0] row[f'aligned_{k}_X'] = aligned[0]
row[f'aligned_{key}_Y'] = aligned[1] row[f'aligned_{k}_Y'] = aligned[1]
row[f'aligned_{key}_Z'] = aligned[2] 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) w.writerow(row)
print(f"\nSaved: {args.out}") print(f"\nSaved: {args.out}")
...@@ -379,25 +568,22 @@ def main(): ...@@ -379,25 +568,22 @@ def main():
print("[WARN] matplotlib not available; skipping plot", file=sys.stderr) print("[WARN] matplotlib not available; skipping plot", file=sys.stderr)
return return
# COLMAP trajectory P = np.array([ce['center'] for ce, _ in pairs])
P = np.array([ce['center'] for ce, er in pairs])
fig = plt.figure(figsize=(12, 8)) fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d') ax = fig.add_subplot(111, projection='3d')
ax.plot(P[:,0], P[:,1], P[:,2], 'k-', lw=1, label='COLMAP', alpha=0.8) 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') ax.scatter(P[0,0], P[0,1], P[0,2], c='k', s=40, marker='o')
colors = ['tab:blue', 'tab:orange', 'tab:green'] colors = ['tab:blue', 'tab:orange', 'tab:green']
for (label, kx, ky, kz), col in zip(col_specs, colors): for (label, kx, ky, kz), col in zip(col_specs, colors):
if label not in results: if label not in pos_results:
continue continue
s, R, t, res = results[label] s, R_align, t, res = pos_results[label]
Q = np.array([[er[kx], er[ky], er[kz]] for ce, er in pairs]) Q = np.array([[er[kx], er[ky], er[kz]] for _, er in pairs])
Qa = apply_sim3(s, R, t, Q) Qa = apply_sim3(s, R_align, t, Q)
rmse = np.sqrt(np.nanmean(res**2)) rmse = np.sqrt(np.nanmean(res**2))
ax.plot(Qa[:,0], Qa[:,1], Qa[:,2], '-', color=col, lw=1, 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.scatter(Qa[0,0], Qa[0,1], Qa[0,2], c=col, s=40, marker='o')
ax.set_xlabel('X (COLMAP)') ax.set_xlabel('X (COLMAP)')
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment