diff --git a/modules/zividsamples/gui/calibration/calibration_buttons_widget.py b/modules/zividsamples/gui/calibration/calibration_buttons_widget.py index 27978c69..d895841d 100644 --- a/modules/zividsamples/gui/calibration/calibration_buttons_widget.py +++ b/modules/zividsamples/gui/calibration/calibration_buttons_widget.py @@ -1,7 +1,16 @@ from typing import List, Optional from PyQt5.QtCore import pyqtSignal -from PyQt5.QtWidgets import QApplication, QCheckBox, QGroupBox, QHBoxLayout, QPushButton, QWidget +from PyQt5.QtWidgets import ( + QApplication, + QCheckBox, + QGroupBox, + QHBoxLayout, + QLabel, + QPushButton, + QVBoxLayout, + QWidget, +) class HandEyeCalibrationButtonsWidget(QWidget): @@ -11,38 +20,50 @@ class HandEyeCalibrationButtonsWidget(QWidget): def __init__(self, parent: Optional[QWidget] = None) -> None: super().__init__(parent) - # Define buttons self.calibrate_button = QPushButton("Calibrate") self.calibrate_button.setObjectName("HandEye-calibrate_button") self.use_fixed_objects_checkbox = QCheckBox("Fixed Objects - for low DOF systems") self.use_fixed_objects_checkbox.setObjectName("HandEye-fixed_objects_checkbox") + self._status_label = QLabel() + self._status_label.setWordWrap(True) + self._status_label.setVisible(False) - # Connect signals self.calibrate_button.clicked.connect(self.on_calibrate_button_clicked) self.use_fixed_objects_checkbox.toggled.connect(self.on_use_fixed_objects_toggled) - # Add buttons to layout + buttons_row = QHBoxLayout() + buttons_row.addWidget(self.calibrate_button) + buttons_row.addWidget(self.use_fixed_objects_checkbox) + calibrate_group_box = QGroupBox("Calibrate") - calibrate_group_box_layout = QHBoxLayout() + calibrate_group_box_layout = QVBoxLayout() calibrate_group_box.setLayout(calibrate_group_box_layout) + calibrate_group_box_layout.addLayout(buttons_row) + calibrate_group_box_layout.addWidget(self._status_label) - calibrate_group_box_layout.addWidget(self.calibrate_button) - calibrate_group_box_layout.addWidget(self.use_fixed_objects_checkbox) - - buttons_layout = QHBoxLayout() - buttons_layout.addWidget(calibrate_group_box) - - self.setLayout(buttons_layout) + outer_layout = QHBoxLayout() + outer_layout.addWidget(calibrate_group_box) + self.setLayout(outer_layout) def on_calibrate_button_clicked(self) -> None: - self.calibrate_button.setStyleSheet("background-color: yellow;") + self.calibrate_button.setEnabled(False) + self.calibrate_button.setStyleSheet("background-color: #C07800; color: white;") QApplication.processEvents() self.calibrate_button_clicked.emit() self.calibrate_button.setStyleSheet("") + self.calibrate_button.setEnabled(True) def on_use_fixed_objects_toggled(self, checked: bool) -> None: self.use_fixed_objects_toggled.emit(checked) + def set_calibration_status(self, style: str, message: str) -> None: + self._status_label.setStyleSheet(style) + self._status_label.setText(message) + self._status_label.setVisible(True) + + def hide_calibration_status(self) -> None: + self._status_label.setVisible(False) + def disable_buttons(self) -> None: self.calibrate_button.setEnabled(False) diff --git a/modules/zividsamples/gui/calibration/hand_eye_calibration_gui.py b/modules/zividsamples/gui/calibration/hand_eye_calibration_gui.py index c1d07839..3266a34f 100644 --- a/modules/zividsamples/gui/calibration/hand_eye_calibration_gui.py +++ b/modules/zividsamples/gui/calibration/hand_eye_calibration_gui.py @@ -36,7 +36,7 @@ from zividsamples.gui.wizard.robot_configuration import RobotConfiguration from zividsamples.gui.wizard.rotation_format_configuration import RotationInformation from zividsamples.gui.wizard.settings_selector import SettingsPixelMappingIntrinsics -from zividsamples.save_load_transformation_matrix import save_transformation_matrix +from zividsamples.save_load_transformation_matrix import load_transformation_matrix, save_transformation_matrix from zividsamples.save_residuals import save_residuals from zividsamples.transformation_matrix import TransformationMatrix @@ -111,7 +111,6 @@ def create_widgets(self, initial_rotation_information: RotationInformation) -> N hand_eye_configuration=self.hand_eye_configuration ) self.pose_pair_selection_widget = PosePairSelectionWidget(directory=self.data_directory) - self.pose_pair_selection_widget.setVisible(False) self.hand_eye_calibration_buttons = HandEyeCalibrationButtonsWidget() self.hand_eye_calibration_buttons.calibrate_button.setEnabled(False) self.hand_eye_calibration_buttons.setObjectName("HE-Calibration-hand_eye_calibration_buttons") @@ -147,6 +146,7 @@ def connect_signals(self) -> None: self.pose_pair_selection_widget.pose_pair_clicked.connect(self.on_pose_pair_clicked) self.pose_pair_selection_widget.pose_pairs_updated.connect(self.on_pose_pairs_update) self.pose_pair_selection_widget.loading_finished.connect(self._on_pose_pairs_loading_finished) + self.pose_pair_selection_widget.load_from_disk_requested.connect(self.on_load_calibration_data_button_clicked) def _on_pose_pairs_loading_finished(self) -> None: self.loading_finished.emit() @@ -227,8 +227,13 @@ def _resolve_config_from_saved_session( def on_pending_changes(self) -> None: self.pose_pair_selection_widget.clear() self.pose_pair_selection_widget.set_directory(self.data_directory) - if not self.data_directory_has_data(): - return + hand_eye_transform_path = self.data_directory / "hand_eye_transform.yaml" + if hand_eye_transform_path.exists(): + hand_eye_transform = load_transformation_matrix(hand_eye_transform_path) + if not hand_eye_transform.is_identity(): + self.calibration_finished.emit(hand_eye_transform) + + def on_load_calibration_data_button_clicked(self) -> None: calibration_object = self.hand_eye_configuration.calibration_object marker_configuration = self.marker_configuration if self.session_info is not None: @@ -323,6 +328,7 @@ def on_start_auto_run(self) -> bool: ) if reply == QMessageBox.Yes: self.pose_pair_selection_widget.clear() + self.hand_eye_calibration_buttons.hide_calibration_status() return True return False @@ -335,7 +341,8 @@ def on_pose_pairs_update(self, number_of_pose_pairs: int) -> None: self.hand_eye_calibration_buttons.calibrate_button.setEnabled( number_of_pose_pairs >= self.minimum_pose_pairs_for_calibration ) - self.pose_pair_selection_widget.setVisible(number_of_pose_pairs > 0) + if number_of_pose_pairs == 0: + self.hand_eye_calibration_buttons.hide_calibration_status() def process_capture(self, frame: zivid.Frame, rgba: NDArray[Shape["N, M, 4"], UInt8], settings: SettingsPixelMappingIntrinsics) -> None: # type: ignore try: @@ -411,6 +418,35 @@ def on_use_fixed_objects_toggled(self, checked: bool) -> None: self.hand_eye_calibration_buttons.use_fixed_objects_checkbox.setChecked(self.fixed_objects.has_data()) self.fixed_objects = updated_fixed_objects + def _update_calibration_status(self, calibration_result) -> None: + try: + status = calibration_result.status() + except RuntimeError: + self.hand_eye_calibration_buttons.set_calibration_status( + "background-color: #555555; color: white; padding: 6px; border-radius: 4px;", + "Calibration status not available. Feature available from Zivid SDK version 2.18.", + ) + return + status_config = { + zivid.calibration.HandEyeStatus.ok: ( + "background-color: #2d7a2d; color: white; padding: 6px; border-radius: 4px;", + "Calibration OK — open the Verify section to validate the accuracy of the hand-eye transform before deployment.", + ), + zivid.calibration.HandEyeStatus.insufficient_motion: ( + "background-color: #b86e00; color: white; padding: 6px; border-radius: 4px;", + "Insufficient motion: capture more poses with greater variation in position and rotation across all degrees of freedom.", + ), + zivid.calibration.HandEyeStatus.insufficient_data_quality: ( + "background-color: #a02020; color: white; padding: 6px; border-radius: 4px;", + "Poor data quality: check lighting, exposure, focus, and that the robot and calibration object remain stationary during capture.", + ), + } + style, message = status_config.get( + status, + ("background-color: gray; color: white; padding: 6px;", f"Status: {status}"), + ) + self.hand_eye_calibration_buttons.set_calibration_status(style, message) + def on_calibrate_button_clicked(self) -> None: self._calibrate(save_to_disk=True) @@ -442,9 +478,12 @@ def _calibrate(self, save_to_disk: bool = True) -> None: save_residuals(calibration_result.residuals(), hand_eye_residuals_path) self.pose_pair_selection_widget.set_residuals(calibration_result.residuals()) + self._update_calibration_status(calibration_result) if save_to_disk: hand_eye_transform_path = self.data_directory / "hand_eye_transform.yaml" - show_yaml_dialog(hand_eye_transform_path, "Hand Eye Calibration Transform") + show_yaml_dialog( + hand_eye_transform_path, "Hand Eye Calibration Transform", close_button_label="Continue" + ) self.update_instructions( has_detection_result=False, robot_pose_confirmed=False, @@ -452,10 +491,11 @@ def _calibrate(self, save_to_disk: bool = True) -> None: ) self.calibration_finished.emit(hand_eye_transformation_matrix) else: - raise RuntimeError() + raise RuntimeError("Calibration returned an invalid result") except RuntimeError as ex: print(f"Failed to calibrate eye to hand: {ex}") QMessageBox.critical(self, "Hand-Eye Calibration Error", str(ex)) + self.hand_eye_calibration_buttons.hide_calibration_status() self.calibration_finished.emit(TransformationMatrix()) def confirm_robot_pose(self, confirmed: bool = True) -> None: @@ -466,6 +506,14 @@ def confirm_robot_pose(self, confirmed: bool = True) -> None: ) def on_confirm_robot_pose_button_clicked(self, checked: bool) -> None: + rotation_warning = self.robot_pose_widget.check_rotation_warning() if checked else None + if rotation_warning: + QMessageBox.warning( + self, "Invalid Rotation", f"Cannot uniquely determine rotation
{rotation_warning}" + ) + self.confirm_robot_pose_button.setChecked(False) + self.confirm_robot_pose(False) + return self.confirm_robot_pose(checked) def on_robot_pose_manually_updated(self) -> None: diff --git a/modules/zividsamples/gui/calibration/pose_pair_selection_widget.py b/modules/zividsamples/gui/calibration/pose_pair_selection_widget.py index 37a78695..00c0b6cb 100644 --- a/modules/zividsamples/gui/calibration/pose_pair_selection_widget.py +++ b/modules/zividsamples/gui/calibration/pose_pair_selection_widget.py @@ -25,6 +25,7 @@ QWidget, ) from zivid.experimental import PixelMapping, calibration +from zividsamples.gui.qt_application import ZIVID_BLUE_BUTTON_STYLE from zividsamples.gui.widgets.cv2_handler import CV2Handler from zividsamples.gui.wizard.data_directory import SessionInfo from zividsamples.gui.wizard.hand_eye_configuration import CalibrationObject @@ -171,6 +172,9 @@ def update_information(self, pose_pair: PosePair, save_to_disk: bool = True) -> ) self.robot_pose_label.setText(self._translation_to_string(self.pose_pair.robot_pose.translation)) + def release_frame(self) -> None: + self.pose_pair.camera_frame = None # type: ignore[assignment] + def camera_pose_yaml_text(self) -> str: if self.pose_pair.camera_pose is None: return "" @@ -330,6 +334,7 @@ class PosePairSelectionWidget(QWidget): pose_pair_clicked = pyqtSignal(PosePair) pose_pairs_updated = pyqtSignal(int) loading_finished = pyqtSignal() + load_from_disk_requested = pyqtSignal() def __init__(self, directory: Path, parent: Optional[QWidget] = None) -> None: super().__init__(parent) @@ -361,6 +366,9 @@ def create_widgets(self) -> None: self.pose_pair_scrollable_area.setVerticalScrollBarPolicy(Qt.ScrollBarAsNeeded) self.pose_pair_scrollable_area.setWidget(self.pose_pair_container) + self.load_from_disk_button = QPushButton() + self.load_from_disk_button.setStyleSheet(ZIVID_BLUE_BUTTON_STYLE) + self.load_from_disk_button.setVisible(False) self.clear_pose_pairs_button = QPushButton("Clear") def setup_layout(self) -> None: @@ -370,6 +378,7 @@ def setup_layout(self) -> None: self.pose_pairs_layout = QVBoxLayout(self.pose_pair_container) self.pose_pairs_layout.setAlignment(Qt.AlignTop) + self.pose_pairs_layout.addWidget(self.load_from_disk_button) button_layout = QHBoxLayout() button_layout.addWidget(self.clear_pose_pairs_button) @@ -384,9 +393,34 @@ def setup_layout(self) -> None: def connect_signals(self) -> None: self.clear_pose_pairs_button.clicked.connect(self.on_clear_button_clicked) + self.load_from_disk_button.clicked.connect(self._on_load_from_disk_clicked) + + def _on_load_from_disk_clicked(self): + self.load_from_disk_button.setVisible(False) + self.load_from_disk_requested.emit() + + def _available_pose_ids(self) -> List[int]: + pose_ids = [] + for path in self.directory.glob("robot_pose_*.yaml"): + pose_id_str = path.stem[len("robot_pose_") :] + if pose_id_str.isdigit() and (self.directory / f"calibration_object_pose_{pose_id_str}.zdf").exists(): + pose_ids.append(int(pose_id_str)) + return sorted(pose_ids) + + def _update_load_button(self): + if self.pose_pair_widgets: + self.load_from_disk_button.setVisible(False) + return + available = len(self._available_pose_ids()) + if available > 0: + self.load_from_disk_button.setText(f"Load {available} pose pair(s) from disk") + self.load_from_disk_button.setVisible(True) + else: + self.load_from_disk_button.setVisible(False) def set_directory(self, directory: Path) -> None: self.directory = directory + self._update_load_button() def load_pose_pairs(self, calibration_object: CalibrationObject, marker_configuration: MarkerConfiguration) -> None: if len(self.pose_pair_widgets) > 0: @@ -407,15 +441,12 @@ def load_pose_pairs(self, calibration_object: CalibrationObject, marker_configur self._last_operation_was_reprocess = False self._set_loaded_from_disk(False) self._clear_layout(self.pose_pairs_layout) + self.pose_pairs_layout.addWidget(self.load_from_disk_button) + self.load_from_disk_button.setVisible(False) self.pose_pair_widgets.clear() self.pose_pairs_updated.emit(0) - pose_ids = [ - pid - for pid in range(20) - if (self.directory / f"robot_pose_{pid}.yaml").exists() - and (self.directory / f"calibration_object_pose_{pid}.zdf").exists() - ] + pose_ids = self._available_pose_ids() if not pose_ids: return @@ -426,7 +457,6 @@ def load_pose_pairs(self, calibration_object: CalibrationObject, marker_configur self.pose_pairs_group_box.setStyleSheet(r"QGroupBox {border: 2px solid yellow;}") self.pose_pairs_group_box.setTitle("Pose Pairs (loading...)") - self.pose_pairs_group_box.setVisible(True) thread = QThread() worker = _PosePairLoadWorker( @@ -473,7 +503,7 @@ def _on_pose_pair_loaded(self, generation: int, poseID: int, pose_pair: PosePair poseID=poseID, directory=self.directory, pose_pair=pose_pair, save_to_disk=False ) pose_pair_widget.clickable_labels.clicked.connect(lambda: self.on_pose_pair_widget_clicked(pose_pair_widget)) - self.pose_pairs_layout.insertWidget(pose_pair_widget.poseID, pose_pair_widget) + self.pose_pairs_layout.insertWidget(pose_pair_widget.poseID + 1, pose_pair_widget) self.pose_pair_widgets[poseID] = pose_pair_widget self.pose_pairs_updated.emit(len(self.pose_pair_widgets)) self.pose_pair_clicked.emit(pose_pair_widget.pose_pair) @@ -546,7 +576,23 @@ def on_pose_pair_widget_clicked(self, pose_pair_widget: PosePairWidget) -> None: self.pose_pair_clicked.emit(pose_pair_widget.pose_pair) def on_clear_button_clicked(self) -> None: - self.clear() + reply = QMessageBox.question( + self, + "Delete All Pose Pairs", + "This will permanently delete all pose pair files from disk. This action cannot be undone.", + QMessageBox.Yes | QMessageBox.No, + ) + if reply == QMessageBox.Yes: + for widget in self.pose_pair_widgets.values(): + for path in [ + widget.robot_pose_yaml_path, + widget.camera_frame_path, + widget.camera_image_path, + widget.camera_pose_yaml_path, + ]: + if path.exists(): + path.unlink() + self.clear() def create_title_row(self) -> QHBoxLayout: checkbox_and_poseID_spacer = QSpacerItem(75, 40, QSizePolicy.Fixed, QSizePolicy.Minimum) @@ -573,7 +619,10 @@ def loaded_from_disk(self) -> bool: def _set_loaded_from_disk(self, value: bool) -> None: self._loaded_from_disk = value - self.clear_pose_pairs_button.setVisible(not value) + self._update_clear_button_visibility() + + def _update_clear_button_visibility(self) -> None: + self.clear_pose_pairs_button.setVisible(bool(self.pose_pair_widgets)) def add_pose_pair(self, pose_pair: PosePair) -> Optional[PosePairWidget]: if self.is_loading(): @@ -594,9 +643,10 @@ def add_pose_pair(self, pose_pair: PosePair) -> Optional[PosePairWidget]: pose_pair_widget = PosePairWidget(poseID=poseID, directory=self.directory, pose_pair=pose_pair) pose_pair_widget.clickable_labels.clicked.connect(lambda: self.on_pose_pair_widget_clicked(pose_pair_widget)) - self.pose_pairs_layout.insertWidget(pose_pair_widget.poseID, pose_pair_widget) + self.pose_pairs_layout.insertWidget(pose_pair_widget.poseID + 1, pose_pair_widget) self.pose_pair_widgets[poseID] = pose_pair_widget self.pose_pairs_updated.emit(len(self.pose_pair_widgets)) + self._update_clear_button_visibility() return pose_pair_widget def get_current_poseID(self) -> int: @@ -647,6 +697,10 @@ def _clear_layout(self, layout: QLayout) -> None: item = layout.takeAt(0) widget = item.widget() if widget: + if widget is self.load_from_disk_button: + continue + if isinstance(widget, PosePairWidget): + widget.release_frame() widget.deleteLater() sublayout = item.layout() if sublayout: @@ -656,5 +710,7 @@ def clear(self) -> None: self.cancel_loading() self._set_loaded_from_disk(False) self._clear_layout(self.pose_pairs_layout) + self.pose_pairs_layout.addWidget(self.load_from_disk_button) self.pose_pair_widgets.clear() self.pose_pairs_updated.emit(0) + self._update_load_button() diff --git a/modules/zividsamples/gui/hand_eye_app.py b/modules/zividsamples/gui/hand_eye_app.py index da8ee035..ed57117d 100644 --- a/modules/zividsamples/gui/hand_eye_app.py +++ b/modules/zividsamples/gui/hand_eye_app.py @@ -8,10 +8,11 @@ import functools import time from enum import Enum +from pathlib import Path from typing import Dict, List, Optional import zivid -from PyQt5.QtCore import Qt, QTimer +from PyQt5.QtCore import QSettings, Qt, QTimer from PyQt5.QtGui import QCloseEvent, QKeyEvent from PyQt5.QtWidgets import ( QAction, @@ -32,6 +33,7 @@ from zividsamples.gui.verification.touch_gui import TouchGUI from zividsamples.gui.widgets.camera_buttons_widget import CameraButtonsWidget from zividsamples.gui.widgets.live_2d_widget import Live2DWidget +from zividsamples.gui.widgets.show_yaml_dialog import show_yaml_dialog from zividsamples.gui.widgets.tutorial_widget import TutorialWidget from zividsamples.gui.wizard.camera_selection import select_camera from zividsamples.gui.wizard.data_directory import DataDirectoryManager @@ -43,6 +45,7 @@ from zividsamples.gui.wizard.robot_configuration import RobotConfiguration, select_robot_configuration from zividsamples.gui.wizard.rotation_format_configuration import RotationInformation, select_rotation_format from zividsamples.gui.wizard.settings_selector import SettingsForHandEyeGUI, select_settings_for_hand_eye +from zividsamples.save_load_transformation_matrix import load_transformation_matrix from zividsamples.transformation_matrix import TransformationMatrix @@ -92,6 +95,11 @@ class HandEyeAppBase(QMainWindow): select_robot_configuration_action: QAction select_rotation_format_action: QAction toggle_advanced_view_action: QAction + reset_to_defaults_action: QAction + load_he_transform_action: QAction + view_he_transform_action: QAction + current_he_transform: Optional[TransformationMatrix] + current_he_transform_path: Optional[Path] _SESSION_DATA_LOADED_TOOLTIP = "Start a new session to capture new Hand Eye Calibration data" @@ -120,6 +128,7 @@ def initialize(self) -> None: self.connect_signals() self.current_tab_widget = self.hand_eye_calibration_gui self.on_instructions_updated() + self._try_load_he_transform_from_session() for widget in self.tab_widgets: self.data_directory_manager.register_tab_widget(widget, widget.objectName()) @@ -136,6 +145,8 @@ def static_configuration(self) -> None: self.projection_handle = None self.last_frame = None self.common_instructions = {} + self.current_he_transform = None + self.current_he_transform_path = None def update_tab_order(self) -> None: tab_widgets = ( @@ -158,6 +169,8 @@ def create_toolbar(self) -> None: self.save_frame_action.setToolTip("Save the last captured frame") self.save_frame_action.setShortcut("Ctrl+S") file_menu.addAction(self.save_frame_action) + self.load_he_transform_action = QAction("Load HE Transform", self) + file_menu.addAction(self.load_he_transform_action) close_action = QAction("Close", self) close_action.triggered.connect(self.close) file_menu.addAction(close_action) @@ -181,10 +194,17 @@ def create_toolbar(self) -> None: self.select_rotation_format_action = QAction("Rotation Format", self) robot_submenu.addAction(self.select_rotation_format_action) + config_menu.addSeparator() + self.reset_to_defaults_action = QAction("Reset to Defaults", self) + config_menu.addAction(self.reset_to_defaults_action) + view_menu = self.menuBar().addMenu("View") self.toggle_advanced_view_action = QAction("Advanced", self, checkable=True) self.toggle_advanced_view_action.setChecked(False) view_menu.addAction(self.toggle_advanced_view_action) + self.view_he_transform_action = QAction("HE Transform...", self) + self.view_he_transform_action.setEnabled(False) + view_menu.addAction(self.view_he_transform_action) def connect_signals(self) -> None: self.live2d_widget.camera_disconnected.connect(self.on_camera_disconnected) @@ -194,12 +214,16 @@ def connect_signals(self) -> None: self.directory_load_session_action.triggered.connect(self.on_data_directory_load_session_action_triggered) self.directory_new_session_action.triggered.connect(self.on_data_directory_new_session_action_triggered) self.save_frame_action.triggered.connect(self.on_save_last_frame_action_triggered) + self.load_he_transform_action.triggered.connect(self.on_load_he_transform_action_triggered) self.select_hand_eye_configuration_action.triggered.connect(self.hand_eye_configuration_action_triggered) self.select_marker_configuration_action.triggered.connect(self.on_select_marker_configuration) self.select_hand_eye_settings_action.triggered.connect(self.on_select_hand_eye_settings_action_triggered) self.select_rotation_format_action.triggered.connect(self.on_select_rotation_format) self.set_fixed_objects_action.triggered.connect(self.on_select_fixed_objects_action_triggered) self.toggle_advanced_view_action.triggered.connect(self.on_toggle_advanced_view_action_triggered) + self.view_he_transform_action.triggered.connect(self.on_view_he_transform_action_triggered) + self.tutorial_widget.view_he_transform_requested.connect(self.on_view_he_transform_action_triggered) + self.reset_to_defaults_action.triggered.connect(self.on_reset_to_defaults) self.select_robot_configuration_action.triggered.connect(self.on_select_robot_configuration_action_triggered) self.camera_buttons.capture_button_clicked.connect(self.on_capture_button_clicked) self.camera_buttons.connect_button_clicked.connect(self.on_connect_button_clicked) @@ -346,9 +370,38 @@ def on_calibration_finished(self, transformation_matrix: TransformationMatrix) - if self.robot_configuration.can_control() and self.auto_run_state == AutoRunState.CALIBRATING: self.finish_auto_run() if not transformation_matrix.is_identity(): - self.hand_eye_verification_gui.set_hand_eye_transformation_matrix(transformation_matrix) - self.touch_gui.set_hand_eye_transformation_matrix(transformation_matrix) - self.stitch_gui.set_hand_eye_transformation_matrix(transformation_matrix) + he_transform_path = self.hand_eye_calibration_gui.data_directory / "hand_eye_transform.yaml" + self._set_he_transform(transformation_matrix, he_transform_path) + + def _set_he_transform(self, transformation_matrix: TransformationMatrix, path: Path) -> None: + self.current_he_transform = transformation_matrix + self.current_he_transform_path = path + self.hand_eye_verification_gui.set_hand_eye_transformation_matrix(transformation_matrix) + self.touch_gui.set_hand_eye_transformation_matrix(transformation_matrix) + self.stitch_gui.set_hand_eye_transformation_matrix(transformation_matrix) + + def _try_load_he_transform_from_session(self) -> None: + he_transform_path = self.hand_eye_calibration_gui.data_directory / "hand_eye_transform.yaml" + if he_transform_path.exists(): + transformation_matrix = load_transformation_matrix(he_transform_path) + if not transformation_matrix.is_identity(): + self._set_he_transform(transformation_matrix, he_transform_path) + + def on_load_he_transform_action_triggered(self) -> None: + file_name = QFileDialog.getOpenFileName( + self, + caption="Load HE Transform", + filter="YAML files (*.yaml *.yml)", + )[0] + if file_name: + path = Path(file_name) + transformation_matrix = load_transformation_matrix(path) + if not transformation_matrix.is_identity(): + self._set_he_transform(transformation_matrix, path) + else: + QMessageBox.warning( + self, "Load HE Transform", "The selected file contains an identity matrix or could not be loaded." + ) def on_auto_run_toggled(self) -> None: if self.auto_run_state == AutoRunState.INACTIVE: @@ -387,6 +440,24 @@ def on_instructions_updated(self) -> None: self.tutorial_widget.add_steps(self.current_tab_widget.instruction_steps) self.tutorial_widget.set_description(self.current_tab_widget.description) self.tutorial_widget.update_text() + self._update_he_transform_status_ui() + + def _update_he_transform_status_ui(self) -> None: + is_calibrate = self.current_tab_widget == self.hand_eye_calibration_gui + is_verify = self.current_tab_widget in [ + self.hand_eye_verification_gui, + self.touch_gui, + self.stitch_gui, + ] + show = is_calibrate or is_verify + loaded = self.current_he_transform is not None + self.tutorial_widget.set_he_transform_status(show=show, loaded=loaded, mandatory=is_verify) + self.view_he_transform_action.setEnabled(loaded and show) + + def on_view_he_transform_action_triggered(self) -> None: + if self.current_he_transform_path is None: + return + show_yaml_dialog(self.current_he_transform_path, "HE Transform Matrix") def on_robot_connected(self) -> None: self.setup_instructions() @@ -411,8 +482,7 @@ def on_actual_pose_updated(self, robot_target: RobotTarget) -> None: if self.current_tab_widget == self.hand_eye_calibration_gui: self.on_capture_button_clicked() elif self.current_tab_widget == self.hand_eye_verification_gui: - time.sleep(2) - self.robot_control_widget.on_move_to_next_target(blocking=False) + QTimer.singleShot(2000, lambda: self.robot_control_widget.on_move_to_next_target(blocking=False)) elif self.auto_run_state != AutoRunState.INACTIVE: error_message = ( f"Expected to be home now, but arrived at {robot_target.name} {robot_target.pose}" @@ -442,8 +512,6 @@ def update_projection(self, project: bool = True) -> None: self.robot_control_widget.enable_disable_buttons(auto_run=True, touch=False) error_msg = None try: - if self.camera is None: - raise RuntimeError("No camera connected.") try: projector_image = self.current_tab_widget.generate_projector_image(self.camera) except ValueError as ex: @@ -541,6 +609,7 @@ def on_data_directory_load_session_action_triggered(self) -> None: widget.notify_current_tab(self.current_tab_widget) if self.current_tab_widget.is_loading(): self.camera_buttons.disable_buttons() + self._try_load_he_transform_from_session() def on_data_directory_new_session_action_triggered(self) -> None: self.data_directory_manager.start_new_session() @@ -558,7 +627,8 @@ def on_save_last_frame_action_triggered(self) -> None: directory=self.current_tab_widget.data_directory.joinpath("last_capture.zdf").resolve().as_posix(), filter="Zivid Frame (*.zdf *.ply *.pcd *.xyz)", )[0] - self.last_frame.save(file_name) + if file_name: + self.last_frame.save(file_name) else: QMessageBox.warning(self, "Save Capture", "No capture to save.") @@ -591,10 +661,23 @@ def on_select_fixed_objects_action_triggered(self) -> None: def on_toggle_advanced_view_action_triggered(self, checked: bool) -> None: self.hand_eye_calibration_gui.toggle_advanced_view(checked) self.hand_eye_verification_gui.toggle_advanced_view(checked) + self.touch_gui.toggle_advanced_view(checked) + + def on_reset_to_defaults(self) -> None: + reply = QMessageBox.question( + self, + "Reset to Defaults", + "This will reset all configuration to defaults.\nThe application will close. Continue?", + QMessageBox.Yes | QMessageBox.No, + QMessageBox.No, + ) + if reply == QMessageBox.Yes: + QSettings("Zivid", "HandEyeGUI").clear() + self.close() def on_select_robot_configuration_action_triggered(self) -> None: selected_robot = select_robot_configuration(self.robot_configuration, show_anyway=True) - if self.robot_configuration.robot_type == selected_robot: + if self.robot_configuration == selected_robot: return self.robot_configuration = selected_robot self.setup_instructions() diff --git a/modules/zividsamples/gui/preparation/infield_correction_data_selection_widget.py b/modules/zividsamples/gui/preparation/infield_correction_data_selection_widget.py index f87df6b7..375c28c2 100644 --- a/modules/zividsamples/gui/preparation/infield_correction_data_selection_widget.py +++ b/modules/zividsamples/gui/preparation/infield_correction_data_selection_widget.py @@ -26,6 +26,7 @@ QVBoxLayout, QWidget, ) +from zividsamples.gui.qt_application import ZIVID_BLUE_BUTTON_STYLE from zividsamples.gui.widgets.cv2_handler import CV2Handler from zividsamples.gui.widgets.fov import CameraFOV, FOVThresholds, PointsOfInterest, PointsOfInterest2D, PositionInFOV @@ -498,11 +499,13 @@ def calculate_trapezoidal_corners(self, valid_mask: np.ndarray) -> np.ndarray: class InfieldCorrectionInputWidget(QWidget): infield_correction_input_data: Union[InfieldCorrectionInputData, InfieldCorrectionInputDataCore] + # pylint: disable=too-many-positional-arguments def __init__( self, poseID: int, directory: Path, infield_correction_input_data: Union[InfieldCorrectionInputData, InfieldCorrectionInputDataCore], + save_to_disk: bool = True, parent: Optional[QWidget] = None, ) -> None: super().__init__(parent) @@ -534,19 +537,25 @@ def __init__( pose_pair_layout.addWidget(self.clickable_labels) self.setLayout(pose_pair_layout) - self.update_information(infield_correction_input_data) + self.update_information(infield_correction_input_data, save_to_disk=save_to_disk) def update_information( - self, infield_correction_input_data: Union[InfieldCorrectionInputData, InfieldCorrectionInputDataCore] + self, + infield_correction_input_data: Union[InfieldCorrectionInputData, InfieldCorrectionInputDataCore], + save_to_disk: bool = True, ) -> None: self.infield_correction_input_data = infield_correction_input_data - self.infield_correction_input_data.save_data(self.directory / f"infield_calibration_input_{self.poseID}") + if save_to_disk: + self.infield_correction_input_data.save_data(self.directory / f"infield_calibration_input_{self.poseID}") self.update_gui() def update_gui(self) -> None: self.position_in_fov_label.setText(str(self.infield_correction_input_data.position_in_fov)) self.trueness_label.setText(self.infield_correction_input_data.local_trueness_as_string()) + def release_frame(self) -> None: + self.infield_correction_input_data.camera_frame = None # type: ignore[assignment] + class _InfieldLoadWorker(QObject): """Loads infield correction data from disk in a background thread.""" @@ -606,6 +615,9 @@ def create_widgets(self) -> None: self.infield_input_scrollable_area.setVerticalScrollBarPolicy(Qt.ScrollBarAsNeeded) self.infield_input_scrollable_area.setWidget(self.infield_input_container) + self.load_from_disk_button = QPushButton() + self.load_from_disk_button.setStyleSheet(ZIVID_BLUE_BUTTON_STYLE) + self.load_from_disk_button.setVisible(False) self.remove_last_infield_input_button = QPushButton("Remove Last") self.remove_last_infield_input_button.setEnabled(False) self.clear_infield_input_button = QPushButton("Clear") @@ -617,6 +629,7 @@ def setup_layout(self) -> None: self.infield_input_layout = QVBoxLayout(self.infield_input_container) self.infield_input_layout.setAlignment(Qt.AlignTop) + self.infield_input_layout.addWidget(self.load_from_disk_button) button_layout = QHBoxLayout() button_layout.addWidget(self.remove_last_infield_input_button) @@ -633,6 +646,28 @@ def setup_layout(self) -> None: def connect_signals(self) -> None: self.clear_infield_input_button.clicked.connect(self.on_clear_button_clicked) self.remove_last_infield_input_button.clicked.connect(self.on_remove_last_infield_input_button_clicked) + self.load_from_disk_button.clicked.connect(self._on_load_from_disk_clicked) + + def _on_load_from_disk_clicked(self): + self.load_from_disk_button.setVisible(False) + existing_files = sorted(self.data_directory.glob("infield_calibration_input_*.json")) + if existing_files: + self.load_existing_data(existing_files) + + def set_directory(self, directory: Path): + self.data_directory = directory + self._update_load_button() + + def _update_load_button(self): + if self.infield_input_data_widgets: + self.load_from_disk_button.setVisible(False) + return + existing_files = list(self.data_directory.glob("infield_calibration_input_*.json")) + if existing_files: + self.load_from_disk_button.setText(f"Load {len(existing_files)} infield capture(s) from disk") + self.load_from_disk_button.setVisible(True) + else: + self.load_from_disk_button.setVisible(False) def update_data_directory(self, data_directory: Path) -> None: existing_files = list(data_directory.glob("infield_calibration_input_*.json")) @@ -670,7 +705,6 @@ def load_existing_data(self, existing_files: List[Path]) -> None: self.infield_input_group_box.setStyleSheet(r"QGroupBox {border: 2px solid yellow;}") self.infield_input_group_box.setTitle("Infield Correction Input Data (loading...)") - self.setVisible(True) self._loader_thread = QThread() self._loader_worker = _InfieldLoadWorker( @@ -697,7 +731,20 @@ def cancel_loading(self) -> None: def _on_item_loaded(self, generation: int, data: InfieldCorrectionInputDataCore) -> None: if generation != self._load_generation: return - self.add_infield_input_data(data) + poseID = len(self.infield_input_data_widgets) + infield_correction_input_widget = InfieldCorrectionInputWidget( + poseID=poseID, directory=self.data_directory, infield_correction_input_data=data, save_to_disk=False + ) + infield_correction_input_widget.clickable_labels.clicked.connect( + lambda: self.on_infield_input_data_widget_clicked(infield_correction_input_widget) + ) + infield_correction_input_widget.selected_checkbox.stateChanged.connect( + self.on_infield_input_data_widget_selection_box_clicked + ) + self.infield_input_layout.insertWidget(poseID, infield_correction_input_widget) + self.infield_input_data_widgets[poseID] = infield_correction_input_widget + self.infield_input_data_updated.emit(self.can_calculate_correction()) + self.remove_last_infield_input_button.setEnabled(True) def _on_loading_finished(self, generation: int) -> None: if generation != self._load_generation: @@ -740,7 +787,6 @@ def create_title_row(self) -> QHBoxLayout: return title_layout def show_as_busy(self, active: bool) -> None: - self.setVisible(active or len(self.infield_input_data_widgets) > 0) self.infield_input_group_box.setStyleSheet(r"QGroupBox {border: 2px solid yellow;}" if active else "") self.infield_input_group_box.setTitle( "Infield Correction Input Data (processing...)" if active else "Infield Correction Input Data" @@ -824,6 +870,10 @@ def _clear_layout(self, layout: QLayout) -> None: item = layout.takeAt(0) widget = item.widget() if widget: + if widget is self.load_from_disk_button: + continue + if isinstance(widget, InfieldCorrectionInputWidget): + widget.release_frame() widget.deleteLater() sublayout = item.layout() if sublayout: @@ -832,19 +882,19 @@ def _clear_layout(self, layout: QLayout) -> None: def clear_gui(self) -> None: self.cancel_loading() self._clear_layout(self.infield_input_layout) + self.infield_input_layout.addWidget(self.load_from_disk_button) self.infield_input_data_widgets.clear() self.infield_input_data_updated.emit(self.can_calculate_correction()) self.remove_last_infield_input_button.setEnabled(False) - self.setVisible(False) + self._update_load_button() def clear_all(self) -> None: - self.clear_gui() if self.infield_input_data_widgets: reply = QMessageBox.question( self, "Clear All Infield Input Data", - "This will remove all loaded Infield Correction Input Data. Do you want to proceed?" - "Note that while it is possible to review the infield session, it is not possible to" + "This will remove all loaded Infield Correction Input Data. Do you want to proceed? " + "Note that while it is possible to review the infield session, it is not possible to " "recalculate and apply infield correction from a previous session.", QMessageBox.Yes | QMessageBox.No, ) diff --git a/modules/zividsamples/gui/preparation/infield_correction_gui.py b/modules/zividsamples/gui/preparation/infield_correction_gui.py index 9b56d211..aec8d8e0 100644 --- a/modules/zividsamples/gui/preparation/infield_correction_gui.py +++ b/modules/zividsamples/gui/preparation/infield_correction_gui.py @@ -95,7 +95,6 @@ def create_widgets(self) -> None: hand_eye_configuration=self.hand_eye_configuration, hide_descriptive_image=True ) self.infield_input_data_selection_widget = InfieldCorrectionDataSelectionWidget(directory=self.data_directory) - self.infield_input_data_selection_widget.setVisible(False) self.infield_correction_result_widget = InfieldCorrectionResultWidget() def setup_layout(self) -> None: @@ -154,7 +153,7 @@ def is_loading(self) -> bool: def on_pending_changes(self) -> None: self.infield_input_data_selection_widget.clear_gui() - self.infield_input_data_selection_widget.update_data_directory(self.data_directory) + self.infield_input_data_selection_widget.set_directory(self.data_directory) def on_tab_visibility_changed(self, is_current: bool) -> None: pass diff --git a/modules/zividsamples/gui/qt_application.py b/modules/zividsamples/gui/qt_application.py index b2b38b07..0a7bfda2 100644 --- a/modules/zividsamples/gui/qt_application.py +++ b/modules/zividsamples/gui/qt_application.py @@ -91,6 +91,26 @@ class ZividColors: }} """ +ZIVID_BLUE_BUTTON_STYLE = f""" +QPushButton {{ + background-color: {_color_text(ZividColors.DARK_BLUE, 1)}; + color: white; + border: none; + padding: 10px; + border-radius: 4px; +}} +QPushButton:hover {{ + background-color: {_color_text(ZividColors.LIGHT_BLUE, 1)}; +}} +QPushButton:pressed {{ + background-color: {_color_text(ZividColors.DARK_GRAY, 1)}; +}} +QPushButton:disabled {{ + background-color: {_color_text(ZividColors.DARK_BLUE, 0.5)}; + color: {_color_text(ZividColors.DISABLED_TEXT, 1)}; +}} +""" + BUTTON_STYLE = f""" QPushButton {{ background-color: {_color_text(ZividColors.ITEM_BACKGROUND, 1)}; diff --git a/modules/zividsamples/gui/verification/hand_eye_verification_gui.py b/modules/zividsamples/gui/verification/hand_eye_verification_gui.py index cf38c0a3..e597d1c5 100644 --- a/modules/zividsamples/gui/verification/hand_eye_verification_gui.py +++ b/modules/zividsamples/gui/verification/hand_eye_verification_gui.py @@ -170,7 +170,7 @@ def update_instructions(self, has_set_object_poses_in_robot_frame: bool, robot_p self.has_confirmed_robot_pose or not self.robot_configuration.has_no_robot() ) self.instruction_steps = {} - if self.robot_configuration.can_control: + if self.robot_configuration.can_control(): self.instruction_steps[ "Move Robot (click 'Move to next target', 'Home' or Disconnect→manually move robot→Connect)" ] = self.has_confirmed_robot_pose @@ -180,7 +180,7 @@ def update_instructions(self, has_set_object_poses_in_robot_frame: bool, robot_p self.has_set_object_poses_in_robot_frame ) if self.has_confirmed_robot_pose and self.has_set_object_poses_in_robot_frame: - if self.robot_configuration.can_control: + if self.robot_configuration.can_control(): self.instruction_steps[ "Move Robot (click 'Move to next target' or Disconnect→manually move robot→Connect)" ] = False @@ -322,8 +322,6 @@ def on_robot_pose_manually_updated(self) -> None: def on_actual_pose_updated(self, robot_target: RobotTarget) -> None: self.robot_pose_widget.set_transformation_matrix(robot_target.pose) self.confirm_robot_pose() - self.calculate_calibration_object_in_camera_frame_pose() - self.update_projection.emit(True) def on_target_pose_updated(self, robot_target: RobotTarget) -> None: self.robot_pose_widget.set_transformation_matrix(robot_target.pose) @@ -398,6 +396,8 @@ def generate_projector_image(self, camera: zivid.Camera) -> NDArray[Shape["N, M, self.cv2_handler.draw_circles(projector_image, non_nan_projector_image_indices, color) # Add diagonal lines across white checkers rows, cols = (5, 6) if projector_pixels.shape[0] == 30 else (3, 4) + if non_nan_projector_image_indices.shape[0] != rows * cols: + return projector_image organized_projector_pixel_indices = non_nan_projector_image_indices.reshape(rows, cols, -1) diagonal_lines = np.array( [ @@ -461,6 +461,10 @@ def set_hand_eye_transformation_matrix(self, transformation_matrix: Transformati self.hand_eye_pose_widget.set_transformation_matrix(transformation_matrix) self.calculate_calibration_object_in_camera_frame_pose() self.update_projection.emit(True) + self.update_instructions( + has_set_object_poses_in_robot_frame=self.has_set_object_poses_in_robot_frame, + robot_pose_confirmed=self.has_confirmed_robot_pose, + ) def get_tab_widgets_in_order(self) -> List[QWidget]: widgets: List[QWidget] = [] diff --git a/modules/zividsamples/gui/verification/stitch_gui.py b/modules/zividsamples/gui/verification/stitch_gui.py index b62681e4..ab00a025 100644 --- a/modules/zividsamples/gui/verification/stitch_gui.py +++ b/modules/zividsamples/gui/verification/stitch_gui.py @@ -15,7 +15,9 @@ from nptyping import NDArray, Shape, UInt8 from PyQt5.QtCore import pyqtSignal from PyQt5.QtGui import QCloseEvent, QImage -from PyQt5.QtWidgets import QCheckBox, QHBoxLayout, QPushButton, QVBoxLayout, QWidget +from PyQt5.QtWidgets import QCheckBox, QFileDialog, QHBoxLayout, QMessageBox, QPushButton, QVBoxLayout, QWidget +from zivid.experimental.point_cloud_export import export_unorganized_point_cloud +from zivid.experimental.point_cloud_export.file_format import PLY from zividsamples.gui.robot.robot_control import RobotTarget from zividsamples.gui.verification.capture_at_pose_selection_widget import CaptureAtPose, CaptureAtPoseSelectionWidget from zividsamples.gui.widgets.pointcloud_visualizer import VisualizerWidget @@ -34,7 +36,9 @@ class StitchGUI(TabWidgetWithRobotSupport): hand_eye_configuration: HandEyeConfiguration has_detection_result: bool = False has_confirmed_robot_pose: bool = False + has_captured: bool = False point_cloud_widget: VisualizerWidget + stitched_point_cloud: Optional[zivid.UnorganizedPointCloud] loading_finished = pyqtSignal() instructions_updated: pyqtSignal = pyqtSignal() description: List[str] @@ -88,6 +92,9 @@ def create_widgets(self, initial_rotation_information: RotationInformation) -> N self.uniform_color_check_box.setText("Use uniform color for point clouds") self.uniform_color_check_box.setChecked(True) self.point_cloud_widget = VisualizerWidget() + self.save_point_cloud_button = QPushButton("Save Point Cloud") + self.save_point_cloud_button.setEnabled(False) + self.stitched_point_cloud: Optional[zivid.UnorganizedPointCloud] = None def setup_layout(self) -> None: layout = QVBoxLayout() @@ -104,6 +111,7 @@ def setup_layout(self) -> None: left_panel.addWidget(self.hand_eye_pose_widget) right_panel.addWidget(self.capture_at_pose_selection_widget) right_panel.addWidget(self.uniform_color_check_box) + right_panel.addWidget(self.save_point_cloud_button) center_layout.addLayout(left_panel) center_layout.addLayout(right_panel) layout.addLayout(center_layout) @@ -117,9 +125,11 @@ def connect_signals(self) -> None: self.capture_at_pose_selection_widget.loading_finished.connect(self.update_stitched_view) self.capture_at_pose_selection_widget.loading_finished.connect(self.loading_finished) self.uniform_color_check_box.stateChanged.connect(self.update_stitched_view) + self.save_point_cloud_button.clicked.connect(self.on_save_point_cloud_clicked) def update_instructions(self, captured: bool, robot_pose_confirmed: bool) -> None: self.has_confirmed_robot_pose = robot_pose_confirmed + self.has_captured = captured self.instruction_steps = {} if self.robot_configuration.can_control(): self.instruction_steps[ @@ -186,6 +196,22 @@ def update_stitched_view(self) -> None: if unorganized_point_cloud.size > 0: unorganized_point_cloud = unorganized_point_cloud.voxel_downsampled(voxel_size=1, min_points_per_voxel=1) self.point_cloud_widget.set_point_cloud(unorganized_point_cloud) + self.stitched_point_cloud = unorganized_point_cloud if unorganized_point_cloud.size > 0 else None + self.save_point_cloud_button.setEnabled(self.stitched_point_cloud is not None) + + def on_save_point_cloud_clicked(self): + if self.stitched_point_cloud is None: + return + file_path = QFileDialog.getSaveFileName( + self, "Save Point Cloud", "stitched_pointcloud.ply", "PLY Files (*.ply)" + )[0] + if file_path: + if not file_path.endswith(".ply"): + file_path += ".ply" + try: + export_unorganized_point_cloud(self.stitched_point_cloud, PLY(file_path, layout=PLY.Layout.unordered)) + except Exception as ex: + QMessageBox.critical(self, "Save failed", str(ex)) def on_capture_at_pose_selected(self, capture_at_pose: CaptureAtPose) -> None: self.robot_pose_widget.set_transformation_matrix(capture_at_pose.robot_pose) @@ -203,6 +229,7 @@ def process_capture(self, frame: zivid.Frame, _: NDArray[Shape["N, M, 4"], UInt8 def set_hand_eye_transformation_matrix(self, transformation_matrix: TransformationMatrix) -> None: self.hand_eye_pose_widget.set_transformation_matrix(transformation_matrix) + self.update_instructions(captured=self.has_captured, robot_pose_confirmed=self.has_confirmed_robot_pose) def get_tab_widgets_in_order(self) -> List[QWidget]: widgets: List[QWidget] = [] diff --git a/modules/zividsamples/gui/verification/touch_gui.py b/modules/zividsamples/gui/verification/touch_gui.py index 74d9b7ed..e96988a8 100644 --- a/modules/zividsamples/gui/verification/touch_gui.py +++ b/modules/zividsamples/gui/verification/touch_gui.py @@ -84,11 +84,14 @@ def create_widgets(self, initial_rotation_information: RotationInformation) -> N initial_rotation_information=initial_rotation_information, ) self.markers_in_robot_base_frame_pose_widget.setMinimumHeight(180) - self.touch_configuration_widget = TouchConfigurationWidget() + self.touch_configuration_widget = TouchConfigurationWidget( + initial_rotation_information=initial_rotation_information, + eye_in_hand=self.hand_eye_configuration.eye_in_hand, + ) self.calibration_object_image = ImageViewer() self.calibration_object_image.setMinimumHeight(300) self.calibration_object_image.setMinimumWidth(300) - self.confirm_marker_button = QPushButton("Confirm marker to touch") + self.confirm_marker_button = QPushButton("Apply Touch Settings") self.confirm_marker_button.setCheckable(True) self.confirm_marker_button.setObjectName("Touch-confirm_marker_button") @@ -115,12 +118,19 @@ def setup_layout(self) -> None: def connect_signals(self) -> None: self.confirm_marker_button.clicked.connect(self.on_confirm_marker_button_clicked) + self.touch_configuration_widget.marker_id_selection.valueChanged.connect(self.on_touch_configuration_changed) + self.touch_configuration_widget.marker_dictionary_selection.currentIndexChanged.connect( + self.on_touch_configuration_changed + ) + self.touch_configuration_widget.z_offset_spinbox.valueChanged.connect(self.on_touch_configuration_changed) + self.touch_configuration_widget.touch_tool_pose_widget.pose_updated.connect(self.on_touch_configuration_changed) def update_instructions(self, marker_confirmed: bool, marker_captured: bool) -> None: self.marker_confirmed = marker_confirmed + self.marker_captured = marker_captured self.instruction_steps = {} - self.instruction_steps["Confirm marker to touch"] = self.marker_confirmed - self.instruction_steps["Capture"] = marker_captured and self.marker_confirmed + self.instruction_steps["Apply Touch Settings"] = self.marker_confirmed + self.instruction_steps["Capture"] = self.marker_captured and self.marker_confirmed self.instruction_steps["Touch"] = False self.instructions_updated.emit() self.confirm_marker_button.setChecked(self.marker_confirmed) @@ -135,6 +145,9 @@ def on_tab_visibility_changed(self, is_current: bool) -> None: def on_confirm_marker_button_clicked(self) -> None: self.update_instructions(marker_confirmed=self.confirm_marker_button.isChecked(), marker_captured=False) + def on_touch_configuration_changed(self) -> None: + self.update_instructions(marker_confirmed=False, marker_captured=False) + def hand_eye_configuration_update(self, hand_eye_configuration: HandEyeConfiguration) -> None: self.hand_eye_configuration = hand_eye_configuration self.hand_eye_pose_widget.on_eye_in_hand_toggled(self.hand_eye_configuration.eye_in_hand) @@ -143,6 +156,10 @@ def hand_eye_configuration_update(self, hand_eye_configuration: HandEyeConfigura def rotation_format_update(self, rotation_information: RotationInformation) -> None: self.hand_eye_pose_widget.set_rotation_format(rotation_information) self.robot_pose_widget.set_rotation_format(rotation_information) + self.touch_configuration_widget.set_rotation_format(rotation_information) + + def toggle_advanced_view(self, checked: bool): + self.touch_configuration_widget.toggle_advanced_view(checked) def robot_configuration_update(self, _: RobotConfiguration) -> None: pass @@ -184,8 +201,7 @@ def process_capture(self, frame: zivid.Frame, rgba: NDArray[Shape["N, M, 4"], UI } self.markers_in_robot_base_frame_pose_widget.set_markers(detected_marker_poses_in_robot_frame) touch_pose = list(detected_marker_poses_in_robot_frame.values())[0] - touch_tool = TransformationMatrix() - touch_tool.translation[2] = -touch_configuration.z_offset + touch_tool = touch_configuration.touch_tool_transform self.touch_pose_updated.emit(touch_pose * touch_tool) self.update_instructions(marker_confirmed=self.marker_confirmed, marker_captured=True) @@ -199,6 +215,7 @@ def get_tab_widgets_in_order(self) -> List[QWidget]: def set_hand_eye_transformation_matrix(self, transformation_matrix: TransformationMatrix) -> None: self.hand_eye_pose_widget.set_transformation_matrix(transformation_matrix) + self.update_instructions(marker_confirmed=self.marker_confirmed, marker_captured=self.marker_captured) def closeEvent(self, a0: QCloseEvent) -> None: self.touch_configuration_widget.closeEvent(a0) diff --git a/modules/zividsamples/gui/widgets/detection_visualization.py b/modules/zividsamples/gui/widgets/detection_visualization.py index 8860d138..8c611ddd 100644 --- a/modules/zividsamples/gui/widgets/detection_visualization.py +++ b/modules/zividsamples/gui/widgets/detection_visualization.py @@ -57,12 +57,14 @@ def __init__( self.descriptive_image_label.setFixedWidth(self.descriptive_image_width) self.descriptive_image_label.setFixedHeight( int(self.descriptive_image_width * descriptive_image.height() / descriptive_image.width()) + if not descriptive_image.isNull() + else self.descriptive_image_width ) self.descriptive_image_label.setPixmap(descriptive_image) group_box_contents_layout = QHBoxLayout() - group_box_contents_layout.addWidget(self.calibration_object_image_viewer) - group_box_contents_layout.addWidget(self.error_message_label) + group_box_contents_layout.addWidget(self.calibration_object_image_viewer, 1) + group_box_contents_layout.addWidget(self.error_message_label, 1) group_box_contents_layout.addWidget(self.descriptive_image_label) self.group_box.setLayout(group_box_contents_layout) @@ -86,6 +88,8 @@ def update_layout(self) -> None: self.descriptive_image_label.setPixmap(descriptive_image) self.descriptive_image_label.setFixedHeight( int(self.descriptive_image_width * descriptive_image.height() / descriptive_image.width()) + if not descriptive_image.isNull() + else self.descriptive_image_width ) def on_hand_eye_configuration_updated(self, hand_eye_configuration: HandEyeConfiguration) -> None: @@ -127,7 +131,7 @@ def set_error_message(self, error_message: str) -> None: self.calibration_object_image_viewer.hide() def get_qimage(self, calibration_object: CalibrationObject) -> QImage: - calibration_object_pixmap = self.calibration_object_pixmap[self.hand_eye_configuration.calibration_object] + calibration_object_pixmap = self.calibration_object_pixmap[calibration_object] if calibration_object_pixmap is None: raise RuntimeError(f"No image available for {calibration_object.name}") return calibration_object_pixmap.toImage() diff --git a/modules/zividsamples/gui/widgets/image_viewer.py b/modules/zividsamples/gui/widgets/image_viewer.py index 7d160854..ec6389ba 100644 --- a/modules/zividsamples/gui/widgets/image_viewer.py +++ b/modules/zividsamples/gui/widgets/image_viewer.py @@ -1,7 +1,7 @@ from typing import Optional -from PyQt5.QtCore import QRectF, QSize, Qt, pyqtSlot -from PyQt5.QtGui import QImage, QPainter, QPixmap, QWheelEvent +from PyQt5.QtCore import QRectF, Qt, pyqtSlot +from PyQt5.QtGui import QImage, QPainter, QPixmap, QResizeEvent, QWheelEvent from PyQt5.QtWidgets import QDialog, QGraphicsPixmapItem, QGraphicsScene, QGraphicsView, QVBoxLayout, QWidget @@ -42,11 +42,10 @@ def wheelEvent(self, event: QWheelEvent) -> None: else: self._zoom = 0 - def resize(self, event: QSize) -> None: - super().resize(event) - print("Resizing image_viewer") - self._zoom = 0 - self.fitInView(self.sceneRect(), Qt.KeepAspectRatio) + def resizeEvent(self, event: QResizeEvent) -> None: + super().resizeEvent(event) + if self._zoom == 0: + self.fitInView(self.sceneRect(), Qt.KeepAspectRatio) class ImageViewerDialog(QDialog): diff --git a/modules/zividsamples/gui/widgets/live_2d_widget.py b/modules/zividsamples/gui/widgets/live_2d_widget.py index 5c9dac15..dd5133b5 100644 --- a/modules/zividsamples/gui/widgets/live_2d_widget.py +++ b/modules/zividsamples/gui/widgets/live_2d_widget.py @@ -84,7 +84,7 @@ def update_exposure_based_on_relative_brightness(self, settings_2d: zivid.Settin else relative_brightness / (exposure_increase / current_exposure_time) ) acquisition.gain = max(1.0, min(acquisition.gain * remaining_relative_brightness, 16.0)) - acquisition.brightness = 0.1 + acquisition.brightness = 0.0 return settings_2d def update_settings_2d(self, settings_2d: zivid.Settings2D, camera_model: str) -> None: diff --git a/modules/zividsamples/gui/widgets/pointcloud_visualizer.py b/modules/zividsamples/gui/widgets/pointcloud_visualizer.py index b0ac1f70..c0342910 100644 --- a/modules/zividsamples/gui/widgets/pointcloud_visualizer.py +++ b/modules/zividsamples/gui/widgets/pointcloud_visualizer.py @@ -1,39 +1,56 @@ import threading -from time import sleep -from typing import Union +from typing import Optional, Union import zivid class VisualizerWidget: visualizer_thread: threading.Thread - visualizer: zivid.visualization.Visualizer + visualizer: Optional[zivid.visualization.Visualizer] def __init__(self) -> None: + self._ready = threading.Event() + self.visualizer = None + self._failed = False + self._start_thread() + + def _start_thread(self) -> None: + self._ready.clear() self.visualizer_thread = threading.Thread(target=self.run, daemon=True) self.visualizer_thread.start() def run(self) -> None: - self.visualizer = zivid.visualization.Visualizer() - self.visualizer.set_window_title("Zivid Point Cloud Visualizer") - self.visualizer.colors_enabled = True - self.visualizer.axis_indicator_enabled = True - self.visualizer.hide() - self.visualizer.run() - self.visualizer.release() + try: + visualizer = zivid.visualization.Visualizer() + visualizer.set_window_title("Zivid Point Cloud Visualizer") + visualizer.colors_enabled = True + visualizer.axis_indicator_enabled = True + visualizer.hide() + except RuntimeError as ex: + self._failed = True + self.visualizer = None + print(f"Point cloud visualizer unavailable: {ex}") + self._ready.set() + return + self.visualizer = visualizer + self._ready.set() + visualizer.run() + visualizer.release() def set_point_cloud(self, data: Union[zivid.Frame, zivid.PointCloud, zivid.UnorganizedPointCloud]) -> None: - if not self.visualizer_thread.is_alive(): - self.visualizer_thread = threading.Thread(target=self.run, daemon=True) - self.visualizer_thread.start() - sleep(0.2) # Give some time for the thread to start - self.visualizer.show(data) + if not self.visualizer_thread.is_alive() and not self._failed: + self._start_thread() + self._ready.wait() + if self.visualizer is not None: + self.visualizer.show(data) def hide(self) -> None: - if self.visualizer_thread.is_alive(): + self._ready.wait() + if self.visualizer is not None: self.visualizer.hide() def close(self) -> None: - if self.visualizer_thread.is_alive(): + self._ready.wait() + if self.visualizer is not None and self.visualizer_thread.is_alive(): self.visualizer.close() self.visualizer_thread.join() diff --git a/modules/zividsamples/gui/widgets/pose_widget.py b/modules/zividsamples/gui/widgets/pose_widget.py index 01c75d59..9132511d 100644 --- a/modules/zividsamples/gui/widgets/pose_widget.py +++ b/modules/zividsamples/gui/widgets/pose_widget.py @@ -1,4 +1,5 @@ import re +import warnings from abc import abstractmethod from enum import Enum from pathlib import Path @@ -7,8 +8,8 @@ import numpy as np import zivid from PyQt5.QtCore import QSignalBlocker, Qt, pyqtSignal -from PyQt5.QtGui import QPixmap -from PyQt5.QtWidgets import QGridLayout, QGroupBox, QLabel, QLineEdit, QScrollArea, QVBoxLayout, QWidget +from PyQt5.QtGui import QFont, QPixmap +from PyQt5.QtWidgets import QGridLayout, QGroupBox, QHBoxLayout, QLabel, QLineEdit, QScrollArea, QVBoxLayout, QWidget from scipy.spatial.transform import Rotation from zividsamples.gui.qt_application import create_horizontal_line, create_vertical_line from zividsamples.gui.widgets.aspect_ratio_label import AspectRatioLabel @@ -145,7 +146,7 @@ def rotation_from_parameters(self, rotation_parameters: Sequence[float]) -> Rota rotation_matrix = np.asarray(rotation_parameters).reshape([3, 3]) rotation = Rotation.from_matrix(rotation_matrix) else: - raise ValueError(f"Invalid variant: {self.variant}") + raise ValueError(f"Invalid variant: {self.rotation_information.rotation_format.name}") return rotation def parameters_from_rotation(self, rotation: Rotation) -> List[float]: @@ -189,6 +190,51 @@ def _rotation_label_text(self) -> str: ) return f"Rotation as {self.rotation_information.rotation_format.name}{degrees}" + def _rotation_column_label_texts(self) -> List[str]: + fmt = self.rotation_information.rotation_format + if fmt == RotationFormats.euler: + return list(self.rotation_information.euler_variant) + if fmt == RotationFormats.rotation_vector: + return ["X", "Y", "Z"] + if fmt == RotationFormats.angle_axis: + return ["Angle", "X", "Y", "Z"] + if fmt == RotationFormats.quaternion: + return ["W", "X", "Y", "Z"] + return [] + + def _update_rotation_column_labels(self): + texts = self._rotation_column_label_texts() + for i, label in enumerate(self.rotation_column_labels): + if i < len(texts): + label.setText(f"{texts[i].upper()}:") + label.setVisible(bool(texts[i])) + else: + label.setText("") + label.setVisible(False) + + def _add_rotation_labels_and_editors_to_layout(self): + for i in reversed(range(self.rotation_parameters_layout.count())): + item = self.rotation_parameters_layout.itemAt(i) + if item.widget() is not None: + item.widget().setParent(None) + elif item.layout() is not None: + sub = item.layout() + while sub.count(): + sub.itemAt(0).widget().setParent(None) + self.rotation_parameters_layout.removeItem(sub) + num_params = self.rotation_information.rotation_format.number_of_parameters + is_matrix = num_params == 9 + for index, (label, editor) in enumerate( + zip(self.rotation_column_labels, self.rotation_parameter_editors, strict=False) + ): + if is_matrix: + self.rotation_parameters_layout.addWidget(editor, index // 3, index % 3) + else: + cell = QHBoxLayout() + cell.addWidget(label) + cell.addWidget(editor) + self.rotation_parameters_layout.addLayout(cell, 0, index) + def parameter_text_to_float(text: str) -> float: sanitized_text = text.strip().replace(",", ".") @@ -225,6 +271,7 @@ def __init__( ) self.rotation_vector_user_parameters: List[float] = [0.0, 0.0, 0.0] + self.euler_user_parameters: List[float] = [0.0, 0.0, 0.0] self.rotation_parameters: List[float] = [] self.setup_widgets() self.setup_layout() @@ -236,17 +283,27 @@ def setup_widgets(self) -> None: self.translation_parameters_label = QLabel() self.translation_parameters_label.setText("Translation") self.translation_parameter_editors: List[QLineEdit] = [QLineEdit() for _ in range(3)] + editor_font = QFont() + editor_font.setPointSize(12) for index, parameter_editor in enumerate(self.translation_parameter_editors): parameter_editor.setObjectName(f"translation_parameter_{index}") parameter_editor.setReadOnly(self.read_only) + parameter_editor.setFont(editor_font) + + self.translation_column_labels: List[QLabel] = [QLabel(f"{axis}:") for axis in "XYZ"] self.rotation_parameters_layout = QGridLayout() self.rotation_parameters_label = QLabel() self.rotation_parameters_label.setText(self._rotation_label_text()) + self.rotation_column_labels: List[QLabel] = [QLabel() for _ in range(9)] + for label in self.rotation_column_labels: + label.setAlignment(Qt.AlignCenter) + self._update_rotation_column_labels() self.rotation_parameter_editors: List[QLineEdit] = [QLineEdit() for _ in range(9)] for index, parameter_editor in enumerate(self.rotation_parameter_editors): parameter_editor.setObjectName(f"rotation_parameter_{index}") parameter_editor.setReadOnly(self.read_only) + parameter_editor.setFont(editor_font) self.advanced_divider = create_horizontal_line() self.pose_label = QLabel() @@ -267,17 +324,19 @@ def setup_layout(self) -> None: row_offset = row_offset + 1 self.grid_layout.addWidget(self.translation_parameters_label, row_offset, 0) - for index, parameter_editor in enumerate(self.translation_parameter_editors, start=1): - self.grid_layout.addWidget(parameter_editor, row_offset, index, 1, 1) + for index, (label, parameter_editor) in enumerate( + zip(self.translation_column_labels, self.translation_parameter_editors, strict=False), start=1 + ): + cell = QHBoxLayout() + cell.addWidget(label) + cell.addWidget(parameter_editor) + self.grid_layout.addLayout(cell, row_offset, index, 1, 1) row_offset = row_offset + 1 self.grid_layout.addWidget(create_horizontal_line(), row_offset, 0, 1, 4) row_offset = row_offset + 1 - for index, parameter_editor in enumerate(self.rotation_parameter_editors): - row = index // 3 if self.rotation_information.rotation_format.number_of_parameters == 9 else 0 - col = index % 3 if self.rotation_information.rotation_format.number_of_parameters == 9 else index - self.rotation_parameters_layout.addWidget(parameter_editor, row, col) + self._add_rotation_labels_and_editors_to_layout() self.grid_layout.addWidget(self.rotation_parameters_label, row_offset, 0) self.grid_layout.addLayout(self.rotation_parameters_layout, row_offset, 1, 1, 3) row_offset = row_offset + 1 @@ -326,6 +385,11 @@ def setup_connections(self) -> None: parameter_editor.editingFinished.connect(self.on_rotation_parameter_changed) self.pose_text.editingFinished.connect(self.on_pose_text_changed) + def on_rotation_format_update(self, rotation_information: RotationInformation) -> None: + self.euler_user_parameters = [0.0, 0.0, 0.0] + self.rotation_vector_user_parameters = [0.0, 0.0, 0.0] + super().on_rotation_format_update(rotation_information) + def toggle_advanced_section(self, checked: bool) -> None: for widget in self.advanced_widgets: widget.setVisible(checked) @@ -342,20 +406,18 @@ def toggle_advanced_section(self, checked: bool) -> None: def update_from_transformation_matrix(self) -> None: self.rotation_parameters = self.parameters_from_rotation(self.transformation_matrix.rotation) - if self.rotation_information.rotation_format == "Rotation Vector" and not np.all( + if self.rotation_information.rotation_format == RotationFormats.rotation_vector and not np.all( np.asarray(self.rotation_vector_user_parameters) == np.zeros(3) ): self.rotation_parameters = self.rotation_vector_user_parameters + if self.rotation_information.rotation_format == RotationFormats.euler and not np.all( + np.asarray(self.euler_user_parameters) == np.zeros(3) + ): + self.rotation_parameters = self.euler_user_parameters self.rotation_parameters_label.setText(self._rotation_label_text()) self.pose_label.setText(f"Translation + {self._rotation_label_text()}") - for i in reversed(range(self.rotation_parameters_layout.count())): - widget = self.rotation_parameters_layout.itemAt(i).widget() - if widget is not None: - widget.setParent(None) - for index, parameter_editor in enumerate(self.rotation_parameter_editors): - row = index // 3 if self.rotation_information.rotation_format.number_of_parameters == 9 else 0 - col = index % 3 if self.rotation_information.rotation_format.number_of_parameters == 9 else index - self.rotation_parameters_layout.addWidget(parameter_editor, row, col) + self._update_rotation_column_labels() + self._add_rotation_labels_and_editors_to_layout() with QSignalBlocker(self.pose_text): self.pose_text.setText( " ".join([f"{value:.3f}" for value in list(self.transformation_matrix.translation)]) @@ -391,8 +453,10 @@ def on_rotation_parameter_changed(self) -> None: for i, parameter_editor in enumerate(self.rotation_parameter_editors) if i < self.rotation_information.rotation_format.number_of_parameters ] - if self.rotation_information.rotation_format == "Rotation Vector": + if self.rotation_information.rotation_format == RotationFormats.rotation_vector: self.rotation_vector_user_parameters = updated_rotation_parameters + if self.rotation_information.rotation_format == RotationFormats.euler: + self.euler_user_parameters = updated_rotation_parameters updated_rotation = self.rotation_from_parameters(updated_rotation_parameters) parameters_from_updated_rotation = self.parameters_from_rotation(updated_rotation) has_valid_input = True @@ -400,11 +464,16 @@ def on_rotation_parameter_changed(self) -> None: if index != modified_index and not np.isclose(parameters_from_updated_rotation[index], rotation_parameters): self.sender().setStyleSheet("background-color: yellow; color: black;") has_valid_input = False - if self.rotation_information.rotation_format == "Rotation Vector": + if self.rotation_information.rotation_format == RotationFormats.rotation_vector: rotation_from_scipy_rotvec = self.rotation_from_parameters(parameters_from_updated_rotation) rotation_from_user_rotvec = self.rotation_from_parameters(self.rotation_vector_user_parameters) if np.allclose(rotation_from_scipy_rotvec.as_rotvec(), rotation_from_user_rotvec.as_rotvec()): has_valid_input = True + if self.rotation_information.rotation_format == RotationFormats.euler: + rotation_from_scipy_euler = self.rotation_from_parameters(parameters_from_updated_rotation) + rotation_from_user_euler = self.rotation_from_parameters(self.euler_user_parameters) + if np.allclose(rotation_from_scipy_euler.as_matrix(), rotation_from_user_euler.as_matrix()): + has_valid_input = True if has_valid_input: self.rotation_parameters = updated_rotation_parameters self.transformation_matrix.rotation = updated_rotation @@ -428,17 +497,37 @@ def on_translation_parameter_changed(self) -> None: def on_pose_text_changed(self) -> None: parameter_list = re.split(r"[,\s]+", self.pose_text.text().strip()) if len(parameter_list) < (3 + len(self.rotation_parameters)): - # Wait for user to finish entering data return - self.transformation_matrix.translation = np.array([float(value) for value in parameter_list[:3]]) - updated_rotation_parameters = [float(value) for value in parameter_list[3:]] - if self.rotation_information.rotation_format == "Rotation Vector": + try: + self.transformation_matrix.translation = np.array([float(value) for value in parameter_list[:3]]) + updated_rotation_parameters = [float(value) for value in parameter_list[3:]] + except ValueError: + self.update_from_transformation_matrix() + return + if self.rotation_information.rotation_format == RotationFormats.rotation_vector: self.rotation_vector_user_parameters = updated_rotation_parameters + if self.rotation_information.rotation_format == RotationFormats.euler: + self.euler_user_parameters = updated_rotation_parameters updated_rotation = self.rotation_from_parameters(updated_rotation_parameters) self.rotation_parameters = updated_rotation_parameters self.transformation_matrix.rotation = updated_rotation self.update_from_transformation_matrix() + def check_rotation_warning(self) -> Optional[str]: + if self.rotation_information.rotation_format != RotationFormats.euler: + return None + rotation = self.rotation_from_parameters(self.rotation_parameters) + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + rotation.as_euler(self.rotation_information.euler_variant, degrees=self.rotation_information.use_degrees) + for w in caught: + if "Gimbal lock" in str(w.message): + for i, editor in enumerate(self.rotation_parameter_editors): + if i < self.rotation_information.rotation_format.number_of_parameters: + editor.setStyleSheet("background-color: yellow; color: black;") + return str(w.message) + return None + def reset_parameter_editor_styles(self) -> None: for parameter_editor in self.rotation_parameter_editors: parameter_editor.setStyleSheet("") diff --git a/modules/zividsamples/gui/widgets/show_yaml_dialog.py b/modules/zividsamples/gui/widgets/show_yaml_dialog.py index 8ccb98d4..2e4c95a1 100644 --- a/modules/zividsamples/gui/widgets/show_yaml_dialog.py +++ b/modules/zividsamples/gui/widgets/show_yaml_dialog.py @@ -1,24 +1,54 @@ from pathlib import Path from PyQt5.QtCore import QSize, QTimer -from PyQt5.QtWidgets import QDialog, QPushButton, QTextEdit, QVBoxLayout +from PyQt5.QtWidgets import ( + QApplication, + QDialog, + QFileDialog, + QHBoxLayout, + QLabel, + QMessageBox, + QPushButton, + QTextEdit, + QVBoxLayout, +) -def show_yaml_dialog(yaml_path: Path, title: str) -> None: +def show_yaml_dialog(yaml_path: Path, title: str, close_button_label: str = "Close") -> None: dialog = QDialog() dialog.setWindowTitle(title) layout = QVBoxLayout() + path_label = QLabel(f"Saved to
{yaml_path}") + path_label.setWordWrap(True) + layout.addWidget(path_label) + text_edit = QTextEdit() - text_edit.setPlainText(yaml_path.read_text(encoding="utf-8")) + try: + contents = yaml_path.read_text(encoding="utf-8") + except OSError as ex: + contents = f"(Error reading file: {ex})" + text_edit.setPlainText(contents) text_edit.setReadOnly(True) text_edit.setLineWrapMode(QTextEdit.NoWrap) layout.addWidget(text_edit) - close_button = QPushButton("Close") + button_layout = QHBoxLayout() + + copy_button = QPushButton("Copy") + copy_button.clicked.connect(lambda: QApplication.clipboard().setText(contents)) + button_layout.addWidget(copy_button) + + save_as_button = QPushButton("Save As...") + save_as_button.clicked.connect(lambda: _save_as(dialog, contents, yaml_path)) + button_layout.addWidget(save_as_button) + + close_button = QPushButton(close_button_label) close_button.clicked.connect(dialog.accept) - layout.addWidget(close_button) + button_layout.addWidget(close_button) + + layout.addLayout(button_layout) dialog.setLayout(layout) @@ -28,11 +58,26 @@ def adjust_dialog_size() -> None: margin = 20 button_height = close_button.sizeHint().height() + path_label_height = path_label.sizeHint().height() document_size.setWidth(document_size.width() + 2 * margin) - document_size.setHeight(document_size.height() + button_height + 3 * margin) + document_size.setHeight(document_size.height() + button_height + path_label_height + 4 * margin) - dialog.resize(document_size.expandedTo(QSize(300, 200))) + dialog.resize(document_size.expandedTo(QSize(600, 450))) QTimer.singleShot(0, adjust_dialog_size) dialog.exec_() + + +def _save_as(parent: QDialog, contents: str, default_path: Path) -> None: + file_path = QFileDialog.getSaveFileName( + parent, + "Save As", + str(default_path), + "YAML Files (*.yaml);;All Files (*)", + )[0] + if file_path: + try: + Path(file_path).write_text(contents, encoding="utf-8") + except OSError as ex: + QMessageBox.critical(parent, "Save failed", str(ex)) diff --git a/modules/zividsamples/gui/widgets/tutorial_widget.py b/modules/zividsamples/gui/widgets/tutorial_widget.py index bb295910..d895f471 100644 --- a/modules/zividsamples/gui/widgets/tutorial_widget.py +++ b/modules/zividsamples/gui/widgets/tutorial_widget.py @@ -1,25 +1,31 @@ from typing import Dict, List, Optional +from PyQt5.QtCore import QUrl, pyqtSignal from PyQt5.QtWidgets import QGroupBox, QTextBrowser, QVBoxLayout, QWidget class TutorialWidget(QWidget): - title: str = "" - description: List[str] = [] - steps: Dict[str, bool] = {} + view_he_transform_requested = pyqtSignal() def __init__( self, parent: Optional[QWidget] = None, ) -> None: super().__init__(parent) + self.title: str = "" + self.description: List[str] = [] + self.steps: Dict[str, bool] = {} + self._he_show: bool = False + self._he_loaded: bool = False + self._he_mandatory: bool = False self.group_box = QGroupBox("Tutorial", self) self.text_area = QTextBrowser() self.text_area.setAcceptRichText(True) self.text_area.setReadOnly(True) - self.text_area.setOpenExternalLinks(True) + self.text_area.setOpenLinks(False) + self.text_area.anchorClicked.connect(self._on_anchor_clicked) group_layout = QVBoxLayout() group_layout.addWidget(self.text_area) @@ -47,13 +53,43 @@ def add_steps(self, steps: Dict[str, bool]) -> None: self.steps.update(steps) self.update_text() + def set_he_transform_status(self, show: bool, loaded: bool, mandatory: bool) -> None: + self._he_show = show + self._he_loaded = loaded + self._he_mandatory = mandatory + self.update_text() + + def _on_anchor_clicked(self, url: QUrl) -> None: + if url.toString() == "show_he_transform": + self.view_he_transform_requested.emit() + def update_text(self) -> None: self.text_area.clear() text = f"

{self.title}

" - text += "" + he_row = "" + if self._he_show: + if self._he_loaded: + he_row = ( + "" + ) + elif self._he_mandatory: + he_row = ( + "" + "" + ) + else: + he_row = "" + text += "
☑HE Transform: Loaded  " + "[view]" + "
☐ ⚠HE Transform: not loaded — use File > Load HE Transform
☐HE Transform: will be set after calibration
" + if self._he_show and self._he_mandatory: + text += he_row for step, completed in self.steps.items(): - checkmark = "✅" if completed else "☐" # ✓ for checked, ☐ for unchecked - text += f"" + checkmark = "☑" if completed else "☐" + style = "style='color: lime;'" if completed else "" + text += f"" + if self._he_show and not self._he_mandatory: + text += he_row text += "
{checkmark}{step}
{checkmark}{step}
" text += "

" + "

".join(paragraph for paragraph in self.description) + "

" self.text_area.setHtml(text) diff --git a/modules/zividsamples/gui/wizard/data_directory.py b/modules/zividsamples/gui/wizard/data_directory.py index 4a1f28ce..e7867ae0 100644 --- a/modules/zividsamples/gui/wizard/data_directory.py +++ b/modules/zividsamples/gui/wizard/data_directory.py @@ -131,7 +131,10 @@ def __init__(self) -> None: self.root_folder = Path(qsettings.value("root_folder", str(Path.cwd()), type=str)) session_name = qsettings.value("session_name", "", type=str) if session_name and (self.root_folder / session_name).exists(): - self.session = SessionInfo.from_existing(self.root_folder, session_name) + try: + self.session = SessionInfo.from_existing(self.root_folder, session_name) + except (KeyError, json.JSONDecodeError, OSError): + self.session = SessionInfo.new(self.root_folder) else: self.session = SessionInfo.new(self.root_folder) self.show_on_startup = qsettings.value("show_on_startup", True, type=bool) diff --git a/modules/zividsamples/gui/wizard/hand_eye_configuration.py b/modules/zividsamples/gui/wizard/hand_eye_configuration.py index f1f3ac6c..56d46369 100644 --- a/modules/zividsamples/gui/wizard/hand_eye_configuration.py +++ b/modules/zividsamples/gui/wizard/hand_eye_configuration.py @@ -38,7 +38,10 @@ def __init__(self, *, eye_in_hand: Optional[bool] = None, calibration_object: Op calibration_object_name = settings.value( "calibration_object", CalibrationObject.Checkerboard.name, type=str ) - self.calibration_object = CalibrationObject[calibration_object_name] + try: + self.calibration_object = CalibrationObject[calibration_object_name] + except KeyError: + self.calibration_object = CalibrationObject.Checkerboard self.show_dialog = settings.value("show_dialog", True, type=bool) settings.endGroup() @@ -82,6 +85,7 @@ def __init__( radio_button_group.addButton(self.eye_in_hand_radio_button) radio_button_group.addButton(self.eye_to_hand_radio_button) self.eye_in_hand_radio_button.setChecked(self.hand_eye_configuration.eye_in_hand) + self.eye_to_hand_radio_button.setChecked(not self.hand_eye_configuration.eye_in_hand) if self.calibration_object_selection_active: self.checkerboard_object_radio_button = QRadioButton("Checkerboard") diff --git a/modules/zividsamples/gui/wizard/marker_configuration.py b/modules/zividsamples/gui/wizard/marker_configuration.py index 0e75837a..6218c9cf 100644 --- a/modules/zividsamples/gui/wizard/marker_configuration.py +++ b/modules/zividsamples/gui/wizard/marker_configuration.py @@ -137,7 +137,10 @@ def __init__(self, *, id_list: Optional[List[int]] = None, dictionary: Optional[ self.id_list = id_list else: id_list_str = settings.value("marker_configuration.id_list", "1, 2, 3, 4", type=str) - self.id_list = [int(x) for x in id_list_str.split(",")] + try: + self.id_list = [int(x) for x in id_list_str.split(",")] + except ValueError: + self.id_list = [1, 2, 3, 4] if dictionary is not None: self.dictionary = dictionary else: diff --git a/modules/zividsamples/gui/wizard/robot_configuration.py b/modules/zividsamples/gui/wizard/robot_configuration.py index 348ffc46..ccb13eb7 100644 --- a/modules/zividsamples/gui/wizard/robot_configuration.py +++ b/modules/zividsamples/gui/wizard/robot_configuration.py @@ -36,7 +36,10 @@ def __init__(self, *, robot_type: Optional[RobotEnum] = None, ip_addr: Optional[ if robot_type is not None: self._robot_type = robot_type else: - self._robot_type = RobotEnum(qsettings.value("robot_type", RobotEnum.NO_ROBOT.value)) + try: + self._robot_type = RobotEnum(qsettings.value("robot_type", RobotEnum.NO_ROBOT.value)) + except ValueError: + self._robot_type = RobotEnum.NO_ROBOT if ip_addr is not None: self.ip_addr = ip_addr else: diff --git a/modules/zividsamples/gui/wizard/rotation_format_configuration.py b/modules/zividsamples/gui/wizard/rotation_format_configuration.py index f2cf8181..5ed4619d 100644 --- a/modules/zividsamples/gui/wizard/rotation_format_configuration.py +++ b/modules/zividsamples/gui/wizard/rotation_format_configuration.py @@ -65,9 +65,12 @@ def __init__( if rotation_format is not None: self.rotation_format = rotation_format else: - self.rotation_format = RotationFormats.from_name( - settings.value("rotation_format", RotationFormats.euler.name, type=str) - ) + try: + self.rotation_format = RotationFormats.from_name( + settings.value("rotation_format", RotationFormats.euler.name, type=str) + ) + except ValueError: + self.rotation_format = RotationFormats.euler if euler_variant is not None: self.euler_variant = euler_variant diff --git a/modules/zividsamples/gui/wizard/settings_selector.py b/modules/zividsamples/gui/wizard/settings_selector.py index a2e54ba5..f1ff11d6 100644 --- a/modules/zividsamples/gui/wizard/settings_selector.py +++ b/modules/zividsamples/gui/wizard/settings_selector.py @@ -50,26 +50,33 @@ def save_settings(self, qsettings: QSettings) -> None: def load_settings( cls: type["SettingsPixelMappingIntrinsics"], qsettings: QSettings ) -> "SettingsPixelMappingIntrinsics": - settings_2d3d = ( - zivid.Settings() - if not qsettings.contains("settings_2d3d") - else zivid.Settings.from_serialized(qsettings.value("settings_2d3d")) - ) - qsettings.beginGroup("pixel_mapping") - default_pixel_mapping = PixelMapping() - pixel_mapping = PixelMapping( - row_stride=qsettings.value("row_stride", default_pixel_mapping.row_stride, type=int), - col_stride=qsettings.value("col_stride", default_pixel_mapping.col_stride, type=int), - row_offset=qsettings.value("row_offset", default_pixel_mapping.row_offset, type=float), - col_offset=qsettings.value("col_offset", default_pixel_mapping.col_offset, type=float), - ) - qsettings.endGroup() - intrinsics = ( - zivid.CameraIntrinsics() - if not qsettings.contains("intrinsics") - else zivid.CameraIntrinsics.from_serialized(qsettings.value("intrinsics")) - ) - return cls(settings_2d3d=settings_2d3d, pixel_mapping=pixel_mapping, intrinsics=intrinsics) + try: + settings_2d3d = ( + zivid.Settings() + if not qsettings.contains("settings_2d3d") + else zivid.Settings.from_serialized(qsettings.value("settings_2d3d")) + ) + qsettings.beginGroup("pixel_mapping") + default_pixel_mapping = PixelMapping() + pixel_mapping = PixelMapping( + row_stride=qsettings.value("row_stride", default_pixel_mapping.row_stride, type=int), + col_stride=qsettings.value("col_stride", default_pixel_mapping.col_stride, type=int), + row_offset=qsettings.value("row_offset", default_pixel_mapping.row_offset, type=float), + col_offset=qsettings.value("col_offset", default_pixel_mapping.col_offset, type=float), + ) + qsettings.endGroup() + intrinsics = ( + zivid.CameraIntrinsics() + if not qsettings.contains("intrinsics") + else zivid.CameraIntrinsics.from_serialized(qsettings.value("intrinsics")) + ) + return cls(settings_2d3d=settings_2d3d, pixel_mapping=pixel_mapping, intrinsics=intrinsics) + except Exception: # pylint: disable=broad-except + return cls( + settings_2d3d=zivid.Settings(), + pixel_mapping=PixelMapping(), + intrinsics=zivid.CameraIntrinsics(), + ) class SettingsForHandEyeGUI: @@ -463,8 +470,11 @@ def accept(self) -> None: ) else: production_settings = hand_eye_settings = current_widget.get_settings() - infield_frame = zivid.calibration.capture_calibration_board(self.camera) - infield_correction_settings = infield_frame.settings + try: + infield_frame = zivid.calibration.capture_calibration_board(self.camera) + infield_correction_settings = infield_frame.settings + except Exception: # pylint: disable=broad-except + infield_correction_settings = production_settings self.settings_for_hand_eye_gui = SettingsForHandEyeGUI( production=SettingsPixelMappingIntrinsics( settings_2d3d=production_settings, diff --git a/modules/zividsamples/gui/wizard/touch_configuration.py b/modules/zividsamples/gui/wizard/touch_configuration.py index 716182a1..cd2db1d1 100644 --- a/modules/zividsamples/gui/wizard/touch_configuration.py +++ b/modules/zividsamples/gui/wizard/touch_configuration.py @@ -7,15 +7,60 @@ from typing import List, Optional -from PyQt5.QtCore import QSettings +import numpy as np +from PyQt5.QtCore import QSettings, QSignalBlocker from PyQt5.QtGui import QCloseEvent from PyQt5.QtWidgets import ( QComboBox, + QDoubleSpinBox, QFormLayout, QSpinBox, + QVBoxLayout, QWidget, ) from zivid.calibration import MarkerDictionary +from zividsamples.gui.widgets.pose_widget import PoseWidget, PoseWidgetDisplayMode +from zividsamples.gui.wizard.rotation_format_configuration import RotationInformation +from zividsamples.transformation_matrix import TransformationMatrix + +_TOUCH_TOOL_TRANSFORM_KEY = "touch_tool_transform" +_DEFAULT_Z_OFFSET_MM = 300 + + +def _touch_tool_transform_from_qsettings(qsettings: QSettings) -> TransformationMatrix: + """Load touch_tool_transform from QSettings, or build from legacy z_offset. + + Args: + qsettings: QSettings group to read the touch tool transform from + + Returns: + The stored touch tool transform, or one built from the legacy z_offset + """ + try: + stored = qsettings.value(_TOUCH_TOOL_TRANSFORM_KEY) + if stored is not None and isinstance(stored, (list, tuple)) and len(stored) == 16: + matrix = np.array(stored, dtype=np.float32).reshape(4, 4) + return TransformationMatrix.from_matrix(matrix) + z_offset = qsettings.value("z_offset", _DEFAULT_Z_OFFSET_MM, type=int) + transform = TransformationMatrix() + transform.translation[2] = -float(z_offset) + return transform + except (ValueError, TypeError): + transform = TransformationMatrix() + transform.translation[2] = -float(_DEFAULT_Z_OFFSET_MM) + return transform + + +def _touch_tool_transform_to_list(transform: TransformationMatrix) -> List[float]: + """Serialize touch tool transform for QSettings. + + Args: + transform: Touch tool transform to serialize + + Returns: + The transform as a flat list of 16 floats + """ + return transform.as_matrix().flatten().tolist() class TouchConfiguration: @@ -25,7 +70,7 @@ def __init__( *, marker_id: Optional[int] = None, marker_dictionary: Optional[str] = None, - z_offset: Optional[int] = None, + touch_tool_transform: Optional[TransformationMatrix] = None, ): qsettings = QSettings("Zivid", "HandEyeGUI") qsettings.beginGroup("touch_configuration") @@ -37,10 +82,10 @@ def __init__( self.marker_dictionary = marker_dictionary else: self.marker_dictionary = qsettings.value("marker_dictionary", MarkerDictionary.aruco4x4_250, type=str) - if z_offset is not None: - self.z_offset = z_offset + if touch_tool_transform is not None: + self.touch_tool_transform = touch_tool_transform else: - self.z_offset = qsettings.value("z_offset", 300, type=int) + self.touch_tool_transform = _touch_tool_transform_from_qsettings(qsettings) qsettings.endGroup() def save_choice(self) -> None: @@ -48,17 +93,27 @@ def save_choice(self) -> None: qsettings.beginGroup("touch_configuration") qsettings.setValue("marker_id", self.marker_id) qsettings.setValue("marker_dictionary", self.marker_dictionary) - qsettings.setValue("z_offset", self.z_offset) + qsettings.setValue(_TOUCH_TOOL_TRANSFORM_KEY, _touch_tool_transform_to_list(self.touch_tool_transform)) qsettings.endGroup() def __str__(self): - return f"TouchConfiguration(marker_id={self.marker_id}, marker_dictionary={self.marker_dictionary}, z_offset={self.z_offset})" + return ( + f"TouchConfiguration(marker_id={self.marker_id}, marker_dictionary={self.marker_dictionary}, " + f"touch_tool_transform=<4x4>)" + ) class TouchConfigurationWidget(QWidget): - def __init__(self, initial_touch_configuration: TouchConfiguration = TouchConfiguration()): + def __init__( + self, + initial_touch_configuration: TouchConfiguration = TouchConfiguration(), + *, + initial_rotation_information: Optional[RotationInformation] = None, + eye_in_hand: bool = True, + ): super().__init__() self.touch_configuration = initial_touch_configuration + rotation_info = initial_rotation_information or RotationInformation() self.marker_id_selection = QSpinBox() self.marker_id_selection.setRange( @@ -70,21 +125,52 @@ def __init__(self, initial_touch_configuration: TouchConfiguration = TouchConfig self.marker_dictionary_selection.addItems(MarkerDictionary.valid_values()) self.marker_dictionary_selection.setCurrentText(self.touch_configuration.marker_dictionary) self.marker_dictionary_selection.setObjectName("Touch-marker_dictionary_selection") - self.z_offset = QSpinBox() - self.z_offset.setRange(0, 400) - self.z_offset.setValue(self.touch_configuration.z_offset) - self.z_offset.setSuffix(" mm") - self.z_offset.setObjectName("Touch-z_offset") + + self.z_offset_spinbox = QDoubleSpinBox() + self.z_offset_spinbox.setRange(-10000, 10000) + self.z_offset_spinbox.setDecimals(1) + self.z_offset_spinbox.setSuffix(" mm") + self.z_offset_spinbox.setValue(-self.touch_configuration.touch_tool_transform.translation[2]) + self.z_offset_spinbox.setObjectName("Touch-z_offset") + + self.touch_tool_pose_widget = PoseWidget( + title="Touch Tool Transform", + initial_rotation_information=rotation_info, + eye_in_hand=eye_in_hand, + display_mode=PoseWidgetDisplayMode.OnlyPose, + initial_transformation_matrix=self.touch_configuration.touch_tool_transform, + descriptive_image_paths=None, + ) + self.touch_tool_pose_widget.setVisible(False) + marker_list_layout = QFormLayout() marker_list_layout.addRow("Marker to touch:", self.marker_id_selection) marker_list_layout.addRow("Marker dictionary:", self.marker_dictionary_selection) - marker_list_layout.addRow("Touch tool length:", self.z_offset) + marker_list_layout.addRow("Z offset:", self.z_offset_spinbox) - self.setLayout(marker_list_layout) + layout = QVBoxLayout() + layout.addLayout(marker_list_layout) + layout.addWidget(self.touch_tool_pose_widget) + self.setLayout(layout) self.marker_id_selection.valueChanged.connect(self.on_marker_id_changed) self.marker_dictionary_selection.currentIndexChanged.connect(self.on_marker_dictionary_changed) - self.z_offset.valueChanged.connect(self.on_z_offset_changed) + self.z_offset_spinbox.valueChanged.connect(self._on_z_offset_changed) + self.touch_tool_pose_widget.pose_updated.connect(self._on_touch_tool_pose_updated) + + def _on_z_offset_changed(self): + transform = self.touch_configuration.touch_tool_transform + transform.translation[2] = -self.z_offset_spinbox.value() + self.touch_tool_pose_widget.set_transformation_matrix(transform) + + def _on_touch_tool_pose_updated(self): + self.touch_configuration.touch_tool_transform = self.touch_tool_pose_widget.get_transformation_matrix() + with QSignalBlocker(self.z_offset_spinbox): + self.z_offset_spinbox.setValue(-self.touch_configuration.touch_tool_transform.translation[2]) + + def toggle_advanced_view(self, advanced: bool): + self.z_offset_spinbox.setVisible(not advanced) + self.touch_tool_pose_widget.setVisible(advanced) def on_marker_id_changed(self) -> None: self.touch_configuration.marker_id = self.marker_id_selection.value() @@ -101,12 +187,14 @@ def on_marker_dictionary_changed(self) -> None: 0, MarkerDictionary.marker_count(self.touch_configuration.marker_dictionary) - 1 ) - def on_z_offset_changed(self) -> None: - self.touch_configuration.z_offset = self.z_offset.value() + def set_rotation_format(self, rotation_information: RotationInformation) -> None: + self.touch_tool_pose_widget.set_rotation_format(rotation_information) def closeEvent(self, a0: QCloseEvent) -> None: self.touch_configuration.save_choice() return super().closeEvent(a0) def get_tab_widgets_in_order(self) -> List[QWidget]: - return [self.marker_id_selection, self.marker_dictionary_selection, self.z_offset] + widgets: List[QWidget] = [self.marker_id_selection, self.marker_dictionary_selection] + widgets.extend(self.touch_tool_pose_widget.get_tab_widgets_in_order()) + return widgets