ykcheungab
632 lines
23 KiB
Python
632 lines
23 KiB
Python
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()
|