Merge pull request #84 from AllenNeuralDynamics/wip/get_location_from…

…_visual_data_BA

Wip/get location from visual data ba

parallax/__init__.py

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

parallax/camera.py

@@ -721,6 +721,8 @@ def __init__(self):
         )
         self._next_frame = 0
         self.device_color_type = None
+        self.width = 4000
+        self.height = 3000
 
     def name(self, sn_only=False):
         """Get the name of the mock camera"""

parallax/model.py

@@ -1,6 +1,7 @@
 """
 The Model class is the core component for managing cameras, stages, and calibration data.
 """
+from collections import OrderedDict
 from PyQt5.QtCore import QObject, pyqtSignal
 from .camera import MockCamera, PySpinCamera, close_cameras, list_cameras
 from .stage_listener import Stage, StageInfo
@@ -47,8 +48,8 @@ def __init__(self, version="V1", bundle_adjustment=False):
         self.pos_x = {}
         self.camera_intrinsic = {}
         self.camera_extrinsic = {}
-        self.stereo_instance = None
         self.best_camera_pair = None
+        self.stereo_calib_instance = {}
         self.calibration = None
         self.calibrations = {}
         self.coords_debug = {}
@@ -60,7 +61,7 @@ def __init__(self, version="V1", bundle_adjustment=False):
         self.reticle_metadata = {}
 
         # clicked pts
-        self.clicked_pts = {}
+        self.clicked_pts = OrderedDict()
 
     def add_calibration(self, cal):
         """Add a calibration."""
@@ -147,7 +148,10 @@ def reset_stage_calib_info(self):
         self.stages_calib = {}
 
     def add_pts(self, camera_name, pts):
-        """Add points."""
+        """Add points. If a new camera is added and the size exceeds 2, remove the oldest."""
+        if len(self.clicked_pts) == 2 and camera_name not in self.clicked_pts:
+            # Remove the oldest entry (first added item)
+            self.clicked_pts.popitem(last=False)
         self.clicked_pts[camera_name] = pts
 
     def get_pts(self, camera_name):
@@ -160,7 +164,7 @@ def get_cameras_detected_pts(self):
 
     def reset_pts(self):
         """Reset points."""
-        self.clicked_pts = {}
+        self.clicked_pts = OrderedDict()
 
     def add_transform(self, stage_sn, transform, scale):
         """Add transformation matrix between local to global coordinates."""
@@ -233,11 +237,14 @@ def get_camera_intrinsic(self, camera_name):
         """Get camera intrinsic parameters."""
         return self.camera_intrinsic.get(camera_name)
 
-    def add_stereo_instance(self, instance):
-        self.stereo_instance = instance
+    def add_stereo_calib_instance(self, sorted_key, instance):
+        self.stereo_calib_instance[sorted_key] = instance
 
-    def reset_stereo_instance(self):
-        self.stereo_instance = None
+    def get_stereo_calib_instance(self, sorted_key):
+        return self.stereo_calib_instance.get(sorted_key)
+
+    def reset_stereo_calib_instance(self):
+        self.stereo_calib_instance = {}
 
     def add_camera_extrinsic(self, name1, name2, retVal, R, T, E, F):
         """Add camera extrinsic parameters."""

parallax/probe_calibration.py

@@ -601,8 +601,7 @@ 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, noise_threshold=100) # TODO original
-        #self.transM_LR = self._get_transM(filtered_df, remove_noise=False) # Test
+        self.transM_LR = self._get_transM(filtered_df, noise_threshold=100)
         if self.transM_LR is None:
             return
 
@@ -620,9 +619,8 @@ 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
-
+        self.transM_LR = self._get_transM(filtered_df, save_to_csv=True, file_name=self.file_name, noise_threshold=20) 
+
         if self.transM_LR is None:
             return
 

parallax/screen_coords_mapper.py

@@ -20,7 +20,10 @@ def __init__(self, model, screen_widgets, reticle_selector, x, y, z):
     def _clicked_position(self, camera_name, pos):
         """Get clicked position."""
         self._register_pt(camera_name, pos)
-        global_coords = self._get_global_coords(camera_name, pos)
+        if not self.model.bundle_adjustment:
+            global_coords = self._get_global_coords_stereo(camera_name, pos)
+        else:
+            global_coords = self._get_global_coords_BA(camera_name, pos)
         if global_coords is None:
             return
 
@@ -73,9 +76,9 @@ def _register_pt(self, camera_name, pos):
         if pos is not None and camera_name is not None:
             self.model.add_pts(camera_name, pos)
 
-    def _get_global_coords(self, camera_name, pos):
+    def _get_global_coords_stereo(self, camera_name, pos):
         """Calculate global coordinates based on the best camera pair."""
-        if self.model.stereo_instance is None:
+        if self.model.stereo_calib_instance is None:
             logger.debug("Stereo instance is None")
             return None
 
@@ -105,9 +108,56 @@ def _get_global_coords(self, camera_name, pos):
             tip_coordsB = pos
 
         # Calculate global coordinates using stereo instance
-        global_coords = self.model.stereo_instance.get_global_coords(
+        stereo_instance = self._get_calibration_instance(camA_best, camB_best)
+        if stereo_instance is None:
+            logger.debug(f"Stereo calibration instance not found for cameras: {camA_best}, {camB_best}")
+            return None
+
+        global_coords = stereo_instance.get_global_coords(
             camA_best, tip_coordsA, camB_best, tip_coordsB
         )
 
         return global_coords[0]
 
+    def _get_global_coords_BA(self, camera_name, pos):
+        """Calculate global coordinates based on the best camera pair."""
+        if self.model.stereo_calib_instance is None:
+            logger.debug("Stereo instance is None")
+            return None
+
+        # Get detected points from cameras
+        cameras_detected_pts = self.model.get_cameras_detected_pts()
+        if len(cameras_detected_pts) < 2:
+            logger.debug("Not enough detected points to calculate global coordinates")
+            return None
+
+        camA, camB = None, None
+        tip_coordsA, tip_coordsB = None, None
+        for camera, pts in cameras_detected_pts.items():
+            if camera_name == camera:
+                camA = camera
+                tip_coordsA = pos
+            else:
+                camB = camera
+                tip_coordsB = pts
+
+        if not camA or not camB or tip_coordsA is None or tip_coordsB is None:
+            logger.debug("Insufficient camera data to compute global coordinates")
+            return None
+
+        # Calculate global coordinates using stereo instance
+        stereo_instance = self._get_calibration_instance(camA, camB)
+        if stereo_instance is None:
+            logger.debug(f"Stereo calibration instance not found for cameras: {camA}, {camB}")
+            return None
+
+        # Calculate global coordinates using the stereo instance
+        global_coords = stereo_instance.get_global_coords(
+            camA, tip_coordsA, camB, tip_coordsB
+        )
+
+        return global_coords[0]
+
+    def _get_calibration_instance(self, camA, camB):
+        sorted_key = tuple(sorted((camA, camB)))
+        return self.model.get_stereo_calib_instance(sorted_key)

parallax/stage_widget.py

@@ -246,7 +246,7 @@ def reticle_detect_default_status(self):
             # Disable probe calibration
             self.probe_detect_default_status()
         self.model.reset_stage_calib_info()
-        self.model.reset_stereo_instance()
+        self.model.reset_stereo_calib_instance()
         self.model.reset_camera_extrinsic()
         self.probeCalibration.clear()
 
@@ -501,9 +501,11 @@ def calibrate_stereo(self, cam_names, intrinsics, img_coords):
                     self.camA_best, self.camB_best = camA, camB
                     coordsA_best, coordsB_best = coordsA, coordsB
                     itmxA_best, itmxB_best = itmxA, itmxB
+
 
         # Update the model with the calibration results
-        self.model.add_stereo_instance(self.calibrationStereo)
+        sorted_key = tuple(sorted((self.camA_best, self.camB_best)))
+        self.model.add_stereo_calib_instance(sorted_key, self.calibrationStereo)
         self.model.add_camera_extrinsic(
             self.camA_best, self.camB_best, min_err, R_AB_best, T_AB_best, E_AB_best, F_AB_best
         )
@@ -522,9 +524,6 @@ def calibrate_all_cameras(self, cam_names, intrinsics, img_coords):
         # Stereo Camera Calibration
         calibrationStereo = None
 
-        # Dictionary to store instances with sorted camera names as keys
-        self.calibrationStereoInstances = {}
-
         # Perform calibration between pairs of cameras
         print(cam_names)
 
@@ -540,20 +539,13 @@ def calibrate_all_cameras(self, cam_names, intrinsics, img_coords):
                     camA, coordsA, itmxA, camB, coordsB, itmxB
                 )
                 print("\n--------------------------------------------------------")
-                print(f"camsera pair: {camA}-{camB}, err: {np.round(err, 2) * 1000} µm³")
-                logger.debug(f"=== camera pair: {camA}-{camB}, err: {np.round(err, 2) * 1000} µm³ ===")
+                print(f"camsera pair: {camA}-{camB}")
+                logger.debug(f"=== camera pair: {camA}-{camB} ===")
                 logger.debug(f"R: \n{R_AB}\nT: \n{T_AB}")
 
                 # Store the instance with a sorted tuple key
                 sorted_key = tuple(sorted((camA, camB)))
-                self.calibrationStereoInstances[sorted_key] = {
-                    "instance": calibrationStereo,
-                    "error": err,
-                    "R_AB": R_AB,
-                    "T_AB": T_AB,
-                    "E_AB": E_AB,
-                    "F_AB": F_AB,
-                }
+                self.model.add_stereo_calib_instance(sorted_key, calibrationStereo)
 
                 #calibrationStereo.print_calibrate_stereo_results(camA, camB)
                 err = calibrationStereo.test_performance(camA, coordsA, camB, coordsB, print_results=True)
@@ -565,7 +557,7 @@ def calibrate_all_cameras(self, cam_names, intrinsics, img_coords):
     # Example of how to retrieve the instance with either (camA, camB) or (camB, camA)
     def get_calibration_instance(self, camA, camB):
         sorted_key = tuple(sorted((camA, camB)))
-        return self.calibrationStereoInstances.get(sorted_key)
+        return self.model.get_stereo_calib_instance(sorted_key)
 
     def calibrate_cameras(self):
         """
@@ -687,10 +679,6 @@ def probe_detect_on_screens(self, camA, timestampA, snA, stage_info, tip_coordsA
         if (camA is None) or (timestampA is None) or (snA is None) or (tip_coordsA is None):
             return
 
-        if self.calibrationStereoInstances is None:
-            logger.debug(f"Camera calibration has not done. {camA}")
-            return
-
         for screen in self.screen_widgets:
             camB = screen.get_camera_name()
             if camA == camB:
@@ -710,7 +698,6 @@ def probe_detect_on_screens(self, camA, timestampA, snA, stage_info, tip_coordsA
                 logger.debug(f"Camera calibration has not done {camA}, {camB}")
                 continue
 
-            calibrationStereoInstance = calibrationStereoInstance["instance"]
             global_coords = calibrationStereoInstance.get_global_coords(
                 camA, tip_coordsA, camB, tip_coordsB
             )