diff --git a/py_workspace/visualize_extrinsics.py b/py_workspace/visualize_extrinsics.py index 754f74f..b9982ba 100644 --- a/py_workspace/visualize_extrinsics.py +++ b/py_workspace/visualize_extrinsics.py @@ -33,25 +33,17 @@ def parse_pose(pose_str: str) -> np.ndarray: raise ValueError(f"Failed to parse pose string: {e}") -def world_to_plot(points: np.ndarray, world_basis: str = "cv") -> np.ndarray: +def world_to_plot(points: np.ndarray) -> np.ndarray: """ Transforms world-space points to plot-space. '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 @@ -176,7 +168,6 @@ 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. @@ -207,17 +198,17 @@ def add_camera_trace( # --- Apply Global Basis Transform --- # Transform everything from World Space -> Plot Space - center_plot = world_to_plot(center[None, :], world_basis=world_basis)[0] + center_plot = world_to_plot(center[None, :])[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, :], 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] + 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] - pts_plot = world_to_plot(pts_world, world_basis=world_basis) + pts_plot = world_to_plot(pts_world) # Create lines for frustum # Edges: 0-1, 0-2, 0-3, 0-4 (pyramid sides) @@ -373,12 +364,6 @@ 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], @@ -395,7 +380,6 @@ def main( ground_size: float, show_origin_axes: bool, origin_axes_scale: Optional[float], - world_basis: str, ): """Visualize camera extrinsics from JSON using Plotly.""" @@ -445,7 +429,6 @@ def main( frustum_scale=frustum_scale, fov_deg=fov, intrinsics=cam_intrinsics, - world_basis=world_basis, ) if show_origin_axes: @@ -458,10 +441,10 @@ def main( z_end = np.array([0, 0, axis_len]) # Transform to plot space - 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] + 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] fig.add_trace( go.Scatter3d( @@ -517,7 +500,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, world_basis=world_basis) + pts_ground_plot = world_to_plot(pts_ground) # Reshape back x_mesh_plot = pts_ground_plot[:, 0].reshape(x_mesh.shape) @@ -539,30 +522,18 @@ 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)", - 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), - ), - ) + 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 birdseye: # For birdseye, we force top-down view (looking down +Y towards X-Z plane) @@ -573,7 +544,7 @@ def main( ) fig.update_layout( - title=f"Camera Extrinsics
World Basis: {world_basis.upper()} ({' +Y down, +Z fwd' if world_basis == 'cv' else '+Y up, +Z backward'})", + title=f"Camera Extrinsics
World Basis: CV (+Y down, +Z fwd)", scene=scene_dict, margin=dict(l=0, r=0, b=0, t=60), legend=dict(x=0, y=1),