1. tuned arrow mesh drawing

2. centralixzed arrow config in layout.py
3. seperate mesh render and display resolution to speed up (~40 fps)
This commit is contained in:
ykcheungab
2026-06-19 14:28:28 +08:00
parent 3fd9b839a9
commit 2e5c047d1e
19 changed files with 980 additions and 147 deletions
Binary file not shown.
+5 -5
View File
@@ -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
+82 -14
View File
@@ -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()
+11
View File
@@ -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()
+14 -2
View File
@@ -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",
+66 -11
View File
@@ -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)
+131 -1
View File
@@ -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:
+74
View File
@@ -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
+24 -22
View File
@@ -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)
+45 -7
View File
@@ -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
+47 -10
View File
@@ -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
+5 -1
View File
@@ -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
+87
View File
@@ -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
+89 -21
View File
@@ -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
+122 -39
View File
@@ -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:
+4 -2
View File
@@ -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,
)
+72 -1
View File
@@ -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)
+60 -11
View File
@@ -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
+42
View File
@@ -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)