fix: restore --world-basis option in visualize_extrinsics.py

py_workspace/visualize_extrinsics.py

@@ -6,7 +6,7 @@ import json
 import click
 import numpy as np
 import plotly.graph_objects as go
-from typing import Any, Dict, Optional, List, Tuple
+from typing import Any, Dict, Optional, List
 import configparser
 from pathlib import Path
 import re
@@ -33,17 +33,25 @@ def parse_pose(pose_str: str) -> np.ndarray:
         raise ValueError(f"Failed to parse pose string: {e}")
 
 
-def world_to_plot(points: np.ndarray) -> np.ndarray:
+def world_to_plot(points: np.ndarray, world_basis: str = "cv") -> np.ndarray:
     """
     Transforms world-space points to plot-space.
-    Currently a no-op as 'cv' basis is the only supported convention.
+    'cv' basis: +X right, +Y down, +Z forward (no-op).
+    'opengl' basis: +X right, +Y up, +Z backward.
 
     Args:
         points: (N, 3) array of points in world coordinates.
+        world_basis: 'cv' or 'opengl'.
 
     Returns:
         (N, 3) array of points.
     """
+    if world_basis == "opengl":
+        # CV -> OpenGL: Y = -Y, Z = -Z
+        pts_plot = points.copy()
+        pts_plot[:, 1] *= -1
+        pts_plot[:, 2] *= -1
+        return pts_plot
     return points
 
 
@@ -168,6 +176,7 @@ def add_camera_trace(
     fov_deg: float = 60.0,
     intrinsics: Optional[Dict[str, float]] = None,
     color: str = "blue",
+    world_basis: str = "cv",
 ):
     """
     Adds a camera frustum and axes to the Plotly figure.
@@ -198,17 +207,17 @@ def add_camera_trace(
 
     # --- Apply Global Basis Transform ---
     # Transform everything from World Space -> Plot Space
-    center_plot = world_to_plot(center[None, :])[0]
+    center_plot = world_to_plot(center[None, :], world_basis=world_basis)[0]
 
     x_end_world = center + x_axis_world * scale
     y_end_world = center + y_axis_world * scale
     z_end_world = center + z_axis_world * scale
 
-    x_end_plot = world_to_plot(x_end_world[None, :])[0]
-    y_end_plot = world_to_plot(y_end_world[None, :])[0]
-    z_end_plot = world_to_plot(z_end_world[None, :])[0]
+    x_end_plot = world_to_plot(x_end_world[None, :], world_basis=world_basis)[0]
+    y_end_plot = world_to_plot(y_end_world[None, :], world_basis=world_basis)[0]
+    z_end_plot = world_to_plot(z_end_world[None, :], world_basis=world_basis)[0]
 
-    pts_plot = world_to_plot(pts_world)
+    pts_plot = world_to_plot(pts_world, world_basis=world_basis)
 
     # Create lines for frustum
     # Edges: 0-1, 0-2, 0-3, 0-4 (pyramid sides)
@@ -364,6 +373,12 @@ def add_camera_trace(
     type=float,
     help="Scale of the world-origin axes triad. Defaults to --scale if not provided.",
 )
+@click.option(
+    "--world-basis",
+    type=click.Choice(["cv", "opengl"]),
+    default="cv",
+    help="World coordinate basis convention. 'cv' is +Y down, +Z forward. 'opengl' is +Y up, +Z backward.",
+)
 def main(
     input: str,
     output: Optional[str],
@@ -380,6 +395,7 @@ def main(
     ground_size: float,
     show_origin_axes: bool,
     origin_axes_scale: Optional[float],
+    world_basis: str,
 ):
     """Visualize camera extrinsics from JSON using Plotly."""
 
@@ -429,6 +445,7 @@ def main(
             frustum_scale=frustum_scale,
             fov_deg=fov,
             intrinsics=cam_intrinsics,
+            world_basis=world_basis,
         )
 
     if show_origin_axes:
@@ -441,10 +458,10 @@ def main(
         z_end = np.array([0, 0, axis_len])
 
         # Transform to plot space
-        origin_plot = world_to_plot(origin[None, :])[0]
-        x_end_plot = world_to_plot(x_end[None, :])[0]
-        y_end_plot = world_to_plot(y_end[None, :])[0]
-        z_end_plot = world_to_plot(z_end[None, :])[0]
+        origin_plot = world_to_plot(origin[None, :], world_basis=world_basis)[0]
+        x_end_plot = world_to_plot(x_end[None, :], world_basis=world_basis)[0]
+        y_end_plot = world_to_plot(y_end[None, :], world_basis=world_basis)[0]
+        z_end_plot = world_to_plot(z_end[None, :], world_basis=world_basis)[0]
 
         fig.add_trace(
             go.Scatter3d(
@@ -500,7 +517,7 @@ def main(
         pts_ground = np.stack(
             [x_mesh.flatten(), y_mesh.flatten(), z_mesh.flatten()], axis=1
         )
-        pts_ground_plot = world_to_plot(pts_ground)
+        pts_ground_plot = world_to_plot(pts_ground, world_basis=world_basis)
 
         # Reshape back
         x_mesh_plot = pts_ground_plot[:, 0].reshape(x_mesh.shape)
@@ -522,6 +539,7 @@ def main(
 
     # Configure layout
     # CV basis: +Y down, +Z forward
+    if world_basis == "cv":
         scene_dict: Dict[str, Any] = dict(
             xaxis_title="X (Right)",
             yaxis_title="Y (Down)",
@@ -534,6 +552,17 @@ def main(
                 eye=dict(x=1.25, y=-1.25, z=1.25),
             ),
         )
+    else:
+        scene_dict: Dict[str, Any] = dict(
+            xaxis_title="X (Right)",
+            yaxis_title="Y (Up)",
+            zaxis_title="Z (Backward)",
+            aspectmode="data",
+            camera=dict(
+                up=dict(x=0, y=1, z=0),
+                eye=dict(x=1.25, y=1.25, z=1.25),
+            ),
+        )
 
     if birdseye:
         # For birdseye, we force top-down view (looking down +Y towards X-Z plane)
@@ -544,7 +573,7 @@ def main(
         )
 
     fig.update_layout(
-        title="Camera Extrinsics<br><sup>World Basis: CV (+Y down, +Z fwd)</sup>",
+        title=f"Camera Extrinsics<br><sup>World Basis: {world_basis.upper()} ({' +Y down, +Z fwd' if world_basis == 'cv' else '+Y up, +Z backward'})</sup>",
         scene=scene_dict,
         margin=dict(l=0, r=0, b=0, t=60),
         legend=dict(x=0, y=1),