Bugfix/trans m

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"

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