feat: add fusion pose updater and improve pose comparison workflow

py_workspace/README.md

@@ -44,14 +44,15 @@ Arguments:
 ### Camera Extrinsics
 
 ```bash
-uv run calibrate_extrinsics.py \ 
-   -s output/ -m aruco/markers/standard_box_markers_600mm.parquet \ 
-   --aruco-dictionary DICT_APRILTAG_36h11 \ 
-   --verify-depth -refine-depth -no-preview \ 
-   -—max-samples 20 \ 
-   --report-csv output/e2e_refine_depth_smoke.csv \ 
-   --auto-align \ 
-   --output output/e2e_refine_depth_smoke.json
+uv run calibrate_extrinsics.py \
+   -s output/ -m aruco/markers/standard_box_markers_600mm.parquet \
+   --aruco-dictionary DICT_APRILTAG_36h11 \
+   --verify-depth \
+   --refine-depth \
+   --no-preview \
+   --report-csv output/e2e_refine_depth.csv \
+   --auto-align \
+   --output output/e2e_refine_depth.json
 ```
 
 ### Visualize Extrinsics
@@ -60,10 +61,16 @@ Visualize camera poses and frustums from a JSON extrinsics file using Plotly.
 
 ```bash
 uv run visualize_extrinsics.py \
-    --input output/e2e_refine_depth_smoke_rerun.json \
+    --input output/e2e_refine_depth.json \
     --zed-configs ../zed_settings \
     --origin-axes-scale 2 \
-    --output output/e2e_refine_depth_smoke_rerun.html
+    --output output/e2e_refine_depth.html
+
+uv run compare_pose_sets.py \
+    --pose-a-json output/e2e_refine_depth.json \
+    --pose-b-json ../zed_settings/inside_network.json \
+    --report-json output/ab_report.json \
+    --plot-output output/ab_report.html
 ```
 
 **Basic 3D Visualization (Interactive HTML):**

py_workspace/apply_calibration_to_fusion_config.py

@@ -0,0 +1,156 @@
+#!/usr/bin/env python3
+"""
+Apply calibration poses to a ZED Fusion configuration file.
+
+This script takes the output from `calibrate_extrinsics.py` and updates the 
+`FusionConfiguration.pose` entries in a fusion configuration JSON file 
+(e.g., `inside_network.json`).
+
+Input Formats:
+1. Calibration JSON (--calibration-json):
+   {
+     "12345678": {
+       "pose": "1.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 1.0",
+       ...
+     }
+   }
+   The pose is a space-separated string of 16 floats representing a 4x4 matrix 
+   in row-major order (T_world_from_cam).
+
+2. Fusion Config JSON (--fusion-config-json):
+   {
+     "12345678": {
+       "FusionConfiguration": {
+         "pose": "...",
+         ...
+       },
+       ...
+     }
+   }
+
+Usage Example:
+    uv run apply_calibration_to_fusion_config.py \\
+        --calibration-json output/calibration.json \\
+        --fusion-config-json zed_settings/inside_network.json \\
+        --output-json output/updated_fusion_config.json
+"""
+
+import json
+import click
+import sys
+
+
+def validate_pose_string(pose_str: str) -> bool:
+    """Ensures the pose string contains exactly 16 floats."""
+    try:
+        parts = pose_str.split()
+        if len(parts) != 16:
+            return False
+        [float(p) for p in parts]
+        return True
+    except (ValueError, AttributeError):
+        return False
+
+
+@click.command()
+@click.option(
+    "--calibration-json",
+    required=True,
+    type=click.Path(exists=True),
+    help="Path to calibration output JSON.",
+)
+@click.option(
+    "--fusion-config-json",
+    required=True,
+    type=click.Path(exists=True),
+    help="Path to fusion configuration JSON.",
+)
+@click.option(
+    "--output-json",
+    required=True,
+    type=click.Path(),
+    help="Path to save the updated fusion configuration.",
+)
+@click.option(
+    "--strict/--no-strict",
+    default=False,
+    help="Fail if a calibration serial is missing in fusion config.",
+)
+def main(
+    calibration_json: str, fusion_config_json: str, output_json: str, strict: bool
+) -> None:
+    with open(calibration_json, "r") as f:
+        calib_data: dict[str, dict[str, str]] = json.load(f)
+
+    with open(fusion_config_json, "r") as f:
+        fusion_data: dict[str, dict[str, dict[str, str]]] = json.load(f)
+
+    updated_count = 0
+    missing_serials: list[str] = []
+    untouched_fusion_serials = set(fusion_data.keys())
+
+    # Sort serials for deterministic reporting
+    calib_serials = sorted(calib_data.keys())
+
+    for serial in calib_serials:
+        pose_str = calib_data[serial].get("pose")
+        if not pose_str:
+            click.echo(
+                f"Warning: Serial {serial} in calibration file has no 'pose' entry. Skipping.",
+                err=True,
+            )
+            continue
+
+        if not validate_pose_string(pose_str):
+            click.echo(
+                f"Error: Invalid pose string for serial {serial}. Must be 16 floats.",
+                err=True,
+            )
+            sys.exit(1)
+
+        if serial in fusion_data:
+            if "FusionConfiguration" in fusion_data[serial]:
+                fusion_data[serial]["FusionConfiguration"]["pose"] = pose_str
+                updated_count += 1
+                untouched_fusion_serials.discard(serial)
+            else:
+                click.echo(
+                    f"Warning: Serial {serial} found in fusion config but lacks 'FusionConfiguration' key.",
+                    err=True,
+                )
+                if strict:
+                    sys.exit(1)
+        else:
+            missing_serials.append(serial)
+            if strict:
+                click.echo(
+                    f"Error: Serial {serial} from calibration not found in fusion config (strict mode).",
+                    err=True,
+                )
+                sys.exit(1)
+
+    with open(output_json, "w") as f:
+        json.dump(fusion_data, f, indent=4)
+
+    click.echo("\n--- Summary ---")
+    click.echo(f"Updated: {updated_count}")
+
+    if missing_serials:
+        click.echo(
+            f"Serials in calibration but missing in fusion config: {', '.join(sorted(missing_serials))}"
+        )
+    else:
+        click.echo("All calibration serials were found in fusion config.")
+
+    if untouched_fusion_serials:
+        click.echo(
+            f"Fusion entries untouched: {', '.join(sorted(untouched_fusion_serials))}"
+        )
+    else:
+        click.echo("All fusion entries were updated.")
+
+    click.echo(f"Output written to: {output_json}")
+
+
+if __name__ == "__main__":
+    main()

py_workspace/calibrate_extrinsics.py

@@ -383,9 +383,9 @@ def apply_depth_verify_refine_postprocess(
                         results[str(serial)]["depth_pool"] = pool_metadata
 
                     improvement = verify_res.rmse - verify_res_post.rmse
-                    results[str(serial)]["refine_depth"]["improvement_rmse"] = (
-                        improvement
-                    )
+                    results[str(serial)]["refine_depth"][
+                        "improvement_rmse"
+                    ] = improvement
 
                     click.echo(
                         f"Camera {serial} refined: RMSE={verify_res_post.rmse:.3f}m "