Wip/calculator

parallax/__init__.py

@@ -4,7 +4,7 @@
 
 import os
 
-__version__ = "0.37.22"
+__version__ = "0.37.23"
 
 # allow multiple OpenMP instances
 os.environ["KMP_DUPLICATE_LIB_OK"] = "True"

parallax/calculator.py

@@ -0,0 +1,186 @@
+import os
+import logging
+import numpy as np
+from PyQt5.QtWidgets import QWidget, QGroupBox, QLineEdit, QPushButton
+from PyQt5.uic import loadUi
+from PyQt5.QtCore import Qt
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.DEBUG)
+
+package_dir = os.path.dirname(os.path.abspath(__file__))
+debug_dir = os.path.join(os.path.dirname(package_dir), "debug")
+ui_dir = os.path.join(os.path.dirname(package_dir), "ui")
+
+class Calculator(QWidget):
+    def __init__(self, model):
+        super().__init__()
+        self.model = model
+
+        self.ui = loadUi(os.path.join(ui_dir, "calc.ui"), self)
+        self.setWindowTitle(f"Calculator")
+        self.setWindowFlags(Qt.Window | Qt.WindowMinimizeButtonHint | \
+            Qt.WindowMaximizeButtonHint | Qt.WindowCloseButtonHint)
+
+        # Create the number of GroupBox for the number of stages
+        self.create_stage_groupboxes()
+        self.model.add_calc_instance(self)
+
+    def show(self):
+        # Refresh the list of stage to show
+        self.set_calc_functions()
+        # Show
+        super().show()  # Show the widget
+
+    def set_calc_functions(self):
+        for stage_sn, item in self.model.transforms.items():
+            transM, scale = item[0], item[1]
+            if transM is not None: # Set calc function for calibrated stages
+                push_button = self.findChild(QPushButton, f"convert_{stage_sn}")
+                if not push_button:
+                    logger.warning(f"Error: QPushButton for {stage_sn} not found")
+                    continue
+                self.enable(stage_sn)
+                push_button.clicked.connect(self.create_convert_function(stage_sn, transM, scale))
+            else:   # Block calc functions for uncalibrated stages
+                self.disable(stage_sn)
+
+    def create_convert_function(self, stage_sn, transM, scale):
+        logger.debug(f"\n=== Creating convert function ===")
+        logger.debug(f"Stage SN: {stage_sn}")
+        logger.debug(f"transM: {transM}")
+        logger.debug(f"scale: {scale}")
+        return lambda: self.convert(stage_sn, transM, scale)
+
+    def convert(self, sn, transM, scale):
+        # Enable the groupBox for the stage
+        globalX = self.findChild(QLineEdit, f"globalX_{sn}").text()
+        globalY = self.findChild(QLineEdit, f"globalY_{sn}").text()
+        globalZ = self.findChild(QLineEdit, f"globalZ_{sn}").text()
+        localX = self.findChild(QLineEdit, f"localX_{sn}").text()
+        localY = self.findChild(QLineEdit, f"localY_{sn}").text()
+        localZ = self.findChild(QLineEdit, f"localZ_{sn}").text()
+
+        logger.debug("- Convert -")
+        logger.debug(f"Global: {globalX}, {globalY}, {globalZ}")
+        logger.debug(f"Local: {localX}, {localY}, {localZ}")
+        trans_type, local_pts, global_pts = self.get_transform_type(globalX, globalY, globalZ, localX, localY, localZ)
+        if trans_type == "global_to_local":
+            local_pts_ret = self.apply_inverse_transformation(global_pts, transM, scale)
+            self.show_local_pts_result(sn, local_pts_ret)
+        elif trans_type == "local_to_global":
+            global_pts_ret = self.apply_transformation(local_pts, transM, scale)
+            self.show_global_pts_result(sn, global_pts_ret)
+        else:
+            logger.warning(f"Error: Invalid transforsmation type for {sn}")
+            return
+
+    def show_local_pts_result(self, sn, local_pts):
+        # Show the local points in the QLineEdit
+        self.findChild(QLineEdit, f"localX_{sn}").setText(f"{local_pts[0]:.2f}")
+        self.findChild(QLineEdit, f"localY_{sn}").setText(f"{local_pts[1]:.2f}")
+        self.findChild(QLineEdit, f"localZ_{sn}").setText(f"{local_pts[2]:.2f}")
+
+    def show_global_pts_result(self, sn, global_pts):
+        self.findChild(QLineEdit, f"globalX_{sn}").setText(f"{global_pts[0]:.2f}")
+        self.findChild(QLineEdit, f"globalY_{sn}").setText(f"{global_pts[1]:.2f}")
+        self.findChild(QLineEdit, f"globalZ_{sn}").setText(f"{global_pts[2]:.2f}")
+
+    def get_transform_type(self, globalX, globalY, globalZ, localX, localY, localZ):
+        def is_valid_number(s):
+            try:
+                float(s)
+                return True
+            except ValueError:
+                return False
+
+        # Strip any whitespace or tabs from the inputs
+        globalX = globalX.strip() if globalX else ""
+        globalY = globalY.strip() if globalY else ""
+        globalZ = globalZ.strip() if globalZ else ""
+        localX = localX.strip() if localX else ""
+        localY = localY.strip() if localY else ""
+        localZ = localZ.strip() if localZ else ""
+
+        # Check if all global and local coordinates are valid numbers
+        global_valid = all(is_valid_number(val) for val in [globalX, globalY, globalZ])
+        local_valid = all(is_valid_number(val) for val in [localX, localY, localZ])
+
+        if global_valid and local_valid:
+            # Convert to float
+            global_pts = [float(globalX), float(globalY), float(globalZ)]
+            local_pts = [float(localX), float(localY), float(localZ)]
+            return "global_to_local", local_pts, global_pts
+        elif local_valid:
+            local_pts = [float(localX), float(localY), float(localZ)]
+            return "local_to_global", local_pts, None
+        elif global_valid:
+            global_pts = [float(globalX), float(globalY), float(globalZ)]
+            return "global_to_local", None, global_pts
+        else:
+            return None, None, None
+
+    def apply_transformation(self, local_point_, transM_LR, scale):
+        local_point = local_point_ * scale
+        local_point = np.append(local_point, 1)
+        global_point = np.dot(transM_LR, local_point)
+        logger.debug(f"local_to_global: {local_point_} -> {global_point[:3]}")
+        logger.debug(f"R: {transM_LR[:3, :3]}\nT: {transM_LR[:3, 3]}")
+        return global_point[:3]
+
+    def apply_inverse_transformation(self, global_point, transM_LR, scale):
+        # Transpose the 3x3 rotation part
+        R_T = transM_LR[:3, :3].T
+        local_point = np.dot(R_T, global_point - transM_LR[:3, 3])
+        logger.debug(f"global_to_local {global_point} -> {local_point / scale}")
+        logger.debug(f"R.T: {R_T}\nT: {transM_LR[:3, 3]}")
+        return local_point / scale
+
+    def disable(self, sn):
+        # Clear the QLineEdit for the stage
+        self.findChild(QLineEdit, f"localX_{sn}").setText(f"")
+        self.findChild(QLineEdit, f"localY_{sn}").setText(f"")
+        self.findChild(QLineEdit, f"localZ_{sn}").setText(f"")
+        self.findChild(QLineEdit, f"globalX_{sn}").setText(f"")
+        self.findChild(QLineEdit, f"globalY_{sn}").setText(f"")
+        self.findChild(QLineEdit, f"globalZ_{sn}").setText(f"")
+
+        # Find the QGroupBox for the stage
+        group_box = self.findChild(QGroupBox, f"groupBox_{sn}")
+        group_box.setEnabled(False)
+        group_box.setStyleSheet("background-color: #333333;")
+        group_box.setTitle(f"{sn} (Uncalibrated)")
+
+    def enable(self, sn):
+        # Find the QGroupBox for the stage
+        group_box = self.findChild(QGroupBox, f"groupBox_{sn}")
+        if not group_box.isEnabled():
+            group_box.setEnabled(True)
+            group_box.setStyleSheet("background-color: black;")
+            group_box.setTitle(f"{sn}")
+
+    def create_stage_groupboxes(self):
+        # Loop through the number of stages and create copies of groupBoxStage
+        for sn in self.model.stages.keys():
+            # Load the QGroupBox from the calc_QGroupBox.ui file
+            group_box = QGroupBox(self)
+            loadUi(os.path.join(ui_dir, "calc_QGroupBox.ui"), group_box)
+
+            # Set the visible title of the QGroupBox to sn
+            group_box.setTitle(f"{sn}")
+
+            # Append _{sn} to the QGroupBox object name
+            group_box.setObjectName(f"groupBox_{sn}")
+
+            # Find all QLineEdits and QPushButtons in the group_box and rename them
+            # globalX -> globalX_{sn} .. 
+            # localX -> localX_{sn} ..
+            for line_edit in group_box.findChildren(QLineEdit):
+                line_edit.setObjectName(f"{line_edit.objectName()}_{sn}")
+
+            # convert -> convert_{sn}
+            for push_button in group_box.findChildren(QPushButton):
+                push_button.setObjectName(f"{push_button.objectName()}_{sn}")
+
+            # Add the newly created QGroupBox to the layout
+            self.ui.verticalLayout_QBox.addWidget(group_box)

parallax/main_window_wip.py

@@ -183,6 +183,7 @@ def refresh_stages(self):
         """Search for connected stages"""
         if not self.dummy:
             self.model.scan_for_usb_stages()
+            self.model.init_transforms()
 
     def record_button_handler(self):
         """
@@ -775,4 +776,5 @@ def save_user_configs(self):
 
     def closeEvent(self, event):
         self.model.close_all_point_meshes()
+        self.model.close_clac_instance()
         event.accept()

parallax/model.py

@@ -28,6 +28,9 @@ def __init__(self, version="V1", bundle_adjustment=False):
         # point mesh
         self.point_mesh_instances = {}
 
+        # Calculator
+        self.calc_instance = None
+
         # stage
         self.nStages = 0
         self.stages = {}
@@ -46,18 +49,9 @@ def __init__(self, version="V1", bundle_adjustment=False):
         self.calibrations = {}
         self.coords_debug = {}
 
-        self.cal_in_progress = False
-        self.accutest_in_progress = False
-        self.lcorr, self.rcorr = False, False
-
-        self.img_point_last = None
-        self.obj_point_last = None
+        # Transformation matrices of stages to global coords
         self.transforms = {}
 
-    def set_last_object_point(self, obj_point):
-        """Set the last object point."""
-        self.obj_point_last = obj_point
-
     def add_calibration(self, cal):
         """Add a calibration."""
         self.calibrations[cal.name] = cal
@@ -66,27 +60,15 @@ def set_calibration(self, calibration):
         """Set the calibration."""
         self.calibration = calibration
 
-    def set_lcorr(self, xc, yc):
-        """Set left coordinates."""
-        self.lcorr = [xc, yc]
-
-    def clear_lcorr(self):
-        """Clear left coordinates."""
-        self.lcorr = False
-
-    def set_rcorr(self, xc, yc):
-        """Set right coordinates."""
-        self.rcorr = [xc, yc]
-
-    def clear_rcorr(self):
-        """Clear right coordinates."""
-        self.rcorr = False
-
     def init_stages(self):
         """Initialize stages."""
         self.stages = {}
         self.stages_calib = {}
 
+    def init_transforms(self):
+        for stage_sn in self.stages.keys():
+            self.transforms[stage_sn] = [None, None]
+
     def add_video_source(self, video_source):
         """Add a video source."""
         self.cameras.append(video_source)
@@ -134,7 +116,16 @@ def get_stage(self, stage_sn):
         return self.stages.get(stage_sn)
 
     def add_stage_calib_info(self, stage_sn, info):
-        """Add a stage."""
+        """Add a stage.
+        info['detection_status']
+        info['transM']
+        info['L2_err']
+        info['scale']
+        info['dist_traveled']
+        info['status_x']
+        info['status_y']
+        info['status_z']
+        """
         self.stages_calib[stage_sn] = info
 
     def get_stage_calib_info(self, stage_sn):
@@ -145,6 +136,10 @@ def reset_stage_calib_info(self):
         """Reset stage calibration info."""
         self.stages_calib = {}
 
+    def add_transform(self, stage_sn, transform, scale):
+        """Add transformation matrix between local to global coordinates."""
+        self.transforms[stage_sn] = [transform, scale]
+
     def add_probe_detector(self, probeDetector):
         """Add a probe detector."""
         self.probeDetectors.append(probeDetector)
@@ -154,7 +149,7 @@ def reset_coords_intrinsic_extrinsic(self):
         self.coords_axis = {}
         self.camera_intrinsic = {}
         self.camera_extrinsic = {}
-    
+
     def add_pos_x(self, camera_name, pt):
         """Add position x."""
         self.pos_x[camera_name] = pt
@@ -220,4 +215,12 @@ def add_point_mesh_instance(self, instance):
     def close_all_point_meshes(self):
         for instance in self.point_mesh_instances.values():
             instance.close()
-        self.point_mesh_instances.clear()
+        self.point_mesh_instances.clear()
+
+    def add_calc_instance(self, instance):
+        self.calc_instance = instance
+
+    def close_clac_instance(self):
+        if self.calc_instance is not None:
+            self.calc_instance.close()
+            self.calc_instance = None

parallax/point_mesh.py

@@ -71,7 +71,6 @@ def _parse_csv(self):
         self.local_pts_org = self.df[['local_x', 'local_y', 'local_z']].values
         self.local_pts = self._local_to_global(self.local_pts_org, self.R[self.sn], self.T[self.sn], self.S[self.sn])
         self.points_dict['local_pts'] = self.local_pts
-        print(self.R[self.sn], self.T[self.sn], self.S[self.sn])
 
         self.global_pts = self.df[['global_x', 'global_y', 'global_z']].values
         self.points_dict['global_pts'] = self.global_pts

parallax/probe_calibration.py

@@ -152,14 +152,16 @@ def clear(self, sn = None):
         """
         Clears all stored data and resets the transformation matrix to its default state.
         """
+        self.model_LR, self.transM_LR, self.transM_LR_prev = None, None, None
+        self.scale = np.array([1, 1, 1])
+
         if sn is None:
             self._create_file()
         else:
             self.df = pd.read_csv(self.csv_file)
             self.df = self.df[self.df["sn"] != sn]
             self.df.to_csv(self.csv_file, index=False)
-        self.model_LR, self.transM_LR, self.transM_LR_prev = None, None, None
-        self.scale = np.array([1, 1, 1])
+            self.model.add_transform(sn, self.transM_LR, self.scale)
 
     def _remove_duplicates(self, df):
         # Drop duplicate rows based on 'ts_local_coords', 'global_x', 'global_y', 'global_z' columns
@@ -264,6 +266,7 @@ def _get_transM_LR_orthogonal(self, local_points, global_points, remove_noise=Tr
     def _get_transM(self, df, remove_noise=True, save_to_csv=False, file_name=None, noise_threshold=40):
 
         local_points, global_points = self._get_local_global_points(df)
+        valid_indices = np.ones(len(local_points), dtype=bool) # Initialize valid_indices as a mask with all True values
 
         if remove_noise:
             if self._is_criteria_met_points_min_max() and len(local_points) > 10 \
@@ -598,7 +601,8 @@ def update(self, stage, debug_info=None):
         self._update_local_global_point(debug_info) # Do no update if it is duplicates
 
         filtered_df = self._filter_df_by_sn(self.stage.sn)
-        self.transM_LR = self._get_transM(filtered_df)
+        self.transM_LR = self._get_transM(filtered_df, noise_threshold=100) # TODO original
+        #self.transM_LR = self._get_transM(filtered_df, remove_noise=False) # Test
         if self.transM_LR is None:
             return
 
@@ -615,7 +619,9 @@ def update(self, stage, debug_info=None):
     def complete_calibration(self, filtered_df):
         # save the filtered points to a new file
         self.file_name = f"points_{self.stage.sn}.csv"
-        self.transM_LR = self._get_transM(filtered_df, save_to_csv=True, file_name=self.file_name, noise_threshold=20) 
+        self.transM_LR = self._get_transM(filtered_df, save_to_csv=True, file_name=self.file_name, noise_threshold=20) # TODO original
+        #self.transM_LR = self._get_transM(filtered_df, save_to_csv=True, file_name=self.file_name, remove_noise=False)  # Test
+
         if self.transM_LR is None:
             return
 
@@ -638,6 +644,9 @@ def complete_calibration(self, filtered_df):
             else:
                 return
 
+        # Register into model
+        self.model.add_transform(self.stage.sn, self.transM_LR, self.scale)
+
         # Emit the signal to indicate that calibration is complete         
         self.calib_complete.emit(self.stage.sn, self.transM_LR, self.scale)
         logger.debug(