From 87ac152907e5198404ec5b5b106269bb5f15ba5c Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Wed, 15 Jul 2026 14:25:03 +0800 Subject: [PATCH] update csv export and added fixed mesh axis drawing --- examples/README.md | 2 +- examples/gui/dashboard.py | 74 +++++++++++ examples/gui/layout.py | 35 ++++- examples/gui/widgets/log_panel.py | 51 +++++--- examples/gui/widgets/mesh_viewport.py | 1 + examples/gui/widgets/settings_panel.py | 75 ++++++++++- examples/pipeline/__init__.py | 4 + examples/pipeline/config.py | 2 + examples/pipeline/csv_recorder.py | 114 ++++++++++++++++ examples/pipeline/service.py | 81 +++++++++++- examples/viz/offscreen_mesh.py | 174 ++++++++++++++++++++++++- 11 files changed, 578 insertions(+), 35 deletions(-) create mode 100644 examples/pipeline/csv_recorder.py diff --git a/examples/README.md b/examples/README.md index 8de1a4d..00ac50f 100644 --- a/examples/README.md +++ b/examples/README.md @@ -59,7 +59,7 @@ pip install -e ".[gui]" ## Notes - `finger_pipeline_3dview.py` is the main interactive demo. It uses `gui/app_paths.py` so the same entry point works in editable and packaged runs. -- Dashboard controls: click **启动** to start acquisition; `Q` quits, `R` resets, `S` starts recording, `E` stops recording, and `Esc` toggles fullscreen. +- Dashboard controls: click **启动** to start acquisition; use **Record CSV / Stop CSV** and the CSV save-directory browser to log contact/force data; `Q` quits, `R` resets, `S` starts video recording, `E` stops video recording, and `Esc` toggles fullscreen. - `finger_pipeline.py` is the leanest finger example if you want to validate UV unroll + force/contact processing without the full dashboard. - `finger_pipeline.py` uses `orisys.finger.FingerRuntime` as the high-level entry point, then runs tactile analysis through `runtime.tactile`. - In `--profile` mode, the example splits the loop into capture, UV unroll, deformation, contact, and display stages so you can measure each step separately. diff --git a/examples/gui/dashboard.py b/examples/gui/dashboard.py index a05e0d4..820c24e 100644 --- a/examples/gui/dashboard.py +++ b/examples/gui/dashboard.py @@ -2,6 +2,7 @@ from __future__ import annotations +import math import sys import time @@ -17,6 +18,7 @@ from PyQt5.QtWidgets import ( ) from pipeline.config import PipelineConfig +from pipeline.csv_recorder import CsvRecorder from pipeline.service import FingerPipeline from .app_paths import resource_path @@ -54,6 +56,7 @@ class FingerDashboardWindow(QMainWindow): self.config = config self.layout = layout self.pipeline = FingerPipeline(config) + self.csv_recorder = CsvRecorder(config.csv_save_dir) self._running = False self._frame_index = 0 self._wall_fps = 0.0 @@ -103,6 +106,7 @@ class FingerDashboardWindow(QMainWindow): self.settings_panel.set_config_path(self.config.config) self.settings_panel.set_calibration_path(self.config.calib or "") self.settings_panel.set_finger_export_dir(self.config.export_dir) + self.settings_panel.set_csv_save_dir(self.config.csv_save_dir) self.settings_panel.log_visibility_changed.connect(self._set_log_panel_visible) self.settings_panel.free_rotation_changed.connect(self.mesh_viewport.set_free_rotation) @@ -112,9 +116,12 @@ class FingerDashboardWindow(QMainWindow): self.settings_panel.browse_video_requested.connect(self._select_camera) self.settings_panel.browse_config_requested.connect(self._select_config) self.settings_panel.browse_finger_assets_requested.connect(self._select_finger_assets) + self.settings_panel.browse_csv_dir_requested.connect(self._select_csv_dir) + self.settings_panel.csv_record_toggled.connect(self._on_csv_record_toggled) self.settings_panel.btn_play_pause.clicked.connect(self._on_play_pause_clicked) self.settings_panel.btn_reset_tracker.clicked.connect(self._on_reset) self.settings_panel.set_play_pause_state("run") + self.settings_panel.set_csv_controls_for_pipeline(False) self._panel_widgets: dict[str, QWidget] = {} self._panel_widgets["mesh"] = self.mesh_viewport @@ -314,6 +321,7 @@ class FingerDashboardWindow(QMainWindow): cal = self.settings_panel.calibration_path() self.config.calib = cal or None self.config.export_dir = self.settings_panel.finger_export_dir() + self.config.csv_save_dir = self.settings_panel.csv_save_dir() or "./outputs/csv" try: self.pipeline = FingerPipeline(self.config) @@ -334,6 +342,7 @@ class FingerDashboardWindow(QMainWindow): self.timer.start(self.layout.timer_interval_ms) self._set_play_pause_button("pause") self.settings_panel.btn_reset_tracker.setEnabled(True) + self.settings_panel.set_csv_controls_for_pipeline(True) print( f"▶ 启动 — 摄像机={self.config.video}, 配置={self.config.config}, " f"标定={self.config.calib or '(none)'}, 资产={self.config.export_dir}" @@ -342,6 +351,7 @@ class FingerDashboardWindow(QMainWindow): def _on_stop(self) -> None: if not self._running: return + self._stop_csv_recording(announce=True) self._running = False self.timer.stop() self.log_handler.clear_status_line() @@ -354,6 +364,7 @@ class FingerDashboardWindow(QMainWindow): self._set_play_pause_button("run") self.settings_panel.btn_reset_tracker.setEnabled(False) self.settings_panel.set_finger_export_dir_enabled(True) + self.settings_panel.set_csv_controls_for_pipeline(False) def _on_reset(self) -> None: if not self._running: @@ -413,10 +424,26 @@ class FingerDashboardWindow(QMainWindow): profile.record("total", total_ms) self.pipeline.record_total_frame(total_ms) + measurement = self.pipeline.measure_for_csv(frame, self._frame_index) + if self.csv_recorder.is_recording: + self.csv_recorder.write(measurement) + flow_mag = 0.0 if frame.flow.size: flow_mag = float((frame.flow[:, :, 0] ** 2 + frame.flow[:, :, 1] ** 2).mean() ** 0.5) + contact_xyz_mm = None + if ( + math.isfinite(measurement.contact_x) + and math.isfinite(measurement.contact_y) + and math.isfinite(measurement.contact_z) + ): + contact_xyz_mm = ( + measurement.contact_x, + measurement.contact_y, + measurement.contact_z, + ) + log_cfg = layout.log_config if log_cfg.status_enabled: self.log_handler.set_status_line( @@ -428,6 +455,7 @@ class FingerDashboardWindow(QMainWindow): flow_rms=flow_mag, mask_coverage=frame.mask_coverage, wall_fps=self._wall_fps, + contact_xyz_mm=contact_xyz_mm, ) ) @@ -460,6 +488,50 @@ class FingerDashboardWindow(QMainWindow): if path: self.settings_panel.set_finger_export_dir(path) + def _select_csv_dir(self) -> None: + path = QFileDialog.getExistingDirectory( + self, + "选择 CSV 保存目录", + self.settings_panel.csv_save_dir() or "./outputs/csv", + ) + if path: + self.settings_panel.set_csv_save_dir(path) + self.config.csv_save_dir = path + + def _on_csv_record_toggled(self, start: bool) -> None: + if start: + self._start_csv_recording() + else: + self._stop_csv_recording(announce=True) + + def _start_csv_recording(self) -> None: + if not self._running: + print("请先启动传感器后再记录 CSV。") + self.settings_panel.set_csv_recording(False) + return + save_dir = self.settings_panel.csv_save_dir() or "./outputs/csv" + self.config.csv_save_dir = save_dir + try: + path = self.csv_recorder.start(save_dir) + except OSError as exc: + print(f"无法开始 CSV 记录：{exc}") + self.settings_panel.set_csv_recording(False) + return + self.settings_panel.set_csv_recording(True) + print(f"开始 CSV 记录：{path}") + + def _stop_csv_recording(self, *, announce: bool = False) -> None: + if not self.csv_recorder.is_recording: + self.settings_panel.set_csv_recording(False) + return + path, count = self.csv_recorder.stop() + self.settings_panel.set_csv_recording(False) + if announce: + if path is not None: + print(f"CSV 记录结束：{path}，总行数：{count}") + else: + print(f"CSV 记录结束，总行数：{count}") + def keyPressEvent(self, event): key = event.key() if key == Qt.Key_Escape: @@ -483,6 +555,7 @@ class FingerDashboardWindow(QMainWindow): def closeEvent(self, event): self.timer.stop() + self._stop_csv_recording(announce=True) self.mesh_viewport.close_renderer() self.pipeline.close() event.accept() @@ -493,6 +566,7 @@ def run_dashboard(config: PipelineConfig, *, layout: DashboardLayout = LAYOUT) - from .app import get_or_create_qapp print("\n按键说明：Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏") + print("GUI：Record CSV / Stop CSV 写入接触点与力到所选目录。") 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.") diff --git a/examples/gui/layout.py b/examples/gui/layout.py index 85139b0..1d52e38 100644 --- a/examples/gui/layout.py +++ b/examples/gui/layout.py @@ -19,6 +19,9 @@ MeshLighting (below) controls the 3D mesh panel sun light: MESH_BACKGROUND (below) sets the 3D mesh panel clear color (RGB 0–255). +MESH_AXIS_OVERLAY (below) controls the fixed top-right pre-rendered 3D axis triad: + enabled, padding_px, size_px, opacity + 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" @@ -87,7 +90,7 @@ TOP_ROW = PanelRow( PanelSlot("mesh", "3D mesh", width_px=400, height_px=400), PanelSlot("arrows", "UV Unrolled", width_px=400, height_px=400, enabled=True), PanelSlot("charts", "Force charts", width_px=300, height_px=400), - PanelSlot("settings", "Settings", width_px=200, height_px=0), + PanelSlot("settings", "Settings", width_px=250, height_px=0), ), ) @@ -142,6 +145,20 @@ class MeshBackground: MESH_BACKGROUND = MeshBackground(color=(0, 0, 0)) + +@dataclass(frozen=True) +class MeshAxisOverlay: + """Fixed pre-rendered XYZ triad HUD on the 3D mesh RGB output.""" + + enabled: bool = True + padding_px: int = 12 + size_px: int = 96 + opacity: float = 1.0 + + +MESH_AXIS_OVERLAY = MeshAxisOverlay() + + # 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, @@ -197,6 +214,7 @@ class LogConfig: flow_rms: float = 0.0, mask_coverage: float = 0.0, wall_fps: float | None = None, + contact_xyz_mm: tuple[float, float, float] | None = None, ) -> str: shear_mag = float((fshearx**2 + fsheary**2) ** 0.5) if wall_fps is not None: @@ -206,11 +224,17 @@ class LogConfig: parts.extend( [ f"Fn={fnormal:.1f}", - f"|Fs|={shear_mag:.1f}", - f"Fs=({fshearx:.1f}, {fsheary:.1f})", + f"|(Fx,Fy)|={shear_mag:.1f}", + f"Fx, Fy=({fshearx:.1f}, {fsheary:.1f})", ] ) - return " ".join(parts) + force_line = " ".join(parts) + if contact_xyz_mm is None: + contact_line = "contact=---" + else: + cx, cy, cz = contact_xyz_mm + contact_line = f"contact=({cx:.1f}, {cy:.1f}, {cz:.1f}) mm" + return f"{force_line}\n{contact_line}" LOG_CONFIG = LogConfig() @@ -224,7 +248,7 @@ class DashboardLayout: window_title: str = "Orisys Finger Dashboard" window_width: int = 1300 - window_height: int = 520 + window_height: int = 560 root_margin_px: int = 6 root_spacing_px: int = 6 @@ -249,6 +273,7 @@ class DashboardLayout: mesh_background: MeshBackground = MESH_BACKGROUND mesh_contact_arrow: ContactArrowStyle = MESH_CONTACT_ARROW mesh_flow_arrow: FlowArrowStyle = MESH_FLOW_ARROW + mesh_axis_overlay: MeshAxisOverlay = MESH_AXIS_OVERLAY log_config: LogConfig = LOG_CONFIG diff --git a/examples/gui/widgets/log_panel.py b/examples/gui/widgets/log_panel.py index 1431f9f..7bbcbb2 100644 --- a/examples/gui/widgets/log_panel.py +++ b/examples/gui/widgets/log_panel.py @@ -22,12 +22,13 @@ class TeeStream: class WidgetLogHandler: - """Mirror print output into a QPlainTextEdit with a live status line at the bottom.""" + """Mirror print output into a QPlainTextEdit with pinned status lines at the bottom.""" def __init__(self, widget: QPlainTextEdit): self.widget = widget self.buf = "" - self._has_status_line = False + self._status_text: str | None = None + self._status_block_count = 0 def write(self, text: str): self.buf += text @@ -42,34 +43,42 @@ class WidgetLogHandler: self.buf = "" def set_status_line(self, text: str) -> None: - """Update the pinned bottom line (FPS / forces) without scrolling the log.""" + """Update pinned bottom status (supports multi-line) without scrolling away.""" 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 + self._remove_status_blocks() + self.widget.appendPlainText(text) + self._status_text = text + self._status_block_count = max(1, text.count("\n") + 1) scrollbar = self.widget.verticalScrollBar() scrollbar.setValue(scrollbar.maximum()) def clear_status_line(self) -> None: - if not self._has_status_line: + self._remove_status_blocks() + self._status_text = None + self._status_block_count = 0 + + def _remove_status_blocks(self) -> None: + if self._status_block_count <= 0: return - cursor = QTextCursor(self.widget.document().lastBlock()) - cursor.select(QTextCursor.BlockUnderCursor) + doc = self.widget.document() + end = QTextCursor(doc) + end.movePosition(QTextCursor.End) + start = QTextCursor(doc) + start.movePosition(QTextCursor.End) + for _ in range(self._status_block_count): + start.movePosition(QTextCursor.StartOfBlock) + if start.position() > 0: + start.movePosition(QTextCursor.Left) + cursor = QTextCursor(doc) + cursor.setPosition(start.position()) + cursor.setPosition(end.position(), QTextCursor.KeepAnchor) cursor.removeSelectedText() - self._has_status_line = False + self._status_block_count = 0 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 + status = self._status_text + if status is not None: + self.clear_status_line() self.widget.appendPlainText(line) if status is not None: self.set_status_line(status) diff --git a/examples/gui/widgets/mesh_viewport.py b/examples/gui/widgets/mesh_viewport.py index a3907b7..e1fafa5 100644 --- a/examples/gui/widgets/mesh_viewport.py +++ b/examples/gui/widgets/mesh_viewport.py @@ -120,6 +120,7 @@ class MeshViewportWidget(ImagePanel): mesh_background=self._layout.mesh_background.color, contact_arrow_style=self._layout.mesh_contact_arrow, flow_arrow_style=self._layout.mesh_flow_arrow, + axis_overlay=self._layout.mesh_axis_overlay, ) self._renderer.flow_arrow.set_enabled(self._mesh_state.flow_arrows_enabled) diff --git a/examples/gui/widgets/settings_panel.py b/examples/gui/widgets/settings_panel.py index 22fb18f..1338be7 100644 --- a/examples/gui/widgets/settings_panel.py +++ b/examples/gui/widgets/settings_panel.py @@ -41,6 +41,8 @@ class SettingsPanelWidget(QWidget): browse_video_requested = pyqtSignal() browse_config_requested = pyqtSignal() browse_finger_assets_requested = pyqtSignal() + browse_csv_dir_requested = pyqtSignal() + csv_record_toggled = pyqtSignal(bool) def __init__( self, @@ -57,8 +59,8 @@ class SettingsPanelWidget(QWidget): self.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed) root = QVBoxLayout(self) - root.setContentsMargins(10, 10, 10, 10) - root.setSpacing(10) + root.setContentsMargins(8, 8, 8, 8) + root.setSpacing(6) title = QLabel(layout.settings_panel_title) title.setObjectName("settingsTitle") @@ -125,10 +127,10 @@ class SettingsPanelWidget(QWidget): config_row.addWidget(self.btn_browse_config) root.addLayout(config_row) + # Optional calibration path is kept for future re-enablement but hidden for now. self.cal_edit = QLineEdit("") self.cal_edit.setPlaceholderText("Optional calibration path") - root.addWidget(self._caption("Calibration")) - root.addWidget(self.cal_edit) + self.cal_edit.hide() root.addWidget(self._caption("Finger assets")) finger_assets_row = QHBoxLayout() @@ -143,7 +145,28 @@ class SettingsPanelWidget(QWidget): finger_assets_row.addWidget(self.btn_browse_finger_assets) root.addLayout(finger_assets_row) - root.addSpacing(8) + root.addWidget(self._caption("CSV save directory")) + csv_dir_row = QHBoxLayout() + csv_dir_row.setSpacing(6) + self.csv_dir_edit = QLineEdit("./outputs/csv") + self.csv_dir_edit.setPlaceholderText("Directory for CSV recordings") + csv_dir_row.addWidget(self.csv_dir_edit, 1) + self.btn_browse_csv_dir = QPushButton("…") + self.btn_browse_csv_dir.setFixedWidth(32) + self.btn_browse_csv_dir.setToolTip("Browse CSV save directory") + self.btn_browse_csv_dir.clicked.connect(self.browse_csv_dir_requested.emit) + csv_dir_row.addWidget(self.btn_browse_csv_dir) + root.addLayout(csv_dir_row) + + self.btn_csv_record = QPushButton("Record CSV") + self.btn_csv_record.setToolTip("Start writing contact/force CSV") + self.btn_csv_record.setEnabled(False) + self.btn_csv_record.clicked.connect(self._on_csv_toggle_clicked) + root.addWidget(self.btn_csv_record) + self._csv_recording = False + self._pipeline_running = False + + root.addSpacing(4) rotate_row = QHBoxLayout() rotate_row.setSpacing(6) self.free_rotate_check = QCheckBox("Free rotate 3D view") @@ -211,6 +234,45 @@ class SettingsPanelWidget(QWidget): self.finger_assets_edit.setEnabled(enabled) self.btn_browse_finger_assets.setEnabled(enabled) + def set_csv_save_dir(self, value: str) -> None: + self.csv_dir_edit.setText(value) + + def set_csv_controls_for_pipeline(self, running: bool) -> None: + """Update CSV controls from pipeline start/stop.""" + self._pipeline_running = bool(running) + if not self._pipeline_running: + self._csv_recording = False + self._refresh_csv_controls() + + def set_csv_recording(self, recording: bool) -> None: + """Update CSV controls after a successful start/stop of recording.""" + self._csv_recording = bool(recording) and self._pipeline_running + self._refresh_csv_controls() + + def _refresh_csv_controls(self) -> None: + self.btn_csv_record.setEnabled(self._pipeline_running) + if self._csv_recording: + self.btn_csv_record.setText("Stop CSV") + self.btn_csv_record.setToolTip("Stop CSV recording") + self.btn_csv_record.setProperty("recording", True) + else: + self.btn_csv_record.setText("Record CSV") + self.btn_csv_record.setToolTip("Start writing contact/force CSV") + self.btn_csv_record.setProperty("recording", False) + style = self.btn_csv_record.style() + style.unpolish(self.btn_csv_record) + style.polish(self.btn_csv_record) + self.btn_csv_record.update() + + dir_enabled = not self._csv_recording + self.csv_dir_edit.setEnabled(dir_enabled) + self.btn_browse_csv_dir.setEnabled(dir_enabled) + + def _on_csv_toggle_clicked(self) -> None: + if not self._pipeline_running: + return + self.csv_record_toggled.emit(not self._csv_recording) + def _emit_arrow_background_changed(self, _index: int) -> None: value = self.arrow_background_combo.currentData() if value: @@ -239,3 +301,6 @@ class SettingsPanelWidget(QWidget): def finger_export_dir(self) -> str: return self.finger_assets_edit.text().strip() + + def csv_save_dir(self) -> str: + return self.csv_dir_edit.text().strip() diff --git a/examples/pipeline/__init__.py b/examples/pipeline/__init__.py index 6c20f30..ec58516 100644 --- a/examples/pipeline/__init__.py +++ b/examples/pipeline/__init__.py @@ -1,10 +1,14 @@ """Finger example pipeline: runtime acquisition without GUI dependencies.""" from .config import PipelineConfig, mesh_threshold +from .csv_recorder import CSV_COLUMNS, CsvMeasurement, CsvRecorder from .frame import FingerFrameView from .service import FingerPipeline __all__ = [ + "CSV_COLUMNS", + "CsvMeasurement", + "CsvRecorder", "FingerFrameView", "FingerPipeline", "PipelineConfig", diff --git a/examples/pipeline/config.py b/examples/pipeline/config.py index a44baf5..a199319 100644 --- a/examples/pipeline/config.py +++ b/examples/pipeline/config.py @@ -20,6 +20,7 @@ class PipelineConfig: config: str = "./config/dd02-ov.json" calib: str | None = None export_dir: str = DEFAULT_FINGER_EXPORT_DIR + csv_save_dir: str = "./outputs/csv" verbose: bool = False mesh_map: str = "depth" threshold: float | None = None @@ -45,6 +46,7 @@ class PipelineConfig: lowpass_cutoff_hz=getattr(args, "cutoff", 5.0), profile=getattr(args, "profile", False), profile_every=getattr(args, "profile_every", 30), + csv_save_dir=getattr(args, "csv_save_dir", "./outputs/csv"), ) @property diff --git a/examples/pipeline/csv_recorder.py b/examples/pipeline/csv_recorder.py new file mode 100644 index 0000000..a8d961c --- /dev/null +++ b/examples/pipeline/csv_recorder.py @@ -0,0 +1,114 @@ +"""CSV recorder for DD02 contact / force logging.""" + +from __future__ import annotations + +import csv +from dataclasses import dataclass +from datetime import datetime +from pathlib import Path + + +CSV_COLUMNS = ( + "frame_index", + "timestamp_s", + "contact_x_mm", + "contact_y_mm", + "contact_z_mm", + "normal_x", + "normal_y", + "normal_z", + "force_local_fn", + "force_local_fx", + "force_local_fy", +) + + +@dataclass +class CsvMeasurement: + frame_index: int + timestamp_s: float + contact_x: float + contact_y: float + contact_z: float + normal_x: float + normal_y: float + normal_z: float + force_local_fn: float + force_local_fx: float + force_local_fy: float + + +def measurement_to_csv_row(row: CsvMeasurement) -> dict[str, float | int]: + return { + "frame_index": row.frame_index, + "timestamp_s": row.timestamp_s, + "contact_x_mm": row.contact_x, + "contact_y_mm": row.contact_y, + "contact_z_mm": row.contact_z, + "normal_x": row.normal_x, + "normal_y": row.normal_y, + "normal_z": row.normal_z, + "force_local_fn": row.force_local_fn, + "force_local_fx": row.force_local_fx, + "force_local_fy": row.force_local_fy, + } + + +class CsvRecorder: + """Write one contact/force measurement row per frame.""" + + def __init__(self, save_dir: str | Path = "./outputs/csv") -> None: + self.save_dir = Path(save_dir) + self._file = None + self._writer: csv.DictWriter | None = None + self._path: Path | None = None + self._row_count = 0 + + @property + def is_recording(self) -> bool: + return self._writer is not None + + @property + def path(self) -> Path | None: + return self._path + + @property + def row_count(self) -> int: + return self._row_count + + def start(self, save_dir: str | Path | None = None) -> Path: + if self.is_recording: + self.stop() + + if save_dir is not None: + self.save_dir = Path(save_dir) + self.save_dir.mkdir(parents=True, exist_ok=True) + + stamp = datetime.now().strftime("%Y%m%d_%H%M%S") + self._path = self.save_dir / f"dd02_record_{stamp}.csv" + self._file = self._path.open("w", newline="", encoding="utf-8") + self._writer = csv.DictWriter(self._file, fieldnames=CSV_COLUMNS) + self._writer.writeheader() + self._file.flush() + self._row_count = 0 + return self._path + + def write(self, row: CsvMeasurement) -> None: + if self._writer is None: + return + self._writer.writerow(measurement_to_csv_row(row)) + self._row_count += 1 + if self._file is not None and self._row_count % 30 == 0: + self._file.flush() + + def stop(self) -> tuple[Path | None, int]: + count = self._row_count + saved_path = self._path + if self._file is not None: + self._file.flush() + self._file.close() + self._file = None + self._writer = None + self._path = None + self._row_count = 0 + return saved_path, count diff --git a/examples/pipeline/service.py b/examples/pipeline/service.py index 2da397d..677e732 100644 --- a/examples/pipeline/service.py +++ b/examples/pipeline/service.py @@ -6,11 +6,12 @@ import time from dataclasses import dataclass import numpy as np -from orisys.finger import FingerRuntime +from orisys.finger import FingerRuntime, compute_uv_surface_frames from viz import FrameRecorder, build_uv_surface_mesh, resolve_viz_pose_path from .config import PipelineConfig +from .csv_recorder import CsvMeasurement from .frame import FingerFrameView from .profiling import StageProfiler, run_deformation_timed @@ -40,6 +41,12 @@ class FingerPipeline: self.profiler: StageProfiler | None = ( StageProfiler(report_every=config.profile_every) if config.profile else None ) + self._world_xyz: np.ndarray | None = None + self._surface_mask: np.ndarray | None = None + self._geometric_center: np.ndarray | None = None + self._tangent_x: np.ndarray | None = None + self._tangent_y: np.ndarray | None = None + self._normals: np.ndarray | None = None @property def is_open(self) -> bool: @@ -72,10 +79,26 @@ class FingerPipeline: if domain.surface is None or getattr(domain.surface, "world_xyz", None) is None: return self.uv_mesh = build_uv_surface_mesh(domain.surface.world_xyz, domain.valid_mask) + self._cache_surface_geometry(domain.surface.world_xyz, domain.valid_mask) if self.config.viz_pose.lower() != "none": self.world_to_cam = np.load(resolve_viz_pose_path(self.config.viz_pose)) self.mesh_lookat = self.uv_mesh.rest_vertices.mean(axis=0) + def _cache_surface_geometry(self, world_xyz: np.ndarray, valid_mask: np.ndarray) -> None: + world_xyz = np.asarray(world_xyz, dtype=np.float64) + valid_mask = np.asarray(valid_mask, dtype=bool) + finite = np.all(np.isfinite(world_xyz), axis=2) + mask = valid_mask & finite + self._world_xyz = world_xyz + self._surface_mask = mask + points = world_xyz[mask] + self._geometric_center = ( + points.mean(axis=0) if len(points) else np.zeros(3, dtype=np.float64) + ) + self._tangent_x, self._tangent_y, self._normals = compute_uv_surface_frames( + world_xyz, mask + ) + def close(self) -> None: if self.profiler is not None and self.profiler.frame_count: self.profiler.print_report(prefix="[profile final]") @@ -83,6 +106,12 @@ class FingerPipeline: self.runtime.close() self.runtime = None self._start_time = None + self._world_xyz = None + self._surface_mask = None + self._geometric_center = None + self._tangent_x = None + self._tangent_y = None + self._normals = None def reset(self) -> None: if self.runtime is not None: @@ -165,3 +194,53 @@ class FingerPipeline: def stop_recording(self) -> None: self.recorder.stop() + + def measure_for_csv(self, frame: FingerFrameView, frame_index: int) -> CsvMeasurement: + """Lift contact + local forces into global-frame measurements for CSV export.""" + nan = float("nan") + contact = (nan, nan, nan) + normal = (nan, nan, nan) + + fn = float(frame.fnormal) + fx = float(frame.fshearx) + fy = float(frame.fsheary) + + if ( + frame.centroid_found + and frame.centroid is not None + and np.asarray(frame.centroid).size >= 2 + and self._world_xyz is not None + and self._geometric_center is not None + and self._normals is not None + and self._tangent_x is not None + and self._tangent_y is not None + ): + cx = int(round(float(np.asarray(frame.centroid).reshape(-1)[0]))) + cy = int(round(float(np.asarray(frame.centroid).reshape(-1)[1]))) + h, w = self._world_xyz.shape[:2] + if 0 <= cy < h and 0 <= cx < w: + if self._surface_mask is None or bool(self._surface_mask[cy, cx]): + world = self._world_xyz[cy, cx] + if np.all(np.isfinite(world)): + centered = world - self._geometric_center + contact = (float(centered[0]), float(centered[1]), float(centered[2])) + + n = self._normals[cy, cx] + n_norm = float(np.linalg.norm(n)) + if n_norm > 1e-8: + n_u = n / n_norm + normal = (float(n_u[0]), float(n_u[1]), float(n_u[2])) + + return CsvMeasurement( + frame_index=int(frame_index), + timestamp_s=float(frame.timestamp), + contact_x=contact[0], + contact_y=contact[1], + contact_z=contact[2], + normal_x=normal[0], + normal_y=normal[1], + normal_z=normal[2], + force_local_fn=fn, + force_local_fx=fx, + force_local_fy=fy, + ) diff --git a/examples/viz/offscreen_mesh.py b/examples/viz/offscreen_mesh.py index ac7a5de..a7ff151 100644 --- a/examples/viz/offscreen_mesh.py +++ b/examples/viz/offscreen_mesh.py @@ -84,6 +84,168 @@ def _background_rgb01(color: tuple[int, int, int]) -> np.ndarray: return np.asarray(_background_rgba01(color)[:3], dtype=np.float64) +# Cached pre-rendered mesh-axis triads keyed by size_px. +_AXIS_TRIAD_CACHE: dict[int, np.ndarray] = {} + + +def _project_mesh_axis_to_screen(point_xyz: np.ndarray, *, size_px: int) -> tuple[int, int, float]: + """Project mesh coords to HUD pixels. + + Screen mapping for the fixed mesh-axis reference: + +X -> down, +Y -> left, +Z -> inward (into the page). + + Mild foreshortening keeps +Z visible as a short into-page axis. + Returns (px, py, depth) where larger depth is farther from the viewer. + """ + x, y, z = float(point_xyz[0]), float(point_xyz[1]), float(point_xyz[2]) + # Camera looks predominantly along +Z (into page): u right, v down. + # Soft shear so Z has a short foreshortened stub. + shear = 0.34 + u = -y - shear * z + v = x - shear * z + scale = size_px * 0.30 + # Origin slightly lower-right-of-center so labeled tips stay in-frame. + cx = size_px * 0.58 + cy = size_px * 0.48 + return int(round(cx + u * scale)), int(round(cy + v * scale)), z + + +def _render_axis_triad_rgba(size_px: int) -> np.ndarray: + """Build (and cache) a transparent RGBA 3D-looking XYZ triad.""" + import cv2 + + size_px = max(48, int(size_px)) + cached = _AXIS_TRIAD_CACHE.get(size_px) + if cached is not None: + return cached + + rgba = np.zeros((size_px, size_px, 4), dtype=np.uint8) + rgb = rgba[:, :, :3] + alpha = rgba[:, :, 3] + + # Soft circular plate for contrast on bright contact heatmaps. + plate = np.zeros((size_px, size_px), dtype=np.uint8) + center = (size_px // 2, size_px // 2) + radius = int(size_px * 0.46) + cv2.circle(plate, center, radius, 255, thickness=-1, lineType=cv2.LINE_AA) + rgba[:, :, 0:3][plate > 0] = (18, 18, 18) + alpha[plate > 0] = 140 + + thickness = max(2, size_px // 42) + font = cv2.FONT_HERSHEY_SIMPLEX + font_scale = max(0.35, size_px / 220.0) + origin = np.zeros(3, dtype=np.float64) + o_xy = _project_mesh_axis_to_screen(origin, size_px=size_px)[:2] + + axes = ( + ("X", np.array([1.0, 0.0, 0.0]), (230, 70, 70)), + ("Y", np.array([0.0, 1.0, 0.0]), (70, 210, 95)), + ("Z", np.array([0.0, 0.0, 1.0]), (70, 140, 255)), + ) + # Paint far axes first so nearer shafts occlude correctly. + painted = [] + for label, direction, color in axes: + tip = direction * 1.0 + px, py, depth = _project_mesh_axis_to_screen(tip, size_px=size_px) + painted.append((depth, label, color, (px, py))) + painted.sort(key=lambda item: item[0], reverse=True) + + layer = np.zeros((size_px, size_px, 3), dtype=np.uint8) + for _, label, color, tip_xy in painted: + cv2.arrowedLine( + layer, + o_xy, + tip_xy, + color, + thickness, + line_type=cv2.LINE_AA, + tipLength=0.28, + ) + (tw, th), _ = cv2.getTextSize(label, font, font_scale, max(1, thickness - 1)) + # Offset label slightly past the arrow tip, away from origin. + dx = tip_xy[0] - o_xy[0] + dy = tip_xy[1] - o_xy[1] + norm = max(1.0, float(np.hypot(dx, dy))) + lx = int(tip_xy[0] + 8 * dx / norm - tw // 2) + ly = int(tip_xy[1] + 8 * dy / norm + th // 2) + lx = min(max(1, lx), size_px - tw - 1) + ly = min(max(th + 1, ly), size_px - 2) + cv2.putText( + layer, + label, + (lx, ly), + font, + font_scale, + color, + max(1, thickness - 1), + cv2.LINE_AA, + ) + # Origin hub on the same contiguous buffer. + cv2.circle(layer, o_xy, max(2, thickness), (235, 235, 235), -1, lineType=cv2.LINE_AA) + + mask = np.any(layer > 0, axis=2) + rgb[mask] = layer[mask] + alpha[mask] = 255 + + _AXIS_TRIAD_CACHE[size_px] = rgba + return rgba + + +def draw_axis_overlay( + rgb: np.ndarray, + *, + enabled: bool = True, + padding_px: int = 12, + size_px: int = 96, + opacity: float = 1.0, +) -> np.ndarray: + """Composite a fixed top-right mesh-axis triad onto an RGB frame. + + The triad is pre-rendered and cached; orientation is locked to true mesh + axes (+X down, +Y left, +Z inward) and does not follow free-rotate. + """ + if not enabled or rgb is None or rgb.size == 0: + return rgb + + out = np.ascontiguousarray(rgb) + if out.ndim != 3 or out.shape[2] < 3: + return out + + triad = _render_axis_triad_rgba(size_px) + th, tw = triad.shape[:2] + pad = max(0, int(padding_px)) + h, w = out.shape[:2] + x0 = max(0, w - tw - pad) + y0 = max(0, pad) + x1 = min(w, x0 + tw) + y1 = min(h, y0 + th) + if x1 <= x0 or y1 <= y0: + return out + + roi = out[y0:y1, x0:x1] + overlay = triad[: y1 - y0, : x1 - x0] + alpha = (overlay[:, :, 3].astype(np.float32) / 255.0) * float(np.clip(opacity, 0.0, 1.0)) + if alpha.ndim == 2: + alpha = alpha[:, :, None] + rgb_o = overlay[:, :, :3].astype(np.float32) + base = roi.astype(np.float32) + blended = rgb_o * alpha + base * (1.0 - alpha) + roi[:] = np.clip(blended, 0, 255).astype(np.uint8) + return out + + +def _apply_axis_overlay(rgb: np.ndarray, axis_overlay) -> np.ndarray: + if axis_overlay is None: + return draw_axis_overlay(rgb) + return draw_axis_overlay( + rgb, + enabled=bool(getattr(axis_overlay, "enabled", True)), + padding_px=int(getattr(axis_overlay, "padding_px", 12)), + size_px=int(getattr(axis_overlay, "size_px", 96)), + opacity=float(getattr(axis_overlay, "opacity", 1.0)), + ) + + class _FilamentOffscreenBackend: """Filament OffscreenRenderer (Linux headless / platforms with EGL).""" @@ -104,6 +266,7 @@ class _FilamentOffscreenBackend: mesh_background: tuple[int, int, int] = (0, 0, 0), contact_arrow_style=None, flow_arrow_style=None, + axis_overlay=None, ): import open3d as o3d @@ -119,6 +282,7 @@ class _FilamentOffscreenBackend: self.lookat = lookat self.mesh_lighting = mesh_lighting self.mesh_background = mesh_background + self.axis_overlay = axis_overlay self._shade_factors = ( precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting) if mesh_lighting is not None @@ -257,7 +421,8 @@ class _FilamentOffscreenBackend: mesh_to_o3d=lambda v, t, c: _mesh_to_o3d(o3d, v, t, c), ) - return _numpy_o3d_image_to_rgb(self._renderer.render_to_image()) + rgb = _numpy_o3d_image_to_rgb(self._renderer.render_to_image()) + return _apply_axis_overlay(rgb, self.axis_overlay) def close(self) -> None: self._renderer = None @@ -283,6 +448,7 @@ class _HiddenVisualizerBackend: mesh_background: tuple[int, int, int] = (0, 0, 0), contact_arrow_style=None, flow_arrow_style=None, + axis_overlay=None, ): import open3d as o3d @@ -298,6 +464,7 @@ class _HiddenVisualizerBackend: self.lookat = lookat self.mesh_lighting = mesh_lighting self.mesh_background = mesh_background + self.axis_overlay = axis_overlay self._shade_factors = ( precompute_shade_factors(mesh.rest_vertices, mesh.triangles, mesh_lighting) if mesh_lighting is not None @@ -441,7 +608,8 @@ class _HiddenVisualizerBackend: self.vis.poll_events() self.vis.update_renderer() - return _numpy_o3d_image_to_rgb(self.vis.capture_screen_float_buffer(do_render=False)) + rgb = _numpy_o3d_image_to_rgb(self.vis.capture_screen_float_buffer(do_render=False)) + return _apply_axis_overlay(rgb, self.axis_overlay) def close(self) -> None: if self.vis is not None: @@ -478,6 +646,7 @@ class OffscreenFingerMeshRenderer: mesh_background: tuple[int, int, int] = (0, 0, 0), contact_arrow_style=None, flow_arrow_style=None, + axis_overlay=None, ): lookat = lookat if lookat is not None else mesh.rest_vertices.mean(axis=0) self._backend = _create_backend( @@ -495,6 +664,7 @@ class OffscreenFingerMeshRenderer: mesh_background=mesh_background, contact_arrow_style=contact_arrow_style, flow_arrow_style=flow_arrow_style, + axis_overlay=axis_overlay, ) @property -- 2.52.0