From 2e3d0a0609cd2121c258140fbd7d9c5be96faaa5 Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Sun, 26 Jul 2026 17:03:44 +0800 Subject: [PATCH] added dd06 test support, organized 2d viewer and 3d viewer code organization. --- config/dd06-ov.json | 101 +++ examples/gui/__init__.py | 21 +- examples/gui/log_config.py | 52 ++ examples/gui/preference.json | 2 +- examples/gui/sensors/__init__.py | 23 + examples/gui/sensors/adapter.py | 330 +++++++ examples/gui/sensors/roi_geometry.py | 140 +++ examples/gui/viewer2d/__init__.py | 24 + examples/gui/viewer2d/backend.py | 312 +++++++ examples/gui/viewer2d/styles.py | 120 +++ examples/gui/viewer2d/window.py | 491 +++++++++++ examples/gui/viewer3d/__init__.py | 36 + .../gui/{ => viewer3d}/camera_discovery.py | 0 examples/gui/{ => viewer3d}/dashboard.py | 8 +- examples/gui/{ => viewer3d}/layout.py | 48 +- .../gui/{ => viewer3d}/mesh_render_worker.py | 0 .../gui/{ => viewer3d}/mesh_view_state.py | 0 examples/gui/{ => viewer3d}/preferences.py | 9 +- examples/gui/{ => viewer3d}/styles.py | 0 .../gui/{ => viewer3d}/transport_icons.py | 0 examples/gui/viewer3d/widgets/__init__.py | 15 + .../gui/{ => viewer3d}/widgets/arrow_image.py | 2 +- .../{ => viewer3d}/widgets/force_charts.py | 4 +- .../gui/{ => viewer3d}/widgets/image_panel.py | 2 +- .../{ => viewer3d}/widgets/mesh_viewport.py | 2 +- .../{ => viewer3d}/widgets/settings_panel.py | 0 examples/gui/widgets/__init__.py | 12 +- examples/gui/widgets/log_panel.py | 2 +- scripts/qt_viewer_2.py | 818 +----------------- tests/dd06.py | 175 ++++ tests/test_qt_viewer_backend.py | 165 ++++ tools/ffc/roi_polygon_tool.py | 348 ++++++-- 32 files changed, 2326 insertions(+), 936 deletions(-) create mode 100644 config/dd06-ov.json create mode 100644 examples/gui/log_config.py create mode 100644 examples/gui/sensors/__init__.py create mode 100644 examples/gui/sensors/adapter.py create mode 100644 examples/gui/sensors/roi_geometry.py create mode 100644 examples/gui/viewer2d/__init__.py create mode 100644 examples/gui/viewer2d/backend.py create mode 100644 examples/gui/viewer2d/styles.py create mode 100644 examples/gui/viewer2d/window.py create mode 100644 examples/gui/viewer3d/__init__.py rename examples/gui/{ => viewer3d}/camera_discovery.py (100%) rename examples/gui/{ => viewer3d}/dashboard.py (99%) rename examples/gui/{ => viewer3d}/layout.py (88%) rename examples/gui/{ => viewer3d}/mesh_render_worker.py (100%) rename examples/gui/{ => viewer3d}/mesh_view_state.py (100%) rename examples/gui/{ => viewer3d}/preferences.py (85%) rename examples/gui/{ => viewer3d}/styles.py (100%) rename examples/gui/{ => viewer3d}/transport_icons.py (100%) create mode 100644 examples/gui/viewer3d/widgets/__init__.py rename examples/gui/{ => viewer3d}/widgets/arrow_image.py (96%) rename examples/gui/{ => viewer3d}/widgets/force_charts.py (98%) rename examples/gui/{ => viewer3d}/widgets/image_panel.py (97%) rename examples/gui/{ => viewer3d}/widgets/mesh_viewport.py (99%) rename examples/gui/{ => viewer3d}/widgets/settings_panel.py (100%) create mode 100644 tests/dd06.py create mode 100644 tests/test_qt_viewer_backend.py diff --git a/config/dd06-ov.json b/config/dd06-ov.json new file mode 100644 index 0000000..34e5974 --- /dev/null +++ b/config/dd06-ov.json @@ -0,0 +1,101 @@ +{ + "name": "dd06-ov", + "type": "single_camera", + "version": "1.0", + "camera": { + "resolution_raw": [ + 1280, + 720 + ], + "fps": 30, + "resolution_stitch": [ + 390, + 550 + ], + "source_scale": 0.4, + "old": [ + 672, + 728, + 1, + 0.5 + ], + "work": [ + 504, + 546, + 1 + ], + "work2": [ + 420, + 450, + 0.8 + ], + "work3": [ + 350, + 375, + 4, + 0.7 + ], + "work4": [ + 280, + 300, + 4, + 0.4, + 0.2 + ] + }, + "tracker": { + "adaptive": true, + "lower_threshold": 12.0, + "upper_threshold": 18.0, + "loop_closure_interval": 1, + "algorithm": "dis", + "dis_preset": "medium", + "dis_mode": 3, + "algorithm_list": [ + "raft", + "dis" + ], + "hhd_downsample": 1 + }, + "contact": { + "min_contour_area": 3000, + "flow_threshold": 0.2, + "div_threshold": 0.022, + "expansion_threshold": 0.01, + "kernel_size": [ + 5, + 5 + ], + "denoise_sigma": 12 + }, + "roi": { + "type": "polygon", + "image_size": [ + 1280, + 720 + ], + "created_at": "2026-07-26T11:56:15", + "points": [ + [ + 242, + 1 + ], + [ + 402, + 597 + ], + [ + 799, + 606 + ], + [ + 976, + 2 + ] + ], + "mask_path": "dd06-ov_mask.png", + "metadata": { + "source": "./data/dd06/0726.mp4" + } + } +} diff --git a/examples/gui/__init__.py b/examples/gui/__init__.py index 102a142..562677c 100644 --- a/examples/gui/__init__.py +++ b/examples/gui/__init__.py @@ -1,20 +1,29 @@ -"""Public GUI entry points for finger examples.""" +"""Public GUI entry points for Orisys example apps. -from .dashboard import FingerDashboardWindow, run_dashboard -from .layout import ( - LOG_CONFIG, - LOG_PANEL, +Two apps live under ``examples/gui``: + +- Finger 3D dashboard: ``from gui import run_dashboard`` (also ``gui.viewer3d``) +- 2D sensor monitor: ``from gui.viewer2d import run_viewer`` + +Acquisition adapters (no widgets) live in ``gui.sensors``. +Shared bootstrap: ``gui.app``, ``gui.app_paths``, ``gui.log_config``, ``gui.widgets``. +""" + +from .log_config import LOG_CONFIG, LogConfig +from .viewer3d import ( + FingerDashboardWindow, LAYOUT, + LOG_PANEL, MESH_CONTACT_ARROW, MESH_LIGHTING, PANEL_SETUP, TOP_ROW, DashboardLayout, - LogConfig, MeshLighting, PanelRow, PanelSetup, PanelSlot, + run_dashboard, top_row_splitter_sizes, top_row_slot_index, ) diff --git a/examples/gui/log_config.py b/examples/gui/log_config.py new file mode 100644 index 0000000..a0c0a40 --- /dev/null +++ b/examples/gui/log_config.py @@ -0,0 +1,52 @@ +"""Shared log-panel configuration for Qt viewers (2D and 3D).""" + +from __future__ import annotations + +from dataclasses import dataclass + + +@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 = "Cascadia Mono" + 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, + 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: + parts = [f"FPS={wall_fps:.1f}", f"sdk={fps:.1f}"] + else: + parts = [f"FPS={fps:.1f}"] + parts.extend( + [ + f"Fn={fnormal:.1f}", + f"|(Fx,Fy)|={shear_mag:.1f}", + f"Fx, Fy=({fshearx:.1f}, {fsheary:.1f})", + ] + ) + 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() diff --git a/examples/gui/preference.json b/examples/gui/preference.json index 4e97c5d..ef87142 100644 --- a/examples/gui/preference.json +++ b/examples/gui/preference.json @@ -1,5 +1,5 @@ { - "video_source": "0", + "video_source": "D:/repo/orisys-sdk/data/dd02-0703/1.mp4", "config_path": "./config/dd02-ov.json", "finger_assets_dir": "D:/repo/orisys-sdk/assets/finger/30fps-0703", "csv_save_dir": "./outputs/csv", diff --git a/examples/gui/sensors/__init__.py b/examples/gui/sensors/__init__.py new file mode 100644 index 0000000..e275bd6 --- /dev/null +++ b/examples/gui/sensors/__init__.py @@ -0,0 +1,23 @@ +"""Sensor acquisition adapters for Qt / multi-sensor viewers (no Qt widgets).""" + +from .adapter import ( + Dd06RoiAdapter, + FrameSnapshot, + OrisysSensorAdapter, + SensorAdapter, + calibrate_normal_force, + create_sensor_adapter, + _snapshot_from_sensor, +) +from . import roi_geometry + +__all__ = [ + "Dd06RoiAdapter", + "FrameSnapshot", + "OrisysSensorAdapter", + "SensorAdapter", + "calibrate_normal_force", + "create_sensor_adapter", + "roi_geometry", + "_snapshot_from_sensor", +] diff --git a/examples/gui/sensors/adapter.py b/examples/gui/sensors/adapter.py new file mode 100644 index 0000000..8386494 --- /dev/null +++ b/examples/gui/sensors/adapter.py @@ -0,0 +1,330 @@ +"""Sensor adapters that normalize frames for Qt / multi-sensor viewers.""" + +from __future__ import annotations + +import time +from abc import ABC, abstractmethod +from dataclasses import dataclass +from pathlib import Path +from typing import Any, Optional + +import cv2 +import numpy as np +import orisys + +from .roi_geometry import ( + build_roi_warp, + load_json_config, + load_roi_polygon, + open_video_source, + rectified_output_size, + warp_roi_to_rectangle, +) + + +NORMAL_FORCE_CAL_SLOPE = 3.92685554668795e-05 +NORMAL_FORCE_CAL_OFFSET = 0.0217596967278535 + + +@dataclass +class FrameSnapshot: + """Normalized per-frame payload consumed by the Qt viewer.""" + + image: Optional[np.ndarray] = None + flow: Optional[np.ndarray] = None + depth: Optional[np.ndarray] = None + contour: Optional[np.ndarray] = None + centroid: Any = None + # Raw SDK force channels used for curves (same units for FN/FX/FY). + normal_force: float = 0.0 + shear_x: float = 0.0 + shear_y: float = 0.0 + # Optional calibrated FN in Newtons for export/display overlays. + normal_force_calibrated: float = 0.0 + fps: float = 0.0 + timestamp: float = 0.0 + raw_image: Optional[np.ndarray] = None + processed_image: Optional[np.ndarray] = None + flow_overlay: Optional[np.ndarray] = None + record_frame: Optional[np.ndarray] = None + + +def calibrate_normal_force(raw_value: float) -> float: + """Convert raw normal-force reading to Newtons using y = mx + c.""" + return NORMAL_FORCE_CAL_SLOPE * float(raw_value) + NORMAL_FORCE_CAL_OFFSET + + +class SensorAdapter(ABC): + """Common interface between Qt and concrete sensor backends.""" + + @abstractmethod + def start(self) -> None: + raise NotImplementedError + + @abstractmethod + def read(self) -> Optional[FrameSnapshot]: + raise NotImplementedError + + @abstractmethod + def reset(self) -> None: + raise NotImplementedError + + @abstractmethod + def close(self) -> None: + raise NotImplementedError + + +def _bgr_from_gray_or_bgr(img: np.ndarray) -> np.ndarray: + if img is None: + return None + if img.ndim == 2: + return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + if img.ndim == 3 and img.shape[2] == 1: + return cv2.cvtColor(img[:, :, 0], cv2.COLOR_GRAY2BGR) + return img + + +def _snapshot_from_sensor( + sensor, + *, + raw_image=None, + processed_image=None, +) -> Optional[FrameSnapshot]: + """Build a FrameSnapshot with HHD/forces refreshed every frame.""" + if sensor is None or sensor.img is None: + return None + + # Always decompose so FN/FX/FY update every frame (motion gating freezes forces). + sensor.compute_deformation(decompose=True, check_motion=False) + sensor.compute_contact() + + fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = ( + sensor.read_info( + sensor.info.FPS, + sensor.info.VRAW, + sensor.info.VNORMAL, + sensor.info.IMG, + sensor.info.CONTOUR, + sensor.info.CENTROID, + sensor.info.DEPTH, + sensor.info.FNORMAL, + sensor.info.FSHEARX, + sensor.info.FSHEARY, + ) + ) + + image_bgr = _bgr_from_gray_or_bgr(img) + processed = processed_image if processed_image is not None else image_bgr + record = None + if getattr(sensor, "frame", None) is not None: + record = _bgr_from_gray_or_bgr(sensor.frame) + elif processed is not None: + record = processed + + flow_overlay = None + if image_bgr is not None and flow is not None: + flow_overlay = orisys.util.draw_arrows( + image_bgr, + flow, + threshold=2, + grid_spacing=20, + arrow_scale=0.5, + below_threshold_color=(160, 160, 160), + ) + + fn_raw = float(fnormal) + return FrameSnapshot( + image=image_bgr, + flow=flow, + depth=depth_map, + contour=contour, + centroid=centroid, + normal_force=fn_raw, + shear_x=float(fshearx), + shear_y=float(fsheary), + normal_force_calibrated=calibrate_normal_force(fn_raw), + fps=float(fps), + timestamp=time.time(), + raw_image=raw_image, + processed_image=processed, + flow_overlay=flow_overlay, + record_frame=record, + ) + + +class OrisysSensorAdapter(SensorAdapter): + """Direct orisys.Sensor capture path (compound-eye / single-camera).""" + + def __init__( + self, + video, + config_name, + cal_path=None, + verbose=True, + *, + rotate_ccw90=False, + motion_threshold=1, + ): + self.video = video + self.config_name = config_name + self.cal_path = cal_path + self.verbose = verbose + self.rotate_ccw90 = bool(rotate_ccw90) + self.motion_threshold = motion_threshold + self.sensor = None + + def start(self) -> None: + src = self.video + try: + src = int(str(src).strip()) + except (TypeError, ValueError): + pass + self.sensor = orisys.Sensor( + src, + config_name=self.config_name, + cal_path=self.cal_path, + verbose=self.verbose, + ) + + def read(self) -> Optional[FrameSnapshot]: + if self.sensor is None: + return None + img = self.sensor.get_img() + if img is None: + return None + + if self.rotate_ccw90: + self.sensor.img = cv2.rotate(self.sensor.img, cv2.ROTATE_90_COUNTERCLOCKWISE) + if self.sensor.frame is not None: + self.sensor.frame = cv2.rotate( + self.sensor.frame, cv2.ROTATE_90_COUNTERCLOCKWISE + ) + + return _snapshot_from_sensor( + self.sensor, + raw_image=_bgr_from_gray_or_bgr(self.sensor.frame) + if self.sensor.frame is not None + else None, + processed_image=_bgr_from_gray_or_bgr(self.sensor.img), + ) + + def reset(self) -> None: + if self.sensor is not None: + self.sensor.reset() + + def close(self) -> None: + if self.sensor is not None: + self.sensor.disconnect() + self.sensor = None + + +class Dd06RoiAdapter(SensorAdapter): + """DD06 path: external capture + polygon ROI rectify + already_processed Sensor.""" + + def __init__(self, video, config_name, verbose=True, *, motion_threshold=0): + self.video = video + self.config_name = config_name + self.verbose = verbose + self.motion_threshold = motion_threshold + self.sensor = None + self.cap = None + self.roi_points = None + self.roi_image_size = None + self.out_size = None + self.warp_matrix = None + self.warp_src = None + self.last_frame_size = None + self._source = None + + def start(self) -> None: + config = load_json_config(self.config_name) + self.roi_points, self.roi_image_size = load_roi_polygon(config) + self.out_size = rectified_output_size(config, self.roi_points) + self.sensor = orisys.Sensor( + "external", + config_name=self.config_name, + verbose=self.verbose, + ) + self.cap, self._source = open_video_source(self.video) + self.warp_matrix = None + self.last_frame_size = None + if self.verbose: + print( + f"DD06 adapter: ROI points={len(self.roi_points)} " + f"source_size={self.roi_image_size} output={self.out_size} " + f"video={self._source}" + ) + + def read(self) -> Optional[FrameSnapshot]: + if self.sensor is None or self.cap is None: + return None + + ok, frame = self.cap.read() + if not ok or frame is None: + return None + + frame_h, frame_w = frame.shape[:2] + frame_size = (frame_w, frame_h) + if self.warp_matrix is None or self.last_frame_size != frame_size: + self.warp_matrix, self.warp_src, _ = build_roi_warp( + self.roi_points, + self.roi_image_size, + frame_size, + self.out_size, + ) + self.last_frame_size = frame_size + if self.verbose: + print(f"Built ROI warp for frame {frame_w}x{frame_h}") + + rectified = warp_roi_to_rectangle(frame, self.warp_matrix, self.out_size) + self.sensor.get_img(frame=rectified, already_processed=True) + return _snapshot_from_sensor( + self.sensor, + raw_image=frame, + processed_image=rectified, + ) + + def reset(self) -> None: + if self.sensor is not None: + self.sensor.reset() + + def close(self) -> None: + if self.cap is not None: + self.cap.release() + self.cap = None + if self.sensor is not None: + self.sensor.disconnect() + self.sensor = None + + +def create_sensor_adapter( + sensor_type: str, + *, + video, + config_name, + cal_path=None, + verbose=True, +) -> SensorAdapter: + """Factory for Qt / demo viewers.""" + kind = str(sensor_type or "orisys").strip().lower() + if kind in ("orisys", "direct", "default", "compound_eye", "single_camera"): + # Preserve historical Wuxi viewer rotation for compound-eye demos. + rotate = kind in ("orisys", "direct", "default", "compound_eye") + return OrisysSensorAdapter( + video, + config_name, + cal_path=cal_path, + verbose=verbose, + rotate_ccw90=rotate, + motion_threshold=1, + ) + if kind in ("dd06", "dd06-ov", "roi", "polygon_roi"): + return Dd06RoiAdapter( + video, + config_name, + verbose=verbose, + motion_threshold=0, + ) + raise ValueError( + f"Unknown sensor_type={sensor_type!r}; expected 'orisys' or 'dd06'" + ) diff --git a/examples/gui/sensors/roi_geometry.py b/examples/gui/sensors/roi_geometry.py new file mode 100644 index 0000000..2cff48c --- /dev/null +++ b/examples/gui/sensors/roi_geometry.py @@ -0,0 +1,140 @@ +"""Polygon ROI geometry helpers for rectifying tactile sensor frames.""" + +from __future__ import annotations + +import json +from pathlib import Path + +import cv2 +import numpy as np + + +def load_json_config(path): + path = Path(path) + with path.open("r", encoding="utf-8") as f: + return json.load(f) + + +def load_roi_polygon(config): + """Return (points Nx2 float32, image_size [w,h]) from nested or legacy ROI fields.""" + roi = config.get("roi") if isinstance(config.get("roi"), dict) else config + if not isinstance(roi, dict): + raise ValueError("Config has no usable ROI section") + + image_size = roi.get("image_size") or config.get("camera", {}).get("resolution_raw") + if image_size is None: + raise ValueError("ROI image_size is missing") + + points = roi.get("points") or roi.get("polygon") or roi.get("roi_polygon") + if points is None and roi.get("xyxy") is not None: + x1, y1, x2, y2 = [int(v) for v in roi["xyxy"]] + points = [[x1, y1], [x2, y1], [x2, y2], [x1, y2]] + if not points: + raise ValueError("ROI points are missing") + + pts = np.asarray(points, dtype=np.float32).reshape(-1, 2) + if pts.shape[0] < 4: + raise ValueError(f"ROI needs at least 4 points, got {pts.shape[0]}") + return pts, [int(image_size[0]), int(image_size[1])] + + +def scale_points(points, source_size, target_size): + source_w, source_h = float(source_size[0]), float(source_size[1]) + target_w, target_h = float(target_size[0]), float(target_size[1]) + pts = np.asarray(points, dtype=np.float32).reshape(-1, 2).copy() + if source_w > 0 and source_h > 0 and (source_w, source_h) != (target_w, target_h): + pts[:, 0] *= target_w / source_w + pts[:, 1] *= target_h / source_h + return pts + + +def order_quad_points(points): + """Order 4 points as TL, TR, BR, BL for perspective transforms.""" + pts = np.asarray(points, dtype=np.float32).reshape(-1, 2) + if pts.shape[0] != 4: + hull = cv2.convexHull(pts.astype(np.float32)) + rect = cv2.minAreaRect(hull) + pts = cv2.boxPoints(rect).astype(np.float32) + + s = pts.sum(axis=1) + diff = np.diff(pts, axis=1).reshape(-1) + ordered = np.zeros((4, 2), dtype=np.float32) + ordered[0] = pts[np.argmin(s)] # top-left + ordered[2] = pts[np.argmax(s)] # bottom-right + ordered[1] = pts[np.argmin(diff)] # top-right + ordered[3] = pts[np.argmax(diff)] # bottom-left + return ordered + + +def rectified_output_size(config, points=None): + """Prefer camera.resolution_stitch; fall back to ROI bounding box size.""" + stitch = config.get("camera", {}).get("resolution_stitch") + if stitch is not None and len(stitch) >= 2: + return int(stitch[0]), int(stitch[1]) + if points is not None: + pts = np.asarray(points, dtype=np.float32).reshape(-1, 2) + width = max( + int(np.linalg.norm(pts[1] - pts[0])), + int(np.linalg.norm(pts[2] - pts[3])), + 1, + ) + height = max( + int(np.linalg.norm(pts[3] - pts[0])), + int(np.linalg.norm(pts[2] - pts[1])), + 1, + ) + return width, height + raise ValueError("Cannot determine rectified output size") + + +def build_roi_warp(points, source_image_size, frame_size, output_size): + """ + Build a perspective transform that maps the ROI polygon in frame coords + onto a rectangle of ``output_size`` (width, height). + """ + frame_w, frame_h = int(frame_size[0]), int(frame_size[1]) + scaled = scale_points(points, source_image_size, (frame_w, frame_h)) + src = order_quad_points(scaled) + out_w, out_h = int(output_size[0]), int(output_size[1]) + dst = np.array( + [ + [0.0, 0.0], + [out_w - 1.0, 0.0], + [out_w - 1.0, out_h - 1.0], + [0.0, out_h - 1.0], + ], + dtype=np.float32, + ) + matrix = cv2.getPerspectiveTransform(src, dst) + return matrix, src, (out_w, out_h) + + +def warp_roi_to_rectangle(frame, matrix, output_size): + out_w, out_h = int(output_size[0]), int(output_size[1]) + return cv2.warpPerspective( + frame, + matrix, + (out_w, out_h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_REPLICATE, + ) + + +def open_video_source(video): + """Open a camera index or video file. Returns (cap, resolved_source).""" + text = str(video).strip() + if text.isdigit(): + source = int(text) + cap = cv2.VideoCapture(source, cv2.CAP_DSHOW) + if not cap.isOpened(): + cap.release() + cap = cv2.VideoCapture(source) + else: + path = Path(text) + if not path.exists(): + raise FileNotFoundError(f"Video not found: {path}") + source = str(path) + cap = cv2.VideoCapture(source) + if not cap.isOpened(): + raise RuntimeError(f"Failed to open video source: {video}") + return cap, source diff --git a/examples/gui/viewer2d/__init__.py b/examples/gui/viewer2d/__init__.py new file mode 100644 index 0000000..5673c59 --- /dev/null +++ b/examples/gui/viewer2d/__init__.py @@ -0,0 +1,24 @@ +"""2D Qt sensor monitor (image + force curves; no finger 3D mesh).""" + +from .backend import ( + ForceHistory, + ForcePlotState, + QtViewerBackend, + ViewerFrame, + image_shape_meta, + panel_width_for_aspect, + select_display_image, +) +from .window import QtViewerWindow, run_viewer + +__all__ = [ + "ForceHistory", + "ForcePlotState", + "QtViewerBackend", + "QtViewerWindow", + "ViewerFrame", + "image_shape_meta", + "panel_width_for_aspect", + "run_viewer", + "select_display_image", +] diff --git a/examples/gui/viewer2d/backend.py b/examples/gui/viewer2d/backend.py new file mode 100644 index 0000000..8fc92f2 --- /dev/null +++ b/examples/gui/viewer2d/backend.py @@ -0,0 +1,312 @@ +"""Qt-facing display backend for sensor viewer snapshots. + +Separates acquisition (`gui.sensors.FrameSnapshot`) from Qt display state: +normalized images, force history / filtering, and aspect-ratio metadata. +Does not import camera, ROI, or `orisys.Sensor`. +""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import Optional, Sequence, Tuple + +import cv2 +import numpy as np +from scipy.signal import butter, filtfilt + +FORCE_SAMPLE_RATE_HZ = 30.0 +PLOT_BUFFER_POINTS = 120 +PLOT_WINDOW_SEC = PLOT_BUFFER_POINTS / FORCE_SAMPLE_RATE_HZ +Y_AXIS_MIN_SPAN = 20000.0 + + +@dataclass +class ViewerFrame: + """Normalized display payload for one UI tick.""" + + display_image: np.ndarray + image_height: int + image_width: int + aspect_ratio: float # width / height + depth: Optional[np.ndarray] + normal_force: float + shear_x: float + shear_y: float + normal_force_calibrated: float + fps: float + timestamp: float + record_frame: Optional[np.ndarray] + + +@dataclass +class ForcePlotState: + """Filtered force series ready for pyqtgraph curves.""" + + times: np.ndarray # shape (n,) + values: np.ndarray # shape (n, 3) FN, FX, FY + sample_count: int + x_range: Tuple[float, float] + y_ranges: Tuple[Tuple[float, float], Tuple[float, float], Tuple[float, float]] + + +def lowpass_filter( + data, + cutoff_hz: float, + sample_rate_hz: float = FORCE_SAMPLE_RATE_HZ, + order: int = 2, +) -> np.ndarray: + """Zero-phase Butterworth low-pass for uniformly sampled 1-D data.""" + y = np.asarray(data, dtype=np.float64) + if cutoff_hz == 0: + return y.copy() + if y.size == 0: + return y + + nyquist = 0.5 * sample_rate_hz + if cutoff_hz < 0 or cutoff_hz >= nyquist: + raise ValueError( + f"cutoff_hz must be in (0, {nyquist}) for sample_rate_hz={sample_rate_hz}" + ) + + b, a = butter(order, cutoff_hz / nyquist, btype="low") + padlen = 3 * (max(len(b), len(a)) - 1) + if y.size <= padlen: + return y.copy() + return filtfilt(b, a, y) + + +def ensure_bgr(img: Optional[np.ndarray]) -> Optional[np.ndarray]: + if img is None: + return None + if img.ndim == 2: + return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + if img.ndim == 3 and img.shape[2] == 1: + return cv2.cvtColor(img[:, :, 0], cv2.COLOR_GRAY2BGR) + return img + + +def image_shape_meta(img: np.ndarray) -> Tuple[int, int, float]: + """Return (height, width, width/height) from a display image.""" + h, w = int(img.shape[0]), int(img.shape[1]) + aspect = float(w) / float(h) if h > 0 else 1.0 + return h, w, aspect + + +def select_display_image(snapshot) -> np.ndarray: + """Pick flow overlay / processed / raw image for the left panel.""" + arrows = getattr(snapshot, "flow_overlay", None) + if arrows is not None: + out = ensure_bgr(arrows) + if out is not None: + return out + + for attr in ("processed_image", "image", "raw_image"): + base = getattr(snapshot, attr, None) + out = ensure_bgr(base) + if out is not None: + return out + + return np.zeros((400, 400, 3), dtype=np.uint8) + + +def bgr_to_rgb_bytes(img_bgr: np.ndarray) -> Tuple[np.ndarray, int, int]: + """Convert BGR uint8 image to contiguous RGB for QImage.""" + rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) + if not rgb.flags["C_CONTIGUOUS"]: + rgb = np.ascontiguousarray(rgb) + h, w = rgb.shape[:2] + return rgb, h, w + + +def scale_bgr_to_fit( + img_bgr: np.ndarray, + max_w: int, + max_h: int, +) -> np.ndarray: + """Resize preserving aspect ratio to fit inside (max_w, max_h).""" + if max_w <= 0 or max_h <= 0: + return img_bgr + h, w = img_bgr.shape[:2] + if h <= 0 or w <= 0: + return img_bgr + scale = min(max_w / float(w), max_h / float(h)) + if abs(scale - 1.0) < 1e-6: + return img_bgr + nw = max(1, int(round(w * scale))) + nh = max(1, int(round(h * scale))) + return cv2.resize(img_bgr, (nw, nh), interpolation=cv2.INTER_AREA) + + +def panel_width_for_aspect( + available_height: int, + aspect_ratio: float, + *, + min_size: int = 160, +) -> int: + """Preferred left-panel width given available height and image aspect.""" + h = max(int(available_height), min_size) + w = int(round(h * float(aspect_ratio))) + return max(w, min_size) + + +class ForceHistory: + """Ring-style force history buffer with filtering and y-range smoothing.""" + + def __init__( + self, + npoints: int = PLOT_BUFFER_POINTS, + *, + sample_rate_hz: float = FORCE_SAMPLE_RATE_HZ, + lowpass_cutoff_hz: float = 10.0, + y_axis_min_span: float = Y_AXIS_MIN_SPAN, + y_range_alpha: float = 0.5, + ): + self.npoints = int(npoints) + self.sample_rate_hz = float(sample_rate_hz) + self.lowpass_cutoff_hz = float(lowpass_cutoff_hz) + self.y_axis_min_span = float(y_axis_min_span) + self.y_range_alpha = float(y_range_alpha) + self.window_sec = self.npoints / self.sample_rate_hz + self.reset() + + def reset(self) -> None: + self.data = np.full((self.npoints, 2, 3), np.nan, dtype=np.float64) + self.sample_count = 0 + self._y_range_smooth = [None, None, None] + + def push(self, t: float, fn: float, fx: float, fy: float) -> None: + values = (float(fn), float(fx), float(fy)) + if self.sample_count < self.npoints: + idx = self.sample_count + self.data[idx, 0, :] = t + self.data[idx, 1, :] = values + self.sample_count += 1 + return + + self.data[:-1, 0, :] = self.data[1:, 0, :] + self.data[:-1, 1, :] = self.data[1:, 1, :] + self.data[-1, 0, :] = t + self.data[-1, 1, :] = values + + def _smooth_y_range(self, channel: int, ymin: float, ymax: float) -> Tuple[float, float]: + if self._y_range_smooth[channel] is None: + smoothed = (ymin, ymax) + else: + prev_min, prev_max = self._y_range_smooth[channel] + alpha = self.y_range_alpha + ymin = alpha * ymin + (1.0 - alpha) * prev_min + ymax = alpha * ymax + (1.0 - alpha) * prev_max + smoothed = (ymin, ymax) + + span = smoothed[1] - smoothed[0] + if span < self.y_axis_min_span: + mid = 0.5 * (smoothed[0] + smoothed[1]) + half = self.y_axis_min_span / 2.0 + smoothed = (mid - half, mid + half) + + self._y_range_smooth[channel] = smoothed + pad = max((smoothed[1] - smoothed[0]) * 0.12, 0.01) + return smoothed[0] - pad, smoothed[1] + pad + + def plot_state(self) -> Optional[ForcePlotState]: + n = self.sample_count + if n == 0: + return None + + data_plot = self.data[:n].copy() + for i in range(3): + data_plot[:, 1, i] = lowpass_filter( + data_plot[:, 1, i], + self.lowpass_cutoff_hz, + sample_rate_hz=self.sample_rate_hz, + ) + + times = data_plot[:, 0, 0].copy() + values = data_plot[:, 1, :].copy() + y_ranges = tuple( + self._smooth_y_range(i, float(values[:, i].min()), float(values[:, i].max())) + for i in range(3) + ) + + if n < self.npoints: + x_range = (0.0, self.window_sec) + else: + x0 = float(times[0]) + x1 = float(times[-1]) + if x1 <= x0: + x1 = x0 + self.window_sec + x_range = (x0, x1) + + return ForcePlotState( + times=times, + values=values, + sample_count=n, + x_range=x_range, + y_ranges=y_ranges, # type: ignore[arg-type] + ) + + +class QtViewerBackend: + """Normalize FrameSnapshot into display images + force history.""" + + def __init__( + self, + *, + npoints: int = PLOT_BUFFER_POINTS, + lowpass_cutoff_hz: float = 10.0, + y_axis_min_span: float = Y_AXIS_MIN_SPAN, + ): + self.force_history = ForceHistory( + npoints=npoints, + lowpass_cutoff_hz=lowpass_cutoff_hz, + y_axis_min_span=y_axis_min_span, + ) + self._start_time: Optional[float] = None + self.last_viewer_frame: Optional[ViewerFrame] = None + + def reset(self) -> None: + self.force_history.reset() + self._start_time = None + self.last_viewer_frame = None + + def ingest(self, snapshot, *, now: Optional[float] = None) -> ViewerFrame: + """Convert a FrameSnapshot into ViewerFrame and update force history.""" + display = select_display_image(snapshot) + h, w, aspect = image_shape_meta(display) + + fn = float(getattr(snapshot, "normal_force", 0.0)) + fx = float(getattr(snapshot, "shear_x", 0.0)) + fy = float(getattr(snapshot, "shear_y", 0.0)) + fn_cal = float(getattr(snapshot, "normal_force_calibrated", 0.0)) + fps = float(getattr(snapshot, "fps", 0.0)) + ts = float(getattr(snapshot, "timestamp", 0.0) or 0.0) + if now is None: + now = ts if ts > 0 else 0.0 + + if self._start_time is None: + self._start_time = now + t = now - self._start_time + self.force_history.push(t, fn, fx, fy) + + record = ensure_bgr(getattr(snapshot, "record_frame", None)) + depth = getattr(snapshot, "depth", None) + + frame = ViewerFrame( + display_image=display, + image_height=h, + image_width=w, + aspect_ratio=aspect, + depth=depth, + normal_force=fn, + shear_x=fx, + shear_y=fy, + normal_force_calibrated=fn_cal, + fps=fps, + timestamp=ts, + record_frame=record, + ) + self.last_viewer_frame = frame + return frame + + def plot_state(self) -> Optional[ForcePlotState]: + return self.force_history.plot_state() diff --git a/examples/gui/viewer2d/styles.py b/examples/gui/viewer2d/styles.py new file mode 100644 index 0000000..c1bded0 --- /dev/null +++ b/examples/gui/viewer2d/styles.py @@ -0,0 +1,120 @@ +"""Styles for the 2D Qt sensor monitor.""" + +CURVE_THEME_COLOR = "#00E5FF" + +STYLE = """ +QMainWindow { + background-color: #000000; +} + +/* ---- 图像面板 ---- */ +QLabel#arrowsLabel { + border: 1px solid #262626; + border-radius: 4px; + background-color: #0b0b0b; +} + +/* ---- 顶栏按钮 & 输入 ---- */ +QPushButton { + background-color: #1a1a1a; + color: #999999; + border: 1px solid #333333; + border-radius: 4px; + padding: 6px 16px; + font-family: "SimHei"; + font-size: 13px; +} +QPushButton:hover { + background-color: #262626; + color: #cccccc; + border: 1px solid #555555; +} +QPushButton#btnStart { + background-color: #0d2818; + color: #4caf84; + border: 1px solid #1a4d30; +} +QPushButton#btnStart:hover { + background-color: #143d24; + color: #6fd4a0; +} +QPushButton#btnStart:disabled { + background-color: #0f0f0f; + color: #333333; + border: 1px solid #222222; +} +QPushButton#btnStop { + background-color: #2b1010; + color: #e05555; + border: 1px solid #4d1a1a; +} +QPushButton#btnStop:hover { + background-color: #3d1818; + color: #f07070; +} +QPushButton#btnStop:disabled { + background-color: #0f0f0f; + color: #333333; + border: 1px solid #222222; +} +QLineEdit { + background-color: #0d0d0d; + color: #999999; + border: 1px solid #333333; + border-radius: 4px; + padding: 5px 10px; + font-size: 13px; +} + +/* ---- 力曲线容器 ---- */ +GraphicsLayoutWidget { + border: 1px solid #262626; + border-radius: 4px; +} + +/* ---- 日志面板 ---- */ +QPlainTextEdit { + background-color: #0b0b0b; + color: #777777; + border: 1px solid #262626; + border-radius: 4px; + font-family: "Cascadia Code", "Consolas", "Courier New", monospace; + font-size: 13px; + padding: 8px; + selection-background-color: #1a3a4a; + selection-color: #cccccc; +} + +QPlainTextEdit QScrollBar:vertical { + background-color: #0b0b0b; + width: 6px; + margin: 1px; + border-radius: 3px; +} +QPlainTextEdit QScrollBar::handle:vertical { + background-color: #262626; + border-radius: 3px; + min-height: 30px; +} +QPlainTextEdit QScrollBar::handle:vertical:hover { + background-color: #3a3a3a; +} +QPlainTextEdit QScrollBar::add-line:vertical, +QPlainTextEdit QScrollBar::sub-line:vertical { + height: 0px; +} +QPlainTextEdit QScrollBar::add-page:vertical, +QPlainTextEdit QScrollBar::sub-page:vertical { + background: none; +} + +QSplitter::handle { + background-color: #1a1a1a; +} +QSplitter::handle:vertical { + height: 1px; +} +QSplitter::handle:horizontal { + width: 1px; +} +""" diff --git a/examples/gui/viewer2d/window.py b/examples/gui/viewer2d/window.py new file mode 100644 index 0000000..9bec853 --- /dev/null +++ b/examples/gui/viewer2d/window.py @@ -0,0 +1,491 @@ +"""2D Qt sensor monitor window (image + force curves).""" + +from __future__ import annotations + +import os +import sys +import time +from datetime import datetime +from pathlib import Path + +import cv2 +from PyQt5.QtCore import Qt, QTimer +from PyQt5.QtGui import QFont, QIcon +from PyQt5.QtWidgets import ( + QApplication, + QFileDialog, + QHBoxLayout, + QLabel, + QLineEdit, + QMainWindow, + QPushButton, + QSplitter, + QVBoxLayout, + QWidget, +) + +try: + import pyqtgraph as pg +except Exception as exc: # pragma: no cover + raise ImportError( + "PyQtGraph is not installed. Please install it using 'pip install pyqt5 pyqtgraph'." + ) from exc + +from gui.sensors import create_sensor_adapter +from gui.widgets.log_panel import TeeStream, WidgetLogHandler, make_log_widget +from gui.widgets.qt_pixmap import ndarray_to_pixmap, pixmap_keep_aspect + +from .backend import PLOT_BUFFER_POINTS, PLOT_WINDOW_SEC, QtViewerBackend, panel_width_for_aspect +from .styles import CURVE_THEME_COLOR, STYLE + + +class QtViewerWindow(QMainWindow): + """2D monitor: flow/image panel + FN/FX/FY curves driven by QtViewerBackend.""" + + def __init__(self, args, log_file): + super().__init__() + self.setWindowTitle("Orisys SDK") + self.setStyleSheet(STYLE) + + logo_path = Path(__file__).resolve().parents[3] / "logo.png" + if logo_path.exists(): + self.setWindowIcon(QIcon(str(logo_path))) + + self.sensor_adapter = None + self._last_snapshot = None + self._last_viewer_frame = None + self._display_aspect = 1.0 + self._source_pixmap = None + self.args = args + + self.is_save = False + self.frame_count = 0 + self.writer = None + self.save_path = None + + self.viewer_backend = QtViewerBackend( + npoints=PLOT_BUFFER_POINTS, + lowpass_cutoff_hz=getattr(args, "cutoff", 10.0), + ) + + self._setup_ui() + self._create_plots() + + self.camera_edit.setText(args.video) + self.config_edit.setText(args.config) + self.cal_edit.setText(args.cal) + + self.log_handler = WidgetLogHandler(self.log_widget) + sys.stdout = TeeStream(log_file, self.log_handler) + sys.stderr = sys.stdout + + self.timer = QTimer() + self.timer.timeout.connect(self._on_timer) + self._running = False + + def _setup_ui(self): + central = QWidget() + self.setCentralWidget(central) + root_layout = QVBoxLayout(central) + root_layout.setContentsMargins(6, 6, 6, 6) + root_layout.setSpacing(6) + + bar = QHBoxLayout() + bar.setSpacing(8) + + self.btn_camera = QPushButton("摄像机") + self.btn_camera.setObjectName("btnCamera") + self.btn_camera.clicked.connect(self._select_camera) + bar.addWidget(self.btn_camera) + self.camera_edit = QLineEdit("1") + self.camera_edit.setPlaceholderText("摄像头编号 或 视频路径") + bar.addWidget(self.camera_edit, 1) + + self.btn_config = QPushButton("配置") + self.btn_config.setObjectName("btnConfig") + self.btn_config.clicked.connect(self._select_config) + bar.addWidget(self.btn_config) + self.config_edit = QLineEdit("./config/ddjx01.json") + bar.addWidget(self.config_edit, 1) + + self.btn_reset = QPushButton("重置") + self.btn_reset.setObjectName("btnReset") + self.btn_reset.setEnabled(False) + self.btn_reset.clicked.connect(self._on_reset) + bar.addWidget(self.btn_reset) + self.cal_edit = QLineEdit("./config/ddjx01.npy") + bar.addWidget(self.cal_edit, 1) + + self.btn_start = QPushButton("启动") + self.btn_start.setObjectName("btnStart") + self.btn_start.clicked.connect(self._on_start) + bar.addWidget(self.btn_start) + + self.btn_stop = QPushButton("暂停") + self.btn_stop.setObjectName("btnStop") + self.btn_stop.setEnabled(False) + self.btn_stop.clicked.connect(self._on_stop) + bar.addWidget(self.btn_stop) + + root_layout.addLayout(bar) + + v_splitter = QSplitter(Qt.Vertical) + h_splitter = QSplitter(Qt.Horizontal) + + left = QWidget() + left_layout = QVBoxLayout(left) + left_layout.setContentsMargins(0, 0, 0, 0) + left_layout.setSpacing(6) + + self.arrows_label = QLabel() + self.arrows_label.setObjectName("arrowsLabel") + self.arrows_label.setMinimumSize(160, 160) + self.arrows_label.setScaledContents(False) + self.arrows_label.setAlignment(Qt.AlignCenter) + left_layout.addWidget(self.arrows_label) + + h_splitter.addWidget(left) + + pg.setConfigOption("background", "#000000") + pg.setConfigOption("foreground", "#777777") + pg.setConfigOptions(antialias=True) + self.graph_widget = pg.GraphicsLayoutWidget() + self.graph_widget.setMinimumWidth(300) + h_splitter.addWidget(self.graph_widget) + + self.h_splitter = h_splitter + v_splitter.addWidget(h_splitter) + + from gui.log_config import LOG_CONFIG + + self.log_widget = make_log_widget(LOG_CONFIG) + v_splitter.addWidget(self.log_widget) + + v_splitter.setStretchFactor(0, 4) + v_splitter.setStretchFactor(1, 1) + root_layout.addWidget(v_splitter) + + def _create_plots(self): + titles = ["法向力 (raw)", "切向力 X", "切向力 Y"] + self.plots = [] + self.curves = [] + self.dot_glows = [] + self.dot_cores = [] + + axis_pen = pg.mkPen(color=(38, 38, 38), width=1) + label_font = QFont("SimHei", 9) + + for i, title in enumerate(titles): + p = self.graph_widget.addPlot(title=None) + p.setTitle(title, color="#999999", size="14pt") + p.titleLabel.setFont(QFont("SimHei", 14)) + p.setLabel("bottom", "时间", units="s") + p.getAxis("bottom").label.setFont(label_font) + p.showAxis("top", False) + p.showAxis("right", False) + p.getAxis("left").setPen(axis_pen) + p.getAxis("bottom").setPen(axis_pen) + p.showGrid(x=False, y=True, alpha=0.12) + + tick_font = QFont("Consolas", 11) + tick_font.setStyleHint(QFont.Monospace) + p.getAxis("left").setTickFont(tick_font) + p.getAxis("bottom").setTickFont(tick_font) + + line_pen = pg.mkPen(color=CURVE_THEME_COLOR, width=2) + curve = p.plot(pen=line_pen, fillLevel=0) + dot_glow = p.plot( + [], + [], + pen=None, + symbol="o", + symbolSize=8, + symbolPen=None, + symbolBrush=(0, 229, 255, 76), + ) + dot_core = p.plot( + [], + [], + pen=None, + symbol="o", + symbolSize=4, + symbolPen=None, + symbolBrush=(255, 255, 255, 255), + ) + + p.setXRange(0, PLOT_WINDOW_SEC, padding=0) + p.enableAutoRange(x=False, y=False) + + self.plots.append(p) + self.curves.append(curve) + self.dot_glows.append(dot_glow) + self.dot_cores.append(dot_core) + + if i < 2: + self.graph_widget.nextRow() + + def adjust_image_panel(self): + """Size the left panel from the current image aspect ratio.""" + top_bar_h = 48 + log_h = self.log_widget.height() + spacing = 20 + available = max(self.height() - top_bar_h - log_h - spacing, 160) + image_w = panel_width_for_aspect(available, self._display_aspect, min_size=160) + max_w = max(self.width() // 2, 160) + image_w = min(image_w, max_w) + self.h_splitter.setSizes([image_w, max(self.width() - image_w, 200)]) + self._render_display_pixmap() + + def resizeEvent(self, event): + super().resizeEvent(event) + self._render_display_pixmap() + + def _render_display_pixmap(self): + if self._source_pixmap is None or self._source_pixmap.isNull(): + return + target = self.arrows_label.size() + if target.width() < 2 or target.height() < 2: + return + self.arrows_label.setPixmap(pixmap_keep_aspect(self._source_pixmap, target)) + + def _on_timer(self): + if self.sensor_adapter is None: + return + + snapshot = self.sensor_adapter.read() + if snapshot is None: + print("Failed to get image") + return + self._last_snapshot = snapshot + + viewer_frame = self.viewer_backend.ingest(snapshot, now=time.time()) + self._last_viewer_frame = viewer_frame + first_image = self._source_pixmap is None + aspect_changed = abs(viewer_frame.aspect_ratio - self._display_aspect) > 1e-3 + self._display_aspect = viewer_frame.aspect_ratio + + if self.is_save and self.writer is not None and snapshot.record_frame is not None: + self.frame_count += 1 + save_img = snapshot.record_frame + if len(save_img.shape) == 2 or ( + len(save_img.shape) == 3 and save_img.shape[2] == 1 + ): + if len(save_img.shape) == 3: + save_img = save_img.squeeze(2) + save_img = cv2.cvtColor(save_img, cv2.COLOR_GRAY2BGR) + self.writer.write(save_img) + + print( + f"FPS={viewer_frame.fps:.2f}, " + f"法向力(raw)={viewer_frame.normal_force:.4f}, " + f"切向力X={viewer_frame.shear_x:.4f}, " + f"切向力Y={viewer_frame.shear_y:.4f}, " + f"法向力(N)={viewer_frame.normal_force_calibrated:.6f}" + ) + + self._source_pixmap = ndarray_to_pixmap(viewer_frame.display_image, bgr=True) + if first_image or aspect_changed: + self.adjust_image_panel() + else: + self._render_display_pixmap() + + plot_state = self.viewer_backend.plot_state() + if plot_state is None: + return + + for i, curve in enumerate(self.curves): + x = plot_state.times + y = plot_state.values[:, i] + curve.setData(x, y) + ymin, ymax = plot_state.y_ranges[i] + self.plots[i].setYRange(ymin, ymax) + self.dot_glows[i].setData(x[-1:], y[-1:]) + self.dot_cores[i].setData(x[-1:], y[-1:]) + + x0, x1 = plot_state.x_range + for p in self.plots: + p.setXRange(x0, x1, padding=0) + + def _on_start(self): + if self._running: + return + self.btn_start.setText("启动中...") + self.btn_start.setEnabled(False) + QApplication.processEvents() + + try: + src = self.camera_edit.text() + cfg = self.config_edit.text() + cal = self.cal_edit.text() + verbose = getattr(self.args, "verbose", True) + sensor_type = getattr(self.args, "sensor_type", "orisys") + self.sensor_adapter = create_sensor_adapter( + sensor_type, + video=src, + config_name=cfg, + cal_path=cal, + verbose=verbose, + ) + self.sensor_adapter.start() + except Exception as e: + self.btn_start.setText("启动") + self.btn_start.setEnabled(True) + self.sensor_adapter = None + print(f"启动失败: {e}") + return + + self.viewer_backend.reset() + self._running = True + self.timer.start(10) + self.btn_start.setText("启动") + self.btn_stop.setEnabled(True) + self.btn_reset.setEnabled(True) + print( + f"▶ 启动 — sensor_type={getattr(self.args, 'sensor_type', 'orisys')}, " + f"摄像机={src}, 配置={cfg}, 标定={cal}" + ) + + def _on_stop(self): + if not self._running: + return + self._running = False + self.timer.stop() + self.btn_stop.setText("停止中...") + self.btn_stop.setEnabled(False) + QApplication.processEvents() + + if self.sensor_adapter is not None: + self.sensor_adapter.close() + self.sensor_adapter = None + self._last_snapshot = None + + self.btn_start.setEnabled(True) + self.btn_stop.setText("暂停") + self.btn_reset.setEnabled(False) + print("⏸ 已暂停") + + def _select_camera(self): + path, _ = QFileDialog.getOpenFileName( + self, + "选择视频文件", + "data", + "视频 (*.mp4 *.avi *.mov);;所有文件 (*)", + ) + if path: + self.camera_edit.setText(path) + + def _select_config(self): + path, _ = QFileDialog.getOpenFileName( + self, + "选择配置文件", + "config", + "JSON (*.json);;所有文件 (*)", + ) + if path: + self.config_edit.setText(path) + + def _on_reset(self): + if self.sensor_adapter is None or not self._running: + print("请先启动传感器后再重置追踪器。") + return + self.sensor_adapter.reset() + print("追踪器已重置。") + + def keyPressEvent(self, event): + key = event.key() + if key == Qt.Key_Escape: + if self.isFullScreen(): + self.showNormal() + else: + self.showFullScreen() + elif key == Qt.Key_Q: + print("\n正在退出示例程序...") + self.close() + elif key == Qt.Key_S: + self._start_recording() + elif key == Qt.Key_E: + self._stop_recording() + elif key == Qt.Key_R: + self._on_reset() + else: + super().keyPressEvent(event) + + def _start_recording(self): + if self.sensor_adapter is None: + print("请先启动采集") + return + self.save_path = "./data/pulse_test.mp4" + os.makedirs(os.path.dirname(self.save_path), exist_ok=True) + + save_img = None + if self._last_snapshot is not None: + save_img = self._last_snapshot.record_frame + if save_img is None: + print("当前没有可用帧，无法确定录像尺寸。") + return + + temp_img = save_img.copy() + if len(temp_img.shape) == 2 or ( + len(temp_img.shape) == 3 and temp_img.shape[2] == 1 + ): + if len(temp_img.shape) == 3: + temp_img = temp_img.squeeze(2) + temp_img = cv2.cvtColor(temp_img, cv2.COLOR_GRAY2BGR) + + fh, fw = temp_img.shape[:2] + self.writer = cv2.VideoWriter( + self.save_path, + cv2.VideoWriter_fourcc(*"mp4v"), + 30, + (fw, fh), + ) + if self.writer.isOpened(): + self.is_save = True + print(f"开始录像：{self.save_path}") + else: + print(f"无法创建录像文件：{self.save_path}") + self.writer = None + + def _stop_recording(self): + self.is_save = False + if self.writer is not None: + self.writer.release() + self.writer = None + if self.save_path is not None: + print(f"录像结束：{self.save_path}，总帧数：{self.frame_count}") + else: + print(f"录像结束，总帧数：{self.frame_count}") + self.frame_count = 0 + + def closeEvent(self, event): + self.timer.stop() + if self.writer is not None: + self.writer.release() + if self.sensor_adapter is not None: + self.sensor_adapter.close() + self.sensor_adapter = None + event.accept() + + +def run_viewer(args, *, log_dir: str | Path | None = None) -> int: + """Launch the 2D Qt viewer and block until the window closes.""" + from gui.app import get_or_create_qapp + + if log_dir is None: + log_dir = Path(__file__).resolve().parents[3] / "scripts" / "logs" + else: + log_dir = Path(log_dir) + log_dir.mkdir(parents=True, exist_ok=True) + log_path = log_dir / f"qt_viewer_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log" + log_file = open(log_path, "w", encoding="utf-8") + sys.stdout = log_file + sys.stderr = sys.stdout + + print("\n按键说明：Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏") + + app = get_or_create_qapp("Orisys SDK") + window = QtViewerWindow(args, log_file) + window.showFullScreen() + QApplication.processEvents() + window.adjust_image_panel() + return app.exec_() diff --git a/examples/gui/viewer3d/__init__.py b/examples/gui/viewer3d/__init__.py new file mode 100644 index 0000000..90ae850 --- /dev/null +++ b/examples/gui/viewer3d/__init__.py @@ -0,0 +1,36 @@ +"""Finger 3D Qt dashboard (mesh + UV arrows + force charts).""" + +from .dashboard import FingerDashboardWindow, run_dashboard +from .layout import ( + LOG_PANEL, + LAYOUT, + MESH_CONTACT_ARROW, + MESH_LIGHTING, + PANEL_SETUP, + TOP_ROW, + DashboardLayout, + MeshLighting, + PanelRow, + PanelSetup, + PanelSlot, + top_row_splitter_sizes, + top_row_slot_index, +) + +__all__ = [ + "DashboardLayout", + "FingerDashboardWindow", + "LAYOUT", + "LOG_PANEL", + "MESH_CONTACT_ARROW", + "MESH_LIGHTING", + "MeshLighting", + "PANEL_SETUP", + "TOP_ROW", + "PanelRow", + "PanelSetup", + "PanelSlot", + "run_dashboard", + "top_row_splitter_sizes", + "top_row_slot_index", +] diff --git a/examples/gui/camera_discovery.py b/examples/gui/viewer3d/camera_discovery.py similarity index 100% rename from examples/gui/camera_discovery.py rename to examples/gui/viewer3d/camera_discovery.py diff --git a/examples/gui/dashboard.py b/examples/gui/viewer3d/dashboard.py similarity index 99% rename from examples/gui/dashboard.py rename to examples/gui/viewer3d/dashboard.py index bd2b094..b85828b 100644 --- a/examples/gui/dashboard.py +++ b/examples/gui/viewer3d/dashboard.py @@ -21,7 +21,7 @@ from pipeline.config import PipelineConfig from pipeline.csv_recorder import CsvRecorder from pipeline.service import FingerPipeline -from .app_paths import resource_path +from gui.app_paths import resource_path from .camera_discovery import list_cameras, pick_default_camera from .preferences import get_preferred_video_source, load_preferences, save_preferences from .layout import ( @@ -33,14 +33,12 @@ from .layout import ( ) from .mesh_view_state import MeshViewState from .styles import STYLE +from gui.widgets import TeeStream, WidgetLogHandler, make_log_widget from .widgets import ( ArrowPanelWidget, ForceChartWidget, MeshViewportWidget, SettingsPanelWidget, - TeeStream, - WidgetLogHandler, - make_log_widget, ) @@ -689,7 +687,7 @@ class FingerDashboardWindow(QMainWindow): def run_dashboard(config: PipelineConfig, *, layout: DashboardLayout = LAYOUT) -> int: """Launch the unified finger dashboard and block until the window closes.""" - from .app import get_or_create_qapp + from gui.app import get_or_create_qapp print("\n按键说明：Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏") print("GUI：Record CSV / Stop CSV 写入接触点与力到所选目录。") diff --git a/examples/gui/layout.py b/examples/gui/viewer3d/layout.py similarity index 88% rename from examples/gui/layout.py rename to examples/gui/viewer3d/layout.py index 1d52e38..1e74181 100644 --- a/examples/gui/layout.py +++ b/examples/gui/viewer3d/layout.py @@ -47,6 +47,7 @@ import math from dataclasses import dataclass from typing import Literal +from gui.log_config import LOG_CONFIG, LogConfig from viz.contact_arrow import ContactArrowStyle, DEFAULT_CONTACT_ARROW_STYLE from viz.flow_arrow import FlowArrowStyle from viz.render_2d import ArrowBackground, ArrowCanvasColors @@ -193,53 +194,6 @@ ARROW_CANVAS_COLORS = ArrowCanvasColors( ) -@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 = "Cascadia Mono" - 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, - 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: - parts = [f"FPS={wall_fps:.1f}", f"sdk={fps:.1f}"] - else: - parts = [f"FPS={fps:.1f}"] - parts.extend( - [ - f"Fn={fnormal:.1f}", - f"|(Fx,Fy)|={shear_mag:.1f}", - f"Fx, Fy=({fshearx:.1f}, {fsheary:.1f})", - ] - ) - 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() - - @dataclass(frozen=True) class DashboardLayout: """Window chrome, chart styling, and panel hierarchy.""" diff --git a/examples/gui/mesh_render_worker.py b/examples/gui/viewer3d/mesh_render_worker.py similarity index 100% rename from examples/gui/mesh_render_worker.py rename to examples/gui/viewer3d/mesh_render_worker.py diff --git a/examples/gui/mesh_view_state.py b/examples/gui/viewer3d/mesh_view_state.py similarity index 100% rename from examples/gui/mesh_view_state.py rename to examples/gui/viewer3d/mesh_view_state.py diff --git a/examples/gui/preferences.py b/examples/gui/viewer3d/preferences.py similarity index 85% rename from examples/gui/preferences.py rename to examples/gui/viewer3d/preferences.py index b363f76..9f3f2e5 100644 --- a/examples/gui/preferences.py +++ b/examples/gui/viewer3d/preferences.py @@ -1,4 +1,4 @@ -"""Load/save dashboard user preferences near this module.""" +"""Load/save dashboard user preferences for the finger 3D viewer.""" from __future__ import annotations @@ -6,15 +6,16 @@ import json from pathlib import Path from typing import Any -from .app_paths import resource_path +from gui.app_paths import resource_path PREFERENCE_FILE = "preference.json" LEGACY_CAMERA_SELECTION_FILE = "camera_selection.json" -_GUI_DIR = Path(__file__).resolve().parent +# Keep preference.json at examples/gui/ for backward compatibility. +_GUI_DIR = Path(__file__).resolve().parents[1] def preference_path() -> Path: - # Keep preference file beside this module: examples/gui/preference.json + # Keep preference file at examples/gui/preference.json return _GUI_DIR / PREFERENCE_FILE diff --git a/examples/gui/styles.py b/examples/gui/viewer3d/styles.py similarity index 100% rename from examples/gui/styles.py rename to examples/gui/viewer3d/styles.py diff --git a/examples/gui/transport_icons.py b/examples/gui/viewer3d/transport_icons.py similarity index 100% rename from examples/gui/transport_icons.py rename to examples/gui/viewer3d/transport_icons.py diff --git a/examples/gui/viewer3d/widgets/__init__.py b/examples/gui/viewer3d/widgets/__init__.py new file mode 100644 index 0000000..4a58e76 --- /dev/null +++ b/examples/gui/viewer3d/widgets/__init__.py @@ -0,0 +1,15 @@ +"""Finger-specific Qt widgets for the 3D dashboard.""" + +from .arrow_image import ArrowPanelWidget +from .force_charts import ForceChartWidget +from .image_panel import ImagePanel +from .mesh_viewport import MeshViewportWidget +from .settings_panel import SettingsPanelWidget + +__all__ = [ + "ArrowPanelWidget", + "ForceChartWidget", + "ImagePanel", + "MeshViewportWidget", + "SettingsPanelWidget", +] diff --git a/examples/gui/widgets/arrow_image.py b/examples/gui/viewer3d/widgets/arrow_image.py similarity index 96% rename from examples/gui/widgets/arrow_image.py rename to examples/gui/viewer3d/widgets/arrow_image.py index 6885b31..968afd5 100644 --- a/examples/gui/widgets/arrow_image.py +++ b/examples/gui/viewer3d/widgets/arrow_image.py @@ -9,7 +9,7 @@ from viz.render_2d import ArrowBackground, ArrowCanvasColors, render_uv_arrows from ..layout import DashboardLayout, LAYOUT from .image_panel import ImagePanel -from .qt_pixmap import ndarray_to_pixmap +from gui.widgets.qt_pixmap import ndarray_to_pixmap class ArrowPanelWidget(ImagePanel): diff --git a/examples/gui/widgets/force_charts.py b/examples/gui/viewer3d/widgets/force_charts.py similarity index 98% rename from examples/gui/widgets/force_charts.py rename to examples/gui/viewer3d/widgets/force_charts.py index b1d605d..3557355 100644 --- a/examples/gui/widgets/force_charts.py +++ b/examples/gui/viewer3d/widgets/force_charts.py @@ -6,8 +6,8 @@ import numpy as np from PyQt5.QtGui import QFont from PyQt5.QtWidgets import QSizePolicy -from gui.layout import DashboardLayout, LAYOUT -from gui.styles import LOG_FONT_PRIMARY, UI_FONT_PRIMARY +from ..layout import DashboardLayout, LAYOUT +from ..styles import LOG_FONT_PRIMARY, UI_FONT_PRIMARY from pipeline.frame import FingerFrameView from viz.signal_filters import lowpass_filter diff --git a/examples/gui/widgets/image_panel.py b/examples/gui/viewer3d/widgets/image_panel.py similarity index 97% rename from examples/gui/widgets/image_panel.py rename to examples/gui/viewer3d/widgets/image_panel.py index 5102540..56abb84 100644 --- a/examples/gui/widgets/image_panel.py +++ b/examples/gui/viewer3d/widgets/image_panel.py @@ -6,7 +6,7 @@ from PyQt5.QtCore import Qt from PyQt5.QtGui import QPixmap from PyQt5.QtWidgets import QLabel, QSizePolicy, QVBoxLayout, QWidget -from .qt_pixmap import pixmap_keep_aspect +from gui.widgets.qt_pixmap import pixmap_keep_aspect class ImagePanel(QWidget): diff --git a/examples/gui/widgets/mesh_viewport.py b/examples/gui/viewer3d/widgets/mesh_viewport.py similarity index 99% rename from examples/gui/widgets/mesh_viewport.py rename to examples/gui/viewer3d/widgets/mesh_viewport.py index e1fafa5..14672ee 100644 --- a/examples/gui/widgets/mesh_viewport.py +++ b/examples/gui/viewer3d/widgets/mesh_viewport.py @@ -11,7 +11,7 @@ from viz.offscreen_mesh import OffscreenFingerMeshRenderer from ..layout import DashboardLayout, LAYOUT from ..mesh_view_state import MeshViewState from ..mesh_render_worker import MeshRenderWorker -from .qt_pixmap import ndarray_to_pixmap +from gui.widgets.qt_pixmap import ndarray_to_pixmap from .image_panel import ImagePanel _VIEW_ROTATE_SENSITIVITY = 0.35 diff --git a/examples/gui/widgets/settings_panel.py b/examples/gui/viewer3d/widgets/settings_panel.py similarity index 100% rename from examples/gui/widgets/settings_panel.py rename to examples/gui/viewer3d/widgets/settings_panel.py diff --git a/examples/gui/widgets/__init__.py b/examples/gui/widgets/__init__.py index 6ab2fa6..ca4dad6 100644 --- a/examples/gui/widgets/__init__.py +++ b/examples/gui/widgets/__init__.py @@ -1,19 +1,9 @@ -"""Qt widgets for the finger dashboard.""" +"""Shared Qt widgets used by 2D and 3D viewers.""" -from .settings_panel import SettingsPanelWidget -from .force_charts import ForceChartWidget from .log_panel import TeeStream, WidgetLogHandler, make_log_widget -from .mesh_viewport import MeshViewportWidget -from .arrow_image import ArrowPanelWidget from .qt_pixmap import ndarray_to_pixmap, pixmap_keep_aspect -from .image_panel import ImagePanel __all__ = [ - "ArrowPanelWidget", - "SettingsPanelWidget", - "ForceChartWidget", - "MeshViewportWidget", - "ImagePanel", "TeeStream", "WidgetLogHandler", "make_log_widget", diff --git a/examples/gui/widgets/log_panel.py b/examples/gui/widgets/log_panel.py index 7bbcbb2..6587f35 100644 --- a/examples/gui/widgets/log_panel.py +++ b/examples/gui/widgets/log_panel.py @@ -86,7 +86,7 @@ class WidgetLogHandler: def make_log_widget(config=None) -> QPlainTextEdit: if config is None: - from ..layout import LOG_CONFIG + from ..log_config import LOG_CONFIG config = LOG_CONFIG widget = QPlainTextEdit() diff --git a/scripts/qt_viewer_2.py b/scripts/qt_viewer_2.py index f918a6a..aa34ec9 100644 --- a/scripts/qt_viewer_2.py +++ b/scripts/qt_viewer_2.py @@ -1,794 +1,31 @@ -""" -Orisys SDK 实时监控 — Qt 统一界面 - -将所有可视化和曲线整合到一个窗口中，适合销售演示使用。 -SDK version: 0.3.1 -""" +#!/usr/bin/env python3 +"""Shim for the Orisys 2D Qt sensor monitor (see examples/gui/viewer2d/).""" import os import sys -os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" -import cv2 -def _fix_qt_plugin_path(): - """OpenCV redirects Qt to cv2/qt/plugins; use PyQt5's plugins instead.""" - try: - from PyQt5.QtCore import QLibraryInfo - os.environ["QT_QPA_PLATFORM_PLUGIN_PATH"] = QLibraryInfo.location( - QLibraryInfo.PluginsPath - ) - except Exception: - path = os.environ.get("QT_QPA_PLATFORM_PLUGIN_PATH", "") - if "cv2" in path.replace("\\", "/"): - os.environ.pop("QT_QPA_PLATFORM_PLUGIN_PATH", None) -_fix_qt_plugin_path() -import orisys -import numpy as np -import time -import argparse -from datetime import datetime -from scipy.signal import butter, filtfilt +from pathlib import Path -# Force curve sampling rate (Hz), matches typical sensor loop ~30 FPS. -FORCE_SAMPLE_RATE_HZ = 30.0 -PLOT_BUFFER_POINTS = 120 -PLOT_WINDOW_SEC = PLOT_BUFFER_POINTS / FORCE_SAMPLE_RATE_HZ # 4 s window -# Minimum y-axis span so autoscale does not over-zoom on small fluctuations. -Y_AXIS_MIN_SPAN = 20000 -CURVE_THEME_COLOR = '#00E5FF' -NORMAL_FORCE_CAL_SLOPE = 3.92685554668795e-05 -NORMAL_FORCE_CAL_OFFSET = 0.0217596967278535 +os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0") -from PyQt5.QtWidgets import ( - QMainWindow, QWidget, QHBoxLayout, QVBoxLayout, - QLabel, QApplication, QSplitter, QPlainTextEdit, - QPushButton, QFileDialog, QLineEdit, -) -from PyQt5.QtCore import Qt, QTimer -from PyQt5 import QtGui -from PyQt5.QtGui import QImage, QPixmap, QFont, QIcon +_EXAMPLES_DIR = Path(__file__).resolve().parents[1] / "examples" +if str(_EXAMPLES_DIR) not in sys.path: + sys.path.insert(0, str(_EXAMPLES_DIR)) -try: - import pyqtgraph as pg -except Exception: - print("PyQtGraph is not installed. Please install it using 'pip install pyqt5 pyqtgraph'.") - sys.exit(1) +from gui.viewer2d import run_viewer -# --------------------------------------------------------------------------- -# 工具 -# --------------------------------------------------------------------------- - -def cv2_to_pixmap(img_bgr: np.ndarray) -> QPixmap: - rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) - h, w, ch = rgb.shape - qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888) - return QPixmap.fromImage(qimg) - -def draw_magnitude_map(vfield, threshold=3, colormap=cv2.COLORMAP_JET) -> np.ndarray: - flow_field = vfield - magnitude = np.sqrt(flow_field[:, :, 0] ** 2 + flow_field[:, :, 1] ** 2) - - downsample_factor = 10 - h, w = magnitude.shape - magnitude_downsampled = magnitude[::downsample_factor, ::downsample_factor] - magnitude_smoothed = cv2.resize(magnitude_downsampled, (w, h), interpolation=cv2.INTER_CUBIC) - - magnitude_thresholded = magnitude_smoothed.copy() - magnitude_thresholded[magnitude_smoothed < threshold] = 0 - - mag_min, mag_max = magnitude_thresholded.min(), magnitude_thresholded.max() - if mag_max > mag_min: - magnitude_norm = ((magnitude_thresholded - mag_min) / (mag_max - mag_min) * 255).astype(np.uint8) - else: - magnitude_norm = np.zeros_like(magnitude_thresholded, dtype=np.uint8) - - magnitude_colored = cv2.applyColorMap(magnitude_norm, colormap) - out_img = cv2.resize(magnitude_colored, (400, 400), interpolation=cv2.INTER_CUBIC) - return out_img - - -def lowpass_filter(data, cutoff_hz, sample_rate_hz=FORCE_SAMPLE_RATE_HZ, order=2): - """ - Zero-phase Butterworth low-pass filter for uniformly sampled 1-D data. - - Parameters - ---------- - data : array-like - Input samples acquired at `sample_rate_hz` (default 30 Hz). - cutoff_hz : float - -3 dB cutoff frequency in Hz; use 0 to skip filtering (return raw data). - Otherwise must satisfy 0 < cutoff_hz < sample_rate_hz / 2. - sample_rate_hz : float - Sampling rate of `data` in Hz. - order : int - Butterworth filter order. - - Returns - ------- - np.ndarray - Filtered 1-D array with the same length as `data`. - """ - y = np.asarray(data, dtype=np.float64) - if cutoff_hz == 0: - return y.copy() - if y.size == 0: - return y - - nyquist = 0.5 * sample_rate_hz - if cutoff_hz < 0 or cutoff_hz >= nyquist: - raise ValueError( - f"cutoff_hz must be in (0, {nyquist}) for sample_rate_hz={sample_rate_hz}" - ) - - b, a = butter(order, cutoff_hz / nyquist, btype="low") - padlen = 3 * (max(len(b), len(a)) - 1) - if y.size <= padlen: - return y.copy() - - return filtfilt(b, a, y) - - -def calibrate_normal_force(raw_value): - """Convert raw normal-force reading to Newtons using y = mx + c.""" - return NORMAL_FORCE_CAL_SLOPE * raw_value + NORMAL_FORCE_CAL_OFFSET - - -class TeeStream: - """同时写入多个流""" - def __init__(self, *streams): - self.streams = streams - - def write(self, text): - for s in self.streams: - s.write(text) - - def flush(self): - for s in self.streams: - s.flush() - - -class WidgetLogHandler: - """将 print 输出实时显示到 QPlainTextEdit""" - def __init__(self, widget: QPlainTextEdit): - self.widget = widget - self.buf = "" - - def write(self, text: str): - self.buf += text - while "\n" in self.buf: - line, self.buf = self.buf.split("\n", 1) - if line: - self.widget.appendPlainText(line) - - def flush(self): - if self.buf: - self.widget.appendPlainText(self.buf) - self.buf = "" - -# --------------------------------------------------------------------------- -# 样式表 -# --------------------------------------------------------------------------- - -STYLE = """ -QMainWindow { - background-color: #000000; -} - -/* ---- 图像面板 ---- */ -QLabel#arrowsLabel { - border: 1px solid #262626; - border-radius: 4px; - background-color: #0b0b0b; -} - -/* ---- 顶栏按钮 & 输入 - — */ -QPushButton { - background-color: #1a1a1a; - color: #999999; - border: 1px solid #333333; - border-radius: 4px; - padding: 6px 16px; - font-family: "SimHei"; - font-size: 13px; -} -QPushButton:hover { - background-color: #262626; - color: #cccccc; - border: 1px solid #555555; -} -QPushButton#btnStart { - background-color: #0d2818; - color: #4caf84; - border: 1px solid #1a4d30; -} -QPushButton#btnStart:hover { - background-color: #143d24; - color: #6fd4a0; -} -QPushButton#btnStart:disabled { - background-color: #0f0f0f; - color: #333333; - border: 1px solid #222222; -} -QPushButton#btnStop { - background-color: #2b1010; - color: #e05555; - border: 1px solid #4d1a1a; -} -QPushButton#btnStop:hover { - background-color: #3d1818; - color: #f07070; -} -QPushButton#btnStop:disabled { - background-color: #0f0f0f; - color: #333333; - border: 1px solid #222222; -} -QLineEdit { - background-color: #0d0d0d; - color: #999999; - border: 1px solid #333333; - border-radius: 4px; - padding: 5px 10px; - font-size: 13px; -} - -/* ---- 力曲线容器 ---- */ -GraphicsLayoutWidget { - border: 1px solid #262626; - border-radius: 4px; -} - -/* ---- 日志面板 ---- */ -QPlainTextEdit { - background-color: #0b0b0b; - color: #777777; - border: 1px solid #262626; - border-radius: 4px; - font-family: "Cascadia Code", "Consolas", "Courier New", monospace; - font-size: 13px; - padding: 8px; - selection-background-color: #1a3a4a; - selection-color: #cccccc; -} - -QPlainTextEdit QScrollBar:vertical { - background-color: #0b0b0b; - width: 6px; - margin: 1px; - border-radius: 3px; -} -QPlainTextEdit QScrollBar::handle:vertical { - background-color: #262626; - border-radius: 3px; - min-height: 30px; -} -QPlainTextEdit QScrollBar::handle:vertical:hover { - background-color: #3a3a3a; -} -QPlainTextEdit QScrollBar::add-line:vertical, -QPlainTextEdit QScrollBar::sub-line:vertical { - height: 0px; -} -QPlainTextEdit QScrollBar::add-page:vertical, -QPlainTextEdit QScrollBar::sub-page:vertical { - background: none; -} - -QSplitter::handle { - background-color: #1a1a1a; -} -QSplitter::handle:vertical { - height: 1px; -} -QSplitter::handle:horizontal { - width: 1px; -} -""" - -# --------------------------------------------------------------------------- -# 主窗口 -# --------------------------------------------------------------------------- - -class MainWindow(QMainWindow): - def __init__(self, args, log_file): - super().__init__() - self.setWindowTitle("Orisys SDK") - self.setStyleSheet(STYLE) - - logo_path = os.path.join(os.path.dirname(os.path.dirname(__file__)), "logo.png") - if os.path.exists(logo_path): - self.setWindowIcon(QIcon(logo_path)) - - self.sensor = None - self.start_time = None - self.args = args - - # 录像状态 - self.is_save = False - self.frame_count = 0 - self.writer = None - self.save_path = None - - # 曲线数据缓冲（先填满 npoints 再滚动；120 点 @ 30 Hz = 4 s） - self.npoints = PLOT_BUFFER_POINTS - self.lowpass_cutoff_hz = getattr(args, "cutoff", 5.0) - self._plot_sample_count = 0 - self._init_plot_buffer() - - self._setup_ui() - self._create_plots() - - # 同步参数到 UI - self.camera_edit.setText(args.video) - self.config_edit.setText(args.config) - self.cal_edit.setText(args.cal) - - # stdout → 日志文件 + 界面日志面板 - self.log_handler = WidgetLogHandler(self.log_widget) - sys.stdout = TeeStream(log_file, self.log_handler) - sys.stderr = sys.stdout - - self._y_range_alpha = 0.5 - - self.timer = QTimer() - self.timer.timeout.connect(self._on_timer) - self._running = False - - def _init_plot_buffer(self): - """Empty buffer; x-axis grows until npoints real samples, then scrolls.""" - self.data = np.full((self.npoints, 2, 3), np.nan, dtype=np.float64) - self._plot_sample_count = 0 - self._y_range_smooth = [None, None, None] - - # ---- UI 构建 ---- - - def _setup_ui(self): - central = QWidget() - self.setCentralWidget(central) - root_layout = QVBoxLayout(central) - root_layout.setContentsMargins(6, 6, 6, 6) - root_layout.setSpacing(6) - - # ---- 顶栏 ---- - bar = QHBoxLayout() - bar.setSpacing(8) - - self.btn_camera = QPushButton("摄像机") - self.btn_camera.setObjectName("btnCamera") - self.btn_camera.clicked.connect(self._select_camera) - bar.addWidget(self.btn_camera) - self.camera_edit = QLineEdit("1") - self.camera_edit.setPlaceholderText("摄像头编号 或 视频路径") - bar.addWidget(self.camera_edit, 1) - - self.btn_config = QPushButton("配置") - self.btn_config.setObjectName("btnConfig") - self.btn_config.clicked.connect(self._select_config) - bar.addWidget(self.btn_config) - self.config_edit = QLineEdit("./config/ddjx01.json") - bar.addWidget(self.config_edit, 1) - - self.btn_reset = QPushButton("重置") - self.btn_reset.setObjectName("btnReset") - self.btn_reset.setEnabled(False) - self.btn_reset.clicked.connect(self._on_reset) - bar.addWidget(self.btn_reset) - self.cal_edit = QLineEdit("./config/ddjx01.npy") - bar.addWidget(self.cal_edit, 1) - - self.btn_start = QPushButton("启动") - self.btn_start.setObjectName("btnStart") - self.btn_start.clicked.connect(self._on_start) - bar.addWidget(self.btn_start) - - self.btn_stop = QPushButton("暂停") - self.btn_stop.setObjectName("btnStop") - self.btn_stop.setEnabled(False) - self.btn_stop.clicked.connect(self._on_stop) - bar.addWidget(self.btn_stop) - - root_layout.addLayout(bar) - - # ---- 外层纵向 splitter：主区域 / 日志 ---- - v_splitter = QSplitter(Qt.Vertical) - - # ---- 主区域：图像 (左) | 曲线 (右) ---- - h_splitter = QSplitter(Qt.Horizontal) - - # 左侧图像面板 - left = QWidget() - left_layout = QVBoxLayout(left) - left_layout.setContentsMargins(0, 0, 0, 0) - left_layout.setSpacing(6) - - self.arrows_label = QLabel() - self.arrows_label.setObjectName("arrowsLabel") - self.arrows_label.setMinimumSize(160, 160) - self.arrows_label.setScaledContents(True) - self.arrows_label.setAlignment(Qt.AlignCenter) - left_layout.addWidget(self.arrows_label) - - h_splitter.addWidget(left) - - # 右侧力曲线 - pg.setConfigOption('background', '#000000') - pg.setConfigOption('foreground', '#777777') - pg.setConfigOptions(antialias=True) - self.graph_widget = pg.GraphicsLayoutWidget() - self.graph_widget.setMinimumWidth(300) - h_splitter.addWidget(self.graph_widget) - - self.h_splitter = h_splitter - - v_splitter.addWidget(h_splitter) - - # ---- 底部日志面板 ---- - self.log_widget = QPlainTextEdit() - self.log_widget.setReadOnly(True) - self.log_widget.setMaximumBlockCount(2000) - font = QFont("Consolas", 12) - font.setStyleHint(QFont.Monospace) - self.log_widget.setFont(font) - v_splitter.addWidget(self.log_widget) - - v_splitter.setStretchFactor(0, 4) - v_splitter.setStretchFactor(1, 1) - - root_layout.addWidget(v_splitter) - - def _create_plots(self): - titles = ["法向力 (N)", "切向力 X", "切向力 Y"] - self.plots = [] - self.curves = [] - self.dot_glows = [] - self.dot_cores = [] - - axis_pen = pg.mkPen(color=(38, 38, 38), width=1) # #262626 - label_font = QFont("SimHei", 9) - - for i, title in enumerate(titles): - p = self.graph_widget.addPlot(title=None) - - # 标题：黑体 - p.setTitle(title, color='#999999', size='14pt') - p.titleLabel.setFont(QFont("SimHei", 14)) - - # X 轴标签 - p.setLabel('bottom', '时间', units='s') - p.getAxis('bottom').label.setFont(label_font) - - # 仅保留左 + 下轴线，隐藏右 + 上边框 - p.showAxis('top', False) - p.showAxis('right', False) - p.getAxis('left').setPen(axis_pen) - p.getAxis('bottom').setPen(axis_pen) - - # 仅横向网格，极淡 - p.showGrid(x=False, y=True, alpha=0.12) - - # 等宽刻度字体 - tick_font = QFont("Consolas", 11) - tick_font.setStyleHint(QFont.Monospace) - p.getAxis('left').setTickFont(tick_font) - p.getAxis('bottom').setTickFont(tick_font) - - # 曲线: 青色描边 + 向下渐变填充（与 demo_wuxi 一致） - line_pen = pg.mkPen(color=CURVE_THEME_COLOR, width=2) - # grad_brush = make_gradient_brush(CURVE_THEME_COLOR, max_alpha=120) - curve = p.plot(pen=line_pen, fillLevel=0) - - # 终点光晕: #00e5ff 30% 透明，半径 8 - dot_glow = p.plot([], [], pen=None, symbol='o', symbolSize=8, - symbolPen=None, symbolBrush=(0, 229, 255, 76)) - # 终点核心: 纯白，半径 4 - dot_core = p.plot([], [], pen=None, symbol='o', symbolSize=4, - symbolPen=None, symbolBrush=(255, 255, 255, 255)) - - p.setXRange(0, PLOT_WINDOW_SEC, padding=0) - p.enableAutoRange(x=False, y=False) - - self.plots.append(p) - self.curves.append(curve) - self.dot_glows.append(dot_glow) - self.dot_cores.append(dot_core) - - if i < 2: - self.graph_widget.nextRow() - - def adjust_square_images(self): - """根据窗口高度调整左侧面板宽度，使左侧图像保持正方形""" - top_bar_h = 48 - log_h = self.log_widget.height() - spacing = 20 - available = self.height() - top_bar_h - log_h - spacing - image_w = max(available, 160) - self.h_splitter.setSizes([image_w, self.width() - image_w]) - - # ---- 帧处理 ---- - - def _on_timer(self): - if self.sensor is None: - return - sensor = self.sensor - - isimg = sensor.get_img() - if isimg is None: - print("Failed to get image") - return - - # 旋转原始图像，使力方向与实际物理方向对齐 - sensor.img = cv2.rotate(sensor.img, cv2.ROTATE_90_COUNTERCLOCKWISE) - if sensor.frame is not None: - sensor.frame = cv2.rotate(sensor.frame, cv2.ROTATE_90_COUNTERCLOCKWISE) - - # 录像写入 - if self.is_save and self.writer is not None and sensor.frame is not None: - self.frame_count += 1 - save_img = sensor.frame - if len(save_img.shape) == 2 or (len(save_img.shape) == 3 and save_img.shape[2] == 1): - if len(save_img.shape) == 3: - save_img = save_img.squeeze(2) - save_img = cv2.cvtColor(save_img, cv2.COLOR_GRAY2BGR) - self.writer.write(save_img) - - is_touched = sensor.compute_deformation(check_motion=True, threshold=1) - sensor.compute_contact() - - fps, fn_raw, fx, fy, flow, vnormal, vshear, _ = sensor.read_info( - sensor.info.FPS, sensor.info.FNORMAL, - sensor.info.FSHEARX, sensor.info.FSHEARY, - sensor.info.VRAW, sensor.info.VNORMAL, - sensor.info.VSHEAR, - sensor.info.DEPTH - ) - fn = calibrate_normal_force(fn_raw) - - - print(f"FPS={fps:.2f}, 法向力={fn:.4f}, 切向力X={fx:.4f}, 切向力Y={fy:.4f}") - # print(f"CUPY Available: {sensor.CUPY_AVAILABLE}") - - # 图像 - _bg_bgr = (22, 16, 11) - _img = sensor.img - _h, _w = _img.shape[:2] - black_img = np.empty((_h, _w, 3), dtype=_img.dtype) - black_img[:, :, 0] = _bg_bgr[0] - black_img[:, :, 1] = _bg_bgr[1] - black_img[:, :, 2] = _bg_bgr[2] - - arrows = orisys.util.draw_arrows( - black_img, flow, - threshold=2, grid_spacing=20, - arrow_scale=0.5, - below_threshold_color=(160, 160, 160), - ) - arrows = cv2.resize(arrows, (400, 400), interpolation=cv2.INTER_CUBIC) - self.arrows_label.setPixmap(cv2_to_pixmap(arrows)) - - # 曲线：先顺序写入 npoints 个真实样本，满后再左移滚动 - current_time = time.time() - self.start_time - if self._plot_sample_count < self.npoints: - idx = self._plot_sample_count - self.data[idx, 0, :] = current_time - self.data[idx, 1, 0] = fn - self.data[idx, 1, 1] = fx - self.data[idx, 1, 2] = fy - self._plot_sample_count += 1 - else: - for i in range(3): - self.data[:-1, 0, i] = self.data[1:, 0, i] - self.data[:-1, 1, i] = self.data[1:, 1, i] - self.data[-1, 0, :] = current_time - self.data[-1, 1, 0] = fn - self.data[-1, 1, 1] = fx - self.data[-1, 1, 2] = fy - - n = self._plot_sample_count - if n == 0: - return - - data_plot = self.data[:n].copy() - for i in range(3): - data_plot[:, 1, i] = lowpass_filter( - data_plot[:, 1, i], - self.lowpass_cutoff_hz, - sample_rate_hz=FORCE_SAMPLE_RATE_HZ, - ) - - for i, curve in enumerate(self.curves): - x = data_plot[:, 0, i] - y = data_plot[:, 1, i] - curve.setData(x, y) - ymin, ymax = y.min(), y.max() - if self._y_range_smooth[i] is None: - self._y_range_smooth[i] = (ymin, ymax) - else: - prev_min, prev_max = self._y_range_smooth[i] - alpha = self._y_range_alpha - ymin = alpha * ymin + (1 - alpha) * prev_min - ymax = alpha * ymax + (1 - alpha) * prev_max - span = ymax - ymin - if span < Y_AXIS_MIN_SPAN: - mid = (ymin + ymax) / 2 - half = Y_AXIS_MIN_SPAN / 2 - ymin = mid - half - ymax = mid + half - self._y_range_smooth[i] = (ymin, ymax) - pad = max((ymax - ymin) * 0.12, 0.01) - self.plots[i].setYRange(ymin - pad, ymax + pad) - last_x = x[-1:] - last_y = y[-1:] - self.dot_glows[i].setData(last_x, last_y) - self.dot_cores[i].setData(last_x, last_y) - - if n < self.npoints: - for p in self.plots: - p.setXRange(0, PLOT_WINDOW_SEC, padding=0) - else: - x0 = float(data_plot[0, 0, 0]) - x1 = float(data_plot[-1, 0, 0]) - if x1 <= x0: - x1 = x0 + PLOT_WINDOW_SEC - for p in self.plots: - p.setXRange(x0, x1, padding=0) - - # ---- 键盘 & 录像 ---- - - def _on_start(self): - if self._running: - return - self.btn_start.setText("启动中...") - self.btn_start.setEnabled(False) - QApplication.processEvents() - - try: - src = self.camera_edit.text() - cfg = self.config_edit.text() - cal = self.cal_edit.text() - verbose = getattr(self.args, 'verbose', True) - try: - src = int(src) - except ValueError: - pass - self.sensor = orisys.Sensor(src, config_name=cfg, cal_path=cal, verbose=verbose) - except Exception as e: - self.btn_start.setText("启动") - self.btn_start.setEnabled(True) - print(f"启动失败: {e}") - return - - self.start_time = time.time() - self._init_plot_buffer() - self._running = True - self.timer.start(10) - self.btn_start.setText("启动") - self.btn_stop.setEnabled(True) - self.btn_reset.setEnabled(True) - print(f"▶ 启动 — 摄像机={src}, 配置={cfg}, 标定={cal}") - - def _on_stop(self): - if not self._running: - return - self._running = False - self.timer.stop() - self.btn_stop.setText("停止中...") - self.btn_stop.setEnabled(False) - QApplication.processEvents() - - if self.sensor is not None: - self.sensor.disconnect() - self.sensor = None - - self.btn_start.setEnabled(True) - self.btn_stop.setText("暂停") - self.btn_reset.setEnabled(False) - print("⏸ 已暂停") - - def _select_camera(self): - text = self.camera_edit.text() - path, _ = QFileDialog.getOpenFileName( - self, "选择视频文件", "data", "视频 (*.mp4 *.avi *.mov);;所有文件 (*)", - ) - if path: - self.camera_edit.setText(path) - - def _select_config(self): - path, _ = QFileDialog.getOpenFileName( - self, "选择配置文件", "config", "JSON (*.json);;所有文件 (*)", - ) - if path: - self.config_edit.setText(path) - - def _on_reset(self): - """Reset optical flow tracker when drift occurs (same as basic_pipeline 'r' key).""" - if self.sensor is None or not self._running: - print("请先启动传感器后再重置追踪器。") - return - self.sensor.reset() - print("追踪器已重置。") - - def keyPressEvent(self, event): - key = event.key() - if key == Qt.Key_Escape: - if self.isFullScreen(): - self.showNormal() - else: - self.showFullScreen() - elif key == Qt.Key_Q: - print("\n正在退出示例程序...") - self.close() - elif key == Qt.Key_S: - self._start_recording() - elif key == Qt.Key_E: - self._stop_recording() - elif key == Qt.Key_R: - self._on_reset() - else: - super().keyPressEvent(event) - - def _start_recording(self): - if self.sensor is None: - print("请先启动采集") - return - self.save_path = "./data/pulse_test.mp4" - os.makedirs(os.path.dirname(self.save_path), exist_ok=True) - - save_img = self.sensor.frame - if save_img is None: - print("当前没有可用帧，无法确定录像尺寸。") - return - - temp_img = save_img.copy() - if len(temp_img.shape) == 2 or (len(temp_img.shape) == 3 and temp_img.shape[2] == 1): - if len(temp_img.shape) == 3: - temp_img = temp_img.squeeze(2) - temp_img = cv2.cvtColor(temp_img, cv2.COLOR_GRAY2BGR) - - fh, fw = temp_img.shape[:2] - self.writer = cv2.VideoWriter( - self.save_path, cv2.VideoWriter_fourcc(*'mp4v'), 30, (fw, fh), - ) - if self.writer.isOpened(): - self.is_save = True - print(f"开始录像：{self.save_path}") - else: - print(f"无法创建录像文件：{self.save_path}") - self.writer = None - - def _stop_recording(self): - self.is_save = False - if self.writer is not None: - self.writer.release() - self.writer = None - if self.save_path is not None: - print(f"录像结束：{self.save_path}，总帧数：{self.frame_count}") - else: - print(f"录像结束，总帧数：{self.frame_count}") - self.frame_count = 0 - - def closeEvent(self, event): - self.timer.stop() - if self.writer is not None: - self.writer.release() - if self.sensor is not None: - self.sensor.disconnect() - event.accept() - -# --------------------------------------------------------------------------- -# 入口 -# --------------------------------------------------------------------------- def main(): - script_dir = os.path.dirname(os.path.abspath(__file__)) - log_dir = os.path.join(script_dir, "logs") - os.makedirs(log_dir, exist_ok=True) - log_path = os.path.join(log_dir, f"qt_viewer_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log") - log_file = open(log_path, 'w', encoding='utf-8') - # stdout 到文件的临时重定向（创建 MainWindow 后会加上日志面板） - sys.stdout = log_file - sys.stderr = sys.stdout + import argparse - parser = argparse.ArgumentParser() + parser = argparse.ArgumentParser(description="Orisys 2D Qt sensor monitor") + parser.add_argument( + "--sensor-type", + "-s", + type=str, + default="orisys", + choices=("orisys", "dd06"), + help="Sensor adapter: orisys (direct Sensor) or dd06 (polygon ROI rectify)", + ) parser.add_argument("--video", "-v", type=str, default="0") parser.add_argument("--config", "-c", type=str, default="./config/ddjx01.json") parser.add_argument("--cal", "-cal", type=str, default="./config/ddjx01.npy") @@ -801,18 +38,15 @@ def main(): ) args = parser.parse_args() - print("\n按键说明：Q 退出 | R 重置追踪器 | S 开始录像 | E 结束录像 | ESC 全屏") + # Sensible DD06 defaults when user only switches sensor type. + if args.sensor_type == "dd06": + if args.video in ("0", "1") and args.config.endswith("ddjx01.json"): + args.video = "./data/dd06/0726.mp4" + args.config = "./config/dd06-ov.json" - # 高 DPI 适配（笔记本高分屏） - QApplication.setAttribute(Qt.AA_EnableHighDpiScaling, True) - QApplication.setAttribute(Qt.AA_UseHighDpiPixmaps, True) - app = QApplication(sys.argv) - window = MainWindow(args, log_file) - window.showFullScreen() - QApplication.processEvents() - window.adjust_square_images() + log_dir = Path(__file__).resolve().parent / "logs" + sys.exit(run_viewer(args, log_dir=log_dir)) - sys.exit(app.exec_()) -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/tests/dd06.py b/tests/dd06.py new file mode 100644 index 0000000..574706e --- /dev/null +++ b/tests/dd06.py @@ -0,0 +1,175 @@ +""" +DD06 single-camera pipeline with polygon ROI rectification. + +Flow: +1. Load sensor config + nested polygon ROI +2. Capture raw frames from the video/camera source +3. Perspective-warp the ROI into a rectangle sized to camera.resolution_stitch +4. Feed the rectified frame into Sensor.get_img(..., already_processed=True) +5. Compute deformation/contact and visualize + +SDK version: 0.3.1 +""" +from __future__ import annotations + +import argparse +import os +import sys +from pathlib import Path + +os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" + +import cv2 +import numpy as np +import orisys + +_REPO_ROOT = Path(__file__).resolve().parents[1] +_EXAMPLES_DIR = _REPO_ROOT / "examples" +if str(_EXAMPLES_DIR) not in sys.path: + sys.path.insert(0, str(_EXAMPLES_DIR)) + +from gui.sensors.roi_geometry import ( # noqa: E402 + build_roi_warp, + load_json_config, + load_roi_polygon, + open_video_source, + rectified_output_size, + warp_roi_to_rectangle, +) + + +def main(): + parser = argparse.ArgumentParser( + description="DD06 pipeline: polygon-ROI rectify -> Sensor.get_img(already_processed=True)" + ) + parser.add_argument( + "--video", + "-v", + type=str, + default="./data/dd06/0726.mp4", + help="视频源：摄像头编号或视频文件路径", + ) + parser.add_argument( + "--config", + "-c", + type=str, + default="./config/dd06-ov.json", + help="配置文件名称或路径", + ) + parser.add_argument( + "--verbose", + "-verbose", + action="store_true", + default=True, + help="是否输出详细日志", + ) + args = parser.parse_args() + + config = load_json_config(args.config) + roi_points, roi_image_size = load_roi_polygon(config) + out_w, out_h = rectified_output_size(config, roi_points) + + sensor = orisys.Sensor("external", config_name=args.config, verbose=args.verbose) + cap, source = open_video_source(args.video) + + warp_matrix = None + warp_src = None + last_frame_size = None + + print( + f"ROI points={len(roi_points)} source_size={roi_image_size} " + f"output={out_w}x{out_h} video={source}" + ) + print("按键说明：'q' 退出程序，'r' 重置追踪器。") + + try: + while True: + ok, frame = cap.read() + if not ok or frame is None: + print("Failed to read frame from video source.") + break + + frame_h, frame_w = frame.shape[:2] + frame_size = (frame_w, frame_h) + if warp_matrix is None or last_frame_size != frame_size: + warp_matrix, warp_src, _ = build_roi_warp( + roi_points, + roi_image_size, + frame_size, + (out_w, out_h), + ) + last_frame_size = frame_size + if args.verbose: + print(f"Built ROI warp for frame {frame_w}x{frame_h}:\n{warp_src}") + + rectified = warp_roi_to_rectangle(frame, warp_matrix, (out_w, out_h)) + sensor.get_img(frame=rectified, already_processed=True) + + is_contact_now = sensor.compute_deformation(check_motion=True, threshold=0) + _ = is_contact_now + sensor.compute_contact() + + fps, flow, vnormal, img, contour, centroid, depth_map, fnormal, fshearx, fsheary = ( + sensor.read_info( + sensor.info.FPS, + sensor.info.VRAW, + sensor.info.VNORMAL, + sensor.info.IMG, + sensor.info.CONTOUR, + sensor.info.CENTROID, + sensor.info.DEPTH, + sensor.info.FNORMAL, + sensor.info.FSHEARX, + sensor.info.FSHEARY, + ) + ) + + preview = frame.copy() + cv2.polylines( + preview, + [np.rint(warp_src).astype(np.int32).reshape(-1, 1, 2)], + True, + (0, 255, 255), + 2, + cv2.LINE_AA, + ) + + arrows = orisys.util.draw_arrows( + img, + flow, + threshold=1, + grid_spacing=20, + arrow_scale=1.0, + ) + cv2.imshow("形变矢量场", arrows) + + div_abs = depth_map + if div_abs.max() > div_abs.min(): + div_normalized = ( + (div_abs - div_abs.min()) / (div_abs.max() - div_abs.min()) * 255 + ).astype(np.uint8) + else: + div_normalized = np.zeros_like(div_abs, dtype=np.uint8) + cv2.imshow("depth", cv2.applyColorMap(div_normalized, cv2.COLORMAP_JET)) + + print( + f"FPS={fps:.2f}, 法向力={fnormal:.4f}, 切向力X={fshearx:.4f}, " + f"切向力Y={fsheary:.4f}, 深度图尺寸={depth_map.shape}, " + f"原始分辨率={sensor.img_size_raw}, 矫正={rectified.shape[1]}x{rectified.shape[0]}" + ) + + key = cv2.waitKey(1) & 0xFF + if key == ord("q"): + print("\n正在退出示例程序...") + break + if key == ord("r"): + sensor.reset() + print("追踪器已重置。") + finally: + cap.release() + sensor.disconnect() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main() diff --git a/tests/test_qt_viewer_backend.py b/tests/test_qt_viewer_backend.py new file mode 100644 index 0000000..df101d5 --- /dev/null +++ b/tests/test_qt_viewer_backend.py @@ -0,0 +1,165 @@ +"""Tests for Qt viewer backend force history and aspect-aware display helpers.""" + +from __future__ import annotations + +import sys +import types +from pathlib import Path +from unittest.mock import MagicMock + +import numpy as np +import pytest + +_REPO_ROOT = Path(__file__).resolve().parents[1] +_EXAMPLES_DIR = _REPO_ROOT / "examples" +if str(_EXAMPLES_DIR) not in sys.path: + sys.path.insert(0, str(_EXAMPLES_DIR)) + +from gui.viewer2d.backend import ( # noqa: E402 + QtViewerBackend, + image_shape_meta, + panel_width_for_aspect, + scale_bgr_to_fit, + select_display_image, +) +from gui.sensors import ( # noqa: E402 + FrameSnapshot, + _snapshot_from_sensor, + calibrate_normal_force, +) + + +class FakeForceAdapter: + """Minimal adapter that emits changing FN/FX/FY without a real sensor.""" + + def __init__(self): + self._i = 0 + + def read(self): + i = self._i + self._i += 1 + return FrameSnapshot( + image=np.zeros((100, 200, 3), dtype=np.uint8), + normal_force=1000.0 + 50.0 * i, + shear_x=-200.0 + 10.0 * i, + shear_y=300.0 - 5.0 * i, + normal_force_calibrated=calibrate_normal_force(1000.0 + 50.0 * i), + fps=30.0, + timestamp=float(i) / 30.0, + flow_overlay=np.zeros((100, 200, 3), dtype=np.uint8), + record_frame=np.zeros((100, 200, 3), dtype=np.uint8), + ) + + +def test_backend_force_history_tracks_all_channels(): + adapter = FakeForceAdapter() + backend = QtViewerBackend(npoints=32, lowpass_cutoff_hz=0.0) + + for _ in range(8): + snap = adapter.read() + backend.ingest(snap, now=snap.timestamp) + + state = backend.plot_state() + assert state is not None + assert state.sample_count == 8 + assert state.values.shape == (8, 3) + + # All three channels must change across frames (not stay flat). + fn_span = float(state.values[:, 0].max() - state.values[:, 0].min()) + fx_span = float(state.values[:, 1].max() - state.values[:, 1].min()) + fy_span = float(state.values[:, 2].max() - state.values[:, 2].min()) + assert fn_span > 100.0 + assert fx_span > 20.0 + assert fy_span > 10.0 + + # Raw FN is used for curves, not the tiny calibrated Newton scale. + assert float(state.values[-1, 0]) == pytest.approx(1000.0 + 50.0 * 7) + + +@pytest.mark.parametrize( + "shape,expected_aspect", + [ + ((100, 200, 3), 2.0), # wide + ((120, 120, 3), 1.0), # square + ((200, 100, 3), 0.5), # portrait + ], +) +def test_backend_image_aspect_metadata(shape, expected_aspect): + img = np.zeros(shape, dtype=np.uint8) + h, w, aspect = image_shape_meta(img) + assert (h, w) == (shape[0], shape[1]) + assert aspect == pytest.approx(expected_aspect) + + snap = FrameSnapshot(flow_overlay=img, normal_force=1.0, shear_x=2.0, shear_y=3.0) + backend = QtViewerBackend(lowpass_cutoff_hz=0.0) + frame = backend.ingest(snap, now=0.0) + assert frame.image_height == shape[0] + assert frame.image_width == shape[1] + assert frame.aspect_ratio == pytest.approx(expected_aspect) + + fitted = scale_bgr_to_fit(img, max_w=80, max_h=80) + fh, fw = fitted.shape[:2] + assert fw <= 80 and fh <= 80 + assert abs((fw / fh) - expected_aspect) < 0.05 + + panel_w = panel_width_for_aspect(200, expected_aspect, min_size=160) + assert panel_w >= 160 + assert panel_w == pytest.approx(max(160, int(round(200 * expected_aspect)))) + + +def test_select_display_image_prefers_flow_overlay(): + overlay = np.full((10, 20, 3), 7, dtype=np.uint8) + processed = np.full((10, 20, 3), 3, dtype=np.uint8) + snap = FrameSnapshot(flow_overlay=overlay, processed_image=processed, image=processed) + out = select_display_image(snap) + assert np.array_equal(out, overlay) + + +def test_snapshot_requests_hhd_every_frame_without_motion_gate(): + """Regression: force refresh must not use check_motion=True gating.""" + sensor = MagicMock() + sensor.img = np.zeros((32, 32), dtype=np.uint8) + sensor.frame = np.zeros((32, 32), dtype=np.uint8) + sensor.info = types.SimpleNamespace( + FPS="FPS", + VRAW="VRAW", + VNORMAL="VNORMAL", + IMG="IMG", + CONTOUR="CONTOUR", + CENTROID="CENTROID", + DEPTH="DEPTH", + FNORMAL="FNORMAL", + FSHEARX="FSHEARX", + FSHEARY="FSHEARY", + ) + sensor.read_info.return_value = ( + 30.0, + np.zeros((32, 32, 2), dtype=np.float32), + np.zeros((32, 32), dtype=np.float32), + sensor.img, + None, + None, + np.zeros((32, 32), dtype=np.float32), + 12345.0, + -100.0, + 50.0, + ) + + # Avoid depending on orisys.util.draw_arrows implementation details. + import gui.sensors.adapter as sa + + original_util = sa.orisys.util + sa.orisys.util = types.SimpleNamespace( + draw_arrows=lambda *args, **kwargs: np.zeros((32, 32, 3), dtype=np.uint8) + ) + try: + snap = _snapshot_from_sensor(sensor, raw_image=sensor.frame, processed_image=sensor.img) + finally: + sa.orisys.util = original_util + + sensor.compute_deformation.assert_called_once_with(decompose=True, check_motion=False) + assert snap is not None + assert snap.normal_force == pytest.approx(12345.0) + assert snap.shear_x == pytest.approx(-100.0) + assert snap.shear_y == pytest.approx(50.0) + assert snap.normal_force_calibrated == pytest.approx(calibrate_normal_force(12345.0)) diff --git a/tools/ffc/roi_polygon_tool.py b/tools/ffc/roi_polygon_tool.py index a2f9439..d6bccfd 100644 --- a/tools/ffc/roi_polygon_tool.py +++ b/tools/ffc/roi_polygon_tool.py @@ -1,6 +1,8 @@ from __future__ import annotations +import argparse import json +import os from datetime import datetime from pathlib import Path @@ -8,7 +10,8 @@ import cv2 import numpy as np -WINDOW_NAME = "ROI Polygon Editor" +WINDOW_NAME = "ROI Editor" +POLYGON_WINDOW_NAME = "ROI Polygon Editor" def to_uint8_image(image): @@ -49,33 +52,6 @@ def polygon_to_mask(points, image_size): return mask -def save_polygon_roi_config(path, points, image_size, mask_path=None, metadata=None): - path = Path(path) - path.parent.mkdir(parents=True, exist_ok=True) - data = { - "version": "1.0", - "type": "polygon", - "image_size": [int(image_size[0]), int(image_size[1])], - "points": [[int(x), int(y)] for x, y in points], - "created_at": datetime.now().isoformat(timespec="seconds"), - } - if mask_path is not None: - data["mask_path"] = str(mask_path) - if metadata: - data["metadata"] = metadata - with path.open("w", encoding="utf-8") as f: - json.dump(data, f, indent=2, ensure_ascii=False) - return data - - -def load_roi_config(path): - path = Path(path) - with path.open("r", encoding="utf-8") as f: - data = json.load(f) - data["_config_path"] = str(path) - return data - - def scale_points(points, source_size, target_size): source_w, source_h = int(source_size[0]), int(source_size[1]) target_w, target_h = int(target_size[0]), int(target_size[1]) @@ -97,20 +73,96 @@ def normalize_mask(mask, shape): return np.where(mask > 0, 255, 0).astype(np.uint8) +def build_roi_payload(image_size, points, mask_path=None, metadata=None): + """Build the shared ``roi`` object written into sensor / FFC JSON configs.""" + points = np.asarray(points, dtype=np.int32).reshape(-1, 2) + if len(points) < 3: + raise ValueError("polygon ROI requires at least 3 points") + payload = { + "type": "polygon", + "image_size": [int(image_size[0]), int(image_size[1])], + "points": [[int(x), int(y)] for x, y in points], + "created_at": datetime.now().isoformat(timespec="seconds"), + } + + if mask_path is not None: + payload["mask_path"] = str(mask_path) + if metadata: + payload["metadata"] = metadata + return payload + + +def extract_roi_section(data): + """Return the ROI dict from either a nested ``roi`` field or a legacy top-level ROI file.""" + if not isinstance(data, dict): + raise TypeError("ROI config must be a JSON object") + if isinstance(data.get("roi"), dict): + return data["roi"] + return data + + +def load_roi_config(path): + path = Path(path) + with path.open("r", encoding="utf-8") as f: + data = json.load(f) + data["_config_path"] = str(path) + return data + + +def save_polygon_roi_config(path, points, image_size, mask_path=None, metadata=None): + """Backward-compatible standalone polygon ROI JSON writer.""" + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + data = build_roi_payload( + image_size, + points, + mask_path=mask_path, + metadata=metadata, + ) + data["version"] = "1.0" + with path.open("w", encoding="utf-8") as f: + json.dump(data, f, indent=2, ensure_ascii=False) + return data + + +def upsert_roi_in_config(path, roi_payload, *, create_if_missing=True): + """Read-modify-write ``roi`` into an existing sensor config, preserving other fields.""" + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + if path.exists(): + with path.open("r", encoding="utf-8") as f: + data = json.load(f) + if not isinstance(data, dict): + raise ValueError(f"Config must be a JSON object: {path}") + elif create_if_missing: + data = {"version": "1.0"} + else: + raise FileNotFoundError(f"Config not found: {path}") + + data["roi"] = roi_payload + with path.open("w", encoding="utf-8") as f: + json.dump(data, f, indent=2, ensure_ascii=False) + f.write("\n") + return data + + def load_roi_mask(config_path, frame_shape): config_path = Path(config_path) data = load_roi_config(config_path) + roi = extract_roi_section(data) height, width = frame_shape[:2] - points = data.get("points") or data.get("polygon") or data.get("roi_polygon") + target_size = (width, height) + source_size = roi.get("image_size") or data.get("image_size") or [width, height] + points = roi.get("points") + points = roi.get("points") or roi.get("polygon") or roi.get("roi_polygon") if points: - source_size = data.get("image_size", [width, height]) - polygon = scale_points(points, source_size, (width, height)).reshape(-1, 1, 2) + polygon = scale_points(points, source_size, target_size).reshape(-1, 1, 2) output = np.zeros((height, width), dtype=np.uint8) cv2.fillPoly(output, [polygon], 255) return output - mask_path = data.get("mask_path") or data.get("roi_mask") + mask_path = roi.get("mask_path") or data.get("mask_path") or data.get("roi_mask") if mask_path: mask_path = Path(mask_path) if not mask_path.is_absolute(): @@ -123,6 +175,54 @@ def load_roi_mask(config_path, frame_shape): raise ValueError(f"Unsupported ROI config: {config_path}") +def resolve_video_source(video): + """Accept camera index (int/str digit) or filesystem path.""" + if isinstance(video, int): + return video + text = str(video).strip() + if text.isdigit(): + return int(text) + return text + + +def open_video_capture(video): + source = resolve_video_source(video) + # Avoid MSMF HW transform stalls on Windows camera capture. + os.environ.setdefault("OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS", "0") + if isinstance(source, int): + cap = cv2.VideoCapture(source, cv2.CAP_DSHOW) + if not cap.isOpened(): + cap.release() + cap = cv2.VideoCapture(source) + else: + path = Path(source) + if not path.exists(): + raise FileNotFoundError(f"Video file not found: {path}") + cap = cv2.VideoCapture(str(path)) + if not cap.isOpened(): + raise RuntimeError(f"Failed to open video source: {video}") + return cap + + +def capture_frame(cap, *, max_attempts=60): + """Grab the next readable frame from an opened capture.""" + last_error = None + for _ in range(max_attempts): + ok, frame = cap.read() + if ok and frame is not None and getattr(frame, "size", 0) > 0: + return normalize_editor_frame(frame) + last_error = "empty frame" if ok else "read failed" + raise RuntimeError(f"Failed to capture frame from video source ({last_error})") + + +def capture_frame_from_source(video): + cap = open_video_capture(video) + try: + return capture_frame(cap) + finally: + cap.release() + + class RoiPolygonEditor: def __init__(self, frame): self.frame = normalize_editor_frame(frame) @@ -187,25 +287,85 @@ class RoiPolygonEditor: return view -def build_roi_result(points, image_size, mask, config=None, config_path=None, mask_path=None): - return { - "points": [[int(x), int(y)] for x, y in points], +def build_roi_result( + points, + image_size, + mask, + config=None, + config_path=None, + mask_path=None, +): + result = { + "type": "polygon", + "points": [[int(x), int(y)] for x, y in points] if points is not None else None, "image_size": [int(image_size[0]), int(image_size[1])], "mask": mask, "config": config, "config_path": str(config_path) if config_path is not None else None, "mask_path": str(mask_path) if mask_path is not None else None, } + return result + + +def _save_roi_outputs( + *, + image_size, + output, + points, + metadata=None, + save_mask=True, + nest_in_config=True, +): + mask = polygon_to_mask(points, image_size) + config = None + config_path = None + mask_path = None + + if output is not None: + config_path = Path(output) + config_path.parent.mkdir(parents=True, exist_ok=True) + saved_mask = None + if save_mask: + mask_path = config_path.with_name(config_path.stem + "_mask.png") + cv2.imwrite(str(mask_path), mask) + saved_mask = mask_path.name + + roi_payload = build_roi_payload( + image_size, + points, + mask_path=saved_mask, + metadata=metadata, + ) + if nest_in_config: + config = upsert_roi_in_config(config_path, roi_payload) + else: + # Standalone ROI file (legacy FFC polygon style). + standalone = dict(roi_payload) + standalone["version"] = "1.0" + with config_path.open("w", encoding="utf-8") as f: + json.dump(standalone, f, indent=2, ensure_ascii=False) + f.write("\n") + config = standalone + + return build_roi_result( + points, + image_size, + mask, + config=config, + config_path=config_path, + mask_path=mask_path, + ) def select_polygon_roi( frame, output=None, - window_name=WINDOW_NAME, + window_name=POLYGON_WINDOW_NAME, metadata=None, save_mask=True, destroy_window=True, recapture_callback=None, + nest_in_config=False, ): """Open an OpenCV window, draw a polygon ROI, and return points/mask/config info.""" editor = RoiPolygonEditor(frame) @@ -230,35 +390,95 @@ def select_polygon_roi( continue image_size = (editor.frame.shape[1], editor.frame.shape[0]) - mask = polygon_to_mask(editor.points, image_size) - config = None - config_path = None - mask_path = None - - if output is not None: - config_path = Path(output) - config_path.parent.mkdir(parents=True, exist_ok=True) - saved_mask = None - if save_mask: - mask_path = config_path.with_name(config_path.stem + "_mask.png") - cv2.imwrite(str(mask_path), mask) - saved_mask = mask_path.name - config = save_polygon_roi_config( - config_path, - editor.points, - image_size, - mask_path=saved_mask, - metadata=metadata, - ) - - return build_roi_result( - editor.points, - image_size, - mask, - config=config, - config_path=config_path, - mask_path=mask_path, + return _save_roi_outputs( + image_size=image_size, + output=output, + points=editor.points, + metadata=metadata, + save_mask=save_mask, + nest_in_config=nest_in_config, ) finally: if destroy_window: cv2.destroyWindow(window_name) + + +select_roi = select_polygon_roi + + +def parse_args(argv=None): + parser = argparse.ArgumentParser( + description="Capture a frame from a camera/video and save a polygon ROI to JSON." + ) + parser.add_argument( + "--video", + "-v", + required=True, + help="Camera index (e.g. 0) or path to a video file", + ) + parser.add_argument( + "--config", + "-c", + required=True, + help="Output JSON config path (existing sensor configs are updated in place)", + ) + parser.add_argument( + "--frame-output", + default=None, + help="Optional path to save the captured reference frame (PNG/JPG)", + ) + parser.add_argument( + "--no-mask", + action="store_true", + help="Do not write a companion *_mask.png next to the config", + ) + return parser.parse_args(argv) + + +def main(argv=None): + args = parse_args(argv) + video = resolve_video_source(args.video) + cap = open_video_capture(video) + + def recapture(): + # For file sources, rewind so R always gets a usable frame. + if not isinstance(video, int): + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + return capture_frame(cap) + + try: + frame = capture_frame(cap) + if args.frame_output: + frame_path = Path(args.frame_output) + frame_path.parent.mkdir(parents=True, exist_ok=True) + cv2.imwrite(str(frame_path), frame) + print(f"Saved reference frame: {frame_path}") + + result = select_polygon_roi( + frame, + output=args.config, + metadata={"source": str(args.video)}, + save_mask=not args.no_mask, + recapture_callback=recapture, + nest_in_config=True, + ) + finally: + cap.release() + cv2.destroyAllWindows() + + if result is None: + print("ROI capture cancelled.") + return 1 + + print(f"Saved ROI type={result['type']} image_size={result['image_size']}") + if result.get("points") is not None: + print(f"points={result['points']}") + if result.get("config_path"): + print(f"config={result['config_path']}") + if result.get("mask_path"): + print(f"mask={result['mask_path']}") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main())