diff --git a/assets/finger/30fps/unroll_map_uv_s0.4.npz b/assets/finger/30fps/unroll_map_uv_s0.4.npz index 365fcb5..aafa344 100644 Binary files a/assets/finger/30fps/unroll_map_uv_s0.4.npz and b/assets/finger/30fps/unroll_map_uv_s0.4.npz differ diff --git a/config/dd02-ov.json b/config/dd02-ov.json index afc5543..545a74a 100644 --- a/config/dd02-ov.json +++ b/config/dd02-ov.json @@ -5,13 +5,13 @@ "camera": { "resolution_raw": [1280, 720], "fps": 30, - "resolution_stitch": [672, 728], + "resolution_stitch": [420, 450], "source_scale": 0.4, - "old":[672, 728], + "old":[672, 728, 1,0.5], "work": [504, 546,1], "work2": [420, 450,0.8], "work3": [350, 375,4,0.7], - "work4": [280, 300,4,0.4] + "work4": [280, 300,4,0.4,0.2] }, "tracker": { "adaptive": true, @@ -26,8 +26,8 @@ }, "contact": { "min_contour_area": 3000, - "flow_threshold": 0.5, - "div_threshold": 0.03, + "flow_threshold": 0.2, + "div_threshold": 0.022, "expansion_threshold": 0.01, "kernel_size": [5, 5], "denoise_sigma": 12 diff --git a/examples/finger_pipeline.py b/examples/finger_pipeline.py index 8f19656..f359976 100644 --- a/examples/finger_pipeline.py +++ b/examples/finger_pipeline.py @@ -12,6 +12,7 @@ For the full demo (UV debug panels, dashboard, benchmarks) see: import argparse import os import sys +import time os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0") @@ -19,6 +20,19 @@ import cv2 import orisys from orisys.finger import FingerRuntime +from pipeline.profiling import StageProfiler, run_deformation_timed + +_FINGER_PIPELINE_STAGES = ( + "capture", + "uv_unroll", + "flow", + "hhd", + "contact", + "read_info", + "display", + "total", +) + def main(): parser = argparse.ArgumentParser(description="Minimum UV-unroll finger tactile pipeline") @@ -27,6 +41,17 @@ def main(): parser.add_argument("--config", "-c", default="./config/dd02-ov.json", help="Orisys SDK config") parser.add_argument("--show", action="store_true", default=True, help="Show unrolled image window") parser.add_argument("--verbose", action="store_true", help="SDK verbose logs") + parser.add_argument( + "--profile", + action="store_true", + help="Print per-stage timing (capture, UV, flow, HHD, contact, read_info, display)", + ) + parser.add_argument( + "--profile-every", + type=int, + default=30, + help="When --profile is set, print rolling averages every N frames (default: 30)", + ) args = parser.parse_args() try: @@ -52,16 +77,48 @@ def main(): runtime.warmup(color=False) tactile = runtime.tactile + profiler = ( + StageProfiler(report_every=args.profile_every, stage_order=_FINGER_PIPELINE_STAGES) + if args.profile + else None + ) print("Press 'q' to quit, 'r' to reset tracker.") + if args.profile: + print(f"Profiling enabled (report every {args.profile_every} frames).") try: while True: - ok, unrolled = runtime.grab_and_process(color=False) - if not ok: - break - tactile.compute_deformation(check_motion=True, threshold=0) - tactile.compute_contact() + frame_t0 = time.perf_counter() + if profiler is not None: + t0 = time.perf_counter() + ok, frame = runtime.read_source() + profiler.record("capture", (time.perf_counter() - t0) * 1000.0) + if not ok: + break + t0 = time.perf_counter() + unrolled = runtime.process_frame(frame, color=False) + profiler.record("uv_unroll", (time.perf_counter() - t0) * 1000.0) + else: + ok, unrolled = runtime.grab_and_process(color=False) + if not ok: + break + + if profiler is not None: + flow_ms, hhd_ms = run_deformation_timed( + tactile, check_motion=True, threshold=0 + ) + profiler.record("flow", flow_ms) + profiler.record("hhd", hhd_ms) + else: + tactile.compute_deformation(check_motion=True, threshold=0) + + t0 = time.perf_counter() + tactile.compute_contact() + if profiler is not None: + profiler.record("contact", (time.perf_counter() - t0) * 1000.0) + + t0 = time.perf_counter() fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = tactile.read_info( tactile.info.FPS, tactile.info.VRAW, @@ -72,18 +129,18 @@ def main(): tactile.info.DEPTH, tactile.info.FNORMAL, tactile.info.FSHEARX, - tactile.info.FSHEARY + tactile.info.FSHEARY, ) + if profiler is not None: + profiler.record("read_info", (time.perf_counter() - t0) * 1000.0) - print( + status = ( f"FPS={fps:.2f} normal={fnormal:.2f} shear=({fshearx:.2f}, {fsheary:.2f}) " - f"mask={tactile.mask_stats['coverage']:.2f}", - end="\r", + f"mask={tactile.mask_stats['coverage']:.2f}" ) - if args.show: - + t0 = time.perf_counter() arrows = orisys.util.draw_arrows( unrolled, flow, @@ -91,10 +148,19 @@ def main(): grid_spacing=10, arrow_scale=1.0, ) - # 步骤 3.2：显示接触区域和质心 - image_with_foe = orisys.util.draw_contact(img, contour, centroid) - cv2.imshow("接触区域与质心", image_with_foe) cv2.imshow("UV Unrolled", arrows) + if profiler is not None: + profiler.record("display", (time.perf_counter() - t0) * 1000.0) + + if profiler is not None: + total_ms = (time.perf_counter() - frame_t0) * 1000.0 + profiler.record("total", total_ms) + status += ( + f" | wall={total_ms:.1f}ms flow={profiler.last.get('flow', 0):.1f}ms" + f" hhd={profiler.last.get('hhd', 0):.1f}ms" + ) + profiler.tick() + print(status, end="\r") key = cv2.waitKey(1) & 0xFF if key == ord("q"): @@ -104,6 +170,8 @@ def main(): tactile.reset() print("\nTracker reset.") finally: + if profiler is not None and profiler.frame_count: + profiler.print_report(prefix="[profile final]") runtime.close() cv2.destroyAllWindows() diff --git a/examples/finger_pipeline_3dview.py b/examples/finger_pipeline_3dview.py index ad5a0a5..f4786f4 100644 --- a/examples/finger_pipeline_3dview.py +++ b/examples/finger_pipeline_3dview.py @@ -52,6 +52,17 @@ def _parse_args(): default=5.0, help="Low-pass cutoff for force curves (Hz)", ) + parser.add_argument( + "--profile", + action="store_true", + help="Print per-stage timing to the log panel / console", + ) + parser.add_argument( + "--profile-every", + type=int, + default=30, + help="When --profile is set, print rolling averages every N frames", + ) return parser.parse_args() diff --git a/examples/gui/__init__.py b/examples/gui/__init__.py index e06da2f..102a142 100644 --- a/examples/gui/__init__.py +++ b/examples/gui/__init__.py @@ -2,23 +2,35 @@ from .dashboard import FingerDashboardWindow, run_dashboard from .layout import ( - DashboardLayout, - LAYOUT, + LOG_CONFIG, LOG_PANEL, + LAYOUT, + MESH_CONTACT_ARROW, + MESH_LIGHTING, PANEL_SETUP, TOP_ROW, + DashboardLayout, + LogConfig, + MeshLighting, PanelRow, PanelSetup, PanelSlot, top_row_splitter_sizes, top_row_slot_index, ) +from viz.contact_arrow import ContactArrowStyle __all__ = [ "DashboardLayout", "FingerDashboardWindow", "LAYOUT", + "LOG_CONFIG", "LOG_PANEL", + "LogConfig", + "MESH_CONTACT_ARROW", + "MESH_LIGHTING", + "ContactArrowStyle", + "MeshLighting", "PANEL_SETUP", "TOP_ROW", "PanelRow", diff --git a/examples/gui/dashboard.py b/examples/gui/dashboard.py index 8aee5e0..c235ab5 100644 --- a/examples/gui/dashboard.py +++ b/examples/gui/dashboard.py @@ -3,6 +3,7 @@ from __future__ import annotations import sys +import time from pathlib import Path from PyQt5.QtCore import Qt, QTimer @@ -55,6 +56,9 @@ class FingerDashboardWindow(QMainWindow): self.layout = layout self.pipeline = FingerPipeline(config) self._running = False + self._frame_index = 0 + self._wall_fps = 0.0 + self._wall_fps_alpha = 0.15 self.setWindowTitle(layout.window_title) self.setStyleSheet(STYLE) @@ -124,7 +128,7 @@ class FingerDashboardWindow(QMainWindow): self.mesh_viewport = MeshViewportWidget(layout=layout) self.arrow_panel = ArrowPanelWidget(flow_threshold=layout.arrow_flow_threshold, layout=layout) self.force_charts = ForceChartWidget(layout=layout, lowpass_cutoff_hz=self.config.lowpass_cutoff_hz) - self.log_widget = make_log_widget() + self.log_widget = make_log_widget(layout.log_config) self._panel_widgets["mesh"] = self.mesh_viewport self._panel_widgets["arrows"] = self.arrow_panel @@ -216,6 +220,8 @@ class FingerDashboardWindow(QMainWindow): return self._running = True + 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) @@ -230,6 +236,7 @@ class FingerDashboardWindow(QMainWindow): return self._running = False self.timer.stop() + self.log_handler.clear_status_line() self.btn_stop.setText("停止中...") self.btn_stop.setEnabled(False) QApplication.processEvents() @@ -252,25 +259,70 @@ class FingerDashboardWindow(QMainWindow): def _on_timer(self) -> None: if not self._running: return - frame = self.pipeline.tick() + + frame_t0 = time.perf_counter() + frame, timings = self.pipeline.tick_timed() if frame is None: print("采集结束或读取失败。") self._on_stop() return + self._frame_index += 1 + layout = self.layout + profile = self.pipeline.profiler + + mesh_ms = 0.0 + if layout.mesh_render_stride <= 1 or self._frame_index % layout.mesh_render_stride == 0: + mesh_ms = self.mesh_viewport.update_frame(frame) + if profile is not None and mesh_ms > 0: + 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 profile is not None: + profile.record("arrows", arrows_ms) + + charts_ms = 0.0 + if layout.chart_update_stride <= 1 or self._frame_index % layout.chart_update_stride == 0: + t0 = time.perf_counter() + self.force_charts.update_frame(frame) + charts_ms = (time.perf_counter() - t0) * 1000.0 + if profile is not None: + profile.record("charts", charts_ms) + + total_ms = (time.perf_counter() - frame_t0) * 1000.0 + instant_fps = 1000.0 / total_ms if total_ms > 0 else 0.0 + if self._wall_fps <= 0: + self._wall_fps = instant_fps + else: + self._wall_fps = ( + self._wall_fps_alpha * instant_fps + + (1.0 - self._wall_fps_alpha) * self._wall_fps + ) + + if profile is not None: + profile.record("total", total_ms) + self.pipeline.record_total_frame(total_ms) + flow_mag = 0.0 if frame.flow.size: flow_mag = float((frame.flow[:, :, 0] ** 2 + frame.flow[:, :, 1] ** 2).mean() ** 0.5) - print( - f"FPS={frame.fps:.2f} flow_rms={flow_mag:.2f} " - f"mask={frame.mask_coverage:.2f} normal={frame.fnormal:.2f} " - f"shear=({frame.fshearx:.2f}, {frame.fsheary:.2f})", - end="\r", - ) - self.mesh_viewport.update_frame(frame) - self.arrow_panel.update_frame(frame) - self.force_charts.update_frame(frame) + log_cfg = layout.log_config + if log_cfg.status_enabled: + self.log_handler.set_status_line( + log_cfg.format_status( + fps=frame.fps, + fnormal=frame.fnormal, + fshearx=frame.fshearx, + fsheary=frame.fsheary, + flow_rms=flow_mag, + mask_coverage=frame.mask_coverage, + wall_fps=self._wall_fps, + ) + ) def _select_camera(self) -> None: path, _ = QFileDialog.getOpenFileName( @@ -325,6 +377,9 @@ def run_dashboard(config: PipelineConfig, *, layout: DashboardLayout = LAYOUT) - from .app import get_or_create_qapp print("\n按键说明：Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏") + if config.profile: + print(f"Profiling enabled (report every {config.profile_every} frames).") + print("Log FPS= is end-to-end dashboard rate; sdk= is Sensor internal rate.") app = get_or_create_qapp() window = FingerDashboardWindow(config, layout=layout) diff --git a/examples/gui/layout.py b/examples/gui/layout.py index 4e32e62..f08ee3b 100644 --- a/examples/gui/layout.py +++ b/examples/gui/layout.py @@ -13,15 +13,40 @@ Structure: Each panel uses explicit width_px and height_px. top_stretch / log_stretch control how extra vertical space is shared on resize. +MeshLighting (below) controls the 3D mesh panel sun light: + azimuth_deg — horizontal angle in degrees (0° = +X, 90° = +Y) + elevation_deg — vertical angle in degrees (positive = from above) + intensity — Filament sun strength (typical 30000–120000) + ambient/diffuse — Windows viewer shading weights (0–1) + +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" + min/max_length_fraction, min/max_thickness_scale, color + +Mesh panel has two sizes (edit MESH_RENDER_* and TOP_ROW mesh slot): + display (TOP_ROW mesh width_px/height_px) — Qt widget size on screen + render (MESH_RENDER_WIDTH/HEIGHT) — Open3D offscreen resolution (upscaled for display) + +Performance (3D dashboard; edit DashboardLayout defaults): + timer_interval_ms — Qt timer period (16 ≈ 60 Hz tick rate) + mesh_render_stride — render 3D mesh every N frames (2 ≈ half mesh cost) + 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 PANEL_SETUP to change top vs log proportions. +Edit LOG_CONFIG for live status line and log panel appearance. """ from __future__ import annotations +import math from dataclasses import dataclass from typing import Literal +from viz.contact_arrow import ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE + PanelId = Literal["mesh", "arrows", "charts", "log"] @@ -73,6 +98,91 @@ PANEL_SETUP = PanelSetup( log_stretch=1, ) +# Open3D offscreen resolution for the mesh panel (independent of on-screen widget size). +MESH_RENDER_WIDTH_PX = 256 +MESH_RENDER_HEIGHT_PX = 256 + + +@dataclass(frozen=True) +class MeshLighting: + """3D mesh panel light — edit MESH_LIGHTING or DashboardLayout.mesh_lighting.""" + + azimuth_deg: float = -45.0 # finger outline + elevation_deg: float = -35.0 # tip to root + intensity: float = 75000.0 + ambient: float = 0.35 + diffuse: float = 0.65 + + @property + def direction(self) -> tuple[float, float, float]: + """Unit light direction derived from azimuth / elevation (degrees).""" + az = math.radians(self.azimuth_deg) + el = math.radians(self.elevation_deg) + cos_el = math.cos(el) + return (cos_el * math.cos(az), cos_el * math.sin(az), math.sin(el)) + + +MESH_LIGHTING = MeshLighting( + azimuth_deg=-45.0, + elevation_deg=0, + intensity=75000.0, + ambient=0.5, + diffuse=0.5, +) + +# 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, + force_strength_mode="log", + force_strength_gamma=0.5, + min_length_fraction=0.02, + max_length_fraction=0.30, + min_thickness_scale=0.20, + max_thickness_scale=1.0, + normal_force_scale=1.0, + color=(1.0, 0.92, 0.10), +) + + +@dataclass(frozen=True) +class LogConfig: + """Log panel — edit LOG_CONFIG or DashboardLayout.log_config.""" + + status_enabled: bool = True + max_lines: int = 2000 + font_family: str = "Consolas" + font_pt: int = 12 + show_flow: bool = True + show_mask: bool = True + + def format_status( + self, + *, + fps: float, + fnormal: float, + fshearx: float, + fsheary: float, + flow_rms: float = 0.0, + mask_coverage: float = 0.0, + wall_fps: float | None = None, + ) -> str: + shear_mag = float((fshearx**2 + fsheary**2) ** 0.5) + if wall_fps is not None: + parts = [f"FPS={wall_fps:.1f}", f"sdk={fps:.1f}"] + else: + parts = [f"FPS={fps:.1f}"] + parts.extend( + [ + f"Fn={fnormal:.1f}", + f"|Fs|={shear_mag:.1f}", + f"Fs=({fshearx:.1f}, {fsheary:.1f})", + ] + ) + return " ".join(parts) + + +LOG_CONFIG = LogConfig() + @dataclass(frozen=True) class DashboardLayout: @@ -100,7 +210,27 @@ class DashboardLayout: # 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 - timer_interval_ms: int = 10 + mesh_lighting: MeshLighting = MESH_LIGHTING + mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW + + log_config: LogConfig = LOG_CONFIG + + mesh_render_width_px: int = MESH_RENDER_WIDTH_PX + mesh_render_height_px: int = MESH_RENDER_HEIGHT_PX + + timer_interval_ms: int = 16 + mesh_async_render: bool = True + mesh_render_stride: int = 2 + chart_update_stride: int = 2 + + def mesh_display_size(self) -> tuple[int, int]: + """On-screen mesh panel size from TOP_ROW mesh slot.""" + slot = self.top_slot("mesh") + return slot.width_px, slot.height_px + + def mesh_render_size(self) -> tuple[int, int]: + """Open3D offscreen render target size (may differ from display size).""" + return self.mesh_render_width_px, self.mesh_render_height_px def slot(self, panel_id: PanelId) -> PanelSlot | None: if self.panel_setup.log_panel.id == panel_id: diff --git a/examples/gui/mesh_render_worker.py b/examples/gui/mesh_render_worker.py new file mode 100644 index 0000000..08373f1 --- /dev/null +++ b/examples/gui/mesh_render_worker.py @@ -0,0 +1,74 @@ +"""Background Open3D mesh rendering for the Qt dashboard.""" + +from __future__ import annotations + +import numpy as np +from PyQt5.QtCore import QMutex, QMutexLocker, QThread, pyqtSignal + + +class MeshRenderWorker(QThread): + """Render finger mesh off the GUI thread; drops stale frames.""" + + rgb_ready = pyqtSignal(object) + + def __init__(self, parent=None): + super().__init__(parent) + self._mutex = QMutex() + self._stop = False + self._wake = False + self._renderer = None + self._pending: dict | None = None + + def set_renderer(self, renderer) -> None: + with QMutexLocker(self._mutex): + self._renderer = renderer + + def clear_renderer(self) -> None: + with QMutexLocker(self._mutex): + self._renderer = None + self._pending = None + + def submit(self, *, flow, scalar_map, centroid, centroid_found, fnormal) -> None: + with QMutexLocker(self._mutex): + self._pending = { + "flow": flow, + "scalar_map": scalar_map, + "centroid": centroid, + "centroid_found": centroid_found, + "fnormal": fnormal, + } + self._wake = True + if not self.isRunning(): + self.start() + + def stop_worker(self) -> None: + with QMutexLocker(self._mutex): + self._stop = True + self._wake = True + self.wait(2000) + + def run(self) -> None: + while True: + with QMutexLocker(self._mutex): + if self._stop: + break + if not self._wake or self._renderer is None or self._pending is None: + self.msleep(1) + continue + job = self._pending + self._pending = None + self._wake = False + renderer = self._renderer + + rgb = renderer.render( + flow=job["flow"], + scalar_map=job["scalar_map"], + centroid=job["centroid"], + centroid_found=job["centroid_found"], + fnormal=job["fnormal"], + ) + self.rgb_ready.emit(np.ascontiguousarray(rgb)) + + with QMutexLocker(self._mutex): + if self._pending is not None: + self._wake = True diff --git a/examples/gui/widgets/force_charts.py b/examples/gui/widgets/force_charts.py index 23a6cc0..058b369 100644 --- a/examples/gui/widgets/force_charts.py +++ b/examples/gui/widgets/force_charts.py @@ -66,6 +66,7 @@ class ForceChartWidget: def _init_plot_buffer(self) -> None: self.data = np.full((self.npoints, 2, 3), np.nan, dtype=np.float64) self._plot_sample_count = 0 + self._time_origin: float | None = None self._y_range_smooth = [None, None, None] def _create_plots(self) -> None: @@ -134,6 +135,9 @@ class ForceChartWidget: plot.setXRange(0, self._plot_window_sec, padding=0) def update_frame(self, frame: FingerFrameView) -> None: + if self._time_origin is None: + self._time_origin = frame.timestamp + if self._plot_sample_count < self.npoints: idx = self._plot_sample_count self.data[idx, 0, :] = frame.timestamp @@ -151,36 +155,36 @@ class ForceChartWidget: self.data[-1, 1, 2] = frame.fsheary n = self._plot_sample_count - if n == 0: + if n == 0 or self._time_origin is None: return layout = self._layout - data_plot = self.data[:n].copy() - times = data_plot[:, 0, 0] - float(data_plot[0, 0, 0]) + data_plot = self.data[:n] + times = data_plot[:, 0, 0] - self._time_origin + x_end = float(times[-1]) + if x_end < self._plot_window_sec: + x_start = 0.0 + x_end = self._plot_window_sec + else: + x_start = x_end - self._plot_window_sec + for channel in range(3): - data_plot[:, 1, channel] = lowpass_filter( - data_plot[:, 1, channel], + y = lowpass_filter( + data_plot[:, 1, channel].copy(), self.lowpass_cutoff_hz, sample_rate_hz=self.sample_rate_hz, ) - x_end = float(times[-1]) - x_start = max(0.0, x_end - self._plot_window_sec) - if x_end - x_start < self._plot_window_sec: - x_start = 0.0 - x_end = max(self._plot_window_sec, x_end) - - for i, curve in enumerate(self.curves): - y = data_plot[:, 1, i] + curve = self.curves[channel] curve.setData(times, y) finite = np.isfinite(y) if not np.any(finite): continue ymin, ymax = float(np.min(y[finite])), float(np.max(y[finite])) - if self._y_range_smooth[i] is None: - self._y_range_smooth[i] = (ymin, ymax) + if self._y_range_smooth[channel] is None: + self._y_range_smooth[channel] = (ymin, ymax) else: - prev_min, prev_max = self._y_range_smooth[i] + prev_min, prev_max = self._y_range_smooth[channel] alpha = self._y_range_alpha ymin = alpha * ymin + (1 - alpha) * prev_min ymax = alpha * ymax + (1 - alpha) * prev_max @@ -190,13 +194,11 @@ class ForceChartWidget: half = layout.y_axis_min_span / 2 ymin = mid - half ymax = mid + half - self._y_range_smooth[i] = (ymin, ymax) + self._y_range_smooth[channel] = (ymin, ymax) pad = max((ymax - ymin) * 0.12, 0.01) - self.plots[i].setYRange(ymin - pad, ymax + pad) - last_x = times[-1:] - last_y = y[-1:] - self.dot_glows[i].setData(last_x, last_y) - self.dot_cores[i].setData(last_x, last_y) + self.plots[channel].setYRange(ymin - pad, ymax + pad) + self.dot_glows[channel].setData(times[-1:], y[-1:]) + self.dot_cores[channel].setData(times[-1:], y[-1:]) for plot in self.plots: plot.setXRange(x_start, x_end, padding=0) diff --git a/examples/gui/widgets/log_panel.py b/examples/gui/widgets/log_panel.py index d5b6c18..1431f9f 100644 --- a/examples/gui/widgets/log_panel.py +++ b/examples/gui/widgets/log_panel.py @@ -2,7 +2,7 @@ from __future__ import annotations -from PyQt5.QtGui import QFont +from PyQt5.QtGui import QFont, QTextCursor from PyQt5.QtWidgets import QPlainTextEdit @@ -22,30 +22,68 @@ class TeeStream: class WidgetLogHandler: - """Mirror print output into a QPlainTextEdit.""" + """Mirror print output into a QPlainTextEdit with a live status line at the bottom.""" def __init__(self, widget: QPlainTextEdit): self.widget = widget self.buf = "" + self._has_status_line = False def write(self, text: str): self.buf += text while "\n" in self.buf: line, self.buf = self.buf.split("\n", 1) if line: - self.widget.appendPlainText(line) + self._append_log_line(line) def flush(self): if self.buf: - self.widget.appendPlainText(self.buf) + self._append_log_line(self.buf) self.buf = "" + def set_status_line(self, text: str) -> None: + """Update the pinned bottom line (FPS / forces) without scrolling the log.""" + text = text.rstrip("\n") + if self._has_status_line: + cursor = QTextCursor(self.widget.document().lastBlock()) + cursor.select(QTextCursor.BlockUnderCursor) + cursor.insertText(text) + else: + self.widget.appendPlainText(text) + self._has_status_line = True + scrollbar = self.widget.verticalScrollBar() + scrollbar.setValue(scrollbar.maximum()) -def make_log_widget() -> QPlainTextEdit: + def clear_status_line(self) -> None: + if not self._has_status_line: + return + cursor = QTextCursor(self.widget.document().lastBlock()) + cursor.select(QTextCursor.BlockUnderCursor) + cursor.removeSelectedText() + self._has_status_line = False + + def _append_log_line(self, line: str) -> None: + status = None + if self._has_status_line: + status = self.widget.document().lastBlock().text() + cursor = QTextCursor(self.widget.document().lastBlock()) + cursor.select(QTextCursor.BlockUnderCursor) + cursor.removeSelectedText() + self._has_status_line = False + self.widget.appendPlainText(line) + if status is not None: + self.set_status_line(status) + + +def make_log_widget(config=None) -> QPlainTextEdit: + if config is None: + from ..layout import LOG_CONFIG + + config = LOG_CONFIG widget = QPlainTextEdit() widget.setReadOnly(True) - widget.setMaximumBlockCount(2000) - font = QFont("Consolas", 12) + widget.setMaximumBlockCount(config.max_lines) + font = QFont(config.font_family, config.font_pt) font.setStyleHint(QFont.Monospace) widget.setFont(font) return widget diff --git a/examples/gui/widgets/mesh_viewport.py b/examples/gui/widgets/mesh_viewport.py index 2eb83c2..2c2eb20 100644 --- a/examples/gui/widgets/mesh_viewport.py +++ b/examples/gui/widgets/mesh_viewport.py @@ -2,10 +2,13 @@ from __future__ import annotations +import sys + from pipeline.frame import FingerFrameView from viz.offscreen_mesh import OffscreenFingerMeshRenderer from ..layout import DashboardLayout, LAYOUT +from ..mesh_render_worker import MeshRenderWorker from .qt_pixmap import ndarray_to_pixmap from .image_panel import ImagePanel @@ -16,14 +19,15 @@ class MeshViewportWidget(ImagePanel): def __init__(self, parent=None, *, layout: DashboardLayout = LAYOUT): self._layout = layout self._renderer: OffscreenFingerMeshRenderer | None = None + self._mesh_worker: MeshRenderWorker | None = None self._mesh_map = "depth" self._threshold = 0.0 self._enable_3d = True - slot = layout.top_slot("mesh") + display_w, display_h = layout.mesh_display_size() super().__init__( title=layout.mesh_panel_title, object_name="meshViewport", - default_side=min(slot.width_px, slot.height_px), + default_side=min(display_w, display_h), parent=parent, ) @@ -31,6 +35,7 @@ class MeshViewportWidget(ImagePanel): self._mesh_map = pipeline.config.mesh_map self._threshold = pipeline.config.flow_threshold self._enable_3d = pipeline.config.enable_3d and pipeline.uv_mesh is not None + self._stop_worker() if self._renderer is not None: self._renderer.close() self._renderer = None @@ -42,28 +47,58 @@ class MeshViewportWidget(ImagePanel): return self.label.setText("") self._init_renderer(pipeline) + if self._layout.mesh_async_render and sys.platform != "win32": + self._mesh_worker = MeshRenderWorker(self) + self._mesh_worker.rgb_ready.connect(self._on_rgb_ready) + self._mesh_worker.set_renderer(self._renderer) def _init_renderer(self, pipeline) -> None: if not self._enable_3d or pipeline.uv_mesh is None: return - side = self.side_length + render_w, render_h = self._layout.mesh_render_size() self._renderer = OffscreenFingerMeshRenderer( pipeline.uv_mesh, - width=side, - height=side, + width=render_w, + height=render_h, threshold=self._threshold, color_mode=self._mesh_map, world_to_cam=pipeline.world_to_cam, lookat=pipeline.mesh_lookat, + mesh_lighting=self._layout.mesh_lighting, + contact_arrow_style=self._layout.mesh_contact_arrow, ) - def on_side_length_changed(self, side: int) -> None: - if self._renderer is not None: - self._renderer.resize(side, side) + def _stop_worker(self) -> None: + if self._mesh_worker is not None: + self._mesh_worker.stop_worker() + self._mesh_worker = None - def update_frame(self, frame: FingerFrameView) -> None: + def on_side_length_changed(self, side: int) -> None: + """Display widget resized; offscreen render size stays on layout.mesh_render_*.""" + del side # display size only; ImagePanel upscales the pixmap to fit the label + + def _on_rgb_ready(self, rgb) -> None: + self.set_source_pixmap(ndarray_to_pixmap(rgb, bgr=False)) + + def update_frame(self, frame: FingerFrameView, *, async_render: bool | None = None) -> float: + """Update mesh colors. Returns render wall time (0 when async submit only).""" if not self._enable_3d or self._renderer is None: - return + return 0.0 + + use_async = self._layout.mesh_async_render if async_render is None else async_render + if use_async and self._mesh_worker is not None: + self._mesh_worker.submit( + flow=frame.flow, + scalar_map=frame.depth_map, + centroid=frame.centroid, + centroid_found=frame.centroid_found, + fnormal=frame.fnormal, + ) + return 0.0 + + import time + + t0 = time.perf_counter() rgb = self._renderer.render( flow=frame.flow, scalar_map=frame.depth_map, @@ -72,8 +107,10 @@ class MeshViewportWidget(ImagePanel): fnormal=frame.fnormal, ) self.set_source_pixmap(ndarray_to_pixmap(rgb, bgr=False)) + return (time.perf_counter() - t0) * 1000.0 def close_renderer(self) -> None: + self._stop_worker() if self._renderer is not None: self._renderer.close() self._renderer = None diff --git a/examples/pipeline/config.py b/examples/pipeline/config.py index f425032..c6abf08 100644 --- a/examples/pipeline/config.py +++ b/examples/pipeline/config.py @@ -23,9 +23,11 @@ class PipelineConfig: threshold: float | None = None viz_pose: str = DEFAULT_FINGER_VIZ_POSE enable_3d: bool = True - use_color: bool = True + use_color: bool = False lowpass_cutoff_hz: float = 5.0 force_sample_rate_hz: float = 30.0 + profile: bool = False + profile_every: int = 30 @classmethod def from_args(cls, args) -> "PipelineConfig": @@ -39,6 +41,8 @@ class PipelineConfig: viz_pose=args.viz_pose, enable_3d=not args.no_3d, lowpass_cutoff_hz=getattr(args, "cutoff", 5.0), + profile=getattr(args, "profile", False), + profile_every=getattr(args, "profile_every", 30), ) @property diff --git a/examples/pipeline/profiling.py b/examples/pipeline/profiling.py new file mode 100644 index 0000000..d06ebc4 --- /dev/null +++ b/examples/pipeline/profiling.py @@ -0,0 +1,87 @@ +"""Shared stage timing helpers for finger example pipelines.""" + +from __future__ import annotations + +import time +from collections import defaultdict + + +class StageProfiler: + """Accumulate per-stage wall times and print rolling averages.""" + + DEFAULT_ORDER = ( + "capture", + "uv_unroll", + "flow", + "hhd", + "contact", + "read_info", + "pipeline", + "mesh", + "arrows", + "charts", + "display", + "total", + ) + + def __init__(self, *, report_every: int = 30, stage_order: tuple[str, ...] | None = None): + self.report_every = max(1, int(report_every)) + self.stage_order = stage_order or self.DEFAULT_ORDER + self.frame_count = 0 + self.totals: dict[str, float] = defaultdict(float) + self.last: dict[str, float] = {} + + def record(self, name: str, ms: float) -> None: + self.totals[name] += ms + self.last[name] = ms + + def tick(self) -> None: + self.frame_count += 1 + if self.frame_count % self.report_every == 0: + self.print_report(prefix=f"[profile {self.frame_count} frames]") + + def print_report(self, *, prefix: str = "[profile summary]") -> None: + if self.frame_count == 0: + return + n = self.frame_count + parts = [] + for name in self.stage_order: + if name in self.totals: + parts.append(f"{name}={self.totals[name] / n:.2f}ms") + total_ms = self.totals.get("total", 0.0) / n + if total_ms > 0: + parts.append(f"fps={1000.0 / total_ms:.1f}") + print(f"\n{prefix} " + " | ".join(parts)) + + +def run_deformation_timed(tactile, *, check_motion=True, threshold=0, tail_frames=10): + """Run flow + gated HHD; return (flow_ms, hhd_ms). Mirrors Sensor.compute_deformation.""" + sensor = tactile.raw_sensor + + t0 = time.perf_counter() + sensor.compute_flow() + flow_ms = (time.perf_counter() - t0) * 1000.0 + + run_hhd = True + if check_motion: + is_motion = sensor.detect_motion(threshold=threshold) + try: + tail_frames = max(0, int(tail_frames)) + except (TypeError, ValueError): + tail_frames = 0 + + if is_motion: + sensor.tail_frames_count = 0 + elif sensor.tail_frames_count < tail_frames: + sensor.tail_frames_count += 1 + else: + run_hhd = False + sensor.tail_frames_count = tail_frames + + hhd_ms = 0.0 + if run_hhd: + t1 = time.perf_counter() + sensor.compute_hhd() + hhd_ms = (time.perf_counter() - t1) * 1000.0 + + return flow_ms, hhd_ms diff --git a/examples/pipeline/service.py b/examples/pipeline/service.py index c5ed034..11c9dc3 100644 --- a/examples/pipeline/service.py +++ b/examples/pipeline/service.py @@ -3,6 +3,7 @@ from __future__ import annotations import time +from dataclasses import dataclass import numpy as np from orisys.finger import FingerRuntime @@ -11,6 +12,18 @@ from viz import FrameRecorder, build_uv_surface_mesh, resolve_viz_pose_path from .config import PipelineConfig from .frame import FingerFrameView +from .profiling import StageProfiler, run_deformation_timed + + +@dataclass +class TickTimings: + capture_ms: float = 0.0 + uv_unroll_ms: float = 0.0 + flow_ms: float = 0.0 + hhd_ms: float = 0.0 + contact_ms: float = 0.0 + read_info_ms: float = 0.0 + pipeline_ms: float = 0.0 class FingerPipeline: @@ -24,6 +37,9 @@ class FingerPipeline: self.uv_mesh = None self.world_to_cam: np.ndarray | None = None self.mesh_lookat: np.ndarray | None = None + self.profiler: StageProfiler | None = ( + StageProfiler(report_every=config.profile_every) if config.profile else None + ) @property def is_open(self) -> bool: @@ -60,6 +76,8 @@ class FingerPipeline: self.mesh_lookat = self.uv_mesh.rest_vertices.mean(axis=0) def close(self) -> None: + if self.profiler is not None and self.profiler.frame_count: + self.profiler.print_report(prefix="[profile final]") if self.runtime is not None: self.runtime.close() self.runtime = None @@ -69,33 +87,17 @@ class FingerPipeline: if self.runtime is not None: self.runtime.tactile.reset() - def tick(self) -> FingerFrameView | None: - if self.runtime is None or self._start_time is None: - return None - - ok, unrolled = self.runtime.grab_and_process(color=self.config.use_color) - if not ok: - return None - - tactile = self.runtime.tactile - self.recorder.write_if_recording(tactile.frame) - - tactile.compute_deformation(decompose=True, check_motion=True, threshold=0) - tactile.compute_contact() - - fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = tactile.read_info( + 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.IMG, - tactile.info.CONTOUR, - tactile.info.CENTROID, 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), @@ -103,8 +105,8 @@ class FingerPipeline: flow=flow, vnormal=vnormal, depth_map=depth_map, - img=img, - contour=contour, + img=np.array([]), + contour=np.array([]), centroid=centroid, fnormal=float(fnormal), fshearx=float(fshearx), @@ -114,6 +116,72 @@ class FingerPipeline: raw_frame=tactile.frame, ) + def tick(self) -> FingerFrameView | None: + frame, _timings = self.tick_timed() + return frame + + def tick_timed(self) -> tuple[FingerFrameView | None, TickTimings]: + timings = TickTimings() + if self.runtime is None or self._start_time is None: + return None, timings + + pipeline_t0 = time.perf_counter() + profile = self.profiler is not None + + if profile: + t0 = time.perf_counter() + ok, frame = self.runtime.read_source() + timings.capture_ms = (time.perf_counter() - t0) * 1000.0 + if not ok: + return None, timings + t0 = time.perf_counter() + unrolled = self.runtime.process_frame(frame, color=self.config.use_color) + timings.uv_unroll_ms = (time.perf_counter() - t0) * 1000.0 + else: + ok, unrolled = self.runtime.grab_and_process(color=self.config.use_color) + if not ok: + return None, timings + + tactile = self.runtime.tactile + self.recorder.write_if_recording(tactile.frame) + + if profile: + flow_ms, hhd_ms = run_deformation_timed( + tactile, check_motion=True, threshold=0 + ) + timings.flow_ms = flow_ms + timings.hhd_ms = hhd_ms + self.profiler.record("capture", timings.capture_ms) + self.profiler.record("uv_unroll", timings.uv_unroll_ms) + self.profiler.record("flow", timings.flow_ms) + self.profiler.record("hhd", timings.hhd_ms) + else: + tactile.compute_deformation(decompose=True, check_motion=True, threshold=0) + + t0 = time.perf_counter() + tactile.compute_contact() + timings.contact_ms = (time.perf_counter() - t0) * 1000.0 + if profile: + self.profiler.record("contact", timings.contact_ms) + + t0 = time.perf_counter() + view = self._read_frame_view(tactile, unrolled) + timings.read_info_ms = (time.perf_counter() - t0) * 1000.0 + if profile: + self.profiler.record("read_info", timings.read_info_ms) + + timings.pipeline_ms = (time.perf_counter() - pipeline_t0) * 1000.0 + if profile: + self.profiler.record("pipeline", timings.pipeline_ms) + + return view, timings + + def record_total_frame(self, total_ms: float) -> None: + if self.profiler is None: + return + self.profiler.record("total", total_ms) + self.profiler.tick() + def start_recording(self) -> bool: if self.runtime is None: return False diff --git a/examples/viz/contact_arrow.py b/examples/viz/contact_arrow.py index 02cd92b..75a7f81 100644 --- a/examples/viz/contact_arrow.py +++ b/examples/viz/contact_arrow.py @@ -2,17 +2,90 @@ from __future__ import annotations +from dataclasses import dataclass, replace +from typing import Literal + import numpy as np -from orisys.finger.mesh import UVSurfaceMesh, compute_uv_surface_frames, mesh_uv_to_world_scale +from orisys.finger.mesh import UVSurfaceMesh, compute_uv_surface_frames -DEFAULT_NORMAL_FORCE_SCALE = 1.0 -MIN_ARROW_LENGTH_FRACTION = 0.02 -MAX_ARROW_LENGTH_FRACTION = 0.30 -REFERENCE_NORMAL_FORCE = 10000.0 -MIN_ARROW_THICKNESS_SCALE = 0.20 -MAX_ARROW_THICKNESS_SCALE = 1.0 -CONTACT_ARROW_COLOR = (1.0, 0.92, 0.10) +ForceStrengthMode = Literal["log", "power", "linear"] + + +@dataclass(frozen=True) +class ContactArrowStyle: + """Visual tuning for the 3D contact normal arrow (dashboard: edit MESH_CONTACT_ARROW in layout.py).""" + + normal_force_scale: float = 1.0 + min_length_fraction: float = 0.02 + max_length_fraction: float = 0.30 + min_thickness_scale: float = 0.20 + max_thickness_scale: float = 1.0 + reference_normal_force: float = 10000.0 + force_strength_mode: ForceStrengthMode = "log" + force_strength_gamma: float = 0.5 + color: tuple[float, float, float] = (1.0, 0.92, 0.10) + + +DEFAULT_CONTACT_ARROW_STYLE = ContactArrowStyle() + +# Backward-compatible aliases (match DEFAULT_CONTACT_ARROW_STYLE). +DEFAULT_NORMAL_FORCE_SCALE = DEFAULT_CONTACT_ARROW_STYLE.normal_force_scale +MIN_ARROW_LENGTH_FRACTION = DEFAULT_CONTACT_ARROW_STYLE.min_length_fraction +MAX_ARROW_LENGTH_FRACTION = DEFAULT_CONTACT_ARROW_STYLE.max_length_fraction +MIN_ARROW_THICKNESS_SCALE = DEFAULT_CONTACT_ARROW_STYLE.min_thickness_scale +MAX_ARROW_THICKNESS_SCALE = DEFAULT_CONTACT_ARROW_STYLE.max_thickness_scale +REFERENCE_NORMAL_FORCE = DEFAULT_CONTACT_ARROW_STYLE.reference_normal_force +FORCE_STRENGTH_MODE = DEFAULT_CONTACT_ARROW_STYLE.force_strength_mode +FORCE_STRENGTH_GAMMA = DEFAULT_CONTACT_ARROW_STYLE.force_strength_gamma +CONTACT_ARROW_COLOR = DEFAULT_CONTACT_ARROW_STYLE.color + + +def force_visual_strength( + force: float, + *, + style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE, +) -> float: + """Map normal force Fn to a unitless strength in [0, 1] for length and thickness.""" + reference = style.reference_normal_force + if force <= 0.0 or reference <= 0.0: + return 0.0 + + ratio = force / reference + mode = style.force_strength_mode + + if mode == "linear": + strength = ratio + elif mode == "power": + strength = ratio ** style.force_strength_gamma + else: # log + strength = np.log1p(ratio) / np.log1p(1.0) + + return float(np.clip(strength, 0.0, 1.0)) + + +def thickness_scale_from_force( + force: float, + *, + style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE, +) -> float: + """Radial scale from Fn using the shared force_strength curve.""" + strength = force_visual_strength(force, style=style) + span = style.max_thickness_scale - style.min_thickness_scale + return float(style.min_thickness_scale + span * strength) + + +def length_from_force( + force: float, + mesh_span: float, + *, + style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE, +) -> float: + """Arrow length in world units from Fn and mesh extent.""" + strength = force_visual_strength(force, style=style) + frac_span = style.max_length_fraction - style.min_length_fraction + fraction = style.min_length_fraction + frac_span * strength + return float(mesh_span * fraction * style.normal_force_scale) def _mesh_span(vertices: np.ndarray) -> float: @@ -47,20 +120,6 @@ def _rotation_matrix_z_to(direction: np.ndarray) -> np.ndarray: return np.eye(3, dtype=np.float64) + skew + skew @ skew * (1.0 / (1.0 + dot)) -def thickness_scale_from_force(force: float) -> float: - """Map normal force to a radial scale factor (thin at low force, thicker at high force).""" - if force <= 0.0: - return 0.0 - t = force / REFERENCE_NORMAL_FORCE - return float( - np.clip( - MIN_ARROW_THICKNESS_SCALE + (MAX_ARROW_THICKNESS_SCALE - MIN_ARROW_THICKNESS_SCALE) * t, - MIN_ARROW_THICKNESS_SCALE, - MAX_ARROW_THICKNESS_SCALE, - ) - ) - - def _scale_arrow_template( template_vertices: np.ndarray, *, @@ -78,8 +137,8 @@ def _scale_arrow_template( def create_arrow_template(o3d): """Unit-height arrow mesh along +Z; radial size is scaled per-frame from force.""" arrow = o3d.geometry.TriangleMesh.create_arrow( - cylinder_radius=0.03, - cone_radius=0.06, + cylinder_radius=0.15, + cone_radius=0.3, cylinder_height=0.72, cone_height=0.28, ) @@ -99,19 +158,26 @@ def build_contact_normal_arrow_mesh( mesh_span: float, template_vertices: np.ndarray, template_triangles: np.ndarray, + style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE, ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: - """Build a yellow 3D arrow mesh pointing opposite the surface normal.""" + """Build a 3D arrow mesh pointing opposite the surface normal.""" origin = np.asarray(origin, dtype=np.float64).reshape(3) normal = np.asarray(normal, dtype=np.float64).reshape(3) norm = np.linalg.norm(normal) - color = np.tile(np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64), (len(template_vertices), 1)) + color = np.tile(np.asarray(style.color, dtype=np.float64), (len(template_vertices), 1)) - radial_scale = thickness_scale_from_force(force) + radial_scale = thickness_scale_from_force(force, style=style) if norm < 1e-8 or length <= 0.0 or radial_scale <= 0.0 or not np.all(np.isfinite(origin)): return np.zeros_like(template_vertices), template_triangles, color direction = -normal / norm - length = float(np.clip(length, mesh_span * MIN_ARROW_LENGTH_FRACTION, mesh_span * MAX_ARROW_LENGTH_FRACTION)) + length = float( + np.clip( + length, + mesh_span * style.min_length_fraction, + mesh_span * style.max_length_fraction, + ) + ) if length <= 0.0: return np.zeros_like(template_vertices), template_triangles, color @@ -125,13 +191,19 @@ def build_contact_normal_arrow_mesh( return vertices, template_triangles, color -def create_arrow_mesh_geometry(o3d, template_vertices: np.ndarray, template_triangles: np.ndarray): +def create_arrow_mesh_geometry( + o3d, + template_vertices: np.ndarray, + template_triangles: np.ndarray, + *, + style: ContactArrowStyle = DEFAULT_CONTACT_ARROW_STYLE, +): """Create a persistent Open3D mesh for the contact arrow overlay.""" arrow_mesh = o3d.geometry.TriangleMesh() arrow_mesh.vertices = o3d.utility.Vector3dVector(np.zeros_like(template_vertices)) arrow_mesh.triangles = o3d.utility.Vector3iVector(template_triangles) arrow_mesh.vertex_colors = o3d.utility.Vector3dVector( - np.tile(np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64), (len(template_vertices), 1)) + np.tile(np.asarray(style.color, dtype=np.float64), (len(template_vertices), 1)) ) arrow_mesh.compute_vertex_normals() return arrow_mesh @@ -144,26 +216,38 @@ class ContactArrowOverlay: self, mesh: UVSurfaceMesh, *, - normal_force_scale: float = DEFAULT_NORMAL_FORCE_SCALE, + style: ContactArrowStyle | None = None, + normal_force_scale: float | None = None, enabled: bool = True, ): import open3d as o3d self.mesh = mesh - self.normal_force_scale = float(normal_force_scale) + base_style = style or DEFAULT_CONTACT_ARROW_STYLE + if normal_force_scale is not None: + base_style = replace(base_style, normal_force_scale=float(normal_force_scale)) + self.style = base_style self.enabled = enabled self._mesh_span = _mesh_span(mesh.rest_vertices) - self._pixel_to_world = mesh_uv_to_world_scale(mesh.world_xyz, mesh.valid_mask) _, _, self._surface_normals = compute_uv_surface_frames(mesh.world_xyz, mesh.valid_mask) self._template_vertices, self._template_triangles = ( create_arrow_template(o3d) if enabled else (None, None) ) self.arrow_mesh = ( - create_arrow_mesh_geometry(o3d, self._template_vertices, self._template_triangles) + create_arrow_mesh_geometry( + o3d, + self._template_vertices, + self._template_triangles, + style=self.style, + ) if enabled else None ) + @property + def normal_force_scale(self) -> float: + return self.style.normal_force_scale + def compute( self, centroid, @@ -173,12 +257,10 @@ class ContactArrowOverlay: if not self.enabled or self.arrow_mesh is None: return None + style = self.style template_vertices = self._template_vertices template_triangles = self._template_triangles - color = np.tile( - np.asarray(CONTACT_ARROW_COLOR, dtype=np.float64), - (len(template_vertices), 1), - ) + color = np.tile(np.asarray(style.color, dtype=np.float64), (len(template_vertices), 1)) hidden = (np.zeros_like(template_vertices), template_triangles, color) if not centroid_found or centroid is None: @@ -204,7 +286,7 @@ class ContactArrowOverlay: if force <= 0.0: return hidden - length = force * self.normal_force_scale * self._pixel_to_world + length = length_from_force(force, self._mesh_span, style=style) return build_contact_normal_arrow_mesh( origin, normal, @@ -213,6 +295,7 @@ class ContactArrowOverlay: mesh_span=self._mesh_span, template_vertices=template_vertices, template_triangles=template_triangles, + style=style, ) def apply(self, arrow_vertices: np.ndarray, arrow_colors: np.ndarray) -> None: diff --git a/examples/viz/finger_mesh_viewer.py b/examples/viz/finger_mesh_viewer.py index 3b455fd..7f479d6 100644 --- a/examples/viz/finger_mesh_viewer.py +++ b/examples/viz/finger_mesh_viewer.py @@ -4,7 +4,7 @@ from __future__ import annotations from orisys.finger.viewer import UVMeshViewer -from .contact_arrow import ContactArrowOverlay, DEFAULT_NORMAL_FORCE_SCALE +from .contact_arrow import ContactArrowOverlay, ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE class FingerMeshViewer(UVMeshViewer): @@ -21,7 +21,8 @@ class FingerMeshViewer(UVMeshViewer): value_max=None, window_width: int = 1024, window_height: int = 1024, - normal_force_scale: float = DEFAULT_NORMAL_FORCE_SCALE, + contact_arrow_style: ContactArrowStyle | None = None, + normal_force_scale: float | None = None, show_contact_arrow: bool = True, ): super().__init__( @@ -36,6 +37,7 @@ class FingerMeshViewer(UVMeshViewer): ) self._contact_arrow = ContactArrowOverlay( mesh, + style=contact_arrow_style or DEFAULT_CONTACT_ARROW_STYLE, normal_force_scale=normal_force_scale, enabled=show_contact_arrow, ) diff --git a/examples/viz/mesh_camera.py b/examples/viz/mesh_camera.py index 9041395..9c2f8e0 100644 --- a/examples/viz/mesh_camera.py +++ b/examples/viz/mesh_camera.py @@ -97,7 +97,7 @@ def apply_view_from_world_to_cam(vis, world_to_cam: np.ndarray, *, lookat: np.nd def apply_mesh_lighting(vis) -> None: - """Enable shaded lighting so the tactile mesh reads as a 3D surface.""" + """Enable shaded lighting on the legacy Open3D Visualizer.""" import open3d as o3d render_option = vis.get_render_option() @@ -108,3 +108,74 @@ def apply_mesh_lighting(vis) -> None: if hasattr(render_option, "mesh_shade_option"): render_option.mesh_shade_option = o3d.visualization.MeshShadeOption.Color vis.update_renderer() + + +def normalize_light_direction(direction) -> np.ndarray: + d = np.asarray(direction, dtype=np.float64).reshape(3) + norm = float(np.linalg.norm(d)) + if norm <= 0: + return np.array([0.0, 0.0, -1.0], dtype=np.float64) + return d / norm + + +def apply_filament_sun_light(renderer, *, direction, intensity: float) -> None: + """Configure Filament sun light on an OffscreenRenderer.""" + scene = renderer.scene + if not hasattr(scene, "scene"): + return + filament = scene.scene + sun_dir = normalize_light_direction(direction).tolist() + if hasattr(filament, "set_sun_light"): + filament.set_sun_light(sun_dir, [1.0, 1.0, 1.0], float(intensity)) + if hasattr(filament, "enable_sun_light"): + filament.enable_sun_light(True) + + +def compute_vertex_normals(vertices: np.ndarray, triangles: np.ndarray) -> np.ndarray: + """Per-vertex normals for a triangle mesh (vectorized).""" + vertices = np.asarray(vertices, dtype=np.float64) + triangles = np.asarray(triangles, dtype=np.int32) + v0 = vertices[triangles[:, 0]] + v1 = vertices[triangles[:, 1]] + v2 = vertices[triangles[:, 2]] + face_normals = np.cross(v1 - v0, v2 - v0) + normals = np.zeros_like(vertices) + np.add.at(normals, triangles[:, 0], face_normals) + np.add.at(normals, triangles[:, 1], face_normals) + np.add.at(normals, triangles[:, 2], face_normals) + lengths = np.linalg.norm(normals, axis=1) + valid = lengths > 1e-12 + normals[valid] /= lengths[valid, np.newaxis] + normals[~valid] = np.array([0.0, 0.0, 1.0], dtype=np.float64) + return normals + + +def precompute_shade_factors(vertices: np.ndarray, triangles: np.ndarray, lighting) -> np.ndarray: + """Per-vertex shade multipliers from static mesh geometry and layout lighting.""" + normals = compute_vertex_normals(vertices, triangles) + d = normalize_light_direction(lighting.direction) + ndotl = np.clip(normals @ d, 0.0, 1.0) + return float(lighting.ambient) + float(lighting.diffuse) * ndotl + + +def apply_shade_factors(colors: np.ndarray, shade_factors: np.ndarray | None) -> np.ndarray: + if shade_factors is None: + return colors + return np.clip(np.asarray(colors, dtype=np.float64) * shade_factors[:, np.newaxis], 0.0, 1.0) + + +def shade_vertex_colors( + colors: np.ndarray, + normals: np.ndarray, + *, + direction, + ambient: float, + diffuse: float, +) -> np.ndarray: + """Apply directional shading to vertex colors (legacy viewer path).""" + colors = np.asarray(colors, dtype=np.float64) + normals = np.asarray(normals, dtype=np.float64) + d = normalize_light_direction(direction) + ndotl = np.clip(normals @ d, 0.0, 1.0) + shade = float(ambient) + float(diffuse) * ndotl + return np.clip(colors * shade[:, np.newaxis], 0.0, 1.0) diff --git a/examples/viz/offscreen_mesh.py b/examples/viz/offscreen_mesh.py index 4c2bfde..5f3267c 100644 --- a/examples/viz/offscreen_mesh.py +++ b/examples/viz/offscreen_mesh.py @@ -10,7 +10,13 @@ from orisys.finger.mesh import UVSurfaceMesh from orisys.finger.viewer import flow_magnitude_vertex_colors, scalar_map_vertex_colors from .contact_arrow import ContactArrowOverlay -from .mesh_camera import apply_view_from_world_to_cam, camera_look_at_from_world_to_cam +from .mesh_camera import ( + apply_filament_sun_light, + apply_shade_factors, + apply_view_from_world_to_cam, + camera_look_at_from_world_to_cam, + precompute_shade_factors, +) def _numpy_o3d_image_to_rgb(image) -> np.ndarray: @@ -22,13 +28,17 @@ def _numpy_o3d_image_to_rgb(image) -> np.ndarray: return arr.astype(np.uint8) -def _make_unlit_material(o3d): +def _make_lit_material(o3d): material = o3d.visualization.rendering.MaterialRecord() - material.shader = "defaultUnlit" + material.shader = "defaultLit" material.base_color = (1.0, 1.0, 1.0, 1.0) return material +def _shade_vertex_colors(mesh, colors: np.ndarray, shade_factors) -> np.ndarray: + return apply_shade_factors(colors, shade_factors) + + def _mesh_to_o3d(o3d, vertices: np.ndarray, triangles: np.ndarray, colors: np.ndarray): mesh = o3d.geometry.TriangleMesh() mesh.vertices = o3d.utility.Vector3dVector(vertices.astype(np.float64)) @@ -70,6 +80,8 @@ class _FilamentOffscreenBackend: world_to_cam: np.ndarray | None, lookat: np.ndarray, show_contact_arrow: bool, + mesh_lighting=None, + contact_arrow_style=None, ): import open3d as o3d @@ -83,10 +95,26 @@ class _FilamentOffscreenBackend: self.value_max = value_max self.world_to_cam = world_to_cam self.lookat = lookat - self._contact_arrow = ContactArrowOverlay(mesh, enabled=show_contact_arrow) + self.mesh_lighting = mesh_lighting + self._shade_factors = ( + precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting) + if mesh_lighting is not None + else None + ) + self._contact_arrow = ContactArrowOverlay( + mesh, + enabled=show_contact_arrow, + style=contact_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._material = _make_unlit_material(o3d) + if mesh_lighting is not None: + apply_filament_sun_light( + self._renderer, + direction=mesh_lighting.direction, + intensity=mesh_lighting.intensity, + ) + self._material = _make_lit_material(o3d) self._mesh_added = False self._arrow_added = False self._apply_camera() @@ -111,6 +139,13 @@ class _FilamentOffscreenBackend: 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]) + if self.mesh_lighting is not None: + apply_filament_sun_light( + self._renderer, + direction=self.mesh_lighting.direction, + intensity=self.mesh_lighting.intensity, + ) + self._material = _make_lit_material(self._o3d) self._mesh_added = False self._arrow_added = False self._apply_camera() @@ -172,6 +207,8 @@ class _HiddenVisualizerBackend: world_to_cam: np.ndarray | None, lookat: np.ndarray, show_contact_arrow: bool, + mesh_lighting=None, + contact_arrow_style=None, ): import open3d as o3d @@ -185,7 +222,17 @@ class _HiddenVisualizerBackend: self.value_max = value_max self.world_to_cam = world_to_cam self.lookat = lookat - self._contact_arrow = ContactArrowOverlay(mesh, enabled=show_contact_arrow) + self.mesh_lighting = mesh_lighting + self._shade_factors = ( + precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting) + if mesh_lighting is not None + else None + ) + self._contact_arrow = ContactArrowOverlay( + mesh, + enabled=show_contact_arrow, + style=contact_arrow_style, + ) self.vis = None self.tri_mesh = None self._create_window() @@ -202,6 +249,7 @@ class _HiddenVisualizerBackend: value_min=self.value_min, value_max=self.value_max, ) + colors = _shade_vertex_colors(self.mesh, colors, self._shade_factors) self.vis = o3d.visualization.Visualizer() self.vis.create_window( window_name="Orisys Finger Mesh (embedded)", @@ -222,9 +270,9 @@ class _HiddenVisualizerBackend: 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]) if hasattr(render_option, "light_on"): render_option.light_on = False - render_option.background_color = np.array([0.05, 0.05, 0.08]) if self.world_to_cam is not None: apply_view_from_world_to_cam(self.vis, self.world_to_cam, lookat=self.lookat) @@ -261,11 +309,9 @@ class _HiddenVisualizerBackend: flow=flow, scalar_map=scalar_map, ) - vertices = self.mesh.rest_vertices + colors = _shade_vertex_colors(self.mesh, colors, self._shade_factors) - self.tri_mesh.vertices = o3d.utility.Vector3dVector(vertices.astype(np.float64)) self.tri_mesh.vertex_colors = o3d.utility.Vector3dVector(colors.astype(np.float64)) - self.tri_mesh.compute_vertex_normals() self.vis.update_geometry(self.tri_mesh) arrow_result = self._contact_arrow.compute(centroid, centroid_found, fnormal) @@ -293,7 +339,6 @@ def _create_backend(mesh: UVSurfaceMesh, **kwargs): except RuntimeError: return _HiddenVisualizerBackend(mesh, **kwargs) - class OffscreenFingerMeshRenderer: """Render finger UV mesh + contact arrow to an RGB image for Qt embedding.""" @@ -310,6 +355,8 @@ class OffscreenFingerMeshRenderer: world_to_cam: np.ndarray | None = None, lookat: np.ndarray | None = None, show_contact_arrow: bool = True, + mesh_lighting=None, + contact_arrow_style=None, ): lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0) self._backend = _create_backend( @@ -323,6 +370,8 @@ class OffscreenFingerMeshRenderer: world_to_cam=world_to_cam, lookat=lookat, show_contact_arrow=show_contact_arrow, + mesh_lighting=mesh_lighting, + contact_arrow_style=contact_arrow_style, ) @property diff --git a/tests/test_contact_arrow_scale.py b/tests/test_contact_arrow_scale.py new file mode 100644 index 0000000..45d6f08 --- /dev/null +++ b/tests/test_contact_arrow_scale.py @@ -0,0 +1,42 @@ +import sys +from pathlib import Path + +_EXAMPLES = Path(__file__).resolve().parents[1] / "examples" +if str(_EXAMPLES) not in sys.path: + sys.path.insert(0, str(_EXAMPLES)) + +from viz.contact_arrow import ( + ContactArrowStyle, + DEFAULT_CONTACT_ARROW_STYLE, + force_visual_strength, + length_from_force, + thickness_scale_from_force, +) + +REFERENCE_NORMAL_FORCE = DEFAULT_CONTACT_ARROW_STYLE.reference_normal_force + + +def test_force_visual_strength_monotonic(): + forces = [0.0, 50.0, 175.0, 1000.0, 5000.0, REFERENCE_NORMAL_FORCE, 20000.0] + strengths = [force_visual_strength(f) for f in forces] + assert strengths[0] == 0.0 + assert all(strengths[i] <= strengths[i + 1] for i in range(len(strengths) - 1)) + assert strengths[-1] == 1.0 + + +def test_log_mode_more_sensitive_than_linear_at_low_force(): + low = 175.0 + log_style = ContactArrowStyle(force_strength_mode="log") + lin_style = ContactArrowStyle(force_strength_mode="linear") + assert force_visual_strength(low, style=log_style) > force_visual_strength(low, style=lin_style) + + +def test_length_and_thickness_track_same_strength(): + force = 2500.0 + mesh_span = 10.0 + strength = force_visual_strength(force) + length = length_from_force(force, mesh_span) + thick = thickness_scale_from_force(force) + assert length > mesh_span * 0.02 + assert thick > 0.20 + assert strength == force_visual_strength(force)