GUI for finger v1
This commit is contained in:
ykcheungab
@@ -1,20 +1,38 @@
|
||||
"""Shared visualization helpers for Orisys example scripts."""
|
||||
|
||||
from .finger_panels import show_finger_uv_panels
|
||||
from .force_charts import ForceChartWindow
|
||||
from .mesh_camera import (
|
||||
DEFAULT_FINGER_VIZ_POSE,
|
||||
apply_mesh_lighting,
|
||||
apply_view_from_world_to_cam,
|
||||
camera_look_at_from_world_to_cam,
|
||||
resolve_viz_pose_path,
|
||||
)
|
||||
from .offscreen_mesh import OffscreenFingerMeshRenderer
|
||||
from .render_2d import render_contact_panel, render_depth_panel, render_uv_arrows
|
||||
from .finger_mesh_viewer import FingerMeshViewer
|
||||
from .mesh_viewer import UVMeshViewer, build_uv_surface_mesh
|
||||
from .opencv_maps import depth_colormap, draw_magnitude_map, frame_to_bgr
|
||||
from .recording import FrameRecorder
|
||||
from .signal_filters import lowpass_filter
|
||||
|
||||
__all__ = [
|
||||
"DEFAULT_FINGER_VIZ_POSE",
|
||||
"FingerMeshViewer",
|
||||
"ForceChartWindow",
|
||||
"FrameRecorder",
|
||||
"UVMeshViewer",
|
||||
"apply_mesh_lighting",
|
||||
"apply_view_from_world_to_cam",
|
||||
"build_uv_surface_mesh",
|
||||
"camera_look_at_from_world_to_cam",
|
||||
"depth_colormap",
|
||||
"draw_magnitude_map",
|
||||
"frame_to_bgr",
|
||||
"lowpass_filter",
|
||||
"show_finger_uv_panels",
|
||||
"OffscreenFingerMeshRenderer",
|
||||
"render_contact_panel",
|
||||
"render_depth_panel",
|
||||
"render_uv_arrows",
|
||||
"resolve_viz_pose_path",
|
||||
]
|
||||
|
||||
@@ -0,0 +1,223 @@
|
||||
"""Contact normal-force arrow overlay for finger 3D mesh examples."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from orisys.finger.mesh import UVSurfaceMesh, compute_uv_surface_frames, mesh_uv_to_world_scale
|
||||
|
||||
DEFAULT_NORMAL_FORCE_SCALE = 1.0
|
||||
MIN_ARROW_LENGTH_FRACTION = 0.02
|
||||
MAX_ARROW_LENGTH_FRACTION = 0.30
|
||||
REFERENCE_NORMAL_FORCE = 10000.0
|
||||
MIN_ARROW_THICKNESS_SCALE = 0.20
|
||||
MAX_ARROW_THICKNESS_SCALE = 1.0
|
||||
CONTACT_ARROW_COLOR = (1.0, 0.92, 0.10)
|
||||
|
||||
|
||||
def _mesh_span(vertices: np.ndarray) -> float:
|
||||
extent = np.ptp(vertices, axis=0)
|
||||
return float(max(np.max(extent), 1e-6))
|
||||
|
||||
|
||||
def _rotation_matrix_z_to(direction: np.ndarray) -> np.ndarray:
|
||||
"""Rotation matrix mapping unit +Z to ``direction``."""
|
||||
direction = np.asarray(direction, dtype=np.float64).reshape(3)
|
||||
norm = np.linalg.norm(direction)
|
||||
if norm < 1e-8:
|
||||
return np.eye(3, dtype=np.float64)
|
||||
direction = direction / norm
|
||||
|
||||
z_axis = np.array([0.0, 0.0, 1.0], dtype=np.float64)
|
||||
cross = np.cross(z_axis, direction)
|
||||
dot = float(np.dot(z_axis, direction))
|
||||
if dot > 0.9999:
|
||||
return np.eye(3, dtype=np.float64)
|
||||
if dot < -0.9999:
|
||||
return np.diag([1.0, -1.0, -1.0]).astype(np.float64)
|
||||
|
||||
skew = np.array(
|
||||
[
|
||||
[0.0, -cross[2], cross[1]],
|
||||
[cross[2], 0.0, -cross[0]],
|
||||
[-cross[1], cross[0], 0.0],
|
||||
],
|
||||
dtype=np.float64,
|
||||
)
|
||||
return np.eye(3, dtype=np.float64) + skew + skew @ skew * (1.0 / (1.0 + dot))
|
||||
|
||||
|
||||
def thickness_scale_from_force(force: float) -> float:
|
||||
"""Map normal force to a radial scale factor (thin at low force, thicker at high force)."""
|
||||
if force <= 0.0:
|
||||
return 0.0
|
||||
t = force / REFERENCE_NORMAL_FORCE
|
||||
return float(
|
||||
np.clip(
|
||||
MIN_ARROW_THICKNESS_SCALE + (MAX_ARROW_THICKNESS_SCALE - MIN_ARROW_THICKNESS_SCALE) * t,
|
||||
MIN_ARROW_THICKNESS_SCALE,
|
||||
MAX_ARROW_THICKNESS_SCALE,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def _scale_arrow_template(
|
||||
template_vertices: np.ndarray,
|
||||
*,
|
||||
length_scale: float,
|
||||
thickness_scale: float,
|
||||
) -> np.ndarray:
|
||||
"""Scale a +Z arrow template axially and radially."""
|
||||
scaled = np.asarray(template_vertices, dtype=np.float64).copy()
|
||||
scaled[:, 0] *= thickness_scale
|
||||
scaled[:, 1] *= thickness_scale
|
||||
scaled[:, 2] *= length_scale
|
||||
return scaled
|
||||
|
||||
|
||||
def create_arrow_template(o3d):
|
||||
"""Unit-height arrow mesh along +Z; radial size is scaled per-frame from force."""
|
||||
arrow = o3d.geometry.TriangleMesh.create_arrow(
|
||||
cylinder_radius=0.03,
|
||||
cone_radius=0.06,
|
||||
cylinder_height=0.72,
|
||||
cone_height=0.28,
|
||||
)
|
||||
arrow.compute_vertex_normals()
|
||||
return (
|
||||
np.asarray(arrow.vertices, dtype=np.float64),
|
||||
np.asarray(arrow.triangles, dtype=np.int32),
|
||||
)
|
||||
|
||||
|
||||
def build_contact_normal_arrow_mesh(
|
||||
origin: np.ndarray,
|
||||
normal: np.ndarray,
|
||||
length: float,
|
||||
force: float,
|
||||
*,
|
||||
mesh_span: float,
|
||||
template_vertices: np.ndarray,
|
||||
template_triangles: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||
"""Build a yellow 3D arrow mesh pointing opposite the surface normal."""
|
||||
origin = np.asarray(origin, dtype=np.float64).reshape(3)
|
||||
normal = np.asarray(normal, dtype=np.float64).reshape(3)
|
||||
norm = np.linalg.norm(normal)
|
||||
color = np.tile(np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64), (len(template_vertices), 1))
|
||||
|
||||
radial_scale = thickness_scale_from_force(force)
|
||||
if norm < 1e-8 or length <= 0.0 or radial_scale <= 0.0 or not np.all(np.isfinite(origin)):
|
||||
return np.zeros_like(template_vertices), template_triangles, color
|
||||
|
||||
direction = -normal / norm
|
||||
length = float(np.clip(length, mesh_span * MIN_ARROW_LENGTH_FRACTION, mesh_span * MAX_ARROW_LENGTH_FRACTION))
|
||||
if length <= 0.0:
|
||||
return np.zeros_like(template_vertices), template_triangles, color
|
||||
|
||||
rotation = _rotation_matrix_z_to(direction)
|
||||
scaled_template = _scale_arrow_template(
|
||||
template_vertices,
|
||||
length_scale=length,
|
||||
thickness_scale=radial_scale,
|
||||
)
|
||||
vertices = scaled_template @ rotation.T + origin
|
||||
return vertices, template_triangles, color
|
||||
|
||||
|
||||
def create_arrow_mesh_geometry(o3d, template_vertices: np.ndarray, template_triangles: np.ndarray):
|
||||
"""Create a persistent Open3D mesh for the contact arrow overlay."""
|
||||
arrow_mesh = o3d.geometry.TriangleMesh()
|
||||
arrow_mesh.vertices = o3d.utility.Vector3dVector(np.zeros_like(template_vertices))
|
||||
arrow_mesh.triangles = o3d.utility.Vector3iVector(template_triangles)
|
||||
arrow_mesh.vertex_colors = o3d.utility.Vector3dVector(
|
||||
np.tile(np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64), (len(template_vertices), 1))
|
||||
)
|
||||
arrow_mesh.compute_vertex_normals()
|
||||
return arrow_mesh
|
||||
|
||||
|
||||
class ContactArrowOverlay:
|
||||
"""Example-layer contact arrow driven by centroid and normal force."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
mesh: UVSurfaceMesh,
|
||||
*,
|
||||
normal_force_scale: float = DEFAULT_NORMAL_FORCE_SCALE,
|
||||
enabled: bool = True,
|
||||
):
|
||||
import open3d as o3d
|
||||
|
||||
self.mesh = mesh
|
||||
self.normal_force_scale = float(normal_force_scale)
|
||||
self.enabled = enabled
|
||||
self._mesh_span = _mesh_span(mesh.rest_vertices)
|
||||
self._pixel_to_world = mesh_uv_to_world_scale(mesh.world_xyz, mesh.valid_mask)
|
||||
_, _, self._surface_normals = compute_uv_surface_frames(mesh.world_xyz, mesh.valid_mask)
|
||||
self._template_vertices, self._template_triangles = (
|
||||
create_arrow_template(o3d) if enabled else (None, None)
|
||||
)
|
||||
self.arrow_mesh = (
|
||||
create_arrow_mesh_geometry(o3d, self._template_vertices, self._template_triangles)
|
||||
if enabled
|
||||
else None
|
||||
)
|
||||
|
||||
def compute(
|
||||
self,
|
||||
centroid,
|
||||
centroid_found: bool,
|
||||
fnormal: float,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray] | None:
|
||||
if not self.enabled or self.arrow_mesh is None:
|
||||
return None
|
||||
|
||||
template_vertices = self._template_vertices
|
||||
template_triangles = self._template_triangles
|
||||
color = np.tile(
|
||||
np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64),
|
||||
(len(template_vertices), 1),
|
||||
)
|
||||
hidden = (np.zeros_like(template_vertices), template_triangles, color)
|
||||
|
||||
if not centroid_found or centroid is None:
|
||||
return hidden
|
||||
|
||||
centroid = np.asarray(centroid, dtype=np.float64).reshape(-1)
|
||||
if centroid.size < 2:
|
||||
return hidden
|
||||
|
||||
cx, cy = int(round(centroid[0])), int(round(centroid[1]))
|
||||
h, w = self.mesh.shape
|
||||
if not (0 <= cy < h and 0 <= cx < w):
|
||||
return hidden
|
||||
if self.mesh.index_map[cy, cx] < 0:
|
||||
return hidden
|
||||
|
||||
origin = self.mesh.world_xyz[cy, cx]
|
||||
normal = self._surface_normals[cy, cx]
|
||||
if not np.all(np.isfinite(origin)) or not np.all(np.isfinite(normal)):
|
||||
return hidden
|
||||
|
||||
force = max(float(fnormal), 0.0)
|
||||
if force <= 0.0:
|
||||
return hidden
|
||||
|
||||
length = force * self.normal_force_scale * self._pixel_to_world
|
||||
return build_contact_normal_arrow_mesh(
|
||||
origin,
|
||||
normal,
|
||||
length,
|
||||
force,
|
||||
mesh_span=self._mesh_span,
|
||||
template_vertices=template_vertices,
|
||||
template_triangles=template_triangles,
|
||||
)
|
||||
|
||||
def apply(self, arrow_vertices: np.ndarray, arrow_colors: np.ndarray) -> None:
|
||||
import open3d as o3d
|
||||
|
||||
self.arrow_mesh.vertices = o3d.utility.Vector3dVector(arrow_vertices.astype(np.float64))
|
||||
self.arrow_mesh.vertex_colors = o3d.utility.Vector3dVector(arrow_colors.astype(np.float64))
|
||||
self.arrow_mesh.compute_vertex_normals()
|
||||
@@ -0,0 +1,66 @@
|
||||
"""Finger 3D mesh viewer with example-only contact arrow overlay."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from orisys.finger.viewer import UVMeshViewer
|
||||
|
||||
from .contact_arrow import ContactArrowOverlay, DEFAULT_NORMAL_FORCE_SCALE
|
||||
|
||||
|
||||
class FingerMeshViewer(UVMeshViewer):
|
||||
"""UV mesh viewer with a force-scaled contact normal arrow."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
mesh,
|
||||
*,
|
||||
max_magnitude=None,
|
||||
threshold: float = 0.0,
|
||||
color_mode: str = "flow",
|
||||
value_min=None,
|
||||
value_max=None,
|
||||
window_width: int = 1024,
|
||||
window_height: int = 1024,
|
||||
normal_force_scale: float = DEFAULT_NORMAL_FORCE_SCALE,
|
||||
show_contact_arrow: bool = True,
|
||||
):
|
||||
super().__init__(
|
||||
mesh,
|
||||
max_magnitude=max_magnitude,
|
||||
threshold=threshold,
|
||||
color_mode=color_mode,
|
||||
value_min=value_min,
|
||||
value_max=value_max,
|
||||
window_width=window_width,
|
||||
window_height=window_height,
|
||||
)
|
||||
self._contact_arrow = ContactArrowOverlay(
|
||||
mesh,
|
||||
normal_force_scale=normal_force_scale,
|
||||
enabled=show_contact_arrow,
|
||||
)
|
||||
|
||||
def _extra_geometries_for_init(self):
|
||||
if self._contact_arrow.arrow_mesh is None:
|
||||
return []
|
||||
return [self._contact_arrow.arrow_mesh]
|
||||
|
||||
def _extra_geometries_for_update(self):
|
||||
if self._contact_arrow.arrow_mesh is None:
|
||||
return []
|
||||
return [self._contact_arrow.arrow_mesh]
|
||||
|
||||
def update(
|
||||
self,
|
||||
flow=None,
|
||||
scalar_map=None,
|
||||
*,
|
||||
centroid=None,
|
||||
centroid_found: bool = False,
|
||||
fnormal: float = 0.0,
|
||||
):
|
||||
arrow = self._contact_arrow.compute(centroid, centroid_found, fnormal)
|
||||
if arrow is not None and self._contact_arrow.arrow_mesh is not None:
|
||||
arrow_vertices, _, arrow_colors = arrow
|
||||
self._contact_arrow.apply(arrow_vertices, arrow_colors)
|
||||
return super().update(flow=flow, scalar_map=scalar_map)
|
||||
@@ -1,29 +0,0 @@
|
||||
"""Finger-specific OpenCV panels for example scripts."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import cv2
|
||||
import orisys
|
||||
|
||||
from .opencv_maps import depth_colormap
|
||||
|
||||
|
||||
def show_finger_uv_panels(
|
||||
unrolled,
|
||||
vnormal,
|
||||
depth_map,
|
||||
*,
|
||||
flow_threshold: float = 0.4,
|
||||
grid_spacing: int = 20,
|
||||
arrow_scale: float = 1.0,
|
||||
) -> None:
|
||||
"""Show depth colormap and UV unrolled flow arrows."""
|
||||
cv2.imshow("Depth", depth_colormap(depth_map))
|
||||
arrows = orisys.util.draw_arrows(
|
||||
unrolled,
|
||||
vnormal,
|
||||
threshold=flow_threshold,
|
||||
grid_spacing=grid_spacing,
|
||||
arrow_scale=arrow_scale,
|
||||
)
|
||||
cv2.imshow("UV Unrolled", arrows)
|
||||
@@ -27,8 +27,8 @@ class ForceChartWindow:
|
||||
app_name: str = "Orisys Plot Example",
|
||||
) -> None:
|
||||
pg = _require_pyqtgraph()
|
||||
pg.setConfigOption("background", "w")
|
||||
pg.setConfigOption("foreground", "k")
|
||||
pg.setConfigOption("background", "k")
|
||||
pg.setConfigOption("foreground", "w")
|
||||
|
||||
self._pg = pg
|
||||
self.npoints = npoints
|
||||
@@ -36,6 +36,7 @@ class ForceChartWindow:
|
||||
self.app = pg.mkQApp(app_name)
|
||||
self.win = pg.GraphicsLayoutWidget(show=True, title=title)
|
||||
self.win.setWindowTitle(title)
|
||||
self.pen = pg.mkPen(color="w")
|
||||
|
||||
self.data = np.zeros((npoints, 2, 3), dtype=np.float64)
|
||||
for channel in range(3):
|
||||
@@ -51,7 +52,7 @@ class ForceChartWindow:
|
||||
p3.setLabel("bottom", "时间", units="s")
|
||||
|
||||
self._plots = (p1, p2, p3)
|
||||
self._curves = (p1.plot(pen="k"), p2.plot(pen="k"), p3.plot(pen="k"))
|
||||
self._curves = (p1.plot(pen=self.pen ), p2.plot(pen=self.pen ), p3.plot(pen=self.pen ))
|
||||
|
||||
def push(self, timestamp: float, fn: float, fx: float, fy: float) -> None:
|
||||
for channel in range(3):
|
||||
|
||||
@@ -0,0 +1,110 @@
|
||||
"""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()
|
||||
@@ -1,4 +1,4 @@
|
||||
"""Re-exports for finger 3D mesh examples; implementation lives in orisys.finger."""
|
||||
"""Re-exports for finger 3D mesh examples; core viewer lives in orisys.finger."""
|
||||
|
||||
from orisys.finger.mesh import * # noqa: F403
|
||||
from orisys.finger.viewer import * # noqa: F403
|
||||
from orisys.finger.viewer import UVMeshViewer, init_uv_mesh_viewer, update_uv_mesh_viewer # noqa: F401
|
||||
|
||||
@@ -0,0 +1,357 @@
|
||||
"""Embedded Open3D mesh renderer for Qt dashboards (offscreen or hidden-window capture)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
|
||||
import numpy as np
|
||||
|
||||
from orisys.finger.mesh import UVSurfaceMesh
|
||||
from orisys.finger.viewer import flow_magnitude_vertex_colors, scalar_map_vertex_colors
|
||||
|
||||
from .contact_arrow import ContactArrowOverlay
|
||||
from .mesh_camera import apply_view_from_world_to_cam, camera_look_at_from_world_to_cam
|
||||
|
||||
|
||||
def _numpy_o3d_image_to_rgb(image) -> np.ndarray:
|
||||
arr = np.asarray(image)
|
||||
if arr.ndim == 3 and arr.shape[2] >= 3:
|
||||
if arr.dtype == np.float32 or arr.dtype == np.float64:
|
||||
return (np.clip(arr[:, :, :3], 0.0, 1.0) * 255.0).astype(np.uint8)
|
||||
return arr[:, :, :3].astype(np.uint8)
|
||||
return arr.astype(np.uint8)
|
||||
|
||||
|
||||
def _make_unlit_material(o3d):
|
||||
material = o3d.visualization.rendering.MaterialRecord()
|
||||
material.shader = "defaultUnlit"
|
||||
material.base_color = (1.0, 1.0, 1.0, 1.0)
|
||||
return material
|
||||
|
||||
|
||||
def _mesh_to_o3d(o3d, vertices: np.ndarray, triangles: np.ndarray, colors: np.ndarray):
|
||||
mesh = o3d.geometry.TriangleMesh()
|
||||
mesh.vertices = o3d.utility.Vector3dVector(vertices.astype(np.float64))
|
||||
mesh.triangles = o3d.utility.Vector3iVector(triangles.astype(np.int32))
|
||||
mesh.vertex_colors = o3d.utility.Vector3dVector(colors.astype(np.float64))
|
||||
mesh.compute_vertex_normals()
|
||||
return mesh
|
||||
|
||||
|
||||
def _vertex_colors(mesh, *, color_mode, threshold, value_min, value_max, flow=None, scalar_map=None):
|
||||
if color_mode == "depth":
|
||||
return scalar_map_vertex_colors(
|
||||
scalar_map,
|
||||
mesh,
|
||||
value_min=value_min,
|
||||
value_max=value_max,
|
||||
threshold=threshold,
|
||||
)
|
||||
return flow_magnitude_vertex_colors(
|
||||
flow,
|
||||
mesh,
|
||||
threshold=threshold,
|
||||
)
|
||||
|
||||
|
||||
class _FilamentOffscreenBackend:
|
||||
"""Filament OffscreenRenderer (Linux headless / platforms with EGL)."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
mesh: UVSurfaceMesh,
|
||||
*,
|
||||
width: int,
|
||||
height: int,
|
||||
threshold: float,
|
||||
color_mode: str,
|
||||
value_min,
|
||||
value_max,
|
||||
world_to_cam: np.ndarray | None,
|
||||
lookat: np.ndarray,
|
||||
show_contact_arrow: bool,
|
||||
):
|
||||
import open3d as o3d
|
||||
|
||||
self._o3d = o3d
|
||||
self.mesh = mesh
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.threshold = threshold
|
||||
self.color_mode = color_mode
|
||||
self.value_min = value_min
|
||||
self.value_max = value_max
|
||||
self.world_to_cam = world_to_cam
|
||||
self.lookat = lookat
|
||||
self._contact_arrow = ContactArrowOverlay(mesh, enabled=show_contact_arrow)
|
||||
self._renderer = o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
|
||||
self._renderer.scene.set_background([0.05, 0.05, 0.08, 1.0])
|
||||
self._material = _make_unlit_material(o3d)
|
||||
self._mesh_added = False
|
||||
self._arrow_added = False
|
||||
self._apply_camera()
|
||||
|
||||
def _apply_camera(self) -> None:
|
||||
if self.world_to_cam is not None:
|
||||
center, eye, up = camera_look_at_from_world_to_cam(self.world_to_cam, lookat=self.lookat)
|
||||
else:
|
||||
center = self.lookat.astype(np.float64)
|
||||
extent = np.ptp(self.mesh.rest_vertices, axis=0)
|
||||
span = float(np.max(extent))
|
||||
eye = center + np.array([0.0, 0.0, span * 2.0], dtype=np.float64)
|
||||
up = np.array([0.0, 1.0, 0.0], dtype=np.float64)
|
||||
self._renderer.scene.camera.look_at(center, eye, up)
|
||||
|
||||
def resize(self, width: int, height: int) -> None:
|
||||
width = max(int(width), 64)
|
||||
height = max(int(height), 64)
|
||||
if width == self.width and height == self.height:
|
||||
return
|
||||
self.width = width
|
||||
self.height = height
|
||||
self._renderer = self._o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
|
||||
self._renderer.scene.set_background([0.05, 0.05, 0.08, 1.0])
|
||||
self._mesh_added = False
|
||||
self._arrow_added = False
|
||||
self._apply_camera()
|
||||
|
||||
def render(self, *, flow=None, scalar_map=None, centroid=None, centroid_found=False, fnormal=0.0):
|
||||
o3d = self._o3d
|
||||
colors = _vertex_colors(
|
||||
self.mesh,
|
||||
color_mode=self.color_mode,
|
||||
threshold=self.threshold,
|
||||
value_min=self.value_min,
|
||||
value_max=self.value_max,
|
||||
flow=flow,
|
||||
scalar_map=scalar_map,
|
||||
)
|
||||
body_mesh = _mesh_to_o3d(o3d, self.mesh.rest_vertices, self.mesh.triangles, colors)
|
||||
|
||||
if not self._mesh_added:
|
||||
self._renderer.scene.add_geometry("finger_mesh", body_mesh, self._material)
|
||||
self._mesh_added = True
|
||||
else:
|
||||
self._renderer.scene.remove_geometry("finger_mesh")
|
||||
self._renderer.scene.add_geometry("finger_mesh", body_mesh, self._material)
|
||||
|
||||
arrow_result = self._contact_arrow.compute(centroid, centroid_found, fnormal)
|
||||
if arrow_result is not None and self._contact_arrow.arrow_mesh is not None:
|
||||
arrow_vertices, arrow_triangles, arrow_colors = arrow_result
|
||||
if np.any(arrow_vertices):
|
||||
arrow_mesh = _mesh_to_o3d(o3d, arrow_vertices, arrow_triangles, arrow_colors)
|
||||
if not self._arrow_added:
|
||||
self._renderer.scene.add_geometry("contact_arrow", arrow_mesh, self._material)
|
||||
self._arrow_added = True
|
||||
else:
|
||||
self._renderer.scene.remove_geometry("contact_arrow")
|
||||
self._renderer.scene.add_geometry("contact_arrow", arrow_mesh, self._material)
|
||||
elif self._arrow_added:
|
||||
self._renderer.scene.remove_geometry("contact_arrow")
|
||||
self._arrow_added = False
|
||||
|
||||
return _numpy_o3d_image_to_rgb(self._renderer.render_to_image())
|
||||
|
||||
def close(self) -> None:
|
||||
self._renderer = None
|
||||
|
||||
|
||||
class _HiddenVisualizerBackend:
|
||||
"""Invisible Open3D Visualizer + framebuffer capture (Windows-friendly)."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
mesh: UVSurfaceMesh,
|
||||
*,
|
||||
width: int,
|
||||
height: int,
|
||||
threshold: float,
|
||||
color_mode: str,
|
||||
value_min,
|
||||
value_max,
|
||||
world_to_cam: np.ndarray | None,
|
||||
lookat: np.ndarray,
|
||||
show_contact_arrow: bool,
|
||||
):
|
||||
import open3d as o3d
|
||||
|
||||
self._o3d = o3d
|
||||
self.mesh = mesh
|
||||
self.width = width
|
||||
self.height = height
|
||||
self.threshold = threshold
|
||||
self.color_mode = color_mode
|
||||
self.value_min = value_min
|
||||
self.value_max = value_max
|
||||
self.world_to_cam = world_to_cam
|
||||
self.lookat = lookat
|
||||
self._contact_arrow = ContactArrowOverlay(mesh, enabled=show_contact_arrow)
|
||||
self.vis = None
|
||||
self.tri_mesh = None
|
||||
self._create_window()
|
||||
|
||||
def _create_window(self) -> None:
|
||||
o3d = self._o3d
|
||||
if self.vis is not None:
|
||||
self.vis.destroy_window()
|
||||
|
||||
colors = _vertex_colors(
|
||||
self.mesh,
|
||||
color_mode=self.color_mode,
|
||||
threshold=self.threshold,
|
||||
value_min=self.value_min,
|
||||
value_max=self.value_max,
|
||||
)
|
||||
self.vis = o3d.visualization.Visualizer()
|
||||
self.vis.create_window(
|
||||
window_name="Orisys Finger Mesh (embedded)",
|
||||
width=self.width,
|
||||
height=self.height,
|
||||
visible=False,
|
||||
)
|
||||
|
||||
self.tri_mesh = o3d.geometry.TriangleMesh()
|
||||
self.tri_mesh.vertices = o3d.utility.Vector3dVector(self.mesh.rest_vertices.astype(np.float64))
|
||||
self.tri_mesh.triangles = o3d.utility.Vector3iVector(self.mesh.triangles.astype(np.int32))
|
||||
self.tri_mesh.vertex_colors = o3d.utility.Vector3dVector(colors.astype(np.float64))
|
||||
self.tri_mesh.compute_vertex_normals()
|
||||
self.vis.add_geometry(self.tri_mesh)
|
||||
|
||||
if self._contact_arrow.arrow_mesh is not None:
|
||||
self.vis.add_geometry(self._contact_arrow.arrow_mesh)
|
||||
|
||||
render_option = self.vis.get_render_option()
|
||||
render_option.mesh_show_back_face = True
|
||||
if hasattr(render_option, "light_on"):
|
||||
render_option.light_on = False
|
||||
render_option.background_color = np.array([0.05, 0.05, 0.08])
|
||||
|
||||
if self.world_to_cam is not None:
|
||||
apply_view_from_world_to_cam(self.vis, self.world_to_cam, lookat=self.lookat)
|
||||
else:
|
||||
view_control = self.vis.get_view_control()
|
||||
center = self.lookat.astype(np.float64)
|
||||
extent = np.ptp(self.mesh.rest_vertices, axis=0)
|
||||
span = float(np.max(extent))
|
||||
view_control.set_lookat(center)
|
||||
view_control.set_front(np.array([0.0, 0.0, -1.0]))
|
||||
view_control.set_up(np.array([0.0, 1.0, 0.0]))
|
||||
view_control.set_zoom(0.5 if span > 0 else 0.7)
|
||||
|
||||
self.vis.poll_events()
|
||||
self.vis.update_renderer()
|
||||
|
||||
def resize(self, width: int, height: int) -> None:
|
||||
width = max(int(width), 64)
|
||||
height = max(int(height), 64)
|
||||
if width == self.width and height == self.height:
|
||||
return
|
||||
self.width = width
|
||||
self.height = height
|
||||
self._create_window()
|
||||
|
||||
def render(self, *, flow=None, scalar_map=None, centroid=None, centroid_found=False, fnormal=0.0):
|
||||
o3d = self._o3d
|
||||
colors = _vertex_colors(
|
||||
self.mesh,
|
||||
color_mode=self.color_mode,
|
||||
threshold=self.threshold,
|
||||
value_min=self.value_min,
|
||||
value_max=self.value_max,
|
||||
flow=flow,
|
||||
scalar_map=scalar_map,
|
||||
)
|
||||
vertices = self.mesh.rest_vertices
|
||||
|
||||
self.tri_mesh.vertices = o3d.utility.Vector3dVector(vertices.astype(np.float64))
|
||||
self.tri_mesh.vertex_colors = o3d.utility.Vector3dVector(colors.astype(np.float64))
|
||||
self.tri_mesh.compute_vertex_normals()
|
||||
self.vis.update_geometry(self.tri_mesh)
|
||||
|
||||
arrow_result = self._contact_arrow.compute(centroid, centroid_found, fnormal)
|
||||
if arrow_result is not None and self._contact_arrow.arrow_mesh is not None:
|
||||
arrow_vertices, _, arrow_colors = arrow_result
|
||||
self._contact_arrow.apply(arrow_vertices, arrow_colors)
|
||||
self.vis.update_geometry(self._contact_arrow.arrow_mesh)
|
||||
|
||||
self.vis.poll_events()
|
||||
self.vis.update_renderer()
|
||||
return _numpy_o3d_image_to_rgb(self.vis.capture_screen_float_buffer(do_render=False))
|
||||
|
||||
def close(self) -> None:
|
||||
if self.vis is not None:
|
||||
self.vis.destroy_window()
|
||||
self.vis = None
|
||||
|
||||
|
||||
def _create_backend(mesh: UVSurfaceMesh, **kwargs):
|
||||
# Filament OffscreenRenderer needs EGL headless (not available on Windows).
|
||||
if sys.platform == "win32":
|
||||
return _HiddenVisualizerBackend(mesh, **kwargs)
|
||||
try:
|
||||
return _FilamentOffscreenBackend(mesh, **kwargs)
|
||||
except RuntimeError:
|
||||
return _HiddenVisualizerBackend(mesh, **kwargs)
|
||||
|
||||
|
||||
class OffscreenFingerMeshRenderer:
|
||||
"""Render finger UV mesh + contact arrow to an RGB image for Qt embedding."""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
mesh: UVSurfaceMesh,
|
||||
*,
|
||||
width: int = 512,
|
||||
height: int = 512,
|
||||
threshold: float = 0.0,
|
||||
color_mode: str = "depth",
|
||||
value_min=None,
|
||||
value_max=None,
|
||||
world_to_cam: np.ndarray | None = None,
|
||||
lookat: np.ndarray | None = None,
|
||||
show_contact_arrow: bool = True,
|
||||
):
|
||||
lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0)
|
||||
self._backend = _create_backend(
|
||||
mesh,
|
||||
width=int(width),
|
||||
height=int(height),
|
||||
threshold=threshold,
|
||||
color_mode=color_mode,
|
||||
value_min=value_min,
|
||||
value_max=value_max,
|
||||
world_to_cam=world_to_cam,
|
||||
lookat=lookat,
|
||||
show_contact_arrow=show_contact_arrow,
|
||||
)
|
||||
|
||||
@property
|
||||
def width(self) -> int:
|
||||
return self._backend.width
|
||||
|
||||
@property
|
||||
def height(self) -> int:
|
||||
return self._backend.height
|
||||
|
||||
def resize(self, width: int, height: int) -> None:
|
||||
self._backend.resize(width, height)
|
||||
|
||||
def render(
|
||||
self,
|
||||
*,
|
||||
flow=None,
|
||||
scalar_map=None,
|
||||
centroid=None,
|
||||
centroid_found: bool = False,
|
||||
fnormal: float = 0.0,
|
||||
) -> np.ndarray:
|
||||
return self._backend.render(
|
||||
flow=flow,
|
||||
scalar_map=scalar_map,
|
||||
centroid=centroid,
|
||||
centroid_found=centroid_found,
|
||||
fnormal=fnormal,
|
||||
)
|
||||
|
||||
def close(self) -> None:
|
||||
self._backend.close()
|
||||
@@ -0,0 +1,35 @@
|
||||
"""Pure numpy→BGR render helpers for finger 2D panels (no window I/O)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import orisys
|
||||
|
||||
from .opencv_maps import depth_colormap
|
||||
|
||||
|
||||
def render_depth_panel(depth_map):
|
||||
"""Return a BGR depth colormap image."""
|
||||
return depth_colormap(depth_map)
|
||||
|
||||
|
||||
def render_uv_arrows(
|
||||
unrolled,
|
||||
vnormal,
|
||||
*,
|
||||
flow_threshold: float = 0.4,
|
||||
grid_spacing: int = 20,
|
||||
arrow_scale: float = 1.0,
|
||||
):
|
||||
"""Return a BGR image with UV unrolled flow arrows."""
|
||||
return orisys.util.draw_arrows(
|
||||
unrolled,
|
||||
vnormal,
|
||||
threshold=flow_threshold,
|
||||
grid_spacing=grid_spacing,
|
||||
arrow_scale=arrow_scale,
|
||||
)
|
||||
|
||||
|
||||
def render_contact_panel(img, contour, centroid):
|
||||
"""Return a BGR contact overlay on the raw sensor image."""
|
||||
return orisys.util.draw_contact(img, contour, centroid)
|
||||
@@ -20,8 +20,13 @@ def lowpass_filter(data, cutoff_hz, sample_rate_hz=30, order=2) -> np.ndarray:
|
||||
)
|
||||
|
||||
b, a = butter(order, cutoff_hz / nyquist, btype="low")
|
||||
padlen = 3 * (max(len(b), len(a)) - 1)
|
||||
taplen = max(len(a), len(b))
|
||||
# scipy.signal.filtfilt requires len(x) > padlen (default 3 * taplen for recent scipy).
|
||||
padlen = 3 * taplen
|
||||
if y.size <= padlen:
|
||||
return y.copy()
|
||||
|
||||
return filtfilt(b, a, y)
|
||||
try:
|
||||
return filtfilt(b, a, y)
|
||||
except ValueError:
|
||||
return y.copy()
|
||||
|
||||
Reference in New Issue
Block a user