added mesh surface arrows

This commit is contained in:
ykcheungab
2026-06-19 21:22:28 +08:00
parent 2e5c047d1e
commit 258948bcb7
20 changed files with 951 additions and 194 deletions
+68 -29
View File
@@ -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
View File
@@ -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
+3 -1
View File
@@ -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
View File
@@ -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)
+50
View File
@@ -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)
+8 -3
View File
@@ -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))
+6 -5
View File
@@ -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)
+4
View File
@@ -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
+34
View File
@@ -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,
)
+5 -30
View File
@@ -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)
+17 -4
View File
@@ -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",
+30 -8
View File
@@ -1,14 +1,19 @@
"""Finger 3D mesh viewer with example-only contact arrow overlay.""" """Finger 3D mesh viewer with example-only contact and flow arrow overlays."""
from __future__ import annotations from __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)
+431
View File
@@ -0,0 +1,431 @@
"""Optical-flow arrow tubes on the finger UV surface mesh (TriangleMesh cylinders)."""
from __future__ import annotations
from dataclasses import dataclass
from typing import Literal
import cv2
import numpy as np
from orisys.finger.mesh import DEFAULT_SURFACE_COLOR, UVSurfaceMesh
from .contact_arrow import _rotation_matrix_z_to
# Grey mesh tone used to approximate opacity on backends without true alpha.
_MESH_GREY_RGB = np.asarray(DEFAULT_SURFACE_COLOR, dtype=np.float64)
# Maps style.line_width to tube radius as a fraction of mesh extent.
LINE_WIDTH_TO_SPAN_FRACTION = 0.0003
# Slight tail extension into the head segment to avoid visible gaps.
_TAIL_HEAD_OVERLAP_FRACTION = 0.03
SegmentRole = Literal["tail", "head"]
_CYLINDER_TEMPLATE_VERTICES: np.ndarray | None = None
_CYLINDER_TEMPLATE_TRIANGLES: np.ndarray | None = None
_CONE_TEMPLATE_VERTICES: np.ndarray | None = None
_CONE_TEMPLATE_TRIANGLES: np.ndarray | None = None
_CONE_APEX_AT_LOW_Z: bool | None = None
@dataclass(frozen=True)
class FlowArrowStyle:
"""Visual tuning for 3D mesh flow arrows (dashboard: edit MESH_FLOW_ARROW in layout.py).
``line_width`` / ``head_line_width`` scale tail and head tube radius relative to mesh extent.
``tail_color`` / ``head_color`` are RGB in 0–255 (same convention as OpenCV BGR tuples, but RGB order).
"""
enabled: bool = True
threshold: float = 1.0
grid_spacing: int = 20
arrow_scale: float = 0.5
length_margin: float = 1.0
tail_color: tuple[int, int, int] = (38, 235, 89)
head_color: tuple[int, int, int] = (13, 115, 31)
head_fraction: float = 0.25
opacity: float = 0.65
line_width: float = 2.0
head_line_width: float = 3.0
edge_margin: int = 8
DEFAULT_FLOW_ARROW_STYLE = FlowArrowStyle()
def _mesh_span(vertices: np.ndarray) -> float:
extent = np.ptp(vertices, axis=0)
return float(max(np.max(extent), 1e-6))
def segment_radius(mesh_span: float, line_width: float) -> float:
"""World-space tube radius from mesh extent and a width knob."""
return mesh_span * max(float(line_width), 0.1) * LINE_WIDTH_TO_SPAN_FRACTION
def segment_radius_for_role(mesh_span: float, style: FlowArrowStyle, role: SegmentRole) -> float:
width = style.line_width if role == "tail" else style.head_line_width
return segment_radius(mesh_span, width)
def _prepare_uv_mask(valid_mask, shape: tuple[int, int]) -> np.ndarray | None:
if valid_mask is None:
return None
h, w = shape
mask = np.asarray(valid_mask, dtype=bool)
if mask.shape != (h, w):
mask_u8 = mask.astype(np.uint8)
if mask_u8.ndim > 2:
mask_u8 = mask_u8[:, :, 0]
mask = cv2.resize(mask_u8, (w, h), interpolation=cv2.INTER_NEAREST).astype(bool)
return mask
def _style_rgb01(style: FlowArrowStyle, role: SegmentRole) -> np.ndarray:
rgb = np.asarray(style.tail_color if role == "tail" else style.head_color, dtype=np.float64).reshape(3)
return np.clip(rgb / 255.0, 0.0, 1.0)
def effective_segment_color(
style: FlowArrowStyle,
role: SegmentRole,
*,
use_alpha_blend: bool = True,
) -> np.ndarray:
"""Return RGB in 0–1 for a tail or head segment."""
rgb = _style_rgb01(style, role)
if not use_alpha_blend:
return rgb
opacity = float(np.clip(style.opacity, 0.0, 1.0))
return np.clip(opacity * rgb + (1.0 - opacity) * _MESH_GREY_RGB, 0.0, 1.0)
def _sample_flow_segments(
world_xyz: np.ndarray,
flow: np.ndarray,
*,
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
valid_mask: np.ndarray | None = None,
) -> tuple[np.ndarray, np.ndarray, list[SegmentRole]]:
"""Return segment start/end world points and role per segment (tail then head)."""
empty = np.empty((0, 3), dtype=np.float64)
if flow is None or np.asarray(flow).size == 0:
return empty, empty, []
flow = np.asarray(flow, dtype=np.float64)
world_xyz = np.asarray(world_xyz, dtype=np.float64)
h, w = flow.shape[:2]
if world_xyz.shape[:2] != (h, w):
return empty, empty, []
grid_spacing = max(int(style.grid_spacing), 1)
edge_margin = int(style.edge_margin)
margin = float(style.length_margin)
arrow_scale = max(float(style.arrow_scale), 1e-9)
head_fraction = float(np.clip(style.head_fraction, 0.05, 0.95))
tail_fraction = 1.0 - head_fraction
rr = np.arange(grid_spacing // 2, h, grid_spacing)
cc = np.arange(grid_spacing // 2, w, grid_spacing)
rr, cc = np.meshgrid(rr, cc, indexing="ij")
rr, cc = rr.reshape(-1), cc.reshape(-1)
disp = 2.0 * flow[rr, cc, :]
mag = np.linalg.norm(disp, axis=1)
dx = disp[:, 0] / arrow_scale
dy = disp[:, 1] / arrow_scale
length = np.sqrt(dx * dx + dy * dy)
scale = np.where(
length > grid_spacing * margin,
grid_spacing / np.maximum(length, 1e-9),
1.0,
) * margin
dx, dy = dx * scale, dy * scale
end_x = np.clip(np.round(cc + dx).astype(np.int32), 0, w - 1)
end_y = np.clip(np.round(rr + dy).astype(np.int32), 0, h - 1)
valid = (
(cc >= edge_margin)
& (cc < w - edge_margin)
& (rr >= edge_margin)
& (rr < h - edge_margin)
& (mag >= float(style.threshold))
)
mask = _prepare_uv_mask(valid_mask, (h, w))
if mask is not None:
valid &= mask[rr, cc]
valid &= mask[end_y, end_x]
if not np.any(valid):
return empty, empty, []
starts = world_xyz[rr[valid], cc[valid]]
ends = world_xyz[end_y[valid], end_x[valid]]
finite = np.all(np.isfinite(starts), axis=1) & np.all(np.isfinite(ends), axis=1)
if not np.any(finite):
return empty, empty, []
starts = starts[finite]
ends = ends[finite]
non_zero = np.linalg.norm(ends - starts, axis=1) > 1e-9
if not np.any(non_zero):
return empty, empty, []
starts = starts[non_zero]
ends = ends[non_zero]
junctions = starts + tail_fraction * (ends - starts)
seg_starts = np.vstack((starts, junctions))
seg_ends = np.vstack((junctions, ends))
n = len(starts)
roles: list[SegmentRole] = ["tail"] * n + ["head"] * n
return seg_starts, seg_ends, roles
def build_flow_arrow_lines(
world_xyz: np.ndarray,
flow: np.ndarray,
*,
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
valid_mask: np.ndarray | None = None,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Build line indices for flow arrows (used by tests and debugging)."""
empty_pts = np.empty((0, 3), dtype=np.float64)
empty_lines = np.empty((0, 2), dtype=np.int32)
empty_colors = np.empty((0, 3), dtype=np.float64)
seg_starts, seg_ends, roles = _sample_flow_segments(
world_xyz, flow, style=style, valid_mask=valid_mask
)
if len(seg_starts) == 0:
return empty_pts, empty_lines, empty_colors
points = np.empty((len(seg_starts) * 2, 3), dtype=np.float64)
points[0::2] = seg_starts
points[1::2] = seg_ends
lines = np.column_stack(
(np.arange(0, len(points), 2, dtype=np.int32), np.arange(1, len(points), 2, dtype=np.int32))
)
line_colors = np.vstack(
[effective_segment_color(style, role, use_alpha_blend=True) for role in roles]
)
return points, lines, line_colors
def _cylinder_template(o3d):
global _CYLINDER_TEMPLATE_VERTICES, _CYLINDER_TEMPLATE_TRIANGLES
if _CYLINDER_TEMPLATE_VERTICES is None:
cyl = o3d.geometry.TriangleMesh.create_cylinder(
radius=1.0,
height=1.0,
resolution=8,
split=1,
)
_CYLINDER_TEMPLATE_VERTICES = np.asarray(cyl.vertices, dtype=np.float64)
_CYLINDER_TEMPLATE_TRIANGLES = np.asarray(cyl.triangles, dtype=np.int32)
return _CYLINDER_TEMPLATE_VERTICES, _CYLINDER_TEMPLATE_TRIANGLES
def _cone_apex_at_low_z(vertices: np.ndarray) -> bool:
"""True when the cone tip is on the low-z side of the Open3D template."""
radii = np.linalg.norm(vertices[:, :2], axis=1)
apex_z = float(vertices[np.argmin(radii), 2])
z_mid = 0.5 * (float(vertices[:, 2].min()) + float(vertices[:, 2].max()))
return apex_z < z_mid
def _cone_template(o3d):
global _CONE_TEMPLATE_VERTICES, _CONE_TEMPLATE_TRIANGLES, _CONE_APEX_AT_LOW_Z
if _CONE_TEMPLATE_VERTICES is None:
cone = o3d.geometry.TriangleMesh.create_cone(
radius=1.0,
height=1.0,
resolution=8,
split=1,
)
_CONE_TEMPLATE_VERTICES = np.asarray(cone.vertices, dtype=np.float64)
_CONE_TEMPLATE_TRIANGLES = np.asarray(cone.triangles, dtype=np.int32)
_CONE_APEX_AT_LOW_Z = _cone_apex_at_low_z(_CONE_TEMPLATE_VERTICES)
return _CONE_TEMPLATE_VERTICES, _CONE_TEMPLATE_TRIANGLES
def _transform_axis_segment(
template_vertices: np.ndarray,
start: np.ndarray,
end: np.ndarray,
radius: float,
*,
apex_at_end: bool = False,
) -> np.ndarray:
"""Place a +Z template from ``start`` to ``end`` (axis anchored, not midpoint-centered).
Open3D cylinders are centered on the origin; cone apex side is detected from the template.
``apex_at_end=True`` maps the template apex to ``end`` (flow tip); base to ``start``.
"""
direction = np.asarray(end, dtype=np.float64) - np.asarray(start, dtype=np.float64)
length = float(np.linalg.norm(direction))
if length <= 1e-9 or radius <= 0.0:
return np.empty((0, 3), dtype=np.float64)
dir_hat = direction / length
rotation = _rotation_matrix_z_to(dir_hat)
scaled = np.asarray(template_vertices, dtype=np.float64).copy()
z_min = float(scaled[:, 2].min())
z_max = float(scaled[:, 2].max())
z_span = z_max - z_min
if z_span <= 1e-9:
return np.empty((0, 3), dtype=np.float64)
if apex_at_end:
t = (z_max - scaled[:, 2]) / z_span * length
else:
t = (scaled[:, 2] - z_min) / z_span * length
radial = np.column_stack((scaled[:, 0] * radius, scaled[:, 1] * radius, t))
return radial @ rotation.T + np.asarray(start, dtype=np.float64)
def _transform_cylinder_segment(
template_vertices: np.ndarray,
start: np.ndarray,
end: np.ndarray,
radius: float,
) -> np.ndarray:
return _transform_axis_segment(template_vertices, start, end, radius)
def build_flow_arrow_mesh(
world_xyz: np.ndarray,
flow: np.ndarray,
mesh_span: float,
*,
style: FlowArrowStyle = DEFAULT_FLOW_ARROW_STYLE,
valid_mask: np.ndarray | None = None,
o3d=None,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
"""Build merged mesh flow arrows: cylinder tail + cone head per arrow."""
empty_v = np.empty((0, 3), dtype=np.float64)
empty_t = np.empty((0, 3), dtype=np.int32)
empty_c = np.empty((0, 3), dtype=np.float64)
seg_starts, seg_ends, roles = _sample_flow_segments(
world_xyz, flow, style=style, valid_mask=valid_mask
)
if len(seg_starts) == 0:
return empty_v, empty_t, empty_c
if o3d is None:
import open3d as o3d
cyl_vertices, cyl_triangles = _cylinder_template(o3d)
cone_vertices, cone_triangles = _cone_template(o3d)
vertices_parts: list[np.ndarray] = []
triangles_parts: list[np.ndarray] = []
colors_parts: list[np.ndarray] = []
vertex_offset = 0
for start, end, role in zip(seg_starts, seg_ends, roles):
seg_start = np.asarray(start, dtype=np.float64)
seg_end = np.asarray(end, dtype=np.float64)
axis = seg_end - seg_start
seg_len = float(np.linalg.norm(axis))
if seg_len <= 1e-9:
continue
radius = segment_radius_for_role(mesh_span, style, role)
if role == "tail":
template_vertices, template_triangles = cyl_vertices, cyl_triangles
apex_at_end = False
overlap = _TAIL_HEAD_OVERLAP_FRACTION * seg_len
seg_end = seg_end + (axis / seg_len) * overlap
else:
template_vertices, template_triangles = cone_vertices, cone_triangles
apex_at_end = not bool(_CONE_APEX_AT_LOW_Z)
tail_radius = segment_radius_for_role(mesh_span, style, "tail")
radius = max(radius, tail_radius)
verts = _transform_axis_segment(
template_vertices,
seg_start,
seg_end,
radius,
apex_at_end=apex_at_end,
)
if verts.size == 0:
continue
color = effective_segment_color(style, role, use_alpha_blend=True)
vertices_parts.append(verts)
triangles_parts.append(template_triangles + vertex_offset)
colors_parts.append(np.tile(color, (len(verts), 1)))
vertex_offset += len(verts)
if not vertices_parts:
return empty_v, empty_t, empty_c
return (
np.vstack(vertices_parts),
np.vstack(triangles_parts),
np.vstack(colors_parts),
)
def create_flow_mesh_geometry(o3d):
"""Create a persistent Open3D mesh for the flow arrow overlay."""
mesh = o3d.geometry.TriangleMesh()
mesh.vertices = o3d.utility.Vector3dVector(np.empty((0, 3), dtype=np.float64))
mesh.triangles = o3d.utility.Vector3iVector(np.empty((0, 3), dtype=np.int32))
mesh.vertex_colors = o3d.utility.Vector3dVector(np.empty((0, 3), dtype=np.float64))
mesh.compute_vertex_normals()
return mesh
class FlowArrowOverlay:
"""Example-layer flow arrows as cylinder tail + cone head meshes on the UV surface."""
def __init__(
self,
mesh: UVSurfaceMesh,
*,
style: FlowArrowStyle | None = None,
):
import open3d as o3d
self.mesh = mesh
self.style = style or DEFAULT_FLOW_ARROW_STYLE
self._mesh_span = _mesh_span(mesh.rest_vertices)
_cylinder_template(o3d)
_cone_template(o3d)
self.arrow_mesh = create_flow_mesh_geometry(o3d) if self.style.enabled else None
@property
def enabled(self) -> bool:
return self.style.enabled
def compute(
self,
flow,
valid_mask: np.ndarray | None = None,
) -> tuple[np.ndarray, np.ndarray, np.ndarray] | None:
if not self.style.enabled or self.arrow_mesh is None:
return None
return build_flow_arrow_mesh(
self.mesh.world_xyz,
flow,
self._mesh_span,
style=self.style,
valid_mask=valid_mask,
)
def apply(self, arrow_vertices: np.ndarray, arrow_triangles: np.ndarray, arrow_colors: np.ndarray) -> None:
"""Update the persistent flow mesh geometry in place."""
import open3d as o3d
self.arrow_mesh.vertices = o3d.utility.Vector3dVector(arrow_vertices.astype(np.float64))
self.arrow_mesh.triangles = o3d.utility.Vector3iVector(arrow_triangles.astype(np.int32))
self.arrow_mesh.vertex_colors = o3d.utility.Vector3dVector(arrow_colors.astype(np.float64))
self.arrow_mesh.compute_vertex_normals()
+3 -3
View File
@@ -42,13 +42,13 @@ class ForceChartWindow:
for channel in range(3): 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)
+75 -6
View File
@@ -10,6 +10,7 @@ from orisys.finger.mesh import UVSurfaceMesh
from orisys.finger.viewer import flow_magnitude_vertex_colors, scalar_map_vertex_colors from 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:
-57
View File
@@ -1,57 +0,0 @@
"""OpenCV visualization helpers for example scripts."""
from __future__ import annotations
import cv2
import numpy as np
def depth_colormap(depth_map, colormap=cv2.COLORMAP_JET) -> np.ndarray:
"""Normalize a scalar map to uint8 and apply an OpenCV colormap."""
values = np.asarray(depth_map)
if values.size == 0:
return np.zeros((1, 1, 3), dtype=np.uint8)
if values.max() > values.min():
normalized = ((values - values.min()) / (values.max() - values.min()) * 255).astype(np.uint8)
else:
normalized = np.zeros_like(values, dtype=np.uint8)
return cv2.applyColorMap(normalized, colormap)
def draw_magnitude_map(
vfield,
threshold=3.0,
colormap=cv2.COLORMAP_JET,
output_size=(400, 400),
) -> np.ndarray:
"""Render a smoothed optical-flow magnitude map as a BGR pseudo-color image."""
magnitude = np.sqrt(vfield[:, :, 0] ** 2 + vfield[:, :, 1] ** 2)
downsample_factor = 10
h, w = magnitude.shape
magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor]
magnitude_smoothed = cv2.resize(magnitude_downsampled, (w, h), interpolation=cv2.INTER_CUBIC)
magnitude_thresholded = magnitude_smoothed.copy()
magnitude_thresholded[magnitude_smoothed < threshold] = 0
mag_min, mag_max = magnitude_thresholded.min(), magnitude_thresholded.max()
if mag_max > mag_min:
magnitude_norm = ((magnitude_thresholded - mag_min) / (mag_max - mag_min) * 255).astype(np.uint8)
else:
magnitude_norm = np.zeros_like(magnitude_thresholded, dtype=np.uint8)
magnitude_colored = cv2.applyColorMap(magnitude_norm, colormap)
out_w, out_h = output_size
return cv2.resize(magnitude_colored, (out_w, out_h), interpolation=cv2.INTER_CUBIC)
def frame_to_bgr(frame: np.ndarray) -> np.ndarray:
"""Convert a grayscale or single-channel frame to BGR for display/recording."""
if frame is None:
raise ValueError("frame is None")
if len(frame.shape) == 2:
return cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
if len(frame.shape) == 3 and frame.shape[2] == 1:
return cv2.cvtColor(frame.squeeze(2), cv2.COLOR_GRAY2BGR)
return frame
+1 -1
View File
@@ -7,7 +7,7 @@ from pathlib import Path
import cv2 import cv2
from .opencv_maps import frame_to_bgr from .render_2d import frame_to_bgr
class FrameRecorder: class FrameRecorder:
+82 -6
View File
@@ -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
) )
+1 -1
View File
@@ -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
View File
@@ -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