From 258948bcb7232f01f175dc72331dfa93a420a33e Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Fri, 19 Jun 2026 21:22:28 +0800 Subject: [PATCH] added mesh surface arrows --- examples/gui/dashboard.py | 105 +++++-- examples/gui/layout.py | 55 +++- examples/gui/mesh_render_worker.py | 4 +- examples/gui/styles.py | 53 ++-- examples/gui/transport_icons.py | 50 +++ examples/gui/widgets/arrow_image.py | 11 +- examples/gui/widgets/force_charts.py | 11 +- examples/gui/widgets/mesh_viewport.py | 4 + examples/pipeline/frame.py | 34 ++ examples/pipeline/service.py | 35 +-- examples/viz/__init__.py | 21 +- examples/viz/finger_mesh_viewer.py | 38 ++- examples/viz/flow_arrow.py | 431 ++++++++++++++++++++++++++ examples/viz/force_charts.py | 6 +- examples/viz/offscreen_mesh.py | 81 ++++- examples/viz/opencv_maps.py | 57 ---- examples/viz/recording.py | 2 +- examples/viz/render_2d.py | 88 +++++- examples/viz/signal_filters.py | 2 +- src/orisys/util.py | 57 +++- 20 files changed, 951 insertions(+), 194 deletions(-) create mode 100644 examples/gui/transport_icons.py create mode 100644 examples/viz/flow_arrow.py delete mode 100644 examples/viz/opencv_maps.py diff --git a/examples/gui/dashboard.py b/examples/gui/dashboard.py index c235ab5..3b22e57 100644 --- a/examples/gui/dashboard.py +++ b/examples/gui/dashboard.py @@ -6,7 +6,7 @@ import sys import time from pathlib import Path -from PyQt5.QtCore import Qt, QTimer +from PyQt5.QtCore import QSize, Qt, QTimer from PyQt5.QtGui import QIcon from PyQt5.QtWidgets import ( QApplication, @@ -31,6 +31,7 @@ from .layout import ( top_row_splitter_sizes, ) from .styles import STYLE +from .transport_icons import transport_icon from .widgets import ( ArrowPanelWidget, ForceChartWidget, @@ -41,6 +42,15 @@ from .widgets import ( ) +_PLAY_PAUSE_BUTTON_NAME = "btnPlayPause" +_PLAY_PAUSE_ICON_PX = 18 +_PLAY_PAUSE_ICON_COLORS = { + "run": "#4caf84", + "starting": "#555555", + "pause": "#ff9800", +} + + class FingerDashboardWindow(QMainWindow): """Single-window finger demo: 3D mesh | UV arrows | force charts.""" @@ -88,14 +98,14 @@ class FingerDashboardWindow(QMainWindow): bar = QHBoxLayout() bar.setSpacing(layout.toolbar_spacing_px) - self.btn_camera = QPushButton("摄像机") + self.btn_camera = QPushButton("Sensor") self.btn_camera.clicked.connect(self._select_camera) bar.addWidget(self.btn_camera) self.camera_edit = QLineEdit("0") - self.camera_edit.setPlaceholderText("摄像头编号 或 视频路径") + self.camera_edit.setPlaceholderText("Camera ID or Video Path") bar.addWidget(self.camera_edit, 1) - self.btn_config = QPushButton("配置") + self.btn_config = QPushButton("Config") self.btn_config.clicked.connect(self._select_config) bar.addWidget(self.btn_config) self.config_edit = QLineEdit("./config/dd02-ov.json") @@ -104,34 +114,38 @@ class FingerDashboardWindow(QMainWindow): self.cal_edit = QLineEdit("") bar.addWidget(self.cal_edit, 1) - self.btn_reset = QPushButton("重置") + self.btn_reset = QPushButton("Reset") self.btn_reset.setEnabled(False) self.btn_reset.clicked.connect(self._on_reset) bar.addWidget(self.btn_reset) - self.btn_start = QPushButton("启动") - self.btn_start.setObjectName("btnStart") - self.btn_start.clicked.connect(self._on_start) - bar.addWidget(self.btn_start) - - self.btn_stop = QPushButton("暂停") - self.btn_stop.setObjectName("btnStop") - self.btn_stop.setEnabled(False) - self.btn_stop.clicked.connect(self._on_stop) - bar.addWidget(self.btn_stop) + self.btn_play_pause = QPushButton() + self.btn_play_pause.setObjectName(_PLAY_PAUSE_BUTTON_NAME) + self.btn_play_pause.setFixedSize(36, 32) + self.btn_play_pause.setIconSize(QSize(_PLAY_PAUSE_ICON_PX, _PLAY_PAUSE_ICON_PX)) + self.btn_play_pause.clicked.connect(self._on_play_pause_clicked) + self._set_play_pause_button("run") + bar.addWidget(self.btn_play_pause) root_layout.addLayout(bar) setup = layout.panel_setup - self._panel_widgets: dict[str, QWidget] = {} - self.mesh_viewport = MeshViewportWidget(layout=layout) - self.arrow_panel = ArrowPanelWidget(flow_threshold=layout.arrow_flow_threshold, layout=layout) + arrows_slot = layout.slot("arrows") + self.arrow_panel: ArrowPanelWidget | None = None + if arrows_slot is not None and arrows_slot.enabled: + self.arrow_panel = ArrowPanelWidget( + flow_threshold=layout.arrow_flow_threshold, + background=layout.arrow_background, + layout=layout, + ) self.force_charts = ForceChartWidget(layout=layout, lowpass_cutoff_hz=self.config.lowpass_cutoff_hz) self.log_widget = make_log_widget(layout.log_config) + self._panel_widgets: dict[str, QWidget] = {} self._panel_widgets["mesh"] = self.mesh_viewport - self._panel_widgets["arrows"] = self.arrow_panel + if self.arrow_panel is not None: + self._panel_widgets["arrows"] = self.arrow_panel self._panel_widgets["charts"] = self.force_charts.widget self._panel_widgets["log"] = self.log_widget @@ -194,11 +208,40 @@ class FingerDashboardWindow(QMainWindow): sys.stdout = TeeStream(*streams) sys.stderr = sys.stdout + def _set_play_pause_button(self, state: str) -> None: + """``run`` = green play, ``starting`` = grey disabled play, ``pause`` = orange pause.""" + btn = self.btn_play_pause + if state == "run": + btn.setToolTip("Run") + btn.setEnabled(True) + elif state == "starting": + btn.setToolTip("Starting…") + btn.setEnabled(False) + elif state == "pause": + btn.setToolTip("Pause") + btn.setEnabled(True) + else: + raise ValueError(f"unknown play/pause state: {state!r}") + + kind = "pause" if state == "pause" else "play" + btn.setText("") + btn.setIcon(transport_icon(kind, _PLAY_PAUSE_ICON_COLORS[state])) + btn.setProperty("playState", state) + style = btn.style() + style.unpolish(btn) + style.polish(btn) + btn.update() + + def _on_play_pause_clicked(self) -> None: + if self._running: + self._on_stop() + else: + self._on_start() + def _on_start(self) -> None: if self._running: return - self.btn_start.setText("启动中...") - self.btn_start.setEnabled(False) + self._set_play_pause_button("starting") QApplication.processEvents() self.config.video = self.camera_edit.text().strip() @@ -214,8 +257,7 @@ class FingerDashboardWindow(QMainWindow): QTimer.singleShot(0, self._apply_panel_layout) except Exception as exc: self.mesh_viewport.close_renderer() - self.btn_start.setText("启动") - self.btn_start.setEnabled(True) + self._set_play_pause_button("run") print(f"启动失败: {exc}") return @@ -223,8 +265,7 @@ class FingerDashboardWindow(QMainWindow): self._frame_index = 0 self._wall_fps = 0.0 self.timer.start(self.layout.timer_interval_ms) - self.btn_start.setText("启动") - self.btn_stop.setEnabled(True) + self._set_play_pause_button("pause") self.btn_reset.setEnabled(True) print( f"▶ 启动 — 摄像机={self.config.video}, 配置={self.config.config}, " @@ -237,17 +278,14 @@ class FingerDashboardWindow(QMainWindow): self._running = False self.timer.stop() self.log_handler.clear_status_line() - self.btn_stop.setText("停止中...") - self.btn_stop.setEnabled(False) + self._set_play_pause_button("starting") QApplication.processEvents() self.mesh_viewport.close_renderer() self.pipeline.close() - self.btn_start.setEnabled(True) - self.btn_stop.setText("暂停") + self._set_play_pause_button("run") self.btn_reset.setEnabled(False) - print("⏸ 已暂停") def _on_reset(self) -> None: if not self._running: @@ -278,9 +316,10 @@ class FingerDashboardWindow(QMainWindow): profile.record("mesh", mesh_ms) arrows_ms = 0.0 - t0 = time.perf_counter() - self.arrow_panel.update_frame(frame) - arrows_ms = (time.perf_counter() - t0) * 1000.0 + if self.arrow_panel is not None: + t0 = time.perf_counter() + self.arrow_panel.update_frame(frame) + arrows_ms = (time.perf_counter() - t0) * 1000.0 if profile is not None: profile.record("arrows", arrows_ms) diff --git a/examples/gui/layout.py b/examples/gui/layout.py index f08ee3b..7cec9a5 100644 --- a/examples/gui/layout.py +++ b/examples/gui/layout.py @@ -19,6 +19,8 @@ MeshLighting (below) controls the 3D mesh panel sun light: intensity — Filament sun strength (typical 30000–120000) ambient/diffuse — Windows viewer shading weights (0–1) +MESH_BACKGROUND (below) sets the 3D mesh panel clear color (RGB 0–255). + MESH_CONTACT_ARROW (below) controls the yellow 3D contact arrow on the mesh panel: reference_normal_force — Fn at which arrow reaches full size (log/power/linear curve) force_strength_mode — "log" | "power" | "linear" @@ -34,7 +36,7 @@ Performance (3D dashboard; edit DashboardLayout defaults): chart_update_stride — update force charts every N frames mesh_async_render — background mesh render (Linux only; Windows uses sync) -Edit TOP_ROW.slots to reorder or resize panels. +Edit TOP_ROW.slots to reorder or resize panels; set ``enabled=False`` to hide a panel (e.g. arrows). Edit PANEL_SETUP to change top vs log proportions. Edit LOG_CONFIG for live status line and log panel appearance. """ @@ -46,6 +48,8 @@ from dataclasses import dataclass from typing import Literal from viz.contact_arrow import ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE +from viz.flow_arrow import FlowArrowStyle +from viz.render_2d import ArrowBackground, ArrowCanvasColors PanelId = Literal["mesh", "arrows", "charts", "log"] @@ -84,7 +88,7 @@ class PanelSetup: TOP_ROW = PanelRow( slots=( PanelSlot("mesh", "3D mesh", width_px=500, height_px=500), - PanelSlot("arrows", "UV Unrolled", width_px=500, height_px=500), + PanelSlot("arrows", "UV Unrolled", width_px=500, height_px=500, enabled=False), PanelSlot("charts", "Force charts", width_px=500, height_px=500), ), ) @@ -99,8 +103,8 @@ PANEL_SETUP = PanelSetup( ) # Open3D offscreen resolution for the mesh panel (independent of on-screen widget size). -MESH_RENDER_WIDTH_PX = 256 -MESH_RENDER_HEIGHT_PX = 256 +MESH_RENDER_WIDTH_PX = 500 +MESH_RENDER_HEIGHT_PX = 500 @dataclass(frozen=True) @@ -123,13 +127,23 @@ class MeshLighting: MESH_LIGHTING = MeshLighting( - azimuth_deg=-45.0, + azimuth_deg=0, elevation_deg=0, intensity=75000.0, ambient=0.5, diffuse=0.5, ) + +@dataclass(frozen=True) +class MeshBackground: + """3D mesh panel clear color — edit MESH_BACKGROUND or DashboardLayout.mesh_background.""" + + color: tuple[int, int, int] = (0, 0, 0) + + +MESH_BACKGROUND = MeshBackground(color=(0, 0, 0)) + # 3D mesh contact arrow — edit here for dashboard tuning (see ContactArrowStyle in viz/contact_arrow.py). MESH_CONTACT_ARROW = ContactArrowStyle( reference_normal_force=10000.0, @@ -143,6 +157,26 @@ MESH_CONTACT_ARROW = ContactArrowStyle( color=(1.0, 0.92, 0.10), ) +# 3D mesh flow line arrows — tail_color/head_color are RGB 0–255 (see FlowArrowStyle). +MESH_FLOW_ARROW = FlowArrowStyle( + threshold=2.0, + grid_spacing=20, + arrow_scale=0.5, + length_margin=1.0, + tail_color=(40, 100, 220), + head_color=(40, 100, 220), + head_fraction=0.5, + opacity=1, + line_width=15, + head_line_width=30.0, +) + +# UV arrow synthetic canvas — BGR tuples (outside valid_mask, inside valid_mask). +ARROW_CANVAS_COLORS = ArrowCanvasColors( + outside_bgr=(0, 0, 0), + uv_bgr=(128, 0, 0), +) + @dataclass(frozen=True) class LogConfig: @@ -150,7 +184,7 @@ class LogConfig: status_enabled: bool = True max_lines: int = 2000 - font_family: str = "Consolas" + font_family: str = "Cascadia Mono" font_pt: int = 12 show_flow: bool = True show_mask: bool = True @@ -191,7 +225,7 @@ class DashboardLayout: panel_setup: PanelSetup = PANEL_SETUP window_title: str = "Orisys Finger Dashboard" - window_width: int = 1500 + window_width: int = 1000 window_height: int = 600 root_margin_px: int = 6 @@ -203,15 +237,20 @@ class DashboardLayout: chart_title_font_pt: int = 11 chart_axis_label_font_pt: int = 9 chart_tick_font_pt: int = 9 - y_axis_min_span: float = 20000.0 + y_axis_min_span: float = 10000.0 y_range_smooth_alpha: float = 0.5 curve_color: str = "#00E5FF" # UV arrow panel: min flow magnitude to draw an arrow (display only; not dd02-ov.json contact.flow_threshold) arrow_flow_threshold: float = 1.0 + # "unrolled" = sensor texture; "white" = solid canvas (tune colors via arrow_canvas_colors) + arrow_background: ArrowBackground = "white" + arrow_canvas_colors: ArrowCanvasColors = ARROW_CANVAS_COLORS mesh_lighting: MeshLighting = MESH_LIGHTING + mesh_background: MeshBackground = MESH_BACKGROUND mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW + mesh_flow_arrow: FlowArrowStyle = MESH_FLOW_ARROW log_config: LogConfig = LOG_CONFIG diff --git a/examples/gui/mesh_render_worker.py b/examples/gui/mesh_render_worker.py index 08373f1..6bd00c4 100644 --- a/examples/gui/mesh_render_worker.py +++ b/examples/gui/mesh_render_worker.py @@ -28,7 +28,7 @@ class MeshRenderWorker(QThread): self._renderer = None self._pending = None - def submit(self, *, flow, scalar_map, centroid, centroid_found, fnormal) -> None: + def submit(self, *, flow, scalar_map, centroid, centroid_found, fnormal, valid_mask=None) -> None: with QMutexLocker(self._mutex): self._pending = { "flow": flow, @@ -36,6 +36,7 @@ class MeshRenderWorker(QThread): "centroid": centroid, "centroid_found": centroid_found, "fnormal": fnormal, + "valid_mask": valid_mask, } self._wake = True if not self.isRunning(): @@ -66,6 +67,7 @@ class MeshRenderWorker(QThread): centroid=job["centroid"], centroid_found=job["centroid_found"], fnormal=job["fnormal"], + valid_mask=job["valid_mask"], ) self.rgb_ready.emit(np.ascontiguousarray(rgb)) diff --git a/examples/gui/styles.py b/examples/gui/styles.py index 8f18175..c23ed3d 100644 --- a/examples/gui/styles.py +++ b/examples/gui/styles.py @@ -1,5 +1,10 @@ """Qt stylesheet for the finger dashboard.""" +UI_FONT_STACK = '"Segoe UI Variable", "Segoe UI", "Inter", "Microsoft YaHei UI", sans-serif' +LOG_FONT_STACK = '"Cascadia Mono", "JetBrains Mono", "IBM Plex Mono", "Consolas", monospace' +UI_FONT_PRIMARY = "Segoe UI" +LOG_FONT_PRIMARY = "Cascadia Mono" + STYLE = """ QMainWindow { background-color: #000000; @@ -17,7 +22,7 @@ QPushButton { border: 1px solid #333333; border-radius: 4px; padding: 6px 16px; - font-family: "SimHei"; + font-family: __UI_FONT__; font-size: 13px; } QPushButton:hover { @@ -25,33 +30,36 @@ QPushButton:hover { color: #cccccc; border: 1px solid #555555; } -QPushButton#btnStart { +QPushButton#btnPlayPause { background-color: #0d2818; color: #4caf84; border: 1px solid #1a4d30; + padding: 4px 8px; + font-size: 15px; } -QPushButton#btnStart:hover { +QPushButton#btnPlayPause:hover { background-color: #143d24; color: #6fd4a0; } -QPushButton#btnStart:disabled { - background-color: #0f0f0f; - color: #333333; - border: 1px solid #222222; +QPushButton#btnPlayPause:disabled { + background-color: #1a1a1a; + color: #555555; + border: 1px solid #333333; } -QPushButton#btnStop { - background-color: #2b1010; - color: #e05555; - border: 1px solid #4d1a1a; +QPushButton#btnPlayPause[playState="pause"] { + background-color: #3d2808; + color: #ff9800; + border: 1px solid #6d4400; } -QPushButton#btnStop:hover { - background-color: #3d1818; - color: #f07070; +QPushButton#btnPlayPause[playState="pause"]:hover { + background-color: #523208; + color: #ffb74d; + border: 1px solid #8a5a00; } -QPushButton#btnStop:disabled { - background-color: #0f0f0f; - color: #333333; - border: 1px solid #222222; +QPushButton#btnPlayPause[playState="pause"]:disabled { + background-color: #1a1a1a; + color: #555555; + border: 1px solid #333333; } QLineEdit { background-color: #0d0d0d; @@ -59,6 +67,7 @@ QLineEdit { border: 1px solid #333333; border-radius: 4px; padding: 5px 10px; + font-family: __UI_FONT__; font-size: 13px; } @@ -69,11 +78,11 @@ GraphicsLayoutWidget { QPlainTextEdit { background-color: #0b0b0b; - color: #777777; + color: #999999; border: 1px solid #262626; border-radius: 4px; - font-family: "Cascadia Code", "Consolas", "Courier New", monospace; - font-size: 13px; + font-family: __LOG_FONT__; + font-size: 12px; padding: 8px; selection-background-color: #1a3a4a; selection-color: #cccccc; @@ -111,4 +120,4 @@ QSplitter::handle:vertical { QSplitter::handle:horizontal { width: 1px; } -""" +""".replace("__UI_FONT__", UI_FONT_STACK).replace("__LOG_FONT__", LOG_FONT_STACK) diff --git a/examples/gui/transport_icons.py b/examples/gui/transport_icons.py new file mode 100644 index 0000000..8e336f4 --- /dev/null +++ b/examples/gui/transport_icons.py @@ -0,0 +1,50 @@ +"""Monochrome play/pause icons for the dashboard toolbar (avoids Windows emoji glyphs).""" + +from __future__ import annotations + +from typing import Literal + +from PyQt5.QtCore import QPointF, QRectF, Qt +from PyQt5.QtGui import QColor, QIcon, QPainter, QPixmap, QPolygonF + +TransportKind = Literal["play", "pause"] + +_DEFAULT_ICON_PX = 18 + + +def transport_icon(kind: TransportKind, color: str, *, size: int = _DEFAULT_ICON_PX) -> QIcon: + """Build a flat play triangle or pause bars in ``color`` (#rrggbb).""" + pixmap = QPixmap(size, size) + pixmap.fill(Qt.transparent) + + painter = QPainter(pixmap) + painter.setRenderHint(QPainter.Antialiasing, True) + painter.setBrush(QColor(color)) + painter.setPen(Qt.NoPen) + + if kind == "play": + w = size * 0.52 + h = size * 0.60 + x0 = (size - w) * 0.58 + y0 = (size - h) / 2.0 + painter.drawPolygon( + QPolygonF( + [ + QPointF(x0, y0), + QPointF(x0, y0 + h), + QPointF(x0 + w, y0 + h / 2.0), + ] + ) + ) + else: + bar_w = size * 0.17 + bar_h = size * 0.60 + gap = size * 0.14 + y0 = (size - bar_h) / 2.0 + x0 = (size - (2.0 * bar_w + gap)) / 2.0 + radius = max(1.0, size * 0.06) + painter.drawRoundedRect(QRectF(x0, y0, bar_w, bar_h), radius, radius) + painter.drawRoundedRect(QRectF(x0 + bar_w + gap, y0, bar_w, bar_h), radius, radius) + + painter.end() + return QIcon(pixmap) diff --git a/examples/gui/widgets/arrow_image.py b/examples/gui/widgets/arrow_image.py index 87c6724..c48aec2 100644 --- a/examples/gui/widgets/arrow_image.py +++ b/examples/gui/widgets/arrow_image.py @@ -5,7 +5,7 @@ from __future__ import annotations import cv2 from pipeline.frame import FingerFrameView -from viz.render_2d import render_uv_arrows +from viz.render_2d import ArrowBackground, ArrowCanvasColors, render_uv_arrows from ..layout import DashboardLayout, LAYOUT from .image_panel import ImagePanel @@ -19,6 +19,8 @@ class ArrowPanelWidget(ImagePanel): self, *, flow_threshold: float = 0.5, + background: ArrowBackground = "unrolled", + canvas_colors: ArrowCanvasColors | None = None, layout: DashboardLayout = LAYOUT, parent=None, ): @@ -30,15 +32,18 @@ class ArrowPanelWidget(ImagePanel): parent=parent, ) self.flow_threshold = flow_threshold + self.background = background + self.canvas_colors = canvas_colors if canvas_colors is not None else layout.arrow_canvas_colors def set_flow_threshold(self, threshold: float) -> None: self.flow_threshold = threshold def update_frame(self, frame: FingerFrameView) -> None: arrows = render_uv_arrows( - frame.unrolled, - frame.vnormal, + frame, flow_threshold=self.flow_threshold, + background=self.background, + canvas_colors=self.canvas_colors, ) arrows = cv2.rotate(arrows, cv2.ROTATE_180) self.set_source_pixmap(ndarray_to_pixmap(arrows, bgr=True)) diff --git a/examples/gui/widgets/force_charts.py b/examples/gui/widgets/force_charts.py index 058b369..b1d605d 100644 --- a/examples/gui/widgets/force_charts.py +++ b/examples/gui/widgets/force_charts.py @@ -7,6 +7,7 @@ from PyQt5.QtGui import QFont from PyQt5.QtWidgets import QSizePolicy from gui.layout import DashboardLayout, LAYOUT +from gui.styles import LOG_FONT_PRIMARY, UI_FONT_PRIMARY from pipeline.frame import FingerFrameView from viz.signal_filters import lowpass_filter @@ -72,21 +73,21 @@ class ForceChartWidget: def _create_plots(self) -> None: pg = self._pg layout = self._layout - titles = ["法向力", "切向力 X", "切向力 Y"] + titles = ["Normal Deformation", "X Deformation", "Y Deformation"] self.plots = [] self.curves = [] self.dot_glows = [] self.dot_cores = [] axis_pen = pg.mkPen(color=(38, 38, 38), width=1) - label_font = QFont("SimHei", layout.chart_axis_label_font_pt) + label_font = QFont(UI_FONT_PRIMARY, layout.chart_axis_label_font_pt) title_pt = layout.chart_title_font_pt for i, title in enumerate(titles): plot = self.widget.addPlot(title=None) plot.setTitle(title, color="#999999", size=f"{title_pt}pt") - plot.titleLabel.setFont(QFont("SimHei", title_pt)) - plot.setLabel("bottom", "时间", units="s") + plot.titleLabel.setFont(QFont(UI_FONT_PRIMARY, title_pt)) + plot.setLabel("bottom", "Time", units="s") plot.getAxis("bottom").label.setFont(label_font) plot.showAxis("top", False) plot.showAxis("right", False) @@ -94,7 +95,7 @@ class ForceChartWidget: plot.getAxis("bottom").setPen(axis_pen) plot.showGrid(x=False, y=True, alpha=0.12) - tick_font = QFont("Consolas", layout.chart_tick_font_pt) + tick_font = QFont(LOG_FONT_PRIMARY, layout.chart_tick_font_pt) tick_font.setStyleHint(QFont.Monospace) plot.getAxis("left").setTickFont(tick_font) plot.getAxis("bottom").setTickFont(tick_font) diff --git a/examples/gui/widgets/mesh_viewport.py b/examples/gui/widgets/mesh_viewport.py index 2c2eb20..d881ff9 100644 --- a/examples/gui/widgets/mesh_viewport.py +++ b/examples/gui/widgets/mesh_viewport.py @@ -65,7 +65,9 @@ class MeshViewportWidget(ImagePanel): world_to_cam=pipeline.world_to_cam, lookat=pipeline.mesh_lookat, mesh_lighting=self._layout.mesh_lighting, + mesh_background=self._layout.mesh_background.color, contact_arrow_style=self._layout.mesh_contact_arrow, + flow_arrow_style=self._layout.mesh_flow_arrow, ) def _stop_worker(self) -> None: @@ -93,6 +95,7 @@ class MeshViewportWidget(ImagePanel): centroid=frame.centroid, centroid_found=frame.centroid_found, fnormal=frame.fnormal, + valid_mask=frame.valid_mask, ) return 0.0 @@ -105,6 +108,7 @@ class MeshViewportWidget(ImagePanel): centroid=frame.centroid, centroid_found=frame.centroid_found, fnormal=frame.fnormal, + valid_mask=frame.valid_mask, ) self.set_source_pixmap(ndarray_to_pixmap(rgb, bgr=False)) return (time.perf_counter() - t0) * 1000.0 diff --git a/examples/pipeline/frame.py b/examples/pipeline/frame.py index fe04a11..5068b17 100644 --- a/examples/pipeline/frame.py +++ b/examples/pipeline/frame.py @@ -23,4 +23,38 @@ class FingerFrameView: fsheary: float centroid_found: bool mask_coverage: float + valid_mask: np.ndarray | None = None raw_frame: np.ndarray | None = None + + @classmethod + def from_tactile(cls, tactile, unrolled, *, timestamp: float) -> FingerFrameView: + """Build a display snapshot from tactile state (mask + fields assembled here).""" + fps, flow, vnormal, depth_map, centroid, fnormal, fshearx, fsheary = tactile.read_info( + tactile.info.FPS, + tactile.info.VRAW, + tactile.info.VNORMAL, + tactile.info.DEPTH, + tactile.info.CENTROID, + tactile.info.FNORMAL, + tactile.info.FSHEARX, + tactile.info.FSHEARY, + ) + mask = tactile.valid_mask + return cls( + timestamp=timestamp, + fps=float(fps), + unrolled=unrolled, + flow=flow, + vnormal=vnormal, + depth_map=depth_map, + img=np.array([]), + contour=np.array([]), + centroid=centroid, + fnormal=float(fnormal), + fshearx=float(fshearx), + fsheary=float(fsheary), + centroid_found=bool(tactile.raw_sensor.centroid_found), + mask_coverage=float(tactile.mask_stats["coverage"]), + valid_mask=mask.copy() if mask is not None else None, + raw_frame=tactile.frame, + ) diff --git a/examples/pipeline/service.py b/examples/pipeline/service.py index 11c9dc3..18dcc7a 100644 --- a/examples/pipeline/service.py +++ b/examples/pipeline/service.py @@ -87,35 +87,6 @@ class FingerPipeline: if self.runtime is not None: self.runtime.tactile.reset() - def _read_frame_view(self, tactile, unrolled) -> FingerFrameView: - fps, flow, vnormal, depth_map, centroid, fnormal, fshearx, fsheary = tactile.read_info( - tactile.info.FPS, - tactile.info.VRAW, - tactile.info.VNORMAL, - tactile.info.DEPTH, - tactile.info.CENTROID, - tactile.info.FNORMAL, - tactile.info.FSHEARX, - tactile.info.FSHEARY, - ) - return FingerFrameView( - timestamp=time.time() - self._start_time, - fps=float(fps), - unrolled=unrolled, - flow=flow, - vnormal=vnormal, - depth_map=depth_map, - img=np.array([]), - contour=np.array([]), - centroid=centroid, - fnormal=float(fnormal), - fshearx=float(fshearx), - fsheary=float(fsheary), - centroid_found=bool(tactile.raw_sensor.centroid_found), - mask_coverage=float(tactile.mask_stats["coverage"]), - raw_frame=tactile.frame, - ) - def tick(self) -> FingerFrameView | None: frame, _timings = self.tick_timed() return frame @@ -165,7 +136,11 @@ class FingerPipeline: self.profiler.record("contact", timings.contact_ms) t0 = time.perf_counter() - view = self._read_frame_view(tactile, unrolled) + view = FingerFrameView.from_tactile( + tactile, + unrolled, + timestamp=time.time() - self._start_time, + ) timings.read_info_ms = (time.perf_counter() - t0) * 1000.0 if profile: self.profiler.record("read_info", timings.read_info_ms) diff --git a/examples/viz/__init__.py b/examples/viz/__init__.py index 7cf7e5c..b2656b8 100644 --- a/examples/viz/__init__.py +++ b/examples/viz/__init__.py @@ -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", diff --git a/examples/viz/finger_mesh_viewer.py b/examples/viz/finger_mesh_viewer.py index 7f479d6..11c3ec2 100644 --- a/examples/viz/finger_mesh_viewer.py +++ b/examples/viz/finger_mesh_viewer.py @@ -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) diff --git a/examples/viz/flow_arrow.py b/examples/viz/flow_arrow.py new file mode 100644 index 0000000..4ee41dd --- /dev/null +++ b/examples/viz/flow_arrow.py @@ -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() diff --git a/examples/viz/force_charts.py b/examples/viz/force_charts.py index e3e0956..ef891af 100644 --- a/examples/viz/force_charts.py +++ b/examples/viz/force_charts.py @@ -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) diff --git a/examples/viz/offscreen_mesh.py b/examples/viz/offscreen_mesh.py index 5f3267c..ca5a726 100644 --- a/examples/viz/offscreen_mesh.py +++ b/examples/viz/offscreen_mesh.py @@ -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: diff --git a/examples/viz/opencv_maps.py b/examples/viz/opencv_maps.py deleted file mode 100644 index 5a74c6f..0000000 --- a/examples/viz/opencv_maps.py +++ /dev/null @@ -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 diff --git a/examples/viz/recording.py b/examples/viz/recording.py index 5376746..054c2d2 100644 --- a/examples/viz/recording.py +++ b/examples/viz/recording.py @@ -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: diff --git a/examples/viz/render_2d.py b/examples/viz/render_2d.py index 1d2e92b..7bbc8b5 100644 --- a/examples/viz/render_2d.py +++ b/examples/viz/render_2d.py @@ -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 ) diff --git a/examples/viz/signal_filters.py b/examples/viz/signal_filters.py index 7235cb9..ff58d53 100644 --- a/examples/viz/signal_filters.py +++ b/examples/viz/signal_filters.py @@ -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: diff --git a/src/orisys/util.py b/src/orisys/util.py index ee66820..de2931a 100644 --- a/src/orisys/util.py +++ b/src/orisys/util.py @@ -1,16 +1,50 @@ import cv2 import numpy as np -def draw_arrows(frame, optical_flow, fps=True, threshold=5, grid_spacing=40, arrow_scale=1.0, margin = 0.8, colormap=cv2.COLORMAP_JET, below_threshold_color=(0, 0, 255), edge_margin=15): + +def _prepare_draw_mask(valid_mask, shape): + """Resize a boolean mask to (height, width) for arrow culling.""" + if valid_mask is None: + return None + h, w = shape[:2] + 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 draw_arrows( + frame, + optical_flow, + fps=True, + threshold=5, + grid_spacing=40, + arrow_scale=1.0, + margin=0.8, + colormap=cv2.COLORMAP_JET, + below_threshold_color=(0, 0, 255), + edge_margin=15, + valid_mask=None, +): """ Draw colored optical flow arrows on the image by magnitude (JET colormap). Frame and optical_flow must have the same height/width; arrows are drawn in pixel coordinates. + + Parameters + ---------- + valid_mask : array-like, optional + Boolean (H, W) mask. Grid points outside the mask are skipped (no arrows or dots). """ image = frame.copy() if len(image.shape) == 2 or image.shape[2] == 1: image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) h, w = image.shape[:2] + mask = _prepare_draw_mask(valid_mask, (h, w)) + # Arrow grid in pixel coordinates (assume frame and flow same size) rr = np.arange(grid_spacing // 2, h, grid_spacing) cc = np.arange(grid_spacing // 2, w, grid_spacing) @@ -24,17 +58,29 @@ def draw_arrows(frame, optical_flow, fps=True, threshold=5, grid_spacing=40, arr dy = disp[:, 1] / arrow_scale length = np.sqrt(dx * dx + dy * dy) # Cap arrow length at grid_spacing - scale = np.where(length > grid_spacing*margin, grid_spacing / np.maximum(length, 1e-9), 1.0)*margin + 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 = (cc + dx).astype(np.int32) end_y = (rr + dy).astype(np.int32) valid = ( - (cc >= edge_margin) & (cc < w - edge_margin) & (rr >= edge_margin) & (rr < h - edge_margin) - & (end_x >= 0) & (end_x < w) & (end_y >= 0) & (end_y < h) + (cc >= edge_margin) + & (cc < w - edge_margin) + & (rr >= edge_margin) + & (rr < h - edge_margin) + & (end_x >= 0) + & (end_x < w) + & (end_y >= 0) + & (end_y < h) ) + if mask is not None: + valid &= mask[rr, cc] + valid &= mask[end_y, end_x] + mag_range = mag.max() - mag.min() - norm_mag = ((mag - mag.min()) / mag_range * 255).astype(np.uint8) if mag_range > 0 else np.zeros_like(mag, dtype=np.uint8) + norm_mag = ( + ((mag - mag.min()) / mag_range * 255).astype(np.uint8) if mag_range > 0 else np.zeros_like(mag, dtype=np.uint8) + ) norm_mag[mag < threshold] = 0 colors = cv2.applyColorMap(norm_mag.reshape(-1, 1), colormap) @@ -44,7 +90,6 @@ def draw_arrows(frame, optical_flow, fps=True, threshold=5, grid_spacing=40, arr if mag[i] < threshold: cv2.circle(image, pt1, radius=1, color=below_threshold_color, thickness=-1) else: - # thickness = max(1, int(2 * mag[i] / mag.max())) if mag.max() > 0 else 2 thickness = 1 cv2.arrowedLine(image, pt1, pt2, color, thickness=thickness, tipLength=0.2) return image