feat: add --render-space option to visualize_extrinsics.py

py_workspace/visualize_extrinsics.py

@@ -151,6 +151,7 @@ def add_camera_trace(
     label: str,
     scale: float = 0.2,
     convention: str = "world_from_cam",
+    render_space: str = "opencv",
     frustum_scale: float = 0.5,
     fov_deg: float = 60.0,
     intrinsics: Optional[Dict[str, float]] = None,
@@ -163,26 +164,46 @@ def add_camera_trace(
     t = pose[:3, 3]
 
     if convention == "cam_from_world":
+        # DEPRECATED: calibrate_extrinsics.py outputs world_from_cam.
+        # This path is kept for legacy compatibility but should be avoided for new calibrations.
         # Camera center in world coordinates: C = -R^T * t
         center = -R.T @ t
         # Camera orientation in world coordinates: R_world_from_cam = R^T
         R_world = R.T
     else:
-        # world_from_cam
+        # world_from_cam (Standard convention for calibrate_extrinsics.py)
+        # calibrate_extrinsics.py inverts the solvePnP result before saving.
         center = t
         R_world = R
 
     # Local axes in world frame
-    x_axis = R_world[:, 0]
-    y_axis = R_world[:, 1]
-    z_axis = R_world[:, 2]
+    if render_space == "opengl":
+        # OpenGL convention: X right, Y up, Z backward (toOGL = diag(1,-1,-1))
+        # R_render = R_world @ diag(1, -1, -1)
+        R_render = R_world @ np.diag([1, -1, -1])
+        x_axis = R_render[:, 0]
+        y_axis = R_render[:, 1]
+        z_axis = R_render[:, 2]
 
-    # Frustum points in local coordinates (OpenCV: +Z fwd, +X right, +Y down)
-    pts_local = get_frustum_points(intrinsics, frustum_scale, fov_deg)
+        # To keep the frustum consistent (pointing in the same world direction),
+        # we must also flip its local coordinates because OpenGL looks down -Z.
+        # pts_local_cv = get_frustum_points(...)
+        # pts_local_ogl = diag(1,-1,-1) @ pts_local_cv
+        # pts_world = R_render @ pts_local_ogl + center
+        pts_local_cv = get_frustum_points(intrinsics, frustum_scale, fov_deg)
+        pts_local_ogl = pts_local_cv * np.array([1, -1, -1])
+        pts_world = (R_render @ pts_local_ogl.T).T + center
+    else:
+        # OpenCV convention: X right, Y down, Z forward
+        x_axis = R_world[:, 0]
+        y_axis = R_world[:, 1]
+        z_axis = R_world[:, 2]
 
-    # Transform to world
-    # pts_world = (R_world @ pts_local.T).T + center
-    pts_world = (R_world @ pts_local.T).T + center
+        # Frustum points in local coordinates (OpenCV: +Z fwd, +X right, +Y down)
+        pts_local = get_frustum_points(intrinsics, frustum_scale, fov_deg)
+
+        # Transform to world
+        pts_world = (R_world @ pts_local.T).T + center
 
     # Create lines for frustum
     # Edges: 0-1, 0-2, 0-3, 0-4 (pyramid sides)
@@ -280,6 +301,14 @@ def run_diagnostics(poses: Dict[str, np.ndarray], convention: str):
     """
     print("\n--- Diagnostics ---")
     print(f"Pose Convention: {convention}")
+    if convention == "cam_from_world":
+        print(
+            "  WARNING: 'cam_from_world' is deprecated. calibrate_extrinsics.py outputs 'world_from_cam'."
+        )
+    else:
+        print(
+            "  Note: Using 'world_from_cam' (matches calibrate_extrinsics.py output)."
+        )
 
     centers = []
     rotations = []
@@ -389,7 +418,13 @@ def run_diagnostics(poses: Dict[str, np.ndarray], convention: str):
     "--pose-convention",
     type=click.Choice(["world_from_cam", "cam_from_world"]),
     default="world_from_cam",
-    help="Interpretation of the pose matrix in JSON. Defaults to 'world_from_cam'.",
+    help="Interpretation of the pose matrix in JSON. Defaults to 'world_from_cam' (matches calibrate_extrinsics.py). 'cam_from_world' is deprecated.",
+)
+@click.option(
+    "--render-space",
+    type=click.Choice(["opencv", "opengl"]),
+    default="opencv",
+    help="Render space convention. 'opencv' (default) is camera-local +Z forward. 'opengl' applies diag(1,-1,-1) conversion for visualization.",
 )
 @click.option(
     "--frustum-scale", type=float, default=0.5, help="Scale of the camera frustum."
@@ -429,6 +464,7 @@ def main(
     scale: float,
     birdseye: bool,
     pose_convention: str,
+    render_space: str,
     frustum_scale: float,
     fov: float,
     zed_configs: List[str],
@@ -484,6 +520,7 @@ def main(
             str(serial),
             scale=scale,
             convention=pose_convention,
+            render_space=render_space,
             frustum_scale=frustum_scale,
             fov_deg=fov,
             intrinsics=cam_intrinsics,
@@ -507,7 +544,7 @@ def main(
         )
 
     fig.update_layout(
-        title=f"Camera Extrinsics ({pose_convention})",
+        title=f"Camera Extrinsics ({pose_convention}, {render_space})",
         scene=scene_dict,
         margin=dict(l=0, r=0, b=0, t=40),
         legend=dict(x=0, y=1),