From c132a4ec032fed5afc33100a365b735511bbc5cc Mon Sep 17 00:00:00 2001 From: hannalee2 Date: Thu, 19 Sep 2024 11:25:38 -0700 Subject: [PATCH 1/5] Add width/height for mock camera --- parallax/camera.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/parallax/camera.py b/parallax/camera.py index 7b3b9cc..7fa1620 100755 --- a/parallax/camera.py +++ b/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""" From 2efcb848d4d366f05a3a4176e8b84ab6161a6cc2 Mon Sep 17 00:00:00 2001 From: hannalee2 Date: Thu, 19 Sep 2024 11:26:47 -0700 Subject: [PATCH 2/5] Implemented getting 3D position from visual data on the Bundle Adjustment mode --- parallax/model.py | 23 ++++++++----- parallax/screen_coords_mapper.py | 58 +++++++++++++++++++++++++++++--- parallax/stage_widget.py | 25 ++++++++------ 3 files changed, 83 insertions(+), 23 deletions(-) diff --git a/parallax/model.py b/parallax/model.py index 34f6c29..6d73658 100755 --- a/parallax/model.py +++ b/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.""" diff --git a/parallax/screen_coords_mapper.py b/parallax/screen_coords_mapper.py index 9f6d722..01d2765 100644 --- a/parallax/screen_coords_mapper.py +++ b/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) \ No newline at end of file diff --git a/parallax/stage_widget.py b/parallax/stage_widget.py index 915ac2f..3e95edf 100644 --- a/parallax/stage_widget.py +++ b/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,12 @@ 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) + #self.model.add_stereo_instance(self.calibrationStereo) TODO + 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 ) @@ -523,7 +526,7 @@ def calibrate_all_cameras(self, cam_names, intrinsics, img_coords): calibrationStereo = None # Dictionary to store instances with sorted camera names as keys - self.calibrationStereoInstances = {} + #self.calibrationStereoInstances = {} # Perform calibration between pairs of cameras print(cam_names) @@ -546,14 +549,16 @@ def calibrate_all_cameras(self, cam_names, intrinsics, img_coords): # Store the instance with a sorted tuple key sorted_key = tuple(sorted((camA, camB))) - self.calibrationStereoInstances[sorted_key] = { + """ # TODO + 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 +570,8 @@ 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.calibrationStereoInstances.get(sorted_key) TODO + return self.model.get_stereo_calib_instance(sorted_key) def calibrate_cameras(self): """ @@ -687,10 +693,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 +712,8 @@ 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"] + # calibrationStereoInstance = calibrationStereoInstance["instance"] TODO + #calibrationStereoInstance = calibrationStereoInstance["instance"] global_coords = calibrationStereoInstance.get_global_coords( camA, tip_coordsA, camB, tip_coordsB ) From 7ae27cf05844109e1ef8f86c10b243d34f8a9ff4 Mon Sep 17 00:00:00 2001 From: hannalee2 Date: Thu, 19 Sep 2024 11:29:36 -0700 Subject: [PATCH 3/5] Update release tag --- parallax/__init__.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/parallax/__init__.py b/parallax/__init__.py index df8bafb..879ba9f 100644 --- a/parallax/__init__.py +++ b/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" From 05e3e6a10e7072f7c3e7ee01fdda920043c49532 Mon Sep 17 00:00:00 2001 From: hannalee2 Date: Thu, 19 Sep 2024 11:40:09 -0700 Subject: [PATCH 4/5] Remove unused debug msg --- parallax/stage_widget.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/parallax/stage_widget.py b/parallax/stage_widget.py index 3e95edf..9cf5f43 100644 --- a/parallax/stage_widget.py +++ b/parallax/stage_widget.py @@ -543,8 +543,8 @@ 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 From 1a179f852b7799554fb18570ed59af130d9da375 Mon Sep 17 00:00:00 2001 From: hannalee2 Date: Thu, 19 Sep 2024 12:40:32 -0700 Subject: [PATCH 5/5] Remove unused codes --- parallax/probe_calibration.py | 8 +++----- parallax/stage_widget.py | 16 ---------------- 2 files changed, 3 insertions(+), 21 deletions(-) diff --git a/parallax/probe_calibration.py b/parallax/probe_calibration.py index c2f0e66..fd3be61 100644 --- a/parallax/probe_calibration.py +++ b/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 diff --git a/parallax/stage_widget.py b/parallax/stage_widget.py index 9cf5f43..977ac43 100644 --- a/parallax/stage_widget.py +++ b/parallax/stage_widget.py @@ -504,7 +504,6 @@ def calibrate_stereo(self, cam_names, intrinsics, img_coords): # Update the model with the calibration results - #self.model.add_stereo_instance(self.calibrationStereo) TODO 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( @@ -525,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) @@ -549,15 +545,6 @@ def calibrate_all_cameras(self, cam_names, intrinsics, img_coords): # Store the instance with a sorted tuple key sorted_key = tuple(sorted((camA, camB))) - """ # TODO - 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) @@ -570,7 +557,6 @@ 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) TODO return self.model.get_stereo_calib_instance(sorted_key) def calibrate_cameras(self): @@ -712,8 +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"] TODO - #calibrationStereoInstance = calibrationStereoInstance["instance"] global_coords = calibrationStereoInstance.get_global_coords( camA, tip_coordsA, camB, tip_coordsB )