feat: add --cv-to-opengl option to apply_calibration_to_fusion_config.py

py_workspace/apply_calibration_to_fusion_config.py

@@ -38,6 +38,7 @@ Usage Example:
 import json
 import click
 import sys
+import numpy as np
 
 
 def validate_pose_string(pose_str: str) -> bool:
@@ -76,9 +77,26 @@ def validate_pose_string(pose_str: str) -> bool:
     default=False,
     help="Fail if a calibration serial is missing in fusion config.",
 )
+@click.option(
+    "--cv-to-opengl",
+    is_flag=True,
+    default=False,
+    help="Convert poses from OpenCV convention (Y-down, Z-forward) to OpenGL convention (Y-up, Z-backward).",
+)
 def main(
-    calibration_json: str, fusion_config_json: str, output_json: str, strict: bool
+    calibration_json: str,
+    fusion_config_json: str,
+    output_json: str,
+    strict: bool,
+    cv_to_opengl: bool,
 ) -> None:
+    """
+    Apply calibration poses to a ZED Fusion configuration file.
+
+    If --cv-to-opengl is set, the T_world_from_cam matrix is converted from
+    OpenCV convention to OpenGL convention using T_gl = S @ T_cv @ S,
+    where S = diag(1, -1, -1, 1).
+    """
     with open(calibration_json, "r") as f:
         calib_data: dict[str, dict[str, str]] = json.load(f)
 
@@ -108,6 +126,29 @@ def main(
             )
             sys.exit(1)
 
+        if cv_to_opengl:
+            # Convert OpenCV to OpenGL convention
+            # S = diag(1, -1, -1, 1)
+            # T_gl = S @ T_cv @ S
+            try:
+                vals = [float(p) for p in pose_str.split()]
+                t_cv = np.array(vals).reshape((4, 4))
+
+                # Validate it's a proper transform (roughly)
+                det = np.linalg.det(t_cv[:3, :3])
+                if not np.isclose(abs(det), 1.0, atol=1e-3):
+                    click.echo(
+                        f"Warning: Pose for {serial} may not be a valid rotation matrix (det={det:.4f})",
+                        err=True,
+                    )
+
+                s = np.diag([1, -1, -1, 1])
+                t_gl = s @ t_cv @ s
+                pose_str = " ".join(f"{x:.8f}" for x in t_gl.flatten())
+            except Exception as e:
+                click.echo(f"Error converting pose for serial {serial}: {e}", err=True)
+                sys.exit(1)
+
         if serial in fusion_data:
             if "FusionConfiguration" in fusion_data[serial]:
                 fusion_data[serial]["FusionConfiguration"]["pose"] = pose_str