#!/usr/bin/env python3 """Scan body-left RPY corrections locally and validate them over every B pair. This command is diagnostic only. It never writes or replaces an extrinsic JSON. """ from __future__ import annotations import argparse import csv import json import math from pathlib import Path import numpy as np from scipy.spatial.transform import Rotation from rigorous_calibration import ( inverse_transform, read_pairs, read_planes, rotation_angle_deg, rpy_deg, ) def statistics(values): values = np.asarray(values, float) return { "rms": float(np.sqrt(np.mean(values ** 2))), "median": float(np.median(values)), "p90": float(np.quantile(values, 0.90)), "p95": float(np.quantile(values, 0.95)), "max": float(np.max(values)), } def body_left_rpy(x, rpy_correction_deg): correction = np.eye(4) correction[:3, :3] = Rotation.from_euler( "xyz", np.asarray(rpy_correction_deg, float), degrees=True ).as_matrix() return correction @ x def pair_delta(a_ij, b_ij, x): predicted = inverse_transform(x) @ a_ij @ x delta = inverse_transform(b_ij) @ predicted translation = np.asarray(delta[:3, 3], float) return { "translation_xyz_m": translation.tolist(), "translation_xyz_cm": (100.0 * translation).tolist(), "translation_norm_m": float(np.linalg.norm(translation)), "rotation_rpy_deg_xyz": rpy_deg(delta[:3, :3]), "rotation_angle_deg": rotation_angle_deg(delta[:3, :3]), } def ground_metrics(planes, x, body_height): if len(planes) == 0: return None up = np.array([0.0, 0.0, 1.0]) tilt_deg, height_m = [], [] for plane in planes: normal_body = x[:3, :3] @ plane[:3] normal_body /= np.linalg.norm(normal_body) tilt_deg.append(math.degrees(math.atan2( np.linalg.norm(np.cross(normal_body, up)), float(np.clip(normal_body @ up, -1.0, 1.0)), ))) height_m.append( float(plane[3] - normal_body @ x[:3, 3] - body_height) ) return { "normal_tilt_deg": statistics(tilt_deg), "height_residual_m": statistics(height_m), } def evaluate(label, correction, a_array, b_array, meta, x, pair_index, translation_scale, rotation_scale, planes, body_height): candidate_x = body_left_rpy(x, correction) per_pair = [] translation, rotation, normalized = [], [], [] for index, (a_ij, b_ij, pair_meta) in enumerate(zip(a_array, b_array, meta)): item = pair_delta(a_ij, b_ij, candidate_x) item.update({ "pair_index": index, "i": int(pair_meta[0]), "j": int(pair_meta[1]), }) t = item["translation_norm_m"] r = item["rotation_angle_deg"] translation.append(t) rotation.append(r) normalized.append(math.hypot(t / translation_scale, r / rotation_scale)) per_pair.append(item) return { "label": label, "body_left_rpy_correction_deg_xyz": list(map(float, correction)), "candidate_extrinsic": { "translation_m": candidate_x[:3, 3].tolist(), "rotation_rpy_deg_xyz": rpy_deg(candidate_x[:3, :3]), }, "all_pairs": { "count": len(per_pair), "translation_m": statistics(translation), "rotation_deg": statistics(rotation), "normalized_pair_score": statistics(normalized), "normalized_global_rms": float(np.sqrt(np.mean(np.asarray(normalized) ** 2))), }, "selected_pair": per_pair[pair_index], "ground": ground_metrics(planes, candidate_x, body_height), "per_pair": per_pair, } def candidate_grid(pitch_values, roll_values, yaw_values): answer = [("baseline", (0.0, 0.0, 0.0))] for pitch in pitch_values: answer.append((f"pitch_{pitch:+.3f}", (0.0, pitch, 0.0))) for pitch in (0.0, *pitch_values): for roll in roll_values: answer.append(( f"pitch_{pitch:+.3f}_roll_{roll:+.3f}", (roll, pitch, 0.0), )) for yaw in yaw_values: answer.append((f"yaw_{yaw:+.3f}_diagnostic", (0.0, 0.0, yaw))) unique = [] seen = set() for label, values in answer: key = tuple(round(float(value), 12) for value in values) if key not in seen: seen.add(key) unique.append((label, values)) return unique def z_observability(a_array, x, test_shift_m): shift = np.eye(4) shift[2, 3] = test_shift_m shifted_x = shift @ x effects = [] for a_ij in a_array: before = inverse_transform(x) @ a_ij @ x after = inverse_transform(shifted_x) @ a_ij @ shifted_x delta = inverse_transform(before) @ after effects.append(( float(np.linalg.norm(delta[:3, 3])), rotation_angle_deg(delta[:3, :3]), )) effects = np.asarray(effects, float) maximum = np.max(effects, axis=0) return { "body_left_z_test_shift_m": test_shift_m, "max_predicted_motion_change_translation_m": float(maximum[0]), "max_predicted_motion_change_rotation_deg": float(maximum[1]), "numerically_unobservable": bool(maximum[0] < 1e-10 and maximum[1] < 1e-10), "note": "AX pairs cannot determine X.z when every A rotation preserves body Z; use ground/external height constraints.", } def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("--pairs", required=True) parser.add_argument("--extrinsic", required=True) parser.add_argument("--output", required=True) parser.add_argument("--csv") parser.add_argument("--ground-planes") parser.add_argument("--pair-index", type=int, default=0) parser.add_argument("--pitch-values", nargs="+", type=float, default=[0.1, 0.2, 0.3]) parser.add_argument("--roll-values", nargs="+", type=float, default=[-0.2, -0.1, 0.1]) parser.add_argument("--yaw-values", nargs="+", type=float, default=[-0.2, 0.2]) parser.add_argument("--translation-scale", type=float, default=0.05) parser.add_argument("--rotation-scale", type=float, default=0.5) parser.add_argument("--body-height", type=float, default=0.2335) args = parser.parse_args() a_array, b_array, meta, stations = read_pairs(args.pairs) if not 0 <= args.pair_index < len(a_array): raise IndexError(f"pair-index {args.pair_index} outside [0,{len(a_array)-1}]") with Path(args.extrinsic).open(encoding="utf-8-sig") as stream: x = np.asarray(json.load(stream)["matrix_4x4"], float) planes = read_planes(args.ground_planes) if args.ground_planes else np.empty((0, 4)) candidates = [ evaluate( label, correction, a_array, b_array, meta, x, args.pair_index, args.translation_scale, args.rotation_scale, planes, args.body_height, ) for label, correction in candidate_grid( args.pitch_values, args.roll_values, args.yaw_values ) ] baseline = candidates[0] baseline_scores = np.asarray([ math.hypot( item["translation_norm_m"] / args.translation_scale, item["rotation_angle_deg"] / args.rotation_scale, ) for item in baseline["per_pair"] ]) base_global = baseline["all_pairs"]["normalized_global_rms"] for candidate in candidates: scores = np.asarray([ math.hypot( item["translation_norm_m"] / args.translation_scale, item["rotation_angle_deg"] / args.rotation_scale, ) for item in candidate["per_pair"] ]) delta = scores - baseline_scores candidate["comparison_to_baseline"] = { "normalized_global_rms_change": float( candidate["all_pairs"]["normalized_global_rms"] - base_global ), "improved_pairs": int(np.sum(delta < -1e-12)), "worsened_pairs": int(np.sum(delta > 1e-12)), "unchanged_pairs": int(np.sum(np.abs(delta) <= 1e-12)), "median_per_pair_score_change": float(np.median(delta)), "global_consistency_signal": bool( candidate["all_pairs"]["normalized_global_rms"] < base_global and np.sum(delta < -1e-12) > np.sum(delta > 1e-12) ), } ranking = sorted( candidates, key=lambda item: item["all_pairs"]["normalized_global_rms"], ) report = { "schema_version": 1, "diagnostic_only": True, "extrinsic_was_modified": False, "equation": "delta_ij = B_ij^-1 * (X^-1 * A_ij * X)", "correction_convention": "X_test = DeltaR_body * X; DeltaR uses fixed body xyz RPY axes", "component_frame": "delta translation/RPY components are in station-j LiDAR coordinates, not screen axes", "selection_rule": ( "Never accept a correction from selected_pair alone. Require improvement over all " "refined pairs, directional consistency across pairs, acceptable ground constraints, " "and independent visual review. This script never overwrites X." ), "pairs_file": str(Path(args.pairs).resolve()), "extrinsic_file": str(Path(args.extrinsic).resolve()), "stations": stations, "pairs": len(a_array), "selected_pair_index": args.pair_index, "selected_pair_stations": [int(meta[args.pair_index, 0]), int(meta[args.pair_index, 1])], "normalization": { "translation_scale_m": args.translation_scale, "rotation_scale_deg": args.rotation_scale, }, "z_observability": z_observability(a_array, x, 0.10), "ranking_by_all_pair_normalized_rms": [ { "rank": rank, "label": item["label"], "body_left_rpy_correction_deg_xyz": item["body_left_rpy_correction_deg_xyz"], "normalized_global_rms": item["all_pairs"]["normalized_global_rms"], **item["comparison_to_baseline"], } for rank, item in enumerate(ranking, 1) ], "candidates": candidates, } output = Path(args.output) output.parent.mkdir(parents=True, exist_ok=True) output.write_text(json.dumps(report, ensure_ascii=False, indent=2), encoding="utf-8") csv_path = Path(args.csv) if args.csv else output.with_suffix(".csv") with csv_path.open("w", encoding="utf-8", newline="") as stream: fields = [ "label", "roll_correction_deg", "pitch_correction_deg", "yaw_correction_deg", "selected_pair_translation_cm", "selected_pair_rotation_deg", "all_pair_translation_rms_m", "all_pair_rotation_rms_deg", "normalized_global_rms", "normalized_global_rms_change", "improved_pairs", "worsened_pairs", "global_consistency_signal", "ground_normal_tilt_rms_deg", "ground_height_rms_m", ] writer = csv.DictWriter(stream, fieldnames=fields) writer.writeheader() for item in candidates: correction = item["body_left_rpy_correction_deg_xyz"] ground = item["ground"] comparison = item["comparison_to_baseline"] writer.writerow({ "label": item["label"], "roll_correction_deg": correction[0], "pitch_correction_deg": correction[1], "yaw_correction_deg": correction[2], "selected_pair_translation_cm": item["selected_pair"]["translation_norm_m"] * 100.0, "selected_pair_rotation_deg": item["selected_pair"]["rotation_angle_deg"], "all_pair_translation_rms_m": item["all_pairs"]["translation_m"]["rms"], "all_pair_rotation_rms_deg": item["all_pairs"]["rotation_deg"]["rms"], "normalized_global_rms": item["all_pairs"]["normalized_global_rms"], "normalized_global_rms_change": comparison["normalized_global_rms_change"], "improved_pairs": comparison["improved_pairs"], "worsened_pairs": comparison["worsened_pairs"], "global_consistency_signal": comparison["global_consistency_signal"], "ground_normal_tilt_rms_deg": None if ground is None else ground["normal_tilt_deg"]["rms"], "ground_height_rms_m": None if ground is None else ground["height_residual_m"]["rms"], }) print(json.dumps({ "diagnostic_only": True, "selected_pair": baseline["selected_pair"], "z_observability": report["z_observability"], "top_all_pair_candidates": report["ranking_by_all_pair_normalized_rms"][:8], "output": str(output.resolve()), "csv": str(csv_path.resolve()), }, ensure_ascii=False, indent=2)) if __name__ == "__main__": main()