added mesh surface arrows
This commit is contained in:
+68
-29
@@ -6,7 +6,7 @@ import sys
|
|||||||
import time
|
import time
|
||||||
from pathlib import Path
|
from pathlib import Path
|
||||||
|
|
||||||
from PyQt5.QtCore import Qt, QTimer
|
from PyQt5.QtCore import QSize, Qt, QTimer
|
||||||
from PyQt5.QtGui import QIcon
|
from PyQt5.QtGui import QIcon
|
||||||
from PyQt5.QtWidgets import (
|
from PyQt5.QtWidgets import (
|
||||||
QApplication,
|
QApplication,
|
||||||
@@ -31,6 +31,7 @@ from .layout import (
|
|||||||
top_row_splitter_sizes,
|
top_row_splitter_sizes,
|
||||||
)
|
)
|
||||||
from .styles import STYLE
|
from .styles import STYLE
|
||||||
|
from .transport_icons import transport_icon
|
||||||
from .widgets import (
|
from .widgets import (
|
||||||
ArrowPanelWidget,
|
ArrowPanelWidget,
|
||||||
ForceChartWidget,
|
ForceChartWidget,
|
||||||
@@ -41,6 +42,15 @@ from .widgets import (
|
|||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
_PLAY_PAUSE_BUTTON_NAME = "btnPlayPause"
|
||||||
|
_PLAY_PAUSE_ICON_PX = 18
|
||||||
|
_PLAY_PAUSE_ICON_COLORS = {
|
||||||
|
"run": "#4caf84",
|
||||||
|
"starting": "#555555",
|
||||||
|
"pause": "#ff9800",
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
class FingerDashboardWindow(QMainWindow):
|
class FingerDashboardWindow(QMainWindow):
|
||||||
"""Single-window finger demo: 3D mesh | UV arrows | force charts."""
|
"""Single-window finger demo: 3D mesh | UV arrows | force charts."""
|
||||||
|
|
||||||
@@ -88,14 +98,14 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
bar = QHBoxLayout()
|
bar = QHBoxLayout()
|
||||||
bar.setSpacing(layout.toolbar_spacing_px)
|
bar.setSpacing(layout.toolbar_spacing_px)
|
||||||
|
|
||||||
self.btn_camera = QPushButton("摄像机")
|
self.btn_camera = QPushButton("Sensor")
|
||||||
self.btn_camera.clicked.connect(self._select_camera)
|
self.btn_camera.clicked.connect(self._select_camera)
|
||||||
bar.addWidget(self.btn_camera)
|
bar.addWidget(self.btn_camera)
|
||||||
self.camera_edit = QLineEdit("0")
|
self.camera_edit = QLineEdit("0")
|
||||||
self.camera_edit.setPlaceholderText("摄像头编号 或 视频路径")
|
self.camera_edit.setPlaceholderText("Camera ID or Video Path")
|
||||||
bar.addWidget(self.camera_edit, 1)
|
bar.addWidget(self.camera_edit, 1)
|
||||||
|
|
||||||
self.btn_config = QPushButton("配置")
|
self.btn_config = QPushButton("Config")
|
||||||
self.btn_config.clicked.connect(self._select_config)
|
self.btn_config.clicked.connect(self._select_config)
|
||||||
bar.addWidget(self.btn_config)
|
bar.addWidget(self.btn_config)
|
||||||
self.config_edit = QLineEdit("./config/dd02-ov.json")
|
self.config_edit = QLineEdit("./config/dd02-ov.json")
|
||||||
@@ -104,33 +114,37 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self.cal_edit = QLineEdit("")
|
self.cal_edit = QLineEdit("")
|
||||||
bar.addWidget(self.cal_edit, 1)
|
bar.addWidget(self.cal_edit, 1)
|
||||||
|
|
||||||
self.btn_reset = QPushButton("重置")
|
self.btn_reset = QPushButton("Reset")
|
||||||
self.btn_reset.setEnabled(False)
|
self.btn_reset.setEnabled(False)
|
||||||
self.btn_reset.clicked.connect(self._on_reset)
|
self.btn_reset.clicked.connect(self._on_reset)
|
||||||
bar.addWidget(self.btn_reset)
|
bar.addWidget(self.btn_reset)
|
||||||
|
|
||||||
self.btn_start = QPushButton("启动")
|
self.btn_play_pause = QPushButton()
|
||||||
self.btn_start.setObjectName("btnStart")
|
self.btn_play_pause.setObjectName(_PLAY_PAUSE_BUTTON_NAME)
|
||||||
self.btn_start.clicked.connect(self._on_start)
|
self.btn_play_pause.setFixedSize(36, 32)
|
||||||
bar.addWidget(self.btn_start)
|
self.btn_play_pause.setIconSize(QSize(_PLAY_PAUSE_ICON_PX, _PLAY_PAUSE_ICON_PX))
|
||||||
|
self.btn_play_pause.clicked.connect(self._on_play_pause_clicked)
|
||||||
self.btn_stop = QPushButton("暂停")
|
self._set_play_pause_button("run")
|
||||||
self.btn_stop.setObjectName("btnStop")
|
bar.addWidget(self.btn_play_pause)
|
||||||
self.btn_stop.setEnabled(False)
|
|
||||||
self.btn_stop.clicked.connect(self._on_stop)
|
|
||||||
bar.addWidget(self.btn_stop)
|
|
||||||
|
|
||||||
root_layout.addLayout(bar)
|
root_layout.addLayout(bar)
|
||||||
|
|
||||||
setup = layout.panel_setup
|
setup = layout.panel_setup
|
||||||
self._panel_widgets: dict[str, QWidget] = {}
|
|
||||||
|
|
||||||
self.mesh_viewport = MeshViewportWidget(layout=layout)
|
self.mesh_viewport = MeshViewportWidget(layout=layout)
|
||||||
self.arrow_panel = ArrowPanelWidget(flow_threshold=layout.arrow_flow_threshold, layout=layout)
|
arrows_slot = layout.slot("arrows")
|
||||||
|
self.arrow_panel: ArrowPanelWidget | None = None
|
||||||
|
if arrows_slot is not None and arrows_slot.enabled:
|
||||||
|
self.arrow_panel = ArrowPanelWidget(
|
||||||
|
flow_threshold=layout.arrow_flow_threshold,
|
||||||
|
background=layout.arrow_background,
|
||||||
|
layout=layout,
|
||||||
|
)
|
||||||
self.force_charts = ForceChartWidget(layout=layout, lowpass_cutoff_hz=self.config.lowpass_cutoff_hz)
|
self.force_charts = ForceChartWidget(layout=layout, lowpass_cutoff_hz=self.config.lowpass_cutoff_hz)
|
||||||
self.log_widget = make_log_widget(layout.log_config)
|
self.log_widget = make_log_widget(layout.log_config)
|
||||||
|
|
||||||
|
self._panel_widgets: dict[str, QWidget] = {}
|
||||||
self._panel_widgets["mesh"] = self.mesh_viewport
|
self._panel_widgets["mesh"] = self.mesh_viewport
|
||||||
|
if self.arrow_panel is not None:
|
||||||
self._panel_widgets["arrows"] = self.arrow_panel
|
self._panel_widgets["arrows"] = self.arrow_panel
|
||||||
self._panel_widgets["charts"] = self.force_charts.widget
|
self._panel_widgets["charts"] = self.force_charts.widget
|
||||||
self._panel_widgets["log"] = self.log_widget
|
self._panel_widgets["log"] = self.log_widget
|
||||||
@@ -194,11 +208,40 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
sys.stdout = TeeStream(*streams)
|
sys.stdout = TeeStream(*streams)
|
||||||
sys.stderr = sys.stdout
|
sys.stderr = sys.stdout
|
||||||
|
|
||||||
|
def _set_play_pause_button(self, state: str) -> None:
|
||||||
|
"""``run`` = green play, ``starting`` = grey disabled play, ``pause`` = orange pause."""
|
||||||
|
btn = self.btn_play_pause
|
||||||
|
if state == "run":
|
||||||
|
btn.setToolTip("Run")
|
||||||
|
btn.setEnabled(True)
|
||||||
|
elif state == "starting":
|
||||||
|
btn.setToolTip("Starting…")
|
||||||
|
btn.setEnabled(False)
|
||||||
|
elif state == "pause":
|
||||||
|
btn.setToolTip("Pause")
|
||||||
|
btn.setEnabled(True)
|
||||||
|
else:
|
||||||
|
raise ValueError(f"unknown play/pause state: {state!r}")
|
||||||
|
|
||||||
|
kind = "pause" if state == "pause" else "play"
|
||||||
|
btn.setText("")
|
||||||
|
btn.setIcon(transport_icon(kind, _PLAY_PAUSE_ICON_COLORS[state]))
|
||||||
|
btn.setProperty("playState", state)
|
||||||
|
style = btn.style()
|
||||||
|
style.unpolish(btn)
|
||||||
|
style.polish(btn)
|
||||||
|
btn.update()
|
||||||
|
|
||||||
|
def _on_play_pause_clicked(self) -> None:
|
||||||
|
if self._running:
|
||||||
|
self._on_stop()
|
||||||
|
else:
|
||||||
|
self._on_start()
|
||||||
|
|
||||||
def _on_start(self) -> None:
|
def _on_start(self) -> None:
|
||||||
if self._running:
|
if self._running:
|
||||||
return
|
return
|
||||||
self.btn_start.setText("启动中...")
|
self._set_play_pause_button("starting")
|
||||||
self.btn_start.setEnabled(False)
|
|
||||||
QApplication.processEvents()
|
QApplication.processEvents()
|
||||||
|
|
||||||
self.config.video = self.camera_edit.text().strip()
|
self.config.video = self.camera_edit.text().strip()
|
||||||
@@ -214,8 +257,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
QTimer.singleShot(0, self._apply_panel_layout)
|
QTimer.singleShot(0, self._apply_panel_layout)
|
||||||
except Exception as exc:
|
except Exception as exc:
|
||||||
self.mesh_viewport.close_renderer()
|
self.mesh_viewport.close_renderer()
|
||||||
self.btn_start.setText("启动")
|
self._set_play_pause_button("run")
|
||||||
self.btn_start.setEnabled(True)
|
|
||||||
print(f"启动失败: {exc}")
|
print(f"启动失败: {exc}")
|
||||||
return
|
return
|
||||||
|
|
||||||
@@ -223,8 +265,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self._frame_index = 0
|
self._frame_index = 0
|
||||||
self._wall_fps = 0.0
|
self._wall_fps = 0.0
|
||||||
self.timer.start(self.layout.timer_interval_ms)
|
self.timer.start(self.layout.timer_interval_ms)
|
||||||
self.btn_start.setText("启动")
|
self._set_play_pause_button("pause")
|
||||||
self.btn_stop.setEnabled(True)
|
|
||||||
self.btn_reset.setEnabled(True)
|
self.btn_reset.setEnabled(True)
|
||||||
print(
|
print(
|
||||||
f"▶ 启动 — 摄像机={self.config.video}, 配置={self.config.config}, "
|
f"▶ 启动 — 摄像机={self.config.video}, 配置={self.config.config}, "
|
||||||
@@ -237,17 +278,14 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
self._running = False
|
self._running = False
|
||||||
self.timer.stop()
|
self.timer.stop()
|
||||||
self.log_handler.clear_status_line()
|
self.log_handler.clear_status_line()
|
||||||
self.btn_stop.setText("停止中...")
|
self._set_play_pause_button("starting")
|
||||||
self.btn_stop.setEnabled(False)
|
|
||||||
QApplication.processEvents()
|
QApplication.processEvents()
|
||||||
|
|
||||||
self.mesh_viewport.close_renderer()
|
self.mesh_viewport.close_renderer()
|
||||||
self.pipeline.close()
|
self.pipeline.close()
|
||||||
|
|
||||||
self.btn_start.setEnabled(True)
|
self._set_play_pause_button("run")
|
||||||
self.btn_stop.setText("暂停")
|
|
||||||
self.btn_reset.setEnabled(False)
|
self.btn_reset.setEnabled(False)
|
||||||
print("⏸ 已暂停")
|
|
||||||
|
|
||||||
def _on_reset(self) -> None:
|
def _on_reset(self) -> None:
|
||||||
if not self._running:
|
if not self._running:
|
||||||
@@ -278,6 +316,7 @@ class FingerDashboardWindow(QMainWindow):
|
|||||||
profile.record("mesh", mesh_ms)
|
profile.record("mesh", mesh_ms)
|
||||||
|
|
||||||
arrows_ms = 0.0
|
arrows_ms = 0.0
|
||||||
|
if self.arrow_panel is not None:
|
||||||
t0 = time.perf_counter()
|
t0 = time.perf_counter()
|
||||||
self.arrow_panel.update_frame(frame)
|
self.arrow_panel.update_frame(frame)
|
||||||
arrows_ms = (time.perf_counter() - t0) * 1000.0
|
arrows_ms = (time.perf_counter() - t0) * 1000.0
|
||||||
|
|||||||
+47
-8
@@ -19,6 +19,8 @@ MeshLighting (below) controls the 3D mesh panel sun light:
|
|||||||
intensity — Filament sun strength (typical 30000–120000)
|
intensity — Filament sun strength (typical 30000–120000)
|
||||||
ambient/diffuse — Windows viewer shading weights (0–1)
|
ambient/diffuse — Windows viewer shading weights (0–1)
|
||||||
|
|
||||||
|
MESH_BACKGROUND (below) sets the 3D mesh panel clear color (RGB 0–255).
|
||||||
|
|
||||||
MESH_CONTACT_ARROW (below) controls the yellow 3D contact arrow on the mesh panel:
|
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)
|
reference_normal_force — Fn at which arrow reaches full size (log/power/linear curve)
|
||||||
force_strength_mode — "log" | "power" | "linear"
|
force_strength_mode — "log" | "power" | "linear"
|
||||||
@@ -34,7 +36,7 @@ Performance (3D dashboard; edit DashboardLayout defaults):
|
|||||||
chart_update_stride — update force charts every N frames
|
chart_update_stride — update force charts every N frames
|
||||||
mesh_async_render — background mesh render (Linux only; Windows uses sync)
|
mesh_async_render — background mesh render (Linux only; Windows uses sync)
|
||||||
|
|
||||||
Edit TOP_ROW.slots to reorder or resize panels.
|
Edit TOP_ROW.slots to reorder or resize panels; set ``enabled=False`` to hide a panel (e.g. arrows).
|
||||||
Edit PANEL_SETUP to change top vs log proportions.
|
Edit PANEL_SETUP to change top vs log proportions.
|
||||||
Edit LOG_CONFIG for live status line and log panel appearance.
|
Edit LOG_CONFIG for live status line and log panel appearance.
|
||||||
"""
|
"""
|
||||||
@@ -46,6 +48,8 @@ from dataclasses import dataclass
|
|||||||
from typing import Literal
|
from typing import Literal
|
||||||
|
|
||||||
from viz.contact_arrow import ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE
|
from viz.contact_arrow import ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE
|
||||||
|
from viz.flow_arrow import FlowArrowStyle
|
||||||
|
from viz.render_2d import ArrowBackground, ArrowCanvasColors
|
||||||
|
|
||||||
PanelId = Literal["mesh", "arrows", "charts", "log"]
|
PanelId = Literal["mesh", "arrows", "charts", "log"]
|
||||||
|
|
||||||
@@ -84,7 +88,7 @@ class PanelSetup:
|
|||||||
TOP_ROW = PanelRow(
|
TOP_ROW = PanelRow(
|
||||||
slots=(
|
slots=(
|
||||||
PanelSlot("mesh", "3D mesh", width_px=500, height_px=500),
|
PanelSlot("mesh", "3D mesh", width_px=500, height_px=500),
|
||||||
PanelSlot("arrows", "UV Unrolled", width_px=500, height_px=500),
|
PanelSlot("arrows", "UV Unrolled", width_px=500, height_px=500, enabled=False),
|
||||||
PanelSlot("charts", "Force charts", width_px=500, height_px=500),
|
PanelSlot("charts", "Force charts", width_px=500, height_px=500),
|
||||||
),
|
),
|
||||||
)
|
)
|
||||||
@@ -99,8 +103,8 @@ PANEL_SETUP = PanelSetup(
|
|||||||
)
|
)
|
||||||
|
|
||||||
# Open3D offscreen resolution for the mesh panel (independent of on-screen widget size).
|
# Open3D offscreen resolution for the mesh panel (independent of on-screen widget size).
|
||||||
MESH_RENDER_WIDTH_PX = 256
|
MESH_RENDER_WIDTH_PX = 500
|
||||||
MESH_RENDER_HEIGHT_PX = 256
|
MESH_RENDER_HEIGHT_PX = 500
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
@@ -123,13 +127,23 @@ class MeshLighting:
|
|||||||
|
|
||||||
|
|
||||||
MESH_LIGHTING = MeshLighting(
|
MESH_LIGHTING = MeshLighting(
|
||||||
azimuth_deg=-45.0,
|
azimuth_deg=0,
|
||||||
elevation_deg=0,
|
elevation_deg=0,
|
||||||
intensity=75000.0,
|
intensity=75000.0,
|
||||||
ambient=0.5,
|
ambient=0.5,
|
||||||
diffuse=0.5,
|
diffuse=0.5,
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class MeshBackground:
|
||||||
|
"""3D mesh panel clear color — edit MESH_BACKGROUND or DashboardLayout.mesh_background."""
|
||||||
|
|
||||||
|
color: tuple[int, int, int] = (0, 0, 0)
|
||||||
|
|
||||||
|
|
||||||
|
MESH_BACKGROUND = MeshBackground(color=(0, 0, 0))
|
||||||
|
|
||||||
# 3D mesh contact arrow — edit here for dashboard tuning (see ContactArrowStyle in viz/contact_arrow.py).
|
# 3D mesh contact arrow — edit here for dashboard tuning (see ContactArrowStyle in viz/contact_arrow.py).
|
||||||
MESH_CONTACT_ARROW = ContactArrowStyle(
|
MESH_CONTACT_ARROW = ContactArrowStyle(
|
||||||
reference_normal_force=10000.0,
|
reference_normal_force=10000.0,
|
||||||
@@ -143,6 +157,26 @@ MESH_CONTACT_ARROW = ContactArrowStyle(
|
|||||||
color=(1.0, 0.92, 0.10),
|
color=(1.0, 0.92, 0.10),
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# 3D mesh flow line arrows — tail_color/head_color are RGB 0–255 (see FlowArrowStyle).
|
||||||
|
MESH_FLOW_ARROW = FlowArrowStyle(
|
||||||
|
threshold=2.0,
|
||||||
|
grid_spacing=20,
|
||||||
|
arrow_scale=0.5,
|
||||||
|
length_margin=1.0,
|
||||||
|
tail_color=(40, 100, 220),
|
||||||
|
head_color=(40, 100, 220),
|
||||||
|
head_fraction=0.5,
|
||||||
|
opacity=1,
|
||||||
|
line_width=15,
|
||||||
|
head_line_width=30.0,
|
||||||
|
)
|
||||||
|
|
||||||
|
# UV arrow synthetic canvas — BGR tuples (outside valid_mask, inside valid_mask).
|
||||||
|
ARROW_CANVAS_COLORS = ArrowCanvasColors(
|
||||||
|
outside_bgr=(0, 0, 0),
|
||||||
|
uv_bgr=(128, 0, 0),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
@dataclass(frozen=True)
|
@dataclass(frozen=True)
|
||||||
class LogConfig:
|
class LogConfig:
|
||||||
@@ -150,7 +184,7 @@ class LogConfig:
|
|||||||
|
|
||||||
status_enabled: bool = True
|
status_enabled: bool = True
|
||||||
max_lines: int = 2000
|
max_lines: int = 2000
|
||||||
font_family: str = "Consolas"
|
font_family: str = "Cascadia Mono"
|
||||||
font_pt: int = 12
|
font_pt: int = 12
|
||||||
show_flow: bool = True
|
show_flow: bool = True
|
||||||
show_mask: bool = True
|
show_mask: bool = True
|
||||||
@@ -191,7 +225,7 @@ class DashboardLayout:
|
|||||||
panel_setup: PanelSetup = PANEL_SETUP
|
panel_setup: PanelSetup = PANEL_SETUP
|
||||||
|
|
||||||
window_title: str = "Orisys Finger Dashboard"
|
window_title: str = "Orisys Finger Dashboard"
|
||||||
window_width: int = 1500
|
window_width: int = 1000
|
||||||
window_height: int = 600
|
window_height: int = 600
|
||||||
|
|
||||||
root_margin_px: int = 6
|
root_margin_px: int = 6
|
||||||
@@ -203,15 +237,20 @@ class DashboardLayout:
|
|||||||
chart_title_font_pt: int = 11
|
chart_title_font_pt: int = 11
|
||||||
chart_axis_label_font_pt: int = 9
|
chart_axis_label_font_pt: int = 9
|
||||||
chart_tick_font_pt: int = 9
|
chart_tick_font_pt: int = 9
|
||||||
y_axis_min_span: float = 20000.0
|
y_axis_min_span: float = 10000.0
|
||||||
y_range_smooth_alpha: float = 0.5
|
y_range_smooth_alpha: float = 0.5
|
||||||
curve_color: str = "#00E5FF"
|
curve_color: str = "#00E5FF"
|
||||||
|
|
||||||
# UV arrow panel: min flow magnitude to draw an arrow (display only; not dd02-ov.json contact.flow_threshold)
|
# 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
|
arrow_flow_threshold: float = 1.0
|
||||||
|
# "unrolled" = sensor texture; "white" = solid canvas (tune colors via arrow_canvas_colors)
|
||||||
|
arrow_background: ArrowBackground = "white"
|
||||||
|
arrow_canvas_colors: ArrowCanvasColors = ARROW_CANVAS_COLORS
|
||||||
|
|
||||||
mesh_lighting: MeshLighting = MESH_LIGHTING
|
mesh_lighting: MeshLighting = MESH_LIGHTING
|
||||||
|
mesh_background: MeshBackground = MESH_BACKGROUND
|
||||||
mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW
|
mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW
|
||||||
|
mesh_flow_arrow: FlowArrowStyle = MESH_FLOW_ARROW
|
||||||
|
|
||||||
log_config: LogConfig = LOG_CONFIG
|
log_config: LogConfig = LOG_CONFIG
|
||||||
|
|
||||||
|
|||||||
@@ -28,7 +28,7 @@ class MeshRenderWorker(QThread):
|
|||||||
self._renderer = None
|
self._renderer = None
|
||||||
self._pending = None
|
self._pending = None
|
||||||
|
|
||||||
def submit(self, *, flow, scalar_map, centroid, centroid_found, fnormal) -> None:
|
def submit(self, *, flow, scalar_map, centroid, centroid_found, fnormal, valid_mask=None) -> None:
|
||||||
with QMutexLocker(self._mutex):
|
with QMutexLocker(self._mutex):
|
||||||
self._pending = {
|
self._pending = {
|
||||||
"flow": flow,
|
"flow": flow,
|
||||||
@@ -36,6 +36,7 @@ class MeshRenderWorker(QThread):
|
|||||||
"centroid": centroid,
|
"centroid": centroid,
|
||||||
"centroid_found": centroid_found,
|
"centroid_found": centroid_found,
|
||||||
"fnormal": fnormal,
|
"fnormal": fnormal,
|
||||||
|
"valid_mask": valid_mask,
|
||||||
}
|
}
|
||||||
self._wake = True
|
self._wake = True
|
||||||
if not self.isRunning():
|
if not self.isRunning():
|
||||||
@@ -66,6 +67,7 @@ class MeshRenderWorker(QThread):
|
|||||||
centroid=job["centroid"],
|
centroid=job["centroid"],
|
||||||
centroid_found=job["centroid_found"],
|
centroid_found=job["centroid_found"],
|
||||||
fnormal=job["fnormal"],
|
fnormal=job["fnormal"],
|
||||||
|
valid_mask=job["valid_mask"],
|
||||||
)
|
)
|
||||||
self.rgb_ready.emit(np.ascontiguousarray(rgb))
|
self.rgb_ready.emit(np.ascontiguousarray(rgb))
|
||||||
|
|
||||||
|
|||||||
+31
-22
@@ -1,5 +1,10 @@
|
|||||||
"""Qt stylesheet for the finger dashboard."""
|
"""Qt stylesheet for the finger dashboard."""
|
||||||
|
|
||||||
|
UI_FONT_STACK = '"Segoe UI Variable", "Segoe UI", "Inter", "Microsoft YaHei UI", sans-serif'
|
||||||
|
LOG_FONT_STACK = '"Cascadia Mono", "JetBrains Mono", "IBM Plex Mono", "Consolas", monospace'
|
||||||
|
UI_FONT_PRIMARY = "Segoe UI"
|
||||||
|
LOG_FONT_PRIMARY = "Cascadia Mono"
|
||||||
|
|
||||||
STYLE = """
|
STYLE = """
|
||||||
QMainWindow {
|
QMainWindow {
|
||||||
background-color: #000000;
|
background-color: #000000;
|
||||||
@@ -17,7 +22,7 @@ QPushButton {
|
|||||||
border: 1px solid #333333;
|
border: 1px solid #333333;
|
||||||
border-radius: 4px;
|
border-radius: 4px;
|
||||||
padding: 6px 16px;
|
padding: 6px 16px;
|
||||||
font-family: "SimHei";
|
font-family: __UI_FONT__;
|
||||||
font-size: 13px;
|
font-size: 13px;
|
||||||
}
|
}
|
||||||
QPushButton:hover {
|
QPushButton:hover {
|
||||||
@@ -25,33 +30,36 @@ QPushButton:hover {
|
|||||||
color: #cccccc;
|
color: #cccccc;
|
||||||
border: 1px solid #555555;
|
border: 1px solid #555555;
|
||||||
}
|
}
|
||||||
QPushButton#btnStart {
|
QPushButton#btnPlayPause {
|
||||||
background-color: #0d2818;
|
background-color: #0d2818;
|
||||||
color: #4caf84;
|
color: #4caf84;
|
||||||
border: 1px solid #1a4d30;
|
border: 1px solid #1a4d30;
|
||||||
|
padding: 4px 8px;
|
||||||
|
font-size: 15px;
|
||||||
}
|
}
|
||||||
QPushButton#btnStart:hover {
|
QPushButton#btnPlayPause:hover {
|
||||||
background-color: #143d24;
|
background-color: #143d24;
|
||||||
color: #6fd4a0;
|
color: #6fd4a0;
|
||||||
}
|
}
|
||||||
QPushButton#btnStart:disabled {
|
QPushButton#btnPlayPause:disabled {
|
||||||
background-color: #0f0f0f;
|
background-color: #1a1a1a;
|
||||||
color: #333333;
|
color: #555555;
|
||||||
border: 1px solid #222222;
|
border: 1px solid #333333;
|
||||||
}
|
}
|
||||||
QPushButton#btnStop {
|
QPushButton#btnPlayPause[playState="pause"] {
|
||||||
background-color: #2b1010;
|
background-color: #3d2808;
|
||||||
color: #e05555;
|
color: #ff9800;
|
||||||
border: 1px solid #4d1a1a;
|
border: 1px solid #6d4400;
|
||||||
}
|
}
|
||||||
QPushButton#btnStop:hover {
|
QPushButton#btnPlayPause[playState="pause"]:hover {
|
||||||
background-color: #3d1818;
|
background-color: #523208;
|
||||||
color: #f07070;
|
color: #ffb74d;
|
||||||
|
border: 1px solid #8a5a00;
|
||||||
}
|
}
|
||||||
QPushButton#btnStop:disabled {
|
QPushButton#btnPlayPause[playState="pause"]:disabled {
|
||||||
background-color: #0f0f0f;
|
background-color: #1a1a1a;
|
||||||
color: #333333;
|
color: #555555;
|
||||||
border: 1px solid #222222;
|
border: 1px solid #333333;
|
||||||
}
|
}
|
||||||
QLineEdit {
|
QLineEdit {
|
||||||
background-color: #0d0d0d;
|
background-color: #0d0d0d;
|
||||||
@@ -59,6 +67,7 @@ QLineEdit {
|
|||||||
border: 1px solid #333333;
|
border: 1px solid #333333;
|
||||||
border-radius: 4px;
|
border-radius: 4px;
|
||||||
padding: 5px 10px;
|
padding: 5px 10px;
|
||||||
|
font-family: __UI_FONT__;
|
||||||
font-size: 13px;
|
font-size: 13px;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -69,11 +78,11 @@ GraphicsLayoutWidget {
|
|||||||
|
|
||||||
QPlainTextEdit {
|
QPlainTextEdit {
|
||||||
background-color: #0b0b0b;
|
background-color: #0b0b0b;
|
||||||
color: #777777;
|
color: #999999;
|
||||||
border: 1px solid #262626;
|
border: 1px solid #262626;
|
||||||
border-radius: 4px;
|
border-radius: 4px;
|
||||||
font-family: "Cascadia Code", "Consolas", "Courier New", monospace;
|
font-family: __LOG_FONT__;
|
||||||
font-size: 13px;
|
font-size: 12px;
|
||||||
padding: 8px;
|
padding: 8px;
|
||||||
selection-background-color: #1a3a4a;
|
selection-background-color: #1a3a4a;
|
||||||
selection-color: #cccccc;
|
selection-color: #cccccc;
|
||||||
@@ -111,4 +120,4 @@ QSplitter::handle:vertical {
|
|||||||
QSplitter::handle:horizontal {
|
QSplitter::handle:horizontal {
|
||||||
width: 1px;
|
width: 1px;
|
||||||
}
|
}
|
||||||
"""
|
""".replace("__UI_FONT__", UI_FONT_STACK).replace("__LOG_FONT__", LOG_FONT_STACK)
|
||||||
|
|||||||
@@ -0,0 +1,50 @@
|
|||||||
|
"""Monochrome play/pause icons for the dashboard toolbar (avoids Windows emoji glyphs)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from typing import Literal
|
||||||
|
|
||||||
|
from PyQt5.QtCore import QPointF, QRectF, Qt
|
||||||
|
from PyQt5.QtGui import QColor, QIcon, QPainter, QPixmap, QPolygonF
|
||||||
|
|
||||||
|
TransportKind = Literal["play", "pause"]
|
||||||
|
|
||||||
|
_DEFAULT_ICON_PX = 18
|
||||||
|
|
||||||
|
|
||||||
|
def transport_icon(kind: TransportKind, color: str, *, size: int = _DEFAULT_ICON_PX) -> QIcon:
|
||||||
|
"""Build a flat play triangle or pause bars in ``color`` (#rrggbb)."""
|
||||||
|
pixmap = QPixmap(size, size)
|
||||||
|
pixmap.fill(Qt.transparent)
|
||||||
|
|
||||||
|
painter = QPainter(pixmap)
|
||||||
|
painter.setRenderHint(QPainter.Antialiasing, True)
|
||||||
|
painter.setBrush(QColor(color))
|
||||||
|
painter.setPen(Qt.NoPen)
|
||||||
|
|
||||||
|
if kind == "play":
|
||||||
|
w = size * 0.52
|
||||||
|
h = size * 0.60
|
||||||
|
x0 = (size - w) * 0.58
|
||||||
|
y0 = (size - h) / 2.0
|
||||||
|
painter.drawPolygon(
|
||||||
|
QPolygonF(
|
||||||
|
[
|
||||||
|
QPointF(x0, y0),
|
||||||
|
QPointF(x0, y0 + h),
|
||||||
|
QPointF(x0 + w, y0 + h / 2.0),
|
||||||
|
]
|
||||||
|
)
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
bar_w = size * 0.17
|
||||||
|
bar_h = size * 0.60
|
||||||
|
gap = size * 0.14
|
||||||
|
y0 = (size - bar_h) / 2.0
|
||||||
|
x0 = (size - (2.0 * bar_w + gap)) / 2.0
|
||||||
|
radius = max(1.0, size * 0.06)
|
||||||
|
painter.drawRoundedRect(QRectF(x0, y0, bar_w, bar_h), radius, radius)
|
||||||
|
painter.drawRoundedRect(QRectF(x0 + bar_w + gap, y0, bar_w, bar_h), radius, radius)
|
||||||
|
|
||||||
|
painter.end()
|
||||||
|
return QIcon(pixmap)
|
||||||
@@ -5,7 +5,7 @@ from __future__ import annotations
|
|||||||
import cv2
|
import cv2
|
||||||
|
|
||||||
from pipeline.frame import FingerFrameView
|
from pipeline.frame import FingerFrameView
|
||||||
from viz.render_2d import render_uv_arrows
|
from viz.render_2d import ArrowBackground, ArrowCanvasColors, render_uv_arrows
|
||||||
|
|
||||||
from ..layout import DashboardLayout, LAYOUT
|
from ..layout import DashboardLayout, LAYOUT
|
||||||
from .image_panel import ImagePanel
|
from .image_panel import ImagePanel
|
||||||
@@ -19,6 +19,8 @@ class ArrowPanelWidget(ImagePanel):
|
|||||||
self,
|
self,
|
||||||
*,
|
*,
|
||||||
flow_threshold: float = 0.5,
|
flow_threshold: float = 0.5,
|
||||||
|
background: ArrowBackground = "unrolled",
|
||||||
|
canvas_colors: ArrowCanvasColors | None = None,
|
||||||
layout: DashboardLayout = LAYOUT,
|
layout: DashboardLayout = LAYOUT,
|
||||||
parent=None,
|
parent=None,
|
||||||
):
|
):
|
||||||
@@ -30,15 +32,18 @@ class ArrowPanelWidget(ImagePanel):
|
|||||||
parent=parent,
|
parent=parent,
|
||||||
)
|
)
|
||||||
self.flow_threshold = flow_threshold
|
self.flow_threshold = flow_threshold
|
||||||
|
self.background = background
|
||||||
|
self.canvas_colors = canvas_colors if canvas_colors is not None else layout.arrow_canvas_colors
|
||||||
|
|
||||||
def set_flow_threshold(self, threshold: float) -> None:
|
def set_flow_threshold(self, threshold: float) -> None:
|
||||||
self.flow_threshold = threshold
|
self.flow_threshold = threshold
|
||||||
|
|
||||||
def update_frame(self, frame: FingerFrameView) -> None:
|
def update_frame(self, frame: FingerFrameView) -> None:
|
||||||
arrows = render_uv_arrows(
|
arrows = render_uv_arrows(
|
||||||
frame.unrolled,
|
frame,
|
||||||
frame.vnormal,
|
|
||||||
flow_threshold=self.flow_threshold,
|
flow_threshold=self.flow_threshold,
|
||||||
|
background=self.background,
|
||||||
|
canvas_colors=self.canvas_colors,
|
||||||
)
|
)
|
||||||
arrows = cv2.rotate(arrows, cv2.ROTATE_180)
|
arrows = cv2.rotate(arrows, cv2.ROTATE_180)
|
||||||
self.set_source_pixmap(ndarray_to_pixmap(arrows, bgr=True))
|
self.set_source_pixmap(ndarray_to_pixmap(arrows, bgr=True))
|
||||||
|
|||||||
@@ -7,6 +7,7 @@ from PyQt5.QtGui import QFont
|
|||||||
from PyQt5.QtWidgets import QSizePolicy
|
from PyQt5.QtWidgets import QSizePolicy
|
||||||
|
|
||||||
from gui.layout import DashboardLayout, LAYOUT
|
from gui.layout import DashboardLayout, LAYOUT
|
||||||
|
from gui.styles import LOG_FONT_PRIMARY, UI_FONT_PRIMARY
|
||||||
from pipeline.frame import FingerFrameView
|
from pipeline.frame import FingerFrameView
|
||||||
from viz.signal_filters import lowpass_filter
|
from viz.signal_filters import lowpass_filter
|
||||||
|
|
||||||
@@ -72,21 +73,21 @@ class ForceChartWidget:
|
|||||||
def _create_plots(self) -> None:
|
def _create_plots(self) -> None:
|
||||||
pg = self._pg
|
pg = self._pg
|
||||||
layout = self._layout
|
layout = self._layout
|
||||||
titles = ["法向力", "切向力 X", "切向力 Y"]
|
titles = ["Normal Deformation", "X Deformation", "Y Deformation"]
|
||||||
self.plots = []
|
self.plots = []
|
||||||
self.curves = []
|
self.curves = []
|
||||||
self.dot_glows = []
|
self.dot_glows = []
|
||||||
self.dot_cores = []
|
self.dot_cores = []
|
||||||
|
|
||||||
axis_pen = pg.mkPen(color=(38, 38, 38), width=1)
|
axis_pen = pg.mkPen(color=(38, 38, 38), width=1)
|
||||||
label_font = QFont("SimHei", layout.chart_axis_label_font_pt)
|
label_font = QFont(UI_FONT_PRIMARY, layout.chart_axis_label_font_pt)
|
||||||
title_pt = layout.chart_title_font_pt
|
title_pt = layout.chart_title_font_pt
|
||||||
|
|
||||||
for i, title in enumerate(titles):
|
for i, title in enumerate(titles):
|
||||||
plot = self.widget.addPlot(title=None)
|
plot = self.widget.addPlot(title=None)
|
||||||
plot.setTitle(title, color="#999999", size=f"{title_pt}pt")
|
plot.setTitle(title, color="#999999", size=f"{title_pt}pt")
|
||||||
plot.titleLabel.setFont(QFont("SimHei", title_pt))
|
plot.titleLabel.setFont(QFont(UI_FONT_PRIMARY, title_pt))
|
||||||
plot.setLabel("bottom", "时间", units="s")
|
plot.setLabel("bottom", "Time", units="s")
|
||||||
plot.getAxis("bottom").label.setFont(label_font)
|
plot.getAxis("bottom").label.setFont(label_font)
|
||||||
plot.showAxis("top", False)
|
plot.showAxis("top", False)
|
||||||
plot.showAxis("right", False)
|
plot.showAxis("right", False)
|
||||||
@@ -94,7 +95,7 @@ class ForceChartWidget:
|
|||||||
plot.getAxis("bottom").setPen(axis_pen)
|
plot.getAxis("bottom").setPen(axis_pen)
|
||||||
plot.showGrid(x=False, y=True, alpha=0.12)
|
plot.showGrid(x=False, y=True, alpha=0.12)
|
||||||
|
|
||||||
tick_font = QFont("Consolas", layout.chart_tick_font_pt)
|
tick_font = QFont(LOG_FONT_PRIMARY, layout.chart_tick_font_pt)
|
||||||
tick_font.setStyleHint(QFont.Monospace)
|
tick_font.setStyleHint(QFont.Monospace)
|
||||||
plot.getAxis("left").setTickFont(tick_font)
|
plot.getAxis("left").setTickFont(tick_font)
|
||||||
plot.getAxis("bottom").setTickFont(tick_font)
|
plot.getAxis("bottom").setTickFont(tick_font)
|
||||||
|
|||||||
@@ -65,7 +65,9 @@ class MeshViewportWidget(ImagePanel):
|
|||||||
world_to_cam=pipeline.world_to_cam,
|
world_to_cam=pipeline.world_to_cam,
|
||||||
lookat=pipeline.mesh_lookat,
|
lookat=pipeline.mesh_lookat,
|
||||||
mesh_lighting=self._layout.mesh_lighting,
|
mesh_lighting=self._layout.mesh_lighting,
|
||||||
|
mesh_background=self._layout.mesh_background.color,
|
||||||
contact_arrow_style=self._layout.mesh_contact_arrow,
|
contact_arrow_style=self._layout.mesh_contact_arrow,
|
||||||
|
flow_arrow_style=self._layout.mesh_flow_arrow,
|
||||||
)
|
)
|
||||||
|
|
||||||
def _stop_worker(self) -> None:
|
def _stop_worker(self) -> None:
|
||||||
@@ -93,6 +95,7 @@ class MeshViewportWidget(ImagePanel):
|
|||||||
centroid=frame.centroid,
|
centroid=frame.centroid,
|
||||||
centroid_found=frame.centroid_found,
|
centroid_found=frame.centroid_found,
|
||||||
fnormal=frame.fnormal,
|
fnormal=frame.fnormal,
|
||||||
|
valid_mask=frame.valid_mask,
|
||||||
)
|
)
|
||||||
return 0.0
|
return 0.0
|
||||||
|
|
||||||
@@ -105,6 +108,7 @@ class MeshViewportWidget(ImagePanel):
|
|||||||
centroid=frame.centroid,
|
centroid=frame.centroid,
|
||||||
centroid_found=frame.centroid_found,
|
centroid_found=frame.centroid_found,
|
||||||
fnormal=frame.fnormal,
|
fnormal=frame.fnormal,
|
||||||
|
valid_mask=frame.valid_mask,
|
||||||
)
|
)
|
||||||
self.set_source_pixmap(ndarray_to_pixmap(rgb, bgr=False))
|
self.set_source_pixmap(ndarray_to_pixmap(rgb, bgr=False))
|
||||||
return (time.perf_counter() - t0) * 1000.0
|
return (time.perf_counter() - t0) * 1000.0
|
||||||
|
|||||||
@@ -23,4 +23,38 @@ class FingerFrameView:
|
|||||||
fsheary: float
|
fsheary: float
|
||||||
centroid_found: bool
|
centroid_found: bool
|
||||||
mask_coverage: float
|
mask_coverage: float
|
||||||
|
valid_mask: np.ndarray | None = None
|
||||||
raw_frame: np.ndarray | None = None
|
raw_frame: np.ndarray | None = None
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def from_tactile(cls, tactile, unrolled, *, timestamp: float) -> FingerFrameView:
|
||||||
|
"""Build a display snapshot from tactile state (mask + fields assembled here)."""
|
||||||
|
fps, flow, vnormal, depth_map, centroid, fnormal, fshearx, fsheary = tactile.read_info(
|
||||||
|
tactile.info.FPS,
|
||||||
|
tactile.info.VRAW,
|
||||||
|
tactile.info.VNORMAL,
|
||||||
|
tactile.info.DEPTH,
|
||||||
|
tactile.info.CENTROID,
|
||||||
|
tactile.info.FNORMAL,
|
||||||
|
tactile.info.FSHEARX,
|
||||||
|
tactile.info.FSHEARY,
|
||||||
|
)
|
||||||
|
mask = tactile.valid_mask
|
||||||
|
return cls(
|
||||||
|
timestamp=timestamp,
|
||||||
|
fps=float(fps),
|
||||||
|
unrolled=unrolled,
|
||||||
|
flow=flow,
|
||||||
|
vnormal=vnormal,
|
||||||
|
depth_map=depth_map,
|
||||||
|
img=np.array([]),
|
||||||
|
contour=np.array([]),
|
||||||
|
centroid=centroid,
|
||||||
|
fnormal=float(fnormal),
|
||||||
|
fshearx=float(fshearx),
|
||||||
|
fsheary=float(fsheary),
|
||||||
|
centroid_found=bool(tactile.raw_sensor.centroid_found),
|
||||||
|
mask_coverage=float(tactile.mask_stats["coverage"]),
|
||||||
|
valid_mask=mask.copy() if mask is not None else None,
|
||||||
|
raw_frame=tactile.frame,
|
||||||
|
)
|
||||||
|
|||||||
@@ -87,35 +87,6 @@ class FingerPipeline:
|
|||||||
if self.runtime is not None:
|
if self.runtime is not None:
|
||||||
self.runtime.tactile.reset()
|
self.runtime.tactile.reset()
|
||||||
|
|
||||||
def _read_frame_view(self, tactile, unrolled) -> FingerFrameView:
|
|
||||||
fps, flow, vnormal, depth_map, centroid, fnormal, fshearx, fsheary = tactile.read_info(
|
|
||||||
tactile.info.FPS,
|
|
||||||
tactile.info.VRAW,
|
|
||||||
tactile.info.VNORMAL,
|
|
||||||
tactile.info.DEPTH,
|
|
||||||
tactile.info.CENTROID,
|
|
||||||
tactile.info.FNORMAL,
|
|
||||||
tactile.info.FSHEARX,
|
|
||||||
tactile.info.FSHEARY,
|
|
||||||
)
|
|
||||||
return FingerFrameView(
|
|
||||||
timestamp=time.time() - self._start_time,
|
|
||||||
fps=float(fps),
|
|
||||||
unrolled=unrolled,
|
|
||||||
flow=flow,
|
|
||||||
vnormal=vnormal,
|
|
||||||
depth_map=depth_map,
|
|
||||||
img=np.array([]),
|
|
||||||
contour=np.array([]),
|
|
||||||
centroid=centroid,
|
|
||||||
fnormal=float(fnormal),
|
|
||||||
fshearx=float(fshearx),
|
|
||||||
fsheary=float(fsheary),
|
|
||||||
centroid_found=bool(tactile.raw_sensor.centroid_found),
|
|
||||||
mask_coverage=float(tactile.mask_stats["coverage"]),
|
|
||||||
raw_frame=tactile.frame,
|
|
||||||
)
|
|
||||||
|
|
||||||
def tick(self) -> FingerFrameView | None:
|
def tick(self) -> FingerFrameView | None:
|
||||||
frame, _timings = self.tick_timed()
|
frame, _timings = self.tick_timed()
|
||||||
return frame
|
return frame
|
||||||
@@ -165,7 +136,11 @@ class FingerPipeline:
|
|||||||
self.profiler.record("contact", timings.contact_ms)
|
self.profiler.record("contact", timings.contact_ms)
|
||||||
|
|
||||||
t0 = time.perf_counter()
|
t0 = time.perf_counter()
|
||||||
view = self._read_frame_view(tactile, unrolled)
|
view = FingerFrameView.from_tactile(
|
||||||
|
tactile,
|
||||||
|
unrolled,
|
||||||
|
timestamp=time.time() - self._start_time,
|
||||||
|
)
|
||||||
timings.read_info_ms = (time.perf_counter() - t0) * 1000.0
|
timings.read_info_ms = (time.perf_counter() - t0) * 1000.0
|
||||||
if profile:
|
if profile:
|
||||||
self.profiler.record("read_info", timings.read_info_ms)
|
self.profiler.record("read_info", timings.read_info_ms)
|
||||||
|
|||||||
@@ -9,15 +9,29 @@ from .mesh_camera import (
|
|||||||
resolve_viz_pose_path,
|
resolve_viz_pose_path,
|
||||||
)
|
)
|
||||||
from .offscreen_mesh import OffscreenFingerMeshRenderer
|
from .offscreen_mesh import OffscreenFingerMeshRenderer
|
||||||
from .render_2d import render_contact_panel, render_depth_panel, render_uv_arrows
|
from .render_2d import (
|
||||||
|
ArrowBackground,
|
||||||
|
ArrowCanvasColors,
|
||||||
|
DEFAULT_ARROW_CANVAS_COLORS,
|
||||||
|
depth_colormap,
|
||||||
|
frame_to_bgr,
|
||||||
|
render_contact_panel,
|
||||||
|
render_depth_panel,
|
||||||
|
render_uv_arrows,
|
||||||
|
)
|
||||||
|
from .flow_arrow import DEFAULT_FLOW_ARROW_STYLE, FlowArrowOverlay, FlowArrowStyle
|
||||||
from .finger_mesh_viewer import FingerMeshViewer
|
from .finger_mesh_viewer import FingerMeshViewer
|
||||||
from .mesh_viewer import UVMeshViewer, build_uv_surface_mesh
|
from .mesh_viewer import UVMeshViewer, build_uv_surface_mesh
|
||||||
from .opencv_maps import depth_colormap, draw_magnitude_map, frame_to_bgr
|
|
||||||
from .recording import FrameRecorder
|
from .recording import FrameRecorder
|
||||||
from .signal_filters import lowpass_filter
|
from .signal_filters import lowpass_filter
|
||||||
|
|
||||||
__all__ = [
|
__all__ = [
|
||||||
"DEFAULT_FINGER_VIZ_POSE",
|
"ArrowBackground",
|
||||||
|
"ArrowCanvasColors",
|
||||||
|
"DEFAULT_ARROW_CANVAS_COLORS",
|
||||||
|
"DEFAULT_FLOW_ARROW_STYLE",
|
||||||
|
"FlowArrowOverlay",
|
||||||
|
"FlowArrowStyle",
|
||||||
"FingerMeshViewer",
|
"FingerMeshViewer",
|
||||||
"ForceChartWindow",
|
"ForceChartWindow",
|
||||||
"FrameRecorder",
|
"FrameRecorder",
|
||||||
@@ -27,7 +41,6 @@ __all__ = [
|
|||||||
"build_uv_surface_mesh",
|
"build_uv_surface_mesh",
|
||||||
"camera_look_at_from_world_to_cam",
|
"camera_look_at_from_world_to_cam",
|
||||||
"depth_colormap",
|
"depth_colormap",
|
||||||
"draw_magnitude_map",
|
|
||||||
"frame_to_bgr",
|
"frame_to_bgr",
|
||||||
"lowpass_filter",
|
"lowpass_filter",
|
||||||
"OffscreenFingerMeshRenderer",
|
"OffscreenFingerMeshRenderer",
|
||||||
|
|||||||
@@ -1,14 +1,19 @@
|
|||||||
"""Finger 3D mesh viewer with example-only contact arrow overlay."""
|
"""Finger 3D mesh viewer with example-only contact and flow arrow overlays."""
|
||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import replace
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
from orisys.finger.viewer import UVMeshViewer
|
from orisys.finger.viewer import UVMeshViewer
|
||||||
|
|
||||||
from .contact_arrow import ContactArrowOverlay, ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE
|
from .contact_arrow import ContactArrowOverlay, ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE
|
||||||
|
from .flow_arrow import DEFAULT_FLOW_ARROW_STYLE, FlowArrowOverlay, FlowArrowStyle
|
||||||
|
|
||||||
|
|
||||||
class FingerMeshViewer(UVMeshViewer):
|
class FingerMeshViewer(UVMeshViewer):
|
||||||
"""UV mesh viewer with a force-scaled contact normal arrow."""
|
"""UV mesh viewer with force-scaled contact arrow and flow tube arrows."""
|
||||||
|
|
||||||
def __init__(
|
def __init__(
|
||||||
self,
|
self,
|
||||||
@@ -24,6 +29,8 @@ class FingerMeshViewer(UVMeshViewer):
|
|||||||
contact_arrow_style: ContactArrowStyle | None = None,
|
contact_arrow_style: ContactArrowStyle | None = None,
|
||||||
normal_force_scale: float | None = None,
|
normal_force_scale: float | None = None,
|
||||||
show_contact_arrow: bool = True,
|
show_contact_arrow: bool = True,
|
||||||
|
flow_arrow_style: FlowArrowStyle | None = None,
|
||||||
|
show_flow_arrows: bool = True,
|
||||||
):
|
):
|
||||||
super().__init__(
|
super().__init__(
|
||||||
mesh,
|
mesh,
|
||||||
@@ -41,16 +48,24 @@ class FingerMeshViewer(UVMeshViewer):
|
|||||||
normal_force_scale=normal_force_scale,
|
normal_force_scale=normal_force_scale,
|
||||||
enabled=show_contact_arrow,
|
enabled=show_contact_arrow,
|
||||||
)
|
)
|
||||||
|
style = flow_arrow_style or DEFAULT_FLOW_ARROW_STYLE
|
||||||
|
if not show_flow_arrows:
|
||||||
|
style = replace(style, enabled=False)
|
||||||
|
self._flow_arrow = FlowArrowOverlay(mesh, style=style)
|
||||||
|
|
||||||
|
def _extra_geometries(self):
|
||||||
|
geoms = []
|
||||||
|
if self._contact_arrow.arrow_mesh is not None:
|
||||||
|
geoms.append(self._contact_arrow.arrow_mesh)
|
||||||
|
if self._flow_arrow.enabled and self._flow_arrow.arrow_mesh is not None:
|
||||||
|
geoms.append(self._flow_arrow.arrow_mesh)
|
||||||
|
return geoms
|
||||||
|
|
||||||
def _extra_geometries_for_init(self):
|
def _extra_geometries_for_init(self):
|
||||||
if self._contact_arrow.arrow_mesh is None:
|
return self._extra_geometries()
|
||||||
return []
|
|
||||||
return [self._contact_arrow.arrow_mesh]
|
|
||||||
|
|
||||||
def _extra_geometries_for_update(self):
|
def _extra_geometries_for_update(self):
|
||||||
if self._contact_arrow.arrow_mesh is None:
|
return self._extra_geometries()
|
||||||
return []
|
|
||||||
return [self._contact_arrow.arrow_mesh]
|
|
||||||
|
|
||||||
def update(
|
def update(
|
||||||
self,
|
self,
|
||||||
@@ -60,9 +75,16 @@ class FingerMeshViewer(UVMeshViewer):
|
|||||||
centroid=None,
|
centroid=None,
|
||||||
centroid_found: bool = False,
|
centroid_found: bool = False,
|
||||||
fnormal: float = 0.0,
|
fnormal: float = 0.0,
|
||||||
|
valid_mask: np.ndarray | None = None,
|
||||||
):
|
):
|
||||||
arrow = self._contact_arrow.compute(centroid, centroid_found, fnormal)
|
arrow = self._contact_arrow.compute(centroid, centroid_found, fnormal)
|
||||||
if arrow is not None and self._contact_arrow.arrow_mesh is not None:
|
if arrow is not None and self._contact_arrow.arrow_mesh is not None:
|
||||||
arrow_vertices, _, arrow_colors = arrow
|
arrow_vertices, _, arrow_colors = arrow
|
||||||
self._contact_arrow.apply(arrow_vertices, arrow_colors)
|
self._contact_arrow.apply(arrow_vertices, arrow_colors)
|
||||||
|
|
||||||
|
flow_result = self._flow_arrow.compute(flow, valid_mask=valid_mask)
|
||||||
|
if flow_result is not None and self._flow_arrow.arrow_mesh is not None:
|
||||||
|
flow_vertices, flow_triangles, flow_colors = flow_result
|
||||||
|
self._flow_arrow.apply(flow_vertices, flow_triangles, flow_colors)
|
||||||
|
|
||||||
return super().update(flow=flow, scalar_map=scalar_map)
|
return super().update(flow=flow, scalar_map=scalar_map)
|
||||||
|
|||||||
@@ -0,0 +1,431 @@
|
|||||||
|
"""Optical-flow arrow tubes on the finger UV surface mesh (TriangleMesh cylinders)."""
|
||||||
|
|
||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import Literal
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from orisys.finger.mesh import DEFAULT_SURFACE_COLOR, UVSurfaceMesh
|
||||||
|
|
||||||
|
from .contact_arrow import _rotation_matrix_z_to
|
||||||
|
|
||||||
|
# Grey mesh tone used to approximate opacity on backends without true alpha.
|
||||||
|
_MESH_GREY_RGB = np.asarray(DEFAULT_SURFACE_COLOR, dtype=np.float64)
|
||||||
|
|
||||||
|
# Maps style.line_width to tube radius as a fraction of mesh extent.
|
||||||
|
LINE_WIDTH_TO_SPAN_FRACTION = 0.0003
|
||||||
|
# Slight tail extension into the head segment to avoid visible gaps.
|
||||||
|
_TAIL_HEAD_OVERLAP_FRACTION = 0.03
|
||||||
|
|
||||||
|
SegmentRole = Literal["tail", "head"]
|
||||||
|
|
||||||
|
_CYLINDER_TEMPLATE_VERTICES: np.ndarray | None = None
|
||||||
|
_CYLINDER_TEMPLATE_TRIANGLES: np.ndarray | None = None
|
||||||
|
_CONE_TEMPLATE_VERTICES: np.ndarray | None = None
|
||||||
|
_CONE_TEMPLATE_TRIANGLES: np.ndarray | None = None
|
||||||
|
_CONE_APEX_AT_LOW_Z: bool | None = None
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class FlowArrowStyle:
|
||||||
|
"""Visual tuning for 3D mesh flow arrows (dashboard: edit MESH_FLOW_ARROW in layout.py).
|
||||||
|
|
||||||
|
``line_width`` / ``head_line_width`` scale tail and head tube radius relative to mesh extent.
|
||||||
|
``tail_color`` / ``head_color`` are RGB in 0–255 (same convention as OpenCV BGR tuples, but RGB order).
|
||||||
|
"""
|
||||||
|
|
||||||
|
enabled: bool = True
|
||||||
|
threshold: float = 1.0
|
||||||
|
grid_spacing: int = 20
|
||||||
|
arrow_scale: float = 0.5
|
||||||
|
length_margin: float = 1.0
|
||||||
|
tail_color: tuple[int, int, int] = (38, 235, 89)
|
||||||
|
head_color: tuple[int, int, int] = (13, 115, 31)
|
||||||
|
head_fraction: float = 0.25
|
||||||
|
opacity: float = 0.65
|
||||||
|
line_width: float = 2.0
|
||||||
|
head_line_width: float = 3.0
|
||||||
|
edge_margin: int = 8
|
||||||
|
|
||||||
|
|
||||||
|
DEFAULT_FLOW_ARROW_STYLE = FlowArrowStyle()
|
||||||
|
|
||||||
|
|
||||||
|
def _mesh_span(vertices: np.ndarray) -> float:
|
||||||
|
extent = np.ptp(vertices, axis=0)
|
||||||
|
return float(max(np.max(extent), 1e-6))
|
||||||
|
|
||||||
|
|
||||||
|
def segment_radius(mesh_span: float, line_width: float) -> float:
|
||||||
|
"""World-space tube radius from mesh extent and a width knob."""
|
||||||
|
return mesh_span * max(float(line_width), 0.1) * LINE_WIDTH_TO_SPAN_FRACTION
|
||||||
|
|
||||||
|
|
||||||
|
def segment_radius_for_role(mesh_span: float, style: FlowArrowStyle, role: SegmentRole) -> float:
|
||||||
|
width = style.line_width if role == "tail" else style.head_line_width
|
||||||
|
return segment_radius(mesh_span, width)
|
||||||
|
|
||||||
|
|
||||||
|
def _prepare_uv_mask(valid_mask, shape: tuple[int, int]) -> np.ndarray | None:
|
||||||
|
if valid_mask is None:
|
||||||
|
return None
|
||||||
|
h, w = shape
|
||||||
|
mask = np.asarray(valid_mask, dtype=bool)
|
||||||
|
if mask.shape != (h, w):
|
||||||
|
mask_u8 = mask.astype(np.uint8)
|
||||||
|
if mask_u8.ndim > 2:
|
||||||
|
mask_u8 = mask_u8[:, :, 0]
|
||||||
|
mask = cv2.resize(mask_u8, (w, h), interpolation=cv2.INTER_NEAREST).astype(bool)
|
||||||
|
return mask
|
||||||
|
|
||||||
|
|
||||||
|
def _style_rgb01(style: FlowArrowStyle, role: SegmentRole) -> np.ndarray:
|
||||||
|
rgb = np.asarray(style.tail_color if role == "tail" else style.head_color, dtype=np.float64).reshape(3)
|
||||||
|
return np.clip(rgb / 255.0, 0.0, 1.0)
|
||||||
|
|
||||||
|
|
||||||
|
def effective_segment_color(
|
||||||
|
style: FlowArrowStyle,
|
||||||
|
role: SegmentRole,
|
||||||
|
*,
|
||||||
|
use_alpha_blend: bool = True,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Return RGB in 0–1 for a tail or head segment."""
|
||||||
|
rgb = _style_rgb01(style, role)
|
||||||
|
if not use_alpha_blend:
|
||||||
|
return rgb
|
||||||
|
opacity = float(np.clip(style.opacity, 0.0, 1.0))
|
||||||
|
return np.clip(opacity * rgb + (1.0 - opacity) * _MESH_GREY_RGB, 0.0, 1.0)
|
||||||
|
|
||||||
|
|
||||||
|
def _sample_flow_segments(
|
||||||
|
world_xyz: np.ndarray,
|
||||||
|
flow: np.ndarray,
|
||||||
|
*,
|
||||||
|
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
|
||||||
|
valid_mask: np.ndarray | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, list[SegmentRole]]:
|
||||||
|
"""Return segment start/end world points and role per segment (tail then head)."""
|
||||||
|
empty = np.empty((0, 3), dtype=np.float64)
|
||||||
|
if flow is None or np.asarray(flow).size == 0:
|
||||||
|
return empty, empty, []
|
||||||
|
|
||||||
|
flow = np.asarray(flow, dtype=np.float64)
|
||||||
|
world_xyz = np.asarray(world_xyz, dtype=np.float64)
|
||||||
|
h, w = flow.shape[:2]
|
||||||
|
if world_xyz.shape[:2] != (h, w):
|
||||||
|
return empty, empty, []
|
||||||
|
|
||||||
|
grid_spacing = max(int(style.grid_spacing), 1)
|
||||||
|
edge_margin = int(style.edge_margin)
|
||||||
|
margin = float(style.length_margin)
|
||||||
|
arrow_scale = max(float(style.arrow_scale), 1e-9)
|
||||||
|
head_fraction = float(np.clip(style.head_fraction, 0.05, 0.95))
|
||||||
|
tail_fraction = 1.0 - head_fraction
|
||||||
|
|
||||||
|
rr = np.arange(grid_spacing // 2, h, grid_spacing)
|
||||||
|
cc = np.arange(grid_spacing // 2, w, grid_spacing)
|
||||||
|
rr, cc = np.meshgrid(rr, cc, indexing="ij")
|
||||||
|
rr, cc = rr.reshape(-1), cc.reshape(-1)
|
||||||
|
|
||||||
|
disp = 2.0 * flow[rr, cc, :]
|
||||||
|
mag = np.linalg.norm(disp, axis=1)
|
||||||
|
dx = disp[:, 0] / arrow_scale
|
||||||
|
dy = disp[:, 1] / arrow_scale
|
||||||
|
length = np.sqrt(dx * dx + dy * dy)
|
||||||
|
scale = np.where(
|
||||||
|
length > grid_spacing * margin,
|
||||||
|
grid_spacing / np.maximum(length, 1e-9),
|
||||||
|
1.0,
|
||||||
|
) * margin
|
||||||
|
dx, dy = dx * scale, dy * scale
|
||||||
|
end_x = np.clip(np.round(cc + dx).astype(np.int32), 0, w - 1)
|
||||||
|
end_y = np.clip(np.round(rr + dy).astype(np.int32), 0, h - 1)
|
||||||
|
|
||||||
|
valid = (
|
||||||
|
(cc >= edge_margin)
|
||||||
|
& (cc < w - edge_margin)
|
||||||
|
& (rr >= edge_margin)
|
||||||
|
& (rr < h - edge_margin)
|
||||||
|
& (mag >= float(style.threshold))
|
||||||
|
)
|
||||||
|
|
||||||
|
mask = _prepare_uv_mask(valid_mask, (h, w))
|
||||||
|
if mask is not None:
|
||||||
|
valid &= mask[rr, cc]
|
||||||
|
valid &= mask[end_y, end_x]
|
||||||
|
|
||||||
|
if not np.any(valid):
|
||||||
|
return empty, empty, []
|
||||||
|
|
||||||
|
starts = world_xyz[rr[valid], cc[valid]]
|
||||||
|
ends = world_xyz[end_y[valid], end_x[valid]]
|
||||||
|
finite = np.all(np.isfinite(starts), axis=1) & np.all(np.isfinite(ends), axis=1)
|
||||||
|
if not np.any(finite):
|
||||||
|
return empty, empty, []
|
||||||
|
|
||||||
|
starts = starts[finite]
|
||||||
|
ends = ends[finite]
|
||||||
|
non_zero = np.linalg.norm(ends - starts, axis=1) > 1e-9
|
||||||
|
if not np.any(non_zero):
|
||||||
|
return empty, empty, []
|
||||||
|
|
||||||
|
starts = starts[non_zero]
|
||||||
|
ends = ends[non_zero]
|
||||||
|
junctions = starts + tail_fraction * (ends - starts)
|
||||||
|
|
||||||
|
seg_starts = np.vstack((starts, junctions))
|
||||||
|
seg_ends = np.vstack((junctions, ends))
|
||||||
|
n = len(starts)
|
||||||
|
roles: list[SegmentRole] = ["tail"] * n + ["head"] * n
|
||||||
|
return seg_starts, seg_ends, roles
|
||||||
|
|
||||||
|
|
||||||
|
def build_flow_arrow_lines(
|
||||||
|
world_xyz: np.ndarray,
|
||||||
|
flow: np.ndarray,
|
||||||
|
*,
|
||||||
|
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
|
||||||
|
valid_mask: np.ndarray | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""Build line indices for flow arrows (used by tests and debugging)."""
|
||||||
|
empty_pts = np.empty((0, 3), dtype=np.float64)
|
||||||
|
empty_lines = np.empty((0, 2), dtype=np.int32)
|
||||||
|
empty_colors = np.empty((0, 3), dtype=np.float64)
|
||||||
|
|
||||||
|
seg_starts, seg_ends, roles = _sample_flow_segments(
|
||||||
|
world_xyz, flow, style=style, valid_mask=valid_mask
|
||||||
|
)
|
||||||
|
if len(seg_starts) == 0:
|
||||||
|
return empty_pts, empty_lines, empty_colors
|
||||||
|
|
||||||
|
points = np.empty((len(seg_starts) * 2, 3), dtype=np.float64)
|
||||||
|
points[0::2] = seg_starts
|
||||||
|
points[1::2] = seg_ends
|
||||||
|
lines = np.column_stack(
|
||||||
|
(np.arange(0, len(points), 2, dtype=np.int32), np.arange(1, len(points), 2, dtype=np.int32))
|
||||||
|
)
|
||||||
|
line_colors = np.vstack(
|
||||||
|
[effective_segment_color(style, role, use_alpha_blend=True) for role in roles]
|
||||||
|
)
|
||||||
|
return points, lines, line_colors
|
||||||
|
|
||||||
|
|
||||||
|
def _cylinder_template(o3d):
|
||||||
|
global _CYLINDER_TEMPLATE_VERTICES, _CYLINDER_TEMPLATE_TRIANGLES
|
||||||
|
if _CYLINDER_TEMPLATE_VERTICES is None:
|
||||||
|
cyl = o3d.geometry.TriangleMesh.create_cylinder(
|
||||||
|
radius=1.0,
|
||||||
|
height=1.0,
|
||||||
|
resolution=8,
|
||||||
|
split=1,
|
||||||
|
)
|
||||||
|
_CYLINDER_TEMPLATE_VERTICES = np.asarray(cyl.vertices, dtype=np.float64)
|
||||||
|
_CYLINDER_TEMPLATE_TRIANGLES = np.asarray(cyl.triangles, dtype=np.int32)
|
||||||
|
return _CYLINDER_TEMPLATE_VERTICES, _CYLINDER_TEMPLATE_TRIANGLES
|
||||||
|
|
||||||
|
|
||||||
|
def _cone_apex_at_low_z(vertices: np.ndarray) -> bool:
|
||||||
|
"""True when the cone tip is on the low-z side of the Open3D template."""
|
||||||
|
radii = np.linalg.norm(vertices[:, :2], axis=1)
|
||||||
|
apex_z = float(vertices[np.argmin(radii), 2])
|
||||||
|
z_mid = 0.5 * (float(vertices[:, 2].min()) + float(vertices[:, 2].max()))
|
||||||
|
return apex_z < z_mid
|
||||||
|
|
||||||
|
|
||||||
|
def _cone_template(o3d):
|
||||||
|
global _CONE_TEMPLATE_VERTICES, _CONE_TEMPLATE_TRIANGLES, _CONE_APEX_AT_LOW_Z
|
||||||
|
if _CONE_TEMPLATE_VERTICES is None:
|
||||||
|
cone = o3d.geometry.TriangleMesh.create_cone(
|
||||||
|
radius=1.0,
|
||||||
|
height=1.0,
|
||||||
|
resolution=8,
|
||||||
|
split=1,
|
||||||
|
)
|
||||||
|
_CONE_TEMPLATE_VERTICES = np.asarray(cone.vertices, dtype=np.float64)
|
||||||
|
_CONE_TEMPLATE_TRIANGLES = np.asarray(cone.triangles, dtype=np.int32)
|
||||||
|
_CONE_APEX_AT_LOW_Z = _cone_apex_at_low_z(_CONE_TEMPLATE_VERTICES)
|
||||||
|
return _CONE_TEMPLATE_VERTICES, _CONE_TEMPLATE_TRIANGLES
|
||||||
|
|
||||||
|
|
||||||
|
def _transform_axis_segment(
|
||||||
|
template_vertices: np.ndarray,
|
||||||
|
start: np.ndarray,
|
||||||
|
end: np.ndarray,
|
||||||
|
radius: float,
|
||||||
|
*,
|
||||||
|
apex_at_end: bool = False,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Place a +Z template from ``start`` to ``end`` (axis anchored, not midpoint-centered).
|
||||||
|
|
||||||
|
Open3D cylinders are centered on the origin; cone apex side is detected from the template.
|
||||||
|
``apex_at_end=True`` maps the template apex to ``end`` (flow tip); base to ``start``.
|
||||||
|
"""
|
||||||
|
direction = np.asarray(end, dtype=np.float64) - np.asarray(start, dtype=np.float64)
|
||||||
|
length = float(np.linalg.norm(direction))
|
||||||
|
if length <= 1e-9 or radius <= 0.0:
|
||||||
|
return np.empty((0, 3), dtype=np.float64)
|
||||||
|
|
||||||
|
dir_hat = direction / length
|
||||||
|
rotation = _rotation_matrix_z_to(dir_hat)
|
||||||
|
scaled = np.asarray(template_vertices, dtype=np.float64).copy()
|
||||||
|
|
||||||
|
z_min = float(scaled[:, 2].min())
|
||||||
|
z_max = float(scaled[:, 2].max())
|
||||||
|
z_span = z_max - z_min
|
||||||
|
if z_span <= 1e-9:
|
||||||
|
return np.empty((0, 3), dtype=np.float64)
|
||||||
|
|
||||||
|
if apex_at_end:
|
||||||
|
t = (z_max - scaled[:, 2]) / z_span * length
|
||||||
|
else:
|
||||||
|
t = (scaled[:, 2] - z_min) / z_span * length
|
||||||
|
|
||||||
|
radial = np.column_stack((scaled[:, 0] * radius, scaled[:, 1] * radius, t))
|
||||||
|
return radial @ rotation.T + np.asarray(start, dtype=np.float64)
|
||||||
|
|
||||||
|
|
||||||
|
def _transform_cylinder_segment(
|
||||||
|
template_vertices: np.ndarray,
|
||||||
|
start: np.ndarray,
|
||||||
|
end: np.ndarray,
|
||||||
|
radius: float,
|
||||||
|
) -> np.ndarray:
|
||||||
|
return _transform_axis_segment(template_vertices, start, end, radius)
|
||||||
|
|
||||||
|
|
||||||
|
def build_flow_arrow_mesh(
|
||||||
|
world_xyz: np.ndarray,
|
||||||
|
flow: np.ndarray,
|
||||||
|
mesh_span: float,
|
||||||
|
*,
|
||||||
|
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
|
||||||
|
valid_mask: np.ndarray | None = None,
|
||||||
|
o3d=None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
|
||||||
|
"""Build merged mesh flow arrows: cylinder tail + cone head per arrow."""
|
||||||
|
empty_v = np.empty((0, 3), dtype=np.float64)
|
||||||
|
empty_t = np.empty((0, 3), dtype=np.int32)
|
||||||
|
empty_c = np.empty((0, 3), dtype=np.float64)
|
||||||
|
|
||||||
|
seg_starts, seg_ends, roles = _sample_flow_segments(
|
||||||
|
world_xyz, flow, style=style, valid_mask=valid_mask
|
||||||
|
)
|
||||||
|
if len(seg_starts) == 0:
|
||||||
|
return empty_v, empty_t, empty_c
|
||||||
|
|
||||||
|
if o3d is None:
|
||||||
|
import open3d as o3d
|
||||||
|
|
||||||
|
cyl_vertices, cyl_triangles = _cylinder_template(o3d)
|
||||||
|
cone_vertices, cone_triangles = _cone_template(o3d)
|
||||||
|
|
||||||
|
vertices_parts: list[np.ndarray] = []
|
||||||
|
triangles_parts: list[np.ndarray] = []
|
||||||
|
colors_parts: list[np.ndarray] = []
|
||||||
|
vertex_offset = 0
|
||||||
|
|
||||||
|
for start, end, role in zip(seg_starts, seg_ends, roles):
|
||||||
|
seg_start = np.asarray(start, dtype=np.float64)
|
||||||
|
seg_end = np.asarray(end, dtype=np.float64)
|
||||||
|
axis = seg_end - seg_start
|
||||||
|
seg_len = float(np.linalg.norm(axis))
|
||||||
|
if seg_len <= 1e-9:
|
||||||
|
continue
|
||||||
|
|
||||||
|
radius = segment_radius_for_role(mesh_span, style, role)
|
||||||
|
if role == "tail":
|
||||||
|
template_vertices, template_triangles = cyl_vertices, cyl_triangles
|
||||||
|
apex_at_end = False
|
||||||
|
overlap = _TAIL_HEAD_OVERLAP_FRACTION * seg_len
|
||||||
|
seg_end = seg_end + (axis / seg_len) * overlap
|
||||||
|
else:
|
||||||
|
template_vertices, template_triangles = cone_vertices, cone_triangles
|
||||||
|
apex_at_end = not bool(_CONE_APEX_AT_LOW_Z)
|
||||||
|
tail_radius = segment_radius_for_role(mesh_span, style, "tail")
|
||||||
|
radius = max(radius, tail_radius)
|
||||||
|
|
||||||
|
verts = _transform_axis_segment(
|
||||||
|
template_vertices,
|
||||||
|
seg_start,
|
||||||
|
seg_end,
|
||||||
|
radius,
|
||||||
|
apex_at_end=apex_at_end,
|
||||||
|
)
|
||||||
|
if verts.size == 0:
|
||||||
|
continue
|
||||||
|
color = effective_segment_color(style, role, use_alpha_blend=True)
|
||||||
|
vertices_parts.append(verts)
|
||||||
|
triangles_parts.append(template_triangles + vertex_offset)
|
||||||
|
colors_parts.append(np.tile(color, (len(verts), 1)))
|
||||||
|
vertex_offset += len(verts)
|
||||||
|
|
||||||
|
if not vertices_parts:
|
||||||
|
return empty_v, empty_t, empty_c
|
||||||
|
|
||||||
|
return (
|
||||||
|
np.vstack(vertices_parts),
|
||||||
|
np.vstack(triangles_parts),
|
||||||
|
np.vstack(colors_parts),
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def create_flow_mesh_geometry(o3d):
|
||||||
|
"""Create a persistent Open3D mesh for the flow arrow overlay."""
|
||||||
|
mesh = o3d.geometry.TriangleMesh()
|
||||||
|
mesh.vertices = o3d.utility.Vector3dVector(np.empty((0, 3), dtype=np.float64))
|
||||||
|
mesh.triangles = o3d.utility.Vector3iVector(np.empty((0, 3), dtype=np.int32))
|
||||||
|
mesh.vertex_colors = o3d.utility.Vector3dVector(np.empty((0, 3), dtype=np.float64))
|
||||||
|
mesh.compute_vertex_normals()
|
||||||
|
return mesh
|
||||||
|
|
||||||
|
|
||||||
|
class FlowArrowOverlay:
|
||||||
|
"""Example-layer flow arrows as cylinder tail + cone head meshes on the UV surface."""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
mesh: UVSurfaceMesh,
|
||||||
|
*,
|
||||||
|
style: FlowArrowStyle | None = None,
|
||||||
|
):
|
||||||
|
import open3d as o3d
|
||||||
|
|
||||||
|
self.mesh = mesh
|
||||||
|
self.style = style or DEFAULT_FLOW_ARROW_STYLE
|
||||||
|
self._mesh_span = _mesh_span(mesh.rest_vertices)
|
||||||
|
_cylinder_template(o3d)
|
||||||
|
_cone_template(o3d)
|
||||||
|
self.arrow_mesh = create_flow_mesh_geometry(o3d) if self.style.enabled else None
|
||||||
|
|
||||||
|
@property
|
||||||
|
def enabled(self) -> bool:
|
||||||
|
return self.style.enabled
|
||||||
|
|
||||||
|
def compute(
|
||||||
|
self,
|
||||||
|
flow,
|
||||||
|
valid_mask: np.ndarray | None = None,
|
||||||
|
) -> tuple[np.ndarray, np.ndarray, np.ndarray] | None:
|
||||||
|
if not self.style.enabled or self.arrow_mesh is None:
|
||||||
|
return None
|
||||||
|
|
||||||
|
return build_flow_arrow_mesh(
|
||||||
|
self.mesh.world_xyz,
|
||||||
|
flow,
|
||||||
|
self._mesh_span,
|
||||||
|
style=self.style,
|
||||||
|
valid_mask=valid_mask,
|
||||||
|
)
|
||||||
|
|
||||||
|
def apply(self, arrow_vertices: np.ndarray, arrow_triangles: np.ndarray, arrow_colors: np.ndarray) -> None:
|
||||||
|
"""Update the persistent flow mesh geometry in place."""
|
||||||
|
import open3d as o3d
|
||||||
|
|
||||||
|
self.arrow_mesh.vertices = o3d.utility.Vector3dVector(arrow_vertices.astype(np.float64))
|
||||||
|
self.arrow_mesh.triangles = o3d.utility.Vector3iVector(arrow_triangles.astype(np.int32))
|
||||||
|
self.arrow_mesh.vertex_colors = o3d.utility.Vector3dVector(arrow_colors.astype(np.float64))
|
||||||
|
self.arrow_mesh.compute_vertex_normals()
|
||||||
@@ -42,13 +42,13 @@ class ForceChartWindow:
|
|||||||
for channel in range(3):
|
for channel in range(3):
|
||||||
self.data[:, 0, channel] = np.linspace(-npoints, 0, npoints) / sample_rate_hz
|
self.data[:, 0, channel] = np.linspace(-npoints, 0, npoints) / sample_rate_hz
|
||||||
|
|
||||||
p1 = self.win.addPlot(title="法向力")
|
p1 = self.win.addPlot(title="Normal Deformation")
|
||||||
p1.setLabel("bottom", "时间", units="s")
|
p1.setLabel("bottom", "时间", units="s")
|
||||||
self.win.nextRow()
|
self.win.nextRow()
|
||||||
p2 = self.win.addPlot(title="切向力 X")
|
p2 = self.win.addPlot(title="X Deformation")
|
||||||
p2.setLabel("bottom", "时间", units="s")
|
p2.setLabel("bottom", "时间", units="s")
|
||||||
self.win.nextRow()
|
self.win.nextRow()
|
||||||
p3 = self.win.addPlot(title="切向力 Y")
|
p3 = self.win.addPlot(title="Y Deformation")
|
||||||
p3.setLabel("bottom", "时间", units="s")
|
p3.setLabel("bottom", "时间", units="s")
|
||||||
|
|
||||||
self._plots = (p1, p2, p3)
|
self._plots = (p1, p2, p3)
|
||||||
|
|||||||
@@ -10,6 +10,7 @@ from orisys.finger.mesh import UVSurfaceMesh
|
|||||||
from orisys.finger.viewer import flow_magnitude_vertex_colors, scalar_map_vertex_colors
|
from orisys.finger.viewer import flow_magnitude_vertex_colors, scalar_map_vertex_colors
|
||||||
|
|
||||||
from .contact_arrow import ContactArrowOverlay
|
from .contact_arrow import ContactArrowOverlay
|
||||||
|
from .flow_arrow import FlowArrowOverlay
|
||||||
from .mesh_camera import (
|
from .mesh_camera import (
|
||||||
apply_filament_sun_light,
|
apply_filament_sun_light,
|
||||||
apply_shade_factors,
|
apply_shade_factors,
|
||||||
@@ -64,6 +65,16 @@ def _vertex_colors(mesh, *, color_mode, threshold, value_min, value_max, flow=No
|
|||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def _background_rgba01(color: tuple[int, int, int]) -> list[float]:
|
||||||
|
r, g, b = (int(color[0]), int(color[1]), int(color[2]))
|
||||||
|
return [r / 255.0, g / 255.0, b / 255.0, 1.0]
|
||||||
|
|
||||||
|
|
||||||
|
def _background_rgb01(color: tuple[int, int, int]) -> np.ndarray:
|
||||||
|
return np.asarray(_background_rgba01(color)[:3], dtype=np.float64)
|
||||||
|
|
||||||
|
|
||||||
class _FilamentOffscreenBackend:
|
class _FilamentOffscreenBackend:
|
||||||
"""Filament OffscreenRenderer (Linux headless / platforms with EGL)."""
|
"""Filament OffscreenRenderer (Linux headless / platforms with EGL)."""
|
||||||
|
|
||||||
@@ -81,7 +92,9 @@ class _FilamentOffscreenBackend:
|
|||||||
lookat: np.ndarray,
|
lookat: np.ndarray,
|
||||||
show_contact_arrow: bool,
|
show_contact_arrow: bool,
|
||||||
mesh_lighting=None,
|
mesh_lighting=None,
|
||||||
|
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
||||||
contact_arrow_style=None,
|
contact_arrow_style=None,
|
||||||
|
flow_arrow_style=None,
|
||||||
):
|
):
|
||||||
import open3d as o3d
|
import open3d as o3d
|
||||||
|
|
||||||
@@ -96,6 +109,7 @@ class _FilamentOffscreenBackend:
|
|||||||
self.world_to_cam = world_to_cam
|
self.world_to_cam = world_to_cam
|
||||||
self.lookat = lookat
|
self.lookat = lookat
|
||||||
self.mesh_lighting = mesh_lighting
|
self.mesh_lighting = mesh_lighting
|
||||||
|
self.mesh_background = mesh_background
|
||||||
self._shade_factors = (
|
self._shade_factors = (
|
||||||
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
||||||
if mesh_lighting is not None
|
if mesh_lighting is not None
|
||||||
@@ -106,8 +120,9 @@ class _FilamentOffscreenBackend:
|
|||||||
enabled=show_contact_arrow,
|
enabled=show_contact_arrow,
|
||||||
style=contact_arrow_style,
|
style=contact_arrow_style,
|
||||||
)
|
)
|
||||||
|
self._flow_arrow = FlowArrowOverlay(mesh, style=flow_arrow_style)
|
||||||
self._renderer = o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
|
self._renderer = o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
|
||||||
self._renderer.scene.set_background([0.05, 0.05, 0.08, 1.0])
|
self._renderer.scene.set_background(_background_rgba01(self.mesh_background))
|
||||||
if mesh_lighting is not None:
|
if mesh_lighting is not None:
|
||||||
apply_filament_sun_light(
|
apply_filament_sun_light(
|
||||||
self._renderer,
|
self._renderer,
|
||||||
@@ -117,6 +132,7 @@ class _FilamentOffscreenBackend:
|
|||||||
self._material = _make_lit_material(o3d)
|
self._material = _make_lit_material(o3d)
|
||||||
self._mesh_added = False
|
self._mesh_added = False
|
||||||
self._arrow_added = False
|
self._arrow_added = False
|
||||||
|
self._flow_arrow_added = False
|
||||||
self._apply_camera()
|
self._apply_camera()
|
||||||
|
|
||||||
def _apply_camera(self) -> None:
|
def _apply_camera(self) -> None:
|
||||||
@@ -138,7 +154,7 @@ class _FilamentOffscreenBackend:
|
|||||||
self.width = width
|
self.width = width
|
||||||
self.height = height
|
self.height = height
|
||||||
self._renderer = self._o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
|
self._renderer = self._o3d.visualization.rendering.OffscreenRenderer(self.width, self.height)
|
||||||
self._renderer.scene.set_background([0.05, 0.05, 0.08, 1.0])
|
self._renderer.scene.set_background(_background_rgba01(self.mesh_background))
|
||||||
if self.mesh_lighting is not None:
|
if self.mesh_lighting is not None:
|
||||||
apply_filament_sun_light(
|
apply_filament_sun_light(
|
||||||
self._renderer,
|
self._renderer,
|
||||||
@@ -148,9 +164,19 @@ class _FilamentOffscreenBackend:
|
|||||||
self._material = _make_lit_material(self._o3d)
|
self._material = _make_lit_material(self._o3d)
|
||||||
self._mesh_added = False
|
self._mesh_added = False
|
||||||
self._arrow_added = False
|
self._arrow_added = False
|
||||||
|
self._flow_arrow_added = False
|
||||||
self._apply_camera()
|
self._apply_camera()
|
||||||
|
|
||||||
def render(self, *, flow=None, scalar_map=None, centroid=None, centroid_found=False, fnormal=0.0):
|
def render(
|
||||||
|
self,
|
||||||
|
*,
|
||||||
|
flow=None,
|
||||||
|
scalar_map=None,
|
||||||
|
centroid=None,
|
||||||
|
centroid_found=False,
|
||||||
|
fnormal=0.0,
|
||||||
|
valid_mask=None,
|
||||||
|
):
|
||||||
o3d = self._o3d
|
o3d = self._o3d
|
||||||
colors = _vertex_colors(
|
colors = _vertex_colors(
|
||||||
self.mesh,
|
self.mesh,
|
||||||
@@ -185,6 +211,21 @@ class _FilamentOffscreenBackend:
|
|||||||
self._renderer.scene.remove_geometry("contact_arrow")
|
self._renderer.scene.remove_geometry("contact_arrow")
|
||||||
self._arrow_added = False
|
self._arrow_added = False
|
||||||
|
|
||||||
|
flow_result = self._flow_arrow.compute(flow, valid_mask=valid_mask)
|
||||||
|
if flow_result is not None and self._flow_arrow.arrow_mesh is not None:
|
||||||
|
flow_vertices, flow_triangles, flow_colors = flow_result
|
||||||
|
if len(flow_vertices) > 0:
|
||||||
|
flow_mesh = _mesh_to_o3d(o3d, flow_vertices, flow_triangles, flow_colors)
|
||||||
|
if not self._flow_arrow_added:
|
||||||
|
self._renderer.scene.add_geometry("flow_arrows", flow_mesh, self._material)
|
||||||
|
self._flow_arrow_added = True
|
||||||
|
else:
|
||||||
|
self._renderer.scene.remove_geometry("flow_arrows")
|
||||||
|
self._renderer.scene.add_geometry("flow_arrows", flow_mesh, self._material)
|
||||||
|
elif self._flow_arrow_added:
|
||||||
|
self._renderer.scene.remove_geometry("flow_arrows")
|
||||||
|
self._flow_arrow_added = False
|
||||||
|
|
||||||
return _numpy_o3d_image_to_rgb(self._renderer.render_to_image())
|
return _numpy_o3d_image_to_rgb(self._renderer.render_to_image())
|
||||||
|
|
||||||
def close(self) -> None:
|
def close(self) -> None:
|
||||||
@@ -208,7 +249,9 @@ class _HiddenVisualizerBackend:
|
|||||||
lookat: np.ndarray,
|
lookat: np.ndarray,
|
||||||
show_contact_arrow: bool,
|
show_contact_arrow: bool,
|
||||||
mesh_lighting=None,
|
mesh_lighting=None,
|
||||||
|
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
||||||
contact_arrow_style=None,
|
contact_arrow_style=None,
|
||||||
|
flow_arrow_style=None,
|
||||||
):
|
):
|
||||||
import open3d as o3d
|
import open3d as o3d
|
||||||
|
|
||||||
@@ -223,6 +266,7 @@ class _HiddenVisualizerBackend:
|
|||||||
self.world_to_cam = world_to_cam
|
self.world_to_cam = world_to_cam
|
||||||
self.lookat = lookat
|
self.lookat = lookat
|
||||||
self.mesh_lighting = mesh_lighting
|
self.mesh_lighting = mesh_lighting
|
||||||
|
self.mesh_background = mesh_background
|
||||||
self._shade_factors = (
|
self._shade_factors = (
|
||||||
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting)
|
||||||
if mesh_lighting is not None
|
if mesh_lighting is not None
|
||||||
@@ -233,6 +277,7 @@ class _HiddenVisualizerBackend:
|
|||||||
enabled=show_contact_arrow,
|
enabled=show_contact_arrow,
|
||||||
style=contact_arrow_style,
|
style=contact_arrow_style,
|
||||||
)
|
)
|
||||||
|
self._flow_arrow = FlowArrowOverlay(mesh, style=flow_arrow_style)
|
||||||
self.vis = None
|
self.vis = None
|
||||||
self.tri_mesh = None
|
self.tri_mesh = None
|
||||||
self._create_window()
|
self._create_window()
|
||||||
@@ -268,9 +313,12 @@ class _HiddenVisualizerBackend:
|
|||||||
if self._contact_arrow.arrow_mesh is not None:
|
if self._contact_arrow.arrow_mesh is not None:
|
||||||
self.vis.add_geometry(self._contact_arrow.arrow_mesh)
|
self.vis.add_geometry(self._contact_arrow.arrow_mesh)
|
||||||
|
|
||||||
|
if self._flow_arrow.enabled and self._flow_arrow.arrow_mesh is not None:
|
||||||
|
self.vis.add_geometry(self._flow_arrow.arrow_mesh)
|
||||||
|
|
||||||
render_option = self.vis.get_render_option()
|
render_option = self.vis.get_render_option()
|
||||||
render_option.mesh_show_back_face = True
|
render_option.mesh_show_back_face = True
|
||||||
render_option.background_color = np.array([0.05, 0.05, 0.08])
|
render_option.background_color = _background_rgb01(self.mesh_background)
|
||||||
if hasattr(render_option, "light_on"):
|
if hasattr(render_option, "light_on"):
|
||||||
render_option.light_on = False
|
render_option.light_on = False
|
||||||
|
|
||||||
@@ -298,7 +346,16 @@ class _HiddenVisualizerBackend:
|
|||||||
self.height = height
|
self.height = height
|
||||||
self._create_window()
|
self._create_window()
|
||||||
|
|
||||||
def render(self, *, flow=None, scalar_map=None, centroid=None, centroid_found=False, fnormal=0.0):
|
def render(
|
||||||
|
self,
|
||||||
|
*,
|
||||||
|
flow=None,
|
||||||
|
scalar_map=None,
|
||||||
|
centroid=None,
|
||||||
|
centroid_found=False,
|
||||||
|
fnormal=0.0,
|
||||||
|
valid_mask=None,
|
||||||
|
):
|
||||||
o3d = self._o3d
|
o3d = self._o3d
|
||||||
colors = _vertex_colors(
|
colors = _vertex_colors(
|
||||||
self.mesh,
|
self.mesh,
|
||||||
@@ -320,6 +377,12 @@ class _HiddenVisualizerBackend:
|
|||||||
self._contact_arrow.apply(arrow_vertices, arrow_colors)
|
self._contact_arrow.apply(arrow_vertices, arrow_colors)
|
||||||
self.vis.update_geometry(self._contact_arrow.arrow_mesh)
|
self.vis.update_geometry(self._contact_arrow.arrow_mesh)
|
||||||
|
|
||||||
|
flow_result = self._flow_arrow.compute(flow, valid_mask=valid_mask)
|
||||||
|
if flow_result is not None and self._flow_arrow.arrow_mesh is not None:
|
||||||
|
flow_vertices, flow_triangles, flow_colors = flow_result
|
||||||
|
self._flow_arrow.apply(flow_vertices, flow_triangles, flow_colors)
|
||||||
|
self.vis.update_geometry(self._flow_arrow.arrow_mesh)
|
||||||
|
|
||||||
self.vis.poll_events()
|
self.vis.poll_events()
|
||||||
self.vis.update_renderer()
|
self.vis.update_renderer()
|
||||||
return _numpy_o3d_image_to_rgb(self.vis.capture_screen_float_buffer(do_render=False))
|
return _numpy_o3d_image_to_rgb(self.vis.capture_screen_float_buffer(do_render=False))
|
||||||
@@ -340,7 +403,7 @@ def _create_backend(mesh: UVSurfaceMesh, **kwargs):
|
|||||||
return _HiddenVisualizerBackend(mesh, **kwargs)
|
return _HiddenVisualizerBackend(mesh, **kwargs)
|
||||||
|
|
||||||
class OffscreenFingerMeshRenderer:
|
class OffscreenFingerMeshRenderer:
|
||||||
"""Render finger UV mesh + contact arrow to an RGB image for Qt embedding."""
|
"""Render finger UV mesh + contact/flow arrows to an RGB image for Qt embedding."""
|
||||||
|
|
||||||
def __init__(
|
def __init__(
|
||||||
self,
|
self,
|
||||||
@@ -356,7 +419,9 @@ class OffscreenFingerMeshRenderer:
|
|||||||
lookat: np.ndarray | None = None,
|
lookat: np.ndarray | None = None,
|
||||||
show_contact_arrow: bool = True,
|
show_contact_arrow: bool = True,
|
||||||
mesh_lighting=None,
|
mesh_lighting=None,
|
||||||
|
mesh_background: tuple[int, int, int] = (0, 0, 0),
|
||||||
contact_arrow_style=None,
|
contact_arrow_style=None,
|
||||||
|
flow_arrow_style=None,
|
||||||
):
|
):
|
||||||
lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0)
|
lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0)
|
||||||
self._backend = _create_backend(
|
self._backend = _create_backend(
|
||||||
@@ -371,7 +436,9 @@ class OffscreenFingerMeshRenderer:
|
|||||||
lookat=lookat,
|
lookat=lookat,
|
||||||
show_contact_arrow=show_contact_arrow,
|
show_contact_arrow=show_contact_arrow,
|
||||||
mesh_lighting=mesh_lighting,
|
mesh_lighting=mesh_lighting,
|
||||||
|
mesh_background=mesh_background,
|
||||||
contact_arrow_style=contact_arrow_style,
|
contact_arrow_style=contact_arrow_style,
|
||||||
|
flow_arrow_style=flow_arrow_style,
|
||||||
)
|
)
|
||||||
|
|
||||||
@property
|
@property
|
||||||
@@ -393,6 +460,7 @@ class OffscreenFingerMeshRenderer:
|
|||||||
centroid=None,
|
centroid=None,
|
||||||
centroid_found: bool = False,
|
centroid_found: bool = False,
|
||||||
fnormal: float = 0.0,
|
fnormal: float = 0.0,
|
||||||
|
valid_mask=None,
|
||||||
) -> np.ndarray:
|
) -> np.ndarray:
|
||||||
return self._backend.render(
|
return self._backend.render(
|
||||||
flow=flow,
|
flow=flow,
|
||||||
@@ -400,6 +468,7 @@ class OffscreenFingerMeshRenderer:
|
|||||||
centroid=centroid,
|
centroid=centroid,
|
||||||
centroid_found=centroid_found,
|
centroid_found=centroid_found,
|
||||||
fnormal=fnormal,
|
fnormal=fnormal,
|
||||||
|
valid_mask=valid_mask,
|
||||||
)
|
)
|
||||||
|
|
||||||
def close(self) -> None:
|
def close(self) -> None:
|
||||||
|
|||||||
@@ -1,57 +0,0 @@
|
|||||||
"""OpenCV visualization helpers for example scripts."""
|
|
||||||
|
|
||||||
from __future__ import annotations
|
|
||||||
|
|
||||||
import cv2
|
|
||||||
import numpy as np
|
|
||||||
|
|
||||||
|
|
||||||
def depth_colormap(depth_map, colormap=cv2.COLORMAP_JET) -> np.ndarray:
|
|
||||||
"""Normalize a scalar map to uint8 and apply an OpenCV colormap."""
|
|
||||||
values = np.asarray(depth_map)
|
|
||||||
if values.size == 0:
|
|
||||||
return np.zeros((1, 1, 3), dtype=np.uint8)
|
|
||||||
if values.max() > values.min():
|
|
||||||
normalized = ((values - values.min()) / (values.max() - values.min()) * 255).astype(np.uint8)
|
|
||||||
else:
|
|
||||||
normalized = np.zeros_like(values, dtype=np.uint8)
|
|
||||||
return cv2.applyColorMap(normalized, colormap)
|
|
||||||
|
|
||||||
|
|
||||||
def draw_magnitude_map(
|
|
||||||
vfield,
|
|
||||||
threshold=3.0,
|
|
||||||
colormap=cv2.COLORMAP_JET,
|
|
||||||
output_size=(400, 400),
|
|
||||||
) -> np.ndarray:
|
|
||||||
"""Render a smoothed optical-flow magnitude map as a BGR pseudo-color image."""
|
|
||||||
magnitude = np.sqrt(vfield[:, :, 0] ** 2 + vfield[:, :, 1] ** 2)
|
|
||||||
|
|
||||||
downsample_factor = 10
|
|
||||||
h, w = magnitude.shape
|
|
||||||
magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor]
|
|
||||||
magnitude_smoothed = cv2.resize(magnitude_downsampled, (w, h), interpolation=cv2.INTER_CUBIC)
|
|
||||||
|
|
||||||
magnitude_thresholded = magnitude_smoothed.copy()
|
|
||||||
magnitude_thresholded[magnitude_smoothed < threshold] = 0
|
|
||||||
|
|
||||||
mag_min, mag_max = magnitude_thresholded.min(), magnitude_thresholded.max()
|
|
||||||
if mag_max > mag_min:
|
|
||||||
magnitude_norm = ((magnitude_thresholded - mag_min) / (mag_max - mag_min) * 255).astype(np.uint8)
|
|
||||||
else:
|
|
||||||
magnitude_norm = np.zeros_like(magnitude_thresholded, dtype=np.uint8)
|
|
||||||
|
|
||||||
magnitude_colored = cv2.applyColorMap(magnitude_norm, colormap)
|
|
||||||
out_w, out_h = output_size
|
|
||||||
return cv2.resize(magnitude_colored, (out_w, out_h), interpolation=cv2.INTER_CUBIC)
|
|
||||||
|
|
||||||
|
|
||||||
def frame_to_bgr(frame: np.ndarray) -> np.ndarray:
|
|
||||||
"""Convert a grayscale or single-channel frame to BGR for display/recording."""
|
|
||||||
if frame is None:
|
|
||||||
raise ValueError("frame is None")
|
|
||||||
if len(frame.shape) == 2:
|
|
||||||
return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
|
|
||||||
if len(frame.shape) == 3 and frame.shape[2] == 1:
|
|
||||||
return cv2.cvtColor(frame.squeeze(2), cv2.COLOR_GRAY2BGR)
|
|
||||||
return frame
|
|
||||||
@@ -7,7 +7,7 @@ from pathlib import Path
|
|||||||
|
|
||||||
import cv2
|
import cv2
|
||||||
|
|
||||||
from .opencv_maps import frame_to_bgr
|
from .render_2d import frame_to_bgr
|
||||||
|
|
||||||
|
|
||||||
class FrameRecorder:
|
class FrameRecorder:
|
||||||
|
|||||||
@@ -2,9 +2,51 @@
|
|||||||
|
|
||||||
from __future__ import annotations
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from typing import TYPE_CHECKING, Literal
|
||||||
|
|
||||||
|
import cv2
|
||||||
|
import numpy as np
|
||||||
import orisys
|
import orisys
|
||||||
|
|
||||||
from .opencv_maps import depth_colormap
|
if TYPE_CHECKING:
|
||||||
|
from pipeline.frame import FingerFrameView
|
||||||
|
|
||||||
|
ArrowBackground = Literal["unrolled", "white"]
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass(frozen=True)
|
||||||
|
class ArrowCanvasColors:
|
||||||
|
"""BGR fill colors for synthetic UV arrow background (``background='white'``)."""
|
||||||
|
|
||||||
|
outside_bgr: tuple[int, int, int] = (0, 0, 0)
|
||||||
|
uv_bgr: tuple[int, int, int] = (255, 255, 255)
|
||||||
|
|
||||||
|
|
||||||
|
DEFAULT_ARROW_CANVAS_COLORS = ArrowCanvasColors()
|
||||||
|
|
||||||
|
|
||||||
|
def depth_colormap(depth_map, colormap=cv2.COLORMAP_JET) -> np.ndarray:
|
||||||
|
"""Normalize a scalar map to uint8 and apply an OpenCV colormap."""
|
||||||
|
values = np.asarray(depth_map)
|
||||||
|
if values.size == 0:
|
||||||
|
return np.zeros((1, 1, 3), dtype=np.uint8)
|
||||||
|
if values.max() > values.min():
|
||||||
|
normalized = ((values - values.min()) / (values.max() - values.min()) * 255).astype(np.uint8)
|
||||||
|
else:
|
||||||
|
normalized = np.zeros_like(values, dtype=np.uint8)
|
||||||
|
return cv2.applyColorMap(normalized, colormap)
|
||||||
|
|
||||||
|
|
||||||
|
def frame_to_bgr(frame: np.ndarray) -> np.ndarray:
|
||||||
|
"""Convert a grayscale or single-channel frame to BGR for display/recording."""
|
||||||
|
if frame is None:
|
||||||
|
raise ValueError("frame is None")
|
||||||
|
if len(frame.shape) == 2:
|
||||||
|
return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
|
||||||
|
if len(frame.shape) == 3 and frame.shape[2] == 1:
|
||||||
|
return cv2.cvtColor(frame.squeeze(2), cv2.COLOR_GRAY2BGR)
|
||||||
|
return frame
|
||||||
|
|
||||||
|
|
||||||
def render_depth_panel(depth_map):
|
def render_depth_panel(depth_map):
|
||||||
@@ -12,21 +54,55 @@ def render_depth_panel(depth_map):
|
|||||||
return depth_colormap(depth_map)
|
return depth_colormap(depth_map)
|
||||||
|
|
||||||
|
|
||||||
|
def _as_bgr(color: tuple[int, int, int]) -> tuple[int, int, int]:
|
||||||
|
return tuple(int(np.clip(c, 0, 255)) for c in color[:3])
|
||||||
|
|
||||||
|
|
||||||
|
def _arrow_background(
|
||||||
|
frame: FingerFrameView,
|
||||||
|
background: ArrowBackground,
|
||||||
|
*,
|
||||||
|
canvas_colors: ArrowCanvasColors = DEFAULT_ARROW_CANVAS_COLORS,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Return the BGR canvas for UV arrow drawing."""
|
||||||
|
if background == "unrolled":
|
||||||
|
return frame.unrolled
|
||||||
|
h, w = frame.unrolled.shape[:2]
|
||||||
|
outside = np.array(_as_bgr(canvas_colors.outside_bgr), dtype=np.uint8)
|
||||||
|
uv = np.array(_as_bgr(canvas_colors.uv_bgr), dtype=np.uint8)
|
||||||
|
canvas = np.broadcast_to(outside, (h, w, 3)).copy()
|
||||||
|
mask = frame.valid_mask
|
||||||
|
if mask is None:
|
||||||
|
return canvas
|
||||||
|
mask = np.asarray(mask, dtype=bool)
|
||||||
|
if mask.shape != (h, w):
|
||||||
|
mask_u8 = mask.astype(np.uint8)
|
||||||
|
if mask_u8.ndim > 2:
|
||||||
|
mask_u8 = mask_u8[:, :, 0]
|
||||||
|
mask = cv2.resize(mask_u8, (w, h), interpolation=cv2.INTER_NEAREST).astype(bool)
|
||||||
|
canvas[mask] = uv
|
||||||
|
return canvas
|
||||||
|
|
||||||
|
|
||||||
def render_uv_arrows(
|
def render_uv_arrows(
|
||||||
unrolled,
|
frame: FingerFrameView,
|
||||||
vnormal,
|
|
||||||
*,
|
*,
|
||||||
flow_threshold: float = 0.4,
|
flow_threshold: float = 0.4,
|
||||||
grid_spacing: int = 20,
|
grid_spacing: int = 20,
|
||||||
arrow_scale: float = 1.0,
|
arrow_scale: float = 1.0,
|
||||||
|
background: ArrowBackground = "unrolled",
|
||||||
|
canvas_colors: ArrowCanvasColors = DEFAULT_ARROW_CANVAS_COLORS,
|
||||||
):
|
):
|
||||||
"""Return a BGR image with UV unrolled flow arrows."""
|
"""Return a BGR UV arrow panel from a FingerFrameView (uses frame.valid_mask)."""
|
||||||
return orisys.util.draw_arrows(
|
return orisys.util.draw_arrows(
|
||||||
unrolled,
|
_arrow_background(frame, background, canvas_colors=canvas_colors),
|
||||||
vnormal,
|
frame.flow,
|
||||||
threshold=flow_threshold,
|
threshold=flow_threshold,
|
||||||
grid_spacing=grid_spacing,
|
grid_spacing=grid_spacing,
|
||||||
arrow_scale=arrow_scale,
|
arrow_scale=arrow_scale,
|
||||||
|
valid_mask=frame.valid_mask,
|
||||||
|
below_threshold_color = (128, 0, 0),
|
||||||
|
colormap = cv2.COLORMAP_JET
|
||||||
)
|
)
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -5,7 +5,7 @@ from __future__ import annotations
|
|||||||
import numpy as np
|
import numpy as np
|
||||||
from scipy.signal import butter, filtfilt
|
from scipy.signal import butter, filtfilt
|
||||||
|
|
||||||
def lowpass_filter(data, cutoff_hz, sample_rate_hz=30, order=2) -> np.ndarray:
|
def lowpass_filter(data, cutoff_hz=5, sample_rate_hz=30, order=2) -> np.ndarray:
|
||||||
"""Zero-phase Butterworth low-pass filter for uniformly sampled 1-D data."""
|
"""Zero-phase Butterworth low-pass filter for uniformly sampled 1-D data."""
|
||||||
y = np.asarray(data, dtype=np.float64)
|
y = np.asarray(data, dtype=np.float64)
|
||||||
if cutoff_hz == 0:
|
if cutoff_hz == 0:
|
||||||
|
|||||||
+50
-5
@@ -1,16 +1,50 @@
|
|||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
def draw_arrows(frame, optical_flow, fps=True, threshold=5, grid_spacing=40, arrow_scale=1.0, margin = 0.8, colormap=cv2.COLORMAP_JET, below_threshold_color=(0, 0, 255), edge_margin=15):
|
|
||||||
|
def _prepare_draw_mask(valid_mask, shape):
|
||||||
|
"""Resize a boolean mask to (height, width) for arrow culling."""
|
||||||
|
if valid_mask is None:
|
||||||
|
return None
|
||||||
|
h, w = shape[:2]
|
||||||
|
mask = np.asarray(valid_mask, dtype=bool)
|
||||||
|
if mask.shape != (h, w):
|
||||||
|
mask_u8 = mask.astype(np.uint8)
|
||||||
|
if mask_u8.ndim > 2:
|
||||||
|
mask_u8 = mask_u8[:, :, 0]
|
||||||
|
mask = cv2.resize(mask_u8, (w, h), interpolation=cv2.INTER_NEAREST).astype(bool)
|
||||||
|
return mask
|
||||||
|
|
||||||
|
|
||||||
|
def draw_arrows(
|
||||||
|
frame,
|
||||||
|
optical_flow,
|
||||||
|
fps=True,
|
||||||
|
threshold=5,
|
||||||
|
grid_spacing=40,
|
||||||
|
arrow_scale=1.0,
|
||||||
|
margin=0.8,
|
||||||
|
colormap=cv2.COLORMAP_JET,
|
||||||
|
below_threshold_color=(0, 0, 255),
|
||||||
|
edge_margin=15,
|
||||||
|
valid_mask=None,
|
||||||
|
):
|
||||||
"""
|
"""
|
||||||
Draw colored optical flow arrows on the image by magnitude (JET colormap).
|
Draw colored optical flow arrows on the image by magnitude (JET colormap).
|
||||||
Frame and optical_flow must have the same height/width; arrows are drawn in pixel coordinates.
|
Frame and optical_flow must have the same height/width; arrows are drawn in pixel coordinates.
|
||||||
|
|
||||||
|
Parameters
|
||||||
|
----------
|
||||||
|
valid_mask : array-like, optional
|
||||||
|
Boolean (H, W) mask. Grid points outside the mask are skipped (no arrows or dots).
|
||||||
"""
|
"""
|
||||||
image = frame.copy()
|
image = frame.copy()
|
||||||
if len(image.shape) == 2 or image.shape[2] == 1:
|
if len(image.shape) == 2 or image.shape[2] == 1:
|
||||||
image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
|
image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
|
||||||
|
|
||||||
h, w = image.shape[:2]
|
h, w = image.shape[:2]
|
||||||
|
mask = _prepare_draw_mask(valid_mask, (h, w))
|
||||||
|
|
||||||
# Arrow grid in pixel coordinates (assume frame and flow same size)
|
# Arrow grid in pixel coordinates (assume frame and flow same size)
|
||||||
rr = np.arange(grid_spacing // 2, h, grid_spacing)
|
rr = np.arange(grid_spacing // 2, h, grid_spacing)
|
||||||
cc = np.arange(grid_spacing // 2, w, grid_spacing)
|
cc = np.arange(grid_spacing // 2, w, grid_spacing)
|
||||||
@@ -30,11 +64,23 @@ def draw_arrows(frame, optical_flow, fps=True, threshold=5, grid_spacing=40, arr
|
|||||||
end_y = (rr + dy).astype(np.int32)
|
end_y = (rr + dy).astype(np.int32)
|
||||||
|
|
||||||
valid = (
|
valid = (
|
||||||
(cc >= edge_margin) & (cc < w - edge_margin) & (rr >= edge_margin) & (rr < h - edge_margin)
|
(cc >= edge_margin)
|
||||||
& (end_x >= 0) & (end_x < w) & (end_y >= 0) & (end_y < h)
|
& (cc < w - edge_margin)
|
||||||
|
& (rr >= edge_margin)
|
||||||
|
& (rr < h - edge_margin)
|
||||||
|
& (end_x >= 0)
|
||||||
|
& (end_x < w)
|
||||||
|
& (end_y >= 0)
|
||||||
|
& (end_y < h)
|
||||||
)
|
)
|
||||||
|
if mask is not None:
|
||||||
|
valid &= mask[rr, cc]
|
||||||
|
valid &= mask[end_y, end_x]
|
||||||
|
|
||||||
mag_range = mag.max() - mag.min()
|
mag_range = mag.max() - mag.min()
|
||||||
norm_mag = ((mag - mag.min()) / mag_range * 255).astype(np.uint8) if mag_range > 0 else np.zeros_like(mag, dtype=np.uint8)
|
norm_mag = (
|
||||||
|
((mag - mag.min()) / mag_range * 255).astype(np.uint8) if mag_range > 0 else np.zeros_like(mag, dtype=np.uint8)
|
||||||
|
)
|
||||||
norm_mag[mag < threshold] = 0
|
norm_mag[mag < threshold] = 0
|
||||||
colors = cv2.applyColorMap(norm_mag.reshape(-1, 1), colormap)
|
colors = cv2.applyColorMap(norm_mag.reshape(-1, 1), colormap)
|
||||||
|
|
||||||
@@ -44,7 +90,6 @@ def draw_arrows(frame, optical_flow, fps=True, threshold=5, grid_spacing=40, arr
|
|||||||
if mag[i] < threshold:
|
if mag[i] < threshold:
|
||||||
cv2.circle(image, pt1, radius=1, color=below_threshold_color, thickness=-1)
|
cv2.circle(image, pt1, radius=1, color=below_threshold_color, thickness=-1)
|
||||||
else:
|
else:
|
||||||
# thickness = max(1, int(2 * mag[i] / mag.max())) if mag.max() > 0 else 2
|
|
||||||
thickness = 1
|
thickness = 1
|
||||||
cv2.arrowedLine(image, pt1, pt2, color, thickness=thickness, tipLength=0.2)
|
cv2.arrowedLine(image, pt1, pt2, color, thickness=thickness, tipLength=0.2)
|
||||||
return image
|
return image
|
||||||
|
|||||||
Reference in New Issue
Block a user