diff --git a/py_workspace/README.md b/py_workspace/README.md index 1540bad..498aec5 100755 --- a/py_workspace/README.md +++ b/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):** diff --git a/py_workspace/apply_calibration_to_fusion_config.py b/py_workspace/apply_calibration_to_fusion_config.py new file mode 100644 index 0000000..9fe03c4 --- /dev/null +++ b/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() diff --git a/py_workspace/calibrate_extrinsics.py b/py_workspace/calibrate_extrinsics.py index 4332b59..e2724a0 100644 --- a/py_workspace/calibrate_extrinsics.py +++ b/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 " diff --git a/zed_settings/inside_shared_manual.json b/zed_settings/inside_shared_manual.json new file mode 100644 index 0000000..d2aebe9 --- /dev/null +++ b/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 + } + } +} \ No newline at end of file