From 3947005e8b5644e36271f4d872ed77a1b2feebd7 Mon Sep 17 00:00:00 2001 From: hannalee2 Date: Mon, 3 Jun 2024 16:51:53 -0700 Subject: [PATCH 1/2] Implemented reflection on the transM (Rotation + translation + reflection on z axis --- parallax/coords_transformation.py | 41 +++++++++++++++++++++++++------ parallax/stage_listener.py | 1 + parallax/stage_ui.py | 3 ++- 3 files changed, 37 insertions(+), 8 deletions(-) diff --git a/parallax/coords_transformation.py b/parallax/coords_transformation.py index 90ee21c..2ae4a07 100644 --- a/parallax/coords_transformation.py +++ b/parallax/coords_transformation.py @@ -30,19 +30,25 @@ def extractAngles(self, mat): z = np.arctan2(mat[1, 0], mat[0, 0]) return x, y, z - def combineAngles(self, x, y, z): + def combineAngles(self, x, y, z, reflect_z=False): """Combines separate roll, pitch, and yaw angles into a single rotation matrix.""" eye = np.identity(3) R = self.roll( self.pitch( self.yaw(eye, z), y), x) + + if reflect_z: + reflection_matrix = np.array([[1, 0, 0], + [0, 1, 0], + [0, 0, -1]]) + R = R @ reflection_matrix return R - def func(self, x, measured_pts, global_pts): + def func(self, x, measured_pts, global_pts, reflect_z=False): """Defines an error function for the optimization, which calculates the difference between transformed global points and measured points.""" - R = self.combineAngles(x[2], x[1], x[0]) + R = self.combineAngles(x[2], x[1], x[0], reflect_z=reflect_z) origin = np.array([x[3], x[4], x[5]]).T error_values = np.zeros(len(global_pts) * 3) @@ -54,13 +60,34 @@ def func(self, x, measured_pts, global_pts): return error_values + def total_error(self, x, measured_pts, global_pts, reflect_z=False): + """Calculates the total error for the optimization.""" + error_values = self.func(x, measured_pts, global_pts, reflect_z) + total_error = np.sum(error_values**2) + return total_error + def fit_params(self, measured_pts, global_pts): """Fits parameters to minimize the error defined in func""" x0 = np.array([0, 0, 0, 0, 0, 0]) # initial guess: (x, y, z, x_t, y_t, z_t) if len(measured_pts) < 3 or len(global_pts) < 3: raise ValueError("At least two points are required for optimization.") - res = leastsq(self.func, x0, args=(measured_pts, global_pts)) - rez = res[0] - R = self.combineAngles(rez[2], rez[1], rez[0]) + + # Optimize without reflection + res1 = leastsq(self.func, x0, args=(measured_pts, global_pts, False)) + total_error1 = self.total_error(res1[0], measured_pts, global_pts, False) + + # Optimize with reflection + res2 = leastsq(self.func, x0, args=(measured_pts, global_pts, True)) + total_error2 = self.total_error(res2[0], measured_pts, global_pts, True) + + # Select the transformation with the smaller total error + print(f"no_reflect: {total_error1}, reflect: {total_error2}") + if total_error1 < total_error2: + rez = res1[0] + R = self.combineAngles(rez[2], rez[1], rez[0], reflect_z=False) + else: + rez = res2[0] + R = self.combineAngles(rez[2], rez[1], rez[0], reflect_z=True) + origin = rez[3:] - return origin, R # translation vector and rotation matrix + return origin, R # translation vector and rotation matrix \ No newline at end of file diff --git a/parallax/stage_listener.py b/parallax/stage_listener.py index 9f6e52b..5eb1e8c 100644 --- a/parallax/stage_listener.py +++ b/parallax/stage_listener.py @@ -268,6 +268,7 @@ def handleDataChange(self, probe): local_coords_x = round(probe["Stage_X"] * 1000, 1) local_coords_y = round(probe["Stage_Y"] * 1000, 1) local_coords_z = 15000 - round(probe["Stage_Z"] * 1000, 1) + #local_coords_z = round(probe["Stage_Z"] * 1000, 1) # update into model moving_stage = self.model.stages.get(sn) diff --git a/parallax/stage_ui.py b/parallax/stage_ui.py index bbf7a13..fa5ff40 100644 --- a/parallax/stage_ui.py +++ b/parallax/stage_ui.py @@ -86,7 +86,8 @@ def updateStageLocalCoords(self): if self.selected_stage: self.ui.local_coords_x.setText(str(self.selected_stage.stage_x)) self.ui.local_coords_y.setText(str(self.selected_stage.stage_y)) - self.ui.local_coords_z.setText(str(self.selected_stage.stage_z)) + self.ui.local_coords_z.setText(str(self.selected_stage.stage_z)) # TODO + #self.ui.local_coords_z.setText(str(15000 - self.selected_stage.stage_z)) def updateStageGlobalCoords(self): """Update the displayed global coordinates of the selected stage.""" From 6511e829cbbab04270ceb940689d73e8d18389b0 Mon Sep 17 00:00:00 2001 From: hannalee2 Date: Tue, 4 Jun 2024 09:27:26 -0700 Subject: [PATCH 2/2] Remove unused codes --- parallax/__init__.py | 2 +- parallax/coords_transformation.py | 13 ++++++------- parallax/stage_listener.py | 1 - parallax/stage_ui.py | 3 +-- 4 files changed, 8 insertions(+), 11 deletions(-) diff --git a/parallax/__init__.py b/parallax/__init__.py index d92e5b4..4e6d1c8 100644 --- a/parallax/__init__.py +++ b/parallax/__init__.py @@ -4,7 +4,7 @@ import os -__version__ = "0.37.9" +__version__ = "0.37.10" # allow multiple OpenMP instances os.environ["KMP_DUPLICATE_LIB_OK"] = "True" diff --git a/parallax/coords_transformation.py b/parallax/coords_transformation.py index 2ae4a07..644b8da 100644 --- a/parallax/coords_transformation.py +++ b/parallax/coords_transformation.py @@ -60,11 +60,11 @@ def func(self, x, measured_pts, global_pts, reflect_z=False): return error_values - def total_error(self, x, measured_pts, global_pts, reflect_z=False): + def avg_error(self, x, measured_pts, global_pts, reflect_z=False): """Calculates the total error for the optimization.""" error_values = self.func(x, measured_pts, global_pts, reflect_z) - total_error = np.sum(error_values**2) - return total_error + ave_error = np.sum(error_values**2)/len(error_values) + return ave_error def fit_params(self, measured_pts, global_pts): """Fits parameters to minimize the error defined in func""" @@ -74,15 +74,14 @@ def fit_params(self, measured_pts, global_pts): # Optimize without reflection res1 = leastsq(self.func, x0, args=(measured_pts, global_pts, False)) - total_error1 = self.total_error(res1[0], measured_pts, global_pts, False) + avg_error1 = self.avg_error(res1[0], measured_pts, global_pts, False) # Optimize with reflection res2 = leastsq(self.func, x0, args=(measured_pts, global_pts, True)) - total_error2 = self.total_error(res2[0], measured_pts, global_pts, True) + avg_error2 = self.avg_error(res2[0], measured_pts, global_pts, True) # Select the transformation with the smaller total error - print(f"no_reflect: {total_error1}, reflect: {total_error2}") - if total_error1 < total_error2: + if avg_error1 < avg_error2: rez = res1[0] R = self.combineAngles(rez[2], rez[1], rez[0], reflect_z=False) else: diff --git a/parallax/stage_listener.py b/parallax/stage_listener.py index 5eb1e8c..9f6e52b 100644 --- a/parallax/stage_listener.py +++ b/parallax/stage_listener.py @@ -268,7 +268,6 @@ def handleDataChange(self, probe): local_coords_x = round(probe["Stage_X"] * 1000, 1) local_coords_y = round(probe["Stage_Y"] * 1000, 1) local_coords_z = 15000 - round(probe["Stage_Z"] * 1000, 1) - #local_coords_z = round(probe["Stage_Z"] * 1000, 1) # update into model moving_stage = self.model.stages.get(sn) diff --git a/parallax/stage_ui.py b/parallax/stage_ui.py index fa5ff40..bbf7a13 100644 --- a/parallax/stage_ui.py +++ b/parallax/stage_ui.py @@ -86,8 +86,7 @@ def updateStageLocalCoords(self): if self.selected_stage: self.ui.local_coords_x.setText(str(self.selected_stage.stage_x)) self.ui.local_coords_y.setText(str(self.selected_stage.stage_y)) - self.ui.local_coords_z.setText(str(self.selected_stage.stage_z)) # TODO - #self.ui.local_coords_z.setText(str(15000 - self.selected_stage.stage_z)) + self.ui.local_coords_z.setText(str(self.selected_stage.stage_z)) def updateStageGlobalCoords(self): """Update the displayed global coordinates of the selected stage."""