crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: implement global world-basis conversion for Plotly visualization

Browse Source
This commit is contained in:
crosstyan
2026-02-07 17:40:29 +00:00
parent 57f0dffbc5
commit 79f2ab04dc
3 changed files with 125 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/.beads/issues.jsonl
+3 -1
View File
@@ -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."}
py_workspace/README.md
+1 -1
View File
@@ -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<serial>.conf` filenames.
For full options:
py_workspace/visualize_extrinsics.py
+118 -58
View File
@@ -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,35 +194,37 @@ 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]
# 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]
x_axis_local = np.array([1, 0, 0])
y_axis_local = np.array([0, 1, 0])
z_axis_local = np.array([0, 0, 1])
# 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)
# Transform to world
# 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)
# 1-2, 2-3, 3-4, 4-1 (base)
@@ -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(