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/visualize_extrinsics.py
+121 -61
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,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(