ykcheungab
111 lines
3.4 KiB
Python
111 lines
3.4 KiB
Python
"""Open3D camera helpers for finger mesh examples."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from pathlib import Path
|
|
|
|
import numpy as np
|
|
|
|
DEFAULT_FINGER_VIZ_POSE = "assets/finger/visualization/world_to_cam.npy"
|
|
|
|
_CAMERA_X_ROT_90 = np.array(
|
|
[
|
|
[1.0, 0.0, 0.0],
|
|
[0.0, 0.0, -1.0],
|
|
[0.0, -1.0, 0.0],
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
|
|
_CAMERA_Z_ROT_180 = np.array(
|
|
[
|
|
[1.0, 0.0, 0.0],
|
|
[0.0, 1.0, -1],
|
|
[0.0, 0.0, -1],
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
|
|
|
|
def resolve_viz_pose_path(path: str | Path | None = None) -> Path:
|
|
"""Resolve world_to_cam.npy from cwd or packaged asset layout."""
|
|
raw = Path(path or DEFAULT_FINGER_VIZ_POSE).expanduser()
|
|
if raw.is_file():
|
|
return raw.resolve()
|
|
|
|
here = Path(__file__).resolve()
|
|
candidates = [
|
|
Path.cwd() / raw,
|
|
here.parents[2] / raw,
|
|
]
|
|
for candidate in candidates:
|
|
if candidate.is_file():
|
|
return candidate.resolve()
|
|
|
|
checked = ", ".join(str(p) for p in candidates)
|
|
raise FileNotFoundError(f"Visualization pose file not found: {raw}. Checked: {checked}")
|
|
|
|
|
|
def camera_look_at_from_world_to_cam(
|
|
world_to_cam: np.ndarray,
|
|
*,
|
|
lookat: np.ndarray,
|
|
distance_scale: float = 2.5,
|
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
|
"""Derive Open3D look_at(center, eye, up) from a Blender world_to_cam matrix."""
|
|
world_to_cam = np.asarray(world_to_cam, dtype=np.float64)
|
|
cam_to_world = np.linalg.inv(world_to_cam)
|
|
rotation = cam_to_world[:3, :3] @ _CAMERA_X_ROT_90 @ _CAMERA_Z_ROT_180
|
|
|
|
front = rotation @ np.array([0.0, 0.0, -1.0])
|
|
up = rotation @ np.array([0.0, 1.0, 0.0])
|
|
front_norm = np.linalg.norm(front)
|
|
up_norm = np.linalg.norm(up)
|
|
if front_norm > 0:
|
|
front = front / front_norm
|
|
if up_norm > 0:
|
|
up = up / up_norm
|
|
|
|
lookat = np.asarray(lookat, dtype=np.float64).reshape(3)
|
|
extent = float(np.linalg.norm(lookat - cam_to_world[:3, 3]))
|
|
if extent < 1e-6:
|
|
extent = 1.0
|
|
eye = lookat - front * (extent * distance_scale)
|
|
return lookat, eye, up
|
|
|
|
|
|
def apply_view_from_world_to_cam(vis, world_to_cam: np.ndarray, *, lookat: np.ndarray) -> None:
|
|
"""Apply Blender-exported world_to_cam as the Open3D default view."""
|
|
world_to_cam = np.asarray(world_to_cam, dtype=np.float64)
|
|
cam_to_world = np.linalg.inv(world_to_cam)
|
|
rotation = cam_to_world[:3, :3] @ _CAMERA_X_ROT_90 @ _CAMERA_Z_ROT_180
|
|
|
|
front = rotation @ np.array([0.0, 0.0, -1.0])
|
|
up = rotation @ np.array([0.0, 1.0, 0.0])
|
|
front_norm = np.linalg.norm(front)
|
|
up_norm = np.linalg.norm(up)
|
|
if front_norm > 0:
|
|
front = front / front_norm
|
|
if up_norm > 0:
|
|
up = up / up_norm
|
|
|
|
view_control = vis.get_view_control()
|
|
view_control.set_lookat(lookat.astype(np.float64))
|
|
view_control.set_front(front.astype(np.float64))
|
|
view_control.set_up(up.astype(np.float64))
|
|
vis.update_renderer()
|
|
|
|
|
|
def apply_mesh_lighting(vis) -> None:
|
|
"""Enable shaded lighting so the tactile mesh reads as a 3D surface."""
|
|
import open3d as o3d
|
|
|
|
render_option = vis.get_render_option()
|
|
if hasattr(render_option, "light_on"):
|
|
render_option.light_on = True
|
|
if hasattr(render_option, "mesh_color_option"):
|
|
render_option.mesh_color_option = o3d.visualization.MeshColorOption.Color
|
|
if hasattr(render_option, "mesh_shade_option"):
|
|
render_option.mesh_shade_option = o3d.visualization.MeshShadeOption.Color
|
|
vis.update_renderer()
|