Merge pull request #79 from AllenNeuralDynamics/wip/enhance_probe_det…

…ection

Wip/enhance probe detection

.gitignore

@@ -16,6 +16,7 @@ gitkey.pub
 *.egg-info
 build/
 debug/
+debug_images/
 wheels/
 lib/
 dist/

parallax/__init__.py

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

parallax/calibration_camera.py

@@ -228,8 +228,9 @@ class CalibrationStereo(CalibrationCamera):
     """
 
     def __init__(
-        self, camA, imgpointsA, intrinsicA, camB, imgpointsB, intrinsicB):
+        self, model, camA, imgpointsA, intrinsicA, camB, imgpointsB, intrinsicB):
         """Initialize the CalibrationStereo object"""
+        self.model = model
         self.n_interest_pixels = X_COORDS_HALF
         self.camA = camA
         self.camB = camB
@@ -428,7 +429,7 @@ def test_x_y_z_performance(self, points_3d_G):
         l2_z = np.sqrt(mean_squared_diff_z)
 
         print(
-            f"x: {l2_x*1000}µm³, y: {l2_y*1000}µm³, z:{l2_z*1000}µm³"
+            f"x: {np.round(l2_x*1000, 2)}µm³, y: {np.round(l2_y*1000, 2)}µm³, z:{np.round(l2_z*1000, 2)}µm³"
         )
 
     def test_performance(self, camA, coordA, camB, coordB, print_results=False):
@@ -463,13 +464,14 @@ def test_performance(self, camA, coordA, camB, coordB, print_results=False):
         average_L2_distance = np.mean(euclidean_distances)
         if print_results:
             print(
-                f"(Reprojection error) Object points L2 diff: \
-                {average_L2_distance*1000} µm³"
+                f"(Reprojection error) Object points L2 diff: {np.round(average_L2_distance*1000, 2)} µm³"
             )
             self.test_x_y_z_performance(points_3d_G)
             logger.debug(f"Object points predict:\n{np.around(points_3d_G, decimals=5)}")
 
             self.test_pixel_error()
+
+        self.register_debug_points(camA, camB)
         return average_L2_distance
 
     def test_pixel_error(self):
@@ -479,16 +481,9 @@ def test_pixel_error(self):
             imgpointsA_converted = np.array(
                 self.imgpointsA[i], dtype=np.float32
             ).reshape(-1, 2)
-            solvePnP_method = cv2.SOLVEPNP_ITERATIVE
-            retval, rvecs, tvecs = cv2.solvePnP(
-                self.objpoints[i],
-                imgpointsA_converted,
-                self.mtxA,
-                self.distA,
-                flags=solvePnP_method,
-            )
+
             imgpoints2, _ = cv2.projectPoints(
-                self.objpoints[i], rvecs, tvecs, self.mtxA, self.distA
+                self.objpoints[i], self.rvecA, self.tvecA, self.mtxA, self.distA
             )
 
             imgpoints2_reshaped = imgpoints2.reshape(-1, 2)
@@ -507,16 +502,9 @@ def test_pixel_error(self):
             imgpointsB_converted = np.array(
                 self.imgpointsB[i], dtype=np.float32
             ).reshape(-1, 2)
-            solvePnP_method = cv2.SOLVEPNP_ITERATIVE
-            retval, rvecs, tvecs = cv2.solvePnP(
-                self.objpoints[i],
-                imgpointsB_converted,
-                self.mtxB,
-                self.distB,
-                flags=solvePnP_method,
-            )
+
             imgpoints2, _ = cv2.projectPoints(
-                self.objpoints[i], rvecs, tvecs, self.mtxB, self.distB
+                self.objpoints[i], self.rvecB, self.tvecB, self.mtxB, self.distB
             )
 
             imgpoints2_reshaped = imgpoints2.reshape(-1, 2)
@@ -529,3 +517,45 @@ def test_pixel_error(self):
 
         total_err = mean_error / len(self.objpoints)
         print(f"(Reprojection error) Pixel L2 diff B: {total_err} pixels")
+
+    def register_debug_points(self, camA, camB):
+        # Define the custom object points directly without scaling
+        x = np.arange(-4, 5)  # from -4 to 4
+        y = np.arange(-4, 5)  # from -4 to 4
+        xv, yv = np.meshgrid(x, y, indexing='ij')
+        objpoint = np.column_stack([xv.flatten(), yv.flatten(), np.zeros(xv.size)])
+
+        # Convert the list of object points to a NumPy array
+        objpoints = np.array([objpoint], dtype=np.float32)
+
+        # Call the get_pixel_coordinates method using the object points
+        pixel_coordsA = self.get_pixel_coordinates(objpoints, self.rvecA, self.tvecA, self.mtxA, self.distA)
+        pixel_coordsB = self.get_pixel_coordinates(objpoints, self.rvecB, self.tvecB, self.mtxB, self.distB)
+
+        # Register the pixel coordinates for the debug points
+        self.model.add_coords_for_debug(camA, pixel_coordsA)
+        self.model.add_coords_for_debug(camB, pixel_coordsB)
+
+    def get_pixel_coordinates(self, objpoints, rvec, tvec, mtx, dist):
+        """
+        Projects 3D object points onto the 2D image plane and returns pixel coordinates.
+
+        Parameters:
+            objpoints (list): List of 3D object points.
+            rvec (np.ndarray): Rotation vector.
+            tvec (np.ndarray): Translation vector.
+            mtx (np.ndarray): Camera matrix.
+            dist (np.ndarray): Distortion coefficients.
+
+        Returns:
+            list: List of pixel coordinates corresponding to the object points.
+        """
+        pixel_coordinates = []
+        for points in objpoints:
+            # Project the 3D object points to 2D image points
+            imgpoints, _ = cv2.projectPoints(points, rvec, tvec, mtx, dist)
+            # Convert to integer tuples and append to the list
+            imgpoints_tuples = [tuple(map(lambda x: int(round(x)), point)) for point in imgpoints.reshape(-1, 2)]
+            pixel_coordinates.append(imgpoints_tuples)
+
+        return pixel_coordinates

parallax/coords_transformation.py

@@ -78,18 +78,17 @@ def avg_error(self, x, measured_pts, global_pts, reflect_z=False):
 
     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)
         x0 = np.array([0, 0, 0, 0, 0, 0, 1, 1, 1])  # initial guess: (x, y, z, x_t, y_t, z_t, s_x, s_y, s_z)
 
-        if len(measured_pts) < 3 or len(global_pts) < 3:
-            raise ValueError("At least two points are required for optimization.")
+        if len(measured_pts) <= 3 or len(global_pts) <= 3:
+            raise ValueError("At least three points are required for optimization.")
 
         # Optimize without reflection
-        res1 = leastsq(self.func, x0, args=(measured_pts, global_pts, False))
+        res1 = leastsq(self.func, x0, args=(measured_pts, global_pts, False), maxfev=5000)
         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))
+        res2 = leastsq(self.func, x0, args=(measured_pts, global_pts, True), maxfev=5000)
         avg_error2 = self.avg_error(res2[0], measured_pts, global_pts, True)
 
         # Select the transformation with the smaller total error
@@ -102,8 +101,6 @@ def fit_params(self, measured_pts, global_pts):
             R = self.combineAngles(rez[2], rez[1], rez[0], reflect_z=True)
             avg_err = avg_error1
 
-        #origin = rez[3:]
         origin = rez[3:6]
         scale = rez[6:]
-        #return origin, R  # translation vector and rotation matrix
         return origin, R, scale, avg_err  # translation vector, rotation matrix, and scaling factors