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 \ 
+uv run calibrate_extrinsics.py \
-   -s output/ -m aruco/markers/standard_box_markers_600mm.parquet \ 
+   -s output/ -m aruco/markers/standard_box_markers_600mm.parquet \
-   --aruco-dictionary DICT_APRILTAG_36h11 \ 
+   --aruco-dictionary DICT_APRILTAG_36h11 \
-   --verify-depth -refine-depth -no-preview \ 
+   --verify-depth \
-   -—max-samples 20 \ 
+   --refine-depth \
-   --report-csv output/e2e_refine_depth_smoke.csv \ 
+   --no-preview \
-   --auto-align \ 
+   --report-csv output/e2e_refine_depth.csv \
-   --output output/e2e_refine_depth_smoke.json
+   --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"] = (
+                    results[str(serial)]["refine_depth"][
-                        improvement
+                        "improvement_rmse"
-                    )
+                    ] = improvement
 
                     click.echo(
                         f"Camera {serial} refined: RMSE={verify_res_post.rmse:.3f}m "

zed_settings/inside_shared_manual.json

@@ -0,0 +1,90 @@
+{
+    "41831756": {
+        "FusionConfiguration": {
+            "communication_parameters": {
+                "CommunicationParameters": {
+                    "communication_type": "INTRA PROCESS",
+                    "ip_add": "",
+                    "ip_port": 0
+                }
+            },
+            "input_type": {
+                "InputType": {
+                    "input_type_conf": "41831756",
+                    "input_type_conf_right": "0",
+                    "input_type_input": "AUTO",
+                    "input_virtual_serial_number": 0
+                }
+            },
+            "override_gravity": false,
+            "pose": "-0.920142 -0.007144 -0.391519 2.737071 0.020828 -0.999311 -0.030716 0.998234 -0.391030 -0.036418 0.919657 -4.506511 0.000000 0.000000 0.000000 1.000000",
+            "serial_number": 41831756
+        }
+    },
+    "44289123": {
+        "FusionConfiguration": {
+            "communication_parameters": {
+                "CommunicationParameters": {
+                    "communication_type": "INTRA PROCESS",
+                    "ip_add": "",
+                    "ip_port": 0
+                }
+            },
+            "input_type": {
+                "InputType": {
+                    "input_type_conf": "44289123",
+                    "input_type_conf_right": "0",
+                    "input_type_input": "AUTO",
+                    "input_virtual_serial_number": 0
+                }
+            },
+            "override_gravity": false,
+            "pose": "-0.605210 -0.049218 -0.794543 4.775046 -0.000054 -0.998084 0.061868 1.091021 -0.796066 0.037486 0.604048 -3.432319 0.000000 0.000000 0.000000 1.000000",
+            "serial_number": 44289123
+        }
+    },
+    "44435674": {
+        "FusionConfiguration": {
+            "communication_parameters": {
+                "CommunicationParameters": {
+                    "communication_type": "INTRA PROCESS",
+                    "ip_add": "",
+                    "ip_port": 0
+                }
+            },
+            "input_type": {
+                "InputType": {
+                    "input_type_conf": "44435674",
+                    "input_type_conf_right": "0",
+                    "input_type_input": "AUTO",
+                    "input_virtual_serial_number": 0
+                }
+            },
+            "override_gravity": false,
+            "pose": "0.644946 -0.017302 0.764033 -1.445382 0.003236 -0.999673 -0.025370 1.093303 0.764222 0.018835 -0.644679 2.324294 0.000000 0.000000 0.000000 1.000000",
+            "serial_number": 44435674
+        }
+    },
+    "46195029": {
+        "FusionConfiguration": {
+            "communication_parameters": {
+                "CommunicationParameters": {
+                    "communication_type": "INTRA PROCESS",
+                    "ip_add": "",
+                    "ip_port": 0
+                }
+            },
+            "input_type": {
+                "InputType": {
+                    "input_type_conf": "46195029",
+                    "input_type_conf_right": "0",
+                    "input_type_input": "AUTO",
+                    "input_virtual_serial_number": 0
+                }
+            },
+            "override_gravity": false,
+            "pose": "0.590968 0.031646 -0.806074 4.336595 0.012877 -0.999473 -0.029798 1.141728 -0.806592 0.007230 -0.591065 2.475000 0.000000 0.000000 0.000000 1.000000",
+            "serial_number": 46195029
+        }
+    }
+}