Merge pull request #81 from AllenNeuralDynamics/wip/reticle_offset

Wip/reticle offset

.gitignore

@@ -26,6 +26,7 @@ docs/_build/
 
 # User-settings
 ui/settings.json
+ui/reticle_metadata.json
 
 # Unit test / coverage reports
 .coverage

parallax/__init__.py

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

parallax/calculator.py

@@ -1,7 +1,7 @@
 import os
 import logging
 import numpy as np
-from PyQt5.QtWidgets import QWidget, QGroupBox, QLineEdit, QPushButton
+from PyQt5.QtWidgets import QWidget, QGroupBox, QLineEdit, QPushButton, QLabel
 from PyQt5.uic import loadUi
 from PyQt5.QtCore import Qt
 
@@ -13,9 +13,11 @@
 ui_dir = os.path.join(os.path.dirname(package_dir), "ui")
 
 class Calculator(QWidget):
-    def __init__(self, model):
+    def __init__(self, model, reticle_selector):
         super().__init__()
         self.model = model
+        self.reticle_selector = reticle_selector
+        self.reticle = None
 
         self.ui = loadUi(os.path.join(ui_dir, "calc.ui"), self)
         self.setWindowTitle(f"Calculator")
@@ -24,14 +26,32 @@ def __init__(self, model):
 
         # Create the number of GroupBox for the number of stages
         self.create_stage_groupboxes()
-        self.model.add_calc_instance(self)
+        self.reticle_selector.currentIndexChanged.connect(self.setCurrentReticle)
 
+        self.model.add_calc_instance(self)
+
     def show(self):
         # Refresh the list of stage to show
+        self.change_global_label()
         self.set_calc_functions()
         # Show
         super().show()  # Show the widget
 
+    def setCurrentReticle(self):
+        reticle_name = self.reticle_selector.currentText()
+        if not reticle_name:
+            return        
+        # Extract the letter from reticle_name, assuming it has the format "Global coords (A)"
+        self.reticle = reticle_name.split('(')[-1].strip(')')
+        self.change_global_label()
+
+    def change_global_label(self):
+        if self.reticle is None or self.reticle == "Global coords":
+            self.findChild(QLabel, f"labelGlobal").setText(f"     Global")
+            return
+        else:
+            self.findChild(QLabel, f"labelGlobal").setText(f"     Global ({self.reticle})")
+
     def set_calc_functions(self):
         for stage_sn, item in self.model.transforms.items():
             transM, scale = item[0], item[1]
@@ -120,15 +140,73 @@ def is_valid_number(s):
         else:
             return None, None, None
 
+    def apply_reticle_adjustments(self, global_pts):
+        reticle_metadata = self.model.get_reticle_metadata(self.reticle)
+        reticle_rot = reticle_metadata.get("rot", 0)
+        reticle_rotmat = reticle_metadata.get("rotmat", np.eye(3))  # Default to identity matrix if not found
+        reticle_offset = np.array([
+            reticle_metadata.get("offset_x", global_pts[0]), 
+            reticle_metadata.get("offset_y", global_pts[1]), 
+            reticle_metadata.get("offset_z", global_pts[2])
+        ])
+
+        if reticle_rot != 0:
+            # Transpose because points are row vectors
+            global_pts = global_pts @ reticle_rotmat.T
+        global_pts = global_pts + reticle_offset
+
+        global_x = np.round(global_pts[0], 1)
+        global_y = np.round(global_pts[1], 1)
+        global_z = np.round(global_pts[2], 1)
+        return global_x, global_y, global_z
+
     def apply_transformation(self, local_point_, transM_LR, scale):
+        """Apply transformation to convert local to global coordinates."""
         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]}")
+
+         # Ensure the reticle is defined and get its metadata
+        if self.reticle and self.reticle != "Global coords":
+            # Apply the reticle offset and rotation adjustment
+            global_x, global_y, global_z = self.apply_reticle_adjustments(global_point[:3])
+            # Return the adjusted global coordinates
+            return np.array([global_x, global_y, global_z])
+
         return global_point[:3]
-
+
+    def apply_reticle_adjustments_inverse(self, global_point):
+        """Apply reticle offset and inverse rotation to the global point."""
+        if self.reticle and self.reticle != "Global coords":
+            # Convert global_point to numpy array if it's not already
+            global_point = np.array(global_point)
+
+            # Get the reticle metadata
+            reticle_metadata = self.model.get_reticle_metadata(self.reticle)
+
+            # Get rotation matrix (default to identity if not found)
+            reticle_rotmat = reticle_metadata.get("rotmat", np.eye(3))
+
+            # Get offset values, default to global point coordinates if not found
+            reticle_offset = np.array([
+                reticle_metadata.get("offset_x", 0),  # Default to 0 if no offset is provided
+                reticle_metadata.get("offset_y", 0), 
+                reticle_metadata.get("offset_z", 0)
+            ])
+
+            # Subtract the reticle offset
+            global_point = global_point - reticle_offset
+            # Undo the rotation
+            global_point = np.dot(global_point, reticle_rotmat)
+
+        return global_point
+
     def apply_inverse_transformation(self, global_point, transM_LR, scale):
+        """Apply inverse transformation to convert global to local coordinates."""
+        global_point = self.apply_reticle_adjustments_inverse(global_point)
+
         # Transpose the 3x3 rotation part
         R_T = transM_LR[:3, :3].T
         local_point = np.dot(R_T, global_point - transM_LR[:3, 3])

parallax/main_window_wip.py

@@ -777,4 +777,5 @@ def save_user_configs(self):
     def closeEvent(self, event):
         self.model.close_all_point_meshes()
         self.model.close_clac_instance()
+        self.model.close_reticle_metadata_instance()
         event.accept()

parallax/model.py

@@ -31,6 +31,9 @@ def __init__(self, version="V1", bundle_adjustment=False):
         # Calculator
         self.calc_instance = None
 
+        # reticle metadata
+        self.reticle_metadata_instance = None
+
         # stage
         self.nStages = 0
         self.stages = {}
@@ -52,6 +55,9 @@ def __init__(self, version="V1", bundle_adjustment=False):
         # Transformation matrices of stages to global coords
         self.transforms = {}
 
+        # Reticle metadata
+        self.reticle_metadata = {}
+
     def add_calibration(self, cal):
         """Add a calibration."""
         self.calibrations[cal.name] = cal
@@ -140,6 +146,27 @@ def add_transform(self, stage_sn, transform, scale):
         """Add transformation matrix between local to global coordinates."""
         self.transforms[stage_sn] = [transform, scale]
 
+    def get_transform(self, stage_sn):
+        """Get transformation matrix between local to global coordinates."""
+        return self.transforms.get(stage_sn)
+
+    def add_reticle_metadata(self, reticle_name, metadata):
+        """Add transformation matrix between local to global coordinates."""
+        self.reticle_metadata[reticle_name] = metadata
+
+    def get_reticle_metadata(self, reticle_name):
+        """Get transformation matrix between local to global coordinates."""
+        return self.reticle_metadata.get(reticle_name)
+
+    def remove_reticle_metadata(self, reticle_name):
+        """Remove transformation matrix between local to global coordinates."""
+        if reticle_name in self.reticle_metadata.keys():
+            self.reticle_metadata.pop(reticle_name, None)
+
+    def reset_reticle_metadata(self):
+        """Reset transformation matrix between local to global coordinates."""
+        self.reticle_metadata = {}
+
     def add_probe_detector(self, probeDetector):
         """Add a probe detector."""
         self.probeDetectors.append(probeDetector)
@@ -223,4 +250,12 @@ def add_calc_instance(self, instance):
     def close_clac_instance(self):
         if self.calc_instance is not None:
             self.calc_instance.close()
+            self.calc_instance = None
+
+    def add_reticle_metadata_instance(self, instance):
+        self.reticle_metadata_instance = instance
+
+    def close_reticle_metadata_instance(self):
+        if self.reticle_metadata_instance is not None:
+            self.reticle_metadata_instance.close()
             self.calc_instance = None

parallax/probe_calibration.py

@@ -84,7 +84,6 @@ def __init__(self, model, stage_listener):
                 [0.0, 0.0, 0.0, 0.0],
             ]
         )
-
         self.model_LR, self.transM_LR, self.transM_LR_prev = None, None, None
         self.origin, self.R, self.scale = None, None, np.array([1, 1, 1])
         self.avg_err = None
@@ -272,7 +271,7 @@ def _get_transM(self, df, remove_noise=True, save_to_csv=False, file_name=None,
             if self._is_criteria_met_points_min_max() and len(local_points) > 10 \
                     and self.R is not None and self.origin is not None: 
                 local_points, global_points, valid_indices = self._remove_outliers(
-                        local_points, global_points, threshold=noise_threshold)
+                    local_points, global_points, threshold=noise_threshold)
 
         if len(local_points) <= 3 or len(global_points) <= 3:
             logger.warning("Not enough points for calibration.")
@@ -492,7 +491,7 @@ def _is_enough_points(self):
         return False
 
     def _update_info_ui(self, disp_avg_error=False, save_to_csv=False, file_name=None):
-        sn = self.stage.sn
+        sn = self.stage.sn 
         if sn is not None and sn in self.stages:
             stage_data = self.stages[sn]
 
@@ -504,7 +503,7 @@ def _update_info_ui(self, disp_avg_error=False, save_to_csv=False, file_name=Non
                 error = self.avg_err
             else:
                 error = self.LR_err_L2_current
-
+        
             self.transM_info.emit(
                 sn,
                 self.transM_LR,
@@ -596,7 +595,7 @@ def update(self, stage, debug_info=None):
         Args:
             stage (Stage): The current stage object with new position data.
         """
-        # update points in the file``
+        # update points in the file
         self.stage = stage
         self._update_local_global_point(debug_info) # Do no update if it is duplicates
 
@@ -618,6 +617,7 @@ def update(self, stage, debug_info=None):
 
     def complete_calibration(self, filtered_df):
         # save the filtered points to a new file
+        print("ProbeCalibration: complete_calibration")
         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) # TODO original
         #self.transM_LR = self._get_transM(filtered_df, save_to_csv=True, file_name=self.file_name, remove_noise=False)  # Test
@@ -629,7 +629,7 @@ def complete_calibration(self, filtered_df):
         self._print_formatted_transM()
         print("=========================================================")
         self._update_info_ui(disp_avg_error=True, save_to_csv=True, \
-                             file_name = f"transM_{self.stage.sn}.csv")
+                            file_name = f"transM_{self.stage.sn}.csv") 
 
         if self.model.bundle_adjustment:    
             self.old_transM, self.old_scale = self.transM_LR, self.scale
@@ -645,7 +645,7 @@ def complete_calibration(self, filtered_df):
                 return
 
         # Register into model
-        self.model.add_transform(self.stage.sn, self.transM_LR, self.scale)
+        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)