import os os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" import cv2 import orisys import numpy as np import time import argparse from datetime import datetime import pyqtgraph as pg from PyQt5.QtCore import Qt, QSize, QEvent from PyQt5.QtGui import QImage, QFont, QPixmap, QKeySequence, QCursor from PyQt5.QtWidgets import QLabel, QVBoxLayout, QWidget, QSizePolicy, QShortcut, QApplication from pyqtgraph.Qt import QtGui # Window aspect: width : total height = 936 : (936 + plot strip) ASPECT_W = 936 ASPECT_H_TOTAL = 936 + 624 ARROW_STRETCH = 936 PLOT_STRETCH = 624 npoints = 120 # 图表数据长度 dispaly_angle = -90 # 箭头图显示角度 touch_threshold = 5 # 触碰反应阈值 frame_to_recover = 10 # 触碰后等待回复帧数 # Match arrow panel / Qt style (not pure black) PLOT_THEME_BG = "#0B1016" PLOT_THEME_FG = "#A0A0A0" # OS title bar cannot be tinted to #0B1016; frameless removes it (use Alt+drag to move). USE_FRAMELESS_WINDOW = True class CombinedTactileWindow(QWidget): """ Resizable window that keeps width:height = ASPECT_W : ASPECT_H_TOTAL. Dragging the width or height edge updates the other dimension accordingly. """ RESIZE_MARGIN = 8 def __init__(self): super().__init__() self._aspect_hw = ASPECT_H_TOTAL / float(ASPECT_W) self._resize_guard = False self.user_closed = False self._drag_anchor = None self._frameless_resize_edge = None self._frameless_resize_start_global = None self._frameless_resize_start_geom = None def _min_client_size(self): w = max(320, self.minimumSizeHint().width()) h = int(round(w * self._aspect_hw)) return w, h def _hit_test_resize(self, global_pos): if not USE_FRAMELESS_WINDOW: return None p = self.mapFromGlobal(global_pos) w, h = self.width(), self.height() m = self.RESIZE_MARGIN left = p.x() <= m right = p.x() >= w - m top = p.y() <= m bottom = p.y() >= h - m if top and left: return "tl" if top and right: return "tr" if bottom and left: return "bl" if bottom and right: return "br" if top: return "t" if bottom: return "b" if left: return "l" if right: return "r" return None def _cursor_for_edge(self, edge): return { "tl": Qt.SizeFDiagCursor, "br": Qt.SizeFDiagCursor, "tr": Qt.SizeBDiagCursor, "bl": Qt.SizeBDiagCursor, "l": Qt.SizeHorCursor, "r": Qt.SizeHorCursor, "t": Qt.SizeVerCursor, "b": Qt.SizeVerCursor, }.get(edge, Qt.ArrowCursor) def _apply_frameless_resize(self, dg): edge = self._frameless_resize_edge g0 = self._frameless_resize_start_geom min_w, min_h = self._min_client_size() dx, dy = dg.x(), dg.y() x0, y0, w0, h0 = g0.x(), g0.y(), g0.width(), g0.height() if edge == "br": nw = max(min_w, w0 + dx) nh = int(round(nw * self._aspect_hw)) self.setGeometry(x0, y0, nw, nh) elif edge == "r": nw = max(min_w, w0 + dx) nh = int(round(nw * self._aspect_hw)) self.setGeometry(x0, y0, nw, nh) elif edge == "b": nh = max(min_h, h0 + dy) nw = int(round(nh / self._aspect_hw)) self.setGeometry(x0, y0, nw, nh) elif edge == "l": nw = max(min_w, w0 - dx) nh = int(round(nw * self._aspect_hw)) nx = g0.right() - nw self.setGeometry(nx, y0, nw, nh) elif edge == "t": nh = max(min_h, h0 - dy) nw = int(round(nh / self._aspect_hw)) ny = g0.bottom() - nh self.setGeometry(x0, ny, nw, nh) elif edge == "tr": nw = max(min_w, w0 + dx) nh = int(round(nw * self._aspect_hw)) ny = g0.bottom() - nh self.setGeometry(x0, ny, nw, nh) elif edge == "bl": nw = max(min_w, w0 - dx) nh = int(round(nw * self._aspect_hw)) nx = g0.right() - nw self.setGeometry(nx, y0, nw, nh) elif edge == "tl": nw = max(min_w, w0 - dx) nh = int(round(nw * self._aspect_hw)) nx = g0.right() - nw ny = g0.bottom() - nh self.setGeometry(nx, ny, nw, nh) def install_drag_filters(self): """Alt+left-drag to move when frameless; grabs go to this widget.""" for w in self.findChildren(QWidget): if w is not self: w.installEventFilter(self) def eventFilter(self, watched, event): if not USE_FRAMELESS_WINDOW: return super().eventFilter(watched, event) et = event.type() if et == QEvent.MouseButtonPress and event.button() == Qt.LeftButton: edge = self._hit_test_resize(event.globalPos()) if edge is not None: self._frameless_resize_edge = edge self._frameless_resize_start_global = event.globalPos() self._frameless_resize_start_geom = self.geometry() self.grabMouse() return True if QApplication.keyboardModifiers() & Qt.AltModifier: self._drag_anchor = event.globalPos() - self.frameGeometry().topLeft() self.grabMouse() return True if et == QEvent.MouseMove and not event.buttons(): edge = self._hit_test_resize(event.globalPos()) watched.setCursor(QCursor(self._cursor_for_edge(edge))) return super().eventFilter(watched, event) def mouseMoveEvent(self, event): if USE_FRAMELESS_WINDOW and self._frameless_resize_edge and event.buttons() & Qt.LeftButton: dg = event.globalPos() - self._frameless_resize_start_global self._apply_frameless_resize(dg) event.accept() return if ( USE_FRAMELESS_WINDOW and self._drag_anchor is not None and event.buttons() & Qt.LeftButton ): self.move(event.globalPos() - self._drag_anchor) event.accept() return super().mouseMoveEvent(event) def mouseReleaseEvent(self, event): if USE_FRAMELESS_WINDOW and self._frameless_resize_edge and event.button() == Qt.LeftButton: self.releaseMouse() self._frameless_resize_edge = None self._frameless_resize_start_global = None self._frameless_resize_start_geom = None event.accept() return if USE_FRAMELESS_WINDOW and self._drag_anchor is not None and event.button() == Qt.LeftButton: self.releaseMouse() self._drag_anchor = None event.accept() return super().mouseReleaseEvent(event) def closeEvent(self, event): self.user_closed = True event.accept() def minimumSizeHint(self): w = 320 return QSize(w, int(round(w * self._aspect_hw))) def resizeEvent(self, event): if self._resize_guard: super().resizeEvent(event) return super().resizeEvent(event) w, h = self.width(), self.height() if w < 8 or h < 8: return old = event.oldSize() self._resize_guard = True try: if old.isValid() and old.width() > 0 and old.height() > 0: dw = abs(w - old.width()) dh = abs(h - old.height()) if dw >= dh: new_w, new_h = w, int(round(w * self._aspect_hw)) else: new_w, new_h = int(round(h / self._aspect_hw)), h else: new_w, new_h = w, int(round(w * self._aspect_hw)) if new_w != w or new_h != h: self.resize(new_w, new_h) finally: self._resize_guard = False def compute_arrow_canvas_side(win): """ Pixel size of the square arrow canvas from the window client area and the fixed vertical split ARROW_STRETCH : PLOT_STRETCH (matches QVBoxLayout stretch). """ w = win.width() h = win.height() if w < 16 or h < 16: return max(32, ASPECT_W) h_arrow = int(round(h * ARROW_STRETCH / float(ARROW_STRETCH + PLOT_STRETCH))) # With aspect ASPECT_W:ASPECT_H_TOTAL, h_arrow should equal w; min() absorbs rounding. return max(32, min(w, h_arrow)) def draw_arrows_wuxi( frame, optical_flow, fps=True, threshold=5, grid_spacing=40, arrow_scale=1.0, margin=0.8, colormap=cv2.COLORMAP_PARULA, below_threshold_color=(0, 0, 255), edge_margin=15, target_size=936, ): """ Draw colored optical flow arrows on the image by magnitude (JET colormap). Resizes frame and flow to target_size x target_size; scales flow vectors and grid_spacing / margins / threshold so arrow density matches the original resolution. """ image = frame.copy() if len(image.shape) == 2 or image.shape[2] == 1: image = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) h0, w0 = image.shape[:2] if h0 != target_size or w0 != target_size: sx = target_size / float(w0) sy = target_size / float(h0) image = cv2.resize(image, (target_size, target_size), interpolation=cv2.INTER_LINEAR) u = cv2.resize(optical_flow[:, :, 0], (target_size, target_size), interpolation=cv2.INTER_LINEAR) * sx v = cv2.resize(optical_flow[:, :, 1], (target_size, target_size), interpolation=cv2.INTER_LINEAR) * sy optical_flow = np.stack([u, v], axis=2) scale = max(sx, sy) grid_spacing = max(4, int(round(grid_spacing * scale))) edge_margin = max(1, int(round(edge_margin * scale))) threshold = float(threshold) * scale h, w = image.shape[:2] # Arrow grid in pixel coordinates (assume frame and flow same size) rr = np.arange(grid_spacing // 2, h, grid_spacing) cc = np.arange(grid_spacing // 2, w, grid_spacing) rr, cc = np.meshgrid(rr, cc, indexing="ij") rr, cc = rr.reshape(-1), cc.reshape(-1) # Displacement at each grid point (flow stored as half-displacement) disp = 2.0 * optical_flow[rr, cc, :] mag = np.linalg.norm(disp, axis=1) dx = disp[:, 0] / arrow_scale dy = disp[:, 1] / arrow_scale length = np.sqrt(dx * dx + dy * dy) # Cap arrow length at grid_spacing scale = np.where(length > grid_spacing*margin, grid_spacing / np.maximum(length, 1e-9), 1.0)*margin dx, dy = dx * scale, dy * scale end_x = (cc + dx).astype(np.int32) end_y = (rr + dy).astype(np.int32) valid = ( (cc >= edge_margin) & (cc < w - edge_margin) & (rr >= edge_margin) & (rr < h - edge_margin) & (end_x >= 0) & (end_x < w) & (end_y >= 0) & (end_y < h) ) mag_range = mag.max() - mag.min() norm_mag = ((mag - mag.min()) / mag_range * 255).astype(np.uint8) if mag_range > 0 else np.zeros_like(mag, dtype=np.uint8) norm_mag[mag < threshold] = 0 colors = cv2.applyColorMap(norm_mag.reshape(-1, 1), colormap) for i in np.where(valid)[0]: pt1, pt2 = (int(cc[i]), int(rr[i])), (int(end_x[i]), int(end_y[i])) color = tuple(int(c) for c in colors[i, 0]) if mag[i] < threshold: cv2.circle(image, pt1, radius=1, color=below_threshold_color, thickness=-1) else: # thickness = max(1, int(2 * mag[i] / mag.max())) if mag.max() > 0 else 2 thickness = 1 cv2.arrowedLine(image, pt1, pt2, color, thickness=thickness, tipLength=0.2) return image def set_arrow_label_bgr(label, bgr_image): """Show a BGR OpenCV image in a QLabel (RGB for Qt), centered with equal margins.""" rgb = cv2.cvtColor(bgr_image, cv2.COLOR_BGR2RGB) rgb = np.ascontiguousarray(rgb) h, w, ch = rgb.shape qimg = QImage(rgb.data, w, h, ch * w, QImage.Format_RGB888).copy() pm = QPixmap.fromImage(qimg) lw, lh = label.width(), label.height() if lw > 0 and lh > 0 and (pm.width() != lw or pm.height() != lh): # Keep square aspect; center in label (equal L/R or T/B bars if rect not square) pm = pm.scaled(lw, lh, Qt.KeepAspectRatio, Qt.SmoothTransformation) label.setPixmap(pm) def make_gradient_brush(color_hex, max_alpha=150): """ 创建一个从上到下渐变的笔刷 (兼容 PyQt5 和 PyQt6) """ c_top = QtGui.QColor(color_hex) c_top.setAlpha(max_alpha) # 顶部半透明 c_bot = QtGui.QColor(color_hex) c_bot.setAlpha(0) # 底部全透明 # 创建线性渐变：(0,0)到(0,1) 代表从控件顶部到底部 grad = QtGui.QLinearGradient(0, 0, 0, 1) if hasattr(QtGui.QGradient, 'CoordinateMode'): grad.setCoordinateMode(QtGui.QGradient.CoordinateMode.ObjectBoundingMode) else: grad.setCoordinateMode(QtGui.QGradient.ObjectBoundingMode) # ============================================== grad.setColorAt(0, c_top) # 0.0 (顶端) 颜色 grad.setColorAt(1, c_bot) # 1.0 (底端) 颜色 return QtGui.QBrush(grad) def create_plot(plot_container): pg.setConfigOption("background", PLOT_THEME_BG) pg.setConfigOption("foreground", PLOT_THEME_FG) pg.setConfigOptions(antialias=True) # 抗锯齿 theme_color = '#00E5FF' # '#00E5FF' line_pen = pg.mkPen(color=theme_color, width=2) grad_brush = make_gradient_brush(theme_color, max_alpha=120) axis_font = QFont("Segoe UI", 11) p1 = plot_container.addPlot() p1.setLabel('left', 'N') p1.getAxis('left').label.setFont(axis_font) p1.getAxis('left').label.setRotation(dispaly_angle) p1.showGrid(x=True, y=True, alpha=0.3) plot_container.nextRow() p2 = plot_container.addPlot() p2.setLabel('left', 'X') p2.getAxis('left').label.setFont(axis_font) p2.getAxis('left').label.setRotation(dispaly_angle) p2.showGrid(x=True, y=True, alpha=0.3) plot_container.nextRow() p3 = plot_container.addPlot() p3.setLabel('left', 'Y') # p3.setLabel('bottom', 'Time', units='s') p3.getAxis('left').label.setFont(axis_font) p3.getAxis('left').label.setRotation(dispaly_angle) p3.getAxis('bottom').label.setFont(axis_font) p3.showGrid(x=True, y=True, alpha=0.3) _plot_bg = pg.mkColor(PLOT_THEME_BG) for p in (p1, p2, p3): p.getAxis("bottom").setStyle(showValues=False) p.getAxis("left").setStyle(showValues=False) # Keep title as "N" / "X" / "Y" — no PyQtGraph "(x0.001)" suffix on small ranges. p.getAxis("left").enableAutoSIPrefix(False) # PlotItem.setBackground exists only in newer pyqtgraph; ViewBox works on older installs. p.vb.setBackgroundColor(_plot_bg) plot_af = p1.plot(pen=line_pen, fillLevel=0, fillBrush=grad_brush) plot_tfx = p2.plot(pen=line_pen, fillLevel=0, fillBrush=grad_brush) plot_tfy = p3.plot(pen=line_pen, fillLevel=0, fillBrush=grad_brush) return p1, p2, p3, plot_af, plot_tfx, plot_tfy def _plot_y_range(y): """Avoid degenerate Y span; explicit x/y in setData so older PyQtGraph never plots time as y.""" mn = float(np.min(y)) mx = float(np.max(y)) if not (np.isfinite(mn) and np.isfinite(mx)): return 0.0, 1.0 if mx - mn <= 1e-15: mid = 0.5 * (mn + mx) return mid - 0.5, mid + 0.5 pad = (mx - mn) * 0.05 return mn - pad, mx + pad def update(plot_af, plot_tfx, plot_tfy, data, p1, p2, p3): # global plot_af, plot_tfx, plot_tfy, data, p1, p2, p3 plot_af.setData(data[:, 0, 0], data[:, 1, 0]) p1.setYRange(*_plot_y_range(data[:, 1, 0])) plot_tfx.setData(data[:, 0, 1], data[:, 1, 1]) p2.setYRange(*_plot_y_range(data[:, 1, 1])) plot_tfy.setData(data[:, 0, 2], data[:, 1, 2]) p3.setYRange(*_plot_y_range(data[:, 1, 2])) # 实时图表：单窗口，上为光流箭头，下为曲线 pg.setConfigOption("background", PLOT_THEME_BG) pg.setConfigOption("foreground", PLOT_THEME_FG) app = pg.mkQApp("Wuxi Demo") combined_win = CombinedTactileWindow() combined_win.setWindowTitle("Wuxi tactile — flow + forces") main_layout = QVBoxLayout(combined_win) main_layout.setContentsMargins(0, 0, 0, 0) main_layout.setSpacing(0) # Ignored: do not let pixmap/content drive QLabel size — layout stretch sets arrow vs plot heights. _policy_layout = QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored) arrow_label = QLabel() arrow_label.setAlignment(Qt.AlignHCenter | Qt.AlignVCenter) arrow_label.setStyleSheet( "background-color: #0B1016; margin: 0; padding: 0; border: none;" ) arrow_label.setSizePolicy(_policy_layout) arrow_label.setMinimumSize(0, 0) arrow_label.setScaledContents(False) main_layout.addWidget(arrow_label, stretch=ARROW_STRETCH) plot_widget = pg.GraphicsLayoutWidget() plot_widget.setBackground(PLOT_THEME_BG) plot_widget.setSizePolicy(_policy_layout) plot_widget.setMinimumSize(0, 0) main_layout.addWidget(plot_widget, stretch=PLOT_STRETCH) if USE_FRAMELESS_WINDOW: combined_win.setWindowFlags(Qt.Window | Qt.FramelessWindowHint) # Client area matches theme (no bright border flash on frameless) combined_win.setStyleSheet(f"background-color: {PLOT_THEME_BG};") combined_win.resize(ASPECT_W, ASPECT_H_TOTAL) combined_win.install_drag_filters() combined_win.show() p1, p2, p3, plot_af, plot_tfx, plot_tfy = create_plot(plot_widget) data = np.zeros([npoints,2,3]) start_time = None # Will be set when sensor starts fps = 30.0 for i in range(3): data[:,0,i] = np.linspace(0.0, npoints / fps, npoints) def main(): global start_time parser = argparse.ArgumentParser() parser.add_argument("--video","-v", type=int, default=0, help="视频文件路径") args = parser.parse_args() session_start = datetime.now() print( f"[session] START {session_start:%Y-%m-%d %H:%M:%S} (long-run / crash log marker)", flush=True, ) if USE_FRAMELESS_WINDOW: print( "[window] Frameless: resize = drag window edges/corners; move = Alt+left-drag; quit = Q.", flush=True, ) sensor1 = None try: sensor1 = orisys.Sensor( args.video, config_name="./config/ddjx01_wuxi.json", cal_path="./config/dd01a_0306.npy", isstitch=True, cuda=False, verbose=False, ) def _shortcut_reset_tracker(): sensor1.reset() print("传感器已重置 (快捷键 R)", flush=True) def _shortcut_quit(): combined_win.close() _sc_r = QShortcut(QKeySequence(Qt.Key_R), combined_win) _sc_r.activated.connect(_shortcut_reset_tracker) _sc_q = QShortcut(QKeySequence(Qt.Key_Q), combined_win) _sc_q.activated.connect(_shortcut_quit) start_time = time.time() is_save = False frame_count = 0 writer = None while True: sensor1.get_img() # 提取并拼接复眼图像 sensor1.compute_deformation(check_motion = True, tail_frames = frame_to_recover, threshold = touch_threshold) # 计算形变和分解 # 提取信息： # 向量场: VRAW, VNORMAL, VSHEAR: 原始，法向，切向 # 帧率，时间戳: FPS, TIMESTAMP # 变形量总和: FNORMAL , FSHEARX, FSHEARY : 法向，切向（X)，切向（Y) fps, fn, fx, fy, flow = sensor1.read_info( sensor1.info.FPS, sensor1.info.FNORMAL, sensor1.info.FSHEARX, sensor1.info.FSHEARY, sensor1.info.VRAW, ) # print(fps,fn,fx,fy) # 显示向量场 — BGR background; img may be 2D (H,W) or 3D (H,W,C) _bg_bgr = (22, 16, 11) _img = sensor1.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] # Square side = min(label w,h) so pixmap matches centered KeepAspectRatio scaling app.processEvents() lw, lh = arrow_label.width(), arrow_label.height() if lw > 16 and lh > 16: side = max(32, min(lw, lh)) else: side = compute_arrow_canvas_side(combined_win) arrows = draw_arrows_wuxi( black_img, flow, threshold=2, grid_spacing=20, arrow_scale=0.5, below_threshold_color=(160, 160, 160), target_size=int(side), ) set_arrow_label_bgr(arrow_label, arrows) # 显示图表 - 更新时间轴 current_time = time.time() - start_time for i in range(3): data[:-1, 0, i] = data[1:, 0, i] # shift time data left data[:-1, 1, i] = data[1:, 1, i] # shift force data left data[-1, 0, :] = frame_count / 15.0 # set current time for all plots data[-1, 1, 0] = fn data[-1, 1, 1] = abs(fx) data[-1, 1, 2] = abs(fy) update(plot_af, plot_tfx, plot_tfy, data, p1, p2, p3) # Update plot frame_count += 1 app.processEvents() if combined_win.user_closed or not combined_win.isVisible(): print("\n窗口已关闭，退出程序") if writer is not None: writer.release() writer = None break # Convert combined window to image (arrows + curves) pixmap = combined_win.grab() # Convert QPixmap to QImage qimg = pixmap.toImage() # Convert QImage to numpy array (RGB format) qimg = qimg.convertToFormat(QImage.Format_RGB888) width = qimg.width() height = qimg.height() # Convert QImage to numpy array ptr = qimg.bits() ptr.setsize(height * width * 3) arr = np.array(ptr, dtype=np.uint8).reshape((height, width, 3)) key = cv2.waitKey(1) if key & 0xFF == ord("q"): if writer is not None: writer.release() writer = None print("\n 停止程序") break if key & 0xFF == ord("r"): # 重置光流追踪器（当追踪漂移时使用）；焦点需在 OpenCV 窗口上 sensor1.reset() print("传感器已重置") except BaseException as exc: session_abnormal = datetime.now() print( f"[session] ABNORMAL at {session_abnormal:%Y-%m-%d %H:%M:%S}: " f"{type(exc).__name__}: {exc}", flush=True, ) raise finally: session_end = datetime.now() elapsed = session_end - session_start print( f"[session] END {session_end:%Y-%m-%d %H:%M:%S} " f"(elapsed {elapsed})", flush=True, ) if sensor1 is not None: try: sensor1.disconnect() except Exception as disconnect_err: print(f"[session] disconnect error: {disconnect_err}", flush=True) if __name__ == '__main__': main()