added mesh surface arrows

This commit is contained in:
ykcheungab
2026-06-19 21:22:28 +08:00
parent 2e5c047d1e
commit 258948bcb7
20 changed files with 951 additions and 194 deletions
+17 -4
View File
@@ -9,15 +9,29 @@ from .mesh_camera import (
resolve_viz_pose_path,
)
from .offscreen_mesh import OffscreenFingerMeshRenderer
from .render_2d import render_contact_panel, render_depth_panel, render_uv_arrows
from .render_2d import (
ArrowBackground,
ArrowCanvasColors,
DEFAULT_ARROW_CANVAS_COLORS,
depth_colormap,
frame_to_bgr,
render_contact_panel,
render_depth_panel,
render_uv_arrows,
)
from .flow_arrow import DEFAULT_FLOW_ARROW_STYLE, FlowArrowOverlay, FlowArrowStyle
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",
"ArrowBackground",
"ArrowCanvasColors",
"DEFAULT_ARROW_CANVAS_COLORS",
"DEFAULT_FLOW_ARROW_STYLE",
"FlowArrowOverlay",
"FlowArrowStyle",
"FingerMeshViewer",
"ForceChartWindow",
"FrameRecorder",
@@ -27,7 +41,6 @@ __all__ = [
"build_uv_surface_mesh",
"camera_look_at_from_world_to_cam",
"depth_colormap",
"draw_magnitude_map",
"frame_to_bgr",
"lowpass_filter",
"OffscreenFingerMeshRenderer",
+30 -8
View File
@@ -1,14 +1,19 @@
"""Finger 3D mesh viewer with example-only contact arrow overlay."""
"""Finger 3D mesh viewer with example-only contact and flow arrow overlays."""
from __future__ import annotations
from dataclasses import replace
import numpy as np
from orisys.finger.viewer import UVMeshViewer
from .contact_arrow import ContactArrowOverlay, ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE
from .flow_arrow import DEFAULT_FLOW_ARROW_STYLE, FlowArrowOverlay, FlowArrowStyle
class FingerMeshViewer(UVMeshViewer):
"""UV mesh viewer with a force-scaled contact normal arrow."""
"""UV mesh viewer with force-scaled contact arrow and flow tube arrows."""
def __init__(
self,
@@ -24,6 +29,8 @@ class FingerMeshViewer(UVMeshViewer):
contact_arrow_style: ContactArrowStyle | None = None,
normal_force_scale: float | None = None,
show_contact_arrow: bool = True,
flow_arrow_style: FlowArrowStyle | None = None,
show_flow_arrows: bool = True,
):
super().__init__(
mesh,
@@ -41,16 +48,24 @@ class FingerMeshViewer(UVMeshViewer):
normal_force_scale=normal_force_scale,
enabled=show_contact_arrow,
)
style = flow_arrow_style or DEFAULT_FLOW_ARROW_STYLE
if not show_flow_arrows:
style = replace(style, enabled=False)
self._flow_arrow = FlowArrowOverlay(mesh, style=style)
def _extra_geometries(self):
geoms = []
if self._contact_arrow.arrow_mesh is not None:
geoms.append(self._contact_arrow.arrow_mesh)
if self._flow_arrow.enabled and self._flow_arrow.arrow_mesh is not None:
geoms.append(self._flow_arrow.arrow_mesh)
return geoms
def _extra_geometries_for_init(self):
if self._contact_arrow.arrow_mesh is None:
return []
return [self._contact_arrow.arrow_mesh]
return self._extra_geometries()
def _extra_geometries_for_update(self):
if self._contact_arrow.arrow_mesh is None:
return []
return [self._contact_arrow.arrow_mesh]
return self._extra_geometries()
def update(
self,
@@ -60,9 +75,16 @@ class FingerMeshViewer(UVMeshViewer):
centroid=None,
centroid_found: bool = False,
fnormal: float = 0.0,
valid_mask: np.ndarray | None = None,
):
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)
flow_result = self._flow_arrow.compute(flow, valid_mask=valid_mask)
if flow_result is not None and self._flow_arrow.arrow_mesh is not None:
flow_vertices, flow_triangles, flow_colors = flow_result
self._flow_arrow.apply(flow_vertices, flow_triangles, flow_colors)
return super().update(flow=flow, scalar_map=scalar_map)
+431
View File
@@ -0,0 +1,431 @@
"""Optical-flow arrow tubes on the finger UV surface mesh (TriangleMesh cylinders)."""
from __future__ import annotations
from dataclasses import dataclass
from typing import Literal
import cv2
import numpy as np
from orisys.finger.mesh import DEFAULT_SURFACE_COLOR, UVSurfaceMesh
from .contact_arrow import _rotation_matrix_z_to
# Grey mesh tone used to approximate opacity on backends without true alpha.
_MESH_GREY_RGB = np.asarray(DEFAULT_SURFACE_COLOR, dtype=np.float64)
# Maps style.line_width to tube radius as a fraction of mesh extent.
LINE_WIDTH_TO_SPAN_FRACTION = 0.0003
# Slight tail extension into the head segment to avoid visible gaps.
_TAIL_HEAD_OVERLAP_FRACTION = 0.03
SegmentRole = Literal["tail", "head"]
_CYLINDER_TEMPLATE_VERTICES: np.ndarray | None = None
_CYLINDER_TEMPLATE_TRIANGLES: np.ndarray | None = None
_CONE_TEMPLATE_VERTICES: np.ndarray | None = None
_CONE_TEMPLATE_TRIANGLES: np.ndarray | None = None
_CONE_APEX_AT_LOW_Z: bool | None = None
@dataclass(frozen=True)
class FlowArrowStyle:
"""Visual tuning for 3D mesh flow arrows (dashboard: edit MESH_FLOW_ARROW in layout.py).
``line_width`` / ``head_line_width`` scale tail and head tube radius relative to mesh extent.
``tail_color`` / ``head_color`` are RGB in 0–255 (same convention as OpenCV BGR tuples, but RGB order).
"""
enabled: bool = True
threshold: float = 1.0
grid_spacing: int = 20
arrow_scale: float = 0.5
length_margin: float = 1.0
tail_color: tuple[int, int, int] = (38, 235, 89)
head_color: tuple[int, int, int] = (13, 115, 31)
head_fraction: float = 0.25
opacity: float = 0.65
line_width: float = 2.0
head_line_width: float = 3.0
edge_margin: int = 8
DEFAULT_FLOW_ARROW_STYLE = FlowArrowStyle()
def _mesh_span(vertices: np.ndarray) -> float:
extent = np.ptp(vertices, axis=0)
return float(max(np.max(extent), 1e-6))
def segment_radius(mesh_span: float, line_width: float) -> float:
"""World-space tube radius from mesh extent and a width knob."""
return mesh_span * max(float(line_width), 0.1) * LINE_WIDTH_TO_SPAN_FRACTION
def segment_radius_for_role(mesh_span: float, style: FlowArrowStyle, role: SegmentRole) -> float:
width = style.line_width if role == "tail" else style.head_line_width
return segment_radius(mesh_span, width)
def _prepare_uv_mask(valid_mask, shape: tuple[int, int]) -> np.ndarray | None:
if valid_mask is None:
return None
h, w = shape
mask = np.asarray(valid_mask, dtype=bool)
if mask.shape != (h, w):
mask_u8 = mask.astype(np.uint8)
if mask_u8.ndim > 2:
mask_u8 = mask_u8[:, :, 0]
mask = cv2.resize(mask_u8, (w, h), interpolation=cv2.INTER_NEAREST).astype(bool)
return mask
def _style_rgb01(style: FlowArrowStyle, role: SegmentRole) -> np.ndarray:
rgb = np.asarray(style.tail_color if role == "tail" else style.head_color, dtype=np.float64).reshape(3)
return np.clip(rgb / 255.0, 0.0, 1.0)
def effective_segment_color(
style: FlowArrowStyle,
role: SegmentRole,
*,
use_alpha_blend: bool = True,
) -> np.ndarray:
"""Return RGB in 0–1 for a tail or head segment."""
rgb = _style_rgb01(style, role)
if not use_alpha_blend:
return rgb
opacity = float(np.clip(style.opacity, 0.0, 1.0))
return np.clip(opacity * rgb + (1.0 - opacity) * _MESH_GREY_RGB, 0.0, 1.0)
def _sample_flow_segments(
world_xyz: np.ndarray,
flow: np.ndarray,
*,
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
valid_mask: np.ndarray | None = None,
) -> tuple[np.ndarray, np.ndarray, list[SegmentRole]]:
"""Return segment start/end world points and role per segment (tail then head)."""
empty = np.empty((0, 3), dtype=np.float64)
if flow is None or np.asarray(flow).size == 0:
return empty, empty, []
flow = np.asarray(flow, dtype=np.float64)
world_xyz = np.asarray(world_xyz, dtype=np.float64)
h, w = flow.shape[:2]
if world_xyz.shape[:2] != (h, w):
return empty, empty, []
grid_spacing = max(int(style.grid_spacing), 1)
edge_margin = int(style.edge_margin)
margin = float(style.length_margin)
arrow_scale = max(float(style.arrow_scale), 1e-9)
head_fraction = float(np.clip(style.head_fraction, 0.05, 0.95))
tail_fraction = 1.0 - head_fraction
rr = np.arange(grid_spacing // 2, h, grid_spacing)
cc = np.arange(grid_spacing // 2, w, grid_spacing)
rr, cc = np.meshgrid(rr, cc, indexing="ij")
rr, cc = rr.reshape(-1), cc.reshape(-1)
disp = 2.0 * flow[rr, cc, :]
mag = np.linalg.norm(disp, axis=1)
dx = disp[:, 0] / arrow_scale
dy = disp[:, 1] / arrow_scale
length = np.sqrt(dx * dx + dy * dy)
scale = np.where(
length > grid_spacing * margin,
grid_spacing / np.maximum(length, 1e-9),
1.0,
) * margin
dx, dy = dx * scale, dy * scale
end_x = np.clip(np.round(cc + dx).astype(np.int32), 0, w - 1)
end_y = np.clip(np.round(rr + dy).astype(np.int32), 0, h - 1)
valid = (
(cc >= edge_margin)
& (cc < w - edge_margin)
& (rr >= edge_margin)
& (rr < h - edge_margin)
& (mag >= float(style.threshold))
)
mask = _prepare_uv_mask(valid_mask, (h, w))
if mask is not None:
valid &= mask[rr, cc]
valid &= mask[end_y, end_x]
if not np.any(valid):
return empty, empty, []
starts = world_xyz[rr[valid], cc[valid]]
ends = world_xyz[end_y[valid], end_x[valid]]
finite = np.all(np.isfinite(starts), axis=1) & np.all(np.isfinite(ends), axis=1)
if not np.any(finite):
return empty, empty, []
starts = starts[finite]
ends = ends[finite]
non_zero = np.linalg.norm(ends - starts, axis=1) > 1e-9
if not np.any(non_zero):
return empty, empty, []
starts = starts[non_zero]
ends = ends[non_zero]
junctions = starts + tail_fraction * (ends - starts)
seg_starts = np.vstack((starts, junctions))
seg_ends = np.vstack((junctions, ends))
n = len(starts)
roles: list[SegmentRole] = ["tail"] * n + ["head"] * n
return seg_starts, seg_ends, roles
def build_flow_arrow_lines(
world_xyz: np.ndarray,
flow: np.ndarray,
*,
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
valid_mask: np.ndarray | None = None,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Build line indices for flow arrows (used by tests and debugging)."""
empty_pts = np.empty((0, 3), dtype=np.float64)
empty_lines = np.empty((0, 2), dtype=np.int32)
empty_colors = np.empty((0, 3), dtype=np.float64)
seg_starts, seg_ends, roles = _sample_flow_segments(
world_xyz, flow, style=style, valid_mask=valid_mask
)
if len(seg_starts) == 0:
return empty_pts, empty_lines, empty_colors
points = np.empty((len(seg_starts) * 2, 3), dtype=np.float64)
points[0::2] = seg_starts
points[1::2] = seg_ends
lines = np.column_stack(
(np.arange(0, len(points), 2, dtype=np.int32), np.arange(1, len(points), 2, dtype=np.int32))
)
line_colors = np.vstack(
[effective_segment_color(style, role, use_alpha_blend=True) for role in roles]
)
return points, lines, line_colors
def _cylinder_template(o3d):
global _CYLINDER_TEMPLATE_VERTICES, _CYLINDER_TEMPLATE_TRIANGLES
if _CYLINDER_TEMPLATE_VERTICES is None:
cyl = o3d.geometry.TriangleMesh.create_cylinder(
radius=1.0,
height=1.0,
resolution=8,
split=1,
)
_CYLINDER_TEMPLATE_VERTICES = np.asarray(cyl.vertices, dtype=np.float64)
_CYLINDER_TEMPLATE_TRIANGLES = np.asarray(cyl.triangles, dtype=np.int32)
return _CYLINDER_TEMPLATE_VERTICES, _CYLINDER_TEMPLATE_TRIANGLES
def _cone_apex_at_low_z(vertices: np.ndarray) -> bool:
"""True when the cone tip is on the low-z side of the Open3D template."""
radii = np.linalg.norm(vertices[:, :2], axis=1)
apex_z = float(vertices[np.argmin(radii), 2])
z_mid = 0.5 * (float(vertices[:, 2].min()) + float(vertices[:, 2].max()))
return apex_z < z_mid
def _cone_template(o3d):
global _CONE_TEMPLATE_VERTICES, _CONE_TEMPLATE_TRIANGLES, _CONE_APEX_AT_LOW_Z
if _CONE_TEMPLATE_VERTICES is None:
cone = o3d.geometry.TriangleMesh.create_cone(
radius=1.0,
height=1.0,
resolution=8,
split=1,
)
_CONE_TEMPLATE_VERTICES = np.asarray(cone.vertices, dtype=np.float64)
_CONE_TEMPLATE_TRIANGLES = np.asarray(cone.triangles, dtype=np.int32)
_CONE_APEX_AT_LOW_Z = _cone_apex_at_low_z(_CONE_TEMPLATE_VERTICES)
return _CONE_TEMPLATE_VERTICES, _CONE_TEMPLATE_TRIANGLES
def _transform_axis_segment(
template_vertices: np.ndarray,
start: np.ndarray,
end: np.ndarray,
radius: float,
*,
apex_at_end: bool = False,
) -> np.ndarray:
"""Place a +Z template from ``start`` to ``end`` (axis anchored, not midpoint-centered).
Open3D cylinders are centered on the origin; cone apex side is detected from the template.
``apex_at_end=True`` maps the template apex to ``end`` (flow tip); base to ``start``.
"""
direction = np.asarray(end, dtype=np.float64) - np.asarray(start, dtype=np.float64)
length = float(np.linalg.norm(direction))
if length <= 1e-9 or radius <= 0.0:
return np.empty((0, 3), dtype=np.float64)
dir_hat = direction / length
rotation = _rotation_matrix_z_to(dir_hat)
scaled = np.asarray(template_vertices, dtype=np.float64).copy()
z_min = float(scaled[:, 2].min())
z_max = float(scaled[:, 2].max())
z_span = z_max - z_min
if z_span <= 1e-9:
return np.empty((0, 3), dtype=np.float64)
if apex_at_end:
t = (z_max - scaled[:, 2]) / z_span * length
else:
t = (scaled[:, 2] - z_min) / z_span * length
radial = np.column_stack((scaled[:, 0] * radius, scaled[:, 1] * radius, t))
return radial @ rotation.T + np.asarray(start, dtype=np.float64)
def _transform_cylinder_segment(
template_vertices: np.ndarray,
start: np.ndarray,
end: np.ndarray,
radius: float,
) -> np.ndarray:
return _transform_axis_segment(template_vertices, start, end, radius)
def build_flow_arrow_mesh(
world_xyz: np.ndarray,
flow: np.ndarray,
mesh_span: float,
*,
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
valid_mask: np.ndarray | None = None,
o3d=None,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Build merged mesh flow arrows: cylinder tail + cone head per arrow."""
empty_v = np.empty((0, 3), dtype=np.float64)
empty_t = np.empty((0, 3), dtype=np.int32)
empty_c = np.empty((0, 3), dtype=np.float64)
seg_starts, seg_ends, roles = _sample_flow_segments(
world_xyz, flow, style=style, valid_mask=valid_mask
)
if len(seg_starts) == 0:
return empty_v, empty_t, empty_c
if o3d is None:
import open3d as o3d
cyl_vertices, cyl_triangles = _cylinder_template(o3d)
cone_vertices, cone_triangles = _cone_template(o3d)
vertices_parts: list[np.ndarray] = []
triangles_parts: list[np.ndarray] = []
colors_parts: list[np.ndarray] = []
vertex_offset = 0
for start, end, role in zip(seg_starts, seg_ends, roles):
seg_start = np.asarray(start, dtype=np.float64)
seg_end = np.asarray(end, dtype=np.float64)
axis = seg_end - seg_start
seg_len = float(np.linalg.norm(axis))
if seg_len <= 1e-9:
continue
radius = segment_radius_for_role(mesh_span, style, role)
if role == "tail":
template_vertices, template_triangles = cyl_vertices, cyl_triangles
apex_at_end = False
overlap = _TAIL_HEAD_OVERLAP_FRACTION * seg_len
seg_end = seg_end + (axis / seg_len) * overlap
else:
template_vertices, template_triangles = cone_vertices, cone_triangles
apex_at_end = not bool(_CONE_APEX_AT_LOW_Z)
tail_radius = segment_radius_for_role(mesh_span, style, "tail")
radius = max(radius, tail_radius)
verts = _transform_axis_segment(
template_vertices,
seg_start,
seg_end,
radius,
apex_at_end=apex_at_end,
)
if verts.size == 0:
continue
color = effective_segment_color(style, role, use_alpha_blend=True)
vertices_parts.append(verts)
triangles_parts.append(template_triangles + vertex_offset)
colors_parts.append(np.tile(color, (len(verts), 1)))
vertex_offset += len(verts)
if not vertices_parts:
return empty_v, empty_t, empty_c
return (
np.vstack(vertices_parts),
np.vstack(triangles_parts),
np.vstack(colors_parts),
)
def create_flow_mesh_geometry(o3d):
"""Create a persistent Open3D mesh for the flow arrow overlay."""
mesh = o3d.geometry.TriangleMesh()
mesh.vertices = o3d.utility.Vector3dVector(np.empty((0, 3), dtype=np.float64))
mesh.triangles = o3d.utility.Vector3iVector(np.empty((0, 3), dtype=np.int32))
mesh.vertex_colors = o3d.utility.Vector3dVector(np.empty((0, 3), dtype=np.float64))
mesh.compute_vertex_normals()
return mesh
class FlowArrowOverlay:
"""Example-layer flow arrows as cylinder tail + cone head meshes on the UV surface."""
def __init__(
self,
mesh: UVSurfaceMesh,
*,
style: FlowArrowStyle | None = None,
):
import open3d as o3d
self.mesh = mesh
self.style = style or DEFAULT_FLOW_ARROW_STYLE
self._mesh_span = _mesh_span(mesh.rest_vertices)
_cylinder_template(o3d)
_cone_template(o3d)
self.arrow_mesh = create_flow_mesh_geometry(o3d) if self.style.enabled else None
@property
def enabled(self) -> bool:
return self.style.enabled
def compute(
self,
flow,
valid_mask: np.ndarray | None = None,
) -> tuple[np.ndarray, np.ndarray, np.ndarray] | None:
if not self.style.enabled or self.arrow_mesh is None:
return None
return build_flow_arrow_mesh(
self.mesh.world_xyz,
flow,
self._mesh_span,
style=self.style,
valid_mask=valid_mask,
)
def apply(self, arrow_vertices: np.ndarray, arrow_triangles: np.ndarray, arrow_colors: np.ndarray) -> None:
"""Update the persistent flow mesh geometry in place."""
import open3d as o3d
self.arrow_mesh.vertices = o3d.utility.Vector3dVector(arrow_vertices.astype(np.float64))
self.arrow_mesh.triangles = o3d.utility.Vector3iVector(arrow_triangles.astype(np.int32))
self.arrow_mesh.vertex_colors = o3d.utility.Vector3dVector(arrow_colors.astype(np.float64))
self.arrow_mesh.compute_vertex_normals()
+3 -3
View File
@@ -42,13 +42,13 @@ class ForceChartWindow:
for channel in range(3):
self.data[:, 0, channel] = np.linspace(-npoints, 0, npoints) / sample_rate_hz
p1 = self.win.addPlot(title="法向力")
p1 = self.win.addPlot(title="Normal Deformation")
p1.setLabel("bottom", "时间", units="s")
self.win.nextRow()
p2 = self.win.addPlot(title="切向力 X")
p2 = self.win.addPlot(title="X Deformation")
p2.setLabel("bottom", "时间", units="s")
self.win.nextRow()
p3 = self.win.addPlot(title="切向力 Y")
p3 = self.win.addPlot(title="Y Deformation")
p3.setLabel("bottom", "时间", units="s")
self._plots = (p1, p2, p3)
+75 -6
View File
@@ -10,6 +10,7 @@ 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 .flow_arrow import FlowArrowOverlay
from .mesh_camera import (
apply_filament_sun_light,
apply_shade_factors,
@@ -64,6 +65,16 @@ def _vertex_colors(mesh, *, color_mode, threshold, value_min, value_max, flow=No
)
def _background_rgba01(color: tuple[int, int, int]) -> list[float]:
r, g, b = (int(color[0]), int(color[1]), int(color[2]))
return [r / 255.0, g / 255.0, b / 255.0, 1.0]
def _background_rgb01(color: tuple[int, int, int]) -> np.ndarray:
return np.asarray(_background_rgba01(color)[:3], dtype=np.float64)
class _FilamentOffscreenBackend:
"""Filament OffscreenRenderer (Linux headless / platforms with EGL)."""
@@ -81,7 +92,9 @@ class _FilamentOffscreenBackend:
lookat: np.ndarray,
show_contact_arrow: bool,
mesh_lighting=None,
mesh_background: tuple[int, int, int] = (0, 0, 0),
contact_arrow_style=None,
flow_arrow_style=None,
):
import open3d as o3d
@@ -96,6 +109,7 @@ class _FilamentOffscreenBackend:
self.world_to_cam = world_to_cam
self.lookat = lookat
self.mesh_lighting = mesh_lighting
self.mesh_background = mesh_background
self._shade_factors = (
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
if mesh_lighting is not None
@@ -106,8 +120,9 @@ class _FilamentOffscreenBackend:
enabled=show_contact_arrow,
style=contact_arrow_style,
)
self._flow_arrow = FlowArrowOverlay(mesh, style=flow_arrow_style)
self._renderer = o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
self._renderer.scene.set_background([0.05, 0.05, 0.08, 1.0])
self._renderer.scene.set_background(_background_rgba01(self.mesh_background))
if mesh_lighting is not None:
apply_filament_sun_light(
self._renderer,
@@ -117,6 +132,7 @@ class _FilamentOffscreenBackend:
self._material = _make_lit_material(o3d)
self._mesh_added = False
self._arrow_added = False
self._flow_arrow_added = False
self._apply_camera()
def _apply_camera(self) -> None:
@@ -138,7 +154,7 @@ class _FilamentOffscreenBackend:
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._renderer.scene.set_background(_background_rgba01(self.mesh_background))
if self.mesh_lighting is not None:
apply_filament_sun_light(
self._renderer,
@@ -148,9 +164,19 @@ class _FilamentOffscreenBackend:
self._material = _make_lit_material(self._o3d)
self._mesh_added = False
self._arrow_added = False
self._flow_arrow_added = False
self._apply_camera()
def render(self, *, flow=None, scalar_map=None, centroid=None, centroid_found=False, fnormal=0.0):
def render(
self,
*,
flow=None,
scalar_map=None,
centroid=None,
centroid_found=False,
fnormal=0.0,
valid_mask=None,
):
o3d = self._o3d
colors = _vertex_colors(
self.mesh,
@@ -185,6 +211,21 @@ class _FilamentOffscreenBackend:
self._renderer.scene.remove_geometry("contact_arrow")
self._arrow_added = False
flow_result = self._flow_arrow.compute(flow, valid_mask=valid_mask)
if flow_result is not None and self._flow_arrow.arrow_mesh is not None:
flow_vertices, flow_triangles, flow_colors = flow_result
if len(flow_vertices) > 0:
flow_mesh = _mesh_to_o3d(o3d, flow_vertices, flow_triangles, flow_colors)
if not self._flow_arrow_added:
self._renderer.scene.add_geometry("flow_arrows", flow_mesh, self._material)
self._flow_arrow_added = True
else:
self._renderer.scene.remove_geometry("flow_arrows")
self._renderer.scene.add_geometry("flow_arrows", flow_mesh, self._material)
elif self._flow_arrow_added:
self._renderer.scene.remove_geometry("flow_arrows")
self._flow_arrow_added = False
return _numpy_o3d_image_to_rgb(self._renderer.render_to_image())
def close(self) -> None:
@@ -208,7 +249,9 @@ class _HiddenVisualizerBackend:
lookat: np.ndarray,
show_contact_arrow: bool,
mesh_lighting=None,
mesh_background: tuple[int, int, int] = (0, 0, 0),
contact_arrow_style=None,
flow_arrow_style=None,
):
import open3d as o3d
@@ -223,6 +266,7 @@ class _HiddenVisualizerBackend:
self.world_to_cam = world_to_cam
self.lookat = lookat
self.mesh_lighting = mesh_lighting
self.mesh_background = mesh_background
self._shade_factors = (
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
if mesh_lighting is not None
@@ -233,6 +277,7 @@ class _HiddenVisualizerBackend:
enabled=show_contact_arrow,
style=contact_arrow_style,
)
self._flow_arrow = FlowArrowOverlay(mesh, style=flow_arrow_style)
self.vis = None
self.tri_mesh = None
self._create_window()
@@ -268,9 +313,12 @@ class _HiddenVisualizerBackend:
if self._contact_arrow.arrow_mesh is not None:
self.vis.add_geometry(self._contact_arrow.arrow_mesh)
if self._flow_arrow.enabled and self._flow_arrow.arrow_mesh is not None:
self.vis.add_geometry(self._flow_arrow.arrow_mesh)
render_option = self.vis.get_render_option()
render_option.mesh_show_back_face = True
render_option.background_color = np.array([0.05, 0.05, 0.08])
render_option.background_color = _background_rgb01(self.mesh_background)
if hasattr(render_option, "light_on"):
render_option.light_on = False
@@ -298,7 +346,16 @@ class _HiddenVisualizerBackend:
self.height = height
self._create_window()
def render(self, *, flow=None, scalar_map=None, centroid=None, centroid_found=False, fnormal=0.0):
def render(
self,
*,
flow=None,
scalar_map=None,
centroid=None,
centroid_found=False,
fnormal=0.0,
valid_mask=None,
):
o3d = self._o3d
colors = _vertex_colors(
self.mesh,
@@ -320,6 +377,12 @@ class _HiddenVisualizerBackend:
self._contact_arrow.apply(arrow_vertices, arrow_colors)
self.vis.update_geometry(self._contact_arrow.arrow_mesh)
flow_result = self._flow_arrow.compute(flow, valid_mask=valid_mask)
if flow_result is not None and self._flow_arrow.arrow_mesh is not None:
flow_vertices, flow_triangles, flow_colors = flow_result
self._flow_arrow.apply(flow_vertices, flow_triangles, flow_colors)
self.vis.update_geometry(self._flow_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))
@@ -340,7 +403,7 @@ def _create_backend(mesh: UVSurfaceMesh, **kwargs):
return _HiddenVisualizerBackend(mesh, **kwargs)
class OffscreenFingerMeshRenderer:
"""Render finger UV mesh + contact arrow to an RGB image for Qt embedding."""
"""Render finger UV mesh + contact/flow arrows to an RGB image for Qt embedding."""
def __init__(
self,
@@ -356,7 +419,9 @@ class OffscreenFingerMeshRenderer:
lookat: np.ndarray | None = None,
show_contact_arrow: bool = True,
mesh_lighting=None,
mesh_background: tuple[int, int, int] = (0, 0, 0),
contact_arrow_style=None,
flow_arrow_style=None,
):
lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0)
self._backend = _create_backend(
@@ -371,7 +436,9 @@ class OffscreenFingerMeshRenderer:
lookat=lookat,
show_contact_arrow=show_contact_arrow,
mesh_lighting=mesh_lighting,
mesh_background=mesh_background,
contact_arrow_style=contact_arrow_style,
flow_arrow_style=flow_arrow_style,
)
@property
@@ -393,6 +460,7 @@ class OffscreenFingerMeshRenderer:
centroid=None,
centroid_found: bool = False,
fnormal: float = 0.0,
valid_mask=None,
) -> np.ndarray:
return self._backend.render(
flow=flow,
@@ -400,6 +468,7 @@ class OffscreenFingerMeshRenderer:
centroid=centroid,
centroid_found=centroid_found,
fnormal=fnormal,
valid_mask=valid_mask,
)
def close(self) -> None:
-57
View File
@@ -1,57 +0,0 @@
"""OpenCV visualization helpers for example scripts."""
from __future__ import annotations
import cv2
import numpy as np
def depth_colormap(depth_map, colormap=cv2.COLORMAP_JET) -> np.ndarray:
"""Normalize a scalar map to uint8 and apply an OpenCV colormap."""
values = np.asarray(depth_map)
if values.size == 0:
return np.zeros((1, 1, 3), dtype=np.uint8)
if values.max() > values.min():
normalized = ((values - values.min()) / (values.max() - values.min()) * 255).astype(np.uint8)
else:
normalized = np.zeros_like(values, dtype=np.uint8)
return cv2.applyColorMap(normalized, colormap)
def draw_magnitude_map(
vfield,
threshold=3.0,
colormap=cv2.COLORMAP_JET,
output_size=(400, 400),
) -> np.ndarray:
"""Render a smoothed optical-flow magnitude map as a BGR pseudo-color image."""
magnitude = np.sqrt(vfield[:, :, 0] ** 2 + vfield[:, :, 1] ** 2)
downsample_factor = 10
h, w = magnitude.shape
magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor]
magnitude_smoothed = cv2.resize(magnitude_downsampled, (w, h), interpolation=cv2.INTER_CUBIC)
magnitude_thresholded = magnitude_smoothed.copy()
magnitude_thresholded[magnitude_smoothed < threshold] = 0
mag_min, mag_max = magnitude_thresholded.min(), magnitude_thresholded.max()
if mag_max > mag_min:
magnitude_norm = ((magnitude_thresholded - mag_min) / (mag_max - mag_min) * 255).astype(np.uint8)
else:
magnitude_norm = np.zeros_like(magnitude_thresholded, dtype=np.uint8)
magnitude_colored = cv2.applyColorMap(magnitude_norm, colormap)
out_w, out_h = output_size
return cv2.resize(magnitude_colored, (out_w, out_h), interpolation=cv2.INTER_CUBIC)
def frame_to_bgr(frame: np.ndarray) -> np.ndarray:
"""Convert a grayscale or single-channel frame to BGR for display/recording."""
if frame is None:
raise ValueError("frame is None")
if len(frame.shape) == 2:
return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
if len(frame.shape) == 3 and frame.shape[2] == 1:
return cv2.cvtColor(frame.squeeze(2), cv2.COLOR_GRAY2BGR)
return frame
+1 -1
View File
@@ -7,7 +7,7 @@ from pathlib import Path
import cv2
from .opencv_maps import frame_to_bgr
from .render_2d import frame_to_bgr
class FrameRecorder:
+82 -6
View File
@@ -2,9 +2,51 @@
from __future__ import annotations
from dataclasses import dataclass
from typing import TYPE_CHECKING, Literal
import cv2
import numpy as np
import orisys
from .opencv_maps import depth_colormap
if TYPE_CHECKING:
from pipeline.frame import FingerFrameView
ArrowBackground = Literal["unrolled", "white"]
@dataclass(frozen=True)
class ArrowCanvasColors:
"""BGR fill colors for synthetic UV arrow background (``background='white'``)."""
outside_bgr: tuple[int, int, int] = (0, 0, 0)
uv_bgr: tuple[int, int, int] = (255, 255, 255)
DEFAULT_ARROW_CANVAS_COLORS = ArrowCanvasColors()
def depth_colormap(depth_map, colormap=cv2.COLORMAP_JET) -> np.ndarray:
"""Normalize a scalar map to uint8 and apply an OpenCV colormap."""
values = np.asarray(depth_map)
if values.size == 0:
return np.zeros((1, 1, 3), dtype=np.uint8)
if values.max() > values.min():
normalized = ((values - values.min()) / (values.max() - values.min()) * 255).astype(np.uint8)
else:
normalized = np.zeros_like(values, dtype=np.uint8)
return cv2.applyColorMap(normalized, colormap)
def frame_to_bgr(frame: np.ndarray) -> np.ndarray:
"""Convert a grayscale or single-channel frame to BGR for display/recording."""
if frame is None:
raise ValueError("frame is None")
if len(frame.shape) == 2:
return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
if len(frame.shape) == 3 and frame.shape[2] == 1:
return cv2.cvtColor(frame.squeeze(2), cv2.COLOR_GRAY2BGR)
return frame
def render_depth_panel(depth_map):
@@ -12,21 +54,55 @@ def render_depth_panel(depth_map):
return depth_colormap(depth_map)
def _as_bgr(color: tuple[int, int, int]) -> tuple[int, int, int]:
return tuple(int(np.clip(c, 0, 255)) for c in color[:3])
def _arrow_background(
frame: FingerFrameView,
background: ArrowBackground,
*,
canvas_colors: ArrowCanvasColors = DEFAULT_ARROW_CANVAS_COLORS,
) -> np.ndarray:
"""Return the BGR canvas for UV arrow drawing."""
if background == "unrolled":
return frame.unrolled
h, w = frame.unrolled.shape[:2]
outside = np.array(_as_bgr(canvas_colors.outside_bgr), dtype=np.uint8)
uv = np.array(_as_bgr(canvas_colors.uv_bgr), dtype=np.uint8)
canvas = np.broadcast_to(outside, (h, w, 3)).copy()
mask = frame.valid_mask
if mask is None:
return canvas
mask = np.asarray(mask, dtype=bool)
if mask.shape != (h, w):
mask_u8 = mask.astype(np.uint8)
if mask_u8.ndim > 2:
mask_u8 = mask_u8[:, :, 0]
mask = cv2.resize(mask_u8, (w, h), interpolation=cv2.INTER_NEAREST).astype(bool)
canvas[mask] = uv
return canvas
def render_uv_arrows(
unrolled,
vnormal,
frame: FingerFrameView,
*,
flow_threshold: float = 0.4,
grid_spacing: int = 20,
arrow_scale: float = 1.0,
background: ArrowBackground = "unrolled",
canvas_colors: ArrowCanvasColors = DEFAULT_ARROW_CANVAS_COLORS,
):
"""Return a BGR image with UV unrolled flow arrows."""
"""Return a BGR UV arrow panel from a FingerFrameView (uses frame.valid_mask)."""
return orisys.util.draw_arrows(
unrolled,
vnormal,
_arrow_background(frame, background, canvas_colors=canvas_colors),
frame.flow,
threshold=flow_threshold,
grid_spacing=grid_spacing,
arrow_scale=arrow_scale,
valid_mask=frame.valid_mask,
below_threshold_color = (128, 0, 0),
colormap = cv2.COLORMAP_JET
)
+1 -1
View File
@@ -5,7 +5,7 @@ from __future__ import annotations
import numpy as np
from scipy.signal import butter, filtfilt
def lowpass_filter(data, cutoff_hz, sample_rate_hz=30, order=2) -> np.ndarray:
def lowpass_filter(data, cutoff_hz=5, sample_rate_hz=30, order=2) -> np.ndarray:
"""Zero-phase Butterworth low-pass filter for uniformly sampled 1-D data."""
y = np.asarray(data, dtype=np.float64)
if cutoff_hz == 0: