diff --git a/py_workspace/visualize_extrinsics.py b/py_workspace/visualize_extrinsics.py
index 654b28a..754f74f 100644
--- a/py_workspace/visualize_extrinsics.py
+++ b/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,18 +539,30 @@ def main(
# Configure layout
# CV basis: +Y down, +Z forward
- scene_dict: Dict[str, Any] = dict(
- xaxis_title="X (Right)",
- yaxis_title="Y (Down)",
- zaxis_title="Z (Forward)",
- aspectmode="data",
- camera=dict(
- up=dict(
- x=0, y=-1, z=0
- ), # In Plotly's default view, +Y is up. To show +Y down, we set up to -Y.
- eye=dict(x=1.25, y=-1.25, z=1.25),
- ),
- )
+ if world_basis == "cv":
+ scene_dict: Dict[str, Any] = dict(
+ xaxis_title="X (Right)",
+ yaxis_title="Y (Down)",
+ zaxis_title="Z (Forward)",
+ aspectmode="data",
+ camera=dict(
+ up=dict(
+ x=0, y=-1, z=0
+ ), # In Plotly's default view, +Y is up. To show +Y down, we set up to -Y.
+ 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
World Basis: CV (+Y down, +Z fwd)",
+ title=f"Camera Extrinsics
World Basis: {world_basis.upper()} ({' +Y down, +Z fwd' if world_basis == 'cv' else '+Y up, +Z backward'})",
scene=scene_dict,
margin=dict(l=0, r=0, b=0, t=60),
legend=dict(x=0, y=1),