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