From 79f2ab04dc8d71ae0288351aec0cc7d514c79162 Mon Sep 17 00:00:00 2001 From: crosstyan Date: Sat, 7 Feb 2026 17:40:29 +0000 Subject: [PATCH] feat: implement global world-basis conversion for Plotly visualization --- py_workspace/.beads/issues.jsonl | 4 +- py_workspace/README.md | 2 +- py_workspace/visualize_extrinsics.py | 182 ++++++++++++++++++--------- 3 files changed, 125 insertions(+), 63 deletions(-) diff --git a/py_workspace/.beads/issues.jsonl b/py_workspace/.beads/issues.jsonl index c958855..8ad4c87 100644 --- a/py_workspace/.beads/issues.jsonl +++ b/py_workspace/.beads/issues.jsonl @@ -1,13 +1,15 @@ {"id":"py_workspace-0mu","title":"Implement --render-space in visualize_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T16:08:12.543309499Z","created_by":"crosstyan","updated_at":"2026-02-07T16:08:17.303232927Z","closed_at":"2026-02-07T16:08:17.303232927Z","close_reason":"Implemented --render-space with opencv/opengl choices and updated README"} {"id":"py_workspace-0q7","title":"Fix basedpyright errors in aruco/pose_averaging.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T08:53:33.377735199Z","created_by":"crosstyan","updated_at":"2026-02-07T08:58:49.252312392Z","closed_at":"2026-02-07T08:58:49.252312392Z","close_reason":"Fixed basedpyright errors"} {"id":"py_workspace-214","title":"Migrate visualize_extrinsics to Plotly with diagnose mode","status":"closed","priority":2,"issue_type":"feature","owner":"crosstyan@outlook.com","created_at":"2026-02-07T15:14:40.547616056Z","created_by":"crosstyan","updated_at":"2026-02-07T15:25:00.354290874Z","closed_at":"2026-02-07T15:25:00.354290874Z","close_reason":"Fixed QA issues: Y-up enforcement, README sync, dependencies"} -{"id":"py_workspace-2c1","title":"Add manual ground-plane overlay to visualize_extrinsics.py","status":"open","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T16:15:17.432846006Z","created_by":"crosstyan","updated_at":"2026-02-07T16:15:17.432846006Z"} +{"id":"py_workspace-2c1","title":"Add manual ground-plane overlay to visualize_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T16:15:17.432846006Z","created_by":"crosstyan","updated_at":"2026-02-07T16:16:18.287496896Z","closed_at":"2026-02-07T16:16:18.287496896Z","close_reason":"Implemented ground-plane overlay with CLI options and updated README."} {"id":"py_workspace-62y","title":"Fix depth pooling fallback threshold","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T08:12:12.046607198Z","created_by":"crosstyan","updated_at":"2026-02-07T08:13:12.98625698Z","closed_at":"2026-02-07T08:13:12.98625698Z","close_reason":"Updated fallback threshold to strict comparison"} {"id":"py_workspace-6m5","title":"Robust Optimizer Implementation","status":"closed","priority":0,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T05:22:45.183574374Z","created_by":"crosstyan","updated_at":"2026-02-07T05:22:53.151871639Z","closed_at":"2026-02-07T05:22:53.151871639Z","close_reason":"Implemented robust optimizer with least_squares and soft_l1 loss, updated tests"} {"id":"py_workspace-6sg","title":"Document marker parquet structure","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T02:48:08.95742431Z","created_by":"crosstyan","updated_at":"2026-02-07T02:49:35.897152691Z","closed_at":"2026-02-07T02:49:35.897152691Z","close_reason":"Documented parquet structure in aruco/markers/PARQUET_FORMAT.md"} +{"id":"py_workspace-7ul","title":"Implement global world-basis conversion for Plotly visualization","status":"open","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T17:30:41.94482545Z","created_by":"crosstyan","updated_at":"2026-02-07T17:30:41.94482545Z"} {"id":"py_workspace-98p","title":"Integrate multi-frame depth pooling into calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T07:59:35.333468652Z","created_by":"crosstyan","updated_at":"2026-02-07T08:06:37.662956356Z","closed_at":"2026-02-07T08:06:37.662956356Z","close_reason":"Implemented multi-frame depth pooling and verified with tests"} {"id":"py_workspace-a85","title":"Add CLI option for ArUco dictionary in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:13:41.896728814Z","created_by":"crosstyan","updated_at":"2026-02-07T07:29:52.290976525Z","closed_at":"2026-02-07T07:29:52.290976525Z","close_reason":"Implemented multi-frame depth pooling in calibrate_extrinsics.py"} {"id":"py_workspace-afh","title":"Inspect tmp_visualizer.html camera layout","notes":"Inspected tmp_visualizer.html. cam_0 is at (0,0,0). cam_1 is at (1,0,0). cam_2 is at (0, 0.5, 1.0). Axes are RGB=XYZ. Layout matches expected synthetic geometry.","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T15:40:04.162565539Z","created_by":"crosstyan","updated_at":"2026-02-07T15:42:10.721124074Z","closed_at":"2026-02-07T15:42:10.721124074Z","close_reason":"Inspection complete. Layout matches synthetic input."} +{"id":"py_workspace-cg4","title":"Implement geometry-first auto-align heuristic","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T16:48:33.048250646Z","created_by":"crosstyan","updated_at":"2026-02-07T16:53:54.772815505Z","closed_at":"2026-02-07T16:53:54.772815505Z","close_reason":"Closed"} {"id":"py_workspace-cg9","title":"Implement core alignment utilities (Task 1)","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-06T10:40:36.296030875Z","created_by":"crosstyan","updated_at":"2026-02-06T10:40:46.196825039Z","closed_at":"2026-02-06T10:40:46.196825039Z","close_reason":"Implemented compute_face_normal, rotation_align_vectors, and apply_alignment_to_pose in aruco/alignment.py"} {"id":"py_workspace-ee1","title":"Implement depth-mode argument resolution in calibrate_extrinsics.py","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T06:31:03.430147225Z","created_by":"crosstyan","updated_at":"2026-02-07T06:33:43.204825053Z","closed_at":"2026-02-07T06:33:43.204825053Z","close_reason":"Implemented depth-mode argument resolution logic and verified with multiple test cases."} {"id":"py_workspace-j8b","title":"Research scipy.optimize.least_squares robust optimization for depth residuals","status":"closed","priority":2,"issue_type":"task","owner":"crosstyan@outlook.com","created_at":"2026-02-07T04:54:04.720996955Z","created_by":"crosstyan","updated_at":"2026-02-07T04:55:22.995644Z","closed_at":"2026-02-07T04:55:22.995644Z","close_reason":"Research completed and recommendations provided."} diff --git a/py_workspace/README.md b/py_workspace/README.md index 467fad0..4495b84 100755 --- a/py_workspace/README.md +++ b/py_workspace/README.md @@ -102,7 +102,7 @@ uv run visualize_extrinsics.py -i output/extrinsics.json \ **Troubleshooting:** - **Cameras bunched up?** Check `--pose-convention`. `world_from_cam` is the standard convention for `calibrate_extrinsics.py` outputs. `cam_from_world` is deprecated. -- **Axes flipped?** Use `--render-space opengl` to match C++ viewer conventions (X right, Y up, Z backward). Default is `opencv` (X right, Y down, Z forward). +- **Axes flipped?** Use `--world-basis opengl` to match C++ viewer conventions (X right, Y up, Z backward). Default is `cv` (X right, Y down, Z forward). - **Config not matching?** Ensure JSON keys match the serial numbers in `SN.conf` filenames. For full options: diff --git a/py_workspace/visualize_extrinsics.py b/py_workspace/visualize_extrinsics.py index 78a7a30..26fdffc 100644 --- a/py_workspace/visualize_extrinsics.py +++ b/py_workspace/visualize_extrinsics.py @@ -33,6 +33,24 @@ def parse_pose(pose_str: str) -> np.ndarray: raise ValueError(f"Failed to parse pose string: {e}") +def world_to_plot(points: np.ndarray, basis: str) -> np.ndarray: + """ + Transforms world-space points to plot-space based on the selected basis. + + Args: + points: (N, 3) array of points in world coordinates. + basis: 'cv' (no change) or 'opengl' (flip Y and Z). + + Returns: + (N, 3) array of transformed points. + """ + if basis == "opengl": + # Global transform: diag(1, -1, -1) + # This flips World Y and World Z for the entire scene + return points * np.array([1, -1, -1]) + return points + + def load_zed_configs( paths: List[str], resolution: str, eye: str ) -> Dict[str, Dict[str, float]]: @@ -151,7 +169,7 @@ def add_camera_trace( label: str, scale: float = 0.2, convention: str = "world_from_cam", - render_space: str = "opencv", + world_basis: str = "cv", frustum_scale: float = 0.5, fov_deg: float = 60.0, intrinsics: Optional[Dict[str, float]] = None, @@ -176,34 +194,36 @@ def add_camera_trace( center = t R_world = R - # Local axes in world frame - 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] + # OpenCV convention: X right, Y down, Z forward + x_axis_local = np.array([1, 0, 0]) + y_axis_local = np.array([0, 1, 0]) + z_axis_local = np.array([0, 0, 1]) - # 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 local axes to world + x_axis_world = R_world @ x_axis_local + y_axis_world = R_world @ y_axis_local + z_axis_world = R_world @ z_axis_local - # Frustum points in local coordinates (OpenCV: +Z fwd, +X right, +Y down) - pts_local = get_frustum_points(intrinsics, frustum_scale, fov_deg) + # 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 + # Transform frustum to world + pts_world = (R_world @ pts_local.T).T + center + + # --- Apply Global Basis Transform --- + # Transform everything from World Space -> Plot Space + center_plot = world_to_plot(center[None, :], world_basis)[0] + + # For axes, we need to transform the end points + 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)[0] + y_end_plot = world_to_plot(y_end_world[None, :], world_basis)[0] + z_end_plot = world_to_plot(z_end_world[None, :], world_basis)[0] + + pts_plot = world_to_plot(pts_world, world_basis) # Create lines for frustum # Edges: 0-1, 0-2, 0-3, 0-4 (pyramid sides) @@ -213,9 +233,9 @@ def add_camera_trace( z_lines = [] def add_line(i, j): - x_lines.extend([pts_world[i, 0], pts_world[j, 0], None]) - y_lines.extend([pts_world[i, 1], pts_world[j, 1], None]) - z_lines.extend([pts_world[i, 2], pts_world[j, 2], None]) + x_lines.extend([pts_plot[i, 0], pts_plot[j, 0], None]) + y_lines.extend([pts_plot[i, 1], pts_plot[j, 1], None]) + z_lines.extend([pts_plot[i, 2], pts_plot[j, 2], None]) # Pyramid sides for i in range(1, 5): @@ -243,9 +263,9 @@ def add_camera_trace( # Add center point with label fig.add_trace( go.Scatter3d( - x=[center[0]], - y=[center[1]], - z=[center[2]], + x=[center_plot[0]], + y=[center_plot[1]], + z=[center_plot[2]], mode="markers+text", marker=dict(size=4, color="black"), text=[label], @@ -256,13 +276,12 @@ def add_camera_trace( ) # Add axes (RGB = XYZ) - axis_len = scale # X axis (Red) fig.add_trace( go.Scatter3d( - x=[center[0], center[0] + x_axis[0] * axis_len], - y=[center[1], center[1] + x_axis[1] * axis_len], - z=[center[2], center[2] + x_axis[2] * axis_len], + x=[center_plot[0], x_end_plot[0]], + y=[center_plot[1], x_end_plot[1]], + z=[center_plot[2], x_end_plot[2]], mode="lines", line=dict(color="red", width=3), showlegend=False, @@ -272,9 +291,9 @@ def add_camera_trace( # Y axis (Green) fig.add_trace( go.Scatter3d( - x=[center[0], center[0] + y_axis[0] * axis_len], - y=[center[1], center[1] + y_axis[1] * axis_len], - z=[center[2], center[2] + y_axis[2] * axis_len], + x=[center_plot[0], y_end_plot[0]], + y=[center_plot[1], y_end_plot[1]], + z=[center_plot[2], y_end_plot[2]], mode="lines", line=dict(color="green", width=3), showlegend=False, @@ -284,9 +303,9 @@ def add_camera_trace( # Z axis (Blue) fig.add_trace( go.Scatter3d( - x=[center[0], center[0] + z_axis[0] * axis_len], - y=[center[1], center[1] + z_axis[1] * axis_len], - z=[center[2], center[2] + z_axis[2] * axis_len], + x=[center_plot[0], z_end_plot[0]], + y=[center_plot[1], z_end_plot[1]], + z=[center_plot[2], z_end_plot[2]], mode="lines", line=dict(color="blue", width=3), showlegend=False, @@ -420,11 +439,17 @@ def run_diagnostics(poses: Dict[str, np.ndarray], convention: str): default="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( + "--world-basis", + type=click.Choice(["cv", "opengl"]), + default="cv", + help="Global world basis convention. 'cv' (default) is +Y down, +Z forward. 'opengl' flips Y and Z (diag(1,-1,-1)) for the entire scene.", +) @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.", + default=None, + help="DEPRECATED: Use --world-basis instead. 'opencv' maps to 'cv', 'opengl' maps to 'opengl'.", ) @click.option( "--frustum-scale", type=float, default=0.5, help="Scale of the camera frustum." @@ -486,7 +511,8 @@ def main( scale: float, birdseye: bool, pose_convention: str, - render_space: str, + world_basis: str, + render_space: Optional[str], frustum_scale: float, fov: float, zed_configs: List[str], @@ -499,6 +525,17 @@ def main( show_origin_axes: bool, ): """Visualize camera extrinsics from JSON using Plotly.""" + + # Handle deprecated argument + if render_space is not None: + print( + "WARNING: --render-space is deprecated. Please use --world-basis instead." + ) + if render_space == "opencv": + world_basis = "cv" + elif render_space == "opengl": + world_basis = "opengl" + try: with open(input, "r") as f: data = json.load(f) @@ -546,7 +583,7 @@ def main( str(serial), scale=scale, convention=pose_convention, - render_space=render_space, + world_basis=world_basis, frustum_scale=frustum_scale, fov_deg=fov, intrinsics=cam_intrinsics, @@ -555,11 +592,23 @@ def main( if show_origin_axes: origin = np.zeros(3) axis_len = scale + + # Define world axes points + x_end = np.array([axis_len, 0, 0]) + y_end = np.array([0, axis_len, 0]) + z_end = np.array([0, 0, axis_len]) + + # Transform to plot space + origin_plot = world_to_plot(origin[None, :], world_basis)[0] + x_end_plot = world_to_plot(x_end[None, :], world_basis)[0] + y_end_plot = world_to_plot(y_end[None, :], world_basis)[0] + z_end_plot = world_to_plot(z_end[None, :], world_basis)[0] + fig.add_trace( go.Scatter3d( - x=[origin[0], origin[0] + axis_len], - y=[origin[1], origin[1]], - z=[origin[2], origin[2]], + x=[origin_plot[0], x_end_plot[0]], + y=[origin_plot[1], x_end_plot[1]], + z=[origin_plot[2], x_end_plot[2]], mode="lines", line=dict(color="red", width=4), name="World X", @@ -571,9 +620,9 @@ def main( ) fig.add_trace( go.Scatter3d( - x=[origin[0], origin[0]], - y=[origin[1], origin[1] + axis_len], - z=[origin[2], origin[2]], + x=[origin_plot[0], y_end_plot[0]], + y=[origin_plot[1], y_end_plot[1]], + z=[origin_plot[2], y_end_plot[2]], mode="lines", line=dict(color="green", width=4), name="World Y", @@ -585,9 +634,9 @@ def main( ) fig.add_trace( go.Scatter3d( - x=[origin[0], origin[0]], - y=[origin[1], origin[1]], - z=[origin[2], origin[2] + axis_len], + x=[origin_plot[0], z_end_plot[0]], + y=[origin_plot[1], z_end_plot[1]], + z=[origin_plot[2], z_end_plot[2]], mode="lines", line=dict(color="blue", width=4), name="World Z", @@ -605,11 +654,22 @@ def main( x_mesh, z_mesh = np.meshgrid(x_grid, z_grid) y_mesh = np.full_like(x_mesh, ground_y) + # Flatten for transformation + 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) + + # Reshape back + x_mesh_plot = pts_ground_plot[:, 0].reshape(x_mesh.shape) + y_mesh_plot = pts_ground_plot[:, 1].reshape(y_mesh.shape) + z_mesh_plot = pts_ground_plot[:, 2].reshape(z_mesh.shape) + fig.add_trace( go.Surface( - x=x_mesh, - y=y_mesh, - z=z_mesh, + x=x_mesh_plot, + y=y_mesh_plot, + z=z_mesh_plot, showscale=False, opacity=0.15, colorscale=[[0, "gray"], [1, "gray"]], @@ -636,9 +696,9 @@ def main( ) render_desc = ( - "OpenCV: local +X,+Y,+Z (Y-down)" - if render_space == "opencv" - else "OpenGL: local +X,-Y,-Z (Y-up, Z-back) rel. to OpenCV" + "World Basis: CV (+Y down, +Z fwd)" + if world_basis == "cv" + else "World Basis: OpenGL (+Y up, -Z fwd)" ) fig.update_layout(