diff --git a/parallax/__init__.py b/parallax/__init__.py index d92e5b48..4e6d1c85 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 90ee21c8..644b8dad 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,33 @@ def func(self, x, measured_pts, global_pts): return error_values + 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) + 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""" 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)) + 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)) + avg_error2 = self.avg_error(res2[0], measured_pts, global_pts, True) + + # Select the transformation with the smaller total error + if avg_error1 < avg_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