"""
Camera GUI using PyQt5 and OpenCV
Displays two video streams: deformation arrows and raw image
"""
import os
os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
import sys
import time
import cv2
import numpy as np
import pyqtgraph as pg
from orisys import Sensor
from PyQt5.QtWidgets import (QApplication, QMainWindow, QWidget, QVBoxLayout,
QHBoxLayout, QPushButton, QLabel, QCheckBox, QLineEdit, QFileDialog)
from PyQt5.QtCore import Qt, QTimer
from PyQt5.QtGui import QImage, QPixmap, QIcon
def draw_arrows_fade(img, flow, depth, depth_scale=0.5, grid_spacing=20, arrow_scale=1.0, threshold=5.0):
"""
Draw optical flow arrows on image with transparency based on magnitude.
Args:
img: Input image (BGR or grayscale)
flow: Optical flow field with shape (flow_height, flow_width, 2)
depth: Depth information
depth_scale: Scale factor for depth visualization
grid_spacing: Spacing between arrows in pixels (default: 20)
arrow_scale: Scale factor for arrow length (default: 1.0)
threshold: Minimum flow magnitude to display arrow (default: 5.0)
Returns:
Image with arrows drawn, blended with original image
"""
depth_img = cv2.applyColorMap(((depth+1)*depth_scale* 255.0).astype('uint8'), cv2.COLORMAP_HOT)
# Make a copy of the image
img_copy = depth_img.copy()
# Convert grayscale to BGR if needed
if len(img_copy.shape) == 2 or img_copy.shape[2] == 1:
img_copy = cv2.cvtColor(img_copy, cv2.COLOR_GRAY2BGR)
# Get image and flow dimensions
img_height, img_width = img_copy.shape[:2]
flow_height, flow_width = flow.shape[:2]
# Calculate scaling factors between image and flow dimensions
scale_x = img_width / flow_width
scale_y = img_height / flow_height
# Create grid positions in flow coordinates
grid_y, grid_x = np.mgrid[grid_spacing//2:flow_height:grid_spacing,
grid_spacing//2:flow_width:grid_spacing]
# Flatten grid positions
grid_y_flat = grid_y.flatten()
grid_x_flat = grid_x.flatten()
# Get flow vectors at grid positions
flow_vectors = flow[grid_y_flat, grid_x_flat, :] # Shape: (n_points, 2)
# Calculate flow magnitudes
magnitudes = np.linalg.norm(flow_vectors, axis=1)
# Filter arrows by threshold: only show arrows above threshold
valid_mask = magnitudes >= threshold
# Normalize magnitudes to 0-1 range for alpha blending
# Zero magnitude = transparent, higher magnitude = more opaque white
max_magnitude = magnitudes.max() if magnitudes.max() > 0 else 1.0
alpha_values = magnitudes / max_magnitude
# Scale grid positions to image coordinates
img_grid_x = (grid_x_flat * scale_x).astype(np.int32)
img_grid_y = (grid_y_flat * scale_y).astype(np.int32)
# Scale flow vectors to image coordinates
scaled_flow_x = flow_vectors[:, 0] * scale_x / arrow_scale
scaled_flow_y = flow_vectors[:, 1] * scale_y / arrow_scale
# Calculate arrow endpoints
end_x = (img_grid_x + scaled_flow_x).astype(np.int32)
end_y = (img_grid_y + scaled_flow_y).astype(np.int32)
# Create overlay image with white background for arrows
overlay = np.zeros_like(img_copy)
# Create weight map for blending based on arrow presence and magnitude
weight_map = np.zeros((img_height, img_width), dtype=np.float32)
# Draw arrows on overlay with white color and build weight map
for i in range(len(img_grid_x)):
# Skip if magnitude is below threshold
if not valid_mask[i]:
continue
# Check bounds
if (0 <= img_grid_x[i] < img_width and 0 <= img_grid_y[i] < img_height and
0 <= end_x[i] < img_width and 0 <= end_y[i] < img_height):
# Get alpha value (0 = transparent, 1 = opaque white)
alpha = alpha_values[i]
# Skip if magnitude is zero (transparent)
if alpha <= 0:
continue
# Use constant thickness for all arrows (magnitude affects opacity, not thickness)
thickness = 2
# White color for arrows
color = (255, 255, 255)
# Draw arrow on overlay
cv2.arrowedLine(overlay,
(img_grid_x[i], img_grid_y[i]),
(end_x[i], end_y[i]),
color,
thickness=thickness,
tipLength=0.3)
# Create a line mask for this arrow in the weight map
line_mask = np.zeros((img_height, img_width), dtype=np.float32)
cv2.line(line_mask,
(img_grid_x[i], img_grid_y[i]),
(end_x[i], end_y[i]),
1.0,
thickness=thickness)
# Add arrow tip approximation
cv2.circle(line_mask, (end_x[i], end_y[i]), thickness, 1.0, -1)
# Weight by alpha (magnitude) - use maximum for overlapping arrows
weight_map = np.maximum(weight_map, line_mask * alpha)
# Expand weight map to 3 channels
weight_map_3ch = np.stack([weight_map] * 3, axis=2)
# Blend: result = original * (1 - weight) + overlay * weight
result = (img_copy.astype(np.float32) * (1 - weight_map_3ch) +
overlay.astype(np.float32) * weight_map_3ch)
return result.astype(np.uint8)
class CameraGUI(QMainWindow):
def __init__(self):
super().__init__()
self.setWindowTitle("Orisys Sensor Demo")
# Set minimum window size and aspect ratio
self.setMinimumSize(1200, 800)
self.setGeometry(100, 100, 1366, 768) # Initial size
# Set window icon (logo)
if os.path.exists("orisys_logo_notext.png"):
self.setWindowIcon(QIcon("orisys_logo_notext.png"))
# Sensor variables
self.sensor = None
self.camera_running = False
self.timer = QTimer()
self.timer.timeout.connect(self.update_frame)
# Recording variables
self.recording = False
self.video_writer_arrows = None
self.video_writer_raw = None
# Default to Windows Pictures folder
pictures_folder = os.path.join(os.path.expanduser("~"), "Pictures")
self.record_directory = os.path.join(pictures_folder, "OrisysRecordings")
self.record_filename = "recording"
# Plotting variables
self.npoints = 100 # Chart data length
self.start_time = None # Will be set when sensor starts
self.data = None # Will be initialized in init_plots
self.max_magnitude = 0.0 # Track maximum magnitude for scaling
self.dynamic_scaling = True # Enable dynamic scaling by default
self.max_scale = 500000.0 # Default max scale value
# Create GUI
self.init_ui()
def init_ui(self):
# Central widget
central_widget = QWidget()
self.setCentralWidget(central_widget)
# Main layout: horizontal split between left (controls/images) and right (plots)
main_layout = QHBoxLayout(central_widget)
main_layout.setSpacing(15)
# Left side: Controls and images
left_panel = QWidget()
left_panel_layout = QVBoxLayout(left_panel)
left_panel_layout.setSpacing(10)
# Title (left aligned)
title_label = QLabel("叠动视触觉传感器演示")
title_label.setStyleSheet("""
font-size: 24px;
font-weight: bold;
padding: 10px;
color: #2c3e50;
font-family: 'Segoe UI', Arial, sans-serif;
background-color: #ecf0f1;
border-radius: 8px;
""")
title_label.setAlignment(Qt.AlignLeft | Qt.AlignVCenter)
left_panel_layout.addWidget(title_label)
# Button layout (top left)
button_layout = QHBoxLayout()
button_layout.setSpacing(10)
# Start button
self.start_button = QPushButton("Start")
self.start_button.setStyleSheet("""
QPushButton {
background-color: #27ae60;
color: white;
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 12px 35px;
border-radius: 6px;
border: none;
}
QPushButton:hover {
background-color: #229954;
}
QPushButton:pressed {
background-color: #1e8449;
}
QPushButton:disabled {
background-color: #95a5a6;
color: #ecf0f1;
}
""")
self.start_button.clicked.connect(self.start_camera)
button_layout.addWidget(self.start_button)
# Stop button
self.stop_button = QPushButton("Stop")
self.stop_button.setStyleSheet("""
QPushButton {
background-color: #e74c3c;
color: white;
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 12px 35px;
border-radius: 6px;
border: none;
}
QPushButton:hover {
background-color: #c0392b;
}
QPushButton:pressed {
background-color: #a93226;
}
QPushButton:disabled {
background-color: #95a5a6;
color: #ecf0f1;
}
""")
self.stop_button.setEnabled(False)
self.stop_button.clicked.connect(self.stop_camera)
button_layout.addWidget(self.stop_button)
# Reset button
self.reset_button = QPushButton("Reset")
self.reset_button.setStyleSheet("""
QPushButton {
background-color: #f39c12;
color: white;
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 12px 35px;
border-radius: 6px;
border: none;
}
QPushButton:hover {
background-color: #e67e22;
}
QPushButton:pressed {
background-color: #d35400;
}
QPushButton:disabled {
background-color: #95a5a6;
color: #ecf0f1;
}
""")
self.reset_button.setEnabled(False) # Disabled until camera starts
self.reset_button.clicked.connect(self.reset_sensor)
button_layout.addWidget(self.reset_button)
# Exit button
exit_button = QPushButton("Exit")
exit_button.setStyleSheet("""
QPushButton {
background-color: #7f8c8d;
color: white;
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 12px 35px;
border-radius: 6px;
border: none;
}
QPushButton:hover {
background-color: #5d6d7e;
}
QPushButton:pressed {
background-color: #566573;
}
""")
exit_button.clicked.connect(self.close)
button_layout.addWidget(exit_button)
button_layout.addStretch() # Push buttons to the left
left_panel_layout.addLayout(button_layout)
# Recording controls
recording_layout = QVBoxLayout()
recording_label = QLabel("录制设置")
recording_label.setStyleSheet("""
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #2c3e50;
padding: 5px;
""")
recording_layout.addWidget(recording_label)
# Directory input
dir_layout = QHBoxLayout()
dir_label = QLabel("目录:")
dir_label.setStyleSheet("font-size: 12px; font-family: 'Segoe UI', Arial, sans-serif;")
dir_layout.addWidget(dir_label)
self.record_dir_textbox = QLineEdit()
# Set default to Windows Pictures folder
pictures_folder = os.path.join(os.path.expanduser("~"), "Pictures")
default_dir = os.path.join(pictures_folder, "OrisysRecordings")
self.record_dir_textbox.setText(default_dir)
self.record_dir_textbox.setStyleSheet("""
font-size: 12px;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 4px;
border: 1px solid #bdc3c7;
border-radius: 4px;
""")
self.record_dir_textbox.textChanged.connect(self.on_record_dir_changed)
dir_layout.addWidget(self.record_dir_textbox)
# Browse button
browse_button = QPushButton("浏览")
browse_button.setStyleSheet("""
QPushButton {
background-color: #95a5a6;
color: white;
font-size: 11px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 6px 15px;
border-radius: 4px;
border: none;
}
QPushButton:hover {
background-color: #7f8c8d;
}
QPushButton:pressed {
background-color: #5d6d7e;
}
""")
browse_button.clicked.connect(self.browse_directory)
dir_layout.addWidget(browse_button)
recording_layout.addLayout(dir_layout)
# Filename input
filename_layout = QHBoxLayout()
filename_label = QLabel("文件名:")
filename_label.setStyleSheet("font-size: 12px; font-family: 'Segoe UI', Arial, sans-serif;")
filename_layout.addWidget(filename_label)
self.record_filename_textbox = QLineEdit()
self.record_filename_textbox.setText("recording")
self.record_filename_textbox.setStyleSheet("""
font-size: 12px;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 4px;
border: 1px solid #bdc3c7;
border-radius: 4px;
""")
self.record_filename_textbox.textChanged.connect(self.on_record_filename_changed)
filename_layout.addWidget(self.record_filename_textbox)
recording_layout.addLayout(filename_layout)
# Record button
self.record_button = QPushButton("开始录制")
self.record_button.setStyleSheet("""
QPushButton {
background-color: #3498db;
color: white;
font-size: 13px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 10px 25px;
border-radius: 6px;
border: none;
}
QPushButton:hover {
background-color: #2980b9;
}
QPushButton:pressed {
background-color: #21618c;
}
QPushButton:disabled {
background-color: #95a5a6;
color: #ecf0f1;
}
""")
self.record_button.setEnabled(False) # Disabled until camera starts
self.record_button.clicked.connect(self.toggle_recording)
recording_layout.addWidget(self.record_button)
recording_widget = QWidget()
recording_widget.setLayout(recording_layout)
recording_widget.setStyleSheet("""
QWidget {
border: 1px solid #bdc3c7;
border-radius: 5px;
background-color: #f8f9fa;
padding: 8px;
}
""")
left_panel_layout.addWidget(recording_widget)
# Left side: Image displays (side by side)
images_layout = QHBoxLayout()
# Arrows (deformation) display
arrows_label = QLabel("变形")
arrows_label.setStyleSheet("""
font-size: 15px;
font-weight: bold;
padding: 8px;
color: #2c3e50;
font-family: 'Segoe UI', Arial, sans-serif;
background-color: #ecf0f1;
border-radius: 5px;
""")
arrows_label.setAlignment(Qt.AlignCenter)
self.arrows_image_label = QLabel()
self.arrows_image_label.setMinimumSize(400, 400) # Fixed initial size
self.arrows_image_label.setMaximumSize(400, 400) # Fixed size
self.arrows_image_label.setStyleSheet("background-color: black; border: 2px solid #4a90e2; border-radius: 5px;")
self.arrows_image_label.setAlignment(Qt.AlignCenter)
self.arrows_image_label.setScaledContents(True) # Allow scaling within fixed size
arrows_container = QWidget()
arrows_container_layout = QVBoxLayout(arrows_container)
arrows_container_layout.addWidget(arrows_label)
arrows_container_layout.addWidget(self.arrows_image_label)
images_layout.addWidget(arrows_container)
# Raw image display
raw_label = QLabel("原始图像")
raw_label.setStyleSheet("""
font-size: 15px;
font-weight: bold;
padding: 8px;
color: #2c3e50;
font-family: 'Segoe UI', Arial, sans-serif;
background-color: #ecf0f1;
border-radius: 5px;
""")
raw_label.setAlignment(Qt.AlignCenter)
self.raw_image_label = QLabel()
self.raw_image_label.setMinimumSize(400, 400) # Fixed initial size
self.raw_image_label.setMaximumSize(400, 400) # Fixed size
self.raw_image_label.setStyleSheet("background-color: black; border: 2px solid #4a90e2; border-radius: 5px;")
self.raw_image_label.setAlignment(Qt.AlignCenter)
self.raw_image_label.setScaledContents(True) # Allow scaling within fixed size
raw_container = QWidget()
raw_container_layout = QVBoxLayout(raw_container)
raw_container_layout.addWidget(raw_label)
raw_container_layout.addWidget(self.raw_image_label)
images_layout.addWidget(raw_container)
# FPS/Status label (left side, not spanning to plots)
self.status_label = QLabel("Status: Stopped")
self.status_label.setStyleSheet("""
font-size: 13px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #e74c3c;
padding: 8px;
background-color: #fadbd8;
border-radius: 5px;
""")
self.status_label.setAlignment(Qt.AlignLeft | Qt.AlignVCenter)
left_panel_layout.addWidget(self.status_label)
# Images layout
images_widget = QWidget()
images_widget.setLayout(images_layout)
left_panel_layout.addWidget(images_widget)
left_panel_layout.addStretch() # Push everything to top
# Add left panel to main layout
main_layout.addWidget(left_panel)
# Right side: Plots with controls (all in one bordered container)
plots_container = QWidget()
plots_container.setStyleSheet("""
QWidget {
border: 2px solid #4a90e2;
border-radius: 8px;
background-color: #ffffff;
padding: 10px;
}
""")
plots_container_layout = QVBoxLayout(plots_container)
plots_container_layout.setContentsMargins(10, 10, 10, 10)
plots_container_layout.setSpacing(10)
# Create controls container (no border, just for layout)
controls_container = QWidget()
controls_layout = QVBoxLayout(controls_container)
controls_layout.setContentsMargins(0, 0, 0, 0)
# Title label
controls_title = QLabel("力信息")
controls_title.setStyleSheet("""
font-size: 18px;
font-weight: bold;
padding: 8px;
color: #2c3e50;
font-family: 'Segoe UI', Arial, sans-serif;
""")
controls_title.setAlignment(Qt.AlignCenter)
controls_layout.addWidget(controls_title)
# Scaling controls layout
scaling_layout = QHBoxLayout()
# Dynamic scaling checkbox
self.dynamic_scaling_checkbox = QCheckBox("动态缩放")
self.dynamic_scaling_checkbox.setChecked(True)
self.dynamic_scaling_checkbox.setStyleSheet("""
font-size: 12px;
font-family: 'Segoe UI', Arial, sans-serif;
color: #34495e;
""")
self.dynamic_scaling_checkbox.toggled.connect(self.on_dynamic_scaling_changed)
scaling_layout.addWidget(self.dynamic_scaling_checkbox)
# Max scale label and textbox
max_scale_label = QLabel("最大刻度:")
max_scale_label.setStyleSheet("""
font-size: 12px;
font-family: 'Segoe UI', Arial, sans-serif;
color: #34495e;
""")
scaling_layout.addWidget(max_scale_label)
self.max_scale_textbox = QLineEdit()
self.max_scale_textbox.setText("500000.0")
self.max_scale_textbox.setEnabled(False) # Disabled when dynamic scaling is on
self.max_scale_textbox.setMaximumWidth(100)
self.max_scale_textbox.setStyleSheet("""
font-size: 12px;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 4px;
border: 1px solid #bdc3c7;
border-radius: 4px;
""")
self.max_scale_textbox.textChanged.connect(self.on_max_scale_changed)
scaling_layout.addWidget(self.max_scale_textbox)
scaling_layout.addStretch() # Push controls to the left
controls_layout.addLayout(scaling_layout)
plots_container_layout.addWidget(controls_container)
# Initialize plots
self.init_plots()
plots_container_layout.addWidget(self.plot_widget)
# Add plots container to main layout (right side)
main_layout.addWidget(plots_container)
def on_dynamic_scaling_changed(self, checked):
"""Handle dynamic scaling checkbox state change"""
self.dynamic_scaling = checked
self.max_scale_textbox.setEnabled(not self.dynamic_scaling)
def on_max_scale_changed(self, text):
"""Handle max scale textbox change"""
try:
self.max_scale = float(text)
if self.max_scale <= 0:
self.max_scale = 1.0
except ValueError:
# Invalid input, keep previous value
pass
def on_record_dir_changed(self, text):
"""Handle record directory change"""
if text:
self.record_directory = text
else:
# Default to Windows Pictures folder if empty
pictures_folder = os.path.join(os.path.expanduser("~"), "Pictures")
self.record_directory = os.path.join(pictures_folder, "OrisysRecordings")
def on_record_filename_changed(self, text):
"""Handle record filename change"""
self.record_filename = text if text else "recording"
def browse_directory(self):
"""Open directory browser dialog"""
directory = QFileDialog.getExistingDirectory(
self,
"选择保存目录",
self.record_directory if os.path.exists(self.record_directory) else "."
)
if directory:
self.record_dir_textbox.setText(directory)
self.record_directory = directory
def toggle_recording(self):
"""Toggle recording on/off"""
if not self.recording:
# Start recording
if not os.path.exists(self.record_directory):
os.makedirs(self.record_directory)
# Get frame size from sensor images (original size, not scaled)
if self.sensor and hasattr(self.sensor, 'img'):
h_raw, w_raw = self.sensor.img.shape[:2]
# Arrows image has same dimensions as raw (after cropping)
h_arr = h_raw - 30 if h_raw > 30 else h_raw
w_arr = w_raw - 30 if w_raw > 30 else w_raw
# Create video writers
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
fps = 30.0
arrows_path = os.path.join(self.record_directory, f"{self.record_filename}_arrows.mp4")
raw_path = os.path.join(self.record_directory, f"{self.record_filename}_raw.mp4")
self.video_writer_arrows = cv2.VideoWriter(
arrows_path, fourcc, fps,
(w_arr, h_arr)
)
self.video_writer_raw = cv2.VideoWriter(
raw_path, fourcc, fps,
(w_arr, h_arr)
)
if self.video_writer_arrows.isOpened() and self.video_writer_raw.isOpened():
self.recording = True
self.record_button.setText("停止录制")
self.record_button.setStyleSheet("""
QPushButton {
background-color: #e74c3c;
color: white;
font-size: 13px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 10px 25px;
border-radius: 6px;
border: none;
}
QPushButton:hover {
background-color: #c0392b;
}
QPushButton:pressed {
background-color: #a93226;
}
""")
else:
# Stop recording
self.recording = False
if self.video_writer_arrows:
self.video_writer_arrows.release()
self.video_writer_arrows = None
if self.video_writer_raw:
self.video_writer_raw.release()
self.video_writer_raw = None
self.record_button.setText("开始录制")
self.record_button.setStyleSheet("""
QPushButton {
background-color: #3498db;
color: white;
font-size: 13px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
padding: 10px 25px;
border-radius: 6px;
border: none;
}
QPushButton:hover {
background-color: #2980b9;
}
QPushButton:pressed {
background-color: #21618c;
}
""")
def init_plots(self):
"""Initialize pyqtgraph plots for force visualization"""
# Configure pyqtgraph with modern color theme
pg.setConfigOption('background', '#f8f9fa') # Light gray background
pg.setConfigOption('foreground', '#2c3e50') # Dark blue-gray text
# Create GraphicsLayoutWidget (will scale with window)
self.plot_widget = pg.GraphicsLayoutWidget()
self.plot_widget.setMinimumSize(400, 600) # Minimum size, will scale
self.plot_widget.setBackground('#f8f9fa') # Light gray background
# Modern color palette
colors = {
'normal': '#3498db', # Blue
'shear_x': '#e74c3c', # Red
'shear_y': '#2ecc71', # Green
'magnitude': '#9b59b6' # Purple
}
# Create plots with modern styling
p1 = self.plot_widget.addPlot(title="法相力")
p1.setLabel('bottom', '数据点', units='', color='#34495e')
p1.setLabel('left', '力', color='#34495e')
p1.showGrid(x=True, y=True, alpha=0.3)
p1.getAxis('bottom').setPen(pg.mkPen(color='#7f8c8d', width=1))
p1.getAxis('left').setPen(pg.mkPen(color='#7f8c8d', width=1))
p1.getAxis('bottom').setTextPen(pg.mkPen(color='#34495e'))
p1.getAxis('left').setTextPen(pg.mkPen(color='#34495e'))
self.plot_widget.nextRow()
p2 = self.plot_widget.addPlot(title="切向力 X")
p2.setLabel('bottom', '数据点', units='', color='#34495e')
p2.setLabel('left', '力', color='#34495e')
p2.showGrid(x=True, y=True, alpha=0.3)
p2.getAxis('bottom').setPen(pg.mkPen(color='#7f8c8d', width=1))
p2.getAxis('left').setPen(pg.mkPen(color='#7f8c8d', width=1))
p2.getAxis('bottom').setTextPen(pg.mkPen(color='#34495e'))
p2.getAxis('left').setTextPen(pg.mkPen(color='#34495e'))
self.plot_widget.nextRow()
p3 = self.plot_widget.addPlot(title="切向力 Y")
p3.setLabel('bottom', '数据点', units='', color='#34495e')
p3.setLabel('left', '力', color='#34495e')
p3.showGrid(x=True, y=True, alpha=0.3)
p3.getAxis('bottom').setPen(pg.mkPen(color='#7f8c8d', width=1))
p3.getAxis('left').setPen(pg.mkPen(color='#7f8c8d', width=1))
p3.getAxis('bottom').setTextPen(pg.mkPen(color='#34495e'))
p3.getAxis('left').setTextPen(pg.mkPen(color='#34495e'))
self.plot_widget.nextRow()
p4 = self.plot_widget.addPlot(title="合力大小")
p4.setLabel('bottom', '数据点', units='', color='#34495e')
p4.setLabel('left', '力', color='#34495e')
p4.showGrid(x=True, y=True, alpha=0.3)
p4.getAxis('bottom').setPen(pg.mkPen(color='#7f8c8d', width=1))
p4.getAxis('left').setPen(pg.mkPen(color='#7f8c8d', width=1))
p4.getAxis('bottom').setTextPen(pg.mkPen(color='#34495e'))
p4.getAxis('left').setTextPen(pg.mkPen(color='#34495e'))
# Create plot curves with modern colors and styling
self.plot_af = p1.plot(pen=pg.mkPen(color=colors['normal'], width=2))
self.plot_tfx = p2.plot(pen=pg.mkPen(color=colors['shear_x'], width=2))
self.plot_tfy = p3.plot(pen=pg.mkPen(color=colors['shear_y'], width=2))
self.plot_magnitude = p4.plot(pen=pg.mkPen(color=colors['magnitude'], width=2))
# Store plot references
self.p1 = p1
self.p2 = p2
self.p3 = p3
self.p4 = p4
# Initialize data array: [npoints, 2, 4] where:
# - First dimension: data points
# - Second dimension: [time, force_value]
# - Third dimension: [normal, shear_x, shear_y, magnitude]
# Use relative time (0 to npoints) so x-axis always shows 0 to npoints
self.data = np.zeros([self.npoints, 2, 4])
self.data_count = 0 # Track how many data points we've collected
for i in range(4):
self.data[:, 0, i] = np.arange(self.npoints) # Start with 0 to npoints-1
def update_plots(self, fn, fx, fy, current_time):
"""Update the plots with new force data"""
if self.data is None:
return
# Calculate magnitude: sqrt(fn^2 + fx^2 + fy^2)
magnitude = np.sqrt(fn**2 + fx**2 + fy**2)
# Update maximum magnitude for scaling
self.max_magnitude = max(self.max_magnitude, magnitude)
# Update data count
self.data_count += 1
if self.data_count <= self.npoints:
# Before we have npoints, just append to the array
idx = self.data_count - 1 # 0-indexed
self.data[idx, 0, :] = idx # X-axis: 0, 1, 2, ..., data_count-1
self.data[idx, 1, 0] = fn
self.data[idx, 1, 1] = fx
self.data[idx, 1, 2] = fy
self.data[idx, 1, 3] = magnitude
# Show only the data we have so far
show_indices = slice(0, self.data_count)
# X-axis range: 0 to data_count-1
x_min, x_max = 0, max(0, self.data_count - 1)
else:
# After we have npoints, shift force data left (rolling window)
# Shift force values left
for i in range(4):
self.data[:-1, 1, i] = self.data[1:, 1, i] # shift force data left only
# Set new data at the end
self.data[-1, 1, 0] = fn
self.data[-1, 1, 1] = fx
self.data[-1, 1, 2] = fy
self.data[-1, 1, 3] = magnitude
# Update x-axis values to show sliding window of actual data point indices
# X-axis shows the actual data point numbers (data_count - npoints + 1 to data_count)
start_idx = self.data_count - self.npoints
for i in range(4):
self.data[:, 0, i] = np.arange(start_idx, start_idx + self.npoints)
# Show all npoints
show_indices = slice(None)
# X-axis range: start_idx to start_idx + npoints - 1
x_min, x_max = start_idx, start_idx + self.npoints - 1
# Set x-axis range to show the sliding window
self.p1.setXRange(x_min, x_max)
self.p2.setXRange(x_min, x_max)
self.p3.setXRange(x_min, x_max)
self.p4.setXRange(x_min, x_max)
# Calculate scaling based on dynamic scaling setting
if self.dynamic_scaling:
# Dynamic scaling: use current maximum force sum magnitude from visible data * 1.05
# Calculate max from current visible data (magnitude is in index 3)
if len(self.data[show_indices, 1, 3]) > 0:
current_max_magnitude = max(self.data[show_indices, 1, 3])
if current_max_magnitude > 0:
scale_max = current_max_magnitude * 1.05
else:
scale_max = 1.0
else:
scale_max = 1.0
else:
# Fixed scaling: use value from textbox
scale_max = self.max_scale
# Update plot data with scaled Y ranges
self.plot_af.setData(self.data[show_indices, 0, 0], self.data[show_indices, 1, 0])
y_min_0, y_max_0 = min(self.data[show_indices, 1, 0]), max(self.data[show_indices, 1, 0])
if y_max_0 > y_min_0:
# Normal force: start from 0 (no negative part)
self.p1.setYRange(0, scale_max)
else:
self.p1.setYRange(0, max(1.0, y_max_0 + 1))
self.plot_tfx.setData(self.data[show_indices, 0, 1], self.data[show_indices, 1, 1])
y_min_1, y_max_1 = min(self.data[show_indices, 1, 1]), max(self.data[show_indices, 1, 1])
if y_max_1 > y_min_1:
self.p2.setYRange(-scale_max, scale_max)
else:
self.p2.setYRange(y_min_1 - 1, y_max_1 + 1)
self.plot_tfy.setData(self.data[show_indices, 0, 2], self.data[show_indices, 1, 2])
y_min_2, y_max_2 = min(self.data[show_indices, 1, 2]), max(self.data[show_indices, 1, 2])
if y_max_2 > y_min_2:
self.p3.setYRange(-scale_max, scale_max)
else:
self.p3.setYRange(y_min_2 - 1, y_max_2 + 1)
# Update magnitude plot
self.plot_magnitude.setData(self.data[show_indices, 0, 3], self.data[show_indices, 1, 3])
y_min_mag, y_max_mag = min(self.data[show_indices, 1, 3]), max(self.data[show_indices, 1, 3])
if y_max_mag > y_min_mag:
self.p4.setYRange(0, scale_max) # Magnitude is always positive
else:
self.p4.setYRange(-1, 1)
def start_camera(self):
if not self.camera_running:
try:
# Open sensor (0 for camera, or use video file path)
# sensor1 = Sensor("./s1.mp4",isstitch=True,cuda = True,verbose = True)
self.sensor = Sensor(0, isstitch=True, cuda=True, verbose=True)
# Initialize start time for plots
self.start_time = time.time()
self.data_count = 0 # Reset data count when starting
self.max_magnitude = 0.0 # Reset max magnitude
# Reset plot data
self.data = np.zeros([self.npoints, 2, 4])
for i in range(4):
self.data[:, 0, i] = np.arange(self.npoints)
self.camera_running = True
self.timer.start(30) # ~30 FPS
self.status_label.setText("Status: Running")
self.status_label.setStyleSheet("""
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #27ae60;
padding: 10px;
background-color: #d5f4e6;
border-radius: 5px;
""")
self.start_button.setEnabled(False)
self.stop_button.setEnabled(True)
self.reset_button.setEnabled(True) # Enable reset when camera starts
self.record_button.setEnabled(True) # Enable recording when camera starts
except Exception as e:
self.status_label.setText(f"Status: Sensor Error - {str(e)}")
self.status_label.setStyleSheet("""
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #f39c12;
padding: 10px;
background-color: #fef5e7;
border-radius: 5px;
""")
self.sensor = None
def stop_camera(self):
if self.camera_running:
self.camera_running = False
self.timer.stop()
if self.sensor is not None:
try:
self.sensor.disconnect()
except:
pass
self.sensor = None
self.start_time = None
self.data_count = 0 # Reset data count
self.max_magnitude = 0.0 # Reset max magnitude
self.status_label.setText("Status: Stopped")
self.status_label.setStyleSheet("""
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #e74c3c;
padding: 10px;
background-color: #fadbd8;
border-radius: 5px;
""")
self.start_button.setEnabled(True)
self.stop_button.setEnabled(False)
self.reset_button.setEnabled(False) # Disable reset when camera stops
self.record_button.setEnabled(False) # Disable recording when camera stops
# Stop recording if active
if self.recording:
self.toggle_recording()
# Clear images
self.arrows_image_label.clear()
self.arrows_image_label.setStyleSheet("background-color: black; border: 2px solid #333;")
self.raw_image_label.clear()
self.raw_image_label.setStyleSheet("background-color: black; border: 2px solid #333;")
def reset_sensor(self):
"""Reset the sensor"""
if self.sensor is not None and self.camera_running:
try:
self.sensor.reset()
# Reset plot data
self.data_count = 0
self.max_magnitude = 0.0
self.data = np.zeros([self.npoints, 2, 4])
for i in range(4):
self.data[:, 0, i] = np.arange(self.npoints)
self.status_label.setText("Status: Sensor Reset")
self.status_label.setStyleSheet("""
font-size: 13px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #f39c12;
padding: 8px;
background-color: #fef5e7;
border-radius: 5px;
""")
except Exception as e:
self.status_label.setText(f"Status: Reset Error - {str(e)}")
self.status_label.setStyleSheet("""
font-size: 13px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #e74c3c;
padding: 8px;
background-color: #fadbd8;
border-radius: 5px;
""")
def update_frame(self):
if self.camera_running and self.sensor is not None:
try:
# Extract and stitch compound eye image
self.sensor.get_img()
# Compute deformation and decomposition
self.sensor.compute_deformation()
# Extract information:
# Vector fields: VRAW, VNORMAL, VSHEAR: raw, normal, tangential
# Frame rate, timestamp: FPS, TIMESTAMP
# Total deformation: FNORMAL, FSHEARX, FSHEARY: normal, tangential (X), tangential (Y)
fps, fn, fx, fy, flow, depth = self.sensor.read_info(
self.sensor.info.FPS,
self.sensor.info.FNORMAL,
self.sensor.info.FSHEARX,
self.sensor.info.FSHEARY,
self.sensor.info.VRAW,
self.sensor.info.DEPTH
)
# Generate arrows visualization
arrows = draw_arrows_fade(self.sensor.img, flow, depth)
# Crop edges (15 pixels from each side) - only if image is large enough
h, w = arrows.shape[:2]
if h > 30 and w > 30:
arrows_cropped = arrows[15:-15, 15:-15, :]
else:
arrows_cropped = arrows
# Crop raw image edges if large enough
h_raw, w_raw = self.sensor.img.shape[:2]
if h_raw > 30 and w_raw > 30:
raw_cropped = self.sensor.img[15:-15, 15:-15]
else:
raw_cropped = self.sensor.img
# Convert arrows image BGR to RGB
arrows_rgb = cv2.cvtColor(arrows_cropped, cv2.COLOR_BGR2RGB)
h_arr, w_arr, ch_arr = arrows_rgb.shape
bytes_per_line_arr = ch_arr * w_arr
qt_image_arr = QImage(arrows_rgb.data, w_arr, h_arr, bytes_per_line_arr, QImage.Format_RGB888)
# Scale to fixed 400x400 size (maintains aspect ratio)
scaled_pixmap_arr = QPixmap.fromImage(qt_image_arr).scaled(
400, 400,
Qt.KeepAspectRatio,
Qt.SmoothTransformation
)
self.arrows_image_label.setPixmap(scaled_pixmap_arr)
# Record arrows frame if recording
if self.recording and self.video_writer_arrows:
# Convert back to BGR for video writer
arrows_bgr = cv2.cvtColor(arrows_cropped, cv2.COLOR_RGB2BGR)
self.video_writer_arrows.write(arrows_bgr)
# Convert raw image BGR to RGB
raw_rgb = cv2.cvtColor(raw_cropped, cv2.COLOR_BGR2RGB)
h_raw_img, w_raw_img, ch_raw = raw_rgb.shape
bytes_per_line_raw = ch_raw * w_raw_img
qt_image_raw = QImage(raw_rgb.data, w_raw_img, h_raw_img, bytes_per_line_raw, QImage.Format_RGB888)
# Scale to fixed 400x400 size (maintains aspect ratio)
scaled_pixmap_raw = QPixmap.fromImage(qt_image_raw).scaled(
400, 400,
Qt.KeepAspectRatio,
Qt.SmoothTransformation
)
self.raw_image_label.setPixmap(scaled_pixmap_raw)
# Record raw frame if recording
if self.recording and self.video_writer_raw:
# Convert back to BGR for video writer
raw_bgr = cv2.cvtColor(raw_cropped, cv2.COLOR_RGB2BGR)
self.video_writer_raw.write(raw_bgr)
# Update status with FPS info
self.status_label.setText(f"Status: Running - FPS: {fps:.1f}, FNormal: {fn:.2f}, FShearX: {fx:.2f}, FShearY: {fy:.2f}")
self.status_label.setStyleSheet("""
font-size: 13px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #27ae60;
padding: 8px;
background-color: #d5f4e6;
border-radius: 5px;
""")
# Update plots with force data
if self.start_time is not None:
current_time = time.time() - self.start_time
self.update_plots(fn, fx, fy, current_time)
except Exception as e:
# Sensor read failed
self.stop_camera()
self.status_label.setText(f"Status: Sensor Error - {str(e)}")
self.status_label.setStyleSheet("""
font-size: 14px;
font-weight: bold;
font-family: 'Segoe UI', Arial, sans-serif;
color: #f39c12;
padding: 10px;
background-color: #fef5e7;
border-radius: 5px;
""")
def closeEvent(self, event):
self.stop_camera()
event.accept()
def main():
app = QApplication(sys.argv)
window = CameraGUI()
window.show()
sys.exit(app.exec_())
if __name__ == '__main__':
main()