crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add fusion pose updater and improve pose comparison workflow

Browse Source
This commit is contained in:
crosstyan
2026-02-09 03:26:50 +00:00
parent c497af7783
commit 77a93b71f2
4 changed files with 266 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/README.md
+17 -10
View File
@@ -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
+156
View File
@@ -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
+3 -3
View File
@@ -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 "
zed_settings/inside_shared_manual.json
+90
View File
@@ -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
}
}
}