From fd235ccd28892d05de7a8301c138bf9c0d24b01f Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Wed, 24 Jul 2024 19:02:08 +0200
Subject: [PATCH 01/87] Reuse globbed image paths

---
 calibration_utils.py | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 8ff22a6..f7e083f 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -469,15 +469,11 @@ def getting_features(self, img_path, name, features = None):
             return allCorners, allIds, imsize
 
         elif features == None or features == "charucos":
-            img_path = glob.glob(img_path + "/*")
-            img_path.sort()
-            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
+            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(self.img_path)
             return allCorners, allIds, imsize
 
         if features == "checker_board":
-            img_path = glob.glob(img_path + "/*")
-            img_path.sort()
-            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
+            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(self.img_path)
             return allCorners, allIds, imsize
         ###### ADD HERE WHAT IT IS NEEDED ######
 

From 83be7d5658fc36ce6c872a2e0250b36ed4d33222 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 13:44:17 +0200
Subject: [PATCH 02/87] Remove trace logging

---
 calibration_utils.py | 345 -------------------------------------------
 1 file changed, 345 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index f7e083f..96ceaf8 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -159,7 +159,6 @@ def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, disa
         self.filtering_enable = filtering_enable
         self.ccm_model = distortion_model
         self.model = model
-        self.traceLevel = traceLevel
         self.output_scale_factor = outputScaleFactor
         self.disableCamera = disableCamera
         self.errors = {}
@@ -174,8 +173,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         """Function to calculate calibration for stereo camera."""
         start_time = time.time()
         # init object data
-        if self.traceLevel == 2 or self.traceLevel == 10:
-            print(f'squareX is {squaresX}')
         self.enable_rectification_disp = True
         if intrinsic_img != {}:
             for cam in intrinsic_img:
@@ -275,10 +272,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     if cam_info["name"] not in self.cameraIntrinsics.keys():
                         self.cameraIntrinsics[cam_info["name"]] = cameraMatrixInit
 
-                    if self.traceLevel == 3 or self.traceLevel == 10:
-                        print(
-                            f'Camera Matrix initialization with HFOV of {cam_info["name"]} is.............')
-                        print(cameraMatrixInit)
                     distCoeffsInit = np.zeros((12, 1))
                     if cam_info["name"] not in self.cameraDistortion:
                         self.cameraDistortion[cam_info["name"]] = distCoeffsInit
@@ -329,9 +322,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 cam_info['reprojection_error'] = ret
                 print("Reprojection error of {0}: {1}".format(
                     cam_info['name'], ret))
-                if self.traceLevel == 3 or self.traceLevel == 10:
-                    print("Estimated intrinsics of {0}: \n {1}".format(
-                    cam_info['name'], intrinsics))
                 
                 coverage_name = cam_info['name']
                 print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}'
@@ -731,168 +721,10 @@ def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, rep
                 rms_error = np.sqrt(total_error_squared / total_points if total_points else 0)
                 whole_error.append(rms_error)
 
-                if self.traceLevel in {2, 4, 10}:
-                    print(f"Overall RMS re-projection error for frame {i}: {rms_error}")
 
             total_error_squared = 0
             total_points = 0
 
-            if self.traceLevel == 8 or self.traceLevel == 10:
-                centroid_x = np.mean(np.array(display_corners).T[0])
-                centroid_y = np.mean(np.array(display_corners).T[1])
-
-                # Calculate distances from the center
-                distances = [distance((centroid_x, centroid_y), point) for point in np.array(corners2)]
-                max_distance = max(distances)
-                if max_distance > circle_size:
-                    circle_size = max_distance
-                circle = plt.Circle((centroid_x, centroid_y), circle_size, color='black', fill=True, label = "Calibrated area", alpha = 0.2)
-                fig, ax = plt.subplots()
-                ax.add_artist(circle)
-                ax.set_title(f"Reprojection error for frame {i}, camera {camera}")
-                ax.scatter(np.array(corners2).T[0], np.array(corners2).T[1], label = "Original", alpha = 0.5, color = "Black")
-                img = ax.scatter(np.array(imgpoints2).T[0], np.array(imgpoints2).T[1], c=errors, cmap = GnRd, label = "Reprojected", vmin=0, vmax=threshold)
-                ax.imshow(frame, alpha = 0.5)
-                ax.plot([],[], label = f"Rerprojection Remade: {round(rms_error, 4)}", color = "white")
-                ax.plot([],[], label = f"Whole Rerprojection OpenCV: {round(reprojection, 4)}", color = "white")
-                cbar = plt.colorbar(img, ax=ax)
-                cbar.set_label("Reprojection error")
-                ax.set_xlabel('Width')
-                ax.set_xlim([0,self.width[camera]])
-                ax.set_ylim([0,self.height[camera]])
-                ax.legend()
-                ax.set_ylabel('Y coordinate')
-                plt.grid()
-                plt.show()
-
-            if self.traceLevel == 9 or self.traceLevel == 10:
-                centroid_x = np.mean(np.array(removed_corners).T[0])
-                centroid_y = np.mean(np.array(removed_corners).T[1])
-
-                # Calculate distances from the center
-                distances = [distance((centroid_x, centroid_y), point) for point in np.array(corners2[removed_mask])]
-                max_distance = max(distances)
-                if max_distance > circle_size:
-                    circle_size = max_distance
-                circle = plt.Circle((centroid_x, centroid_y), circle_size, color='black', fill=True, label = "Calibrated area", alpha = 0.2)
-                fig, ax = plt.subplots()
-                ax.add_artist(circle)
-                ax.set_title(f"Removed error for frame {i}, camera {camera}")
-                ax.scatter(np.array(corners2[removed_mask]).T[0], np.array(corners2[removed_mask]).T[1], label = "Original", alpha = 0.5, color = "Black")
-                img = ax.scatter(np.array(imgpoints2[removed_mask]).T[0], np.array(imgpoints2[removed_mask]).T[1], c=errors[removed_mask], cmap = GnRd, label = "Reprojected", vmin=0, vmax=max(errors[removed_mask]))
-                ax.imshow(frame, alpha = 0.5)
-                ax.plot([],[], label = f"Rerprojection Remade: {round(np.mean(errors[removed_mask]), 4)}", color = "white")
-                ax.plot([],[], label = f"Whole Rerprojection OpenCV: {round(reprojection, 4)}", color = "white")
-                cbar = plt.colorbar(img, ax=ax)
-                cbar.set_label("Reprojection error")
-                ax.set_xlabel('Width')
-                ax.set_xlim([0,self.width[camera]])
-                ax.set_ylim([0,self.height[camera]])
-                ax.legend()
-                ax.set_ylabel('Y coordinate')
-                plt.grid()
-                plt.show()
-
-        if self.traceLevel == 3 or self.traceLevel == 5 or self.traceLevel == 10:
-            center_x, center_y = self.width[camera] / 2, self.height[camera] / 2
-            distances = [distance((center_x, center_y), point) for point in np.array(display_corners)]
-            max_distance = max(distances)
-            circle = plt.Circle((center_x, center_y), max_distance, color='black', fill=True, label = "Calibrated area", alpha = 0.2)
-            fig, ax = plt.subplots()
-            ax.add_artist(circle)
-            ax.set_title(f"Reprojection map camera {camera}")
-            ax.scatter(np.array(display_points).T[0], np.array(display_points).T[1], label = "Original", alpha = 0.5, color = "Black")
-            img = ax.scatter(np.array(display_corners).T[0], np.array(display_corners).T[1], c=all_error, cmap = GnRd, label = "Reprojected", vmin=0, vmax=threshold)
-            ax.imshow(frame, alpha = 0.5)
-            ax.plot([],[], label = f"Rerprojection Remade ALL: {round(np.sqrt(np.mean(np.array(whole_error)**2)), 4)}", color = "white")
-            ax.plot([],[], label = f"Whole Rerprojection OpenCV: {round(reprojection, 4)}", color = "white")
-            cbar = plt.colorbar(img, ax=ax)
-            cbar.set_label("Reprojection error")
-            ax.set_xlabel('Width')
-            ax.set_xlim([0,self.width[camera]])
-            ax.set_ylim([0,self.height[camera]])
-            ax.legend()
-            ax.set_ylabel('Height')
-            plt.grid()
-            plt.show()
-
-        if self.traceLevel == 3 or self.traceLevel == 5 or self.traceLevel == 10:
-            x = np.array(display_points).T[0]
-            y = np.array(display_points).T[1]
-            z = np.array(all_error)
-
-            x_flat = x.flatten()
-            y_flat = y.flatten()
-            z_flat = z.flatten()
-
-            fig, ax = plt.subplots()
-
-            self.errors[camera] = z_flat
-            _, bins, _ = ax.hist(all_error, range= [0,30], bins = 100, label = f"{camera}", color= "Blue", edgecolor= "black")
-            ax.hist(removed_error, bins = bins, color= "Blue", edgecolor= "black")
-            ax.plot([],[], label = f"MAX : {max(reported_error)}")
-            ax.plot([],[], label = f"MIN : {min(reported_error)}")
-            ax.legend()
-            ax.set_title("Reprojection error for shorter dataset")
-            ax.set_xlabel("Reprojection error")
-            plt.show()
-            grid_x, grid_y = np.mgrid[min(x_flat):max(x_flat):100j, min(y_flat):max(y_flat):100j]
-            grid_z = abs(griddata((x_flat, y_flat), z_flat, (grid_x, grid_y), method='cubic'))
-
-            plt.title(f"Reprojection error of {camera}")
-            plt.contourf(grid_x, grid_y, grid_z, 50, cmap=GnRd)
-            plt.colorbar(label='Reprojection error')
-            contours = plt.contour(grid_x, grid_y, grid_z, 7, colors ="black", alpha = 0.7, linewidths = 0.5, linestyles = "dashed")
-            plt.clabel(contours, inline=True, fontsize=8)
-            plt.xlim([0,self.width[camera]])
-            plt.ylim([0,self.height[camera]])
-            plt.grid()
-            plt.show()
-
-        if self.traceLevel == 11:
-            sorted_points, quadrant_coords = sort_points_into_quadrants(display_points, self.width[camera], self.height[camera], error=all_error)
-            quadrant_width = self.width // nx
-            quadrant_height = self.height // ny
-
-            image = np.full((self.height, self.width, 3), 255, dtype=np.uint8)
-
-            # Define the quadrant lines
-            quadrant_width = self.width // nx
-            quadrant_height = self.height // ny
-            index = 0
-            import math
-            for i in range(nx):
-                for j in range(ny):
-                    # Scale the color from red (0, 0, 255) to green (0, 255, 0)
-                    count = np.mean(sorted_points[index])
-                    if math.isnan(count):
-                        count = "Missing"
-                        red = 255
-                        green = 0
-                    if len(sorted_points[index]) < ((self.squaresX*self.squaresY))*0.05:
-                        count = f"Only {len(sorted_points[index])} points"
-                        red = 255
-                        green = 0 
-                    else:
-                        count = round(count, 4)
-                        red = int(((count / threshold)) * 255)
-                        green = int((1 -count / threshold) * 255)
-                    color = (0, green, red)
-
-                    left = j * quadrant_width
-                    upper = i * quadrant_height
-                    right = left + quadrant_width
-                    bottom = upper + quadrant_height
-                    index += 1
-
-                    cv2.rectangle(image, (left, upper), (right, bottom), color, -1)
-
-                    text_position = (left + 10, upper + quadrant_height // 2)
-                    cv2.putText(image, str(count), text_position, cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA)
-
-            cv2.imshow('Quadrants with Points', image)
-            cv2.waitKey(1)
-            cv2.destroyAllWindows()
         if camera not in self.all_features.keys():
             self.all_features[camera] = display_corners
         self.all_features[camera] = display_corners
@@ -934,14 +766,6 @@ def camera_pose_charuco(self, objpoints: np.array, corners: np.array, ids: np.ar
             
             ini_threshold += threshold_stepper
             index += 1
-        if self.traceLevel == 13:
-            image = cv2.imread(self.img_path[self.index])
-            plt.title(f"Number of rejected corners in filtering, iterations needed: {ini_threshold}, inliers: {round(len(objects)/len(corners[:,0,0]), 4) *100} %")
-            plt.imshow(image)
-            plt.scatter(corners[:,0,0],corners[:,0,1], marker= "o", label = f"Detected all corners: {len(corners[:,0,0])}", color = "Red")
-            plt.scatter(imgpoints2[:,0,0],imgpoints2[:,0,1], label = f"Filtering method: {len(objects)}", marker = "x", color = "Green")
-            plt.legend()
-            plt.show()
         if ret:
             return rvec, tvec, objects
         else:
@@ -980,8 +804,6 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
         count = 0
         skip_vis = False
         for im in images:
-            if self.traceLevel == 3 or self.traceLevel == 10:
-                print("=> Processing image {0}".format(im))
             img_pth = Path(im)
             frame = cv2.imread(im)
             gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
@@ -1013,9 +835,6 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
             
             ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = self.detect_charuco_board(gray)
 
-            if self.traceLevel == 2 or self.traceLevel == 4 or self.traceLevel == 10:
-                print('{0} number of Markers corners detected in the image {1}'.format(
-                    len(charuco_corners), img_pth.name))
 
             if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
 
@@ -1031,20 +850,6 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
                 print(im)
                 return f'Failed to detect more than 3 markers on image {im}', None, None, None, None, None
 
-            if self.traceLevel == 2 or self.traceLevel == 4 or self.traceLevel == 10:
-                rgb_img = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
-                cv2.aruco.drawDetectedMarkers(rgb_img, marker_corners, marker_ids, (0, 0, 255))
-                cv2.aruco.drawDetectedCornersCharuco(rgb_img, charuco_corners, charuco_ids, (0, 255, 0))
-
-                if rgb_img.shape[1] > 1920:
-                    rgb_img = cv2.resize(rgb_img, (0, 0), fx=0.7, fy=0.7)
-                if not skip_vis:
-                    name = img_pth.name + ' - ' + "marker frame"
-                    cv2.imshow(name, rgb_img)
-                    k = cv2.waitKey(1)
-                    if k == 27: # Esc key to skip vis
-                        skip_vis = True
-                cv2.destroyAllWindows()
         # imsize = gray.shape[::-1]
         return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
 
@@ -1100,11 +905,6 @@ def scale_intrinsics(self, intrinsics, originalShape, destShape):
                                     * scale - destShape[0]) / 2
         scaled_intrinsics[0][2] -= (originalShape[1]     # c_x width of the image
                                     * scale - destShape[1]) / 2
-        if self.traceLevel == 3 or self.traceLevel == 10:
-            print('original_intrinsics')
-            print(intrinsics)
-            print('scaled_intrinsics')
-            print(scaled_intrinsics)
 
         return scaled_intrinsics
 
@@ -1129,20 +929,6 @@ def undistort_visualization(self, img_list, K, D, img_size, name):
             if index == 0:
                 undistorted_file_path = self.data_path + '/' + name + f'_undistorted.png'
                 cv2.imwrite(undistorted_file_path, undistorted_img)
-            if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
-                cv2.putText(undistorted_img, "Press S to save image", (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 2*undistorted_img.shape[0]/1750, (0, 0, 255), 2)
-                cv2.imshow("undistorted", undistorted_img)
-                k = cv2.waitKey(1)
-                if k == ord("s") or k == ord("S"):
-                    undistorted_img = cv2.remap(
-                        img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
-                    undistorted_file_path = self.data_path + '/' + name + f'_{index}_undistorted.png'
-                    cv2.imwrite(undistorted_file_path, undistorted_img)
-                if k == 27:  # Esc key to stop
-                    break
-                print(f'image path - {im}')
-                print(f'Image Undistorted Size {undistorted_img.shape}')
-                cv2.destroyWindow("undistorted")
 
 
     def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
@@ -1184,10 +970,6 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
             cameraMatrixInit = self.cameraIntrinsics[name]
             threshold = 2 * imsize[1]/800.0
        
-        if self.traceLevel == 3 or self.traceLevel == 10:
-            print(
-                f'Camera Matrix initialization with HFOV of {hfov} is.............')
-            print(cameraMatrixInit)
         if name not in self.cameraDistortion:
             distCoeffsInit = np.zeros((5, 1))
         else:
@@ -1221,8 +1003,6 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
         intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
         distortion_array = {}
         index = 0
-        if self.traceLevel != 0:
-            fig, ax = plt.subplots()
         camera_matrix = cameraMatrixInit
         distortion_coefficients = distCoeffsInit
         rotation_vectors = rvecs
@@ -1257,10 +1037,6 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
                         distortion_array[i] = []
                     distortion_array[i].append(distortion_coefficients[i][0])
                 print(f"Each filtering {time.time() - start}")
-                if self.traceLevel == 12:
-                    _, bins, _ = ax.hist(all_error, range = [0,30], bins = 100, label = f"Iteration {index}, number filtered: {len(removed_corners)}", color= plt.cm.summer(1-(index-1)/10), alpha = 0.3, edgecolor ="black")  
-                    ax.hist(removed_error, bins = bins, color= plt.cm.summer(1-(index-1)/7), alpha = 0.8, edgecolor = "black")
-                    ax.vlines(threshold, ymin = -0.5, ymax = len(all_error), color = "red")
                 start = time.time()
                 try:
                     (ret, camera_matrix, distortion_coefficients,
@@ -1284,76 +1060,10 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
                 index += 1
                 if  index > 5 or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
                     print(f"Whole procedure: {time.time() - whole}")
-                    if self.traceLevel == 12:
-                        fig.suptitle(f"Histograms of reprojection error for threshold {threshold}")
-                        ax.legend()
-                        ax.grid()
-                        plt.show()
-                        plt.title(f"Reprojection error after {int(index) + 1} iterations, threshold {threshold}")
-                        plt.plot(iterations_array, reprojection)
-                        plt.xlabel("Iteration")
-                        plt.ylabel("Reprojection error")
-                        plt.grid()
-                        plt.show()
-                        plt.title(f"Corners rejected after {int(index) + 1} iterations, threshold {threshold}")
-                        plt.plot(iterations_array, num_corners)
-                        plt.xlabel("Iteration")
-                        plt.ylabel("Rejected corners")
-                        plt.grid()
-                        plt.show()
-                        plt.title(f"Distortion after {int(index) + 1} iterations, threshold {threshold}")
-                        for key, distortion in distortion_array.items():
-                            if distortion[0] != 0:
-                                plt.plot(iterations_array,distortion, label = f"Distortion: {key}")
-                        plt.ylabel("Absolute change")
-                        plt.grid()
-                        plt.legend()
-                        plt.show()
-                        plt.subplot(2,2,1)
-                        plt.title(f"Intrinsic and extrinsics after {int(index) + 1} iterations, threshold {threshold}")
-                        plt.plot(iterations_array, intrinsic_array['f_x'],  label = "f_x")
-                        plt.plot([],[], color = "white", label = f"Start: {round(intrinsic_array['f_x'][0], 2)}")
-                        plt.plot([],[], color = "white", label = f"Finish: {round(intrinsic_array['f_x'][len(intrinsic_array['f_x'])-1], 2)}")
-                        plt.plot(iterations_array, intrinsic_array['f_y'], label = "f_y")
-                        plt.plot([],[], color = "white", label = f"Start: {round(intrinsic_array['f_y'][0], 2)}")
-                        plt.plot([],[], color = "white", label = f"Finish: {round(intrinsic_array['f_y'][len(intrinsic_array['f_y'])-1], 2)}")
-                        plt.xlabel("Iterations")
-                        plt.ylabel("Absolute change")
-                        plt.grid()
-                        plt.legend()
-
-                        plt.subplot(2,2,2)
-                        plt.plot(iterations_array, intrinsic_array['c_x'], label = "c_x")
-                        plt.plot([],[], color = "white", label = f"Start: {round(intrinsic_array['c_x'][0], 2)}")
-                        plt.plot([],[], color = "white", label = f"Finish: {round(intrinsic_array['c_x'][len(intrinsic_array['c_x'])-1], 2)}")
-                        plt.plot(iterations_array, intrinsic_array['c_y'], label = "c_y")
-                        plt.plot([],[], color = "white", label = f"Start: {round(intrinsic_array['c_y'][0], 2)}")
-                        plt.plot([],[], color = "white", label = f"Finish: {round(intrinsic_array['c_y'][len(intrinsic_array['c_y'])-1], 2)}")
-                        plt.xlabel("Iterations")
-                        plt.ylabel("Absolute change")
-                        plt.grid()
-                        plt.legend()
-
-                        plt.subplot(2,1,2)
-                        plt.plot(iterations_array, translation_array_x - translation_array_x[0], label = "Translation x")
-                        plt.plot(iterations_array, translation_array_y - translation_array_y[0], label = "Translation y")
-                        plt.plot(iterations_array, translation_array_z - translation_array_z[0], label = "Translation z")
-                        plt.grid()
-                        plt.xlabel("Iterations")
-                        plt.ylabel("Absolute change")
-                        plt.legend()
-                        plt.show()
-                        traceLevel_copy = self.traceLevel
-                        self.traceLevel = 8
-                        filtered_corners, filtered_ids , all_error,removed_corners, removed_ids, removed_error= self.features_filtering_function(rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = 1.0)
-                        self.traceLevel = traceLevel_copy
                     break
                 previous_ids = removed_ids
         except:
             return f"Failed to calibrate camera {name}", None, None, None, None, None, None, None ,None , None
-        if self.traceLevel == 3 or self.traceLevel == 10:
-            print('Per View Errors...')
-            print(perViewErrors)
         return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
     def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
@@ -1445,12 +1155,6 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
         obj_pts = []
         one_pts = self.board.chessboardCorners
 
-        if self.traceLevel == 2 or self.traceLevel == 4 or self.traceLevel == 10:
-            print('Length of allIds_l')
-            print(len(allIds_l))
-            print('Length of allIds_r')
-            print(len(allIds_r))
-
         for i in range(len(allIds_l)):
             left_sub_corners = []
             right_sub_corners = []
@@ -1517,11 +1221,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                 # self.P_l = P_l
                 # self.P_r = P_r
                 r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-                if self.traceLevel == 5 or self.traceLevel == 10:
-                    print(f'R_L Euler angles in XYZ {r_euler}')
                 r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-                if self.traceLevel == 5 or self.traceLevel == 10:
-                    print(f'R_R Euler angles in XYZ {r_euler}')
 
                 # print(f'P_l is \n {P_l}')
                 # print(f'P_r is \n {P_r}') 
@@ -1536,10 +1236,6 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                 distortion_flags = self.get_distortion_flags(left_name)
                 flags += distortion_flags
                 # print(flags)
-                if self.traceLevel == 3 or self.traceLevel == 10:
-                    print('Printing Extrinsics guesses...')
-                    print(r_in)
-                    print(t_in)
                 ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
                 obj_pts, left_corners_sampled, right_corners_sampled,
                 cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
@@ -1562,11 +1258,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                 # self.P_l = P_l
                 # self.P_r = P_r
                 r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-                if self.traceLevel == 5 or self.traceLevel == 10:
-                    print(f'R_L Euler angles in XYZ {r_euler}')
                 r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-                if self.traceLevel == 5 or self.traceLevel == 10:
-                    print(f'R_R Euler angles in XYZ {r_euler}')
 
                 return [ret, R, T, R_l, R_r, P_l, P_r]
             elif self.cameraModel == 'fisheye':
@@ -1591,19 +1283,12 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                 # TODO(sACHIN): Try without intrinsic guess
                 # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
                 # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW
-                if self.traceLevel == 3 or self.traceLevel == 10:
-                    print('Fisyeye stereo model..................') 
                 (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate(
                     obj_pts_truncated, left_corners_truncated, right_corners_truncated,
                     cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
                     flags=flags, criteria=stereocalib_criteria), None, None
                 r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
                 print(f'Reprojection error is {ret}')
-                if self.traceLevel == 3 or self.traceLevel == 10:
-                    print('Printing Extrinsics res...')
-                    print(R)
-                    print(T)
-                    print(f'Euler angles in XYZ {r_euler} degs')
                 isHorizontal = np.absolute(T[0]) > np.absolute(T[1])
 
                 R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify(
@@ -1614,11 +1299,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                     None, R, T, flags=0)
 
                 r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-                if self.traceLevel == 3 or self.traceLevel == 10:
-                    print(f'R_L Euler angles in XYZ {r_euler}')
                 r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-                if self.traceLevel == 3 or self.traceLevel == 10:
-                    print(f'R_R Euler angles in XYZ {r_euler}')            
 
                 return [ret, R, T, R_l, R_r, P_l, P_r]
 
@@ -1657,8 +1338,6 @@ def display_rectification(self, image_data_pairs, images_corners_l, images_corne
 
     def scale_image(self, img, scaled_res):
         expected_height = img.shape[0]*(scaled_res[1]/img.shape[1])
-        if self.traceLevel == 2 or self.traceLevel == 10:
-            print("Expected Height: {}".format(expected_height))
 
         if not (img.shape[0] == scaled_res[0] and img.shape[1] == scaled_res[1]):
             if int(expected_height) == scaled_res[0]:
@@ -1820,11 +1499,6 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
             diff = scaled_height - scaled_res[0]
             if diff < 0:
                 scaled_res = (int(scaled_height), scaled_res[1])
-        if self.traceLevel == 3 or self.traceLevel == 10:
-            print(
-                f'Is scale Req: {scale_req}\n scale value: {scale} \n scalable Res: {scalable_res} \n scale Res: {scaled_res}')
-            print("Original res Left :{}".format(frame_left_shape))
-            print("Original res Right :{}".format(frame_right_shape))
         # print("Scale res :{}".format(scaled_res))
 
         M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res)
@@ -1838,8 +1512,6 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
         print('Original intrinsics ....')
         print(f"L {M_lp}")
         print(f"R: {M_rp}")
-        if self.traceLevel == 3 or self.traceLevel == 10:
-            print(f'Width and height is {scaled_res[::-1]}')
         # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
         # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0)
         # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
@@ -1899,17 +1571,6 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
 
             image_data_pairs.append((img_l, img_r))
             
-            if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
-                #cv2.imshow("undistorted-Left", img_l)
-                #cv2.imshow("undistorted-right", img_r)
-                # print(f'image path - {im}')
-                # print(f'Image Undistorted Size {undistorted_img.shape}')
-                k = cv2.waitKey(0)
-                if k == 27:  # Esc key to stop
-                    break
-        if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
-          cv2.destroyWindow("undistorted-Left")
-          cv2.destroyWindow("undistorted-right")  
         # compute metrics
         imgpoints_r = []
         imgpoints_l = []
@@ -1918,9 +1579,6 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
         for i, image_data_pair in enumerate(image_data_pairs):
             res2_l = self.detect_charuco_board(image_data_pair[0])
             res2_r = self.detect_charuco_board(image_data_pair[1])
-            
-            # if self.traceLevel == 2 or self.traceLevel == 4 or self.traceLevel == 10:
-            
 
             img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]])
             img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB)
@@ -1977,9 +1635,6 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
                     epi_error_sum += curr_epipolar_error
                 localError = epi_error_sum / len(corners_l)
                 image_epipolar_color.append(corner_epipolar_color)
-                if self.traceLevel == 2 or self.traceLevel == 3 or self.traceLevel == 4 or self.traceLevel == 10:
-                    print("Epipolar Error per image on host in " + img_pth_right.name + " : " +
-                        str(localError))
             else:
                 print('Numer of corners is in left -> and right ->')
                 return -1

From e2ba90f18ae151ae5672953b7213a79d21f2036a Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 13:50:14 +0200
Subject: [PATCH 03/87] Remove self.all_features, self.all_erros, self.charucos

---
 calibration_utils.py | 31 +++++++++++--------------------
 1 file changed, 11 insertions(+), 20 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 96ceaf8..af099e2 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -189,11 +189,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         self.calib_model = {}
         self.collected_features = {}
         self.collected_ids = {}
-        self.all_features = {}
-        self.all_errors = {}
         self.errors = {}
         self.data_path = filepath
-        self.charucos = charucos 
         self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000)
         self.squaresX = squaresX
         self.squaresY = squaresY
@@ -256,7 +253,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 cam_info["img_path"] = self.img_path
                 self.name = cam_info["name"]
                 if per_ccm:
-                    all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], features=features)
+                    all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], features=features, charucos=charucos)
                     if isinstance(all_features, str) and all_ids is None:
                         if cam_info["name"] not in self.errors.keys():
                             self.errors[cam_info["name"]] = []
@@ -303,7 +300,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     cam_info['filtered_ids'] = filtered_ids
                     cam_info['filtered_corners'] = filtered_features
 
-                    ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images)
+                    ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos)
                     if isinstance(ret, str) and all_ids is None:
                         if cam_info["name"] not in self.errors.keys():
                             self.errors[cam_info["name"]] = []
@@ -311,7 +308,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                         continue
                 else:
                     ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
-                        images_path, cam_info['hfov'], cam_info["name"])
+                        images_path, cam_info['hfov'], cam_info["name"], charucos)
                     cam_info['filtered_ids'] = filtered_ids
                     cam_info['filtered_corners'] = filtered_corners
                 self.cameraIntrinsics[cam_info["name"]] = intrinsics
@@ -447,11 +444,11 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     
         return 1, board_config
 
-    def getting_features(self, img_path, name, features = None):
-        if self.charucos != {}:
+    def getting_features(self, img_path, name, features = None, charucos=None):
+        if charucos != {}:
             allCorners = []
             allIds = []
-            for index, charuco_img in enumerate(self.charucos[name]):
+            for index, charuco_img in enumerate(charucos[name]):
                 ids, charucos = charuco_img
                 allCorners.append(charucos)
                 allIds.append(ids)
@@ -621,7 +618,7 @@ def is_binary_string(s: str) -> bool:
             flags = self.distortion_model[name]
         return flags
 
-    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files):
+    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         coverageImage = np.ones(imsize[::-1], np.uint8) * 255
@@ -632,7 +629,7 @@ def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorne
                 distortion_flags = self.get_distortion_flags(name)
                 ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
                     all_Features, all_features_Ids,allCorners, allIds, imsize, hfov, name, distortion_flags)
-            if self.charucos == {}:
+            if charucos == {}:
                 self.undistort_visualization(
                     image_files, camera_matrix, distortion_coefficients, imsize, name)
 
@@ -725,12 +722,6 @@ def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, rep
             total_error_squared = 0
             total_points = 0
 
-        if camera not in self.all_features.keys():
-            self.all_features[camera] = display_corners
-        self.all_features[camera] = display_corners
-        if camera not in self.all_errors.keys():
-            self.all_errors[camera] = all_error
-        self.all_errors[camera] = reported_error
         return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
 
     def detect_charuco_board(self, image: np.array):
@@ -853,17 +844,17 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
         # imsize = gray.shape[::-1]
         return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
 
-    def calibrate_intrinsics(self, image_files, hfov, name):
+    def calibrate_intrinsics(self, image_files, hfov, name, charucos):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         assert len(
             image_files) != 0, "ERROR: Images not read correctly, check directory"
-        if self.charucos == {}:
+        if charucos == {}:
             allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files)
         else:
             allCorners = []
             allIds = []
-            for index, charuco_img in enumerate(self.charucos[name]):
+            for index, charuco_img in enumerate(charucos[name]):
                 ids, charucos = charuco_img
                 allCorners.append(charucos)
                 allIds.append(ids)

From 5be08c7004b469f8ef30ab17e763b1839adc604a Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 13:53:48 +0200
Subject: [PATCH 04/87] Remove self.collected_ids, self.collected_features,
 mark constant members

---
 calibration_utils.py | 56 +++++++++++++++++++-------------------------
 1 file changed, 24 insertions(+), 32 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index af099e2..54c765a 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -173,7 +173,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         """Function to calculate calibration for stereo camera."""
         start_time = time.time()
         # init object data
-        self.enable_rectification_disp = True
         if intrinsic_img != {}:
             for cam in intrinsic_img:
                 intrinsic_img[cam].sort(reverse=True)
@@ -182,24 +181,21 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 extrinsic_img[cam].sort(reverse=True)
         self.intrinsic_img = intrinsic_img
         self.extrinsic_img = extrinsic_img
-        self.cameraModel = camera_model
         self.cameraIntrinsics = {}
         self.cameraDistortion = {}
         self.distortion_model = {}
         self.calib_model = {}
-        self.collected_features = {}
-        self.collected_ids = {}
         self.errors = {}
-        self.data_path = filepath
-        self.aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000)
-        self.squaresX = squaresX
-        self.squaresY = squaresY
-        self.board = aruco.CharucoBoard_create(
+        self._enable_rectification_disp = True
+        self._cameraModel = camera_model
+        self._data_path = filepath
+        self._aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000)
+        self._board = aruco.CharucoBoard_create(
             # 22, 16,
             squaresX, squaresY,
             square_size,
             mrk_size,
-            self.aruco_dictionary)
+            self._aruco_dictionary)
 
         self.cams = []
         # parameters = aruco.DetectorParameters_create()
@@ -237,7 +233,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     self.calib_model[cam_info["name"]] = self.cameraModel_ccm
                     self.distortion_model[cam_info["name"]] = self.model_ccm
                 else:
-                    self.calib_model[cam_info["name"]] = self.cameraModel
+                    self.calib_model[cam_info["name"]] = self._cameraModel
                     if cam_info["name"] in self.ccm_model:
                         self.distortion_model[cam_info["name"]] = self.ccm_model[cam_info["name"]]
                     else:
@@ -278,7 +274,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     else:
                         filtered_images = images_path
                     current_time = time.time()
-                    if self.cameraModel != "fisheye":
+                    if self._cameraModel != "fisheye":
                         print("Filtering corners")
                         removed_features, filtered_features, filtered_ids = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraMatrixInit, distCoeffsInit)
 
@@ -289,10 +285,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                             continue
 
                         print(f"Filtering takes: {time.time()-current_time}")
-                        if  cam_info["name"] not in self.collected_features.keys():
-                            self.collected_features[cam_info["name"]] = filtered_features
-                        if  cam_info["name"] not in self.collected_ids.keys():
-                            self.collected_ids[cam_info["name"]] = filtered_ids
                     else:
                         filtered_features = all_features
                         filtered_ids = all_ids
@@ -516,7 +508,7 @@ def filtering_features(self,allCorners, allIds, name,imsize, hfov, cameraMatrixI
                      perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
                         charucoCorners=filtered_corners,
                         charucoIds=filtered_ids,
-                        board=self.board,
+                        board=self._board,
                         imageSize=imsize,
                         cameraMatrix=cameraMatrixInit,
                         distCoeffs=distCoeffsInit,
@@ -685,7 +677,7 @@ def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, rep
             frame = cv2.imread(frame_path)
             if ids is not None and corners.size > 0:
                 ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-                objPoints = np.array([self.board.chessboardCorners[id] for id in ids], dtype=np.float32)
+                objPoints = np.array([self._board.chessboardCorners[id] for id in ids], dtype=np.float32)
                 imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
                 corners2 = corners.reshape(-1, 2)
                 imgpoints2 = imgpoints2.reshape(-1, 2)
@@ -727,11 +719,11 @@ def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, rep
     def detect_charuco_board(self, image: np.array):
         arucoParams = cv2.aruco.DetectorParameters_create()
         arucoParams.minMarkerDistanceRate = 0.01
-        corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, self.aruco_dictionary, parameters=arucoParams)  # First, detect markers
-        marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, self.board, corners, ids, rejectedCorners=rejectedImgPoints)
+        corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, self._aruco_dictionary, parameters=arucoParams)  # First, detect markers
+        marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, self._board, corners, ids, rejectedCorners=rejectedImgPoints)
         # If found, add object points, image points (after refining them)
         if len(marker_corners) > 0:
-            ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, self.board, minMarkers = 1)
+            ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, self._board, minMarkers = 1)
             return ret, corners, ids, marker_corners, marker_ids
         else:
             return None, None, None, None, None
@@ -771,7 +763,7 @@ def compute_reprojection_errors(self, obj_pts: np.array, img_pts: np.array, K: n
         return errs 
     
     def charuco_ids_to_objpoints(self, ids):
-        one_pts = self.board.chessboardCorners
+        one_pts = self._board.chessboardCorners
         objpts = []
         for j in range(len(ids)):   
             objpts.append(one_pts[ids[j]])
@@ -904,7 +896,7 @@ def undistort_visualization(self, img_list, K, D, img_size, name):
             # print(im)
             img = cv2.imread(im)
             # h, w = img.shape[:2]
-            if self.cameraModel == 'perspective':
+            if self._cameraModel == 'perspective':
                 kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0)
                 # print(f'K scaled is \n {kScaled} and size is \n {img_size}')
                 # print(f'D Value is \n {D}')
@@ -928,7 +920,7 @@ def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_c
             corners = allCorners[i]
             ids = allIds[i]
             objpts = self.charuco_ids_to_objpoints(ids)
-            if self.cameraModel == "fisheye":
+            if self._cameraModel == "fisheye":
                 errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
             else:
                 errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i])
@@ -1036,7 +1028,7 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
                      perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
                         charucoCorners=filtered_corners,
                         charucoIds=filtered_ids,
-                        board=self.board,
+                        board=self._board,
                         imageSize=imsize,
                         cameraMatrix=cameraMatrixInit,
                         distCoeffs=distCoeffsInit,
@@ -1058,7 +1050,7 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
         return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
     def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
-        one_pts = self.board.chessboardCorners
+        one_pts = self._board.chessboardCorners
         obj_points = []
         for i in range(len(allIds)):
             obj_points.append(self.charuco_ids_to_objpoints(allIds[i]))
@@ -1144,7 +1136,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
         right_corners_sampled = []
         left_ids_sampled = []
         obj_pts = []
-        one_pts = self.board.chessboardCorners
+        one_pts = self._board.chessboardCorners
 
         for i in range(len(allIds_l)):
             left_sub_corners = []
@@ -1219,7 +1211,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                 return [ret, R, T, R_l, R_r, P_l, P_r]
             #### ADD OTHER CALIBRATION METHODS ###
         else:
-            if self.cameraModel == 'perspective':
+            if self._cameraModel == 'perspective':
                 flags = 0
                 # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
                 # print(flags)
@@ -1252,7 +1244,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                 r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
 
                 return [ret, R, T, R_l, R_r, P_l, P_r]
-            elif self.cameraModel == 'fisheye':
+            elif self._cameraModel == 'fisheye':
                 # make sure all images have the same *number of* points
                 min_num_points = min([len(pts) for pts in obj_pts])
                 obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts]
@@ -1357,7 +1349,7 @@ def scale_image(self, img, scaled_res):
             return img
     
     def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal):
-        if self.cameraModel == 'perspective':
+        if self._cameraModel == 'perspective':
             mapx_l, mapy_l = cv2.initUndistortRectifyMap(
                 M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
             mapx_r, mapy_r = cv2.initUndistortRectifyMap(
@@ -1647,7 +1639,7 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
         avg_epipolar = epi_error_sum / total_corners
         print("Average Epipolar Error is : " + str(avg_epipolar))
 
-        if self.enable_rectification_disp:
+        if self._enable_rectification_disp:
             self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal)
 
         return avg_epipolar
@@ -1658,7 +1650,7 @@ def create_save_mesh(self):  # , output_path):
         print("Mesh path")
         print(curr_path)
 
-        if self.cameraModel == "perspective":
+        if self._cameraModel == "perspective":
             map_x_l, map_y_l = cv2.initUndistortRectifyMap(
                 self.M1, self.d1, self.R1, self.M2, self.img_shape, cv2.CV_32FC1)
             map_x_r, map_y_r = cv2.initUndistortRectifyMap(

From c5d18697155700df2707e7f00d30963f124ce3df Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 13:56:13 +0200
Subject: [PATCH 05/87] Invert disabled camera condition

---
 calibration_utils.py | 213 ++++++++++++++++++++++---------------------
 1 file changed, 107 insertions(+), 106 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 54c765a..6e3a717 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -222,121 +222,122 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         for camera in board_config['cameras'].keys():
             cam_info = board_config['cameras'][camera]
             self.id = cam_info["name"]
-            if cam_info["name"] not in self.disableCamera:
-                print(
-                    '<------------Calibrating {} ------------>'.format(cam_info['name']))
-                images_path = filepath + '/' + cam_info['name']
-                if "calib_model" in cam_info.keys():
-                    self.cameraModel_ccm, self.model_ccm = cam_info["calib_model"].split("_")
-                    if self.cameraModel_ccm == "fisheye":
-                        self.model_ccm == None
-                    self.calib_model[cam_info["name"]] = self.cameraModel_ccm
-                    self.distortion_model[cam_info["name"]] = self.model_ccm
+            if cam_info["name"] in self.disableCamera:
+                continue
+            print(
+                '<------------Calibrating {} ------------>'.format(cam_info['name']))
+            images_path = filepath + '/' + cam_info['name']
+            if "calib_model" in cam_info.keys():
+                self.cameraModel_ccm, self.model_ccm = cam_info["calib_model"].split("_")
+                if self.cameraModel_ccm == "fisheye":
+                    self.model_ccm == None
+                self.calib_model[cam_info["name"]] = self.cameraModel_ccm
+                self.distortion_model[cam_info["name"]] = self.model_ccm
+            else:
+                self.calib_model[cam_info["name"]] = self._cameraModel
+                if cam_info["name"] in self.ccm_model:
+                    self.distortion_model[cam_info["name"]] = self.ccm_model[cam_info["name"]]
                 else:
-                    self.calib_model[cam_info["name"]] = self._cameraModel
-                    if cam_info["name"] in self.ccm_model:
-                        self.distortion_model[cam_info["name"]] = self.ccm_model[cam_info["name"]]
-                    else:
-                        self.distortion_model[cam_info["name"]] = self.model
+                    self.distortion_model[cam_info["name"]] = self.model
 
 
-                features = None
-                self.img_path = glob.glob(images_path + "/*")
-                if charucos == {}:
-                    self.img_path = sorted(self.img_path, key=lambda x: int(x.split('_')[1]))
+            features = None
+            self.img_path = glob.glob(images_path + "/*")
+            if charucos == {}:
+                self.img_path = sorted(self.img_path, key=lambda x: int(x.split('_')[1]))
+            else:
+                self.img_path.sort()
+            cam_info["img_path"] = self.img_path
+            self.name = cam_info["name"]
+            if per_ccm:
+                all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], features=features, charucos=charucos)
+                if isinstance(all_features, str) and all_ids is None:
+                    if cam_info["name"] not in self.errors.keys():
+                        self.errors[cam_info["name"]] = []
+                    self.errors[cam_info["name"]].append(all_features)
+                    continue
+                cam_info["imsize"] = imsize
+
+                f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+                print("INTRINSIC CALIBRATION")
+                cameraMatrixInit = np.array([[f,    0.0,      imsize[0]/2],
+                                             [0.0,     f,      imsize[1]/2],
+                                             [0.0,   0.0,        1.0]])
+                if cam_info["name"] not in self.cameraIntrinsics.keys():
+                    self.cameraIntrinsics[cam_info["name"]] = cameraMatrixInit
+
+                distCoeffsInit = np.zeros((12, 1))
+                if cam_info["name"] not in self.cameraDistortion:
+                    self.cameraDistortion[cam_info["name"]] = distCoeffsInit
+
+                if cam_info["name"] in self.intrinsic_img:
+                    all_features, all_ids, filtered_images = self.remove_features(filtered_features, filtered_ids, self.intrinsic_img[cam_info["name"]], image_files)
                 else:
-                    self.img_path.sort()
-                cam_info["img_path"] = self.img_path
-                self.name = cam_info["name"]
-                if per_ccm:
-                    all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], features=features, charucos=charucos)
-                    if isinstance(all_features, str) and all_ids is None:
-                        if cam_info["name"] not in self.errors.keys():
-                            self.errors[cam_info["name"]] = []
-                        self.errors[cam_info["name"]].append(all_features)
-                        continue
-                    cam_info["imsize"] = imsize
-
-                    f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
-                    print("INTRINSIC CALIBRATION")
-                    cameraMatrixInit = np.array([[f,    0.0,      imsize[0]/2],
-                                                 [0.0,     f,      imsize[1]/2],
-                                                 [0.0,   0.0,        1.0]])
-                    if cam_info["name"] not in self.cameraIntrinsics.keys():
-                        self.cameraIntrinsics[cam_info["name"]] = cameraMatrixInit
-
-                    distCoeffsInit = np.zeros((12, 1))
-                    if cam_info["name"] not in self.cameraDistortion:
-                        self.cameraDistortion[cam_info["name"]] = distCoeffsInit
-
-                    if cam_info["name"] in self.intrinsic_img:
-                        all_features, all_ids, filtered_images = self.remove_features(filtered_features, filtered_ids, self.intrinsic_img[cam_info["name"]], image_files)
-                    else:
-                        filtered_images = images_path
-                    current_time = time.time()
-                    if self._cameraModel != "fisheye":
-                        print("Filtering corners")
-                        removed_features, filtered_features, filtered_ids = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraMatrixInit, distCoeffsInit)
-
-                        if filtered_features is None:
-                            if cam_info["name"] not in self.errors.keys():
-                                self.errors[cam_info["name"]] = []
-                            self.errors[cam_info["name"]].append(removed_features)
-                            continue
-
-                        print(f"Filtering takes: {time.time()-current_time}")
-                    else:
-                        filtered_features = all_features
-                        filtered_ids = all_ids
-
-                    cam_info['filtered_ids'] = filtered_ids
-                    cam_info['filtered_corners'] = filtered_features
+                    filtered_images = images_path
+                current_time = time.time()
+                if self._cameraModel != "fisheye":
+                    print("Filtering corners")
+                    removed_features, filtered_features, filtered_ids = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraMatrixInit, distCoeffsInit)
 
-                    ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos)
-                    if isinstance(ret, str) and all_ids is None:
+                    if filtered_features is None:
                         if cam_info["name"] not in self.errors.keys():
                             self.errors[cam_info["name"]] = []
-                        self.errors[cam_info["name"]].append(ret)
+                        self.errors[cam_info["name"]].append(removed_features)
                         continue
+
+                    print(f"Filtering takes: {time.time()-current_time}")
                 else:
-                    ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
-                        images_path, cam_info['hfov'], cam_info["name"], charucos)
-                    cam_info['filtered_ids'] = filtered_ids
-                    cam_info['filtered_corners'] = filtered_corners
-                self.cameraIntrinsics[cam_info["name"]] = intrinsics
-                self.cameraDistortion[cam_info["name"]] = dist_coeff
-                cam_info['intrinsics'] = intrinsics
-                cam_info['dist_coeff'] = dist_coeff
-                cam_info['size'] = size # (Width, height)
-                cam_info['reprojection_error'] = ret
-                print("Reprojection error of {0}: {1}".format(
-                    cam_info['name'], ret))
-                
-                coverage_name = cam_info['name']
-                print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}'
-                height, width, _ = coverageImage.shape
-
-                if width > resizeWidth and height > resizeHeight:
-                    coverageImage = cv2.resize(
-                    coverageImage, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width)
-
-                height, width, _ = coverageImage.shape
-                if height > resizeHeight:
-                    height_offset = (height - resizeHeight)//2
-                    coverageImage = coverageImage[height_offset:height_offset+resizeHeight, :]
-                
-                height, width, _ = coverageImage.shape
-                height_offset = (resizeHeight - height)//2
-                width_offset = (resizeWidth - width)//2
-                subImage = np.pad(coverageImage, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0)
-                cv2.putText(subImage, print_text, (50, 50+height_offset), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/1750, (0, 0, 0), 2)
-                if combinedCoverageImage is None:
-                    combinedCoverageImage = subImage
-                else:
-                    combinedCoverageImage = np.hstack((combinedCoverageImage, subImage))
-                coverage_file_path = filepath + '/' + coverage_name + '_coverage.png'
-                
-                cv2.imwrite(coverage_file_path, subImage)
+                    filtered_features = all_features
+                    filtered_ids = all_ids
+
+                cam_info['filtered_ids'] = filtered_ids
+                cam_info['filtered_corners'] = filtered_features
+
+                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos)
+                if isinstance(ret, str) and all_ids is None:
+                    if cam_info["name"] not in self.errors.keys():
+                        self.errors[cam_info["name"]] = []
+                    self.errors[cam_info["name"]].append(ret)
+                    continue
+            else:
+                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
+                    images_path, cam_info['hfov'], cam_info["name"], charucos)
+                cam_info['filtered_ids'] = filtered_ids
+                cam_info['filtered_corners'] = filtered_corners
+            self.cameraIntrinsics[cam_info["name"]] = intrinsics
+            self.cameraDistortion[cam_info["name"]] = dist_coeff
+            cam_info['intrinsics'] = intrinsics
+            cam_info['dist_coeff'] = dist_coeff
+            cam_info['size'] = size # (Width, height)
+            cam_info['reprojection_error'] = ret
+            print("Reprojection error of {0}: {1}".format(
+                cam_info['name'], ret))
+            
+            coverage_name = cam_info['name']
+            print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}'
+            height, width, _ = coverageImage.shape
+
+            if width > resizeWidth and height > resizeHeight:
+                coverageImage = cv2.resize(
+                coverageImage, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width)
+
+            height, width, _ = coverageImage.shape
+            if height > resizeHeight:
+                height_offset = (height - resizeHeight)//2
+                coverageImage = coverageImage[height_offset:height_offset+resizeHeight, :]
+            
+            height, width, _ = coverageImage.shape
+            height_offset = (resizeHeight - height)//2
+            width_offset = (resizeWidth - width)//2
+            subImage = np.pad(coverageImage, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0)
+            cv2.putText(subImage, print_text, (50, 50+height_offset), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/1750, (0, 0, 0), 2)
+            if combinedCoverageImage is None:
+                combinedCoverageImage = subImage
+            else:
+                combinedCoverageImage = np.hstack((combinedCoverageImage, subImage))
+            coverage_file_path = filepath + '/' + coverage_name + '_coverage.png'
+            
+            cv2.imwrite(coverage_file_path, subImage)
         if self.errors != {}:
             string = ""
             for key in self.errors.keys():

From 5025db1f5a2ac6c9a8c1c4193b2c31e3fb0d7a6d Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 14:03:46 +0200
Subject: [PATCH 06/87] Remove self.width, self.height, merge calibration and
 image loading loops

---
 calibration_utils.py | 43 ++++++++++++++++++++-----------------------
 1 file changed, 20 insertions(+), 23 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 6e3a717..f8b5a9b 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -201,29 +201,26 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         # parameters = aruco.DetectorParameters_create()
         combinedCoverageImage = None
         resizeWidth, resizeHeight = 1280, 800
-        self.height = {}
-        self.width = {}
         assert mrk_size != None,  "ERROR: marker size not set"
-        for camera in board_config['cameras'].keys():
-            cam_info = board_config['cameras'][camera]
-            if cam_info["name"] not in self.disableCamera:
-                images_path = filepath + '/' + cam_info['name']
-                image_files = glob.glob(images_path + "/*")
-                image_files.sort()
-                for im in image_files:
-                    frame = cv2.imread(im)
-                    self.height[cam_info["name"]], self.width[cam_info["name"]], _ = frame.shape
-                    widthRatio = resizeWidth / self.width[cam_info["name"]]
-                    heightRatio = resizeHeight / self.height[cam_info["name"]]
-                    if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-                        resizeWidth = self.width[cam_info["name"]]
-                        resizeHeight = self.height[cam_info["name"]]
-                    break
         for camera in board_config['cameras'].keys():
             cam_info = board_config['cameras'][camera]
             self.id = cam_info["name"]
             if cam_info["name"] in self.disableCamera:
                 continue
+
+            images_path = filepath + '/' + cam_info['name']
+            image_files = glob.glob(images_path + "/*")
+            image_files.sort()
+            for im in image_files:
+                frame = cv2.imread(im)
+                height, width, _ = frame.shape
+                widthRatio = resizeWidth / width
+                heightRatio = resizeHeight / height
+                if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
+                    resizeWidth = width
+                    resizeHeight = height
+                break
+
             print(
                 '<------------Calibrating {} ------------>'.format(cam_info['name']))
             images_path = filepath + '/' + cam_info['name']
@@ -250,7 +247,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             cam_info["img_path"] = self.img_path
             self.name = cam_info["name"]
             if per_ccm:
-                all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], features=features, charucos=charucos)
+                all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], width, height, features=features, charucos=charucos)
                 if isinstance(all_features, str) and all_ids is None:
                     if cam_info["name"] not in self.errors.keys():
                         self.errors[cam_info["name"]] = []
@@ -301,7 +298,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     continue
             else:
                 ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
-                    images_path, cam_info['hfov'], cam_info["name"], charucos)
+                    images_path, cam_info['hfov'], cam_info["name"], charucos, width, height)
                 cam_info['filtered_ids'] = filtered_ids
                 cam_info['filtered_corners'] = filtered_corners
             self.cameraIntrinsics[cam_info["name"]] = intrinsics
@@ -437,7 +434,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     
         return 1, board_config
 
-    def getting_features(self, img_path, name, features = None, charucos=None):
+    def getting_features(self, img_path, name, width, height, features = None, charucos=None):
         if charucos != {}:
             allCorners = []
             allIds = []
@@ -445,7 +442,7 @@ def getting_features(self, img_path, name, features = None, charucos=None):
                 ids, charucos = charuco_img
                 allCorners.append(charucos)
                 allIds.append(ids)
-            imsize = (self.width[name], self.height[name])
+            imsize = (width, height)
             return allCorners, allIds, imsize
 
         elif features == None or features == "charucos":
@@ -837,7 +834,7 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
         # imsize = gray.shape[::-1]
         return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
 
-    def calibrate_intrinsics(self, image_files, hfov, name, charucos):
+    def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         assert len(
@@ -851,7 +848,7 @@ def calibrate_intrinsics(self, image_files, hfov, name, charucos):
                 ids, charucos = charuco_img
                 allCorners.append(charucos)
                 allIds.append(ids)
-            imsize = (self.height[name], self.width[name])
+            imsize = (height, width)
 
         coverageImage = np.ones(imsize[::-1], np.uint8) * 255
         coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)

From 8cc9481901c28dad55c4a1d3fce9197ddf0e00fa Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 14:16:27 +0200
Subject: [PATCH 07/87] Remove self.calib_model

---
 calibration_utils.py | 39 +++++++++++++++++++++------------------
 1 file changed, 21 insertions(+), 18 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index f8b5a9b..3ed6975 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -184,7 +184,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         self.cameraIntrinsics = {}
         self.cameraDistortion = {}
         self.distortion_model = {}
-        self.calib_model = {}
         self.errors = {}
         self._enable_rectification_disp = True
         self._cameraModel = camera_model
@@ -202,6 +201,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         combinedCoverageImage = None
         resizeWidth, resizeHeight = 1280, 800
         assert mrk_size != None,  "ERROR: marker size not set"
+        calibModels = {} # Still needs to be passed to stereo calibration
         for camera in board_config['cameras'].keys():
             cam_info = board_config['cameras'][camera]
             self.id = cam_info["name"]
@@ -228,14 +228,15 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 self.cameraModel_ccm, self.model_ccm = cam_info["calib_model"].split("_")
                 if self.cameraModel_ccm == "fisheye":
                     self.model_ccm == None
-                self.calib_model[cam_info["name"]] = self.cameraModel_ccm
+                calib_model = self.cameraModel_ccm
                 self.distortion_model[cam_info["name"]] = self.model_ccm
             else:
-                self.calib_model[cam_info["name"]] = self._cameraModel
+                calib_model = self._cameraModel
                 if cam_info["name"] in self.ccm_model:
                     self.distortion_model[cam_info["name"]] = self.ccm_model[cam_info["name"]]
                 else:
                     self.distortion_model[cam_info["name"]] = self.model
+            calibModels[cam_info['name']] = calib_model
 
 
             features = None
@@ -290,7 +291,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 cam_info['filtered_ids'] = filtered_ids
                 cam_info['filtered_corners'] = filtered_features
 
-                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos)
+                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model)
                 if isinstance(ret, str) and all_ids is None:
                     if cam_info["name"] not in self.errors.keys():
                         self.errors[cam_info["name"]] = []
@@ -298,7 +299,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     continue
             else:
                 ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
-                    images_path, cam_info['hfov'], cam_info["name"], charucos, width, height)
+                    images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model)
                 cam_info['filtered_ids'] = filtered_ids
                 cam_info['filtered_corners'] = filtered_corners
             self.cameraIntrinsics[cam_info["name"]] = intrinsics
@@ -381,7 +382,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                                     removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'] = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], self.cameraIntrinsics["name"], self.cameraDistortion["name"])
                             
                             extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info[
-                                                                   'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, features)
+                                                                   'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, features, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']])
                             if extrinsics[0] == -1:
                                 return -1, extrinsics[1]
     
@@ -426,7 +427,9 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                                                         right_cam_info['dist_coeff'], 
                                                         extrinsics[2], # Translation between left and right Cameras
                                                         extrinsics[3], # Left Rectification rotation 
-                                                        extrinsics[4]) # Right Rectification rotation"""
+                                                        extrinsics[4], # Right Rectification rotation
+                                                        calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
+                                                        )"""
                                                                                             
     
                             left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
@@ -608,13 +611,13 @@ def is_binary_string(s: str) -> bool:
             flags = self.distortion_model[name]
         return flags
 
-    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos):
+    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos, calib_model):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         coverageImage = np.ones(imsize[::-1], np.uint8) * 255
         coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
         coverageImage = self.draw_corners(allCorners, coverageImage)
-        if self.calib_model[name] == 'perspective':
+        if calib_model == 'perspective':
             if features == None or features == "charucos":
                 distortion_flags = self.get_distortion_flags(name)
                 ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
@@ -834,7 +837,7 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
         # imsize = gray.shape[::-1]
         return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
 
-    def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height):
+    def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         assert len(
@@ -853,7 +856,7 @@ def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height)
         coverageImage = np.ones(imsize[::-1], np.uint8) * 255
         coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
         coverageImage = self.draw_corners(allCorners, coverageImage)
-        if self.calib_model[name] == 'perspective':
+        if calib_model == 'perspective':
             distortion_flags = self.get_distortion_flags(name)
             ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
                 allCorners, allIds, imsize, hfov, name, distortion_flags)
@@ -1129,7 +1132,7 @@ def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
         return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
 
 
-    def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds_r, allCorners_r, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, features = None):
+    def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds_r, allCorners_r, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, left_calib_model, right_calib_model, features = None):
         left_corners_sampled = []
         right_corners_sampled = []
         left_ids_sampled = []
@@ -1163,7 +1166,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                                 cv2.TERM_CRITERIA_EPS, 300, 1e-9)
         if per_ccm and extrinsic_per_ccm:
             for i in range(len(left_corners_sampled)):
-                if self.calib_model[left_name] == "perspective":
+                if left_calib_model == "perspective":
                     left_corners_sampled[i] = cv2.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
                     #left_corners_sampled[i] = cv2.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, None)
 
@@ -1171,7 +1174,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                     left_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
                     #left_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, None)
             for i in range(len(right_corners_sampled)):
-                if self.calib_model[right_name] == "perspective":
+                if right_calib_model == "perspective":
                     right_corners_sampled[i] = cv2.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, distCoeff_r, P=cameraMatrix_r)
                     #right_corners_sampled[i] = cv2.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, None)
                 else:
@@ -1450,7 +1453,7 @@ def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r,
         return avg_epipolar
 
 
-    def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, t, r_l, r_r):
+    def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, t, r_l, r_r, left_calib_model, right_calib_model):
         images_left = glob.glob(left_img_pth + '/*.png')
         images_right = glob.glob(right_img_pth + '/*.png')
         images_left.sort()
@@ -1511,14 +1514,14 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
         movePos = True
 
 
-        print(self.calib_model[left_name], self.calib_model[right_name])
-        if self.calib_model[left_name] == 'perspective':
+        print(left_calib_model, right_calib_model)
+        if left_calib_model == 'perspective':
             mapx_l, mapy_l = cv2.initUndistortRectifyMap(
                 M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
         else:
             mapx_l, mapy_l = cv2.fisheye.initUndistortRectifyMap(
                 M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
-        if self.calib_model[right_name] == "perspective":
+        if right_calib_model == "perspective":
             mapx_r, mapy_r = cv2.initUndistortRectifyMap(
                 M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
         else:

From 5d2e0ab6c8b4f1c68472c76fe7f203c7b5a35ea6 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 14:46:04 +0200
Subject: [PATCH 08/87] Remove self.distortion_model

---
 calibration_utils.py | 63 ++++++++++++++++++++++----------------------
 1 file changed, 32 insertions(+), 31 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 3ed6975..691ab1f 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -183,7 +183,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         self.extrinsic_img = extrinsic_img
         self.cameraIntrinsics = {}
         self.cameraDistortion = {}
-        self.distortion_model = {}
         self.errors = {}
         self._enable_rectification_disp = True
         self._cameraModel = camera_model
@@ -202,6 +201,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         resizeWidth, resizeHeight = 1280, 800
         assert mrk_size != None,  "ERROR: marker size not set"
         calibModels = {} # Still needs to be passed to stereo calibration
+        distortionModels = {} # Still needs to be passed to stereo calibration
         for camera in board_config['cameras'].keys():
             cam_info = board_config['cameras'][camera]
             self.id = cam_info["name"]
@@ -229,14 +229,16 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 if self.cameraModel_ccm == "fisheye":
                     self.model_ccm == None
                 calib_model = self.cameraModel_ccm
-                self.distortion_model[cam_info["name"]] = self.model_ccm
+                distortion_model = self.model_ccm
             else:
                 calib_model = self._cameraModel
                 if cam_info["name"] in self.ccm_model:
-                    self.distortion_model[cam_info["name"]] = self.ccm_model[cam_info["name"]]
+                    distortion_model = self.ccm_model[cam_info["name"]]
                 else:
-                    self.distortion_model[cam_info["name"]] = self.model
+                    distortion_model = self.model
             calibModels[cam_info['name']] = calib_model
+            distortionModels[cam_info['name']] = distortion_model
+            print(distortion_model)
 
 
             features = None
@@ -275,7 +277,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 current_time = time.time()
                 if self._cameraModel != "fisheye":
                     print("Filtering corners")
-                    removed_features, filtered_features, filtered_ids = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraMatrixInit, distCoeffsInit)
+                    removed_features, filtered_features, filtered_ids = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraMatrixInit, distCoeffsInit, distortion_model)
 
                     if filtered_features is None:
                         if cam_info["name"] not in self.errors.keys():
@@ -291,7 +293,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 cam_info['filtered_ids'] = filtered_ids
                 cam_info['filtered_corners'] = filtered_features
 
-                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model)
+                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model)
                 if isinstance(ret, str) and all_ids is None:
                     if cam_info["name"] not in self.errors.keys():
                         self.errors[cam_info["name"]] = []
@@ -299,7 +301,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     continue
             else:
                 ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
-                    images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model)
+                    images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model, distortion_model)
                 cam_info['filtered_ids'] = filtered_ids
                 cam_info['filtered_corners'] = filtered_corners
             self.cameraIntrinsics[cam_info["name"]] = intrinsics
@@ -378,11 +380,10 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                                         array = self.extrinsic_img[left_cam_info["name"]]
                                     left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
                                     right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'] = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], self.cameraIntrinsics["name"], self.cameraDistortion["name"])
-                                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'] = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], self.cameraIntrinsics["name"], self.cameraDistortion["name"])
+                                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'] = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], self.cameraIntrinsics["name"], self.cameraDistortion["name"], distortionModels[left_cam_info['name']])
+                                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'] = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], self.cameraIntrinsics["name"], self.cameraDistortion["name"], distortionModels[right_cam_info['name']])
                             
-                            extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info[
-                                                                   'dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, features, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']])
+                            extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
                             if extrinsics[0] == -1:
                                 return -1, extrinsics[1]
     
@@ -457,7 +458,7 @@ def getting_features(self, img_path, name, width, height, features = None, charu
             return allCorners, allIds, imsize
         ###### ADD HERE WHAT IT IS NEEDED ######
 
-    def filtering_features(self,allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit):
+    def filtering_features(self,allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit, distortionModel):
 
          # check if there are any suspicious corners with high reprojection error
         rvecs = []
@@ -489,7 +490,7 @@ def filtering_features(self,allCorners, allIds, name,imsize, hfov, cameraMatrixI
 
         # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
         ret = 0.0
-        distortion_flags = self.get_distortion_flags(name)
+        distortion_flags = self.get_distortion_flags(distortionModel)
         flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
         current = time.time()
         filtered_corners, filtered_ids,all_error, removed_corners, removed_ids, removed_error = self.features_filtering_function(rvecs, tvecs, cameraMatrixInit, distCoeffsInit, ret, allCorners, allIds, camera = name)
@@ -536,23 +537,23 @@ def remove_features(self, allCorners, allIds, array, img_files = None):
 
         return filteredCorners, filteredIds, img_path
 
-    def get_distortion_flags(self,name):
+    def get_distortion_flags(self, distortionModel):
         def is_binary_string(s: str) -> bool:
         # Check if all characters in the string are '0' or '1'
             return all(char in '01' for char in s)
-        if self.distortion_model[name] == None:
+        if distortionModel == None:
             print("Use DEFAULT model")
             flags = cv2.CALIB_RATIONAL_MODEL
-        elif is_binary_string(self.distortion_model[name]):
+        elif is_binary_string(distortionModel):
             flags = cv2.CALIB_RATIONAL_MODEL
             flags += cv2.CALIB_TILTED_MODEL
             flags += cv2.CALIB_THIN_PRISM_MODEL
-            binary_number = int(self.distortion_model[name], 2)
+            binary_number = int(distortionModel, 2)
             # Print the results
             if binary_number == 0:
                 clauses_status = [True, True,True, True, True, True, True, True, True]
             else:
-                clauses_status = [(binary_number & (1 << i)) != 0 for i in range(len(self.distortion_model[name]))]
+                clauses_status = [(binary_number & (1 << i)) != 0 for i in range(len(distortionModel))]
                 clauses_status = clauses_status[::-1]
             if clauses_status[0]:
                 print("FIX_K1")
@@ -582,36 +583,36 @@ def is_binary_string(s: str) -> bool:
                 print("FIX_PRISM_DISTORTION")
                 flags += cv2.CALIB_FIX_S1_S2_S3_S4
 
-        elif isinstance(self.distortion_model[name], str):
-            if self.distortion_model[name] == "NORMAL":
+        elif isinstance(distortionModel, str):
+            if distortionModel == "NORMAL":
                 print("Using NORMAL model")
                 flags = cv2.CALIB_RATIONAL_MODEL
                 flags += cv2.CALIB_TILTED_MODEL
 
-            elif self.distortion_model[name] == "TILTED":
+            elif distortionModel == "TILTED":
                 print("Using TILTED model")
                 flags = cv2.CALIB_RATIONAL_MODEL
                 flags += cv2.CALIB_TILTED_MODEL
 
-            elif self.distortion_model[name] == "PRISM":
+            elif distortionModel == "PRISM":
                 print("Using PRISM model")
                 flags = cv2.CALIB_RATIONAL_MODEL
                 flags += cv2.CALIB_TILTED_MODEL
                 flags += cv2.CALIB_THIN_PRISM_MODEL
 
-            elif self.distortion_model[name] == "THERMAL":
+            elif distortionModel == "THERMAL":
                 print("Using THERMAL model")
                 flags = cv2.CALIB_RATIONAL_MODEL
                 flags += cv2.CALIB_FIX_K3
                 flags += cv2.CALIB_FIX_K5
                 flags += cv2.CALIB_FIX_K6
 
-        elif isinstance(self.distortion_model[name], int):
+        elif isinstance(distortionModel, int):
             print("Using CUSTOM flags")
-            flags = self.distortion_model[name]
+            flags = distortionModel
         return flags
 
-    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos, calib_model):
+    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos, calib_model, distortionModel):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         coverageImage = np.ones(imsize[::-1], np.uint8) * 255
@@ -619,7 +620,7 @@ def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorne
         coverageImage = self.draw_corners(allCorners, coverageImage)
         if calib_model == 'perspective':
             if features == None or features == "charucos":
-                distortion_flags = self.get_distortion_flags(name)
+                distortion_flags = self.get_distortion_flags(distortionModel)
                 ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
                     all_Features, all_features_Ids,allCorners, allIds, imsize, hfov, name, distortion_flags)
             if charucos == {}:
@@ -837,7 +838,7 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
         # imsize = gray.shape[::-1]
         return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
 
-    def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model):
+    def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         assert len(
@@ -857,7 +858,7 @@ def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height,
         coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
         coverageImage = self.draw_corners(allCorners, coverageImage)
         if calib_model == 'perspective':
-            distortion_flags = self.get_distortion_flags(name)
+            distortion_flags = self.get_distortion_flags(distortionModel)
             ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
                 allCorners, allIds, imsize, hfov, name, distortion_flags)
             self.undistort_visualization(
@@ -1132,7 +1133,7 @@ def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
         return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
 
 
-    def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds_r, allCorners_r, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, left_calib_model, right_calib_model, features = None):
+    def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds_r, allCorners_r, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, left_calib_model, right_calib_model, left_distortion_model, right_distortion_model, features = None):
         left_corners_sampled = []
         right_corners_sampled = []
         left_ids_sampled = []
@@ -1217,7 +1218,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
                 # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
                 # print(flags)
                 flags = cv2.CALIB_FIX_INTRINSIC
-                distortion_flags = self.get_distortion_flags(left_name)
+                distortion_flags = self.get_distortion_flags(left_distortion_model)
                 flags += distortion_flags
                 # print(flags)
                 ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(

From 67003e0bea713dde5ced6a4c912f0a75a13fca8c Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 15:34:53 +0200
Subject: [PATCH 09/87] Remove self.cameraIntrinsics, self.cameraDistortion

---
 calibration_utils.py | 79 +++++++++++++++++---------------------------
 1 file changed, 31 insertions(+), 48 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 691ab1f..c4bb56b 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -181,8 +181,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 extrinsic_img[cam].sort(reverse=True)
         self.intrinsic_img = intrinsic_img
         self.extrinsic_img = extrinsic_img
-        self.cameraIntrinsics = {}
-        self.cameraDistortion = {}
         self.errors = {}
         self._enable_rectification_disp = True
         self._cameraModel = camera_model
@@ -202,6 +200,9 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         assert mrk_size != None,  "ERROR: marker size not set"
         calibModels = {} # Still needs to be passed to stereo calibration
         distortionModels = {} # Still needs to be passed to stereo calibration
+        allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
+        allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
+
         for camera in board_config['cameras'].keys():
             cam_info = board_config['cameras'][camera]
             self.id = cam_info["name"]
@@ -260,15 +261,11 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
                 f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
                 print("INTRINSIC CALIBRATION")
-                cameraMatrixInit = np.array([[f,    0.0,      imsize[0]/2],
+                cameraIntrinsics = np.array([[f,    0.0,      imsize[0]/2],
                                              [0.0,     f,      imsize[1]/2],
                                              [0.0,   0.0,        1.0]])
-                if cam_info["name"] not in self.cameraIntrinsics.keys():
-                    self.cameraIntrinsics[cam_info["name"]] = cameraMatrixInit
 
-                distCoeffsInit = np.zeros((12, 1))
-                if cam_info["name"] not in self.cameraDistortion:
-                    self.cameraDistortion[cam_info["name"]] = distCoeffsInit
+                distCoeff = np.zeros((12, 1))
 
                 if cam_info["name"] in self.intrinsic_img:
                     all_features, all_ids, filtered_images = self.remove_features(filtered_features, filtered_ids, self.intrinsic_img[cam_info["name"]], image_files)
@@ -277,7 +274,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 current_time = time.time()
                 if self._cameraModel != "fisheye":
                     print("Filtering corners")
-                    removed_features, filtered_features, filtered_ids = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraMatrixInit, distCoeffsInit, distortion_model)
+                    removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
 
                     if filtered_features is None:
                         if cam_info["name"] not in self.errors.keys():
@@ -293,21 +290,21 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 cam_info['filtered_ids'] = filtered_ids
                 cam_info['filtered_corners'] = filtered_features
 
-                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model)
+                ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
                 if isinstance(ret, str) and all_ids is None:
                     if cam_info["name"] not in self.errors.keys():
                         self.errors[cam_info["name"]] = []
                     self.errors[cam_info["name"]].append(ret)
                     continue
             else:
-                ret, intrinsics, dist_coeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
+                ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
                     images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model, distortion_model)
                 cam_info['filtered_ids'] = filtered_ids
                 cam_info['filtered_corners'] = filtered_corners
-            self.cameraIntrinsics[cam_info["name"]] = intrinsics
-            self.cameraDistortion[cam_info["name"]] = dist_coeff
-            cam_info['intrinsics'] = intrinsics
-            cam_info['dist_coeff'] = dist_coeff
+            allCameraIntrinsics[cam_info["name"]] = cameraIntrinsics
+            allCameraDistCoeffs[cam_info["name"]] = distCoeff
+            cam_info['intrinsics'] = cameraIntrinsics
+            cam_info['dist_coeff'] = distCoeff
             cam_info['size'] = size # (Width, height)
             cam_info['reprojection_error'] = ret
             print("Reprojection error of {0}: {1}".format(
@@ -380,8 +377,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                                         array = self.extrinsic_img[left_cam_info["name"]]
                                     left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
                                     right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'] = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], self.cameraIntrinsics["name"], self.cameraDistortion["name"], distortionModels[left_cam_info['name']])
-                                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'] = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], self.cameraIntrinsics["name"], self.cameraDistortion["name"], distortionModels[right_cam_info['name']])
+                                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
+                                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
                             
                             extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
                             if extrinsics[0] == -1:
@@ -519,9 +516,7 @@ def filtering_features(self,allCorners, allIds, name,imsize, hfov, cameraMatrixI
         except:
             return f"First intrisic calibration failed for {name}", None, None
 
-        self.cameraIntrinsics[name] = camera_matrix
-        self.cameraDistortion[name] = distortion_coefficients
-        return removed_corners, filtered_corners, filtered_ids
+        return removed_corners, filtered_corners, filtered_ids, camera_matrix, distortion_coefficients
     
     def remove_features(self, allCorners, allIds, array, img_files = None):
         filteredCorners = allCorners.copy()
@@ -612,7 +607,7 @@ def is_binary_string(s: str) -> bool:
             flags = distortionModel
         return flags
 
-    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos, calib_model, distortionModel):
+    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos, calib_model, distortionModel, cameraIntrinsics, distCoeff):
         image_files = glob.glob(image_files + "/*")
         image_files.sort()
         coverageImage = np.ones(imsize[::-1], np.uint8) * 255
@@ -621,15 +616,15 @@ def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorne
         if calib_model == 'perspective':
             if features == None or features == "charucos":
                 distortion_flags = self.get_distortion_flags(distortionModel)
-                ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
-                    all_Features, all_features_Ids,allCorners, allIds, imsize, hfov, name, distortion_flags)
+                ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
+                    all_Features, all_features_Ids,allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
             if charucos == {}:
                 self.undistort_visualization(
-                    image_files, camera_matrix, distortion_coefficients, imsize, name)
+                    image_files, cameraIntrinsics, distCoeff, imsize, name)
 
-                return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+                return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
             else:
-                return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+                return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
             #### ADD ADDITIONAL FEATURES CALIBRATION ####
         else:
             if features == None or features == "charucos":
@@ -747,7 +742,7 @@ def camera_pose_charuco(self, objpoints: np.array, corners: np.array, ids: np.ar
             imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
 
             ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners, K, d)
-            imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, self.cameraIntrinsics[self.name], self.cameraDistortion[self.name])
+            imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, K, d)
             
             ini_threshold += threshold_stepper
             index += 1
@@ -940,25 +935,13 @@ def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_c
         return corners_removed, allIds, allCorners
 
 
-    def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, name, distortion_flags):
+    def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
         """
         Calibrates the camera using the dected corners.
         """
         f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2)))
 
-        if name not in self.cameraIntrinsics:
-            cameraMatrixInit = np.array([[f,    0.0,      imsize[0]/2],
-                                     [0.0,     f,      imsize[1]/2],
-                                     [0.0,   0.0,        1.0]])
-            threshold = 20 * imsize[1]/800.0
-        else:
-            cameraMatrixInit = self.cameraIntrinsics[name]
-            threshold = 2 * imsize[1]/800.0
-       
-        if name not in self.cameraDistortion:
-            distCoeffsInit = np.zeros((5, 1))
-        else:
-            distCoeffsInit = self.cameraDistortion[name]
+        threshold = 2 * imsize[1]/800.0
          # check if there are any suspicious corners with high reprojection error
         rvecs = []
         tvecs = []
@@ -969,7 +952,7 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
         for corners, ids in zip(allCorners, allIds):
             self.index = index
             objpts = self.charuco_ids_to_objpoints(ids)
-            rvec, tvec, newids = self.camera_pose_charuco(objpts, corners, ids, cameraMatrixInit, distCoeffsInit)
+            rvec, tvec, newids = self.camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
             tvecs.append(tvec)
             rvecs.append(rvec)
             index += 1
@@ -988,8 +971,8 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
         intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
         distortion_array = {}
         index = 0
-        camera_matrix = cameraMatrixInit
-        distortion_coefficients = distCoeffsInit
+        camera_matrix = cameraIntrinsics
+        distortion_coefficients = distCoeff
         rotation_vectors = rvecs
         translation_vectors = tvecs
         translation_array_x = []
@@ -1032,14 +1015,14 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
                         charucoIds=filtered_ids,
                         board=self._board,
                         imageSize=imsize,
-                        cameraMatrix=cameraMatrixInit,
-                        distCoeffs=distCoeffsInit,
+                        cameraMatrix=cameraIntrinsics,
+                        distCoeffs=distCoeff,
                         flags=flags,
                         criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
                 except:
                     raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=self.id)
-                cameraMatrixInit = camera_matrix
-                distCoeffsInit = distortion_coefficients
+                cameraIntrinsics = camera_matrix
+                distCoeff = distortion_coefficients
                 threshold = 5 * imsize[1]/800.0
                 print(f"Each calibration {time.time()-start}")
                 index += 1

From 466b3c7510844fbf4668aaf48ac81b9b42c1877b Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 15:37:32 +0200
Subject: [PATCH 10/87] Remove redundant .keys()

---
 calibration_utils.py | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index c4bb56b..24d40cb 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -203,7 +203,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
         allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
 
-        for camera in board_config['cameras'].keys():
+        for camera in board_config['cameras']:
             cam_info = board_config['cameras'][camera]
             self.id = cam_info["name"]
             if cam_info["name"] in self.disableCamera:
@@ -225,7 +225,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             print(
                 '<------------Calibrating {} ------------>'.format(cam_info['name']))
             images_path = filepath + '/' + cam_info['name']
-            if "calib_model" in cam_info.keys():
+            if "calib_model" in cam_info:
                 self.cameraModel_ccm, self.model_ccm = cam_info["calib_model"].split("_")
                 if self.cameraModel_ccm == "fisheye":
                     self.model_ccm == None
@@ -253,7 +253,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             if per_ccm:
                 all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], width, height, features=features, charucos=charucos)
                 if isinstance(all_features, str) and all_ids is None:
-                    if cam_info["name"] not in self.errors.keys():
+                    if cam_info["name"] not in self.errors:
                         self.errors[cam_info["name"]] = []
                     self.errors[cam_info["name"]].append(all_features)
                     continue
@@ -277,7 +277,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
 
                     if filtered_features is None:
-                        if cam_info["name"] not in self.errors.keys():
+                        if cam_info["name"] not in self.errors:
                             self.errors[cam_info["name"]] = []
                         self.errors[cam_info["name"]].append(removed_features)
                         continue
@@ -292,7 +292,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
                 ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
                 if isinstance(ret, str) and all_ids is None:
-                    if cam_info["name"] not in self.errors.keys():
+                    if cam_info["name"] not in self.errors:
                         self.errors[cam_info["name"]] = []
                     self.errors[cam_info["name"]].append(ret)
                     continue
@@ -337,7 +337,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             cv2.imwrite(coverage_file_path, subImage)
         if self.errors != {}:
             string = ""
-            for key in self.errors.keys():
+            for key in self.errors:
                 string += self.errors[key][0] + "\n"
             raise StereoExceptions(message=string, stage="intrinsic")
 
@@ -347,7 +347,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             cv2.waitKey(1)
             cv2.destroyAllWindows()
         
-        for camera in board_config['cameras'].keys():
+        for camera in board_config['cameras']:
             left_cam_info = board_config['cameras'][camera]
             if str(left_cam_info["name"]) not in self.disableCamera:
                 if 'extrinsics' in left_cam_info:
@@ -1001,7 +1001,7 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
                 iterations_array.append(index)
                 reprojection.append(ret)
                 for i in range(len(distortion_coefficients)):
-                    if i not in distortion_array.keys():
+                    if i not in distortion_array:
                         distortion_array[i] = []
                     distortion_array[i].append(distortion_coefficients[i][0])
                 print(f"Each filtering {time.time() - start}")

From 07b8b401697d32276383d7097078ced833cd7948 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 16:01:10 +0200
Subject: [PATCH 11/87] Move alibration into a temporary function in another
 scope

---
 calibration_utils.py | 256 +++++++++++++++++++++----------------------
 1 file changed, 126 insertions(+), 130 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 24d40cb..be46580 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -152,6 +152,127 @@ def summary(self) -> str:
         return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
 
+
+def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageImage, features, filepath, cam_info, calibModels, distortionModels, charucos, allCameraIntrinsics, allCameraDistCoeffs, model, ccm_model, _cameraModel, intrinsic_img):
+    images_path = filepath + '/' + cam_info['name']
+    image_files = glob.glob(images_path + "/*")
+    image_files.sort()
+    for im in image_files:
+        frame = cv2.imread(im)
+        height, width, _ = frame.shape
+        widthRatio = resizeWidth / width
+        heightRatio = resizeHeight / height
+        if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
+            resizeWidth = width
+            resizeHeight = height
+        break
+
+    print(
+        '<------------Calibrating {} ------------>'.format(cam_info['name']))
+    images_path = filepath + '/' + cam_info['name']
+    if "calib_model" in cam_info:
+        cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
+        if cameraModel_ccm == "fisheye":
+            model_ccm == None
+        calib_model = cameraModel_ccm
+        distortion_model = model_ccm
+    else:
+        calib_model = _cameraModel
+        if cam_info["name"] in ccm_model:
+            distortion_model = ccm_model[cam_info["name"]]
+        else:
+            distortion_model = model
+    calibModels[cam_info['name']] = calib_model
+    distortionModels[cam_info['name']] = distortion_model
+    print(distortion_model)
+
+
+    img_path = glob.glob(images_path + "/*")
+    if charucos == {}:
+        img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+    else:
+        img_path.sort()
+    cam_info["img_path"] = img_path
+
+    if per_ccm:
+        all_features, all_ids, imsize = calibration.getting_features(images_path, cam_info["name"], width, height, features=features, charucos=charucos)
+        if isinstance(all_features, str) and all_ids is None:
+            raise RuntimeError(f'Exception {all_features}') # TODO : Handle
+        cam_info["imsize"] = imsize
+
+        f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+        print("INTRINSIC CALIBRATION")
+        cameraIntrinsics = np.array([[f,    0.0,      imsize[0]/2],
+                                     [0.0,     f,      imsize[1]/2],
+                                     [0.0,   0.0,        1.0]])
+
+        distCoeff = np.zeros((12, 1))
+
+        if cam_info["name"] in intrinsic_img:
+            all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
+        else:
+            filtered_images = images_path
+        current_time = time.time()
+        if _cameraModel != "fisheye":
+            print("Filtering corners")
+            removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
+
+            if filtered_features is None:
+                raise RuntimeError('Exception') # TODO : Handle
+
+            print(f"Filtering takes: {time.time()-current_time}")
+        else:
+            filtered_features = all_features
+            filtered_ids = all_ids
+
+        cam_info['filtered_ids'] = filtered_ids
+        cam_info['filtered_corners'] = filtered_features
+
+        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
+        if isinstance(ret, str) and all_ids is None:
+            raise RuntimeError('Exception' + ret) # TODO : Handle
+    else:
+        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
+            images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model, distortion_model)
+        cam_info['filtered_ids'] = filtered_ids
+        cam_info['filtered_corners'] = filtered_corners
+    allCameraIntrinsics[cam_info["name"]] = cameraIntrinsics
+    allCameraDistCoeffs[cam_info["name"]] = distCoeff
+    cam_info['intrinsics'] = cameraIntrinsics
+    cam_info['dist_coeff'] = distCoeff
+    cam_info['size'] = size # (Width, height)
+    cam_info['reprojection_error'] = ret
+    print("Reprojection error of {0}: {1}".format(
+        cam_info['name'], ret))
+    
+    coverage_name = cam_info['name']
+    print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}'
+    height, width, _ = coverageImage.shape
+
+    if width > resizeWidth and height > resizeHeight:
+        coverageImage = cv2.resize(
+        coverageImage, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width)
+
+    height, width, _ = coverageImage.shape
+    if height > resizeHeight:
+        height_offset = (height - resizeHeight)//2
+        coverageImage = coverageImage[height_offset:height_offset+resizeHeight, :]
+    
+    height, width, _ = coverageImage.shape
+    height_offset = (resizeHeight - height)//2
+    width_offset = (resizeWidth - width)//2
+    subImage = np.pad(coverageImage, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0)
+    cv2.putText(subImage, print_text, (50, 50+height_offset), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/1750, (0, 0, 0), 2)
+    if combinedCoverageImage is None:
+        combinedCoverageImage = subImage
+    else:
+        combinedCoverageImage = np.hstack((combinedCoverageImage, subImage))
+    coverage_file_path = filepath + '/' + coverage_name + '_coverage.png'
+    
+    cv2.imwrite(coverage_file_path, subImage)
+    return combinedCoverageImage
+
+
 class StereoCalibration(object):
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
@@ -195,146 +316,22 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         self.cams = []
         # parameters = aruco.DetectorParameters_create()
-        combinedCoverageImage = None
+        combinedCoverageImage = None # TODO : It might not actually be combined in the calibration loop
         resizeWidth, resizeHeight = 1280, 800
         assert mrk_size != None,  "ERROR: marker size not set"
         calibModels = {} # Still needs to be passed to stereo calibration
         distortionModels = {} # Still needs to be passed to stereo calibration
         allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
         allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
+        features = None
 
         for camera in board_config['cameras']:
             cam_info = board_config['cameras'][camera]
             self.id = cam_info["name"]
             if cam_info["name"] in self.disableCamera:
                 continue
+            combinedCoverageImage = calibrate_outside(self, resizeWidth, resizeHeight, combinedCoverageImage, features, filepath, cam_info, calibModels, distortionModels, charucos, allCameraIntrinsics, allCameraDistCoeffs, self.model, self.ccm_model, self._cameraModel, self.intrinsic_img)
 
-            images_path = filepath + '/' + cam_info['name']
-            image_files = glob.glob(images_path + "/*")
-            image_files.sort()
-            for im in image_files:
-                frame = cv2.imread(im)
-                height, width, _ = frame.shape
-                widthRatio = resizeWidth / width
-                heightRatio = resizeHeight / height
-                if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-                    resizeWidth = width
-                    resizeHeight = height
-                break
-
-            print(
-                '<------------Calibrating {} ------------>'.format(cam_info['name']))
-            images_path = filepath + '/' + cam_info['name']
-            if "calib_model" in cam_info:
-                self.cameraModel_ccm, self.model_ccm = cam_info["calib_model"].split("_")
-                if self.cameraModel_ccm == "fisheye":
-                    self.model_ccm == None
-                calib_model = self.cameraModel_ccm
-                distortion_model = self.model_ccm
-            else:
-                calib_model = self._cameraModel
-                if cam_info["name"] in self.ccm_model:
-                    distortion_model = self.ccm_model[cam_info["name"]]
-                else:
-                    distortion_model = self.model
-            calibModels[cam_info['name']] = calib_model
-            distortionModels[cam_info['name']] = distortion_model
-            print(distortion_model)
-
-
-            features = None
-            self.img_path = glob.glob(images_path + "/*")
-            if charucos == {}:
-                self.img_path = sorted(self.img_path, key=lambda x: int(x.split('_')[1]))
-            else:
-                self.img_path.sort()
-            cam_info["img_path"] = self.img_path
-            self.name = cam_info["name"]
-            if per_ccm:
-                all_features, all_ids, imsize = self.getting_features(images_path, cam_info["name"], width, height, features=features, charucos=charucos)
-                if isinstance(all_features, str) and all_ids is None:
-                    if cam_info["name"] not in self.errors:
-                        self.errors[cam_info["name"]] = []
-                    self.errors[cam_info["name"]].append(all_features)
-                    continue
-                cam_info["imsize"] = imsize
-
-                f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
-                print("INTRINSIC CALIBRATION")
-                cameraIntrinsics = np.array([[f,    0.0,      imsize[0]/2],
-                                             [0.0,     f,      imsize[1]/2],
-                                             [0.0,   0.0,        1.0]])
-
-                distCoeff = np.zeros((12, 1))
-
-                if cam_info["name"] in self.intrinsic_img:
-                    all_features, all_ids, filtered_images = self.remove_features(filtered_features, filtered_ids, self.intrinsic_img[cam_info["name"]], image_files)
-                else:
-                    filtered_images = images_path
-                current_time = time.time()
-                if self._cameraModel != "fisheye":
-                    print("Filtering corners")
-                    removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = self.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
-
-                    if filtered_features is None:
-                        if cam_info["name"] not in self.errors:
-                            self.errors[cam_info["name"]] = []
-                        self.errors[cam_info["name"]].append(removed_features)
-                        continue
-
-                    print(f"Filtering takes: {time.time()-current_time}")
-                else:
-                    filtered_features = all_features
-                    filtered_ids = all_ids
-
-                cam_info['filtered_ids'] = filtered_ids
-                cam_info['filtered_corners'] = filtered_features
-
-                ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
-                if isinstance(ret, str) and all_ids is None:
-                    if cam_info["name"] not in self.errors:
-                        self.errors[cam_info["name"]] = []
-                    self.errors[cam_info["name"]].append(ret)
-                    continue
-            else:
-                ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = self.calibrate_intrinsics(
-                    images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model, distortion_model)
-                cam_info['filtered_ids'] = filtered_ids
-                cam_info['filtered_corners'] = filtered_corners
-            allCameraIntrinsics[cam_info["name"]] = cameraIntrinsics
-            allCameraDistCoeffs[cam_info["name"]] = distCoeff
-            cam_info['intrinsics'] = cameraIntrinsics
-            cam_info['dist_coeff'] = distCoeff
-            cam_info['size'] = size # (Width, height)
-            cam_info['reprojection_error'] = ret
-            print("Reprojection error of {0}: {1}".format(
-                cam_info['name'], ret))
-            
-            coverage_name = cam_info['name']
-            print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}'
-            height, width, _ = coverageImage.shape
-
-            if width > resizeWidth and height > resizeHeight:
-                coverageImage = cv2.resize(
-                coverageImage, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width)
-
-            height, width, _ = coverageImage.shape
-            if height > resizeHeight:
-                height_offset = (height - resizeHeight)//2
-                coverageImage = coverageImage[height_offset:height_offset+resizeHeight, :]
-            
-            height, width, _ = coverageImage.shape
-            height_offset = (resizeHeight - height)//2
-            width_offset = (resizeWidth - width)//2
-            subImage = np.pad(coverageImage, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0)
-            cv2.putText(subImage, print_text, (50, 50+height_offset), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/1750, (0, 0, 0), 2)
-            if combinedCoverageImage is None:
-                combinedCoverageImage = subImage
-            else:
-                combinedCoverageImage = np.hstack((combinedCoverageImage, subImage))
-            coverage_file_path = filepath + '/' + coverage_name + '_coverage.png'
-            
-            cv2.imwrite(coverage_file_path, subImage)
         if self.errors != {}:
             string = ""
             for key in self.errors:
@@ -495,7 +492,7 @@ def filtering_features(self,allCorners, allIds, name,imsize, hfov, cameraMatrixI
         for index, corners in enumerate(filtered_corners):
             if len(corners) < 4:
                 corner_detector.remove(corners)
-        if len(corner_detector) < int(len(self.img_path)*0.75):
+        if len(corner_detector) < int(len(allCorners)*0.75):
             return f"More than 1/4 of images has less than 4 corners for {name}", None, None
 
                 
@@ -670,8 +667,7 @@ def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, rep
         display_points = []
         circle_size = 0
         reported_error = []
-        for i, (corners, ids, frame_path) in enumerate(zip(filtered_corners, filtered_id, self.img_path)):
-            frame = cv2.imread(frame_path)
+        for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
             if ids is not None and corners.size > 0:
                 ids = ids.flatten()  # Flatten the IDs from 2D to 1D
                 objPoints = np.array([self._board.chessboardCorners[id] for id in ids], dtype=np.float32)

From 1182a25cff1b99a42b885f567b4d76c601bc56b9 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 16:33:39 +0200
Subject: [PATCH 12/87] Split into smaller functions

---
 calibration_utils.py | 97 +++++++++++++++++++++++++-------------------
 1 file changed, 55 insertions(+), 42 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index be46580..754d7d1 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -150,10 +150,42 @@ def summary(self) -> str:
         Returns a more comprehensive summary of the exception
         """
         return f"'{self.args[0]}' (occured during stage '{self.stage}')"
+def get_and_filter_features(calibration, images_path, width, height, features, charucos, cam_info, intrinsic_img, _cameraModel, distortion_model):
+    all_features, all_ids, imsize = calibration.getting_features(images_path, width, height, features=features, charucos=charucos)
 
+    if isinstance(all_features, str) and all_ids is None:
+        raise RuntimeError(f'Exception {all_features}') # TODO : Handle
+    cam_info["imsize"] = imsize
 
+    f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+    print("INTRINSIC CALIBRATION")
+    cameraIntrinsics = np.array([[f,    0.0,      imsize[0]/2],
+                                 [0.0,     f,      imsize[1]/2],
+                                 [0.0,   0.0,        1.0]])
 
-def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageImage, features, filepath, cam_info, calibModels, distortionModels, charucos, allCameraIntrinsics, allCameraDistCoeffs, model, ccm_model, _cameraModel, intrinsic_img):
+    distCoeff = np.zeros((12, 1))
+
+    if cam_info["name"] in intrinsic_img:
+        raise RuntimeError('This is broken')
+        all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
+    else:
+        filtered_images = images_path
+    current_time = time.time()
+    if _cameraModel != "fisheye":
+        print("Filtering corners")
+        removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
+
+        if filtered_features is None:
+            raise RuntimeError('Exception') # TODO : Handle
+
+        print(f"Filtering takes: {time.time()-current_time}")
+    else:
+        filtered_features = all_features
+        filtered_ids = all_ids
+    return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
+
+
+def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageImage, features, filepath, cam_info, calibModels, distortionModels, allCharucos, allCameraIntrinsics, allCameraDistCoeffs, model, ccm_model, _cameraModel, intrinsic_img):
     images_path = filepath + '/' + cam_info['name']
     image_files = glob.glob(images_path + "/*")
     image_files.sort()
@@ -169,6 +201,7 @@ def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageIm
 
     print(
         '<------------Calibrating {} ------------>'.format(cam_info['name']))
+
     images_path = filepath + '/' + cam_info['name']
     if "calib_model" in cam_info:
         cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
@@ -184,56 +217,36 @@ def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageIm
             distortion_model = model
     calibModels[cam_info['name']] = calib_model
     distortionModels[cam_info['name']] = distortion_model
-    print(distortion_model)
+
+    
 
 
     img_path = glob.glob(images_path + "/*")
-    if charucos == {}:
+    if allCharucos == {}:
         img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
     else:
         img_path.sort()
     cam_info["img_path"] = img_path
 
     if per_ccm:
-        all_features, all_ids, imsize = calibration.getting_features(images_path, cam_info["name"], width, height, features=features, charucos=charucos)
-        if isinstance(all_features, str) and all_ids is None:
-            raise RuntimeError(f'Exception {all_features}') # TODO : Handle
-        cam_info["imsize"] = imsize
-
-        f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
-        print("INTRINSIC CALIBRATION")
-        cameraIntrinsics = np.array([[f,    0.0,      imsize[0]/2],
-                                     [0.0,     f,      imsize[1]/2],
-                                     [0.0,   0.0,        1.0]])
-
-        distCoeff = np.zeros((12, 1))
-
-        if cam_info["name"] in intrinsic_img:
-            all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
-        else:
-            filtered_images = images_path
-        current_time = time.time()
-        if _cameraModel != "fisheye":
-            print("Filtering corners")
-            removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
-
-            if filtered_features is None:
-                raise RuntimeError('Exception') # TODO : Handle
-
-            print(f"Filtering takes: {time.time()-current_time}")
-        else:
-            filtered_features = all_features
-            filtered_ids = all_ids
-
+        start = time.time()
+        print('starting getting and filtering')
+        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, images_path, width, height, features, allCharucos[cam_info["name"]], cam_info, intrinsic_img, _cameraModel, distortion_model)
+        
+        print(f'getting and filtering took {round(time.time() - start, 2)}s')
+        
         cam_info['filtered_ids'] = filtered_ids
         cam_info['filtered_corners'] = filtered_features
 
-        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
+        start = time.time()
+        print('starting calibrate_wf')
+        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, allCharucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
         if isinstance(ret, str) and all_ids is None:
             raise RuntimeError('Exception' + ret) # TODO : Handle
+        print(f'calibrate_wf took {round(time.time() - start, 2)}s')
     else:
         ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
-            images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model, distortion_model)
+            images_path, cam_info['hfov'], cam_info["name"], allCharucos, width, height, calib_model, distortion_model)
         cam_info['filtered_ids'] = filtered_ids
         cam_info['filtered_corners'] = filtered_corners
     allCameraIntrinsics[cam_info["name"]] = cameraIntrinsics
@@ -432,23 +445,23 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     
         return 1, board_config
 
-    def getting_features(self, img_path, name, width, height, features = None, charucos=None):
-        if charucos != {}:
+    def getting_features(self, img_path, width, height, features = None, charucos=None):
+        if charucos:
             allCorners = []
             allIds = []
-            for index, charuco_img in enumerate(charucos[name]):
-                ids, charucos = charuco_img
-                allCorners.append(charucos)
+            for index, charuco_img in enumerate(charucos):
+                ids, charuco = charuco_img
+                allCorners.append(charuco)
                 allIds.append(ids)
             imsize = (width, height)
             return allCorners, allIds, imsize
 
         elif features == None or features == "charucos":
-            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(self.img_path)
+            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
             return allCorners, allIds, imsize
 
         if features == "checker_board":
-            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(self.img_path)
+            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
             return allCorners, allIds, imsize
         ###### ADD HERE WHAT IT IS NEEDED ######
 

From e57055fde6475161850ebd3ad35736b6713aa3ad Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 18:00:27 +0200
Subject: [PATCH 13/87] Speedup initial getting and filtering of corners

---
 calibration_utils.py | 128 ++++++++++++++++++++++++++++++++-----------
 1 file changed, 96 insertions(+), 32 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 754d7d1..c3e116e 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -6,9 +6,11 @@
 import shutil
 import numpy as np
 from scipy.spatial.transform import Rotation
+import multiprocessing
 import time
 import json
 import cv2.aruco as aruco
+import threading
 import logging
 logging.getLogger('matplotlib').setLevel(logging.WARNING)
 
@@ -150,6 +152,7 @@ def summary(self) -> str:
         Returns a more comprehensive summary of the exception
         """
         return f"'{self.args[0]}' (occured during stage '{self.stage}')"
+
 def get_and_filter_features(calibration, images_path, width, height, features, charucos, cam_info, intrinsic_img, _cameraModel, distortion_model):
     all_features, all_ids, imsize = calibration.getting_features(images_path, width, height, features=features, charucos=charucos)
 
@@ -170,6 +173,7 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
     else:
         filtered_images = images_path
+
     current_time = time.time()
     if _cameraModel != "fisheye":
         print("Filtering corners")
@@ -285,6 +289,63 @@ def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageIm
     cv2.imwrite(coverage_file_path, subImage)
     return combinedCoverageImage
 
+def estimate_pose_and_filter_single(*args):
+    corners, ids, K, d, min_inliers, max_threshold, threshold_stepper, squaresX, squaresY, squareSize, markerSize = args[0]
+    
+    
+    aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000)
+    board = aruco.CharucoBoard_create(
+        # 22, 16,
+        squaresX, squaresY,
+        squareSize,
+        markerSize,
+        aruco_dictionary)
+    
+    objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
+    
+    ini_threshold=5
+    threshold = None
+    
+    objects = []
+    all_objects = []
+    while len(objects) < len(objpoints[:,0,0]) * min_inliers:
+        if ini_threshold > max_threshold:
+            break
+        ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+        all_objects.append(objects)
+        imgpoints2 = objpoints.copy()
+
+        all_corners2 = corners.copy()
+        all_corners2 = np.array([all_corners2[id[0]] for id in objects])
+        imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+
+        ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, K, d)
+        
+        ini_threshold += threshold_stepper
+    if not ret:
+        raise RuntimeError('Exception') # TODO : Handle
+    
+    if ids is not None and corners.size > 0:
+        ids = ids.flatten()  # Flatten the IDs from 2D to 1D
+        imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, K, d)
+        corners2 = corners.reshape(-1, 2)
+        imgpoints2 = imgpoints2.reshape(-1, 2)
+
+        errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
+        if threshold == None:
+            threshold = max(2*np.median(errors), 150)
+        valid_mask = errors <= threshold
+        removed_mask = ~valid_mask
+
+        # Collect valid IDs in the original format (array of arrays)
+        valid_ids = ids[valid_mask]
+        #filtered_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+
+        # Collect data for valid points
+        #filtered_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+
+        #removed_corners.extend(corners2[removed_mask])
+        return valid_ids.reshape(-1, 1).astype(np.int32), corners2[valid_mask].reshape(-1, 1, 2), corners2[removed_mask]
 
 class StereoCalibration(object):
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
@@ -326,6 +387,10 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             square_size,
             mrk_size,
             self._aruco_dictionary)
+        self.squaresX = squaresX
+        self.squaresY = squaresY
+        self.squareSize = square_size
+        self.markerSize = mrk_size
 
         self.cams = []
         # parameters = aruco.DetectorParameters_create()
@@ -465,51 +530,52 @@ def getting_features(self, img_path, width, height, features = None, charucos=No
             return allCorners, allIds, imsize
         ###### ADD HERE WHAT IT IS NEEDED ######
 
-    def filtering_features(self,allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit, distortionModel):
-
+    def estimate_pose_and_filter(self, allCorners, allIds, name,imsize, hfov, K, d):
          # check if there are any suspicious corners with high reprojection error
-        rvecs = []
-        tvecs = []
         index = 0
-        self.index = 0
         max_threshold = 75 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
         threshold_stepper = int(1.5 * (hfov / 30 + imsize[1] / 800))
         if threshold_stepper < 1:
             threshold_stepper = 1
         print(threshold_stepper)
-        min_inlier = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
+        min_inliers = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
         overall_pose = time.time()
         for index, corners in enumerate(allCorners):
             if len(corners) < 4:
-                return f"Less than 4 corners detected on {index} image.", None, None
-        for corners, ids in zip(allCorners, allIds):
-            current = time.time()
-            self.index = index
-            objpts = self.charuco_ids_to_objpoints(ids)
-            rvec, tvec, newids = self.camera_pose_charuco(objpts, corners, ids, cameraMatrixInit, distCoeffsInit, max_threshold = max_threshold, min_inliers=min_inlier, ini_threshold = 5, threshold_stepper=threshold_stepper)
-            #allCorners[index] = np.array([corners[id[0]-1] for id in newids])
-            #allIds[index] = np.array([ids[id[0]-1] for id in newids])
-            tvecs.append(tvec)
-            rvecs.append(rvec)
-            print(f"Pose estimation {index}, {time.time() -current}s")
-            index += 1
+                raise RuntimeError(f"Less than 4 corners detected on {index} image.")
+        current = time.time()
+        
+        filtered_corners = []
+        filtered_ids = []
+        removed_corners = []
+            
+        
+        #for corners, ids in zip(allCorners, allIds):
+        current = time.time()
+        
+        pool = multiprocessing.Pool(processes=16)
+
+        # Map the functions to the pool
+        results = pool.map(func=estimate_pose_and_filter_single, iterable=[(a, b, K, d, min_inliers, max_threshold, threshold_stepper, self.squaresX, self.squaresY, self.squareSize, self.markerSize) for a, b in zip(allCorners, allIds)])
+
+        filtered_ids, filtered_corners, removed_corners = zip(*results)
+
         print(f"Overall pose estimation {time.time() - overall_pose}s")
 
-        # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
-        ret = 0.0
+        if sum([len(corners) < 4 for corners in filtered_corners]) > 0.15 * len(allCorners):
+            raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {name}")
+        print(f"Filtering {time.time() -current}s")
+        
+        return filtered_corners, filtered_ids, removed_corners
+
+    def filtering_features(self, allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit, distortionModel):
+
+         # check if there are any suspicious corners with high reprojection error
+        filtered_corners, filtered_ids, removed_corners = self.estimate_pose_and_filter(allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit)
+
         distortion_flags = self.get_distortion_flags(distortionModel)
         flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
-        current = time.time()
-        filtered_corners, filtered_ids,all_error, removed_corners, removed_ids, removed_error = self.features_filtering_function(rvecs, tvecs, cameraMatrixInit, distCoeffsInit, ret, allCorners, allIds, camera = name)
-        corner_detector = filtered_corners.copy()
-        for index, corners in enumerate(filtered_corners):
-            if len(corners) < 4:
-                corner_detector.remove(corners)
-        if len(corner_detector) < int(len(allCorners)*0.75):
-            return f"More than 1/4 of images has less than 4 corners for {name}", None, None
-
                 
-        print(f"Filtering {time.time() -current}s")
         try:
             (ret, camera_matrix, distortion_coefficients,
                      rotation_vectors, translation_vectors,
@@ -954,12 +1020,10 @@ def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, a
          # check if there are any suspicious corners with high reprojection error
         rvecs = []
         tvecs = []
-        self.index = 0
         index = 0
         max_threshold = 10 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
         min_inlier = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
         for corners, ids in zip(allCorners, allIds):
-            self.index = index
             objpts = self.charuco_ids_to_objpoints(ids)
             rvec, tvec, newids = self.camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
             tvecs.append(tvec)

From dc865783420d1b4ff81e33a13cd5cd3353ac1d02 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 20:02:43 +0200
Subject: [PATCH 14/87] Preparation for more multiprocessing, proxy class for
 possing aruco to a different process

---
 calibration_utils.py | 318 +++++++++++++++++++++----------------------
 1 file changed, 157 insertions(+), 161 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index c3e116e..8b41bf5 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -36,6 +36,41 @@
           (0.5, 1.0, 1.0),   # all channels set to 1.0 at 0.5 to create white
           (1.0, 0.0, 0.0))   # no blue at 1
 }
+initial_max_threshold = 15
+initial_min_filtered = 0.05
+calibration_max_threshold = 10
+
+class ProxyDict:
+  def __init__(self, squaresX, squaresY, squareSize, markerSize, dictSize):
+    self.squaresX = squaresX
+    self.squaresY = squaresY
+    self.squareSize = squareSize
+    self.markerSize = markerSize
+    self.dictSize = dictSize
+
+  def __getstate__(self):
+      state = self.__dict__.copy()
+      for hidden in ['_board', '_dictionary']:
+        if hidden in state:
+          del state[hidden]
+      return state
+
+  def __build(self):
+    self._dictionary = aruco.Dictionary_get(self.dictSize)
+    self._board = aruco.CharucoBoard_create(self.squaresX, self.squaresY, self.squareSize, self.markerSize, self._dictionary)
+
+  @property
+  def dictionary(self):
+    if not hasattr(self, '_dictionary'):
+      self.__build()
+    return self._dictionary
+
+  @property
+  def board(self):
+    if not hasattr(self, '_board'):
+      self.__build()
+    return self._board
+
 # Create the colormap using the dictionary
 GnRd = colors.LinearSegmentedColormap('GnRd', cdict)
 def get_quadrant_coordinates(width, height, nx, ny):
@@ -153,6 +188,57 @@ def summary(self) -> str:
         """
         return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
+def estimate_pose_and_filter_single(args):
+    calibration, corners, ids, K, d, min_inliers, max_threshold, threshold_stepper = args
+
+    board = calibration._board
+    
+    objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
+    
+    ini_threshold=5
+    threshold = None
+    
+    objects = []
+    all_objects = []
+    while len(objects) < len(objpoints[:,0,0]) * min_inliers:
+        if ini_threshold > max_threshold:
+            break
+        ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+        all_objects.append(objects)
+        imgpoints2 = objpoints.copy()
+
+        all_corners2 = corners.copy()
+        all_corners2 = np.array([all_corners2[id[0]] for id in objects])
+        imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+
+        ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, K, d)
+        
+        ini_threshold += threshold_stepper
+    if not ret:
+        raise RuntimeError('Exception') # TODO : Handle
+    
+    if ids is not None and corners.size > 0:
+        ids = ids.flatten()  # Flatten the IDs from 2D to 1D
+        imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, K, d)
+        corners2 = corners.reshape(-1, 2)
+        imgpoints2 = imgpoints2.reshape(-1, 2)
+
+        errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
+        if threshold == None:
+            threshold = max(2*np.median(errors), 150)
+        valid_mask = errors <= threshold
+        removed_mask = ~valid_mask
+
+        # Collect valid IDs in the original format (array of arrays)
+        valid_ids = ids[valid_mask]
+        #filtered_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+
+        # Collect data for valid points
+        #filtered_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+
+        #removed_corners.extend(corners2[removed_mask])
+        return valid_ids.reshape(-1, 1).astype(np.int32), corners2[valid_mask].reshape(-1, 1, 2), corners2[removed_mask]
+
 def get_and_filter_features(calibration, images_path, width, height, features, charucos, cam_info, intrinsic_img, _cameraModel, distortion_model):
     all_features, all_ids, imsize = calibration.getting_features(images_path, width, height, features=features, charucos=charucos)
 
@@ -188,54 +274,11 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         filtered_ids = all_ids
     return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
 
-
-def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageImage, features, filepath, cam_info, calibModels, distortionModels, allCharucos, allCameraIntrinsics, allCameraDistCoeffs, model, ccm_model, _cameraModel, intrinsic_img):
-    images_path = filepath + '/' + cam_info['name']
-    image_files = glob.glob(images_path + "/*")
-    image_files.sort()
-    for im in image_files:
-        frame = cv2.imread(im)
-        height, width, _ = frame.shape
-        widthRatio = resizeWidth / width
-        heightRatio = resizeHeight / height
-        if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-            resizeWidth = width
-            resizeHeight = height
-        break
-
-    print(
-        '<------------Calibrating {} ------------>'.format(cam_info['name']))
-
-    images_path = filepath + '/' + cam_info['name']
-    if "calib_model" in cam_info:
-        cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
-        if cameraModel_ccm == "fisheye":
-            model_ccm == None
-        calib_model = cameraModel_ccm
-        distortion_model = model_ccm
-    else:
-        calib_model = _cameraModel
-        if cam_info["name"] in ccm_model:
-            distortion_model = ccm_model[cam_info["name"]]
-        else:
-            distortion_model = model
-    calibModels[cam_info['name']] = calib_model
-    distortionModels[cam_info['name']] = distortion_model
-
-    
-
-
-    img_path = glob.glob(images_path + "/*")
-    if allCharucos == {}:
-        img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
-    else:
-        img_path.sort()
-    cam_info["img_path"] = img_path
-
+def calibrate_outside(calibration, features, images_path, cam_info, calib_model, distortion_model, charucos, _cameraModel, intrinsic_img, width, height):
     if per_ccm:
         start = time.time()
         print('starting getting and filtering')
-        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, images_path, width, height, features, allCharucos[cam_info["name"]], cam_info, intrinsic_img, _cameraModel, distortion_model)
+        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, images_path, width, height, features, charucos, cam_info, intrinsic_img, _cameraModel, distortion_model)
         
         print(f'getting and filtering took {round(time.time() - start, 2)}s')
         
@@ -244,17 +287,16 @@ def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageIm
 
         start = time.time()
         print('starting calibrate_wf')
-        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, allCharucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
+        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
         if isinstance(ret, str) and all_ids is None:
             raise RuntimeError('Exception' + ret) # TODO : Handle
         print(f'calibrate_wf took {round(time.time() - start, 2)}s')
     else:
         ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
-            images_path, cam_info['hfov'], cam_info["name"], allCharucos, width, height, calib_model, distortion_model)
+            images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model, distortion_model)
         cam_info['filtered_ids'] = filtered_ids
         cam_info['filtered_corners'] = filtered_corners
-    allCameraIntrinsics[cam_info["name"]] = cameraIntrinsics
-    allCameraDistCoeffs[cam_info["name"]] = distCoeff
+
     cam_info['intrinsics'] = cameraIntrinsics
     cam_info['dist_coeff'] = distCoeff
     cam_info['size'] = size # (Width, height)
@@ -262,90 +304,64 @@ def calibrate_outside(calibration, resizeWidth, resizeHeight, combinedCoverageIm
     print("Reprojection error of {0}: {1}".format(
         cam_info['name'], ret))
     
-    coverage_name = cam_info['name']
-    print_text = f'Coverage Image of {coverage_name} with reprojection error of {round(ret,5)}'
-    height, width, _ = coverageImage.shape
-
-    if width > resizeWidth and height > resizeHeight:
-        coverageImage = cv2.resize(
-        coverageImage, (0, 0), fx= resizeWidth / width, fy= resizeWidth / width)
-
-    height, width, _ = coverageImage.shape
-    if height > resizeHeight:
-        height_offset = (height - resizeHeight)//2
-        coverageImage = coverageImage[height_offset:height_offset+resizeHeight, :]
-    
-    height, width, _ = coverageImage.shape
-    height_offset = (resizeHeight - height)//2
-    width_offset = (resizeWidth - width)//2
-    subImage = np.pad(coverageImage, ((height_offset, height_offset), (width_offset, width_offset), (0, 0)), 'constant', constant_values=0)
-    cv2.putText(subImage, print_text, (50, 50+height_offset), cv2.FONT_HERSHEY_SIMPLEX, 2*coverageImage.shape[0]/1750, (0, 0, 0), 2)
-    if combinedCoverageImage is None:
-        combinedCoverageImage = subImage
-    else:
-        combinedCoverageImage = np.hstack((combinedCoverageImage, subImage))
-    coverage_file_path = filepath + '/' + coverage_name + '_coverage.png'
+    return cameraIntrinsics, distCoeff
+
+def calibrate_ccms(calibration, board_config, filepath, charucos, model, ccm_model, _cameraModel, intrinsic_img, features, squaresX, squaresY, squareSize, markerSize):
+    calibModels = {} # Still needs to be passed to stereo calibration
+    distortionModels = {} # Still needs to be passed to stereo calibration
+    allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
+    allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
+    combinedCoverageImage = None # TODO : It might not actually be combined in the calibration loop
+    resizeWidth, resizeHeight = 1280, 800
     
-    cv2.imwrite(coverage_file_path, subImage)
-    return combinedCoverageImage
+    activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in calibration.disableCamera]
 
-def estimate_pose_and_filter_single(*args):
-    corners, ids, K, d, min_inliers, max_threshold, threshold_stepper, squaresX, squaresY, squareSize, markerSize = args[0]
-    
-    
-    aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000)
-    board = aruco.CharucoBoard_create(
-        # 22, 16,
-        squaresX, squaresY,
-        squareSize,
-        markerSize,
-        aruco_dictionary)
-    
-    objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
-    
-    ini_threshold=5
-    threshold = None
-    
-    objects = []
-    all_objects = []
-    while len(objects) < len(objpoints[:,0,0]) * min_inliers:
-        if ini_threshold > max_threshold:
+    for cam, cam_info in activeCameras:
+        images_path = filepath + '/' + cam_info['name']
+        image_files = glob.glob(images_path + "/*")
+        image_files.sort()
+        for im in image_files:
+            frame = cv2.imread(im)
+            height, width, _ = frame.shape
+            widthRatio = resizeWidth / width
+            heightRatio = resizeHeight / height
+            if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
+                resizeWidth = width
+                resizeHeight = height
             break
-        ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-        all_objects.append(objects)
-        imgpoints2 = objpoints.copy()
-
-        all_corners2 = corners.copy()
-        all_corners2 = np.array([all_corners2[id[0]] for id in objects])
-        imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
-
-        ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, K, d)
-        
-        ini_threshold += threshold_stepper
-    if not ret:
-        raise RuntimeError('Exception') # TODO : Handle
-    
-    if ids is not None and corners.size > 0:
-        ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-        imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, K, d)
-        corners2 = corners.reshape(-1, 2)
-        imgpoints2 = imgpoints2.reshape(-1, 2)
-
-        errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
-        if threshold == None:
-            threshold = max(2*np.median(errors), 150)
-        valid_mask = errors <= threshold
-        removed_mask = ~valid_mask
+        cam_info['width'] = width
+        cam_info['height'] = height
+
+        images_path = filepath + '/' + cam_info['name']
+        if "calib_model" in cam_info:
+            cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
+            if cameraModel_ccm == "fisheye":
+                model_ccm == None
+            calib_model = cameraModel_ccm
+            distortion_model = model_ccm
+        else:
+            calib_model = _cameraModel
+            if cam_info["name"] in ccm_model:
+                distortion_model = ccm_model[cam_info["name"]]
+            else:
+                distortion_model = model
+        calibModels[cam_info['name']] = calib_model
+        distortionModels[cam_info['name']] = distortion_model
 
-        # Collect valid IDs in the original format (array of arrays)
-        valid_ids = ids[valid_mask]
-        #filtered_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+        img_path = glob.glob(images_path + "/*")
+        if charucos == {}:
+            img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+        else:
+            img_path.sort()
+        cam_info["img_path"] = img_path
 
-        # Collect data for valid points
-        #filtered_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+    #with Pool() as pool:
+    #    result = pool.map(calibrate_outside, [[features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height'], squaresX, squaresY, squareSize, markerSize] for _, cam_info in activeCameras])
+    for cam, cam_info in activeCameras:
+            
+        K, d = calibrate_outside(calibration, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height'])
 
-        #removed_corners.extend(corners2[removed_mask])
-        return valid_ids.reshape(-1, 1).astype(np.int32), corners2[valid_mask].reshape(-1, 1, 2), corners2[removed_mask]
+    return calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs
 
 class StereoCalibration(object):
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
@@ -356,7 +372,6 @@ def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, disa
         self.model = model
         self.output_scale_factor = outputScaleFactor
         self.disableCamera = disableCamera
-        self.errors = {}
         self.initial_max_threshold = initial_max_threshold
         self.initial_min_filtered = initial_min_filtered
         self.calibration_max_threshold = calibration_max_threshold
@@ -364,6 +379,14 @@ def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, disa
 
         """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
+    @property
+    def _aruco_dictionary(self):
+        return self._proxyDict.dictionary
+
+    @property
+    def _board(self):
+        return self._proxyDict.board
+
     def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify, charucos = {}, intrinsic_img = {}, extrinsic_img = []):
         """Function to calculate calibration for stereo camera."""
         start_time = time.time()
@@ -376,47 +399,22 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 extrinsic_img[cam].sort(reverse=True)
         self.intrinsic_img = intrinsic_img
         self.extrinsic_img = extrinsic_img
-        self.errors = {}
         self._enable_rectification_disp = True
         self._cameraModel = camera_model
         self._data_path = filepath
-        self._aruco_dictionary = aruco.Dictionary_get(aruco.DICT_4X4_1000)
-        self._board = aruco.CharucoBoard_create(
-            # 22, 16,
-            squaresX, squaresY,
-            square_size,
-            mrk_size,
-            self._aruco_dictionary)
+        self._proxyDict = ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000)
         self.squaresX = squaresX
         self.squaresY = squaresY
         self.squareSize = square_size
         self.markerSize = mrk_size
 
         self.cams = []
+        features = None
         # parameters = aruco.DetectorParameters_create()
-        combinedCoverageImage = None # TODO : It might not actually be combined in the calibration loop
-        resizeWidth, resizeHeight = 1280, 800
         assert mrk_size != None,  "ERROR: marker size not set"
-        calibModels = {} # Still needs to be passed to stereo calibration
-        distortionModels = {} # Still needs to be passed to stereo calibration
-        allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
-        allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
-        features = None
 
-        for camera in board_config['cameras']:
-            cam_info = board_config['cameras'][camera]
-            self.id = cam_info["name"]
-            if cam_info["name"] in self.disableCamera:
-                continue
-            combinedCoverageImage = calibrate_outside(self, resizeWidth, resizeHeight, combinedCoverageImage, features, filepath, cam_info, calibModels, distortionModels, charucos, allCameraIntrinsics, allCameraDistCoeffs, self.model, self.ccm_model, self._cameraModel, self.intrinsic_img)
-
-        if self.errors != {}:
-            string = ""
-            for key in self.errors:
-                string += self.errors[key][0] + "\n"
-            raise StereoExceptions(message=string, stage="intrinsic")
-
-        combinedCoverageImage = cv2.resize(combinedCoverageImage, (0, 0), fx=self.output_scale_factor, fy=self.output_scale_factor)
+        calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs = calibrate_ccms(self, board_config, filepath, charucos, self.model, self.ccm_model, self._cameraModel, self.intrinsic_img, features, self.squaresX, self.squaresY, self.squareSize, self.markerSize)
+
         if enable_disp_rectify:
             # cv2.imshow('coverage image', combinedCoverageImage)
             cv2.waitKey(1)
@@ -553,10 +551,8 @@ def estimate_pose_and_filter(self, allCorners, allIds, name,imsize, hfov, K, d):
         #for corners, ids in zip(allCorners, allIds):
         current = time.time()
         
-        pool = multiprocessing.Pool(processes=16)
-
-        # Map the functions to the pool
-        results = pool.map(func=estimate_pose_and_filter_single, iterable=[(a, b, K, d, min_inliers, max_threshold, threshold_stepper, self.squaresX, self.squaresY, self.squareSize, self.markerSize) for a, b in zip(allCorners, allIds)])
+        with multiprocessing.Pool(processes=16) as pool:
+            results = pool.map(func=estimate_pose_and_filter_single, iterable=[(self, a, b, K, d, min_inliers, max_threshold, threshold_stepper) for a, b in zip(allCorners, allIds)])
 
         filtered_ids, filtered_corners, removed_corners = zip(*results)
 

From 94b4166ddcadd83b42639eefa926d77b58dde4e8 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 20:11:35 +0200
Subject: [PATCH 15/87] Run individual ccm calibrations in multiple threads

---
 calibration_utils.py | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 8b41bf5..d726b12 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -7,6 +7,8 @@
 import numpy as np
 from scipy.spatial.transform import Rotation
 import multiprocessing
+from multiprocessing.pool import ThreadPool
+
 import time
 import json
 import cv2.aruco as aruco
@@ -274,7 +276,9 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         filtered_ids = all_ids
     return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
 
-def calibrate_outside(calibration, features, images_path, cam_info, calib_model, distortion_model, charucos, _cameraModel, intrinsic_img, width, height):
+def calibrate_outside(args):
+    calibration, features, images_path, cam_info, calib_model, distortion_model, charucos, _cameraModel, intrinsic_img, width, height = args
+
     if per_ccm:
         start = time.time()
         print('starting getting and filtering')
@@ -355,11 +359,8 @@ def calibrate_ccms(calibration, board_config, filepath, charucos, model, ccm_mod
             img_path.sort()
         cam_info["img_path"] = img_path
 
-    #with Pool() as pool:
-    #    result = pool.map(calibrate_outside, [[features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height'], squaresX, squaresY, squareSize, markerSize] for _, cam_info in activeCameras])
-    for cam, cam_info in activeCameras:
-            
-        K, d = calibrate_outside(calibration, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height'])
+    with ThreadPool(3) as pool:
+        result = pool.map(calibrate_outside, [[calibration, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height']] for _, cam_info in activeCameras])
 
     return calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs
 

From eb86106667794e25444cd086950b38e3cb40c5ff Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 20:16:01 +0200
Subject: [PATCH 16/87] Invert exit condition

---
 calibration_utils.py | 167 ++++++++++++++++++++++---------------------
 1 file changed, 84 insertions(+), 83 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index d726b12..7912cee 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -423,89 +423,90 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         
         for camera in board_config['cameras']:
             left_cam_info = board_config['cameras'][camera]
-            if str(left_cam_info["name"]) not in self.disableCamera:
-                if 'extrinsics' in left_cam_info:
-                    if 'to_cam' in left_cam_info['extrinsics']:
-                        left_cam = camera
-                        right_cam = left_cam_info['extrinsics']['to_cam']
-                        left_path = filepath + '/' + left_cam_info['name']
-    
-                        right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
-                        if str(right_cam_info["name"]) not in self.disableCamera:
-                            right_path = filepath + '/' + right_cam_info['name']
-                            print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
-                                left_cam_info['name'], right_cam_info['name']))
-    
-                            specTranslation = left_cam_info['extrinsics']['specTranslation']
-                            rot = left_cam_info['extrinsics']['rotation']
-    
-                            translation = np.array(
-                                [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-                            rotation = Rotation.from_euler(
-                                'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-                            if per_ccm and extrinsic_per_ccm:
-                                if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
-                                    if left_cam_info["name"] in self.extrinsic_img:
-                                        array = self.extrinsic_img[left_cam_info["name"]]
-                                    elif right_cam_info["name"] in self.extrinsic_img:
-                                        array = self.extrinsic_img[left_cam_info["name"]]
-                                    left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-                                    right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
-                                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
-                            
-                            extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
-                            if extrinsics[0] == -1:
-                                return -1, extrinsics[1]
-    
-                            if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
-                                board_config['stereo_config']['rectification_left'] = extrinsics[3]
-                                board_config['stereo_config']['rectification_right'] = extrinsics[4]
-
-                            elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-                                board_config['stereo_config']['rectification_left'] = extrinsics[4]
-                                board_config['stereo_config']['rectification_right'] = extrinsics[3]
-    
-                            """ for stereoObj in board_config['stereo_config']:
-    
-                                if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
-                                    stereoObj['rectification_left'] = extrinsics[3]
-                                    stereoObj['rectification_right'] = extrinsics[4] """
-    
-                            print('<-------------Epipolar error of {} and {} ------------>'.format(
-                                left_cam_info['name'], right_cam_info['name']))
-                            #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
-                            #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
-                            if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
-                                scale = right_cam_info['intrinsics'][0][0]
-                            else:
-                                scale = left_cam_info['intrinsics'][0][0]
-                            if per_ccm and extrinsic_per_ccm:
-                                scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
-                                print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
-                                left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-                                left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
-                            else:
-                                print(f"Epipolar error {extrinsics[0]}")
-                                left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-                                left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
-                            """self.test_epipolar_charuco(left_cam_info['name'], 
-                                                        right_cam_info['name'],
-                                                        left_path, 
-                                                        right_path, 
-                                                        left_cam_info['intrinsics'], 
-                                                        left_cam_info['dist_coeff'], 
-                                                        right_cam_info['intrinsics'], 
-                                                        right_cam_info['dist_coeff'], 
-                                                        extrinsics[2], # Translation between left and right Cameras
-                                                        extrinsics[3], # Left Rectification rotation 
-                                                        extrinsics[4], # Right Rectification rotation
-                                                        calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
-                                                        )"""
-                                                                                            
-    
-                            left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
-                            left_cam_info['extrinsics']['translation'] = extrinsics[2]
+            if str(left_cam_info["name"]) in self.disableCamera:
+                continue
+            if 'extrinsics' in left_cam_info:
+                if 'to_cam' in left_cam_info['extrinsics']:
+                    left_cam = camera
+                    right_cam = left_cam_info['extrinsics']['to_cam']
+                    left_path = filepath + '/' + left_cam_info['name']
+
+                    right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
+                    if str(right_cam_info["name"]) not in self.disableCamera:
+                        right_path = filepath + '/' + right_cam_info['name']
+                        print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
+                            left_cam_info['name'], right_cam_info['name']))
+
+                        specTranslation = left_cam_info['extrinsics']['specTranslation']
+                        rot = left_cam_info['extrinsics']['rotation']
+
+                        translation = np.array(
+                            [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+                        rotation = Rotation.from_euler(
+                            'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+                        if per_ccm and extrinsic_per_ccm:
+                            if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
+                                if left_cam_info["name"] in self.extrinsic_img:
+                                    array = self.extrinsic_img[left_cam_info["name"]]
+                                elif right_cam_info["name"] in self.extrinsic_img:
+                                    array = self.extrinsic_img[left_cam_info["name"]]
+                                left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+                                right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+                                removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
+                                removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
+                        
+                        extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
+                        if extrinsics[0] == -1:
+                            return -1, extrinsics[1]
+
+                        if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
+                            board_config['stereo_config']['rectification_left'] = extrinsics[3]
+                            board_config['stereo_config']['rectification_right'] = extrinsics[4]
+
+                        elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+                            board_config['stereo_config']['rectification_left'] = extrinsics[4]
+                            board_config['stereo_config']['rectification_right'] = extrinsics[3]
+
+                        """ for stereoObj in board_config['stereo_config']:
+
+                            if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
+                                stereoObj['rectification_left'] = extrinsics[3]
+                                stereoObj['rectification_right'] = extrinsics[4] """
+
+                        print('<-------------Epipolar error of {} and {} ------------>'.format(
+                            left_cam_info['name'], right_cam_info['name']))
+                        #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
+                        #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
+                        if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
+                            scale = right_cam_info['intrinsics'][0][0]
+                        else:
+                            scale = left_cam_info['intrinsics'][0][0]
+                        if per_ccm and extrinsic_per_ccm:
+                            scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
+                            print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
+                            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
+                            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
+                        else:
+                            print(f"Epipolar error {extrinsics[0]}")
+                            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
+                            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
+                        """self.test_epipolar_charuco(left_cam_info['name'], 
+                                                    right_cam_info['name'],
+                                                    left_path, 
+                                                    right_path, 
+                                                    left_cam_info['intrinsics'], 
+                                                    left_cam_info['dist_coeff'], 
+                                                    right_cam_info['intrinsics'], 
+                                                    right_cam_info['dist_coeff'], 
+                                                    extrinsics[2], # Translation between left and right Cameras
+                                                    extrinsics[3], # Left Rectification rotation 
+                                                    extrinsics[4], # Right Rectification rotation
+                                                    calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
+                                                    )"""
+                                                                                        
+
+                        left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
+                        left_cam_info['extrinsics']['translation'] = extrinsics[2]
     
         return 1, board_config
 

From 0fc2cb891c1c63a9af84720333aa42e7c8b6bccb Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 20:16:48 +0200
Subject: [PATCH 17/87] Invert move exit conditions

---
 calibration_utils.py | 166 ++++++++++++++++++++++---------------------
 1 file changed, 84 insertions(+), 82 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 7912cee..2efcb43 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -425,88 +425,90 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             left_cam_info = board_config['cameras'][camera]
             if str(left_cam_info["name"]) in self.disableCamera:
                 continue
-            if 'extrinsics' in left_cam_info:
-                if 'to_cam' in left_cam_info['extrinsics']:
-                    left_cam = camera
-                    right_cam = left_cam_info['extrinsics']['to_cam']
-                    left_path = filepath + '/' + left_cam_info['name']
-
-                    right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
-                    if str(right_cam_info["name"]) not in self.disableCamera:
-                        right_path = filepath + '/' + right_cam_info['name']
-                        print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
-                            left_cam_info['name'], right_cam_info['name']))
-
-                        specTranslation = left_cam_info['extrinsics']['specTranslation']
-                        rot = left_cam_info['extrinsics']['rotation']
-
-                        translation = np.array(
-                            [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-                        rotation = Rotation.from_euler(
-                            'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-                        if per_ccm and extrinsic_per_ccm:
-                            if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
-                                if left_cam_info["name"] in self.extrinsic_img:
-                                    array = self.extrinsic_img[left_cam_info["name"]]
-                                elif right_cam_info["name"] in self.extrinsic_img:
-                                    array = self.extrinsic_img[left_cam_info["name"]]
-                                left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-                                right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                                removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
-                                removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
-                        
-                        extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
-                        if extrinsics[0] == -1:
-                            return -1, extrinsics[1]
-
-                        if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
-                            board_config['stereo_config']['rectification_left'] = extrinsics[3]
-                            board_config['stereo_config']['rectification_right'] = extrinsics[4]
-
-                        elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-                            board_config['stereo_config']['rectification_left'] = extrinsics[4]
-                            board_config['stereo_config']['rectification_right'] = extrinsics[3]
-
-                        """ for stereoObj in board_config['stereo_config']:
-
-                            if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
-                                stereoObj['rectification_left'] = extrinsics[3]
-                                stereoObj['rectification_right'] = extrinsics[4] """
-
-                        print('<-------------Epipolar error of {} and {} ------------>'.format(
-                            left_cam_info['name'], right_cam_info['name']))
-                        #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
-                        #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
-                        if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
-                            scale = right_cam_info['intrinsics'][0][0]
-                        else:
-                            scale = left_cam_info['intrinsics'][0][0]
-                        if per_ccm and extrinsic_per_ccm:
-                            scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
-                            print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
-                            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-                            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
-                        else:
-                            print(f"Epipolar error {extrinsics[0]}")
-                            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-                            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
-                        """self.test_epipolar_charuco(left_cam_info['name'], 
-                                                    right_cam_info['name'],
-                                                    left_path, 
-                                                    right_path, 
-                                                    left_cam_info['intrinsics'], 
-                                                    left_cam_info['dist_coeff'], 
-                                                    right_cam_info['intrinsics'], 
-                                                    right_cam_info['dist_coeff'], 
-                                                    extrinsics[2], # Translation between left and right Cameras
-                                                    extrinsics[3], # Left Rectification rotation 
-                                                    extrinsics[4], # Right Rectification rotation
-                                                    calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
-                                                    )"""
-                                                                                        
-
-                        left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
-                        left_cam_info['extrinsics']['translation'] = extrinsics[2]
+            if not 'extrinsics' in left_cam_info:
+                continue
+            if not 'to_cam' in left_cam_info['extrinsics']:
+                continue
+            left_cam = camera
+            right_cam = left_cam_info['extrinsics']['to_cam']
+            left_path = filepath + '/' + left_cam_info['name']
+
+            right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
+            if str(right_cam_info["name"]) not in self.disableCamera:
+                right_path = filepath + '/' + right_cam_info['name']
+                print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
+                    left_cam_info['name'], right_cam_info['name']))
+
+                specTranslation = left_cam_info['extrinsics']['specTranslation']
+                rot = left_cam_info['extrinsics']['rotation']
+
+                translation = np.array(
+                    [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+                rotation = Rotation.from_euler(
+                    'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+                if per_ccm and extrinsic_per_ccm:
+                    if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
+                        if left_cam_info["name"] in self.extrinsic_img:
+                            array = self.extrinsic_img[left_cam_info["name"]]
+                        elif right_cam_info["name"] in self.extrinsic_img:
+                            array = self.extrinsic_img[left_cam_info["name"]]
+                        left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+                        right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+                        removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
+                        removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
+                
+                extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
+                if extrinsics[0] == -1:
+                    return -1, extrinsics[1]
+
+                if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
+                    board_config['stereo_config']['rectification_left'] = extrinsics[3]
+                    board_config['stereo_config']['rectification_right'] = extrinsics[4]
+
+                elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+                    board_config['stereo_config']['rectification_left'] = extrinsics[4]
+                    board_config['stereo_config']['rectification_right'] = extrinsics[3]
+
+                """ for stereoObj in board_config['stereo_config']:
+
+                    if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
+                        stereoObj['rectification_left'] = extrinsics[3]
+                        stereoObj['rectification_right'] = extrinsics[4] """
+
+                print('<-------------Epipolar error of {} and {} ------------>'.format(
+                    left_cam_info['name'], right_cam_info['name']))
+                #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
+                #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
+                if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
+                    scale = right_cam_info['intrinsics'][0][0]
+                else:
+                    scale = left_cam_info['intrinsics'][0][0]
+                if per_ccm and extrinsic_per_ccm:
+                    scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
+                    print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
+                    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
+                    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
+                else:
+                    print(f"Epipolar error {extrinsics[0]}")
+                    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
+                    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
+                """self.test_epipolar_charuco(left_cam_info['name'], 
+                                            right_cam_info['name'],
+                                            left_path, 
+                                            right_path, 
+                                            left_cam_info['intrinsics'], 
+                                            left_cam_info['dist_coeff'], 
+                                            right_cam_info['intrinsics'], 
+                                            right_cam_info['dist_coeff'], 
+                                            extrinsics[2], # Translation between left and right Cameras
+                                            extrinsics[3], # Left Rectification rotation 
+                                            extrinsics[4], # Right Rectification rotation
+                                            calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
+                                            )"""
+                                                                                    
+
+                left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
+                left_cam_info['extrinsics']['translation'] = extrinsics[2]
     
         return 1, board_config
 

From c908edd2e1d92429d07a4f3b8bb0c61a076b4a8a Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 20:20:47 +0200
Subject: [PATCH 18/87] Move stereo calibration into it's own function

---
 calibration_utils.py | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 2efcb43..19e8a8e 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -421,6 +421,11 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             cv2.waitKey(1)
             cv2.destroyAllWindows()
         
+        self.calibrate_stereo_pairs(filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features)
+    
+        return 1, board_config
+
+    def calibrate_stereo_pairs(self, filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features):
         for camera in board_config['cameras']:
             left_cam_info = board_config['cameras'][camera]
             if str(left_cam_info["name"]) in self.disableCamera:
@@ -429,6 +434,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 continue
             if not 'to_cam' in left_cam_info['extrinsics']:
                 continue
+
             left_cam = camera
             right_cam = left_cam_info['extrinsics']['to_cam']
             left_path = filepath + '/' + left_cam_info['name']
@@ -509,8 +515,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
                 left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
                 left_cam_info['extrinsics']['translation'] = extrinsics[2]
-    
-        return 1, board_config
 
     def getting_features(self, img_path, width, height, features = None, charucos=None):
         if charucos:

From 8334368a42cd085f699b0c28ae34c6e8a9c048a5 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 25 Jul 2024 20:27:57 +0200
Subject: [PATCH 19/87] Move intrinsic calibration back into class

---
 calibration_utils.py | 111 +++++++++++++++++++++----------------------
 1 file changed, 55 insertions(+), 56 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 19e8a8e..5a7f202 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -276,7 +276,7 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         filtered_ids = all_ids
     return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
 
-def calibrate_outside(args):
+def calibrate_ccm_intrinsics(args):
     calibration, features, images_path, cam_info, calib_model, distortion_model, charucos, _cameraModel, intrinsic_img, width, height = args
 
     if per_ccm:
@@ -310,60 +310,6 @@ def calibrate_outside(args):
     
     return cameraIntrinsics, distCoeff
 
-def calibrate_ccms(calibration, board_config, filepath, charucos, model, ccm_model, _cameraModel, intrinsic_img, features, squaresX, squaresY, squareSize, markerSize):
-    calibModels = {} # Still needs to be passed to stereo calibration
-    distortionModels = {} # Still needs to be passed to stereo calibration
-    allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
-    allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
-    combinedCoverageImage = None # TODO : It might not actually be combined in the calibration loop
-    resizeWidth, resizeHeight = 1280, 800
-    
-    activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in calibration.disableCamera]
-
-    for cam, cam_info in activeCameras:
-        images_path = filepath + '/' + cam_info['name']
-        image_files = glob.glob(images_path + "/*")
-        image_files.sort()
-        for im in image_files:
-            frame = cv2.imread(im)
-            height, width, _ = frame.shape
-            widthRatio = resizeWidth / width
-            heightRatio = resizeHeight / height
-            if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-                resizeWidth = width
-                resizeHeight = height
-            break
-        cam_info['width'] = width
-        cam_info['height'] = height
-
-        images_path = filepath + '/' + cam_info['name']
-        if "calib_model" in cam_info:
-            cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
-            if cameraModel_ccm == "fisheye":
-                model_ccm == None
-            calib_model = cameraModel_ccm
-            distortion_model = model_ccm
-        else:
-            calib_model = _cameraModel
-            if cam_info["name"] in ccm_model:
-                distortion_model = ccm_model[cam_info["name"]]
-            else:
-                distortion_model = model
-        calibModels[cam_info['name']] = calib_model
-        distortionModels[cam_info['name']] = distortion_model
-
-        img_path = glob.glob(images_path + "/*")
-        if charucos == {}:
-            img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
-        else:
-            img_path.sort()
-        cam_info["img_path"] = img_path
-
-    with ThreadPool(3) as pool:
-        result = pool.map(calibrate_outside, [[calibration, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height']] for _, cam_info in activeCameras])
-
-    return calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs
-
 class StereoCalibration(object):
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
@@ -414,7 +360,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         # parameters = aruco.DetectorParameters_create()
         assert mrk_size != None,  "ERROR: marker size not set"
 
-        calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs = calibrate_ccms(self, board_config, filepath, charucos, self.model, self.ccm_model, self._cameraModel, self.intrinsic_img, features, self.squaresX, self.squaresY, self.squareSize, self.markerSize)
+        calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs = self.get_features_and_calibrate_intrinsics(board_config, filepath, charucos, self.model, self.ccm_model, self._cameraModel, self.intrinsic_img, features)
 
         if enable_disp_rectify:
             # cv2.imshow('coverage image', combinedCoverageImage)
@@ -425,6 +371,59 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     
         return 1, board_config
 
+    def get_features_and_calibrate_intrinsics(self, board_config, filepath, charucos, model, ccm_model, _cameraModel, intrinsic_img, features):
+        calibModels = {} # Still needs to be passed to stereo calibration
+        distortionModels = {} # Still needs to be passed to stereo calibration
+        allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
+        allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
+        resizeWidth, resizeHeight = 1280, 800
+        
+        activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
+
+        for cam, cam_info in activeCameras:
+            images_path = filepath + '/' + cam_info['name']
+            image_files = glob.glob(images_path + "/*")
+            image_files.sort()
+            for im in image_files:
+                frame = cv2.imread(im)
+                height, width, _ = frame.shape
+                widthRatio = resizeWidth / width
+                heightRatio = resizeHeight / height
+                if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
+                    resizeWidth = width
+                    resizeHeight = height
+                break
+            cam_info['width'] = width
+            cam_info['height'] = height
+
+            images_path = filepath + '/' + cam_info['name']
+            if "calib_model" in cam_info:
+                cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
+                if cameraModel_ccm == "fisheye":
+                    model_ccm == None
+                calib_model = cameraModel_ccm
+                distortion_model = model_ccm
+            else:
+                calib_model = _cameraModel
+                if cam_info["name"] in ccm_model:
+                    distortion_model = ccm_model[cam_info["name"]]
+                else:
+                    distortion_model = model
+            calibModels[cam_info['name']] = calib_model
+            distortionModels[cam_info['name']] = distortion_model
+
+            img_path = glob.glob(images_path + "/*")
+            if charucos == {}:
+                img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+            else:
+                img_path.sort()
+            cam_info["img_path"] = img_path
+
+        with ThreadPool(3) as pool:
+            result = pool.map(calibrate_ccm_intrinsics, [[self, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height']] for _, cam_info in activeCameras])
+
+        return calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs
+
     def calibrate_stereo_pairs(self, filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features):
         for camera in board_config['cameras']:
             left_cam_info = board_config['cameras'][camera]

From f4f51c4493a0e95567193ef79f278e840333ad47 Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sat, 27 Jul 2024 18:45:48 +0200
Subject: [PATCH 20/87] Remove old unused functions

---
 calibration_utils.py | 99 --------------------------------------------
 1 file changed, 99 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 5a7f202..60c29dc 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -73,109 +73,12 @@ def board(self):
       self.__build()
     return self._board
 
-# Create the colormap using the dictionary
-GnRd = colors.LinearSegmentedColormap('GnRd', cdict)
-def get_quadrant_coordinates(width, height, nx, ny):
-    quadrant_width = width // nx
-    quadrant_height = height // ny
-    quadrant_coords = []
-    
-    for i in range(int(nx)):
-        for j in range(int(ny)):
-            left = i * quadrant_width
-            upper = j * quadrant_height
-            right = left + quadrant_width
-            bottom = upper + quadrant_height
-            quadrant_coords.append((left, upper, right, bottom))
-    
-    return quadrant_coords
-
-def sort_points_into_quadrants(points, width, height, error, nx = 4, ny = 4):
-    quadrant_coords = get_quadrant_coordinates(width, height, nx, ny)
-    quadrants = {i: [] for i in range(int(nx*ny))}  # Create a dictionary to store points by quadrant index
-
-    for x, y in points:
-        # Find the correct quadrant for each point
-        for index, (left, upper, right, bottom) in enumerate(quadrant_coords):
-            if left <= x < right and upper <= y < bottom:
-                quadrants[index].append(error[index])
-                break
-            
-    return quadrants, quadrant_coords
-
 def distance(point1, point2):
     return np.sqrt((point1[0] - point2[0])**2 + (point1[1] - point2[1])**2)
 # Creates a set of 13 polygon coordinates
 rectProjectionMode = 0
 
 colors = [(0, 255 , 0), (0, 0, 255)]
-def setPolygonCoordinates(height, width):
-    horizontal_shift = width//4
-    vertical_shift = height//4
-
-    margin = 60
-    slope = 150
-
-    p_coordinates = [
-        [[margin, margin], [margin, height-margin],
-            [width-margin, height-margin], [width-margin, margin]],
-
-        [[margin, 0], [margin, height], [width//2, height-slope], [width//2, slope]],
-        [[horizontal_shift, 0], [horizontal_shift, height], [
-            width//2 + horizontal_shift, height-slope], [width//2 + horizontal_shift, slope]],
-        [[horizontal_shift*2-margin, 0], [horizontal_shift*2-margin, height], [width//2 +
-                                                                               horizontal_shift*2-margin, height-slope], [width//2 + horizontal_shift*2-margin, slope]],
-
-        [[width-margin, 0], [width-margin, height],
-            [width//2, height-slope], [width//2, slope]],
-        [[width-horizontal_shift, 0], [width-horizontal_shift, height], [width //
-                                                                         2-horizontal_shift, height-slope], [width//2-horizontal_shift, slope]],
-        [[width-horizontal_shift*2+margin, 0], [width-horizontal_shift*2+margin, height], [width //
-                                                                                           2-horizontal_shift*2+margin, height-slope], [width//2-horizontal_shift*2+margin, slope]],
-
-        [[0, margin], [width, margin], [
-            width-slope, height//2], [slope, height//2]],
-        [[0, vertical_shift], [width, vertical_shift], [width-slope,
-                                                        height//2+vertical_shift], [slope, height//2+vertical_shift]],
-        [[0, vertical_shift*2-margin], [width, vertical_shift*2-margin], [width-slope,
-                                                                          height//2+vertical_shift*2-margin], [slope, height//2+vertical_shift*2-margin]],
-
-        [[0, height-margin], [width, height-margin],
-         [width-slope, height//2], [slope, height//2]],
-        [[0, height-vertical_shift], [width, height-vertical_shift], [width -
-                                                                      slope, height//2-vertical_shift], [slope, height//2-vertical_shift]],
-        [[0, height-vertical_shift*2+margin], [width, height-vertical_shift*2+margin], [width -
-                                                                                        slope, height//2-vertical_shift*2+margin], [slope, height//2-vertical_shift*2+margin]]
-    ]
-    return p_coordinates
-
-
-def getPolygonCoordinates(idx, p_coordinates):
-    return p_coordinates[idx]
-
-
-def getNumOfPolygons(p_coordinates):
-    return len(p_coordinates)
-
-# Filters polygons to just those at the given indexes.
-
-
-def select_polygon_coords(p_coordinates, indexes):
-    if indexes == None:
-        # The default
-        return p_coordinates
-    else:
-        print("Filtering polygons to those at indexes=", indexes)
-        return [p_coordinates[i] for i in indexes]
-
-
-def image_filename(polygon_index, total_num_of_captured_images):
-    return "p{polygon_index}_{total_num_of_captured_images}.png".format(polygon_index=polygon_index, total_num_of_captured_images=total_num_of_captured_images)
-
-
-def polygon_from_image_name(image_name):
-    """Returns the polygon index from an image name (ex: "left_p10_0.png" => 10)"""
-    return int(re.findall("p(\d+)", image_name)[0])
 
 class StereoExceptions(Exception):
     def __init__(self, message, stage, path=None, *args, **kwargs) -> None:
@@ -357,8 +260,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         self.cams = []
         features = None
-        # parameters = aruco.DetectorParameters_create()
-        assert mrk_size != None,  "ERROR: marker size not set"
 
         calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs = self.get_features_and_calibrate_intrinsics(board_config, filepath, charucos, self.model, self.ccm_model, self._cameraModel, self.intrinsic_img, features)
 

From 82aeb04519cce2a69f36dff80104cc550bf391df Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sat, 27 Jul 2024 19:40:41 +0200
Subject: [PATCH 21/87] Split calibration into data loading and calibration

---
 calibration_utils.py | 99 ++++++++++++++++++++++----------------------
 1 file changed, 50 insertions(+), 49 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 60c29dc..bcface5 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -179,9 +179,7 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         filtered_ids = all_ids
     return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
 
-def calibrate_ccm_intrinsics(args):
-    calibration, features, images_path, cam_info, calib_model, distortion_model, charucos, _cameraModel, intrinsic_img, width, height = args
-
+def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, calib_model, distortion_model, charucos, _cameraModel, intrinsic_img, width, height):
     if per_ccm:
         start = time.time()
         print('starting getting and filtering')
@@ -261,18 +259,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         self.cams = []
         features = None
 
-        calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs = self.get_features_and_calibrate_intrinsics(board_config, filepath, charucos, self.model, self.ccm_model, self._cameraModel, self.intrinsic_img, features)
-
-        if enable_disp_rectify:
-            # cv2.imshow('coverage image', combinedCoverageImage)
-            cv2.waitKey(1)
-            cv2.destroyAllWindows()
-        
-        self.calibrate_stereo_pairs(filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features)
-    
-        return 1, board_config
-
-    def get_features_and_calibrate_intrinsics(self, board_config, filepath, charucos, model, ccm_model, _cameraModel, intrinsic_img, features):
         calibModels = {} # Still needs to be passed to stereo calibration
         distortionModels = {} # Still needs to be passed to stereo calibration
         allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
@@ -282,48 +268,63 @@ def get_features_and_calibrate_intrinsics(self, board_config, filepath, charucos
         activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
 
         for cam, cam_info in activeCameras:
-            images_path = filepath + '/' + cam_info['name']
-            image_files = glob.glob(images_path + "/*")
-            image_files.sort()
-            for im in image_files:
-                frame = cv2.imread(im)
-                height, width, _ = frame.shape
-                widthRatio = resizeWidth / width
-                heightRatio = resizeHeight / height
-                if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-                    resizeWidth = width
-                    resizeHeight = height
-                break
+            width, height, img_path, calib_model, distortion_model, images_path = self.load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
             cam_info['width'] = width
             cam_info['height'] = height
-
-            images_path = filepath + '/' + cam_info['name']
-            if "calib_model" in cam_info:
-                cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
-                if cameraModel_ccm == "fisheye":
-                    model_ccm == None
-                calib_model = cameraModel_ccm
-                distortion_model = model_ccm
-            else:
-                calib_model = _cameraModel
-                if cam_info["name"] in ccm_model:
-                    distortion_model = ccm_model[cam_info["name"]]
-                else:
-                    distortion_model = model
             calibModels[cam_info['name']] = calib_model
             distortionModels[cam_info['name']] = distortion_model
+            cam_info["img_path"] = img_path
+            cam_info['images_path'] = images_path
 
-            img_path = glob.glob(images_path + "/*")
-            if charucos == {}:
-                img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+        for cam, cam_info in activeCameras:
+            cameraIntrinsics, distCoeff = calibrate_ccm_intrinsics(self, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], self._cameraModel, intrinsic_img, cam_info['width'], cam_info['height'])
+            allCameraIntrinsics[cam_info['name']] = cameraIntrinsics
+            allCameraDistCoeffs[cam_info['name']] = distCoeff
+
+        if enable_disp_rectify:
+            # cv2.imshow('coverage image', combinedCoverageImage)
+            cv2.waitKey(1)
+            cv2.destroyAllWindows()
+        
+        self.calibrate_stereo_pairs(filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features)
+    
+        return 1, board_config
+
+    def load_camera_data(self, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
+        images_path = filepath + '/' + cam_info['name']
+        image_files = glob.glob(images_path + "/*")
+        image_files.sort()
+        for im in image_files:
+            frame = cv2.imread(im)
+            height, width, _ = frame.shape
+            widthRatio = resizeWidth / width
+            heightRatio = resizeHeight / height
+            if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
+                resizeWidth = width
+                resizeHeight = height
+            break
+
+        images_path = filepath + '/' + cam_info['name']
+        if "calib_model" in cam_info:
+            cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
+            if cameraModel_ccm == "fisheye":
+                model_ccm == None
+            calib_model = cameraModel_ccm
+            distortion_model = model_ccm
+        else:
+            calib_model = _cameraModel
+            if cam_info["name"] in ccm_model:
+                distortion_model = ccm_model[cam_info["name"]]
             else:
-                img_path.sort()
-            cam_info["img_path"] = img_path
+                distortion_model = model
 
-        with ThreadPool(3) as pool:
-            result = pool.map(calibrate_ccm_intrinsics, [[self, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], _cameraModel, intrinsic_img, cam_info['width'], cam_info['height']] for _, cam_info in activeCameras])
+        img_path = glob.glob(images_path + "/*")
+        if charucos == {}:
+            img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+        else:
+            img_path.sort()
 
-        return calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs
+        return width, height, img_path, calib_model, distortion_model, images_path
 
     def calibrate_stereo_pairs(self, filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features):
         for camera in board_config['cameras']:

From 2459632ac3b236734f64d3473ac74df1ab0c98bd Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sat, 27 Jul 2024 19:52:31 +0200
Subject: [PATCH 22/87] Change stereo calibration to calibrate one pair at a
 time

---
 calibration_utils.py | 189 +++++++++++++++++++++----------------------
 1 file changed, 92 insertions(+), 97 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index bcface5..4a66b1a 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -281,13 +281,20 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             allCameraIntrinsics[cam_info['name']] = cameraIntrinsics
             allCameraDistCoeffs[cam_info['name']] = distCoeff
 
-        if enable_disp_rectify:
-            # cv2.imshow('coverage image', combinedCoverageImage)
-            cv2.waitKey(1)
-            cv2.destroyAllWindows()
-        
-        self.calibrate_stereo_pairs(filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features)
-    
+        stereoPairs = []
+        for camera in board_config['cameras']:
+            left_cam_info = board_config['cameras'][camera]
+            if str(left_cam_info["name"]) in self.disableCamera:
+                continue
+            if not 'extrinsics' in left_cam_info:
+                continue
+            if not 'to_cam' in left_cam_info['extrinsics']:
+                continue
+            stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
+
+        for left, right in stereoPairs:
+            self.calibrate_stereo_pair(left, right, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features)
+
         return 1, board_config
 
     def load_camera_data(self, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
@@ -326,96 +333,84 @@ def load_camera_data(self, filepath, cam_info, _cameraModel, ccm_model, model, c
 
         return width, height, img_path, calib_model, distortion_model, images_path
 
-    def calibrate_stereo_pairs(self, filepath, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features):
-        for camera in board_config['cameras']:
-            left_cam_info = board_config['cameras'][camera]
-            if str(left_cam_info["name"]) in self.disableCamera:
-                continue
-            if not 'extrinsics' in left_cam_info:
-                continue
-            if not 'to_cam' in left_cam_info['extrinsics']:
-                continue
-
-            left_cam = camera
-            right_cam = left_cam_info['extrinsics']['to_cam']
-            left_path = filepath + '/' + left_cam_info['name']
-
-            right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
-            if str(right_cam_info["name"]) not in self.disableCamera:
-                right_path = filepath + '/' + right_cam_info['name']
-                print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
-                    left_cam_info['name'], right_cam_info['name']))
-
-                specTranslation = left_cam_info['extrinsics']['specTranslation']
-                rot = left_cam_info['extrinsics']['rotation']
-
-                translation = np.array(
-                    [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-                rotation = Rotation.from_euler(
-                    'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-                if per_ccm and extrinsic_per_ccm:
-                    if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
-                        if left_cam_info["name"] in self.extrinsic_img:
-                            array = self.extrinsic_img[left_cam_info["name"]]
-                        elif right_cam_info["name"] in self.extrinsic_img:
-                            array = self.extrinsic_img[left_cam_info["name"]]
-                        left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-                        right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                        removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
-                        removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
-                
-                extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
-                if extrinsics[0] == -1:
-                    return -1, extrinsics[1]
-
-                if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
-                    board_config['stereo_config']['rectification_left'] = extrinsics[3]
-                    board_config['stereo_config']['rectification_right'] = extrinsics[4]
-
-                elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-                    board_config['stereo_config']['rectification_left'] = extrinsics[4]
-                    board_config['stereo_config']['rectification_right'] = extrinsics[3]
-
-                """ for stereoObj in board_config['stereo_config']:
-
-                    if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
-                        stereoObj['rectification_left'] = extrinsics[3]
-                        stereoObj['rectification_right'] = extrinsics[4] """
-
-                print('<-------------Epipolar error of {} and {} ------------>'.format(
-                    left_cam_info['name'], right_cam_info['name']))
-                #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
-                #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
-                if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
-                    scale = right_cam_info['intrinsics'][0][0]
-                else:
-                    scale = left_cam_info['intrinsics'][0][0]
-                if per_ccm and extrinsic_per_ccm:
-                    scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
-                    print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
-                    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-                    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
-                else:
-                    print(f"Epipolar error {extrinsics[0]}")
-                    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-                    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
-                """self.test_epipolar_charuco(left_cam_info['name'], 
-                                            right_cam_info['name'],
-                                            left_path, 
-                                            right_path, 
-                                            left_cam_info['intrinsics'], 
-                                            left_cam_info['dist_coeff'], 
-                                            right_cam_info['intrinsics'], 
-                                            right_cam_info['dist_coeff'], 
-                                            extrinsics[2], # Translation between left and right Cameras
-                                            extrinsics[3], # Left Rectification rotation 
-                                            extrinsics[4], # Right Rectification rotation
-                                            calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
-                                            )"""
-                                                                                    
-
-                left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
-                left_cam_info['extrinsics']['translation'] = extrinsics[2]
+    def calibrate_stereo_pair(self, left_cam, right_cam, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features):
+        left_cam_info = board_config['cameras'][left_cam]
+
+        right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
+        if str(right_cam_info["name"]) not in self.disableCamera:
+            print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
+                left_cam_info['name'], right_cam_info['name']))
+
+            specTranslation = left_cam_info['extrinsics']['specTranslation']
+            rot = left_cam_info['extrinsics']['rotation']
+
+            translation = np.array(
+                [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+            rotation = Rotation.from_euler(
+                'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+            if per_ccm and extrinsic_per_ccm:
+                if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
+                    if left_cam_info["name"] in self.extrinsic_img:
+                        array = self.extrinsic_img[left_cam_info["name"]]
+                    elif right_cam_info["name"] in self.extrinsic_img:
+                        array = self.extrinsic_img[left_cam_info["name"]]
+                    left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+                    right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
+                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
+            
+            extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
+            if extrinsics[0] == -1:
+                return -1, extrinsics[1]
+
+            if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
+                board_config['stereo_config']['rectification_left'] = extrinsics[3]
+                board_config['stereo_config']['rectification_right'] = extrinsics[4]
+
+            elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+                board_config['stereo_config']['rectification_left'] = extrinsics[4]
+                board_config['stereo_config']['rectification_right'] = extrinsics[3]
+
+            """ for stereoObj in board_config['stereo_config']:
+
+                if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
+                    stereoObj['rectification_left'] = extrinsics[3]
+                    stereoObj['rectification_right'] = extrinsics[4] """
+
+            print('<-------------Epipolar error of {} and {} ------------>'.format(
+                left_cam_info['name'], right_cam_info['name']))
+            #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
+            #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
+            if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
+                scale = right_cam_info['intrinsics'][0][0]
+            else:
+                scale = left_cam_info['intrinsics'][0][0]
+            if per_ccm and extrinsic_per_ccm:
+                scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
+                print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
+                left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
+                left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
+            else:
+                print(f"Epipolar error {extrinsics[0]}")
+                left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
+                left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
+            """self.test_epipolar_charuco(left_cam_info['name'], 
+                                        right_cam_info['name'],
+                                        left_path, 
+                                        right_path, 
+                                        left_cam_info['intrinsics'], 
+                                        left_cam_info['dist_coeff'], 
+                                        right_cam_info['intrinsics'], 
+                                        right_cam_info['dist_coeff'], 
+                                        extrinsics[2], # Translation between left and right Cameras
+                                        extrinsics[3], # Left Rectification rotation 
+                                        extrinsics[4], # Right Rectification rotation
+                                        calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
+                                        )"""
+                                                                                
+
+            left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
+            left_cam_info['extrinsics']['translation'] = extrinsics[2]
 
     def getting_features(self, img_path, width, height, features = None, charucos=None):
         if charucos:

From a699a0ab9a273240f22da3a9bbb2919f3ead0b5c Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sat, 27 Jul 2024 20:03:44 +0200
Subject: [PATCH 23/87] Pass more variables via cam_info

---
 calibration_utils.py | 35 +++++++++++++++--------------------
 1 file changed, 15 insertions(+), 20 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 4a66b1a..9f7d2fb 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -179,11 +179,11 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         filtered_ids = all_ids
     return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
 
-def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, calib_model, distortion_model, charucos, _cameraModel, intrinsic_img, width, height):
+def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, charucos, _cameraModel, intrinsic_img):
     if per_ccm:
         start = time.time()
         print('starting getting and filtering')
-        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, images_path, width, height, features, charucos, cam_info, intrinsic_img, _cameraModel, distortion_model)
+        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, images_path, cam_info['width'], cam_info['height'], features, charucos, cam_info, intrinsic_img, _cameraModel, cam_info['distortion_model'])
         
         print(f'getting and filtering took {round(time.time() - start, 2)}s')
         
@@ -192,13 +192,13 @@ def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, calib
 
         start = time.time()
         print('starting calibrate_wf')
-        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, calib_model, distortion_model, cameraIntrinsics, distCoeff)
+        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, cam_info['calib_model'], cam_info['distortion_model'], cameraIntrinsics, distCoeff)
         if isinstance(ret, str) and all_ids is None:
             raise RuntimeError('Exception' + ret) # TODO : Handle
         print(f'calibrate_wf took {round(time.time() - start, 2)}s')
     else:
         ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
-            images_path, cam_info['hfov'], cam_info["name"], charucos, width, height, calib_model, distortion_model)
+            images_path, cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
         cam_info['filtered_ids'] = filtered_ids
         cam_info['filtered_corners'] = filtered_corners
 
@@ -208,8 +208,6 @@ def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, calib
     cam_info['reprojection_error'] = ret
     print("Reprojection error of {0}: {1}".format(
         cam_info['name'], ret))
-    
-    return cameraIntrinsics, distCoeff
 
 class StereoCalibration(object):
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
@@ -259,10 +257,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         self.cams = []
         features = None
 
-        calibModels = {} # Still needs to be passed to stereo calibration
-        distortionModels = {} # Still needs to be passed to stereo calibration
-        allCameraIntrinsics = {} # Still needs to be passed to stereo calibration
-        allCameraDistCoeffs = {} # Still needs to be passed to stereo calibration
         resizeWidth, resizeHeight = 1280, 800
         
         activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
@@ -271,15 +265,13 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             width, height, img_path, calib_model, distortion_model, images_path = self.load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
             cam_info['width'] = width
             cam_info['height'] = height
-            calibModels[cam_info['name']] = calib_model
-            distortionModels[cam_info['name']] = distortion_model
+            cam_info['calib_model'] = calib_model
+            cam_info['distortion_model'] = distortion_model
             cam_info["img_path"] = img_path
             cam_info['images_path'] = images_path
 
         for cam, cam_info in activeCameras:
-            cameraIntrinsics, distCoeff = calibrate_ccm_intrinsics(self, features, images_path, cam_info, calibModels[cam_info['name']], distortionModels[cam_info['name']], charucos[cam_info['name']], self._cameraModel, intrinsic_img, cam_info['width'], cam_info['height'])
-            allCameraIntrinsics[cam_info['name']] = cameraIntrinsics
-            allCameraDistCoeffs[cam_info['name']] = distCoeff
+            calibrate_ccm_intrinsics(self, features, images_path, cam_info, charucos[cam_info['name']], self._cameraModel, intrinsic_img)
 
         stereoPairs = []
         for camera in board_config['cameras']:
@@ -293,7 +285,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
 
         for left, right in stereoPairs:
-            self.calibrate_stereo_pair(left, right, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features)
+            self.calibrate_stereo_pair(left, right, board_config, features)
 
         return 1, board_config
 
@@ -333,7 +325,7 @@ def load_camera_data(self, filepath, cam_info, _cameraModel, ccm_model, model, c
 
         return width, height, img_path, calib_model, distortion_model, images_path
 
-    def calibrate_stereo_pair(self, left_cam, right_cam, board_config, calibModels, distortionModels, allCameraIntrinsics, allCameraDistCoeffs, features):
+    def calibrate_stereo_pair(self, left_cam, right_cam, board_config, features):
         left_cam_info = board_config['cameras'][left_cam]
 
         right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
@@ -354,12 +346,15 @@ def calibrate_stereo_pair(self, left_cam, right_cam, board_config, calibModels,
                         array = self.extrinsic_img[left_cam_info["name"]]
                     elif right_cam_info["name"] in self.extrinsic_img:
                         array = self.extrinsic_img[left_cam_info["name"]]
+                        
+                        
+                        
                     left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
                     right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']],  distortionModels[left_cam_info['name']])
-                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], allCameraIntrinsics[left_cam_info['name']], allCameraDistCoeffs[left_cam_info['name']], distortionModels[right_cam_info['name']])
+                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
+                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
             
-            extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, calibModels[left_cam_info['name']], calibModels[right_cam_info['name']], distortionModels[left_cam_info['name']], distortionModels[right_cam_info['name']], features)
+            extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
             if extrinsics[0] == -1:
                 return -1, extrinsics[1]
 

From 9c6496b6302c1f6330ed0b8217dd165d69414f38 Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sat, 27 Jul 2024 20:10:42 +0200
Subject: [PATCH 24/87] Move right cam disable check out of
 calibrate_stereo_pair

---
 calibration_utils.py | 158 +++++++++++++++++++++----------------------
 1 file changed, 79 insertions(+), 79 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 9f7d2fb..26749ee 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -274,7 +274,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             calibrate_ccm_intrinsics(self, features, images_path, cam_info, charucos[cam_info['name']], self._cameraModel, intrinsic_img)
 
         stereoPairs = []
-        for camera in board_config['cameras']:
+        for camera, _ in activeCameras:
             left_cam_info = board_config['cameras'][camera]
             if str(left_cam_info["name"]) in self.disableCamera:
                 continue
@@ -282,6 +282,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 continue
             if not 'to_cam' in left_cam_info['extrinsics']:
                 continue
+            if str(board_config['cameras'][left_cam_info['extrinsics']['to_cam']]['name']) in self.disableCamera:
+                continue
             stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
 
         for left, right in stereoPairs:
@@ -327,85 +329,83 @@ def load_camera_data(self, filepath, cam_info, _cameraModel, ccm_model, model, c
 
     def calibrate_stereo_pair(self, left_cam, right_cam, board_config, features):
         left_cam_info = board_config['cameras'][left_cam]
-
         right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
-        if str(right_cam_info["name"]) not in self.disableCamera:
-            print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
-                left_cam_info['name'], right_cam_info['name']))
-
-            specTranslation = left_cam_info['extrinsics']['specTranslation']
-            rot = left_cam_info['extrinsics']['rotation']
-
-            translation = np.array(
-                [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-            rotation = Rotation.from_euler(
-                'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-            if per_ccm and extrinsic_per_ccm:
-                if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
-                    if left_cam_info["name"] in self.extrinsic_img:
-                        array = self.extrinsic_img[left_cam_info["name"]]
-                    elif right_cam_info["name"] in self.extrinsic_img:
-                        array = self.extrinsic_img[left_cam_info["name"]]
-                        
-                        
-                        
-                    left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-                    right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
-                    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
-            
-            extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
-            if extrinsics[0] == -1:
-                return -1, extrinsics[1]
-
-            if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
-                board_config['stereo_config']['rectification_left'] = extrinsics[3]
-                board_config['stereo_config']['rectification_right'] = extrinsics[4]
-
-            elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-                board_config['stereo_config']['rectification_left'] = extrinsics[4]
-                board_config['stereo_config']['rectification_right'] = extrinsics[3]
-
-            """ for stereoObj in board_config['stereo_config']:
-
-                if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
-                    stereoObj['rectification_left'] = extrinsics[3]
-                    stereoObj['rectification_right'] = extrinsics[4] """
-
-            print('<-------------Epipolar error of {} and {} ------------>'.format(
-                left_cam_info['name'], right_cam_info['name']))
-            #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
-            #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
-            if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
-                scale = right_cam_info['intrinsics'][0][0]
-            else:
-                scale = left_cam_info['intrinsics'][0][0]
-            if per_ccm and extrinsic_per_ccm:
-                scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
-                print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
-                left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-                left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
-            else:
-                print(f"Epipolar error {extrinsics[0]}")
-                left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-                left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
-            """self.test_epipolar_charuco(left_cam_info['name'], 
-                                        right_cam_info['name'],
-                                        left_path, 
-                                        right_path, 
-                                        left_cam_info['intrinsics'], 
-                                        left_cam_info['dist_coeff'], 
-                                        right_cam_info['intrinsics'], 
-                                        right_cam_info['dist_coeff'], 
-                                        extrinsics[2], # Translation between left and right Cameras
-                                        extrinsics[3], # Left Rectification rotation 
-                                        extrinsics[4], # Right Rectification rotation
-                                        calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
-                                        )"""
-                                                                                
-
-            left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
-            left_cam_info['extrinsics']['translation'] = extrinsics[2]
+        print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
+            left_cam_info['name'], right_cam_info['name']))
+
+        specTranslation = left_cam_info['extrinsics']['specTranslation']
+        rot = left_cam_info['extrinsics']['rotation']
+
+        translation = np.array(
+            [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+        rotation = Rotation.from_euler(
+            'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+        if per_ccm and extrinsic_per_ccm:
+            if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
+                if left_cam_info["name"] in self.extrinsic_img:
+                    array = self.extrinsic_img[left_cam_info["name"]]
+                elif right_cam_info["name"] in self.extrinsic_img:
+                    array = self.extrinsic_img[left_cam_info["name"]]
+                    
+                    
+                    
+                left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+                right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+                removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
+                removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
+        
+        extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
+        if extrinsics[0] == -1:
+            return -1, extrinsics[1]
+
+        if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
+            board_config['stereo_config']['rectification_left'] = extrinsics[3]
+            board_config['stereo_config']['rectification_right'] = extrinsics[4]
+
+        elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+            board_config['stereo_config']['rectification_left'] = extrinsics[4]
+            board_config['stereo_config']['rectification_right'] = extrinsics[3]
+
+        """ for stereoObj in board_config['stereo_config']:
+
+            if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
+                stereoObj['rectification_left'] = extrinsics[3]
+                stereoObj['rectification_right'] = extrinsics[4] """
+
+        print('<-------------Epipolar error of {} and {} ------------>'.format(
+            left_cam_info['name'], right_cam_info['name']))
+        #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
+        #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
+        if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
+            scale = right_cam_info['intrinsics'][0][0]
+        else:
+            scale = left_cam_info['intrinsics'][0][0]
+        if per_ccm and extrinsic_per_ccm:
+            scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
+            print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
+            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
+            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
+        else:
+            print(f"Epipolar error {extrinsics[0]}")
+            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
+            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
+        """self.test_epipolar_charuco(left_cam_info['name'], 
+                                    right_cam_info['name'],
+                                    left_path, 
+                                    right_path, 
+                                    left_cam_info['intrinsics'], 
+                                    left_cam_info['dist_coeff'], 
+                                    right_cam_info['intrinsics'], 
+                                    right_cam_info['dist_coeff'], 
+                                    extrinsics[2], # Translation between left and right Cameras
+                                    extrinsics[3], # Left Rectification rotation 
+                                    extrinsics[4], # Right Rectification rotation
+                                    calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
+                                    )"""
+                                                                            
+
+        left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
+        left_cam_info['extrinsics']['translation'] = extrinsics[2]
 
     def getting_features(self, img_path, width, height, features = None, charucos=None):
         if charucos:

From 1a373fc911abf6e184fc6c20ba54d24c4e3f7f95 Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sat, 27 Jul 2024 22:09:01 +0200
Subject: [PATCH 25/87] Move main calibration functions out of class members

---
 calibration_utils.py | 39 +++++++++++++++++++++++----------------
 1 file changed, 23 insertions(+), 16 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 26749ee..81bb1ff 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -179,11 +179,11 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         filtered_ids = all_ids
     return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
 
-def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, charucos, _cameraModel, intrinsic_img):
+def calibrate_ccm_intrinsics(calibration, features, cam_info, charucos, _cameraModel, intrinsic_img):
     if per_ccm:
         start = time.time()
         print('starting getting and filtering')
-        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, images_path, cam_info['width'], cam_info['height'], features, charucos, cam_info, intrinsic_img, _cameraModel, cam_info['distortion_model'])
+        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos, cam_info, intrinsic_img, _cameraModel, cam_info['distortion_model'])
         
         print(f'getting and filtering took {round(time.time() - start, 2)}s')
         
@@ -198,7 +198,7 @@ def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, charu
         print(f'calibrate_wf took {round(time.time() - start, 2)}s')
     else:
         ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
-            images_path, cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
+            cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
         cam_info['filtered_ids'] = filtered_ids
         cam_info['filtered_corners'] = filtered_corners
 
@@ -209,6 +209,19 @@ def calibrate_ccm_intrinsics(calibration, features, images_path, cam_info, charu
     print("Reprojection error of {0}: {1}".format(
         cam_info['name'], ret))
 
+def calibrate_stereo_pair(calibration, left, right, board_config, features):
+    calibration.calibrate_stereo_pair(left, right, board_config, features)
+
+def load_camera_data(calibration, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
+    width, height, img_path, calib_model, distortion_model, images_path = calibration.load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight)
+    cam_info['width'] = width
+    cam_info['height'] = height
+    cam_info['calib_model'] = calib_model
+    cam_info['distortion_model'] = distortion_model
+    cam_info["img_path"] = img_path
+    cam_info['images_path'] = images_path
+    return cam_info
+
 class StereoCalibration(object):
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
@@ -261,18 +274,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         
         activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
 
-        for cam, cam_info in activeCameras:
-            width, height, img_path, calib_model, distortion_model, images_path = self.load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
-            cam_info['width'] = width
-            cam_info['height'] = height
-            cam_info['calib_model'] = calib_model
-            cam_info['distortion_model'] = distortion_model
-            cam_info["img_path"] = img_path
-            cam_info['images_path'] = images_path
-
-        for cam, cam_info in activeCameras:
-            calibrate_ccm_intrinsics(self, features, images_path, cam_info, charucos[cam_info['name']], self._cameraModel, intrinsic_img)
-
         stereoPairs = []
         for camera, _ in activeCameras:
             left_cam_info = board_config['cameras'][camera]
@@ -286,8 +287,14 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 continue
             stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
 
+        for cam, cam_info in activeCameras:
+            cam_info = load_camera_data(self, filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
+
+        for cam, cam_info in activeCameras:
+            calibrate_ccm_intrinsics(self, features, cam_info, charucos[cam_info['name']], self._cameraModel, intrinsic_img)
+
         for left, right in stereoPairs:
-            self.calibrate_stereo_pair(left, right, board_config, features)
+            calibrate_stereo_pair(self, left, right, board_config, features)
 
         return 1, board_config
 

From e7e94789c83d0e03fcd08eb705e4ffab1919fa40 Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sun, 28 Jul 2024 00:45:39 +0200
Subject: [PATCH 26/87] Split calibrate_ccm_intrinsics into normal and per_ccm
 version

---
 calibration_utils.py | 63 +++++++++++++++++++++++++++-----------------
 1 file changed, 39 insertions(+), 24 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 81bb1ff..d94c36d 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -179,28 +179,37 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
         filtered_ids = all_ids
     return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
 
-def calibrate_ccm_intrinsics(calibration, features, cam_info, charucos, _cameraModel, intrinsic_img):
-    if per_ccm:
-        start = time.time()
-        print('starting getting and filtering')
-        filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos, cam_info, intrinsic_img, _cameraModel, cam_info['distortion_model'])
-        
-        print(f'getting and filtering took {round(time.time() - start, 2)}s')
-        
-        cam_info['filtered_ids'] = filtered_ids
-        cam_info['filtered_corners'] = filtered_features
-
-        start = time.time()
-        print('starting calibrate_wf')
-        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, cam_info['calib_model'], cam_info['distortion_model'], cameraIntrinsics, distCoeff)
-        if isinstance(ret, str) and all_ids is None:
-            raise RuntimeError('Exception' + ret) # TODO : Handle
-        print(f'calibrate_wf took {round(time.time() - start, 2)}s')
-    else:
-        ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
-            cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
-        cam_info['filtered_ids'] = filtered_ids
-        cam_info['filtered_corners'] = filtered_corners
+def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, charucos, _cameraModel, intrinsic_img):
+    start = time.time()
+    print('starting getting and filtering')
+    filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos, cam_info, intrinsic_img, _cameraModel, cam_info['distortion_model'])
+    
+    print(f'getting and filtering took {round(time.time() - start, 2)}s')
+    
+    cam_info['filtered_ids'] = filtered_ids
+    cam_info['filtered_corners'] = filtered_features
+
+    start = time.time()
+    print('starting calibrate_wf')
+    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, cam_info['calib_model'], cam_info['distortion_model'], cameraIntrinsics, distCoeff)
+    if isinstance(ret, str) and all_ids is None:
+        raise RuntimeError('Exception' + ret) # TODO : Handle
+    print(f'calibrate_wf took {round(time.time() - start, 2)}s')
+
+    cam_info['intrinsics'] = cameraIntrinsics
+    cam_info['dist_coeff'] = distCoeff
+    cam_info['size'] = size # (Width, height)
+    cam_info['reprojection_error'] = ret
+    print("Reprojection error of {0}: {1}".format(
+        cam_info['name'], ret))
+
+    return cam_info
+
+def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
+    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
+        cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
+    cam_info['filtered_ids'] = filtered_ids
+    cam_info['filtered_corners'] = filtered_corners
 
     cam_info['intrinsics'] = cameraIntrinsics
     cam_info['dist_coeff'] = distCoeff
@@ -209,6 +218,8 @@ def calibrate_ccm_intrinsics(calibration, features, cam_info, charucos, _cameraM
     print("Reprojection error of {0}: {1}".format(
         cam_info['name'], ret))
 
+    return cam_info
+
 def calibrate_stereo_pair(calibration, left, right, board_config, features):
     calibration.calibrate_stereo_pair(left, right, board_config, features)
 
@@ -290,8 +301,12 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         for cam, cam_info in activeCameras:
             cam_info = load_camera_data(self, filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
 
-        for cam, cam_info in activeCameras:
-            calibrate_ccm_intrinsics(self, features, cam_info, charucos[cam_info['name']], self._cameraModel, intrinsic_img)
+        if per_ccm:
+            for cam, cam_info in activeCameras:
+                cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info, charucos[cam_info['name']], self._cameraModel, intrinsic_img)
+        else:
+            for cam, cam_info in activeCameras:
+                cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
 
         for left, right in stereoPairs:
             calibrate_stereo_pair(self, left, right, board_config, features)

From 3ece2f0c9c6844b5fd5ee8a024a6fd10ece82635 Mon Sep 17 00:00:00 2001
From: Tommy <tomprochazka0@gmail.com>
Date: Sun, 28 Jul 2024 01:01:07 +0200
Subject: [PATCH 27/87] Move filtering features out of calibrate ccm function

---
 calibration_utils.py | 34 ++++++++++++++--------------------
 1 file changed, 14 insertions(+), 20 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index d94c36d..7092ad8 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -145,6 +145,8 @@ def estimate_pose_and_filter_single(args):
         return valid_ids.reshape(-1, 1).astype(np.int32), corners2[valid_mask].reshape(-1, 1, 2), corners2[removed_mask]
 
 def get_and_filter_features(calibration, images_path, width, height, features, charucos, cam_info, intrinsic_img, _cameraModel, distortion_model):
+    start = time.time()
+    print('starting getting and filtering')
     all_features, all_ids, imsize = calibration.getting_features(images_path, width, height, features=features, charucos=charucos)
 
     if isinstance(all_features, str) and all_ids is None:
@@ -177,21 +179,19 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
     else:
         filtered_features = all_features
         filtered_ids = all_ids
-    return filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff
-
-def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, charucos, _cameraModel, intrinsic_img):
-    start = time.time()
-    print('starting getting and filtering')
-    filtered_features, filtered_ids, all_features, all_ids, filtered_images, cameraIntrinsics, distCoeff = get_and_filter_features(calibration, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos, cam_info, intrinsic_img, _cameraModel, cam_info['distortion_model'])
-    
     print(f'getting and filtering took {round(time.time() - start, 2)}s')
     
     cam_info['filtered_ids'] = filtered_ids
     cam_info['filtered_corners'] = filtered_features
+    cam_info['intrinsics'] = cameraIntrinsics
+    cam_info['dist_coeff'] = distCoeff
+
+    return cam_info
 
+def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info):
     start = time.time()
     print('starting calibrate_wf')
-    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], all_features, all_ids, filtered_features, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, filtered_images, charucos, cam_info['calib_model'], cam_info['distortion_model'], cameraIntrinsics, distCoeff)
+    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], cam_info['filtered_corners'], cam_info['filtered_ids'], cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
     if isinstance(ret, str) and all_ids is None:
         raise RuntimeError('Exception' + ret) # TODO : Handle
     print(f'calibrate_wf took {round(time.time() - start, 2)}s')
@@ -303,7 +303,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         if per_ccm:
             for cam, cam_info in activeCameras:
-                cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info, charucos[cam_info['name']], self._cameraModel, intrinsic_img)
+                cam_info = get_and_filter_features(self, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos[cam_info['name']], cam_info, intrinsic_img, self._cameraModel, cam_info['distortion_model'])
+                cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info)
         else:
             for cam, cam_info in activeCameras:
                 cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
@@ -600,9 +601,7 @@ def is_binary_string(s: str) -> bool:
             flags = distortionModel
         return flags
 
-    def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, features, image_files, charucos, calib_model, distortionModel, cameraIntrinsics, distCoeff):
-        image_files = glob.glob(image_files + "/*")
-        image_files.sort()
+    def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
         coverageImage = np.ones(imsize[::-1], np.uint8) * 255
         coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
         coverageImage = self.draw_corners(allCorners, coverageImage)
@@ -610,10 +609,7 @@ def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorne
             if features == None or features == "charucos":
                 distortion_flags = self.get_distortion_flags(distortionModel)
                 ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
-                    all_Features, all_features_Ids,allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
-            if charucos == {}:
-                self.undistort_visualization(
-                    image_files, cameraIntrinsics, distCoeff, imsize, name)
+                    allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
 
                 return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
             else:
@@ -624,8 +620,6 @@ def calibrate_wf_intrinsics(self, name, all_Features, all_features_Ids, allCorne
                 print('Fisheye--------------------------------------------------')
                 ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = self.calibrate_fisheye(
                     allCorners, allIds, imsize, hfov, name)
-                self.undistort_visualization(
-                        image_files, camera_matrix, distortion_coefficients, imsize, name)
                 print('Fisheye rotation vector', rotation_vectors[0])
                 print('Fisheye translation vector', translation_vectors[0])
 
@@ -845,7 +839,7 @@ def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height,
         coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
         coverageImage = self.draw_corners(allCorners, coverageImage)
         if calib_model == 'perspective':
-            distortion_flags = self.get_distortion_flags(distortionModel)
+            distortion_flags = self.get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
             ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
                 allCorners, allIds, imsize, hfov, name, distortion_flags)
             self.undistort_visualization(
@@ -927,7 +921,7 @@ def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_c
         return corners_removed, allIds, allCorners
 
 
-    def calibrate_camera_charuco(self, all_Features, all_features_Ids, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
+    def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
         """
         Calibrates the camera using the dected corners.
         """

From a8bc6b31101ae196e4cf02c45f2a475bef5d10e2 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 29 Jul 2024 09:06:25 +0200
Subject: [PATCH 28/87] Split getting and filtering features

---
 calibration_utils.py | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 7092ad8..f5de1c8 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -144,19 +144,20 @@ def estimate_pose_and_filter_single(args):
         #removed_corners.extend(corners2[removed_mask])
         return valid_ids.reshape(-1, 1).astype(np.int32), corners2[valid_mask].reshape(-1, 1, 2), corners2[removed_mask]
 
-def get_and_filter_features(calibration, images_path, width, height, features, charucos, cam_info, intrinsic_img, _cameraModel, distortion_model):
-    start = time.time()
-    print('starting getting and filtering')
+def get_features(calibration, images_path, width, height, features, charucos, cam_info):
     all_features, all_ids, imsize = calibration.getting_features(images_path, width, height, features=features, charucos=charucos)
 
     if isinstance(all_features, str) and all_ids is None:
         raise RuntimeError(f'Exception {all_features}') # TODO : Handle
     cam_info["imsize"] = imsize
+    
+    return cam_info, all_features, all_ids
 
-    f = imsize[0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+def filter_features(calibration, images_path, cam_info, intrinsic_img, _cameraModel, distortion_model, all_features, all_ids):
+    f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
     print("INTRINSIC CALIBRATION")
-    cameraIntrinsics = np.array([[f,    0.0,      imsize[0]/2],
-                                 [0.0,     f,      imsize[1]/2],
+    cameraIntrinsics = np.array([[f,    0.0,      cam_info['imsize'][0]/2],
+                                 [0.0,     f,      cam_info['imsize'][1]/2],
                                  [0.0,   0.0,        1.0]])
 
     distCoeff = np.zeros((12, 1))
@@ -170,7 +171,7 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
     current_time = time.time()
     if _cameraModel != "fisheye":
         print("Filtering corners")
-        removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"],imsize,cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
+        removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"], cam_info['imsize'], cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
 
         if filtered_features is None:
             raise RuntimeError('Exception') # TODO : Handle
@@ -179,7 +180,6 @@ def get_and_filter_features(calibration, images_path, width, height, features, c
     else:
         filtered_features = all_features
         filtered_ids = all_ids
-    print(f'getting and filtering took {round(time.time() - start, 2)}s')
     
     cam_info['filtered_ids'] = filtered_ids
     cam_info['filtered_corners'] = filtered_features
@@ -303,7 +303,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         if per_ccm:
             for cam, cam_info in activeCameras:
-                cam_info = get_and_filter_features(self, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos[cam_info['name']], cam_info, intrinsic_img, self._cameraModel, cam_info['distortion_model'])
+                cam_info, all_features, all_ids = get_features(self, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos[cam_info['name']], cam_info)
+                cam_info = filter_features(self, cam_info['images_path'], cam_info, intrinsic_img, self._cameraModel, cam_info['distortion_model'], all_features, all_ids)
                 cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info)
         else:
             for cam, cam_info in activeCameras:

From b85039c476b6d2d222f38883422b4cd8f9aeaadf Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 29 Jul 2024 09:17:58 +0200
Subject: [PATCH 29/87] Split filtering into fisheye and non-fisheye

---
 calibration_utils.py | 44 ++++++++++++++++++++++++++++++++++----------
 1 file changed, 34 insertions(+), 10 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index f5de1c8..7b023b9 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -153,7 +153,7 @@ def get_features(calibration, images_path, width, height, features, charucos, ca
     
     return cam_info, all_features, all_ids
 
-def filter_features(calibration, images_path, cam_info, intrinsic_img, _cameraModel, distortion_model, all_features, all_ids):
+def filter_features(calibration, images_path, cam_info, intrinsic_img, distortion_model, all_features, all_ids):
     f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
     print("INTRINSIC CALIBRATION")
     cameraIntrinsics = np.array([[f,    0.0,      cam_info['imsize'][0]/2],
@@ -169,17 +169,38 @@ def filter_features(calibration, images_path, cam_info, intrinsic_img, _cameraMo
         filtered_images = images_path
 
     current_time = time.time()
-    if _cameraModel != "fisheye":
-        print("Filtering corners")
-        removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"], cam_info['imsize'], cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
+    print("Filtering corners")
+    removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"], cam_info['imsize'], cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
 
-        if filtered_features is None:
-            raise RuntimeError('Exception') # TODO : Handle
+    if filtered_features is None:
+        raise RuntimeError('Exception') # TODO : Handle
+
+    print(f"Filtering takes: {time.time()-current_time}")
+    
+    cam_info['filtered_ids'] = filtered_ids
+    cam_info['filtered_corners'] = filtered_features
+    cam_info['intrinsics'] = cameraIntrinsics
+    cam_info['dist_coeff'] = distCoeff
+
+    return cam_info
 
-        print(f"Filtering takes: {time.time()-current_time}")
+def filter_features_fisheye(calibration, images_path, cam_info, intrinsic_img, all_features, all_ids):
+    f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+    print("INTRINSIC CALIBRATION")
+    cameraIntrinsics = np.array([[f,    0.0,      cam_info['imsize'][0]/2],
+                                 [0.0,     f,      cam_info['imsize'][1]/2],
+                                 [0.0,   0.0,        1.0]])
+
+    distCoeff = np.zeros((12, 1))
+
+    if cam_info["name"] in intrinsic_img:
+        raise RuntimeError('This is broken')
+        all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
     else:
-        filtered_features = all_features
-        filtered_ids = all_ids
+        filtered_images = images_path
+
+    filtered_features = all_features
+    filtered_ids = all_ids
     
     cam_info['filtered_ids'] = filtered_ids
     cam_info['filtered_corners'] = filtered_features
@@ -304,7 +325,10 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         if per_ccm:
             for cam, cam_info in activeCameras:
                 cam_info, all_features, all_ids = get_features(self, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos[cam_info['name']], cam_info)
-                cam_info = filter_features(self, cam_info['images_path'], cam_info, intrinsic_img, self._cameraModel, cam_info['distortion_model'], all_features, all_ids)
+                if self._cameraModel == "fisheye":
+                    cam_info = filter_features_fisheye(self, cam_info['images_path'], cam_info, intrinsic_img, all_features, all_ids)
+                else:
+                    cam_info = filter_features(self, cam_info['images_path'], cam_info, intrinsic_img, cam_info['distortion_model'], all_features, all_ids)
                 cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info)
         else:
             for cam, cam_info in activeCameras:

From cdb55bcb912a9c73bf6656af9c1c3747001c568f Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 29 Jul 2024 09:38:07 +0200
Subject: [PATCH 30/87] Split calibration into filterint and calibration

---
 calibration_utils.py | 83 +++++++++++++++++++++++++++++++++-----------
 1 file changed, 63 insertions(+), 20 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 7b023b9..35e43e3 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -150,10 +150,6 @@ def get_features(calibration, images_path, width, height, features, charucos, ca
     if isinstance(all_features, str) and all_ids is None:
         raise RuntimeError(f'Exception {all_features}') # TODO : Handle
     cam_info["imsize"] = imsize
-    
-    return cam_info, all_features, all_ids
-
-def filter_features(calibration, images_path, cam_info, intrinsic_img, distortion_model, all_features, all_ids):
     f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
     print("INTRINSIC CALIBRATION")
     cameraIntrinsics = np.array([[f,    0.0,      cam_info['imsize'][0]/2],
@@ -161,27 +157,73 @@ def filter_features(calibration, images_path, cam_info, intrinsic_img, distortio
                                  [0.0,   0.0,        1.0]])
 
     distCoeff = np.zeros((12, 1))
+    cam_info['intrinsics'] = cameraIntrinsics
+    cam_info['dist_coeff'] = distCoeff
+    
+    return cam_info, all_features, all_ids
 
-    if cam_info["name"] in intrinsic_img:
-        raise RuntimeError('This is broken')
-        all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
-    else:
-        filtered_images = images_path
+def estimate_pose_and_filter(calibration, cam_info, allCorners, allIds):
+    hfov = cam_info['hfov']
+    imsize = cam_info['imsize']
+     # check if there are any suspicious corners with high reprojection error
+    index = 0
+    max_threshold = 75 + calibration.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
+    threshold_stepper = int(1.5 * (hfov / 30 + imsize[1] / 800))
+    if threshold_stepper < 1:
+        threshold_stepper = 1
+    print(threshold_stepper)
+    min_inliers = 1 - calibration.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
+    overall_pose = time.time()
+    for index, corners in enumerate(allCorners):
+        if len(corners) < 4:
+            raise RuntimeError(f"Less than 4 corners detected on {index} image.")
+    current = time.time()
+    
+    filtered_corners = []
+    filtered_ids = []
+    removed_corners = []
+        
+    
+    #for corners, ids in zip(allCorners, allIds):
+    current = time.time()
+    
+    with multiprocessing.Pool(processes=16) as pool:
+        results = pool.map(func=estimate_pose_and_filter_single, iterable=[(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], min_inliers, max_threshold, threshold_stepper) for a, b in zip(allCorners, allIds)])
 
-    current_time = time.time()
-    print("Filtering corners")
-    removed_features, filtered_features, filtered_ids, cameraIntrinsics, distCoeff = calibration.filtering_features(all_features, all_ids, cam_info["name"], cam_info['imsize'], cam_info["hfov"], cameraIntrinsics, distCoeff, distortion_model)
+    filtered_ids, filtered_corners, removed_corners = zip(*results)
 
-    if filtered_features is None:
-        raise RuntimeError('Exception') # TODO : Handle
+    print(f"Overall pose estimation {time.time() - overall_pose}s")
+
+    if sum([len(corners) < 4 for corners in filtered_corners]) > 0.15 * len(allCorners):
+        raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
+    print(f"Filtering {time.time() -current}s")
 
-    print(f"Filtering takes: {time.time()-current_time}")
-    
     cam_info['filtered_ids'] = filtered_ids
-    cam_info['filtered_corners'] = filtered_features
-    cam_info['intrinsics'] = cameraIntrinsics
-    cam_info['dist_coeff'] = distCoeff
+    cam_info['filtered_corners'] = filtered_corners
+    return cam_info
 
+def calibrate_charuco(calibration, cam_info):
+    distortion_flags = calibration.get_distortion_flags(cam_info['distortion_model'])
+    flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
+            
+    try:
+        (ret, camera_matrix, distortion_coefficients,
+                 rotation_vectors, translation_vectors,
+                 stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+                 perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+                    charucoCorners=cam_info['filtered_corners'],
+                    charucoIds=cam_info['filtered_ids'],
+                    board=calibration._board,
+                    imageSize=cam_info['imsize'],
+                    cameraMatrix=cam_info['intrinsics'],
+                    distCoeffs=cam_info['dist_coeff'],
+                    flags=flags,
+                    criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
+    except:
+        return f"First intrisic calibration failed for {cam_info['imsize']}", None, None
+
+    cam_info['intrinsics'] = camera_matrix
+    cam_info['dist_coeff'] = distortion_coefficients
     return cam_info
 
 def filter_features_fisheye(calibration, images_path, cam_info, intrinsic_img, all_features, all_ids):
@@ -328,7 +370,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 if self._cameraModel == "fisheye":
                     cam_info = filter_features_fisheye(self, cam_info['images_path'], cam_info, intrinsic_img, all_features, all_ids)
                 else:
-                    cam_info = filter_features(self, cam_info['images_path'], cam_info, intrinsic_img, cam_info['distortion_model'], all_features, all_ids)
+                    cam_info = estimate_pose_and_filter(self, cam_info, all_features, all_ids)
+                    cam_info = calibrate_charuco(self, cam_info)
                 cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info)
         else:
             for cam, cam_info in activeCameras:

From e01601b5a93cff6a11eff8579139da3939eee778 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 29 Jul 2024 10:33:55 +0200
Subject: [PATCH 31/87] Simplify estimate_pose_and_filter

---
 calibration_utils.py |  69 +++++++----------
 worker.py            | 173 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 200 insertions(+), 42 deletions(-)
 create mode 100644 worker.py

diff --git a/calibration_utils.py b/calibration_utils.py
index 35e43e3..7091d2d 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -93,9 +93,7 @@ def summary(self) -> str:
         """
         return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
-def estimate_pose_and_filter_single(args):
-    calibration, corners, ids, K, d, min_inliers, max_threshold, threshold_stepper = args
-
+def estimate_pose_and_filter_single(calibration, corners, ids, K, d, min_inliers, max_threshold, threshold_stepper):
     board = calibration._board
     
     objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
@@ -160,49 +158,33 @@ def get_features(calibration, images_path, width, height, features, charucos, ca
     cam_info['intrinsics'] = cameraIntrinsics
     cam_info['dist_coeff'] = distCoeff
     
-    return cam_info, all_features, all_ids
-
-def estimate_pose_and_filter(calibration, cam_info, allCorners, allIds):
-    hfov = cam_info['hfov']
-    imsize = cam_info['imsize']
      # check if there are any suspicious corners with high reprojection error
-    index = 0
-    max_threshold = 75 + calibration.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
-    threshold_stepper = int(1.5 * (hfov / 30 + imsize[1] / 800))
+    max_threshold = 75 + calibration.initial_max_threshold * (cam_info['hfov']/ 30 + cam_info['imsize'][1] / 800 * 0.2)
+    threshold_stepper = int(1.5 * (cam_info['hfov'] / 30 + cam_info['imsize'][1] / 800))
     if threshold_stepper < 1:
         threshold_stepper = 1
-    print(threshold_stepper)
-    min_inliers = 1 - calibration.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
-    overall_pose = time.time()
-    for index, corners in enumerate(allCorners):
-        if len(corners) < 4:
-            raise RuntimeError(f"Less than 4 corners detected on {index} image.")
-    current = time.time()
+    min_inliers = 1 - calibration.initial_min_filtered * (cam_info['hfov'] / 60 + cam_info['imsize'][1] / 800 * 0.2)
+    cam_info['max_threshold'] = max_threshold
+    cam_info['threshold_stepper'] = threshold_stepper
+    cam_info['min_inliers'] = min_inliers
     
+    return cam_info, all_features, all_ids
+
+def estimate_pose_and_filter(calibration, cam_info, allCorners, allIds):
     filtered_corners = []
     filtered_ids = []
-    removed_corners = []
-        
-    
-    #for corners, ids in zip(allCorners, allIds):
-    current = time.time()
-    
-    with multiprocessing.Pool(processes=16) as pool:
-        results = pool.map(func=estimate_pose_and_filter_single, iterable=[(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], min_inliers, max_threshold, threshold_stepper) for a, b in zip(allCorners, allIds)])
+    for a, b in zip(allCorners, allIds):
+        ids, corners, _ = estimate_pose_and_filter_single(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
+        filtered_corners.append(corners)
+        filtered_ids.append(ids)
 
-    filtered_ids, filtered_corners, removed_corners = zip(*results)
+    return filtered_corners, filtered_ids
 
-    print(f"Overall pose estimation {time.time() - overall_pose}s")
 
-    if sum([len(corners) < 4 for corners in filtered_corners]) > 0.15 * len(allCorners):
+def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
+    if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
         raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
-    print(f"Filtering {time.time() -current}s")
 
-    cam_info['filtered_ids'] = filtered_ids
-    cam_info['filtered_corners'] = filtered_corners
-    return cam_info
-
-def calibrate_charuco(calibration, cam_info):
     distortion_flags = calibration.get_distortion_flags(cam_info['distortion_model'])
     flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
             
@@ -211,8 +193,8 @@ def calibrate_charuco(calibration, cam_info):
                  rotation_vectors, translation_vectors,
                  stdDeviationsIntrinsics, stdDeviationsExtrinsics,
                  perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-                    charucoCorners=cam_info['filtered_corners'],
-                    charucoIds=cam_info['filtered_ids'],
+                    charucoCorners=filteredCorners,
+                    charucoIds=filteredIds,
                     board=calibration._board,
                     imageSize=cam_info['imsize'],
                     cameraMatrix=cam_info['intrinsics'],
@@ -224,6 +206,8 @@ def calibrate_charuco(calibration, cam_info):
 
     cam_info['intrinsics'] = camera_matrix
     cam_info['dist_coeff'] = distortion_coefficients
+    cam_info['filtered_corners'] = filteredCorners
+    cam_info['filtered_ids'] = filteredIds
     return cam_info
 
 def filter_features_fisheye(calibration, images_path, cam_info, intrinsic_img, all_features, all_ids):
@@ -251,10 +235,10 @@ def filter_features_fisheye(calibration, images_path, cam_info, intrinsic_img, a
 
     return cam_info
 
-def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info):
+def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, filtered_corners, filtered_ids):
     start = time.time()
     print('starting calibrate_wf')
-    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], cam_info['filtered_corners'], cam_info['filtered_ids'], cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
+    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
     if isinstance(ret, str) and all_ids is None:
         raise RuntimeError('Exception' + ret) # TODO : Handle
     print(f'calibrate_wf took {round(time.time() - start, 2)}s')
@@ -370,9 +354,10 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 if self._cameraModel == "fisheye":
                     cam_info = filter_features_fisheye(self, cam_info['images_path'], cam_info, intrinsic_img, all_features, all_ids)
                 else:
-                    cam_info = estimate_pose_and_filter(self, cam_info, all_features, all_ids)
-                    cam_info = calibrate_charuco(self, cam_info)
-                cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info)
+                    filtered_corners, filtered_ids = estimate_pose_and_filter(self, cam_info, all_features, all_ids)
+                    
+                    cam_info = calibrate_charuco(self, cam_info, filtered_corners, filtered_ids)
+                cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info, filtered_corners, filtered_ids)
         else:
             for cam, cam_info in activeCameras:
                 cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
diff --git a/worker.py b/worker.py
new file mode 100644
index 0000000..8693a35
--- /dev/null
+++ b/worker.py
@@ -0,0 +1,173 @@
+from typing import Dict, Any, Callable, Iterable
+from collections.abc import Iterable
+import multiprocessing
+import random
+import queue
+
+class ParallelTask:
+  def __init__(self, fun: Callable[[Any], Any], args: Iterable[Any] = (), kwargs: Dict[str, Any] = {}):
+    self._fun = fun
+    self._args = args
+    self._kwargs = kwargs
+    self._id = random.getrandbits(64)
+    self._retvals = None
+
+  def __repr__(self):
+    return f'<Task {self._fun} {self._id}>'
+
+  # For accessing task return values
+  def __iter__(self):
+    yield Retvals(task=self)
+
+  def __getitem__(self, key):
+    return Retvals(task=self, key=key)
+
+  @property
+  def retvals(self):
+    return self._retvals
+
+  def _canExecute(self) -> bool:
+    for arg in self._args:
+      if isinstance(arg, Retvals) and not arg.resolved():
+        return False
+    for kwargs in self._kwargs.values():
+      if isinstance(kwargs, Retvals) and not kwargs.resolved():
+        return False
+    return True
+
+  def _substituteArgs(self):
+    newargs = []
+    for arg in self._args:
+      if isinstance(arg, Retvals):
+        rev = arg.get()
+        print('got retval', rev)
+        newargs.extend(rev)
+      else:
+        newargs.append(arg)
+    self._args = newargs
+
+    for key, value in self._kwargs.items():
+      if isinstance(value, Retvals):
+        self._kwargs[key] = value.get()
+
+class ParallelTaskGroup:
+  def __init__(self, fun: Callable[[Any], Any], args: Iterable[Iterable[Any]] = [], kwargs: Iterable[Dict[str, Any]] = []):
+    if len(args) != 0 and len(kwargs) != 0 and len(args) != len(kwargs):
+      raise RuntimeError('The length of args and kwargs must match, or one must not be provided')
+    if len(args) == 0:
+      args = [()] * len(kwargs)
+    if len(kwargs) == 0:
+      kwargs = [{}] * len(args)
+    self._fun = fun
+    self._args = args
+    self._kwargs = kwargs
+    def _mapTask(args_kwargs):
+      args, kwargs = args_kwargs
+      if not isinstance(args, (tuple, list)):
+        args = [args]
+      return ParallelTask(self._fun, args, kwargs)
+    self._tasks = list(map(_mapTask, zip(self._args, self._kwargs)))
+
+  # For accessing task return values
+  def __iter__(self):
+    yield Retvals(taskGroup=self)
+
+  def __getitem__(self, key):
+    return Retvals(taskGroup=self, key=key)
+
+class Retvals:
+  def __init__(self, task: ParallelTask = None, taskGroup: ParallelTaskGroup = None, key: slice | tuple | int = None):
+    self._task = task
+    self._taskGroup = taskGroup
+    self._key = key
+
+  def resolved(self) -> bool:
+    if self._task:
+      return self._task._retvals is not None
+    else:
+      for task in self._taskGroup._tasks:
+        if task._retvals is None:
+          return False
+      return True
+
+  def get(self) -> Any | Iterable[Any]:
+    if self._task:
+      if self._key is None:
+        if isinstance(self._task._retvals, tuple):
+          return self._task._retvals
+        else:
+          return [self._task._retvals]
+      elif isinstance(self._key, tuple):
+        return [self._task._retvals[i] for i in self._key]
+      elif isinstance(self._key, slice):
+        return self._task._retvals[self._key]
+      return [self._task._retvals[self._key]]
+    else:
+      def lmap(*args):
+        return [list(map(*args))]
+      if self._key is None:
+        return lmap(lambda task: task._retvals, self._taskGroup._tasks)
+      elif isinstance(self._key, tuple):
+        return lmap(lambda task: [task._retvals[i] for i in self._key], self._taskGroup._tasks)
+      elif isinstance(self._key, slice):
+        return lmap(lambda task: task._retvals[self._key], self._taskGroup._tasks)
+      return lmap(lambda task: [task._retvals[self._key]], self._taskGroup._tasks)
+
+  def __repr__(self):
+    if self._task:
+      return f'<Retvals <key:{self._key}> of {self._task}>'
+    else:
+      return f'<Retvals <key:{self._key}> of {self._taskGroup}>'
+
+def worker_controller(_stop: multiprocessing.Event, _in: multiprocessing.Queue, _out: multiprocessing.Queue, _wId: int) -> None:
+  while not _stop.is_set():
+    try:
+      task: ParallelTask | None = _in.get(timeout=0.1)
+    except queue.Empty:
+      continue
+    print(task._args, task._kwargs)
+    retvals = task._fun(*task._args, **task._kwargs)
+    _out.put((task._id, retvals))
+
+class ParallelWorker:
+  def __init__(self, workers: int = 16):
+    self._workerIn = multiprocessing.Queue()
+    self._workerOut = multiprocessing.Queue()
+    self._stop = multiprocessing.Event()
+    self._workers = []
+
+    for i in range(workers):
+      p = multiprocessing.Process(target=worker_controller, args=(self._stop, self._workerIn, self._workerOut, i))
+      self._workers.append(p)
+      p.start()
+
+  def _iterTasks(self, tasks: Iterable[ParallelTask]):
+    for task in tasks:
+      if isinstance(task, ParallelTaskGroup):
+        for task in task._tasks:
+          yield task
+      else:
+        yield task
+
+  def execute(self, tasks: Iterable[ParallelTask]) -> None:
+    tasks = list(self._iterTasks(tasks))
+    doneTasks = []
+    remaining = len(tasks)
+    while remaining != 0:
+      for task in tasks:
+        if task._canExecute():
+          task._substituteArgs()
+          self._workerIn.put(task)
+          doneTasks.append(task)
+          tasks.remove(task)
+
+      if not self._workerOut.empty():
+        tId, retvals = self._workerOut.get()
+        remaining -= 1
+        for task in doneTasks:
+          if task._id == tId:
+            task._retvals = retvals
+
+    self._stop.set()
+    for worker in self._workers:
+      worker.join()
\ No newline at end of file

From 9d9517f7d982510ef3c6b9e00b8fe40c1025ce8c Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 29 Jul 2024 10:40:30 +0200
Subject: [PATCH 32/87] Simplify arguments

---
 calibration_utils.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 7091d2d..e06745d 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -142,8 +142,8 @@ def estimate_pose_and_filter_single(calibration, corners, ids, K, d, min_inliers
         #removed_corners.extend(corners2[removed_mask])
         return valid_ids.reshape(-1, 1).astype(np.int32), corners2[valid_mask].reshape(-1, 1, 2), corners2[removed_mask]
 
-def get_features(calibration, images_path, width, height, features, charucos, cam_info):
-    all_features, all_ids, imsize = calibration.getting_features(images_path, width, height, features=features, charucos=charucos)
+def get_features(calibration, features, charucos, cam_info):
+    all_features, all_ids, imsize = calibration.getting_features(cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
 
     if isinstance(all_features, str) and all_ids is None:
         raise RuntimeError(f'Exception {all_features}') # TODO : Handle
@@ -210,7 +210,7 @@ def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
     cam_info['filtered_ids'] = filteredIds
     return cam_info
 
-def filter_features_fisheye(calibration, images_path, cam_info, intrinsic_img, all_features, all_ids):
+def filter_features_fisheye(calibration, cam_info, intrinsic_img, all_features, all_ids):
     f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
     print("INTRINSIC CALIBRATION")
     cameraIntrinsics = np.array([[f,    0.0,      cam_info['imsize'][0]/2],
@@ -223,7 +223,7 @@ def filter_features_fisheye(calibration, images_path, cam_info, intrinsic_img, a
         raise RuntimeError('This is broken')
         all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
     else:
-        filtered_images = images_path
+        filtered_images = cam_info['images_path']
 
     filtered_features = all_features
     filtered_ids = all_ids
@@ -350,9 +350,9 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         if per_ccm:
             for cam, cam_info in activeCameras:
-                cam_info, all_features, all_ids = get_features(self, cam_info['images_path'], cam_info['width'], cam_info['height'], features, charucos[cam_info['name']], cam_info)
+                cam_info, all_features, all_ids = get_features(self, features, charucos[cam_info['name']], cam_info)
                 if self._cameraModel == "fisheye":
-                    cam_info = filter_features_fisheye(self, cam_info['images_path'], cam_info, intrinsic_img, all_features, all_ids)
+                    cam_info = filter_features_fisheye(self, cam_info, intrinsic_img, all_features, all_ids)
                 else:
                     filtered_corners, filtered_ids = estimate_pose_and_filter(self, cam_info, all_features, all_ids)
                     

From 9fbe78f9b4ecdbe6ca4264babb1b1312b091316c Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 29 Jul 2024 11:04:33 +0200
Subject: [PATCH 33/87] Add parallelization back

---
 calibration_utils.py | 22 +++++++++++++++++-----
 worker.py            |  2 --
 2 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index e06745d..fd798b8 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -23,6 +23,7 @@
 import matplotlib.pyplot as plt
 from scipy.interpolate import griddata
 plt.rcParams.update({'font.size': 16})
+from .worker import ParallelTask, ParallelTaskGroup, ParallelWorker
 import matplotlib.colors as colors
 import logging
 logging.getLogger('matplotlib').setLevel(logging.WARNING)
@@ -348,20 +349,31 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         for cam, cam_info in activeCameras:
             cam_info = load_camera_data(self, filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
 
+        tasks = []
+        camInfos = []
         if per_ccm:
             for cam, cam_info in activeCameras:
-                cam_info, all_features, all_ids = get_features(self, features, charucos[cam_info['name']], cam_info)
+                ret = ParallelTask(get_features, [self, features, charucos[cam_info['name']], cam_info])
                 if self._cameraModel == "fisheye":
-                    cam_info = filter_features_fisheye(self, cam_info, intrinsic_img, all_features, all_ids)
+                    ret2 = ParallelTask(filter_features_fisheye, [self, ret[0], intrinsic_img, ret[1], ret[2]])
                 else:
-                    filtered_corners, filtered_ids = estimate_pose_and_filter(self, cam_info, all_features, all_ids)
+                    featuresAndIds = ParallelTask(estimate_pose_and_filter, [self, *ret])
                     
-                    cam_info = calibrate_charuco(self, cam_info, filtered_corners, filtered_ids)
-                cam_info = calibrate_ccm_intrinsics_per_ccm(self, features, cam_info, filtered_corners, filtered_ids)
+                    ret2 = ParallelTask(calibrate_charuco, [self, ret[0], *featuresAndIds])
+                ret3 = ParallelTask(calibrate_ccm_intrinsics_per_ccm, [self, features, *ret2, *featuresAndIds])
+                tasks.extend([ret, ret2, ret3, featuresAndIds])
+                camInfos.append((cam, ret3))
         else:
             for cam, cam_info in activeCameras:
                 cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
 
+        pw = ParallelWorker(16)
+        pw.execute(tasks)
+
+        for cam, cam_info in camInfos:
+            board_config['cameras'][cam] = cam_info.retvals
+            
+
         for left, right in stereoPairs:
             calibrate_stereo_pair(self, left, right, board_config, features)
 
diff --git a/worker.py b/worker.py
index 8693a35..99fc2f6 100644
--- a/worker.py
+++ b/worker.py
@@ -40,7 +40,6 @@ def _substituteArgs(self):
     for arg in self._args:
       if isinstance(arg, Retvals):
         rev = arg.get()
-        print('got retval', rev)
         newargs.extend(rev)
       else:
         newargs.append(arg)
@@ -125,7 +124,6 @@ def worker_controller(_stop: multiprocessing.Event, _in: multiprocessing.Queue,
       task: ParallelTask | None = _in.get(timeout=0.1)
     except queue.Empty:
       continue
-    print(task._args, task._kwargs)
     retvals = task._fun(*task._args, **task._kwargs)
     _out.put((task._id, retvals))
 

From 065e027a45fda969f1978a1f121c6092f8adf83d Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 29 Jul 2024 11:37:51 +0200
Subject: [PATCH 34/87] Type hints

---
 worker.py | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/worker.py b/worker.py
index 99fc2f6..847f919 100644
--- a/worker.py
+++ b/worker.py
@@ -1,11 +1,13 @@
-from typing import Dict, Any, Callable, Iterable
+from typing import Dict, Any, Callable, Iterable, TypeVar, Generic
 from collections.abc import Iterable
 import multiprocessing
 import random
 import queue
 
-class ParallelTask:
-  def __init__(self, fun: Callable[[Any], Any], args: Iterable[Any] = (), kwargs: Dict[str, Any] = {}):
+R = TypeVar('R')
+
+class ParallelTask(Generic[R]):
+  def __init__(self, fun: Callable[..., R], args: Iterable[Any] = (), kwargs: Dict[str, Any] = {}):
     self._fun = fun
     self._args = args
     self._kwargs = kwargs
@@ -17,13 +19,13 @@ def __repr__(self):
 
   # For accessing task return values
   def __iter__(self):
-    yield Retvals(task=self)
+    yield Retvals[R](task=self)
 
   def __getitem__(self, key):
-    return Retvals(task=self, key=key)
+    return Retvals[R](task=self, key=key)
 
   @property
-  def retvals(self):
+  def retvals(self) -> R:
     return self._retvals
 
   def _canExecute(self) -> bool:
@@ -74,7 +76,7 @@ def __iter__(self):
   def __getitem__(self, key):
     return Retvals(taskGroup=self, key=key)
 
-class Retvals:
+class Retvals(Generic[R]):
   def __init__(self, task: ParallelTask = None, taskGroup: ParallelTaskGroup = None, key: slice | tuple | int = None):
     self._task = task
     self._taskGroup = taskGroup
@@ -89,7 +91,7 @@ def resolved(self) -> bool:
           return False
       return True
 
-  def get(self) -> Any | Iterable[Any]:
+  def get(self) -> R:
     if self._task:
       if self._key is None:
         if isinstance(self._task._retvals, tuple):

From aac8d979e79f46e91420078feadfdf1288546d2b Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Wed, 31 Jul 2024 16:13:51 +0200
Subject: [PATCH 35/87] Fix parallelization

---
 calibration_utils.py |  70 +++++-----
 worker.py            | 319 +++++++++++++++++++++++++------------------
 2 files changed, 220 insertions(+), 169 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index fd798b8..cd3fa37 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -94,7 +94,7 @@ def summary(self) -> str:
         """
         return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
-def estimate_pose_and_filter_single(calibration, corners, ids, K, d, min_inliers, max_threshold, threshold_stepper):
+def estimate_pose_and_filter_single(calibration, cam_info, corners, ids):
     board = calibration._board
     
     objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
@@ -104,10 +104,10 @@ def estimate_pose_and_filter_single(calibration, corners, ids, K, d, min_inliers
     
     objects = []
     all_objects = []
-    while len(objects) < len(objpoints[:,0,0]) * min_inliers:
-        if ini_threshold > max_threshold:
+    while len(objects) < len(objpoints[:,0,0]) * cam_info['min_inliers']:
+        if ini_threshold > cam_info['max_threshold']:
             break
-        ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+        ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, cam_info['intrinsics'], cam_info['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
         all_objects.append(objects)
         imgpoints2 = objpoints.copy()
 
@@ -115,15 +115,15 @@ def estimate_pose_and_filter_single(calibration, corners, ids, K, d, min_inliers
         all_corners2 = np.array([all_corners2[id[0]] for id in objects])
         imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
 
-        ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, K, d)
+        ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, cam_info['intrinsics'], cam_info['dist_coeff'])
         
-        ini_threshold += threshold_stepper
+        ini_threshold += cam_info['threshold_stepper']
     if not ret:
         raise RuntimeError('Exception') # TODO : Handle
     
     if ids is not None and corners.size > 0:
         ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-        imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, K, d)
+        imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, cam_info['intrinsics'], cam_info['dist_coeff'])
         corners2 = corners.reshape(-1, 2)
         imgpoints2 = imgpoints2.reshape(-1, 2)
 
@@ -141,7 +141,7 @@ def estimate_pose_and_filter_single(calibration, corners, ids, K, d, min_inliers
         #filtered_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
 
         #removed_corners.extend(corners2[removed_mask])
-        return valid_ids.reshape(-1, 1).astype(np.int32), corners2[valid_mask].reshape(-1, 1, 2), corners2[removed_mask]
+        return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
 def get_features(calibration, features, charucos, cam_info):
     all_features, all_ids, imsize = calibration.getting_features(cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
@@ -183,27 +183,28 @@ def estimate_pose_and_filter(calibration, cam_info, allCorners, allIds):
 
 
 def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
-    if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
-        raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
+    # TODO : If we still need this check it needs to be elsewhere
+    # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners): 
+    #     raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
 
     distortion_flags = calibration.get_distortion_flags(cam_info['distortion_model'])
     flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
             
-    try:
-        (ret, camera_matrix, distortion_coefficients,
-                 rotation_vectors, translation_vectors,
-                 stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-                 perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-                    charucoCorners=filteredCorners,
-                    charucoIds=filteredIds,
-                    board=calibration._board,
-                    imageSize=cam_info['imsize'],
-                    cameraMatrix=cam_info['intrinsics'],
-                    distCoeffs=cam_info['dist_coeff'],
-                    flags=flags,
-                    criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
-    except:
-        return f"First intrisic calibration failed for {cam_info['imsize']}", None, None
+    #try:
+    (ret, camera_matrix, distortion_coefficients,
+             rotation_vectors, translation_vectors,
+             stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+             perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+                charucoCorners=filteredCorners,
+                charucoIds=filteredIds,
+                board=calibration._board,
+                imageSize=cam_info['imsize'],
+                cameraMatrix=cam_info['intrinsics'],
+                distCoeffs=cam_info['dist_coeff'],
+                flags=flags,
+                criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
+    #except:
+    #    return f"First intrisic calibration failed for {cam_info['imsize']}", None, None
 
     cam_info['intrinsics'] = camera_matrix
     cam_info['dist_coeff'] = distortion_coefficients
@@ -351,27 +352,28 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         tasks = []
         camInfos = []
+        pw = ParallelWorker(16)
         if per_ccm:
             for cam, cam_info in activeCameras:
-                ret = ParallelTask(get_features, [self, features, charucos[cam_info['name']], cam_info])
+                ret = pw.run(get_features, self, features, charucos[cam_info['name']], cam_info)
                 if self._cameraModel == "fisheye":
-                    ret2 = ParallelTask(filter_features_fisheye, [self, ret[0], intrinsic_img, ret[1], ret[2]])
+                    ret2 = pw.run(filter_features_fisheye, self, ret[0], intrinsic_img, ret[1], ret[2])
                 else:
-                    featuresAndIds = ParallelTask(estimate_pose_and_filter, [self, *ret])
+                    #featuresAndIds = ParallelTask(estimate_pose_and_filter, [self, *ret])
+                    featuresAndIds = pw.map(estimate_pose_and_filter_single, self, ret[0], ret[1], ret[2])
+        #estimate_pose_and_filter_single(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
                     
-                    ret2 = ParallelTask(calibrate_charuco, [self, ret[0], *featuresAndIds])
-                ret3 = ParallelTask(calibrate_ccm_intrinsics_per_ccm, [self, features, *ret2, *featuresAndIds])
+                    ret2 = pw.run(calibrate_charuco, self, ret[0], featuresAndIds[0], featuresAndIds[1])
+                ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, self, features, ret2, featuresAndIds[0], featuresAndIds[1])
                 tasks.extend([ret, ret2, ret3, featuresAndIds])
                 camInfos.append((cam, ret3))
         else:
             for cam, cam_info in activeCameras:
                 cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
 
-        pw = ParallelWorker(16)
-        pw.execute(tasks)
-
+        pw.execute()
         for cam, cam_info in camInfos:
-            board_config['cameras'][cam] = cam_info.retvals
+            board_config['cameras'][cam] = cam_info.ret()
             
 
         for left, right in stereoPairs:
diff --git a/worker.py b/worker.py
index 847f919..b48e944 100644
--- a/worker.py
+++ b/worker.py
@@ -1,173 +1,222 @@
-from typing import Dict, Any, Callable, Iterable, TypeVar, Generic
-from collections.abc import Iterable
+from typing import Dict, Any, Callable, Iterable, Tuple, TypeVar, Generic, List
+from collections import abc
 import multiprocessing
 import random
 import queue
 
-R = TypeVar('R')
+T = TypeVar('T')
 
-class ParallelTask(Generic[R]):
-  def __init__(self, fun: Callable[..., R], args: Iterable[Any] = (), kwargs: Dict[str, Any] = {}):
+def allArgs(args, kwargs):
+  for arg in args:
+    yield arg
+  for kwarg in kwargs.values():
+    yield kwarg
+
+class Retvals:
+  def __init__(self, taskOrGroup: 'ParallelTask', key: slice | tuple | int):
+    self._taskOrGroup = taskOrGroup
+    self._key = key
+
+  def ret(self):
+    if isinstance(self._key, list | tuple):
+      ret = self._taskOrGroup.ret()
+      return [ret[i] for i in self._key]
+    return self._taskOrGroup.ret()[self._key]
+
+  def finished(self):
+    return self._taskOrGroup.finished()
+
+class ParallelTask(Generic[T]):
+  def __init__(self, worker: 'ParallelWorker', fun, args, kwargs):
+    self._worker = worker
     self._fun = fun
     self._args = args
     self._kwargs = kwargs
+    self._ret = None
+    self._exc = None
     self._id = random.getrandbits(64)
-    self._retvals = None
+    self._finished = False
+
+  def __getitem__(self, key):
+    return Retvals(self, key)
 
   def __repr__(self):
     return f'<Task {self._fun} {self._id}>'
 
-  # For accessing task return values
-  def __iter__(self):
-    yield Retvals[R](task=self)
+  def resolveArguments(self) -> None:
+    def _replace(args):
+      for arg in args:
+        if isinstance(arg, ParallelTask | ParallelTaskGroup | Retvals):
+          yield arg.ret()
+        else:
+          yield arg
+    self._args = list(_replace(self._args))
+    for key, value in self._kwargs:
+      if isinstance(value, ParallelTask | ParallelTaskGroup | Retvals):
+        self._kwargs[key] = value.ret()
+
+  def isExecutable(self) -> bool:
+    for arg in allArgs(self._args, self._kwargs):
+      if isinstance(arg, ParallelTask | ParallelTaskGroup | Retvals) and not arg.finished():
+        return False
+    return True
 
-  def __getitem__(self, key):
-    return Retvals[R](task=self, key=key)
+  def finished(self) -> bool:
+    return self._finished
 
-  @property
-  def retvals(self) -> R:
-    return self._retvals
+  def finish(self, ret, exc) -> None:
+    self._ret = ret
+    self._exc = exc
+    self._finished = True
 
-  def _canExecute(self) -> bool:
-    for arg in self._args:
-      if isinstance(arg, Retvals) and not arg.resolved():
-        return False
-    for kwargs in self._kwargs.values():
-      if isinstance(kwargs, Retvals) and not kwargs.resolved():
-        return False
-    return True
+  def exc(self) -> BaseException | None:
+    return self._exc
 
-  def _substituteArgs(self):
-    newargs = []
-    for arg in self._args:
-      if isinstance(arg, Retvals):
-        rev = arg.get()
-        newargs.extend(rev)
-      else:
-        newargs.append(arg)
-    self._args = newargs
-
-    for key, value in self._kwargs.items():
-      if isinstance(value, Retvals):
-        self._kwargs[key] = value.get()
-
-class ParallelTaskGroup:
-  def __init__(self, fun: Callable[[Any], Any], args: Iterable[Iterable[Any]] = [], kwargs: Iterable[Dict[str, Any]] = []):
-    if len(args) != 0 and len(kwargs) != 0 and len(args) != len(kwargs):
-      raise RuntimeError('The length of args and kwargs must match, or one must not be provided')
-    if len(args) == 0:
-      args = [()] * len(kwargs)
-    if len(kwargs) == 0:
-      kwargs = [{}] * len(args)
+  def ret(self) -> T | None:
+    return self._ret
+
+class ParallelTaskGroup(Generic[T]):
+  def __init__(self, fun, args, kwargs):
     self._fun = fun
     self._args = args
     self._kwargs = kwargs
-    def _mapTask(args_kwargs):
-      args, kwargs = args_kwargs
-      if not isinstance(args, (tuple, list)):
-        args = [args]
-      return ParallelTask(self._fun, args, kwargs)
-    self._tasks = list(map(_mapTask, zip(self._args, self._kwargs)))
-
-  # For accessing task return values
-  def __iter__(self):
-    yield Retvals(taskGroup=self)
 
   def __getitem__(self, key):
-    return Retvals(taskGroup=self, key=key)
+    return Retvals(self, key)
 
-class Retvals(Generic[R]):
-  def __init__(self, task: ParallelTask = None, taskGroup: ParallelTaskGroup = None, key: slice | tuple | int = None):
-    self._task = task
-    self._taskGroup = taskGroup
-    self._key = key
+  def finished(self) -> bool:
+    for task in self._tasks:
+      if not task.finished():
+        return False
+    return True
 
-  def resolved(self) -> bool:
-    if self._task:
-      return self._task._retvals is not None
-    else:
-      for task in self._taskGroup._tasks:
-        if task._retvals is None:
-          return False
-      return True
-
-  def get(self) -> R:
-    if self._task:
-      if self._key is None:
-        if isinstance(self._task._retvals, tuple):
-          return self._task._retvals
+  def exc(self) -> List[BaseException | None]:
+    return list(map(lambda t: t.exc(), self._tasks))
+
+  def tasks(self) -> List[ParallelTask]:
+    nTasks = 1
+    for arg in allArgs(self._args, self._kwargs):
+      if isinstance(arg, abc.Sized):
+        nTasks = max(nTasks, len(arg))
+      elif isinstance(arg, ParallelTask | Retvals):
+        nTasks = max(nTasks, len(arg.ret()))
+    self._tasks = []
+    for arg in allArgs(self._args, self._kwargs):
+      if isinstance(arg, abc.Sized) and len(arg) != nTasks:
+        raise RuntimeError('All sized arguments must have the same length or 1')
+    
+    def argsAtI(i: int):
+      for arg in self._args:
+        if isinstance(arg, list):
+          yield arg[i]
+        elif isinstance(arg, ParallelTask | Retvals) and isinstance(arg.ret(), abc.Iterable) and not isinstance(arg.ret(), (str, bytes, dict)):
+          yield arg.ret()[i]
+        else:
+          yield arg
+    def kwargsAtI(i: int):
+      newKwargs = {}
+      for key, value in self._kwargs:
+        if isinstance(arg, abc.Sized):
+          newKwargs[key] = value[i]
         else:
-          return [self._task._retvals]
-      elif isinstance(self._key, tuple):
-        return [self._task._retvals[i] for i in self._key]
-      elif isinstance(self._key, slice):
-        return self._task._retvals[self._key]
-      return [self._task._retvals[self._key]]
-    else:
-      def lmap(*args):
-        return [list(map(*args))]
-      if self._key is None:
-        return lmap(lambda task: task._retvals, self._taskGroup._tasks)
-      elif isinstance(self._key, tuple):
-        return lmap(lambda task: [task._retvals[i] for i in self._key], self._taskGroup._tasks)
-      elif isinstance(self._key, slice):
-        return lmap(lambda task: task._retvals[self._key], self._taskGroup._tasks)
-      return lmap(lambda task: [task._retvals[self._key]], self._taskGroup._tasks)
+          newKwargs[key] = value
+      return newKwargs
+
+    for i in range(nTasks):
+      task = ParallelTask(self, self._fun, list(argsAtI(i)), kwargsAtI(i))
+      self._tasks.append(task)
+    return self._tasks
+
+  def ret(self) -> List[T | None]:
+    def zip_retvals(tasks):
+      def _iRet(i):
+        for task in tasks:
+          yield task.ret()[i]
+
+      l = len(tasks[0].ret()) if isinstance(tasks[0].ret(), abc.Sized) else 1
+      if l > 1:
+        for i in range(len(tasks[0].ret())):
+          yield list(_iRet(i))
+      else:
+        for task in tasks:
+          yield task.ret()
 
-  def __repr__(self):
-    if self._task:
-      return f'<Retvals <key:{self._key}> of {self._task}>'
-    else:
-      return f'<Retvals <key:{self._key}> of {self._taskGroup}>'
+    return list(zip_retvals(self._tasks))
+
+  def isExecutable(self) -> bool:
+    for arg in allArgs(self._args, self._kwargs):
+      if isinstance(arg, ParallelTask | ParallelTaskGroup | Retvals) and not arg.finished():
+        return False
+    return True
 
 def worker_controller(_stop: multiprocessing.Event, _in: multiprocessing.Queue, _out: multiprocessing.Queue, _wId: int) -> None:
   while not _stop.is_set():
     try:
-      task: ParallelTask | None = _in.get(timeout=0.1)
+      task: ParallelTask | None = _in.get(timeout=0.01)
     except queue.Empty:
       continue
-    retvals = task._fun(*task._args, **task._kwargs)
-    _out.put((task._id, retvals))
-
-class ParallelWorker:
+    #try:
+    ret, exc = None, None
+    ret = task._fun(*task._args, **task._kwargs)
+    #except BaseException as e:
+      #exc = e
+    #finally:
+    _out.put((task._id, ret, exc))
+
+class ParallelWorker(Generic[T]):
   def __init__(self, workers: int = 16):
-    self._workerIn = multiprocessing.Queue()
-    self._workerOut = multiprocessing.Queue()
-    self._stop = multiprocessing.Event()
-    self._workers = []
-
-    for i in range(workers):
-      p = multiprocessing.Process(target=worker_controller, args=(self._stop, self._workerIn, self._workerOut, i))
-      self._workers.append(p)
+    self._workers = workers
+    self._tasks: List[ParallelTask] = []
+
+  def __enter__(self):
+    return self
+
+  def __exit__(self, exc_type, exc_value, exc_traceback):
+    self.execute()
+
+  def run(self, fun: Callable[[Any], T], *args: Tuple[Any], **kwargs: Dict[str, Any]) -> ParallelTask[T]:
+    task = ParallelTask(self, fun, args, kwargs)
+    self._tasks.append(task)
+    return task
+
+  def map(self, fun: Callable[[Any], T], *args: Tuple[Iterable[Any]], **kwargs: Dict[str, Iterable[Any]]) -> ParallelTaskGroup[T]:
+    taskGroup = ParallelTaskGroup(fun, args, kwargs)
+    self._tasks.append(taskGroup)
+    return taskGroup
+
+  def execute(self):
+    workerIn = multiprocessing.Manager().Queue()
+    workerOut = multiprocessing.Manager().Queue()
+    stop = multiprocessing.Event()
+    processes = []
+    for i in range(self._workers):
+      p = multiprocessing.Process(target=worker_controller, args=(stop, workerIn, workerOut, i))
+      processes.append(p)
       p.start()
 
-  def _iterTasks(self, tasks: Iterable[ParallelTask]):
-    for task in tasks:
-      if isinstance(task, ParallelTaskGroup):
-        for task in task._tasks:
-          yield task
-      else:
-        yield task
-
-  def execute(self, tasks: Iterable[ParallelTask]) -> None:
-    tasks = list(self._iterTasks(tasks))
     doneTasks = []
-    remaining = len(tasks)
-    while remaining != 0:
-      for task in tasks:
-        if task._canExecute():
-          task._substituteArgs()
-          self._workerIn.put(task)
-          doneTasks.append(task)
-          tasks.remove(task)
-
-      if not self._workerOut.empty():
-        tId, retvals = self._workerOut.get()
-        remaining -= 1
+    remaining = len(self._tasks)
+
+    while remaining > 0:
+      for task in self._tasks:
+        if task.isExecutable():
+          if isinstance(task, ParallelTask):
+            task.resolveArguments()
+            workerIn.put(task)
+            doneTasks.append(task)
+            self._tasks.remove(task)
+          else:
+            newTasks = task.tasks()
+            remaining += len(newTasks) - 1
+            self._tasks.extend(newTasks)
+            self._tasks.remove(task)
+
+      if not workerOut.empty():
+        tId, ret, exc = workerOut.get()
         for task in doneTasks:
           if task._id == tId:
-            task._retvals = retvals
-
-    self._stop.set()
-    for worker in self._workers:
-      worker.join()
\ No newline at end of file
+            task.finish(ret, exc)
+            remaining -= 1
+    stop.set()
+    for p in processes:
+      p.join()
\ No newline at end of file

From b92640d3d2fbdc91deeddfc09f56fa7261b8dc83 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 2 Aug 2024 10:07:23 +0200
Subject: [PATCH 36/87] Cleanup imports

---
 calibration_utils.py | 62 +++++++++++++-------------------------------
 1 file changed, 18 insertions(+), 44 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index cd3fa37..9279bc9 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1,47 +1,25 @@
 #!/usr/bin/env python3
 
-import cv2
-import glob
-import os
-import shutil
-import numpy as np
 from scipy.spatial.transform import Rotation
-import multiprocessing
-from multiprocessing.pool import ThreadPool
-
-import time
-import json
+import matplotlib.colors as colors
+from .worker import ParallelWorker
+import matplotlib.pyplot as plt
+from collections import deque
 import cv2.aruco as aruco
-import threading
+from pathlib import Path
+import multiprocessing
+import numpy as np
 import logging
+import time
+import glob
+import cv2
+
 logging.getLogger('matplotlib').setLevel(logging.WARNING)
 
-from pathlib import Path
-from functools import reduce
-from collections import deque
-from typing import Optional
-import matplotlib.pyplot as plt
-from scipy.interpolate import griddata
 plt.rcParams.update({'font.size': 16})
-from .worker import ParallelTask, ParallelTaskGroup, ParallelWorker
-import matplotlib.colors as colors
-import logging
-logging.getLogger('matplotlib').setLevel(logging.WARNING)
-per_ccm = True
-extrinsic_per_ccm = False
-cdict = {'red':  ((0.0, 0.0, 0.0),   # no red at 0
-          (0.5, 1.0, 1.0),   # all channels set to 1.0 at 0.5 to create white
-          (1.0, 0.8, 0.8)),  # set to 0.8 so its not too bright at 1
-'green': ((0.0, 0.8, 0.8),   # set to 0.8 so its not too bright at 0
-          (0.5, 1.0, 1.0),   # all channels set to 1.0 at 0.5 to create white
-          (1.0, 0.0, 0.0)),  # no green at 1
-'blue':  ((0.0, 0.0, 0.0),   # no blue at 0
-          (0.5, 1.0, 1.0),   # all channels set to 1.0 at 0.5 to create white
-          (1.0, 0.0, 0.0))   # no blue at 1
-}
-initial_max_threshold = 15
-initial_min_filtered = 0.05
-calibration_max_threshold = 10
+
+PER_CCM = True
+EXTRINSICS_PER_CCM = False
 
 class ProxyDict:
   def __init__(self, squaresX, squaresY, squareSize, markerSize, dictSize):
@@ -74,10 +52,6 @@ def board(self):
       self.__build()
     return self._board
 
-def distance(point1, point2):
-    return np.sqrt((point1[0] - point2[0])**2 + (point1[1] - point2[1])**2)
-# Creates a set of 13 polygon coordinates
-rectProjectionMode = 0
 
 colors = [(0, 255 , 0), (0, 0, 255)]
 
@@ -353,7 +327,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         tasks = []
         camInfos = []
         pw = ParallelWorker(16)
-        if per_ccm:
+        if PER_CCM:
             for cam, cam_info in activeCameras:
                 ret = pw.run(get_features, self, features, charucos[cam_info['name']], cam_info)
                 if self._cameraModel == "fisheye":
@@ -430,7 +404,7 @@ def calibrate_stereo_pair(self, left_cam, right_cam, board_config, features):
             [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
         rotation = Rotation.from_euler(
             'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-        if per_ccm and extrinsic_per_ccm:
+        if PER_CCM and EXTRINSICS_PER_CCM:
             if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
                 if left_cam_info["name"] in self.extrinsic_img:
                     array = self.extrinsic_img[left_cam_info["name"]]
@@ -470,7 +444,7 @@ def calibrate_stereo_pair(self, left_cam, right_cam, board_config, features):
             scale = right_cam_info['intrinsics'][0][0]
         else:
             scale = left_cam_info['intrinsics'][0][0]
-        if per_ccm and extrinsic_per_ccm:
+        if PER_CCM and EXTRINSICS_PER_CCM:
             scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
             print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
             left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
@@ -1199,7 +1173,7 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
 
         stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT +
                                 cv2.TERM_CRITERIA_EPS, 300, 1e-9)
-        if per_ccm and extrinsic_per_ccm:
+        if PER_CCM and EXTRINSICS_PER_CCM:
             for i in range(len(left_corners_sampled)):
                 if left_calib_model == "perspective":
                     left_corners_sampled[i] = cv2.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)

From e79962f30d21b742fd9820bc9ed3900f528bccbb Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 2 Aug 2024 15:27:10 +0200
Subject: [PATCH 37/87] Split stereo calibration into multiple functions

---
 calibration_utils.py | 221 ++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 219 insertions(+), 2 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 9279bc9..4bf39e2 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -243,8 +243,222 @@ def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
 
     return cam_info
 
-def calibrate_stereo_pair(calibration, left, right, board_config, features):
-    calibration.calibrate_stereo_pair(left, right, board_config, features)
+def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model, r_in, t_in):
+    flags = 0
+    # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
+    # print(flags)
+    flags = cv2.CALIB_FIX_INTRINSIC
+    distortion_flags = calibration.get_distortion_flags(left_distortion_model)
+    flags += distortion_flags
+    # print(flags)
+    ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
+    obj_pts, left_corners_sampled, right_corners_sampled,
+    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
+    R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
+
+    r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+    print(f'Epipolar error is {ret}')
+    print('Printing Extrinsics res...')
+    print(R)
+    print(T)
+    print(f'Euler angles in XYZ {r_euler} degs')
+
+
+    R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
+        cameraMatrix_l,
+        distCoeff_l,
+        cameraMatrix_r,
+        distCoeff_r,
+        None, R, T) # , alpha=0.1
+    # self.P_l = P_l
+    # self.P_r = P_r
+    r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
+    r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
+
+    return [ret, R, T, R_l, R_r, P_l, P_r]
+
+def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r):
+    # make sure all images have the same *number of* points
+    min_num_points = min([len(pts) for pts in obj_pts])
+    obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts]
+    left_corners_truncated = [pts[:min_num_points] for pts in left_corners_sampled]
+    right_corners_truncated = [pts[:min_num_points] for pts in right_corners_sampled]
+
+    flags = 0
+    # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
+    # flags |= cv2.fisheye.CALIB_CHECK_COND
+    # flags |= cv2.fisheye.CALIB_FIX_SKEW
+    flags |= cv2.fisheye.CALIB_FIX_INTRINSIC
+    # flags |= cv2.fisheye.CALIB_FIX_K1
+    # flags |= cv2.fisheye.CALIB_FIX_K2
+    # flags |= cv2.fisheye.CALIB_FIX_K3 
+    # flags |= cv2.fisheye.CALIB_FIX_K4
+    # flags |= cv2.CALIB_RATIONAL_MODEL
+    # flags |= cv2.CALIB_USE_INTRINSIC_GUESS
+    # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
+    # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW
+    (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate(
+        obj_pts_truncated, left_corners_truncated, right_corners_truncated,
+        cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
+        flags=flags, criteria=calibration.stereocalib_criteria), None, None
+    r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+    print(f'Reprojection error is {ret}')
+    isHorizontal = np.absolute(T[0]) > np.absolute(T[1])
+
+    R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify(
+        cameraMatrix_l,
+        distCoeff_l,
+        cameraMatrix_r,
+        distCoeff_r,
+        None, R, T, flags=0)
+
+    r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
+    r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
+
+    return [ret, R, T, R_l, R_r, P_l, P_r]
+
+def find_stereo_common_features(calibration, allIds_l, allIds_r, allCorners_l, allCorners_r):
+    left_corners_sampled = []
+    right_corners_sampled = []
+    left_ids_sampled = []
+    obj_pts = []
+    one_pts = calibration._board.chessboardCorners
+
+    for i in range(len(allIds_l)):
+        left_sub_corners = []
+        right_sub_corners = []
+        obj_pts_sub = []
+        #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70:
+        #      continue
+        for j in range(len(allIds_l[i])):
+            idx = np.where(allIds_r[i] == allIds_l[i][j])
+            if idx[0].size == 0:
+                continue
+            left_sub_corners.append(allCorners_l[i][j]) # TODO : This copies even idxs that don't match
+            right_sub_corners.append(allCorners_r[i][idx])
+            obj_pts_sub.append(one_pts[allIds_l[i][j]])
+        if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
+            obj_pts.append(np.array(obj_pts_sub, dtype=np.float32))
+            left_corners_sampled.append(
+                np.array(left_sub_corners, dtype=np.float32))
+            left_ids_sampled.append(np.array(allIds_l[i], dtype=np.int32))
+            right_corners_sampled.append(
+                np.array(right_sub_corners, dtype=np.float32))
+        else:
+            return -1, "Stereo Calib failed due to less common features"
+    return left_corners_sampled, right_corners_sampled, obj_pts
+
+def calibrate_stereo(calibration, allIds_l, allCorners_l, allIds_r, allCorners_r, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, left_calib_model, right_calib_model, left_distortion_model, right_distortion_model, features = None):
+    left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, allIds_l, allIds_r, allCorners_l, allCorners_r)
+
+    if PER_CCM and EXTRINSICS_PER_CCM:
+        for i in range(len(left_corners_sampled)):
+            if left_calib_model == "perspective":
+                left_corners_sampled[i] = cv2.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
+            else:
+                left_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
+
+        for i in range(len(right_corners_sampled)):
+            if right_calib_model == "perspective":
+                right_corners_sampled[i] = cv2.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, distCoeff_r, P=cameraMatrix_r)
+            else:
+                right_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, distCoeff_r, P=cameraMatrix_r)
+
+        if features == None or features == "charucos": 
+            flags = cv2.CALIB_FIX_INTRINSIC
+            ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
+            obj_pts, left_corners_sampled, right_corners_sampled,
+            np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
+            R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
+
+            r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+            scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
+            print(f'Epipolar error without scale: {ret}')
+            print(f'Epipolar error with scale: {ret*np.sqrt(scale)}')
+            print('Printing Extrinsics res...')
+            print(R)
+            print(T)
+            print(f'Euler angles in XYZ {r_euler} degs')    
+            R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
+                cameraMatrix_l,
+                distCoeff_l,
+                cameraMatrix_r,
+                distCoeff_r,
+                None, R, T) # , alpha=0.1
+            # self.P_l = P_l
+            # self.P_r = P_r
+            r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
+            r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
+
+            # print(f'P_l is \n {P_l}')
+            # print(f'P_r is \n {P_r}') 
+            return [ret, R, T, R_l, R_r, P_l, P_r]
+        #### ADD OTHER CALIBRATION METHODS ###
+    else:
+        if calibration._cameraModel == 'perspective':
+            return calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model, r_in, t_in)
+        elif calibration._cameraModel == 'fisheye':
+            return calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r)
+
+def calibrate_stereo_pair(calibration, left_cam, right_cam, board_config, features):
+    left_cam_info = board_config['cameras'][left_cam]
+    right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
+    print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
+        left_cam_info['name'], right_cam_info['name']))
+
+    specTranslation = left_cam_info['extrinsics']['specTranslation']
+    rot = left_cam_info['extrinsics']['rotation']
+
+    translation = np.array(
+        [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+    rotation = Rotation.from_euler(
+        'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+    if PER_CCM and EXTRINSICS_PER_CCM:
+        if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:
+            if left_cam_info["name"] in calibration.extrinsic_img:
+                array = calibration.extrinsic_img[left_cam_info["name"]]
+            elif right_cam_info["name"] in calibration.extrinsic_img:
+                array = calibration.extrinsic_img[left_cam_info["name"]]
+                
+            
+            left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = calibration.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+            right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = calibration.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+            removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = calibration.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
+            removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = calibration.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
+    
+    extrinsics = calibrate_stereo(calibration, left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
+
+    if extrinsics[0] == -1:
+        return -1, extrinsics[1]
+
+    if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
+        board_config['stereo_config']['rectification_left'] = extrinsics[3]
+        board_config['stereo_config']['rectification_right'] = extrinsics[4]
+
+    elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+        board_config['stereo_config']['rectification_left'] = extrinsics[4]
+        board_config['stereo_config']['rectification_right'] = extrinsics[3]
+
+    print('<-------------Epipolar error of {} and {} ------------>'.format(
+        left_cam_info['name'], right_cam_info['name']))
+    #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
+    #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
+    if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
+        scale = right_cam_info['intrinsics'][0][0]
+    else:
+        scale = left_cam_info['intrinsics'][0][0]
+    if PER_CCM and EXTRINSICS_PER_CCM:
+        scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
+        print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
+        left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
+        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
+    else:
+        print(f"Epipolar error {extrinsics[0]}")
+        left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
+        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
+
+    left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
+    left_cam_info['extrinsics']['translation'] = extrinsics[2]
 
 def load_camera_data(calibration, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
     width, height, img_path, calib_model, distortion_model, images_path = calibration.load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight)
@@ -300,6 +514,9 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         self.squaresY = squaresY
         self.squareSize = square_size
         self.markerSize = mrk_size
+        self.stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT +
+                                     cv2.TERM_CRITERIA_EPS, 300, 1e-9)
+
 
         self.cams = []
         features = None

From deec5d3780a39a849702decdc809726fc2968924 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 2 Aug 2024 15:54:57 +0200
Subject: [PATCH 38/87] Move stereo_calibrate contents up

---
 calibration_utils.py | 108 +++++++++++++++++++++++--------------------
 1 file changed, 57 insertions(+), 51 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 4bf39e2..64b0264 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -277,6 +277,36 @@ def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, rig
 
     return [ret, R, T, R_l, R_r, P_l, P_r]
 
+def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, r_in, t_in):
+    flags = cv2.CALIB_FIX_INTRINSIC
+    ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
+    obj_pts, left_corners_sampled, right_corners_sampled,
+    np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
+    R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
+
+    r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+    scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
+    print(f'Epipolar error without scale: {ret}')
+    print(f'Epipolar error with scale: {ret*np.sqrt(scale)}')
+    print('Printing Extrinsics res...')
+    print(R)
+    print(T)
+    print(f'Euler angles in XYZ {r_euler} degs')    
+    R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
+        cameraMatrix_l,
+        distCoeff_l,
+        cameraMatrix_r,
+        distCoeff_r,
+        None, R, T) # , alpha=0.1
+    # self.P_l = P_l
+    # self.P_r = P_r
+    r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
+    r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
+
+    # print(f'P_l is \n {P_l}')
+    # print(f'P_r is \n {P_r}') 
+    return [ret, R, T, R_l, R_r, P_l, P_r]
+
 def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r):
     # make sure all images have the same *number of* points
     min_num_points = min([len(pts) for pts in obj_pts])
@@ -348,57 +378,15 @@ def find_stereo_common_features(calibration, allIds_l, allIds_r, allCorners_l, a
             return -1, "Stereo Calib failed due to less common features"
     return left_corners_sampled, right_corners_sampled, obj_pts
 
-def calibrate_stereo(calibration, allIds_l, allCorners_l, allIds_r, allCorners_r, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, left_calib_model, right_calib_model, left_distortion_model, right_distortion_model, features = None):
-    left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, allIds_l, allIds_r, allCorners_l, allCorners_r)
-
-    if PER_CCM and EXTRINSICS_PER_CCM:
-        for i in range(len(left_corners_sampled)):
-            if left_calib_model == "perspective":
-                left_corners_sampled[i] = cv2.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
-            else:
-                left_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
-
-        for i in range(len(right_corners_sampled)):
-            if right_calib_model == "perspective":
-                right_corners_sampled[i] = cv2.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, distCoeff_r, P=cameraMatrix_r)
-            else:
-                right_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, distCoeff_r, P=cameraMatrix_r)
+def undistort_points_perspective(corners, R, d):
+    for i in range(len(corners)):
+        corners[i] = cv2.undistortPoints(np.array(corners[i]), R, d, P=R)
+    return corners
 
-        if features == None or features == "charucos": 
-            flags = cv2.CALIB_FIX_INTRINSIC
-            ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-            obj_pts, left_corners_sampled, right_corners_sampled,
-            np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
-            R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
-
-            r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-            scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
-            print(f'Epipolar error without scale: {ret}')
-            print(f'Epipolar error with scale: {ret*np.sqrt(scale)}')
-            print('Printing Extrinsics res...')
-            print(R)
-            print(T)
-            print(f'Euler angles in XYZ {r_euler} degs')    
-            R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
-                cameraMatrix_l,
-                distCoeff_l,
-                cameraMatrix_r,
-                distCoeff_r,
-                None, R, T) # , alpha=0.1
-            # self.P_l = P_l
-            # self.P_r = P_r
-            r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-            r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-
-            # print(f'P_l is \n {P_l}')
-            # print(f'P_r is \n {P_r}') 
-            return [ret, R, T, R_l, R_r, P_l, P_r]
-        #### ADD OTHER CALIBRATION METHODS ###
-    else:
-        if calibration._cameraModel == 'perspective':
-            return calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model, r_in, t_in)
-        elif calibration._cameraModel == 'fisheye':
-            return calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r)
+def undistort_points_fisheye(corners, R, d):
+    for i in range(len(corners)):
+        corners[i] = cv2.fisheye.undistortPoints(np.array(corners[i]), R, d, P=R)
+    return corners
 
 def calibrate_stereo_pair(calibration, left_cam, right_cam, board_config, features):
     left_cam_info = board_config['cameras'][left_cam]
@@ -426,7 +414,25 @@ def calibrate_stereo_pair(calibration, left_cam, right_cam, board_config, featur
             removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = calibration.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
             removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = calibration.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
     
-    extrinsics = calibrate_stereo(calibration, left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
+    #extrinsics = calibrate_stereo(calibration, left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
+    left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners'])
+
+    if PER_CCM and EXTRINSICS_PER_CCM:
+        if left_cam_info['calib_model'] == "perspective":
+            left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
+            right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+        else:
+            left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
+            right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+
+        if features == None or features == "charucos": 
+            extrinsics = calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], rotation, translation)
+        #### ADD OTHER CALIBRATION METHODS ###
+    else:
+        if calibration._cameraModel == 'perspective':
+            extrinsics = calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model'], rotation, translation)
+        elif calibration._cameraModel == 'fisheye':
+            extrinsics = calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
 
     if extrinsics[0] == -1:
         return -1, extrinsics[1]

From 5c6a26490bebba7fc6710beb0df80460c255c019 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 2 Aug 2024 16:15:58 +0200
Subject: [PATCH 39/87] Move stereo calibration into main function

---
 calibration_utils.py | 107 ++++++++++++++++++++++++-------------------
 1 file changed, 59 insertions(+), 48 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 64b0264..5ae2357 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -243,7 +243,15 @@ def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
 
     return cam_info
 
-def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model, r_in, t_in):
+def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model, left_cam_info):
+    specTranslation = left_cam_info['extrinsics']['specTranslation']
+    rot = left_cam_info['extrinsics']['rotation']
+
+    t_in = np.array(
+        [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+    r_in = Rotation.from_euler(
+        'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+    
     flags = 0
     # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
     # print(flags)
@@ -277,7 +285,15 @@ def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, rig
 
     return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, r_in, t_in):
+def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_cam_info):
+    specTranslation = left_cam_info['extrinsics']['specTranslation']
+    rot = left_cam_info['extrinsics']['rotation']
+
+    t_in = np.array(
+        [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+    r_in = Rotation.from_euler(
+        'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+
     flags = cv2.CALIB_FIX_INTRINSIC
     ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
     obj_pts, left_corners_sampled, right_corners_sampled,
@@ -388,51 +404,21 @@ def undistort_points_fisheye(corners, R, d):
         corners[i] = cv2.fisheye.undistortPoints(np.array(corners[i]), R, d, P=R)
     return corners
 
-def calibrate_stereo_pair(calibration, left_cam, right_cam, board_config, features):
-    left_cam_info = board_config['cameras'][left_cam]
-    right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
-    print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
-        left_cam_info['name'], right_cam_info['name']))
-
-    specTranslation = left_cam_info['extrinsics']['specTranslation']
-    rot = left_cam_info['extrinsics']['rotation']
-
-    translation = np.array(
-        [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-    rotation = Rotation.from_euler(
-        'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-    if PER_CCM and EXTRINSICS_PER_CCM:
-        if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:
-            if left_cam_info["name"] in calibration.extrinsic_img:
-                array = calibration.extrinsic_img[left_cam_info["name"]]
-            elif right_cam_info["name"] in calibration.extrinsic_img:
-                array = calibration.extrinsic_img[left_cam_info["name"]]
-                
+def remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info):
+    if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:
+        if left_cam_info["name"] in calibration.extrinsic_img:
+            array = calibration.extrinsic_img[left_cam_info["name"]]
+        elif right_cam_info["name"] in calibration.extrinsic_img:
+            array = calibration.extrinsic_img[left_cam_info["name"]]
             
-            left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = calibration.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-            right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = calibration.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-            removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = calibration.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
-            removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = calibration.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
-    
-    #extrinsics = calibrate_stereo(calibration, left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
-    left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners'])
-
-    if PER_CCM and EXTRINSICS_PER_CCM:
-        if left_cam_info['calib_model'] == "perspective":
-            left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
-            right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
-        else:
-            left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
-            right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+        
+        left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = calibration.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+        right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = calibration.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+        removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = calibration.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
+        removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = calibration.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
+    return left_cam_info, right_cam_info
 
-        if features == None or features == "charucos": 
-            extrinsics = calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], rotation, translation)
-        #### ADD OTHER CALIBRATION METHODS ###
-    else:
-        if calibration._cameraModel == 'perspective':
-            extrinsics = calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model'], rotation, translation)
-        elif calibration._cameraModel == 'fisheye':
-            extrinsics = calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam, board_config, extrinsics):
 
     if extrinsics[0] == -1:
         return -1, extrinsics[1]
@@ -457,11 +443,11 @@ def calibrate_stereo_pair(calibration, left_cam, right_cam, board_config, featur
         scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
         print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
         left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
+        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale) # TODO :Remove one of these
     else:
         print(f"Epipolar error {extrinsics[0]}")
         left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
+        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] # TODO : Remove one of these
 
     left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
     left_cam_info['extrinsics']['translation'] = extrinsics[2]
@@ -574,7 +560,32 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             
 
         for left, right in stereoPairs:
-            calibrate_stereo_pair(self, left, right, board_config, features)
+            calibration = self
+            left_cam_info = board_config['cameras'][left]
+            right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
+
+            if PER_CCM and EXTRINSICS_PER_CCM:
+                left_cam_info, right_cam_info = remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info)
+
+            left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners'])
+
+            if PER_CCM and EXTRINSICS_PER_CCM:
+                if left_cam_info['calib_model'] == "perspective":
+                    left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
+                    right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+                else:
+                    left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
+                    right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+
+                if features == None or features == "charucos": 
+                    extrinsics = calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info)
+                #### ADD OTHER CALIBRATION METHODS ###
+            else:
+                if calibration._cameraModel == 'perspective':
+                    extrinsics = calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model'], left_cam_info)
+                elif calibration._cameraModel == 'fisheye':
+                    extrinsics = calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+            calculate_epipolar_error(left_cam_info, right_cam_info, left, right, board_config, extrinsics)
 
         return 1, board_config
 

From 1f1ee94d6831bdde26c70d9fb12c544b87cbcac8 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 2 Aug 2024 16:23:12 +0200
Subject: [PATCH 40/87] Simplify arguments

---
 calibration_utils.py | 37 +++++++++++++++++++++----------------
 1 file changed, 21 insertions(+), 16 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 5ae2357..0e5a2c8 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -243,7 +243,8 @@ def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
 
     return cam_info
 
-def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model, left_cam_info):
+def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
+    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model']
     specTranslation = left_cam_info['extrinsics']['specTranslation']
     rot = left_cam_info['extrinsics']['rotation']
 
@@ -285,7 +286,8 @@ def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, rig
 
     return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_cam_info):
+def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
+    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
     specTranslation = left_cam_info['extrinsics']['specTranslation']
     rot = left_cam_info['extrinsics']['rotation']
 
@@ -323,7 +325,8 @@ def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_samp
     # print(f'P_r is \n {P_r}') 
     return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r):
+def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
+    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
     # make sure all images have the same *number of* points
     min_num_points = min([len(pts) for pts in obj_pts])
     obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts]
@@ -363,7 +366,8 @@ def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_c
 
     return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def find_stereo_common_features(calibration, allIds_l, allIds_r, allCorners_l, allCorners_r):
+def find_stereo_common_features(calibration, left_cam_info, right_cam_info):
+    allIds_l, allIds_r, allCorners_l, allCorners_r = left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners']
     left_corners_sampled = []
     right_corners_sampled = []
     left_ids_sampled = []
@@ -394,14 +398,14 @@ def find_stereo_common_features(calibration, allIds_l, allIds_r, allCorners_l, a
             return -1, "Stereo Calib failed due to less common features"
     return left_corners_sampled, right_corners_sampled, obj_pts
 
-def undistort_points_perspective(corners, R, d):
+def undistort_points_perspective(corners, camInfo):
     for i in range(len(corners)):
-        corners[i] = cv2.undistortPoints(np.array(corners[i]), R, d, P=R)
+        corners[i] = cv2.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
     return corners
 
-def undistort_points_fisheye(corners, R, d):
+def undistort_points_fisheye(corners, camInfo):
     for i in range(len(corners)):
-        corners[i] = cv2.fisheye.undistortPoints(np.array(corners[i]), R, d, P=R)
+        corners[i] = cv2.fisheye.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
     return corners
 
 def remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info):
@@ -451,6 +455,7 @@ def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam,
 
     left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
     left_cam_info['extrinsics']['translation'] = extrinsics[2]
+    return board_config
 
 def load_camera_data(calibration, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
     width, height, img_path, calib_model, distortion_model, images_path = calibration.load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight)
@@ -567,24 +572,24 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             if PER_CCM and EXTRINSICS_PER_CCM:
                 left_cam_info, right_cam_info = remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info)
 
-            left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners'])
+            left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, left_cam_info, right_cam_info)
 
             if PER_CCM and EXTRINSICS_PER_CCM:
                 if left_cam_info['calib_model'] == "perspective":
-                    left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
-                    right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+                    left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info)
+                    right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info)
                 else:
-                    left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'])
-                    right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+                    left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info)
+                    right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info)
 
                 if features == None or features == "charucos": 
-                    extrinsics = calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info)
+                    extrinsics = calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
                 #### ADD OTHER CALIBRATION METHODS ###
             else:
                 if calibration._cameraModel == 'perspective':
-                    extrinsics = calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model'], left_cam_info)
+                    extrinsics = calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
                 elif calibration._cameraModel == 'fisheye':
-                    extrinsics = calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'])
+                    extrinsics = calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
             calculate_epipolar_error(left_cam_info, right_cam_info, left, right, board_config, extrinsics)
 
         return 1, board_config

From ddad1a4626a2879d96b51bd4d73d4380ca3fddb8 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 2 Aug 2024 16:40:16 +0200
Subject: [PATCH 41/87] Parallelize stereo calibration

---
 calibration_utils.py | 41 +++++++++++++++++++----------------------
 1 file changed, 19 insertions(+), 22 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 0e5a2c8..0420ae0 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -539,7 +539,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             cam_info = load_camera_data(self, filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
 
         tasks = []
-        camInfos = []
+        camInfos = {}
         pw = ParallelWorker(16)
         if PER_CCM:
             for cam, cam_info in activeCameras:
@@ -554,43 +554,40 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     ret2 = pw.run(calibrate_charuco, self, ret[0], featuresAndIds[0], featuresAndIds[1])
                 ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, self, features, ret2, featuresAndIds[0], featuresAndIds[1])
                 tasks.extend([ret, ret2, ret3, featuresAndIds])
-                camInfos.append((cam, ret3))
+                camInfos[cam] = ret3
         else:
             for cam, cam_info in activeCameras:
                 cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
 
-        pw.execute()
-        for cam, cam_info in camInfos:
-            board_config['cameras'][cam] = cam_info.ret()
-            
-
         for left, right in stereoPairs:
-            calibration = self
-            left_cam_info = board_config['cameras'][left]
-            right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
+            left_cam_info = camInfos[left]
+            right_cam_info = camInfos[right]
 
             if PER_CCM and EXTRINSICS_PER_CCM:
-                left_cam_info, right_cam_info = remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info)
+                left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, left_cam_info, right_cam_info)
 
-            left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(calibration, left_cam_info, right_cam_info)
+            ret = pw.run(find_stereo_common_features, self, left_cam_info, right_cam_info)
+            left_corners_sampled, right_corners_sampled, obj_pts = ret[0], ret[1], ret[2]
 
             if PER_CCM and EXTRINSICS_PER_CCM:
                 if left_cam_info['calib_model'] == "perspective":
-                    left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info)
-                    right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info)
+                    left_corners_sampled = pw.run(undistort_points_perspective, left_corners_sampled, left_cam_info)
+                    right_corners_sampled = pw.run(undistort_points_perspective, right_corners_sampled, right_cam_info)
                 else:
-                    left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info)
-                    right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info)
+                    left_corners_sampled = pw.run(undistort_points_fisheye, left_corners_sampled, left_cam_info)
+                    right_corners_sampled = pw.run(undistort_points_fisheye, right_corners_sampled, right_cam_info)
 
                 if features == None or features == "charucos": 
-                    extrinsics = calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+                    extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
                 #### ADD OTHER CALIBRATION METHODS ###
             else:
-                if calibration._cameraModel == 'perspective':
-                    extrinsics = calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-                elif calibration._cameraModel == 'fisheye':
-                    extrinsics = calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-            calculate_epipolar_error(left_cam_info, right_cam_info, left, right, board_config, extrinsics)
+                if self._cameraModel == 'perspective':
+                    extrinsics = pw.run(calibrate_stereo_perspective, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+                elif self._cameraModel == 'fisheye':
+                    extrinsics = pw.run(calibrate_stereo_fisheye, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+            pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
+            
+        pw.execute()
 
         return 1, board_config
 

From 1daafc834ac3522e6c5ae4422fae85642e033a8e Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 5 Aug 2024 16:31:58 +0200
Subject: [PATCH 42/87] Return all data into board config

---
 calibration_utils.py | 35 +++++++++++++++++++++++++----------
 1 file changed, 25 insertions(+), 10 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 0420ae0..5458ca9 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -426,14 +426,17 @@ def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam,
 
     if extrinsics[0] == -1:
         return -1, extrinsics[1]
-
-    if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
-        board_config['stereo_config']['rectification_left'] = extrinsics[3]
-        board_config['stereo_config']['rectification_right'] = extrinsics[4]
-
+    stereoConfig = None
+    if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: # TODO : Is this supposed to take the last camera pair?
+        stereoConfig = {
+            'rectification_left': extrinsics[3],
+            'rectification_right': extrinsics[4]
+        }
     elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-        board_config['stereo_config']['rectification_left'] = extrinsics[4]
-        board_config['stereo_config']['rectification_right'] = extrinsics[3]
+        stereoConfig = {
+            'rectification_left': extrinsics[4],
+            'rectification_right': extrinsics[3]
+        }
 
     print('<-------------Epipolar error of {} and {} ------------>'.format(
         left_cam_info['name'], right_cam_info['name']))
@@ -455,7 +458,8 @@ def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam,
 
     left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
     left_cam_info['extrinsics']['translation'] = extrinsics[2]
-    return board_config
+
+    return left_cam_info, stereoConfig
 
 def load_camera_data(calibration, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
     width, height, img_path, calib_model, distortion_model, images_path = calibration.load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight)
@@ -540,6 +544,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         tasks = []
         camInfos = {}
+        stereoConfigs = []
         pw = ParallelWorker(16)
         if PER_CCM:
             for cam, cam_info in activeCameras:
@@ -585,10 +590,20 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                     extrinsics = pw.run(calibrate_stereo_perspective, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
                 elif self._cameraModel == 'fisheye':
                     extrinsics = pw.run(calibrate_stereo_fisheye, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-            pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
-            
+            ret4 = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
+            camInfos[left] = ret4[0]
+            stereoConfigs.append(ret4[1])
+        
         pw.execute()
 
+        # Extract camera info structs and stereo config
+        for cam, camInfo in camInfos.items():
+            board_config['cameras'][cam] = camInfo.ret()
+        
+        for stereoConfig in stereoConfigs:
+            if stereoConfig.ret():
+                board_config['stereo_config'].update(stereoConfig.ret())
+
         return 1, board_config
 
     def load_camera_data(self, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):

From 533a97bd852acc79c29341c5c17ab3eff792a462 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 09:33:26 +0200
Subject: [PATCH 43/87] Remove rectification display

---
 calibration_utils.py | 39 ---------------------------------------
 1 file changed, 39 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 5458ca9..1cfcc83 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -507,7 +507,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 extrinsic_img[cam].sort(reverse=True)
         self.intrinsic_img = intrinsic_img
         self.extrinsic_img = extrinsic_img
-        self._enable_rectification_disp = True
         self._cameraModel = camera_model
         self._data_path = filepath
         self._proxyDict = ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000)
@@ -552,9 +551,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
                 if self._cameraModel == "fisheye":
                     ret2 = pw.run(filter_features_fisheye, self, ret[0], intrinsic_img, ret[1], ret[2])
                 else:
-                    #featuresAndIds = ParallelTask(estimate_pose_and_filter, [self, *ret])
                     featuresAndIds = pw.map(estimate_pose_and_filter_single, self, ret[0], ret[1], ret[2])
-        #estimate_pose_and_filter_single(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
                     
                     ret2 = pw.run(calibrate_charuco, self, ret[0], featuresAndIds[0], featuresAndIds[1])
                 ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, self, features, ret2, featuresAndIds[0], featuresAndIds[1])
@@ -1547,39 +1544,6 @@ def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds
 
                 return [ret, R, T, R_l, R_r, P_l, P_r]
 
-    def display_rectification(self, image_data_pairs, images_corners_l, images_corners_r, image_epipolar_color, isHorizontal):
-        print(
-            "Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.")
-        for idx, image_data_pair in enumerate(image_data_pairs):
-            if isHorizontal:
-                img_concat = cv2.hconcat(
-                    [image_data_pair[0], image_data_pair[1]])
-                for left_pt, right_pt, colorMode in zip(images_corners_l[idx], images_corners_r[idx], image_epipolar_color[idx]):
-                    cv2.line(img_concat,
-                             (int(left_pt[0][0]), int(left_pt[0][1])), (int(right_pt[0][0]) + image_data_pair[0].shape[1], int(right_pt[0][1])),
-                             colors[colorMode], 1)
-            else:
-                img_concat = cv2.vconcat(
-                    [image_data_pair[0], image_data_pair[1]])
-                for left_pt, right_pt, colorMode in zip(images_corners_l[idx], images_corners_r[idx], image_epipolar_color[idx]):
-                    cv2.line(img_concat,
-                             (int(left_pt[0][0]), int(left_pt[0][1])), (int(right_pt[0][0]), int(right_pt[0][1])  + image_data_pair[0].shape[0]),
-                             colors[colorMode], 1)
-
-            img_concat = cv2.resize(
-                img_concat, (0, 0), fx=0.8, fy=0.8)
-
-            # show image
-            cv2.imshow('Stereo Pair', img_concat)
-            k = cv2.waitKey(1)
-            if k == 27:  # Esc key to stop
-                break
-
-                # os._exit(0)
-                # raise SystemExit()
-
-        cv2.destroyWindow('Stereo Pair')
-
     def scale_image(self, img, scaled_res):
         expected_height = img.shape[0]*(scaled_res[1]/img.shape[1])
 
@@ -1900,9 +1864,6 @@ def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_p
         avg_epipolar = epi_error_sum / total_corners
         print("Average Epipolar Error is : " + str(avg_epipolar))
 
-        if self._enable_rectification_disp:
-            self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal)
-
         return avg_epipolar
 
     def create_save_mesh(self):  # , output_path):

From 4c58ec1144807fa013f40a7dda94bd3984a69e9d Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 09:52:46 +0200
Subject: [PATCH 44/87] Remove unused functions

---
 calibration_utils.py | 726 +++----------------------------------------
 1 file changed, 36 insertions(+), 690 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 1cfcc83..4a30c7d 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -461,8 +461,40 @@ def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam,
 
     return left_cam_info, stereoConfig
 
-def load_camera_data(calibration, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
-    width, height, img_path, calib_model, distortion_model, images_path = calibration.load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight)
+def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
+    images_path = filepath + '/' + cam_info['name']
+    image_files = glob.glob(images_path + "/*")
+    image_files.sort()
+    for im in image_files:
+        frame = cv2.imread(im)
+        height, width, _ = frame.shape
+        widthRatio = resizeWidth / width
+        heightRatio = resizeHeight / height
+        if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
+            resizeWidth = width
+            resizeHeight = height
+        break
+
+    images_path = filepath + '/' + cam_info['name']
+    if "calib_model" in cam_info:
+        cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
+        if cameraModel_ccm == "fisheye":
+            model_ccm == None
+        calib_model = cameraModel_ccm
+        distortion_model = model_ccm
+    else:
+        calib_model = _cameraModel
+        if cam_info["name"] in ccm_model:
+            distortion_model = ccm_model[cam_info["name"]]
+        else:
+            distortion_model = model
+
+    img_path = glob.glob(images_path + "/*")
+    if charucos == {}:
+        img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+    else:
+        img_path.sort()
+
     cam_info['width'] = width
     cam_info['height'] = height
     cam_info['calib_model'] = calib_model
@@ -539,7 +571,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
             stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
 
         for cam, cam_info in activeCameras:
-            cam_info = load_camera_data(self, filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
+            cam_info = load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
 
         tasks = []
         camInfos = {}
@@ -603,122 +635,6 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
         return 1, board_config
 
-    def load_camera_data(self, filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
-        images_path = filepath + '/' + cam_info['name']
-        image_files = glob.glob(images_path + "/*")
-        image_files.sort()
-        for im in image_files:
-            frame = cv2.imread(im)
-            height, width, _ = frame.shape
-            widthRatio = resizeWidth / width
-            heightRatio = resizeHeight / height
-            if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-                resizeWidth = width
-                resizeHeight = height
-            break
-
-        images_path = filepath + '/' + cam_info['name']
-        if "calib_model" in cam_info:
-            cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
-            if cameraModel_ccm == "fisheye":
-                model_ccm == None
-            calib_model = cameraModel_ccm
-            distortion_model = model_ccm
-        else:
-            calib_model = _cameraModel
-            if cam_info["name"] in ccm_model:
-                distortion_model = ccm_model[cam_info["name"]]
-            else:
-                distortion_model = model
-
-        img_path = glob.glob(images_path + "/*")
-        if charucos == {}:
-            img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
-        else:
-            img_path.sort()
-
-        return width, height, img_path, calib_model, distortion_model, images_path
-
-    def calibrate_stereo_pair(self, left_cam, right_cam, board_config, features):
-        left_cam_info = board_config['cameras'][left_cam]
-        right_cam_info = board_config['cameras'][left_cam_info['extrinsics']['to_cam']]
-        print('<-------------Extrinsics calibration of {} and {} ------------>'.format(
-            left_cam_info['name'], right_cam_info['name']))
-
-        specTranslation = left_cam_info['extrinsics']['specTranslation']
-        rot = left_cam_info['extrinsics']['rotation']
-
-        translation = np.array(
-            [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-        rotation = Rotation.from_euler(
-            'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-        if PER_CCM and EXTRINSICS_PER_CCM:
-            if left_cam_info["name"] in self.extrinsic_img or right_cam_info["name"] in self.extrinsic_img:
-                if left_cam_info["name"] in self.extrinsic_img:
-                    array = self.extrinsic_img[left_cam_info["name"]]
-                elif right_cam_info["name"] in self.extrinsic_img:
-                    array = self.extrinsic_img[left_cam_info["name"]]
-                    
-                    
-                    
-                left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = self.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-                right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = self.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-                removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = self.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
-                removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = self.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
-        
-        extrinsics = self.calibrate_stereo(left_cam_info['name'], right_cam_info['name'], left_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info['filtered_corners'], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], translation, rotation, left_cam_info['calib_model'], right_cam_info['calib_model'], left_cam_info['distortion_model'], right_cam_info['distortion_model'], features)
-        if extrinsics[0] == -1:
-            return -1, extrinsics[1]
-
-        if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam:
-            board_config['stereo_config']['rectification_left'] = extrinsics[3]
-            board_config['stereo_config']['rectification_right'] = extrinsics[4]
-
-        elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-            board_config['stereo_config']['rectification_left'] = extrinsics[4]
-            board_config['stereo_config']['rectification_right'] = extrinsics[3]
-
-        """ for stereoObj in board_config['stereo_config']:
-
-            if stereoObj['left_cam'] == left_cam and stereoObj['right_cam'] == right_cam and stereoObj['main'] == 1:
-                stereoObj['rectification_left'] = extrinsics[3]
-                stereoObj['rectification_right'] = extrinsics[4] """
-
-        print('<-------------Epipolar error of {} and {} ------------>'.format(
-            left_cam_info['name'], right_cam_info['name']))
-        #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
-        #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
-        if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
-            scale = right_cam_info['intrinsics'][0][0]
-        else:
-            scale = left_cam_info['intrinsics'][0][0]
-        if PER_CCM and EXTRINSICS_PER_CCM:
-            scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
-            print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
-            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale)
-        else:
-            print(f"Epipolar error {extrinsics[0]}")
-            left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-            left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]
-        """self.test_epipolar_charuco(left_cam_info['name'], 
-                                    right_cam_info['name'],
-                                    left_path, 
-                                    right_path, 
-                                    left_cam_info['intrinsics'], 
-                                    left_cam_info['dist_coeff'], 
-                                    right_cam_info['intrinsics'], 
-                                    right_cam_info['dist_coeff'], 
-                                    extrinsics[2], # Translation between left and right Cameras
-                                    extrinsics[3], # Left Rectification rotation 
-                                    extrinsics[4], # Right Rectification rotation
-                                    calibModels[left_cam_info['name']], calibModels[right_cam_info['name']]
-                                    )"""
-                                                                            
-
-        left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
-        left_cam_info['extrinsics']['translation'] = extrinsics[2]
-
     def getting_features(self, img_path, width, height, features = None, charucos=None):
         if charucos:
             allCorners = []
@@ -1147,22 +1063,6 @@ def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height,
             # (Height, width)
             return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
 
-
-    def scale_intrinsics(self, intrinsics, originalShape, destShape):
-        scale = destShape[1] / originalShape[1] # scale on width
-        scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]])
-        scaled_intrinsics = np.matmul(scale_mat, intrinsics)
-        """ print("Scaled height offset : {}".format(
-            (originalShape[0] * scale - destShape[0]) / 2))
-        print("Scaled width offset : {}".format(
-            (originalShape[1] * scale - destShape[1]) / 2)) """
-        scaled_intrinsics[1][2] -= (originalShape[0]      # c_y - along height of the image
-                                    * scale - destShape[0]) / 2
-        scaled_intrinsics[0][2] -= (originalShape[1]     # c_x width of the image
-                                    * scale - destShape[1]) / 2
-
-        return scaled_intrinsics
-
     def undistort_visualization(self, img_list, K, D, img_size, name):
         for index, im in enumerate(img_list):
             # print(im)
@@ -1386,558 +1286,4 @@ def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
                 except:
                     print(f"Failed the full res calib, using calibration with crop factor {crop}")
         
-        return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
-
-
-    def calibrate_stereo(self, left_name, right_name, allIds_l, allCorners_l, allIds_r, allCorners_r, cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, t_in, r_in, left_calib_model, right_calib_model, left_distortion_model, right_distortion_model, features = None):
-        left_corners_sampled = []
-        right_corners_sampled = []
-        left_ids_sampled = []
-        obj_pts = []
-        one_pts = self._board.chessboardCorners
-
-        for i in range(len(allIds_l)):
-            left_sub_corners = []
-            right_sub_corners = []
-            obj_pts_sub = []
-            #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70:
-            #      continue
-            for j in range(len(allIds_l[i])):
-                idx = np.where(allIds_r[i] == allIds_l[i][j])
-                if idx[0].size == 0:
-                    continue
-                left_sub_corners.append(allCorners_l[i][j])
-                right_sub_corners.append(allCorners_r[i][idx])
-                obj_pts_sub.append(one_pts[allIds_l[i][j]])
-            if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
-                obj_pts.append(np.array(obj_pts_sub, dtype=np.float32))
-                left_corners_sampled.append(
-                    np.array(left_sub_corners, dtype=np.float32))
-                left_ids_sampled.append(np.array(allIds_l[i], dtype=np.int32))
-                right_corners_sampled.append(
-                    np.array(right_sub_corners, dtype=np.float32))
-            else:
-                return -1, "Stereo Calib failed due to less common features"
-
-        stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT +
-                                cv2.TERM_CRITERIA_EPS, 300, 1e-9)
-        if PER_CCM and EXTRINSICS_PER_CCM:
-            for i in range(len(left_corners_sampled)):
-                if left_calib_model == "perspective":
-                    left_corners_sampled[i] = cv2.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
-                    #left_corners_sampled[i] = cv2.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, None)
-
-                else:
-                    left_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, distCoeff_l, P=cameraMatrix_l)
-                    #left_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(left_corners_sampled[i]), cameraMatrix_l, None)
-            for i in range(len(right_corners_sampled)):
-                if right_calib_model == "perspective":
-                    right_corners_sampled[i] = cv2.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, distCoeff_r, P=cameraMatrix_r)
-                    #right_corners_sampled[i] = cv2.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, None)
-                else:
-                    right_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, distCoeff_r, P=cameraMatrix_r)
-                    #right_corners_sampled[i] = cv2.fisheye.undistortPoints(np.array(right_corners_sampled[i]), cameraMatrix_r, None)
-
-            if features == None or features == "charucos": 
-                flags = cv2.CALIB_FIX_INTRINSIC
-                ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-                obj_pts, left_corners_sampled, right_corners_sampled,
-                np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
-                R=r_in, T=t_in, criteria=stereocalib_criteria , flags=flags)
-
-                r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-                scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
-                print(f'Epipolar error without scale: {ret}')
-                print(f'Epipolar error with scale: {ret*np.sqrt(scale)}')
-                print('Printing Extrinsics res...')
-                print(R)
-                print(T)
-                print(f'Euler angles in XYZ {r_euler} degs')    
-                R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
-                    cameraMatrix_l,
-                    distCoeff_l,
-                    cameraMatrix_r,
-                    distCoeff_r,
-                    None, R, T) # , alpha=0.1
-                # self.P_l = P_l
-                # self.P_r = P_r
-                r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-                r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-
-                # print(f'P_l is \n {P_l}')
-                # print(f'P_r is \n {P_r}') 
-                return [ret, R, T, R_l, R_r, P_l, P_r]
-            #### ADD OTHER CALIBRATION METHODS ###
-        else:
-            if self._cameraModel == 'perspective':
-                flags = 0
-                # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
-                # print(flags)
-                flags = cv2.CALIB_FIX_INTRINSIC
-                distortion_flags = self.get_distortion_flags(left_distortion_model)
-                flags += distortion_flags
-                # print(flags)
-                ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-                obj_pts, left_corners_sampled, right_corners_sampled,
-                cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-                R=r_in, T=t_in, criteria=stereocalib_criteria , flags=flags)
-
-                r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-                print(f'Epipolar error is {ret}')
-                print('Printing Extrinsics res...')
-                print(R)
-                print(T)
-                print(f'Euler angles in XYZ {r_euler} degs')
-
-
-                R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
-                    cameraMatrix_l,
-                    distCoeff_l,
-                    cameraMatrix_r,
-                    distCoeff_r,
-                    None, R, T) # , alpha=0.1
-                # self.P_l = P_l
-                # self.P_r = P_r
-                r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-                r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-
-                return [ret, R, T, R_l, R_r, P_l, P_r]
-            elif self._cameraModel == 'fisheye':
-                # make sure all images have the same *number of* points
-                min_num_points = min([len(pts) for pts in obj_pts])
-                obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts]
-                left_corners_truncated = [pts[:min_num_points] for pts in left_corners_sampled]
-                right_corners_truncated = [pts[:min_num_points] for pts in right_corners_sampled]
-
-                flags = 0
-                # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
-                # flags |= cv2.fisheye.CALIB_CHECK_COND
-                # flags |= cv2.fisheye.CALIB_FIX_SKEW
-                flags |= cv2.fisheye.CALIB_FIX_INTRINSIC
-                # flags |= cv2.fisheye.CALIB_FIX_K1
-                # flags |= cv2.fisheye.CALIB_FIX_K2
-                # flags |= cv2.fisheye.CALIB_FIX_K3 
-                # flags |= cv2.fisheye.CALIB_FIX_K4
-                # flags |= cv2.CALIB_RATIONAL_MODEL
-                # TODO(sACHIN): Try without intrinsic guess
-                # flags |= cv2.CALIB_USE_INTRINSIC_GUESS
-                # TODO(sACHIN): Try without intrinsic guess
-                # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
-                # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW
-                (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate(
-                    obj_pts_truncated, left_corners_truncated, right_corners_truncated,
-                    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-                    flags=flags, criteria=stereocalib_criteria), None, None
-                r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-                print(f'Reprojection error is {ret}')
-                isHorizontal = np.absolute(T[0]) > np.absolute(T[1])
-
-                R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify(
-                    cameraMatrix_l,
-                    distCoeff_l,
-                    cameraMatrix_r,
-                    distCoeff_r,
-                    None, R, T, flags=0)
-
-                r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-                r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-
-                return [ret, R, T, R_l, R_r, P_l, P_r]
-
-    def scale_image(self, img, scaled_res):
-        expected_height = img.shape[0]*(scaled_res[1]/img.shape[1])
-
-        if not (img.shape[0] == scaled_res[0] and img.shape[1] == scaled_res[1]):
-            if int(expected_height) == scaled_res[0]:
-                # resizing to have both stereo and rgb to have same
-                # resolution to capture extrinsics of the rgb-right camera
-                img = cv2.resize(img, (scaled_res[1], scaled_res[0]),
-                                 interpolation=cv2.INTER_CUBIC)
-                return img
-            else:
-                # resizing and cropping to have both stereo and rgb to have same resolution
-                # to calculate extrinsics of the rgb-right camera
-                scale_width = scaled_res[1]/img.shape[1]
-                dest_res = (
-                    int(img.shape[1] * scale_width), int(img.shape[0] * scale_width))
-                img = cv2.resize(
-                    img, dest_res, interpolation=cv2.INTER_CUBIC)
-                if img.shape[0] < scaled_res[0]:
-                    raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
-                        img.shape[0], scaled_res[0]))
-                # print(gray.shape[0] - req_resolution[0])
-                del_height = (img.shape[0] - scaled_res[0]) // 2
-                # gray = gray[: req_resolution[0], :]
-                img = img[del_height: del_height + scaled_res[0], :]
-                return img
-        else:
-            return img
-    
-    def sgdEpipolar(self, images_left, images_right, M_lp, d_l, M_rp, d_r, r_l, r_r, kScaledL, kScaledR, scaled_res, isHorizontal):
-        if self._cameraModel == 'perspective':
-            mapx_l, mapy_l = cv2.initUndistortRectifyMap(
-                M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
-            mapx_r, mapy_r = cv2.initUndistortRectifyMap(
-                M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
-        else:
-            mapx_l, mapy_l = cv2.fisheye.initUndistortRectifyMap(
-                M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
-            mapx_r, mapy_r = cv2.fisheye.initUndistortRectifyMap(
-                M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
-
-        
-        image_data_pairs = []
-        imagesCount = 0
-
-        for image_left, image_right in zip(images_left, images_right):
-            # read images
-            imagesCount += 1
-            # print(imagesCount)
-            img_l = cv2.imread(image_left, 0)
-            img_r = cv2.imread(image_right, 0)
-
-            img_l = self.scale_image(img_l, scaled_res)
-            img_r = self.scale_image(img_r, scaled_res)
-
-            # warp right image
-            # img_l = cv2.warpPerspective(img_l, self.H1, img_l.shape[::-1],
-            #                             cv2.INTER_CUBIC +
-            #                             cv2.WARP_FILL_OUTLIERS +
-            #                             cv2.WARP_INVERSE_MAP)
-
-            # img_r = cv2.warpPerspective(img_r, self.H2, img_r.shape[::-1],
-            #                             cv2.INTER_CUBIC +
-            #                             cv2.WARP_FILL_OUTLIERS +
-            #                             cv2.WARP_INVERSE_MAP)
-
-            img_l = cv2.remap(img_l, mapx_l, mapy_l, cv2.INTER_LINEAR)
-            img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR)
-
-            image_data_pairs.append((img_l, img_r))
-
-        imgpoints_r = []
-        imgpoints_l = []
-        criteria = (cv2.TERM_CRITERIA_EPS +
-                    cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
-            
-        for i, image_data_pair in enumerate(image_data_pairs):
-            res2_l = self.detect_charuco_board(image_data_pair[0])
-            res2_r = self.detect_charuco_board(image_data_pair[1])
-
-            if res2_l[1] is not None and res2_r[2] is not None and len(res2_l[1]) > 3 and len(res2_r[1]) > 3:
-
-                cv2.cornerSubPix(image_data_pair[0], res2_l[1], 
-                                 winSize=(5, 5),
-                                 zeroZone=(-1, -1),
-                                 criteria=criteria)
-                cv2.cornerSubPix(image_data_pair[1], res2_r[1],
-                                 winSize=(5, 5),
-                                 zeroZone=(-1, -1),
-                                 criteria=criteria)
-
-                # termination criteria
-                img_pth_right = Path(images_right[i])
-                img_pth_left = Path(images_left[i])
-                org = (100, 50)
-                # cv2.imshow('ltext', lText)
-                # cv2.waitKey(0)
-                localError = 0
-                corners_l = []
-                corners_r = []
-                for j in range(len(res2_l[2])):
-                    idx = np.where(res2_r[2] == res2_l[2][j])
-                    if idx[0].size == 0:
-                        continue
-                    corners_l.append(res2_l[1][j])
-                    corners_r.append(res2_r[1][idx])
-
-                imgpoints_l.extend(corners_l)
-                imgpoints_r.extend(corners_r)
-                epi_error_sum = 0
-                for l_pt, r_pt in zip(corners_l, corners_r):
-                    if isHorizontal:
-                        epi_error_sum += abs(l_pt[0][1] - r_pt[0][1])
-                    else:
-                        epi_error_sum += abs(l_pt[0][0] - r_pt[0][0])
-                # localError = epi_error_sum / len(corners_l)
-
-                # print("Average Epipolar in test Error per image on host in " + img_pth_right.name + " : " +
-                #       str(localError))
-                raise SystemExit(1)
-
-        epi_error_sum = 0
-        for l_pt, r_pt in zip(imgpoints_l, imgpoints_r):
-            if isHorizontal:
-                epi_error_sum += abs(l_pt[0][1] - r_pt[0][1])
-            else:
-                epi_error_sum += abs(l_pt[0][0] - r_pt[0][0])
-
-        avg_epipolar = epi_error_sum / len(imgpoints_r)
-        print("Average Epipolar Error in test is : " + str(avg_epipolar))
-        return avg_epipolar
-
-
-    def test_epipolar_charuco(self, left_name, right_name, left_img_pth, right_img_pth, M_l, d_l, M_r, d_r, t, r_l, r_r, left_calib_model, right_calib_model):
-        images_left = glob.glob(left_img_pth + '/*.png')
-        images_right = glob.glob(right_img_pth + '/*.png')
-        images_left.sort()
-        print(images_left)
-        images_right.sort()
-        assert len(images_left) != 0, "ERROR: Images not read correctly"
-        assert len(images_right) != 0, "ERROR: Images not read correctly"
-        isHorizontal = np.absolute(t[0]) > np.absolute(t[1])
-
-        scale = None
-        scale_req = False
-        frame_left_shape = cv2.imread(images_left[0], 0).shape
-        frame_right_shape = cv2.imread(images_right[0], 0).shape
-        scalable_res = frame_left_shape
-        scaled_res = frame_right_shape
-        if frame_right_shape[0] < frame_left_shape[0] and frame_right_shape[1] < frame_left_shape[1]:
-            scale_req = True
-            scale = frame_right_shape[1] / frame_left_shape[1]
-        elif frame_right_shape[0] > frame_left_shape[0] and frame_right_shape[1] > frame_left_shape[1]:
-            scale_req = True
-            scale = frame_left_shape[1] / frame_right_shape[1]
-            scalable_res = frame_right_shape
-            scaled_res = frame_left_shape
-
-        if scale_req:
-            scaled_height = scale * scalable_res[0]
-            diff = scaled_height - scaled_res[0]
-            if diff < 0:
-                scaled_res = (int(scaled_height), scaled_res[1])
-        # print("Scale res :{}".format(scaled_res))
-
-        M_lp = self.scale_intrinsics(M_l, frame_left_shape, scaled_res)
-        M_rp = self.scale_intrinsics(M_r, frame_right_shape, scaled_res)
-
-        criteria = (cv2.TERM_CRITERIA_EPS +
-                    cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
-
-        # TODO(Sachin): Observe Images by adding visualization 
-        # TODO(Sachin): Check if the stetch is only in calibration Images
-        print('Original intrinsics ....')
-        print(f"L {M_lp}")
-        print(f"R: {M_rp}")
-        # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
-        # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0)
-        # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
-        kScaledR = kScaledL = M_rp
-
-        # if self.cameraModel != 'perspective':
-        #     kScaledR = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(M_r, d_r, scaled_res[::-1], np.eye(3), fov_scale=1.1)
-        #     kScaledL = kScaledR
-
-            
-        print('Intrinsics from the getOptimalNewCameraMatrix/Original ....')
-        print(f"L: {kScaledL}")
-        print(f"R: {kScaledR}")
-        oldEpipolarError = None
-        epQueue = deque()
-        movePos = True
-
-
-        print(left_calib_model, right_calib_model)
-        if left_calib_model == 'perspective':
-            mapx_l, mapy_l = cv2.initUndistortRectifyMap(
-                M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
-        else:
-            mapx_l, mapy_l = cv2.fisheye.initUndistortRectifyMap(
-                M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
-        if right_calib_model == "perspective":
-            mapx_r, mapy_r = cv2.initUndistortRectifyMap(
-                M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
-        else:
-            mapx_r, mapy_r = cv2.fisheye.initUndistortRectifyMap(
-                M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
-
-        image_data_pairs = []
-        for image_left, image_right in zip(images_left, images_right):
-            # read images
-            img_l = cv2.imread(image_left, 0)
-            img_r = cv2.imread(image_right, 0)
-
-            img_l = self.scale_image(img_l, scaled_res)
-            img_r = self.scale_image(img_r, scaled_res)
-            # print(img_l.shape)
-            # print(img_r.shape)
-
-            # warp right image
-            # img_l = cv2.warpPerspective(img_l, self.H1, img_l.shape[::-1],
-            #                             cv2.INTER_CUBIC +
-            #                             cv2.WARP_FILL_OUTLIERS +
-            #                             cv2.WARP_INVERSE_MAP)
-
-            # img_r = cv2.warpPerspective(img_r, self.H2, img_r.shape[::-1],
-            #                             cv2.INTER_CUBIC +
-            #                             cv2.WARP_FILL_OUTLIERS +
-            #                             cv2.WARP_INVERSE_MAP)
-
-            img_l = cv2.remap(img_l, mapx_l, mapy_l, cv2.INTER_LINEAR)
-            img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR)
-
-            image_data_pairs.append((img_l, img_r))
-            
-        # compute metrics
-        imgpoints_r = []
-        imgpoints_l = []
-        image_epipolar_color = []
-        # new_imagePairs = [])
-        for i, image_data_pair in enumerate(image_data_pairs):
-            res2_l = self.detect_charuco_board(image_data_pair[0])
-            res2_r = self.detect_charuco_board(image_data_pair[1])
-
-            img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]])
-            img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB)
-            line_row = 0
-            while line_row < img_concat.shape[0]:
-                cv2.line(img_concat,
-                         (0, line_row), (img_concat.shape[1], line_row),
-                         (0, 255, 0), 1)
-                line_row += 30
-
-            cv2.imshow('Stereo Pair', img_concat)
-            k = cv2.waitKey(1)
-            
-            if res2_l[1] is not None and res2_r[2] is not None and len(res2_l[1]) > 3 and len(res2_r[1]) > 3:
-
-                cv2.cornerSubPix(image_data_pair[0], res2_l[1],
-                                 winSize=(5, 5),
-                                 zeroZone=(-1, -1),
-                                 criteria=criteria)
-                cv2.cornerSubPix(image_data_pair[1], res2_r[1],
-                                 winSize=(5, 5),
-                                 zeroZone=(-1, -1),
-                                 criteria=criteria)
-
-                # termination criteria
-                img_pth_right = Path(images_right[i])
-                img_pth_left = Path(images_left[i])
-                org = (100, 50)
-                # cv2.imshow('ltext', lText)
-                # cv2.waitKey(0)
-                localError = 0
-                corners_l = []
-                corners_r = []
-                for j in range(len(res2_l[2])):
-                    idx = np.where(res2_r[2] == res2_l[2][j])
-                    if idx[0].size == 0:
-                        continue
-                    corners_l.append(res2_l[1][j])
-                    corners_r.append(res2_r[1][idx])
-
-                imgpoints_l.append(corners_l)
-                imgpoints_r.append(corners_r)
-                epi_error_sum = 0
-                corner_epipolar_color = []
-                for l_pt, r_pt in zip(corners_l, corners_r):
-                    if isHorizontal:
-                        curr_epipolar_error = abs(l_pt[0][1] - r_pt[0][1])
-                    else:
-                        curr_epipolar_error = abs(l_pt[0][0] - r_pt[0][0])
-                    if curr_epipolar_error >= 1:
-                        corner_epipolar_color.append(1)
-                    else:
-                        corner_epipolar_color.append(0)
-                    epi_error_sum += curr_epipolar_error
-                localError = epi_error_sum / len(corners_l)
-                image_epipolar_color.append(corner_epipolar_color)
-            else:
-                print('Numer of corners is in left -> and right ->')
-                return -1
-            lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
-            rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
-            image_data_pairs[i] = (lText, rText)
-
-
-        epi_error_sum = 0
-        total_corners = 0
-        for corners_l, corners_r in zip(imgpoints_l, imgpoints_r):
-            total_corners += len(corners_l)
-            for l_pt, r_pt in zip(corners_l, corners_r):
-                if isHorizontal:
-                    epi_error_sum += abs(l_pt[0][1] - r_pt[0][1])
-                else:
-                    epi_error_sum += abs(l_pt[0][0] - r_pt[0][0])
-
-        avg_epipolar = epi_error_sum / total_corners
-        print("Average Epipolar Error is : " + str(avg_epipolar))
-
-        return avg_epipolar
-
-    def create_save_mesh(self):  # , output_path):
-
-        curr_path = Path(__file__).parent.resolve()
-        print("Mesh path")
-        print(curr_path)
-
-        if self._cameraModel == "perspective":
-            map_x_l, map_y_l = cv2.initUndistortRectifyMap(
-                self.M1, self.d1, self.R1, self.M2, self.img_shape, cv2.CV_32FC1)
-            map_x_r, map_y_r = cv2.initUndistortRectifyMap(
-                self.M2, self.d2, self.R2, self.M2, self.img_shape, cv2.CV_32FC1)
-        else:    
-            map_x_l, map_y_l = cv2.fisheye.initUndistortRectifyMap(
-                self.M1, self.d1, self.R1, self.M2, self.img_shape, cv2.CV_32FC1)
-            map_x_r, map_y_r = cv2.fisheye.initUndistortRectifyMap(
-                self.M2, self.d2, self.R2, self.M2, self.img_shape, cv2.CV_32FC1)
-
-        """ 
-        map_x_l_fp32 = map_x_l.astype(np.float32)
-        map_y_l_fp32 = map_y_l.astype(np.float32)
-        map_x_r_fp32 = map_x_r.astype(np.float32)
-        map_y_r_fp32 = map_y_r.astype(np.float32)
-        
-                
-        print("shape of maps")
-        print(map_x_l.shape)
-        print(map_y_l.shape)
-        print(map_x_r.shape)
-        print(map_y_r.shape) """
-
-        meshCellSize = 16
-        mesh_left = []
-        mesh_right = []
-
-        for y in range(map_x_l.shape[0] + 1):
-            if y % meshCellSize == 0:
-                row_left = []
-                row_right = []
-                for x in range(map_x_l.shape[1] + 1):
-                    if x % meshCellSize == 0:
-                        if y == map_x_l.shape[0] and x == map_x_l.shape[1]:
-                            row_left.append(map_y_l[y - 1, x - 1])
-                            row_left.append(map_x_l[y - 1, x - 1])
-                            row_right.append(map_y_r[y - 1, x - 1])
-                            row_right.append(map_x_r[y - 1, x - 1])
-                        elif y == map_x_l.shape[0]:
-                            row_left.append(map_y_l[y - 1, x])
-                            row_left.append(map_x_l[y - 1, x])
-                            row_right.append(map_y_r[y - 1, x])
-                            row_right.append(map_x_r[y - 1, x])
-                        elif x == map_x_l.shape[1]:
-                            row_left.append(map_y_l[y, x - 1])
-                            row_left.append(map_x_l[y, x - 1])
-                            row_right.append(map_y_r[y, x - 1])
-                            row_right.append(map_x_r[y, x - 1])
-                        else:
-                            row_left.append(map_y_l[y, x])
-                            row_left.append(map_x_l[y, x])
-                            row_right.append(map_y_r[y, x])
-                            row_right.append(map_x_r[y, x])
-                if (map_x_l.shape[1] % meshCellSize) % 2 != 0:
-                    row_left.append(0)
-                    row_left.append(0)
-                    row_right.append(0)
-                    row_right.append(0)
-
-                mesh_left.append(row_left)
-                mesh_right.append(row_right)
-
-        mesh_left = np.array(mesh_left)
-        mesh_right = np.array(mesh_right)
-        left_mesh_fpath = str(curr_path) + '/../resources/left_mesh.calib'
-        right_mesh_fpath = str(curr_path) + '/../resources/right_mesh.calib'
-        mesh_left.tofile(left_mesh_fpath)
-        mesh_right.tofile(right_mesh_fpath)
+        return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
\ No newline at end of file

From c3706c38139438131aa76af713753fde373ca3dc Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 09:57:15 +0200
Subject: [PATCH 45/87] Remove trailing spaces, tabsize to 2

---
 calibration_utils.py | 2335 +++++++++++++++++++++---------------------
 1 file changed, 1161 insertions(+), 1174 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 4a30c7d..b4433db 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -30,11 +30,11 @@ def __init__(self, squaresX, squaresY, squareSize, markerSize, dictSize):
     self.dictSize = dictSize
 
   def __getstate__(self):
-      state = self.__dict__.copy()
-      for hidden in ['_board', '_dictionary']:
-        if hidden in state:
-          del state[hidden]
-      return state
+    state = self.__dict__.copy()
+    for hidden in ['_board', '_dictionary']:
+      if hidden in state:
+        del state[hidden]
+    return state
 
   def __build(self):
     self._dictionary = aruco.Dictionary_get(self.dictSize)
@@ -52,1238 +52,1225 @@ def board(self):
       self.__build()
     return self._board
 
-
 colors = [(0, 255 , 0), (0, 0, 255)]
 
 class StereoExceptions(Exception):
-    def __init__(self, message, stage, path=None, *args, **kwargs) -> None:
-        self.stage = stage
-        self.path = path
-        super().__init__(message, *args, **kwargs)
-
-    @property
-    def summary(self) -> str:
-        """
-        Returns a more comprehensive summary of the exception
-        """
-        return f"'{self.args[0]}' (occured during stage '{self.stage}')"
+  def __init__(self, message, stage, path=None, *args, **kwargs) -> None:
+    self.stage = stage
+    self.path = path
+    super().__init__(message, *args, **kwargs)
+
+  @property
+  def summary(self) -> str:
+    """
+    Returns a more comprehensive summary of the exception
+    """
+    return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
 def estimate_pose_and_filter_single(calibration, cam_info, corners, ids):
-    board = calibration._board
-    
-    objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
-    
-    ini_threshold=5
-    threshold = None
-    
-    objects = []
-    all_objects = []
-    while len(objects) < len(objpoints[:,0,0]) * cam_info['min_inliers']:
-        if ini_threshold > cam_info['max_threshold']:
-            break
-        ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, cam_info['intrinsics'], cam_info['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-        all_objects.append(objects)
-        imgpoints2 = objpoints.copy()
-
-        all_corners2 = corners.copy()
-        all_corners2 = np.array([all_corners2[id[0]] for id in objects])
-        imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
-
-        ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, cam_info['intrinsics'], cam_info['dist_coeff'])
-        
-        ini_threshold += cam_info['threshold_stepper']
-    if not ret:
-        raise RuntimeError('Exception') # TODO : Handle
-    
-    if ids is not None and corners.size > 0:
-        ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-        imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, cam_info['intrinsics'], cam_info['dist_coeff'])
-        corners2 = corners.reshape(-1, 2)
-        imgpoints2 = imgpoints2.reshape(-1, 2)
+  board = calibration._board
 
-        errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
-        if threshold == None:
-            threshold = max(2*np.median(errors), 150)
-        valid_mask = errors <= threshold
-        removed_mask = ~valid_mask
+  objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
 
-        # Collect valid IDs in the original format (array of arrays)
-        valid_ids = ids[valid_mask]
-        #filtered_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+  ini_threshold=5
+  threshold = None
 
-        # Collect data for valid points
-        #filtered_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+  objects = []
+  all_objects = []
+  while len(objects) < len(objpoints[:,0,0]) * cam_info['min_inliers']:
+    if ini_threshold > cam_info['max_threshold']:
+      break
+    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, cam_info['intrinsics'], cam_info['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+    all_objects.append(objects)
+    imgpoints2 = objpoints.copy()
 
-        #removed_corners.extend(corners2[removed_mask])
-        return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
+    all_corners2 = corners.copy()
+    all_corners2 = np.array([all_corners2[id[0]] for id in objects])
+    imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+
+    ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, cam_info['intrinsics'], cam_info['dist_coeff'])
+
+    ini_threshold += cam_info['threshold_stepper']
+  if not ret:
+    raise RuntimeError('Exception') # TODO : Handle
+
+  if ids is not None and corners.size > 0:
+    ids = ids.flatten()  # Flatten the IDs from 2D to 1D
+    imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, cam_info['intrinsics'], cam_info['dist_coeff'])
+    corners2 = corners.reshape(-1, 2)
+    imgpoints2 = imgpoints2.reshape(-1, 2)
+
+    errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
+    if threshold == None:
+      threshold = max(2*np.median(errors), 150)
+    valid_mask = errors <= threshold
+    removed_mask = ~valid_mask
+
+    # Collect valid IDs in the original format (array of arrays)
+    valid_ids = ids[valid_mask]
+    #filtered_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+
+    # Collect data for valid points
+    #filtered_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+
+    #removed_corners.extend(corners2[removed_mask])
+    return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
 def get_features(calibration, features, charucos, cam_info):
-    all_features, all_ids, imsize = calibration.getting_features(cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
-
-    if isinstance(all_features, str) and all_ids is None:
-        raise RuntimeError(f'Exception {all_features}') # TODO : Handle
-    cam_info["imsize"] = imsize
-    f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
-    print("INTRINSIC CALIBRATION")
-    cameraIntrinsics = np.array([[f,    0.0,      cam_info['imsize'][0]/2],
-                                 [0.0,     f,      cam_info['imsize'][1]/2],
-                                 [0.0,   0.0,        1.0]])
-
-    distCoeff = np.zeros((12, 1))
-    cam_info['intrinsics'] = cameraIntrinsics
-    cam_info['dist_coeff'] = distCoeff
-    
-     # check if there are any suspicious corners with high reprojection error
-    max_threshold = 75 + calibration.initial_max_threshold * (cam_info['hfov']/ 30 + cam_info['imsize'][1] / 800 * 0.2)
-    threshold_stepper = int(1.5 * (cam_info['hfov'] / 30 + cam_info['imsize'][1] / 800))
-    if threshold_stepper < 1:
-        threshold_stepper = 1
-    min_inliers = 1 - calibration.initial_min_filtered * (cam_info['hfov'] / 60 + cam_info['imsize'][1] / 800 * 0.2)
-    cam_info['max_threshold'] = max_threshold
-    cam_info['threshold_stepper'] = threshold_stepper
-    cam_info['min_inliers'] = min_inliers
-    
-    return cam_info, all_features, all_ids
+  all_features, all_ids, imsize = calibration.getting_features(cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
+
+  if isinstance(all_features, str) and all_ids is None:
+    raise RuntimeError(f'Exception {all_features}') # TODO : Handle
+  cam_info["imsize"] = imsize
+  f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+  print("INTRINSIC CALIBRATION")
+  cameraIntrinsics = np.array([[f,  0.0,    cam_info['imsize'][0]/2],
+                 [0.0,   f,    cam_info['imsize'][1]/2],
+                 [0.0,   0.0,    1.0]])
+
+  distCoeff = np.zeros((12, 1))
+  cam_info['intrinsics'] = cameraIntrinsics
+  cam_info['dist_coeff'] = distCoeff
+
+   # check if there are any suspicious corners with high reprojection error
+  max_threshold = 75 + calibration.initial_max_threshold * (cam_info['hfov']/ 30 + cam_info['imsize'][1] / 800 * 0.2)
+  threshold_stepper = int(1.5 * (cam_info['hfov'] / 30 + cam_info['imsize'][1] / 800))
+  if threshold_stepper < 1:
+    threshold_stepper = 1
+  min_inliers = 1 - calibration.initial_min_filtered * (cam_info['hfov'] / 60 + cam_info['imsize'][1] / 800 * 0.2)
+  cam_info['max_threshold'] = max_threshold
+  cam_info['threshold_stepper'] = threshold_stepper
+  cam_info['min_inliers'] = min_inliers
+
+  return cam_info, all_features, all_ids
 
 def estimate_pose_and_filter(calibration, cam_info, allCorners, allIds):
-    filtered_corners = []
-    filtered_ids = []
-    for a, b in zip(allCorners, allIds):
-        ids, corners, _ = estimate_pose_and_filter_single(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
-        filtered_corners.append(corners)
-        filtered_ids.append(ids)
-
-    return filtered_corners, filtered_ids
+  filtered_corners = []
+  filtered_ids = []
+  for a, b in zip(allCorners, allIds):
+    ids, corners, _ = estimate_pose_and_filter_single(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
+    filtered_corners.append(corners)
+    filtered_ids.append(ids)
 
+  return filtered_corners, filtered_ids
 
 def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
-    # TODO : If we still need this check it needs to be elsewhere
-    # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners): 
-    #     raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
-
-    distortion_flags = calibration.get_distortion_flags(cam_info['distortion_model'])
-    flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
-            
-    #try:
-    (ret, camera_matrix, distortion_coefficients,
-             rotation_vectors, translation_vectors,
-             stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-             perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-                charucoCorners=filteredCorners,
-                charucoIds=filteredIds,
-                board=calibration._board,
-                imageSize=cam_info['imsize'],
-                cameraMatrix=cam_info['intrinsics'],
-                distCoeffs=cam_info['dist_coeff'],
-                flags=flags,
-                criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
-    #except:
-    #    return f"First intrisic calibration failed for {cam_info['imsize']}", None, None
-
-    cam_info['intrinsics'] = camera_matrix
-    cam_info['dist_coeff'] = distortion_coefficients
-    cam_info['filtered_corners'] = filteredCorners
-    cam_info['filtered_ids'] = filteredIds
-    return cam_info
+  # TODO : If we still need this check it needs to be elsewhere
+  # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
+  #   raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
+
+  distortion_flags = calibration.get_distortion_flags(cam_info['distortion_model'])
+  flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
+
+  #try:
+  (ret, camera_matrix, distortion_coefficients,
+       rotation_vectors, translation_vectors,
+       stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+       perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+        charucoCorners=filteredCorners,
+        charucoIds=filteredIds,
+        board=calibration._board,
+        imageSize=cam_info['imsize'],
+        cameraMatrix=cam_info['intrinsics'],
+        distCoeffs=cam_info['dist_coeff'],
+        flags=flags,
+        criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
+  #except:
+  #  return f"First intrisic calibration failed for {cam_info['imsize']}", None, None
+
+  cam_info['intrinsics'] = camera_matrix
+  cam_info['dist_coeff'] = distortion_coefficients
+  cam_info['filtered_corners'] = filteredCorners
+  cam_info['filtered_ids'] = filteredIds
+  return cam_info
 
 def filter_features_fisheye(calibration, cam_info, intrinsic_img, all_features, all_ids):
-    f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
-    print("INTRINSIC CALIBRATION")
-    cameraIntrinsics = np.array([[f,    0.0,      cam_info['imsize'][0]/2],
-                                 [0.0,     f,      cam_info['imsize'][1]/2],
-                                 [0.0,   0.0,        1.0]])
+  f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+  print("INTRINSIC CALIBRATION")
+  cameraIntrinsics = np.array([[f,  0.0,    cam_info['imsize'][0]/2],
+                 [0.0,   f,    cam_info['imsize'][1]/2],
+                 [0.0,   0.0,    1.0]])
 
-    distCoeff = np.zeros((12, 1))
+  distCoeff = np.zeros((12, 1))
 
-    if cam_info["name"] in intrinsic_img:
-        raise RuntimeError('This is broken')
-        all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
-    else:
-        filtered_images = cam_info['images_path']
+  if cam_info["name"] in intrinsic_img:
+    raise RuntimeError('This is broken')
+    all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
+  else:
+    filtered_images = cam_info['images_path']
 
-    filtered_features = all_features
-    filtered_ids = all_ids
-    
-    cam_info['filtered_ids'] = filtered_ids
-    cam_info['filtered_corners'] = filtered_features
-    cam_info['intrinsics'] = cameraIntrinsics
-    cam_info['dist_coeff'] = distCoeff
+  filtered_features = all_features
+  filtered_ids = all_ids
 
-    return cam_info
+  cam_info['filtered_ids'] = filtered_ids
+  cam_info['filtered_corners'] = filtered_features
+  cam_info['intrinsics'] = cameraIntrinsics
+  cam_info['dist_coeff'] = distCoeff
+
+  return cam_info
 
 def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, filtered_corners, filtered_ids):
-    start = time.time()
-    print('starting calibrate_wf')
-    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
-    if isinstance(ret, str) and all_ids is None:
-        raise RuntimeError('Exception' + ret) # TODO : Handle
-    print(f'calibrate_wf took {round(time.time() - start, 2)}s')
-
-    cam_info['intrinsics'] = cameraIntrinsics
-    cam_info['dist_coeff'] = distCoeff
-    cam_info['size'] = size # (Width, height)
-    cam_info['reprojection_error'] = ret
-    print("Reprojection error of {0}: {1}".format(
-        cam_info['name'], ret))
-
-    return cam_info
+  start = time.time()
+  print('starting calibrate_wf')
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
+  if isinstance(ret, str) and all_ids is None:
+    raise RuntimeError('Exception' + ret) # TODO : Handle
+  print(f'calibrate_wf took {round(time.time() - start, 2)}s')
+
+  cam_info['intrinsics'] = cameraIntrinsics
+  cam_info['dist_coeff'] = distCoeff
+  cam_info['size'] = size # (Width, height)
+  cam_info['reprojection_error'] = ret
+  print("Reprojection error of {0}: {1}".format(
+    cam_info['name'], ret))
+
+  return cam_info
 
 def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
-    ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
-        cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
-    cam_info['filtered_ids'] = filtered_ids
-    cam_info['filtered_corners'] = filtered_corners
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
+    cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
+  cam_info['filtered_ids'] = filtered_ids
+  cam_info['filtered_corners'] = filtered_corners
 
-    cam_info['intrinsics'] = cameraIntrinsics
-    cam_info['dist_coeff'] = distCoeff
-    cam_info['size'] = size # (Width, height)
-    cam_info['reprojection_error'] = ret
-    print("Reprojection error of {0}: {1}".format(
-        cam_info['name'], ret))
+  cam_info['intrinsics'] = cameraIntrinsics
+  cam_info['dist_coeff'] = distCoeff
+  cam_info['size'] = size # (Width, height)
+  cam_info['reprojection_error'] = ret
+  print("Reprojection error of {0}: {1}".format(
+    cam_info['name'], ret))
 
-    return cam_info
+  return cam_info
 
 def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
-    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model']
-    specTranslation = left_cam_info['extrinsics']['specTranslation']
-    rot = left_cam_info['extrinsics']['rotation']
-
-    t_in = np.array(
-        [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-    r_in = Rotation.from_euler(
-        'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-    
-    flags = 0
-    # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
-    # print(flags)
-    flags = cv2.CALIB_FIX_INTRINSIC
-    distortion_flags = calibration.get_distortion_flags(left_distortion_model)
-    flags += distortion_flags
-    # print(flags)
-    ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-    obj_pts, left_corners_sampled, right_corners_sampled,
-    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-    R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
-
-    r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-    print(f'Epipolar error is {ret}')
-    print('Printing Extrinsics res...')
-    print(R)
-    print(T)
-    print(f'Euler angles in XYZ {r_euler} degs')
-
-
-    R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
-        cameraMatrix_l,
-        distCoeff_l,
-        cameraMatrix_r,
-        distCoeff_r,
-        None, R, T) # , alpha=0.1
-    # self.P_l = P_l
-    # self.P_r = P_r
-    r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-    r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-
-    return [ret, R, T, R_l, R_r, P_l, P_r]
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model']
+  specTranslation = left_cam_info['extrinsics']['specTranslation']
+  rot = left_cam_info['extrinsics']['rotation']
+
+  t_in = np.array(
+    [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+  r_in = Rotation.from_euler(
+    'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+
+  flags = 0
+  # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
+  # print(flags)
+  flags = cv2.CALIB_FIX_INTRINSIC
+  distortion_flags = calibration.get_distortion_flags(left_distortion_model)
+  flags += distortion_flags
+  # print(flags)
+  ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
+  obj_pts, left_corners_sampled, right_corners_sampled,
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
+  R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
+
+  r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+  print(f'Epipolar error is {ret}')
+  print('Printing Extrinsics res...')
+  print(R)
+  print(T)
+  print(f'Euler angles in XYZ {r_euler} degs')
+
+  R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
+    cameraMatrix_l,
+    distCoeff_l,
+    cameraMatrix_r,
+    distCoeff_r,
+    None, R, T) # , alpha=0.1
+  # self.P_l = P_l
+  # self.P_r = P_r
+  r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
+  r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
+
+  return [ret, R, T, R_l, R_r, P_l, P_r]
 
 def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
-    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
-    specTranslation = left_cam_info['extrinsics']['specTranslation']
-    rot = left_cam_info['extrinsics']['rotation']
-
-    t_in = np.array(
-        [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-    r_in = Rotation.from_euler(
-        'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
-
-    flags = cv2.CALIB_FIX_INTRINSIC
-    ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-    obj_pts, left_corners_sampled, right_corners_sampled,
-    np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
-    R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
-
-    r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-    scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
-    print(f'Epipolar error without scale: {ret}')
-    print(f'Epipolar error with scale: {ret*np.sqrt(scale)}')
-    print('Printing Extrinsics res...')
-    print(R)
-    print(T)
-    print(f'Euler angles in XYZ {r_euler} degs')    
-    R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
-        cameraMatrix_l,
-        distCoeff_l,
-        cameraMatrix_r,
-        distCoeff_r,
-        None, R, T) # , alpha=0.1
-    # self.P_l = P_l
-    # self.P_r = P_r
-    r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-    r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-
-    # print(f'P_l is \n {P_l}')
-    # print(f'P_r is \n {P_r}') 
-    return [ret, R, T, R_l, R_r, P_l, P_r]
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
+  specTranslation = left_cam_info['extrinsics']['specTranslation']
+  rot = left_cam_info['extrinsics']['rotation']
+
+  t_in = np.array(
+    [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
+  r_in = Rotation.from_euler(
+    'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+
+  flags = cv2.CALIB_FIX_INTRINSIC
+  ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
+  obj_pts, left_corners_sampled, right_corners_sampled,
+  np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
+  R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
+
+  r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+  scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
+  print(f'Epipolar error without scale: {ret}')
+  print(f'Epipolar error with scale: {ret*np.sqrt(scale)}')
+  print('Printing Extrinsics res...')
+  print(R)
+  print(T)
+  print(f'Euler angles in XYZ {r_euler} degs')
+  R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
+    cameraMatrix_l,
+    distCoeff_l,
+    cameraMatrix_r,
+    distCoeff_r,
+    None, R, T) # , alpha=0.1
+  # self.P_l = P_l
+  # self.P_r = P_r
+  r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
+  r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
+
+  # print(f'P_l is \n {P_l}')
+  # print(f'P_r is \n {P_r}')
+  return [ret, R, T, R_l, R_r, P_l, P_r]
 
 def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
-    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
-    # make sure all images have the same *number of* points
-    min_num_points = min([len(pts) for pts in obj_pts])
-    obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts]
-    left_corners_truncated = [pts[:min_num_points] for pts in left_corners_sampled]
-    right_corners_truncated = [pts[:min_num_points] for pts in right_corners_sampled]
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
+  # make sure all images have the same *number of* points
+  min_num_points = min([len(pts) for pts in obj_pts])
+  obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts]
+  left_corners_truncated = [pts[:min_num_points] for pts in left_corners_sampled]
+  right_corners_truncated = [pts[:min_num_points] for pts in right_corners_sampled]
+
+  flags = 0
+  # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
+  # flags |= cv2.fisheye.CALIB_CHECK_COND
+  # flags |= cv2.fisheye.CALIB_FIX_SKEW
+  flags |= cv2.fisheye.CALIB_FIX_INTRINSIC
+  # flags |= cv2.fisheye.CALIB_FIX_K1
+  # flags |= cv2.fisheye.CALIB_FIX_K2
+  # flags |= cv2.fisheye.CALIB_FIX_K3
+  # flags |= cv2.fisheye.CALIB_FIX_K4
+  # flags |= cv2.CALIB_RATIONAL_MODEL
+  # flags |= cv2.CALIB_USE_INTRINSIC_GUESS
+  # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
+  # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW
+  (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate(
+    obj_pts_truncated, left_corners_truncated, right_corners_truncated,
+    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
+    flags=flags, criteria=calibration.stereocalib_criteria), None, None
+  r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
+  print(f'Reprojection error is {ret}')
+  isHorizontal = np.absolute(T[0]) > np.absolute(T[1])
 
-    flags = 0
-    # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
-    # flags |= cv2.fisheye.CALIB_CHECK_COND
-    # flags |= cv2.fisheye.CALIB_FIX_SKEW
-    flags |= cv2.fisheye.CALIB_FIX_INTRINSIC
-    # flags |= cv2.fisheye.CALIB_FIX_K1
-    # flags |= cv2.fisheye.CALIB_FIX_K2
-    # flags |= cv2.fisheye.CALIB_FIX_K3 
-    # flags |= cv2.fisheye.CALIB_FIX_K4
-    # flags |= cv2.CALIB_RATIONAL_MODEL
-    # flags |= cv2.CALIB_USE_INTRINSIC_GUESS
-    # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
-    # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW
-    (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate(
-        obj_pts_truncated, left_corners_truncated, right_corners_truncated,
-        cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-        flags=flags, criteria=calibration.stereocalib_criteria), None, None
-    r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-    print(f'Reprojection error is {ret}')
-    isHorizontal = np.absolute(T[0]) > np.absolute(T[1])
-
-    R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify(
-        cameraMatrix_l,
-        distCoeff_l,
-        cameraMatrix_r,
-        distCoeff_r,
-        None, R, T, flags=0)
-
-    r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
-    r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
-
-    return [ret, R, T, R_l, R_r, P_l, P_r]
+  R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify(
+    cameraMatrix_l,
+    distCoeff_l,
+    cameraMatrix_r,
+    distCoeff_r,
+    None, R, T, flags=0)
+
+  r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
+  r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
+
+  return [ret, R, T, R_l, R_r, P_l, P_r]
 
 def find_stereo_common_features(calibration, left_cam_info, right_cam_info):
-    allIds_l, allIds_r, allCorners_l, allCorners_r = left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners']
-    left_corners_sampled = []
-    right_corners_sampled = []
-    left_ids_sampled = []
-    obj_pts = []
-    one_pts = calibration._board.chessboardCorners
-
-    for i in range(len(allIds_l)):
-        left_sub_corners = []
-        right_sub_corners = []
-        obj_pts_sub = []
-        #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70:
-        #      continue
-        for j in range(len(allIds_l[i])):
-            idx = np.where(allIds_r[i] == allIds_l[i][j])
-            if idx[0].size == 0:
-                continue
-            left_sub_corners.append(allCorners_l[i][j]) # TODO : This copies even idxs that don't match
-            right_sub_corners.append(allCorners_r[i][idx])
-            obj_pts_sub.append(one_pts[allIds_l[i][j]])
-        if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
-            obj_pts.append(np.array(obj_pts_sub, dtype=np.float32))
-            left_corners_sampled.append(
-                np.array(left_sub_corners, dtype=np.float32))
-            left_ids_sampled.append(np.array(allIds_l[i], dtype=np.int32))
-            right_corners_sampled.append(
-                np.array(right_sub_corners, dtype=np.float32))
-        else:
-            return -1, "Stereo Calib failed due to less common features"
-    return left_corners_sampled, right_corners_sampled, obj_pts
+  allIds_l, allIds_r, allCorners_l, allCorners_r = left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners']
+  left_corners_sampled = []
+  right_corners_sampled = []
+  left_ids_sampled = []
+  obj_pts = []
+  one_pts = calibration._board.chessboardCorners
+
+  for i in range(len(allIds_l)):
+    left_sub_corners = []
+    right_sub_corners = []
+    obj_pts_sub = []
+    #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70:
+    #    continue
+    for j in range(len(allIds_l[i])):
+      idx = np.where(allIds_r[i] == allIds_l[i][j])
+      if idx[0].size == 0:
+        continue
+      left_sub_corners.append(allCorners_l[i][j]) # TODO : This copies even idxs that don't match
+      right_sub_corners.append(allCorners_r[i][idx])
+      obj_pts_sub.append(one_pts[allIds_l[i][j]])
+    if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
+      obj_pts.append(np.array(obj_pts_sub, dtype=np.float32))
+      left_corners_sampled.append(
+        np.array(left_sub_corners, dtype=np.float32))
+      left_ids_sampled.append(np.array(allIds_l[i], dtype=np.int32))
+      right_corners_sampled.append(
+        np.array(right_sub_corners, dtype=np.float32))
+    else:
+      return -1, "Stereo Calib failed due to less common features"
+  return left_corners_sampled, right_corners_sampled, obj_pts
 
 def undistort_points_perspective(corners, camInfo):
-    for i in range(len(corners)):
-        corners[i] = cv2.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
-    return corners
+  for i in range(len(corners)):
+    corners[i] = cv2.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
+  return corners
 
 def undistort_points_fisheye(corners, camInfo):
-    for i in range(len(corners)):
-        corners[i] = cv2.fisheye.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
-    return corners
+  for i in range(len(corners)):
+    corners[i] = cv2.fisheye.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
+  return corners
 
 def remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info):
-    if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:
-        if left_cam_info["name"] in calibration.extrinsic_img:
-            array = calibration.extrinsic_img[left_cam_info["name"]]
-        elif right_cam_info["name"] in calibration.extrinsic_img:
-            array = calibration.extrinsic_img[left_cam_info["name"]]
-            
-        
-        left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = calibration.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-        right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = calibration.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-        removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = calibration.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
-        removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = calibration.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
-    return left_cam_info, right_cam_info
+  if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:
+    if left_cam_info["name"] in calibration.extrinsic_img:
+      array = calibration.extrinsic_img[left_cam_info["name"]]
+    elif right_cam_info["name"] in calibration.extrinsic_img:
+      array = calibration.extrinsic_img[left_cam_info["name"]]
+
+    left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = calibration.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+    right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = calibration.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = calibration.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
+    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = calibration.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
+  return left_cam_info, right_cam_info
 
 def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam, board_config, extrinsics):
 
-    if extrinsics[0] == -1:
-        return -1, extrinsics[1]
-    stereoConfig = None
-    if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: # TODO : Is this supposed to take the last camera pair?
-        stereoConfig = {
-            'rectification_left': extrinsics[3],
-            'rectification_right': extrinsics[4]
-        }
-    elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-        stereoConfig = {
-            'rectification_left': extrinsics[4],
-            'rectification_right': extrinsics[3]
-        }
-
-    print('<-------------Epipolar error of {} and {} ------------>'.format(
-        left_cam_info['name'], right_cam_info['name']))
-    #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
-    #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
-    if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
-        scale = right_cam_info['intrinsics'][0][0]
-    else:
-        scale = left_cam_info['intrinsics'][0][0]
-    if PER_CCM and EXTRINSICS_PER_CCM:
-        scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
-        print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
-        left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale) # TODO :Remove one of these
+  if extrinsics[0] == -1:
+    return -1, extrinsics[1]
+  stereoConfig = None
+  if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: # TODO : Is this supposed to take the last camera pair?
+    stereoConfig = {
+      'rectification_left': extrinsics[3],
+      'rectification_right': extrinsics[4]
+    }
+  elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+    stereoConfig = {
+      'rectification_left': extrinsics[4],
+      'rectification_right': extrinsics[3]
+    }
+
+  print('<-------------Epipolar error of {} and {} ------------>'.format(
+    left_cam_info['name'], right_cam_info['name']))
+  #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
+  #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
+  if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
+    scale = right_cam_info['intrinsics'][0][0]
+  else:
+    scale = left_cam_info['intrinsics'][0][0]
+  if PER_CCM and EXTRINSICS_PER_CCM:
+    scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
+    print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
+    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
+    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale) # TODO :Remove one of these
+  else:
+    print(f"Epipolar error {extrinsics[0]}")
+    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
+    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] # TODO : Remove one of these
+
+  left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
+  left_cam_info['extrinsics']['translation'] = extrinsics[2]
+
+  return left_cam_info, stereoConfig
+
+def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
+  images_path = filepath + '/' + cam_info['name']
+  image_files = glob.glob(images_path + "/*")
+  image_files.sort()
+  for im in image_files:
+    frame = cv2.imread(im)
+    height, width, _ = frame.shape
+    widthRatio = resizeWidth / width
+    heightRatio = resizeHeight / height
+    if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
+      resizeWidth = width
+      resizeHeight = height
+    break
+
+  images_path = filepath + '/' + cam_info['name']
+  if "calib_model" in cam_info:
+    cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
+    if cameraModel_ccm == "fisheye":
+      model_ccm == None
+    calib_model = cameraModel_ccm
+    distortion_model = model_ccm
+  else:
+    calib_model = _cameraModel
+    if cam_info["name"] in ccm_model:
+      distortion_model = ccm_model[cam_info["name"]]
     else:
-        print(f"Epipolar error {extrinsics[0]}")
-        left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-        left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] # TODO : Remove one of these
+      distortion_model = model
+
+  img_path = glob.glob(images_path + "/*")
+  if charucos == {}:
+    img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+  else:
+    img_path.sort()
+
+  cam_info['width'] = width
+  cam_info['height'] = height
+  cam_info['calib_model'] = calib_model
+  cam_info['distortion_model'] = distortion_model
+  cam_info["img_path"] = img_path
+  cam_info['images_path'] = images_path
+  return cam_info
 
-    left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
-    left_cam_info['extrinsics']['translation'] = extrinsics[2]
+class StereoCalibration(object):
+  """Class to Calculate Calibration and Rectify a Stereo Camera."""
+
+  def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, disableCamera: list = [], model = None,distortion_model = {}, filtering_enable = False, initial_max_threshold = 15, initial_min_filtered = 0.05, calibration_max_threshold = 10):
+    self.filtering_enable = filtering_enable
+    self.ccm_model = distortion_model
+    self.model = model
+    self.output_scale_factor = outputScaleFactor
+    self.disableCamera = disableCamera
+    self.initial_max_threshold = initial_max_threshold
+    self.initial_min_filtered = initial_min_filtered
+    self.calibration_max_threshold = calibration_max_threshold
+    self.calibration_min_filtered = initial_min_filtered
 
-    return left_cam_info, stereoConfig
+    """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
-def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
-    images_path = filepath + '/' + cam_info['name']
-    image_files = glob.glob(images_path + "/*")
-    image_files.sort()
-    for im in image_files:
-        frame = cv2.imread(im)
-        height, width, _ = frame.shape
-        widthRatio = resizeWidth / width
-        heightRatio = resizeHeight / height
-        if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-            resizeWidth = width
-            resizeHeight = height
-        break
+  @property
+  def _aruco_dictionary(self):
+    return self._proxyDict.dictionary
 
-    images_path = filepath + '/' + cam_info['name']
-    if "calib_model" in cam_info:
-        cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
-        if cameraModel_ccm == "fisheye":
-            model_ccm == None
-        calib_model = cameraModel_ccm
-        distortion_model = model_ccm
-    else:
-        calib_model = _cameraModel
-        if cam_info["name"] in ccm_model:
-            distortion_model = ccm_model[cam_info["name"]]
+  @property
+  def _board(self):
+    return self._proxyDict.board
+
+  def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify, charucos = {}, intrinsic_img = {}, extrinsic_img = []):
+    """Function to calculate calibration for stereo camera."""
+    start_time = time.time()
+    # init object data
+    if intrinsic_img != {}:
+      for cam in intrinsic_img:
+        intrinsic_img[cam].sort(reverse=True)
+    if extrinsic_img != {}:
+      for cam in extrinsic_img:
+        extrinsic_img[cam].sort(reverse=True)
+    self.intrinsic_img = intrinsic_img
+    self.extrinsic_img = extrinsic_img
+    self._cameraModel = camera_model
+    self._data_path = filepath
+    self._proxyDict = ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000)
+    self.squaresX = squaresX
+    self.squaresY = squaresY
+    self.squareSize = square_size
+    self.markerSize = mrk_size
+    self.stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT +
+                   cv2.TERM_CRITERIA_EPS, 300, 1e-9)
+
+    self.cams = []
+    features = None
+
+    resizeWidth, resizeHeight = 1280, 800
+
+    activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
+
+    stereoPairs = []
+    for camera, _ in activeCameras:
+      left_cam_info = board_config['cameras'][camera]
+      if str(left_cam_info["name"]) in self.disableCamera:
+        continue
+      if not 'extrinsics' in left_cam_info:
+        continue
+      if not 'to_cam' in left_cam_info['extrinsics']:
+        continue
+      if str(board_config['cameras'][left_cam_info['extrinsics']['to_cam']]['name']) in self.disableCamera:
+        continue
+      stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
+
+    for cam, cam_info in activeCameras:
+      cam_info = load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
+
+    tasks = []
+    camInfos = {}
+    stereoConfigs = []
+    pw = ParallelWorker(16)
+    if PER_CCM:
+      for cam, cam_info in activeCameras:
+        ret = pw.run(get_features, self, features, charucos[cam_info['name']], cam_info)
+        if self._cameraModel == "fisheye":
+          ret2 = pw.run(filter_features_fisheye, self, ret[0], intrinsic_img, ret[1], ret[2])
         else:
-            distortion_model = model
+          featuresAndIds = pw.map(estimate_pose_and_filter_single, self, ret[0], ret[1], ret[2])
 
-    img_path = glob.glob(images_path + "/*")
-    if charucos == {}:
-        img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
+          ret2 = pw.run(calibrate_charuco, self, ret[0], featuresAndIds[0], featuresAndIds[1])
+        ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, self, features, ret2, featuresAndIds[0], featuresAndIds[1])
+        tasks.extend([ret, ret2, ret3, featuresAndIds])
+        camInfos[cam] = ret3
     else:
-        img_path.sort()
+      for cam, cam_info in activeCameras:
+        cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
 
-    cam_info['width'] = width
-    cam_info['height'] = height
-    cam_info['calib_model'] = calib_model
-    cam_info['distortion_model'] = distortion_model
-    cam_info["img_path"] = img_path
-    cam_info['images_path'] = images_path
-    return cam_info
+    for left, right in stereoPairs:
+      left_cam_info = camInfos[left]
+      right_cam_info = camInfos[right]
 
-class StereoCalibration(object):
-    """Class to Calculate Calibration and Rectify a Stereo Camera."""
+      if PER_CCM and EXTRINSICS_PER_CCM:
+        left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, left_cam_info, right_cam_info)
 
-    def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, disableCamera: list = [], model = None,distortion_model = {}, filtering_enable = False, initial_max_threshold = 15, initial_min_filtered = 0.05, calibration_max_threshold = 10):
-        self.filtering_enable = filtering_enable
-        self.ccm_model = distortion_model
-        self.model = model
-        self.output_scale_factor = outputScaleFactor
-        self.disableCamera = disableCamera
-        self.initial_max_threshold = initial_max_threshold
-        self.initial_min_filtered = initial_min_filtered
-        self.calibration_max_threshold = calibration_max_threshold
-        self.calibration_min_filtered = initial_min_filtered
-
-        """Class to Calculate Calibration and Rectify a Stereo Camera."""
-
-    @property
-    def _aruco_dictionary(self):
-        return self._proxyDict.dictionary
-
-    @property
-    def _board(self):
-        return self._proxyDict.board
-
-    def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify, charucos = {}, intrinsic_img = {}, extrinsic_img = []):
-        """Function to calculate calibration for stereo camera."""
-        start_time = time.time()
-        # init object data
-        if intrinsic_img != {}:
-            for cam in intrinsic_img:
-                intrinsic_img[cam].sort(reverse=True)
-        if extrinsic_img != {}:
-            for cam in extrinsic_img:
-                extrinsic_img[cam].sort(reverse=True)
-        self.intrinsic_img = intrinsic_img
-        self.extrinsic_img = extrinsic_img
-        self._cameraModel = camera_model
-        self._data_path = filepath
-        self._proxyDict = ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000)
-        self.squaresX = squaresX
-        self.squaresY = squaresY
-        self.squareSize = square_size
-        self.markerSize = mrk_size
-        self.stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT +
-                                     cv2.TERM_CRITERIA_EPS, 300, 1e-9)
-
-
-        self.cams = []
-        features = None
-
-        resizeWidth, resizeHeight = 1280, 800
-        
-        activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
-
-        stereoPairs = []
-        for camera, _ in activeCameras:
-            left_cam_info = board_config['cameras'][camera]
-            if str(left_cam_info["name"]) in self.disableCamera:
-                continue
-            if not 'extrinsics' in left_cam_info:
-                continue
-            if not 'to_cam' in left_cam_info['extrinsics']:
-                continue
-            if str(board_config['cameras'][left_cam_info['extrinsics']['to_cam']]['name']) in self.disableCamera:
-                continue
-            stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
-
-        for cam, cam_info in activeCameras:
-            cam_info = load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
-
-        tasks = []
-        camInfos = {}
-        stereoConfigs = []
-        pw = ParallelWorker(16)
-        if PER_CCM:
-            for cam, cam_info in activeCameras:
-                ret = pw.run(get_features, self, features, charucos[cam_info['name']], cam_info)
-                if self._cameraModel == "fisheye":
-                    ret2 = pw.run(filter_features_fisheye, self, ret[0], intrinsic_img, ret[1], ret[2])
-                else:
-                    featuresAndIds = pw.map(estimate_pose_and_filter_single, self, ret[0], ret[1], ret[2])
-                    
-                    ret2 = pw.run(calibrate_charuco, self, ret[0], featuresAndIds[0], featuresAndIds[1])
-                ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, self, features, ret2, featuresAndIds[0], featuresAndIds[1])
-                tasks.extend([ret, ret2, ret3, featuresAndIds])
-                camInfos[cam] = ret3
+      ret = pw.run(find_stereo_common_features, self, left_cam_info, right_cam_info)
+      left_corners_sampled, right_corners_sampled, obj_pts = ret[0], ret[1], ret[2]
+
+      if PER_CCM and EXTRINSICS_PER_CCM:
+        if left_cam_info['calib_model'] == "perspective":
+          left_corners_sampled = pw.run(undistort_points_perspective, left_corners_sampled, left_cam_info)
+          right_corners_sampled = pw.run(undistort_points_perspective, right_corners_sampled, right_cam_info)
         else:
-            for cam, cam_info in activeCameras:
-                cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
-
-        for left, right in stereoPairs:
-            left_cam_info = camInfos[left]
-            right_cam_info = camInfos[right]
-
-            if PER_CCM and EXTRINSICS_PER_CCM:
-                left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, left_cam_info, right_cam_info)
-
-            ret = pw.run(find_stereo_common_features, self, left_cam_info, right_cam_info)
-            left_corners_sampled, right_corners_sampled, obj_pts = ret[0], ret[1], ret[2]
-
-            if PER_CCM and EXTRINSICS_PER_CCM:
-                if left_cam_info['calib_model'] == "perspective":
-                    left_corners_sampled = pw.run(undistort_points_perspective, left_corners_sampled, left_cam_info)
-                    right_corners_sampled = pw.run(undistort_points_perspective, right_corners_sampled, right_cam_info)
-                else:
-                    left_corners_sampled = pw.run(undistort_points_fisheye, left_corners_sampled, left_cam_info)
-                    right_corners_sampled = pw.run(undistort_points_fisheye, right_corners_sampled, right_cam_info)
-
-                if features == None or features == "charucos": 
-                    extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-                #### ADD OTHER CALIBRATION METHODS ###
-            else:
-                if self._cameraModel == 'perspective':
-                    extrinsics = pw.run(calibrate_stereo_perspective, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-                elif self._cameraModel == 'fisheye':
-                    extrinsics = pw.run(calibrate_stereo_fisheye, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-            ret4 = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
-            camInfos[left] = ret4[0]
-            stereoConfigs.append(ret4[1])
-        
-        pw.execute()
-
-        # Extract camera info structs and stereo config
-        for cam, camInfo in camInfos.items():
-            board_config['cameras'][cam] = camInfo.ret()
-        
-        for stereoConfig in stereoConfigs:
-            if stereoConfig.ret():
-                board_config['stereo_config'].update(stereoConfig.ret())
-
-        return 1, board_config
-
-    def getting_features(self, img_path, width, height, features = None, charucos=None):
-        if charucos:
-            allCorners = []
-            allIds = []
-            for index, charuco_img in enumerate(charucos):
-                ids, charuco = charuco_img
-                allCorners.append(charuco)
-                allIds.append(ids)
-            imsize = (width, height)
-            return allCorners, allIds, imsize
-
-        elif features == None or features == "charucos":
-            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
-            return allCorners, allIds, imsize
-
-        if features == "checker_board":
-            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
-            return allCorners, allIds, imsize
-        ###### ADD HERE WHAT IT IS NEEDED ######
-
-    def estimate_pose_and_filter(self, allCorners, allIds, name,imsize, hfov, K, d):
-         # check if there are any suspicious corners with high reprojection error
-        index = 0
-        max_threshold = 75 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
-        threshold_stepper = int(1.5 * (hfov / 30 + imsize[1] / 800))
-        if threshold_stepper < 1:
-            threshold_stepper = 1
-        print(threshold_stepper)
-        min_inliers = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
-        overall_pose = time.time()
-        for index, corners in enumerate(allCorners):
-            if len(corners) < 4:
-                raise RuntimeError(f"Less than 4 corners detected on {index} image.")
-        current = time.time()
-        
-        filtered_corners = []
-        filtered_ids = []
-        removed_corners = []
-            
-        
-        #for corners, ids in zip(allCorners, allIds):
-        current = time.time()
-        
-        with multiprocessing.Pool(processes=16) as pool:
-            results = pool.map(func=estimate_pose_and_filter_single, iterable=[(self, a, b, K, d, min_inliers, max_threshold, threshold_stepper) for a, b in zip(allCorners, allIds)])
-
-        filtered_ids, filtered_corners, removed_corners = zip(*results)
-
-        print(f"Overall pose estimation {time.time() - overall_pose}s")
-
-        if sum([len(corners) < 4 for corners in filtered_corners]) > 0.15 * len(allCorners):
-            raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {name}")
-        print(f"Filtering {time.time() -current}s")
-        
-        return filtered_corners, filtered_ids, removed_corners
-
-    def filtering_features(self, allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit, distortionModel):
-
-         # check if there are any suspicious corners with high reprojection error
-        filtered_corners, filtered_ids, removed_corners = self.estimate_pose_and_filter(allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit)
+          left_corners_sampled = pw.run(undistort_points_fisheye, left_corners_sampled, left_cam_info)
+          right_corners_sampled = pw.run(undistort_points_fisheye, right_corners_sampled, right_cam_info)
+
+        if features == None or features == "charucos":
+          extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+        #### ADD OTHER CALIBRATION METHODS ###
+      else:
+        if self._cameraModel == 'perspective':
+          extrinsics = pw.run(calibrate_stereo_perspective, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+        elif self._cameraModel == 'fisheye':
+          extrinsics = pw.run(calibrate_stereo_fisheye, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+      ret4 = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
+      camInfos[left] = ret4[0]
+      stereoConfigs.append(ret4[1])
+
+    pw.execute()
+
+    # Extract camera info structs and stereo config
+    for cam, camInfo in camInfos.items():
+      board_config['cameras'][cam] = camInfo.ret()
+
+    for stereoConfig in stereoConfigs:
+      if stereoConfig.ret():
+        board_config['stereo_config'].update(stereoConfig.ret())
+
+    return 1, board_config
+
+  def getting_features(self, img_path, width, height, features = None, charucos=None):
+    if charucos:
+      allCorners = []
+      allIds = []
+      for index, charuco_img in enumerate(charucos):
+        ids, charuco = charuco_img
+        allCorners.append(charuco)
+        allIds.append(ids)
+      imsize = (width, height)
+      return allCorners, allIds, imsize
+
+    elif features == None or features == "charucos":
+      allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
+      return allCorners, allIds, imsize
+
+    if features == "checker_board":
+      allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
+      return allCorners, allIds, imsize
+    ###### ADD HERE WHAT IT IS NEEDED ######
+
+  def estimate_pose_and_filter(self, allCorners, allIds, name,imsize, hfov, K, d):
+     # check if there are any suspicious corners with high reprojection error
+    index = 0
+    max_threshold = 75 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
+    threshold_stepper = int(1.5 * (hfov / 30 + imsize[1] / 800))
+    if threshold_stepper < 1:
+      threshold_stepper = 1
+    print(threshold_stepper)
+    min_inliers = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
+    overall_pose = time.time()
+    for index, corners in enumerate(allCorners):
+      if len(corners) < 4:
+        raise RuntimeError(f"Less than 4 corners detected on {index} image.")
+    current = time.time()
+
+    filtered_corners = []
+    filtered_ids = []
+    removed_corners = []
+
+    #for corners, ids in zip(allCorners, allIds):
+    current = time.time()
+
+    with multiprocessing.Pool(processes=16) as pool:
+      results = pool.map(func=estimate_pose_and_filter_single, iterable=[(self, a, b, K, d, min_inliers, max_threshold, threshold_stepper) for a, b in zip(allCorners, allIds)])
+
+    filtered_ids, filtered_corners, removed_corners = zip(*results)
+
+    print(f"Overall pose estimation {time.time() - overall_pose}s")
+
+    if sum([len(corners) < 4 for corners in filtered_corners]) > 0.15 * len(allCorners):
+      raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {name}")
+    print(f"Filtering {time.time() -current}s")
+
+    return filtered_corners, filtered_ids, removed_corners
+
+  def filtering_features(self, allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit, distortionModel):
+
+     # check if there are any suspicious corners with high reprojection error
+    filtered_corners, filtered_ids, removed_corners = self.estimate_pose_and_filter(allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit)
 
+    distortion_flags = self.get_distortion_flags(distortionModel)
+    flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
+
+    try:
+      (ret, camera_matrix, distortion_coefficients,
+           rotation_vectors, translation_vectors,
+           stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+           perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+            charucoCorners=filtered_corners,
+            charucoIds=filtered_ids,
+            board=self._board,
+            imageSize=imsize,
+            cameraMatrix=cameraMatrixInit,
+            distCoeffs=distCoeffsInit,
+            flags=flags,
+            criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
+    except:
+      return f"First intrisic calibration failed for {name}", None, None
+
+    return removed_corners, filtered_corners, filtered_ids, camera_matrix, distortion_coefficients
+
+  def remove_features(self, allCorners, allIds, array, img_files = None):
+    filteredCorners = allCorners.copy()
+    filteredIds = allIds.copy()
+    if img_files is not None:
+      img_path = img_files.copy()
+
+    for index in array:
+      filteredCorners.pop(index)
+      filteredIds.pop(index)
+      if img_files is not None:
+        img_path.pop(index)
+
+    return filteredCorners, filteredIds, img_path
+
+  def get_distortion_flags(self, distortionModel):
+    def is_binary_string(s: str) -> bool:
+    # Check if all characters in the string are '0' or '1'
+      return all(char in '01' for char in s)
+    if distortionModel == None:
+      print("Use DEFAULT model")
+      flags = cv2.CALIB_RATIONAL_MODEL
+    elif is_binary_string(distortionModel):
+      flags = cv2.CALIB_RATIONAL_MODEL
+      flags += cv2.CALIB_TILTED_MODEL
+      flags += cv2.CALIB_THIN_PRISM_MODEL
+      binary_number = int(distortionModel, 2)
+      # Print the results
+      if binary_number == 0:
+        clauses_status = [True, True,True, True, True, True, True, True, True]
+      else:
+        clauses_status = [(binary_number & (1 << i)) != 0 for i in range(len(distortionModel))]
+        clauses_status = clauses_status[::-1]
+      if clauses_status[0]:
+        print("FIX_K1")
+        flags += cv2.CALIB_FIX_K1
+      if clauses_status[1]:
+        print("FIX_K2")
+        flags += cv2.CALIB_FIX_K2
+      if clauses_status[2]:
+        print("FIX_K3")
+        flags += cv2.CALIB_FIX_K3
+      if clauses_status[3]:
+        print("FIX_K4")
+        flags += cv2.CALIB_FIX_K4
+      if clauses_status[4]:
+        print("FIX_K5")
+        flags += cv2.CALIB_FIX_K5
+      if clauses_status[5]:
+        print("FIX_K6")
+        flags += cv2.CALIB_FIX_K6
+      if clauses_status[6]:
+        print("FIX_TANGENT_DISTORTION")
+        flags += cv2.CALIB_ZERO_TANGENT_DIST
+      if clauses_status[7]:
+        print("FIX_TILTED_DISTORTION")
+        flags += cv2.CALIB_FIX_TAUX_TAUY
+      if clauses_status[8]:
+        print("FIX_PRISM_DISTORTION")
+        flags += cv2.CALIB_FIX_S1_S2_S3_S4
+
+    elif isinstance(distortionModel, str):
+      if distortionModel == "NORMAL":
+        print("Using NORMAL model")
+        flags = cv2.CALIB_RATIONAL_MODEL
+        flags += cv2.CALIB_TILTED_MODEL
+
+      elif distortionModel == "TILTED":
+        print("Using TILTED model")
+        flags = cv2.CALIB_RATIONAL_MODEL
+        flags += cv2.CALIB_TILTED_MODEL
+
+      elif distortionModel == "PRISM":
+        print("Using PRISM model")
+        flags = cv2.CALIB_RATIONAL_MODEL
+        flags += cv2.CALIB_TILTED_MODEL
+        flags += cv2.CALIB_THIN_PRISM_MODEL
+
+      elif distortionModel == "THERMAL":
+        print("Using THERMAL model")
+        flags = cv2.CALIB_RATIONAL_MODEL
+        flags += cv2.CALIB_FIX_K3
+        flags += cv2.CALIB_FIX_K5
+        flags += cv2.CALIB_FIX_K6
+
+    elif isinstance(distortionModel, int):
+      print("Using CUSTOM flags")
+      flags = distortionModel
+    return flags
+
+  def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
+    coverageImage = np.ones(imsize[::-1], np.uint8) * 255
+    coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
+    coverageImage = self.draw_corners(allCorners, coverageImage)
+    if calib_model == 'perspective':
+      if features == None or features == "charucos":
         distortion_flags = self.get_distortion_flags(distortionModel)
-        flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
-                
+        ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
+          allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
+
+        return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+      else:
+        return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+      #### ADD ADDITIONAL FEATURES CALIBRATION ####
+    else:
+      if features == None or features == "charucos":
+        print('Fisheye--------------------------------------------------')
+        ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = self.calibrate_fisheye(
+          allCorners, allIds, imsize, hfov, name)
+        print('Fisheye rotation vector', rotation_vectors[0])
+        print('Fisheye translation vector', translation_vectors[0])
+
+        # (Height, width)
+        return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+
+  def draw_corners(self, charuco_corners, displayframe):
+    for corners in charuco_corners:
+      color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255)))
+      for corner in corners:
+        corner_int = (int(corner[0][0]), int(corner[0][1]))
+        cv2.circle(displayframe, corner_int, 4, color, -1)
+    height, width = displayframe.shape[:2]
+    start_point = (0, 0)  # top of the image
+    end_point = (0, height)
+
+    color = (0, 0, 0)  # blue in BGR
+    thickness = 4
+
+    # Draw the line on the image
+    cv2.line(displayframe, start_point, end_point, color, thickness)
+    return displayframe
+
+  def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, reprojection, filtered_corners,filtered_id, camera, display = True, threshold = None, draw_quadrants = False, nx = 4, ny = 4):
+    whole_error = []
+    all_points = []
+    all_corners = []
+    all_error = []
+    all_ids = []
+    removed_corners = []
+    removed_points = []
+    removed_ids = []
+    removed_error = []
+    display_corners = []
+    display_points = []
+    circle_size = 0
+    reported_error = []
+    for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
+      if ids is not None and corners.size > 0:
+        ids = ids.flatten()  # Flatten the IDs from 2D to 1D
+        objPoints = np.array([self._board.chessboardCorners[id] for id in ids], dtype=np.float32)
+        imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
+        corners2 = corners.reshape(-1, 2)
+        imgpoints2 = imgpoints2.reshape(-1, 2)
+
+        errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
+        if threshold == None:
+          threshold = max(2*np.median(errors), 150)
+        valid_mask = errors <= threshold
+        removed_mask = ~valid_mask
+
+        # Collect valid IDs in the original format (array of arrays)
+        valid_ids = ids[valid_mask]
+        all_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+
+        # Collect data for valid points
+        reported_error.extend(errors)
+        all_error.extend(errors[valid_mask])
+        display_corners.extend(corners2)
+        display_points.extend(imgpoints2[valid_mask])
+        all_points.append(imgpoints2[valid_mask])  # Collect valid points for calibration
+        all_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+
+        removed_corners.extend(corners2[removed_mask])
+        removed_points.extend(imgpoints2[removed_mask])
+        removed_ids.extend(ids[removed_mask])
+        removed_error.extend(errors[removed_mask])
+
+        total_error_squared = np.sum(errors[valid_mask]**2)
+        total_points = len(objPoints[valid_mask])
+        rms_error = np.sqrt(total_error_squared / total_points if total_points else 0)
+        whole_error.append(rms_error)
+
+      total_error_squared = 0
+      total_points = 0
+
+    return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
+
+  def detect_charuco_board(self, image: np.array):
+    arucoParams = cv2.aruco.DetectorParameters_create()
+    arucoParams.minMarkerDistanceRate = 0.01
+    corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, self._aruco_dictionary, parameters=arucoParams)  # First, detect markers
+    marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, self._board, corners, ids, rejectedCorners=rejectedImgPoints)
+    # If found, add object points, image points (after refining them)
+    if len(marker_corners) > 0:
+      ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, self._board, minMarkers = 1)
+      return ret, corners, ids, marker_corners, marker_ids
+    else:
+      return None, None, None, None, None
+
+  def camera_pose_charuco(self, objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
+    objects = []
+    all_objects = []
+    index = 0
+    start_time = time.time()
+    while len(objects) < len(objpoints[:,0,0]) * min_inliers:
+      if ini_threshold > max_threshold:
+        break
+      ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+      all_objects.append(objects)
+      imgpoints2 = objpoints.copy()
+
+      all_corners = corners.copy()
+      all_corners = np.array([all_corners[id[0]] for id in objects])
+      imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+
+      ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners, K, d)
+      imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, K, d)
+
+      ini_threshold += threshold_stepper
+      index += 1
+    if ret:
+      return rvec, tvec, objects
+    else:
+      return None
+
+  def compute_reprojection_errors(self, obj_pts: np.array, img_pts: np.array, K: np.array, dist: np.array, rvec: np.array, tvec: np.array, fisheye = False):
+    if fisheye:
+      proj_pts, _ = cv2.fisheye.projectPoints(obj_pts, rvec, tvec, K, dist)
+    else:
+      proj_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist)
+    errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
+    return errs
+
+  def charuco_ids_to_objpoints(self, ids):
+    one_pts = self._board.chessboardCorners
+    objpts = []
+    for j in range(len(ids)):
+      objpts.append(one_pts[ids[j]])
+    return np.array(objpts)
+
+  def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
+    """
+    Charuco base pose estimation.
+    """
+    # print("POSE ESTIMATION STARTS:")
+    allCorners = []
+    allIds = []
+    all_marker_corners = []
+    all_marker_ids = []
+    all_recovered = []
+    # decimator = 0
+    # SUB PIXEL CORNER DETECTION CRITERION
+    criteria = (cv2.TERM_CRITERIA_EPS +
+          cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
+    count = 0
+    skip_vis = False
+    for im in images:
+      img_pth = Path(im)
+      frame = cv2.imread(im)
+      gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+      expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1])
+
+      if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]):
+        if int(expected_height) == req_resolution[0]:
+          # resizing to have both stereo and rgb to have same
+          # resolution to capture extrinsics of the rgb-right camera
+          gray = cv2.resize(gray, req_resolution[::-1],
+                    interpolation=cv2.INTER_CUBIC)
+        else:
+          # resizing and cropping to have both stereo and rgb to have same resolution
+          # to calculate extrinsics of the rgb-right camera
+          scale_width = req_resolution[1]/gray.shape[1]
+          dest_res = (
+            int(gray.shape[1] * scale_width), int(gray.shape[0] * scale_width))
+          gray = cv2.resize(
+            gray, dest_res, interpolation=cv2.INTER_CUBIC)
+          if gray.shape[0] < req_resolution[0]:
+            raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
+              gray.shape[0], req_resolution[0]))
+          # print(gray.shape[0] - req_resolution[0])
+          del_height = (gray.shape[0] - req_resolution[0]) // 2
+          # gray = gray[: req_resolution[0], :]
+          gray = gray[del_height: del_height + req_resolution[0], :]
+
+        count += 1
+
+      ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = self.detect_charuco_board(gray)
+
+      if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
+
+        charuco_corners = cv2.cornerSubPix(gray, charuco_corners,
+                  winSize=(5, 5),
+                  zeroZone=(-1, -1),
+                  criteria=criteria)
+        allCorners.append(charuco_corners)  # Charco chess corners
+        allIds.append(charuco_ids)  # charuco chess corner id's
+        all_marker_corners.append(marker_corners)
+        all_marker_ids.append(marker_ids)
+      else:
+        print(im)
+        return f'Failed to detect more than 3 markers on image {im}', None, None, None, None, None
+
+    # imsize = gray.shape[::-1]
+    return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
+
+  def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
+    image_files = glob.glob(image_files + "/*")
+    image_files.sort()
+    assert len(
+      image_files) != 0, "ERROR: Images not read correctly, check directory"
+    if charucos == {}:
+      allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files)
+    else:
+      allCorners = []
+      allIds = []
+      for index, charuco_img in enumerate(charucos[name]):
+        ids, charucos = charuco_img
+        allCorners.append(charucos)
+        allIds.append(ids)
+      imsize = (height, width)
+
+    coverageImage = np.ones(imsize[::-1], np.uint8) * 255
+    coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
+    coverageImage = self.draw_corners(allCorners, coverageImage)
+    if calib_model == 'perspective':
+      distortion_flags = self.get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
+      ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
+        allCorners, allIds, imsize, hfov, name, distortion_flags)
+      self.undistort_visualization(
+        image_files, camera_matrix, distortion_coefficients, imsize, name)
+
+      return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+    else:
+      print('Fisheye--------------------------------------------------')
+      ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = self.calibrate_fisheye(
+        allCorners, allIds, imsize, hfov, name)
+      self.undistort_visualization(
+          image_files, camera_matrix, distortion_coefficients, imsize, name)
+      print('Fisheye rotation vector', rotation_vectors[0])
+      print('Fisheye translation vector', translation_vectors[0])
+
+      # (Height, width)
+      return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+
+  def undistort_visualization(self, img_list, K, D, img_size, name):
+    for index, im in enumerate(img_list):
+      # print(im)
+      img = cv2.imread(im)
+      # h, w = img.shape[:2]
+      if self._cameraModel == 'perspective':
+        kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0)
+        # print(f'K scaled is \n {kScaled} and size is \n {img_size}')
+        # print(f'D Value is \n {D}')
+        map1, map2 = cv2.initUndistortRectifyMap(
+          K, D, np.eye(3), kScaled, img_size, cv2.CV_32FC1)
+      else:
+        map1, map2 = cv2.fisheye.initUndistortRectifyMap(
+          K, D, np.eye(3), K, img_size, cv2.CV_32FC1)
+
+      undistorted_img = cv2.remap(
+        img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
+
+      if index == 0:
+        undistorted_file_path = self.data_path + '/' + name + f'_undistorted.png'
+        cv2.imwrite(undistorted_file_path, undistorted_img)
+
+  def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
+    corners_removed = False
+    for i in range(len(allIds)):
+      corners = allCorners[i]
+      ids = allIds[i]
+      objpts = self.charuco_ids_to_objpoints(ids)
+      if self._cameraModel == "fisheye":
+        errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
+      else:
+        errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i])
+      suspicious_err_thr = max(2*np.median(errs), 100)
+      n_offending_pts = np.sum(errs > suspicious_err_thr)
+      offending_pts_idxs = np.where(errs > suspicious_err_thr)[0]
+      # check if there are offending points and if they form a minority
+      if n_offending_pts > 0 and n_offending_pts < len(corners)/5:
+        print(f"removing {n_offending_pts} offending points with errs {errs[offending_pts_idxs]}")
+        corners_removed = True
+        #remove the offending points
+        offset = 0
+        allCorners[i] = np.delete(allCorners[i],offending_pts_idxs, axis = 0)
+        allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
+    return corners_removed, allIds, allCorners
+
+  def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
+    """
+    Calibrates the camera using the dected corners.
+    """
+    f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2)))
+
+    threshold = 2 * imsize[1]/800.0
+     # check if there are any suspicious corners with high reprojection error
+    rvecs = []
+    tvecs = []
+    index = 0
+    max_threshold = 10 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
+    min_inlier = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
+    for corners, ids in zip(allCorners, allIds):
+      objpts = self.charuco_ids_to_objpoints(ids)
+      rvec, tvec, newids = self.camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
+      tvecs.append(tvec)
+      rvecs.append(rvec)
+      index += 1
+
+    # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
+    ret = 0.0
+    flags = cv2.CALIB_USE_INTRINSIC_GUESS
+    flags += distortion_flags
+
+    #   flags = (cv2.CALIB_RATIONAL_MODEL)
+    reprojection = []
+    removed_errors = []
+    num_corners = []
+    num_threshold = []
+    iterations_array = []
+    intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
+    distortion_array = {}
+    index = 0
+    camera_matrix = cameraIntrinsics
+    distortion_coefficients = distCoeff
+    rotation_vectors = rvecs
+    translation_vectors = tvecs
+    translation_array_x = []
+    translation_array_y = []
+    translation_array_z = []
+    corner_checker = 0
+    previous_ids = []
+    import time
+    try:
+      whole = time.time()
+      while True:
+        intrinsic_array['f_x'].append(camera_matrix[0][0])
+        intrinsic_array['f_y'].append(camera_matrix[1][1])
+        intrinsic_array['c_x'].append(camera_matrix[0][2])
+        intrinsic_array['c_y'].append(camera_matrix[1][2])
+        num_threshold.append(threshold)
+
+        translation_array_x.append(np.mean(np.array(translation_vectors).T[0][0]))
+        translation_array_y.append(np.mean(np.array(translation_vectors).T[0][1]))
+        translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
+
+        start = time.time()
+        filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = self.features_filtering_function(rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = threshold)
+        num_corners.append(len(removed_corners))
+        iterations_array.append(index)
+        reprojection.append(ret)
+        for i in range(len(distortion_coefficients)):
+          if i not in distortion_array:
+            distortion_array[i] = []
+          distortion_array[i].append(distortion_coefficients[i][0])
+        print(f"Each filtering {time.time() - start}")
+        start = time.time()
         try:
-            (ret, camera_matrix, distortion_coefficients,
-                     rotation_vectors, translation_vectors,
-                     stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-                     perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-                        charucoCorners=filtered_corners,
-                        charucoIds=filtered_ids,
-                        board=self._board,
-                        imageSize=imsize,
-                        cameraMatrix=cameraMatrixInit,
-                        distCoeffs=distCoeffsInit,
-                        flags=flags,
-                        criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
+          (ret, camera_matrix, distortion_coefficients,
+           rotation_vectors, translation_vectors,
+           stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+           perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+            charucoCorners=filtered_corners,
+            charucoIds=filtered_ids,
+            board=self._board,
+            imageSize=imsize,
+            cameraMatrix=cameraIntrinsics,
+            distCoeffs=distCoeff,
+            flags=flags,
+            criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
         except:
-            return f"First intrisic calibration failed for {name}", None, None
-
-        return removed_corners, filtered_corners, filtered_ids, camera_matrix, distortion_coefficients
-    
-    def remove_features(self, allCorners, allIds, array, img_files = None):
-        filteredCorners = allCorners.copy()
-        filteredIds = allIds.copy()
-        if img_files is not None:
-            img_path = img_files.copy()
-
-        for index in array:
-            filteredCorners.pop(index)
-            filteredIds.pop(index)
-            if img_files is not None:
-                img_path.pop(index)
-
-        return filteredCorners, filteredIds, img_path
-
-    def get_distortion_flags(self, distortionModel):
-        def is_binary_string(s: str) -> bool:
-        # Check if all characters in the string are '0' or '1'
-            return all(char in '01' for char in s)
-        if distortionModel == None:
-            print("Use DEFAULT model")
-            flags = cv2.CALIB_RATIONAL_MODEL
-        elif is_binary_string(distortionModel):
-            flags = cv2.CALIB_RATIONAL_MODEL
-            flags += cv2.CALIB_TILTED_MODEL
-            flags += cv2.CALIB_THIN_PRISM_MODEL
-            binary_number = int(distortionModel, 2)
-            # Print the results
-            if binary_number == 0:
-                clauses_status = [True, True,True, True, True, True, True, True, True]
-            else:
-                clauses_status = [(binary_number & (1 << i)) != 0 for i in range(len(distortionModel))]
-                clauses_status = clauses_status[::-1]
-            if clauses_status[0]:
-                print("FIX_K1")
-                flags += cv2.CALIB_FIX_K1
-            if clauses_status[1]:
-                print("FIX_K2")
-                flags += cv2.CALIB_FIX_K2
-            if clauses_status[2]:
-                print("FIX_K3")
-                flags += cv2.CALIB_FIX_K3
-            if clauses_status[3]:
-                print("FIX_K4")
-                flags += cv2.CALIB_FIX_K4
-            if clauses_status[4]:
-                print("FIX_K5")
-                flags += cv2.CALIB_FIX_K5
-            if clauses_status[5]:
-                print("FIX_K6")
-                flags += cv2.CALIB_FIX_K6
-            if clauses_status[6]:
-                print("FIX_TANGENT_DISTORTION")
-                flags += cv2.CALIB_ZERO_TANGENT_DIST
-            if clauses_status[7]:
-                print("FIX_TILTED_DISTORTION")
-                flags += cv2.CALIB_FIX_TAUX_TAUY
-            if clauses_status[8]:
-                print("FIX_PRISM_DISTORTION")
-                flags += cv2.CALIB_FIX_S1_S2_S3_S4
-
-        elif isinstance(distortionModel, str):
-            if distortionModel == "NORMAL":
-                print("Using NORMAL model")
-                flags = cv2.CALIB_RATIONAL_MODEL
-                flags += cv2.CALIB_TILTED_MODEL
-
-            elif distortionModel == "TILTED":
-                print("Using TILTED model")
-                flags = cv2.CALIB_RATIONAL_MODEL
-                flags += cv2.CALIB_TILTED_MODEL
-
-            elif distortionModel == "PRISM":
-                print("Using PRISM model")
-                flags = cv2.CALIB_RATIONAL_MODEL
-                flags += cv2.CALIB_TILTED_MODEL
-                flags += cv2.CALIB_THIN_PRISM_MODEL
-
-            elif distortionModel == "THERMAL":
-                print("Using THERMAL model")
-                flags = cv2.CALIB_RATIONAL_MODEL
-                flags += cv2.CALIB_FIX_K3
-                flags += cv2.CALIB_FIX_K5
-                flags += cv2.CALIB_FIX_K6
-
-        elif isinstance(distortionModel, int):
-            print("Using CUSTOM flags")
-            flags = distortionModel
-        return flags
-
-    def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
-        coverageImage = np.ones(imsize[::-1], np.uint8) * 255
-        coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
-        coverageImage = self.draw_corners(allCorners, coverageImage)
-        if calib_model == 'perspective':
-            if features == None or features == "charucos":
-                distortion_flags = self.get_distortion_flags(distortionModel)
-                ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
-                    allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
-
-                return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-            else:
-                return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-            #### ADD ADDITIONAL FEATURES CALIBRATION ####
-        else:
-            if features == None or features == "charucos":
-                print('Fisheye--------------------------------------------------')
-                ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = self.calibrate_fisheye(
-                    allCorners, allIds, imsize, hfov, name)
-                print('Fisheye rotation vector', rotation_vectors[0])
-                print('Fisheye translation vector', translation_vectors[0])
-
-                # (Height, width)
-                return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-
-    def draw_corners(self, charuco_corners, displayframe):
-        for corners in charuco_corners:
-            color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255)))
-            for corner in corners:
-                corner_int = (int(corner[0][0]), int(corner[0][1]))
-                cv2.circle(displayframe, corner_int, 4, color, -1)
-        height, width = displayframe.shape[:2]
-        start_point = (0, 0)  # top of the image
-        end_point = (0, height)
-
-        color = (0, 0, 0)  # blue in BGR
-        thickness = 4
-
-        # Draw the line on the image
-        cv2.line(displayframe, start_point, end_point, color, thickness)
-        return displayframe
-
-    def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, reprojection, filtered_corners,filtered_id, camera, display = True, threshold = None, draw_quadrants = False, nx = 4, ny = 4):
-        whole_error = []
-        all_points = []
-        all_corners = []
-        all_error = []
-        all_ids = []
-        removed_corners = []
-        removed_points = []
-        removed_ids = []
-        removed_error = []
-        display_corners = []
-        display_points = []
-        circle_size = 0
-        reported_error = []
-        for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
-            if ids is not None and corners.size > 0:
-                ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-                objPoints = np.array([self._board.chessboardCorners[id] for id in ids], dtype=np.float32)
-                imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
-                corners2 = corners.reshape(-1, 2)
-                imgpoints2 = imgpoints2.reshape(-1, 2)
-
-                errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
-                if threshold == None:
-                    threshold = max(2*np.median(errors), 150)
-                valid_mask = errors <= threshold
-                removed_mask = ~valid_mask
-
-                # Collect valid IDs in the original format (array of arrays)
-                valid_ids = ids[valid_mask]
-                all_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
-
-                # Collect data for valid points
-                reported_error.extend(errors)
-                all_error.extend(errors[valid_mask])
-                display_corners.extend(corners2)
-                display_points.extend(imgpoints2[valid_mask])
-                all_points.append(imgpoints2[valid_mask])  # Collect valid points for calibration
-                all_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
-
-                removed_corners.extend(corners2[removed_mask])
-                removed_points.extend(imgpoints2[removed_mask])
-                removed_ids.extend(ids[removed_mask])
-                removed_error.extend(errors[removed_mask])
-
-                total_error_squared = np.sum(errors[valid_mask]**2)
-                total_points = len(objPoints[valid_mask])
-                rms_error = np.sqrt(total_error_squared / total_points if total_points else 0)
-                whole_error.append(rms_error)
-
-
-            total_error_squared = 0
-            total_points = 0
-
-        return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
-
-    def detect_charuco_board(self, image: np.array):
-        arucoParams = cv2.aruco.DetectorParameters_create()
-        arucoParams.minMarkerDistanceRate = 0.01
-        corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, self._aruco_dictionary, parameters=arucoParams)  # First, detect markers
-        marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, self._board, corners, ids, rejectedCorners=rejectedImgPoints)
-        # If found, add object points, image points (after refining them)
-        if len(marker_corners) > 0:
-            ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, self._board, minMarkers = 1)
-            return ret, corners, ids, marker_corners, marker_ids
-        else:
-            return None, None, None, None, None
-
-    def camera_pose_charuco(self, objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
-        objects = []
-        all_objects = []
-        index = 0
-        start_time = time.time()
-        while len(objects) < len(objpoints[:,0,0]) * min_inliers:
-            if ini_threshold > max_threshold:
-                break
-            ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-            all_objects.append(objects)
-            imgpoints2 = objpoints.copy()
-
-            all_corners = corners.copy()
-            all_corners = np.array([all_corners[id[0]] for id in objects])
-            imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
-
-            ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners, K, d)
-            imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, K, d)
-            
-            ini_threshold += threshold_stepper
-            index += 1
-        if ret:
-            return rvec, tvec, objects
-        else:
-            return None
-        
-    def compute_reprojection_errors(self, obj_pts: np.array, img_pts: np.array, K: np.array, dist: np.array, rvec: np.array, tvec: np.array, fisheye = False):
-        if fisheye:
-            proj_pts, _ = cv2.fisheye.projectPoints(obj_pts, rvec, tvec, K, dist)
-        else:
-            proj_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist)
-        errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
-        return errs 
-    
-    def charuco_ids_to_objpoints(self, ids):
-        one_pts = self._board.chessboardCorners
-        objpts = []
-        for j in range(len(ids)):   
-            objpts.append(one_pts[ids[j]])
-        return np.array(objpts)
-
-
-    def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
-        """
-        Charuco base pose estimation.
-        """
-        # print("POSE ESTIMATION STARTS:")
-        allCorners = []
-        allIds = []
-        all_marker_corners = []
-        all_marker_ids = []
-        all_recovered = []
-        # decimator = 0
-        # SUB PIXEL CORNER DETECTION CRITERION
-        criteria = (cv2.TERM_CRITERIA_EPS +
-                    cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
-        count = 0
-        skip_vis = False
-        for im in images:
-            img_pth = Path(im)
-            frame = cv2.imread(im)
-            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-            expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1])
-
-            if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]):
-                if int(expected_height) == req_resolution[0]:
-                    # resizing to have both stereo and rgb to have same
-                    # resolution to capture extrinsics of the rgb-right camera
-                    gray = cv2.resize(gray, req_resolution[::-1],
-                                      interpolation=cv2.INTER_CUBIC)
-                else:
-                    # resizing and cropping to have both stereo and rgb to have same resolution
-                    # to calculate extrinsics of the rgb-right camera
-                    scale_width = req_resolution[1]/gray.shape[1]
-                    dest_res = (
-                        int(gray.shape[1] * scale_width), int(gray.shape[0] * scale_width))
-                    gray = cv2.resize(
-                        gray, dest_res, interpolation=cv2.INTER_CUBIC)
-                    if gray.shape[0] < req_resolution[0]:
-                        raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
-                            gray.shape[0], req_resolution[0]))
-                    # print(gray.shape[0] - req_resolution[0])
-                    del_height = (gray.shape[0] - req_resolution[0]) // 2
-                    # gray = gray[: req_resolution[0], :]
-                    gray = gray[del_height: del_height + req_resolution[0], :]
-
-                count += 1
-            
-            ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = self.detect_charuco_board(gray)
-
-
-            if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
-
-                charuco_corners = cv2.cornerSubPix(gray, charuco_corners,
-                                    winSize=(5, 5),
-                                    zeroZone=(-1, -1),
-                                    criteria=criteria)
-                allCorners.append(charuco_corners)  # Charco chess corners
-                allIds.append(charuco_ids)  # charuco chess corner id's
-                all_marker_corners.append(marker_corners)
-                all_marker_ids.append(marker_ids)
-            else:
-                print(im)
-                return f'Failed to detect more than 3 markers on image {im}', None, None, None, None, None
-
-        # imsize = gray.shape[::-1]
-        return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
-
-    def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
-        image_files = glob.glob(image_files + "/*")
-        image_files.sort()
-        assert len(
-            image_files) != 0, "ERROR: Images not read correctly, check directory"
-        if charucos == {}:
-            allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files)
-        else:
-            allCorners = []
-            allIds = []
-            for index, charuco_img in enumerate(charucos[name]):
-                ids, charucos = charuco_img
-                allCorners.append(charucos)
-                allIds.append(ids)
-            imsize = (height, width)
-
-        coverageImage = np.ones(imsize[::-1], np.uint8) * 255
-        coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
-        coverageImage = self.draw_corners(allCorners, coverageImage)
-        if calib_model == 'perspective':
-            distortion_flags = self.get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
-            ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
-                allCorners, allIds, imsize, hfov, name, distortion_flags)
-            self.undistort_visualization(
-                image_files, camera_matrix, distortion_coefficients, imsize, name)
-
-            return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-        else:
-            print('Fisheye--------------------------------------------------')
-            ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = self.calibrate_fisheye(
-                allCorners, allIds, imsize, hfov, name)
-            self.undistort_visualization(
-                    image_files, camera_matrix, distortion_coefficients, imsize, name)
-            print('Fisheye rotation vector', rotation_vectors[0])
-            print('Fisheye translation vector', translation_vectors[0])
-
-            # (Height, width)
-            return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-
-    def undistort_visualization(self, img_list, K, D, img_size, name):
-        for index, im in enumerate(img_list):
-            # print(im)
-            img = cv2.imread(im)
-            # h, w = img.shape[:2]
-            if self._cameraModel == 'perspective':
-                kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0)
-                # print(f'K scaled is \n {kScaled} and size is \n {img_size}')
-                # print(f'D Value is \n {D}')
-                map1, map2 = cv2.initUndistortRectifyMap(
-                    K, D, np.eye(3), kScaled, img_size, cv2.CV_32FC1)
-            else:
-                map1, map2 = cv2.fisheye.initUndistortRectifyMap(
-                    K, D, np.eye(3), K, img_size, cv2.CV_32FC1)
-
-            undistorted_img = cv2.remap(
-                img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
-            
-            if index == 0:
-                undistorted_file_path = self.data_path + '/' + name + f'_undistorted.png'
-                cv2.imwrite(undistorted_file_path, undistorted_img)
-
-
-    def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
-        corners_removed = False
-        for i in range(len(allIds)):
-            corners = allCorners[i]
-            ids = allIds[i]
-            objpts = self.charuco_ids_to_objpoints(ids)
-            if self._cameraModel == "fisheye":
-                errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
-            else:
-                errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i])
-            suspicious_err_thr = max(2*np.median(errs), 100)
-            n_offending_pts = np.sum(errs > suspicious_err_thr)
-            offending_pts_idxs = np.where(errs > suspicious_err_thr)[0]
-            # check if there are offending points and if they form a minority
-            if n_offending_pts > 0 and n_offending_pts < len(corners)/5:
-                print(f"removing {n_offending_pts} offending points with errs {errs[offending_pts_idxs]}")
-                corners_removed = True
-                #remove the offending points
-                offset = 0
-                allCorners[i] = np.delete(allCorners[i],offending_pts_idxs, axis = 0)
-                allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
-        return corners_removed, allIds, allCorners
-
-
-    def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
-        """
-        Calibrates the camera using the dected corners.
-        """
-        f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2)))
-
-        threshold = 2 * imsize[1]/800.0
-         # check if there are any suspicious corners with high reprojection error
-        rvecs = []
-        tvecs = []
-        index = 0
-        max_threshold = 10 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
-        min_inlier = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
-        for corners, ids in zip(allCorners, allIds):
-            objpts = self.charuco_ids_to_objpoints(ids)
-            rvec, tvec, newids = self.camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
-            tvecs.append(tvec)
-            rvecs.append(rvec)
-            index += 1
-
-        # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
-        ret = 0.0
-        flags = cv2.CALIB_USE_INTRINSIC_GUESS
-        flags += distortion_flags
-
-        #     flags = (cv2.CALIB_RATIONAL_MODEL)
-        reprojection = []
-        removed_errors = []
-        num_corners = []
-        num_threshold = []
-        iterations_array = []
-        intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
-        distortion_array = {}
-        index = 0
-        camera_matrix = cameraIntrinsics
-        distortion_coefficients = distCoeff
-        rotation_vectors = rvecs
-        translation_vectors = tvecs
-        translation_array_x = []
-        translation_array_y = []
-        translation_array_z = []
-        corner_checker = 0
-        previous_ids = []
-        import time
+          raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=self.id)
+        cameraIntrinsics = camera_matrix
+        distCoeff = distortion_coefficients
+        threshold = 5 * imsize[1]/800.0
+        print(f"Each calibration {time.time()-start}")
+        index += 1
+        if  index > 5 or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
+          print(f"Whole procedure: {time.time() - whole}")
+          break
+        previous_ids = removed_ids
+    except:
+      return f"Failed to calibrate camera {name}", None, None, None, None, None, None, None ,None , None
+    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
+
+  def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
+    one_pts = self._board.chessboardCorners
+    obj_points = []
+    for i in range(len(allIds)):
+      obj_points.append(self.charuco_ids_to_objpoints(allIds[i]))
+
+    f_init = imsize[0]/np.deg2rad(hfov)*1.15
+
+    cameraMatrixInit = np.array([[f_init, 0.      , imsize[0]/2],
+                   [0.      , f_init, imsize[1]/2],
+                   [0.      , 0.      , 1.      ]])
+    distCoeffsInit = np.zeros((4,1))
+     # check if there are any suspicious corners with high reprojection error
+    rvecs = []
+    tvecs = []
+    for corners, ids in zip(allCorners, allIds):
+      objpts = self.charuco_ids_to_objpoints(ids)
+      corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
+      rvec, tvec, new_ids = self.camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
+      tvecs.append(tvec)
+      rvecs.append(rvec)
+    corners_removed, filtered_ids, filtered_corners = self.filter_corner_outliers(allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
+    if corners_removed:
+      obj_points = []
+      for i in range(len(filtered_ids)):
+        obj_points.append(self.charuco_ids_to_objpoints(filtered_ids[i]))
+
+    print("Camera Matrix initialization.............")
+    print(cameraMatrixInit)
+    flags = 0
+    flags |= cv2.fisheye.CALIB_CHECK_COND
+    flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS
+    flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
+    flags |= cv2.fisheye.CALIB_FIX_SKEW
+
+    term_criteria = (cv2.TERM_CRITERIA_COUNT +
+             cv2.TERM_CRITERIA_EPS, 30, 1e-9)
+    try:
+      res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
+    except:
+      # calibration failed for full FOV, let's try to limit the corners to smaller part of FOV first to find initial parameters
+      success = False
+      crop = 0.95
+      while not success:
+        print(f"trying crop factor {crop}")
+        obj_points_limited = []
+        corners_limited = []
+        for obj_pts, corners in zip(obj_points, filtered_corners):
+          obj_points_tmp = []
+          corners_tmp = []
+          for obj_pt, corner in zip(obj_pts, corners):
+            check_x = corner[0,0] > imsize[0]*(1-crop) and corner[0,0] < imsize[0]*crop
+            check_y = corner[0,1] > imsize[1]*(1-crop) and corner[0,1] < imsize[1]*crop
+            if check_x and check_y:
+              obj_points_tmp.append(obj_pt)
+              corners_tmp.append(corner)
+          obj_points_limited.append(np.array(obj_points_tmp))
+          corners_limited.append(np.array(corners_tmp))
         try:
-            whole = time.time()
-            while True:
-                intrinsic_array['f_x'].append(camera_matrix[0][0])
-                intrinsic_array['f_y'].append(camera_matrix[1][1])
-                intrinsic_array['c_x'].append(camera_matrix[0][2])
-                intrinsic_array['c_y'].append(camera_matrix[1][2])
-                num_threshold.append(threshold)
-
-                translation_array_x.append(np.mean(np.array(translation_vectors).T[0][0]))
-                translation_array_y.append(np.mean(np.array(translation_vectors).T[0][1]))
-                translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
-
-
-                start = time.time()
-                filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = self.features_filtering_function(rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = threshold)
-                num_corners.append(len(removed_corners))
-                iterations_array.append(index)
-                reprojection.append(ret)
-                for i in range(len(distortion_coefficients)):
-                    if i not in distortion_array:
-                        distortion_array[i] = []
-                    distortion_array[i].append(distortion_coefficients[i][0])
-                print(f"Each filtering {time.time() - start}")
-                start = time.time()
-                try:
-                    (ret, camera_matrix, distortion_coefficients,
-                     rotation_vectors, translation_vectors,
-                     stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-                     perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-                        charucoCorners=filtered_corners,
-                        charucoIds=filtered_ids,
-                        board=self._board,
-                        imageSize=imsize,
-                        cameraMatrix=cameraIntrinsics,
-                        distCoeffs=distCoeff,
-                        flags=flags,
-                        criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
-                except:
-                    raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=self.id)
-                cameraIntrinsics = camera_matrix
-                distCoeff = distortion_coefficients
-                threshold = 5 * imsize[1]/800.0
-                print(f"Each calibration {time.time()-start}")
-                index += 1
-                if  index > 5 or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
-                    print(f"Whole procedure: {time.time() - whole}")
-                    break
-                previous_ids = removed_ids
+          res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points_limited, corners_limited, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
+          print(f"success with crop factor {crop}")
+          success = True
+          break
         except:
-            return f"Failed to calibrate camera {name}", None, None, None, None, None, None, None ,None , None
-        return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
-
-    def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
-        one_pts = self._board.chessboardCorners
-        obj_points = []
-        for i in range(len(allIds)):
-            obj_points.append(self.charuco_ids_to_objpoints(allIds[i]))
-
-        f_init = imsize[0]/np.deg2rad(hfov)*1.15
-
-        cameraMatrixInit = np.array([[f_init, 0.          , imsize[0]/2],
-                                     [0.          , f_init, imsize[1]/2],
-                                     [0.          , 0.          , 1.          ]])
-        distCoeffsInit = np.zeros((4,1))
-         # check if there are any suspicious corners with high reprojection error
-        rvecs = []
-        tvecs = []
-        for corners, ids in zip(allCorners, allIds):
-            objpts = self.charuco_ids_to_objpoints(ids)
-            corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
-            rvec, tvec, new_ids = self.camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
-            tvecs.append(tvec)
-            rvecs.append(rvec)
-        corners_removed, filtered_ids, filtered_corners = self.filter_corner_outliers(allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
-        if corners_removed:
-            obj_points = []
-            for i in range(len(filtered_ids)):
-                obj_points.append(self.charuco_ids_to_objpoints(filtered_ids[i]))
- 
-        print("Camera Matrix initialization.............")
-        print(cameraMatrixInit)
-        flags = 0
-        flags |= cv2.fisheye.CALIB_CHECK_COND 
-        flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS 
-        flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
-        flags |= cv2.fisheye.CALIB_FIX_SKEW
-        
-
-        term_criteria = (cv2.TERM_CRITERIA_COUNT +
-                         cv2.TERM_CRITERIA_EPS, 30, 1e-9)
+          print(f"failed with crop factor {crop}")
+          if crop > 0.7:
+            crop -= 0.05
+          else:
+            raise Exception("Calibration failed: Tried maximum crop factor and still no success")
+      if success:
+        # trying the full FOV once more with better initial K
+        print(f"new K init {K}")
+        print(f"new d_init {d}")
         try:
-            res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
+          res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, imsize, K, distCoeffsInit, flags=flags, criteria=term_criteria)
         except:
-            # calibration failed for full FOV, let's try to limit the corners to smaller part of FOV first to find initial parameters
-            success = False
-            crop = 0.95
-            while not success:
-                print(f"trying crop factor {crop}")
-                obj_points_limited = []
-                corners_limited = []
-                for obj_pts, corners in zip(obj_points, filtered_corners):
-                    obj_points_tmp = []
-                    corners_tmp = []
-                    for obj_pt, corner in zip(obj_pts, corners):
-                        check_x = corner[0,0] > imsize[0]*(1-crop) and corner[0,0] < imsize[0]*crop
-                        check_y = corner[0,1] > imsize[1]*(1-crop) and corner[0,1] < imsize[1]*crop
-                        if check_x and check_y:
-                            obj_points_tmp.append(obj_pt)
-                            corners_tmp.append(corner)
-                    obj_points_limited.append(np.array(obj_points_tmp))
-                    corners_limited.append(np.array(corners_tmp))
-                try:
-                    res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points_limited, corners_limited, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
-                    print(f"success with crop factor {crop}")
-                    success = True
-                    break
-                except:
-                    print(f"failed with crop factor {crop}")
-                    if crop > 0.7:
-                        crop -= 0.05
-                    else:
-                        raise Exception("Calibration failed: Tried maximum crop factor and still no success")
-            if success:
-                # trying the full FOV once more with better initial K
-                print(f"new K init {K}")
-                print(f"new d_init {d}")
-                try:
-                    res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, imsize, K, distCoeffsInit, flags=flags, criteria=term_criteria) 
-                except:
-                    print(f"Failed the full res calib, using calibration with crop factor {crop}")
-        
-        return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
\ No newline at end of file
+          print(f"Failed the full res calib, using calibration with crop factor {crop}")
+
+    return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
\ No newline at end of file

From d24cb8f84b1f673e035c038d8e0b2c5c8048fe56 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 10:22:20 +0200
Subject: [PATCH 46/87] Use enumerate()

---
 calibration_utils.py | 25 ++++++++++---------------
 1 file changed, 10 insertions(+), 15 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index b4433db..d0ee800 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -371,39 +371,34 @@ def find_stereo_common_features(calibration, left_cam_info, right_cam_info):
   obj_pts = []
   one_pts = calibration._board.chessboardCorners
 
-  for i in range(len(allIds_l)):
+  for i, ids in enumerate(allIds_l):
     left_sub_corners = []
     right_sub_corners = []
     obj_pts_sub = []
-    #if len(allIds_l[i]) < 70 or len(allIds_r[i]) < 70:
-    #    continue
-    for j in range(len(allIds_l[i])):
-      idx = np.where(allIds_r[i] == allIds_l[i][j])
+
+    for j, id in enumerate(ids):
+      idx = np.where(allIds_r[i] == id)
       if idx[0].size == 0:
         continue
       left_sub_corners.append(allCorners_l[i][j]) # TODO : This copies even idxs that don't match
       right_sub_corners.append(allCorners_r[i][idx])
-      obj_pts_sub.append(one_pts[allIds_l[i][j]])
+      obj_pts_sub.append(one_pts[id])
     if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
       obj_pts.append(np.array(obj_pts_sub, dtype=np.float32))
       left_corners_sampled.append(
         np.array(left_sub_corners, dtype=np.float32))
-      left_ids_sampled.append(np.array(allIds_l[i], dtype=np.int32))
+      left_ids_sampled.append(np.array(ids, dtype=np.int32))
       right_corners_sampled.append(
         np.array(right_sub_corners, dtype=np.float32))
     else:
       return -1, "Stereo Calib failed due to less common features"
   return left_corners_sampled, right_corners_sampled, obj_pts
 
-def undistort_points_perspective(corners, camInfo):
-  for i in range(len(corners)):
-    corners[i] = cv2.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
-  return corners
+def undistort_points_perspective(allCorners, camInfo):
+  return [cv2.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
-def undistort_points_fisheye(corners, camInfo):
-  for i in range(len(corners)):
-    corners[i] = cv2.fisheye.undistortPoints(np.array(corners[i]), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics'])
-  return corners
+def undistort_points_fisheye(allCorners, camInfo):
+  return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
 def remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info):
   if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:

From c79676e80c21e69dedb62761a22c8c5ca85e9549 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 11:24:03 +0200
Subject: [PATCH 47/87] Move methods out of class

---
 calibration_utils.py | 1279 ++++++++++++++++++++----------------------
 1 file changed, 622 insertions(+), 657 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index d0ee800..b2a4589 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -117,7 +117,7 @@ def estimate_pose_and_filter_single(calibration, cam_info, corners, ids):
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
 def get_features(calibration, features, charucos, cam_info):
-  all_features, all_ids, imsize = calibration.getting_features(cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
+  all_features, all_ids, imsize = getting_features(calibration, cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
 
   if isinstance(all_features, str) and all_ids is None:
     raise RuntimeError(f'Exception {all_features}') # TODO : Handle
@@ -159,7 +159,7 @@ def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
   #   raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
 
-  distortion_flags = calibration.get_distortion_flags(cam_info['distortion_model'])
+  distortion_flags = get_distortion_flags(cam_info['distortion_model'])
   flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
 
   #try:
@@ -195,7 +195,7 @@ def filter_features_fisheye(calibration, cam_info, intrinsic_img, all_features,
 
   if cam_info["name"] in intrinsic_img:
     raise RuntimeError('This is broken')
-    all_features, all_ids, filtered_images = calibration.remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
+    all_features, all_ids, filtered_images = remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
   else:
     filtered_images = cam_info['images_path']
 
@@ -212,7 +212,7 @@ def filter_features_fisheye(calibration, cam_info, intrinsic_img, all_features,
 def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, filtered_corners, filtered_ids):
   start = time.time()
   print('starting calibrate_wf')
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_wf_intrinsics(cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(calibration, cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
   if isinstance(ret, str) and all_ids is None:
     raise RuntimeError('Exception' + ret) # TODO : Handle
   print(f'calibrate_wf took {round(time.time() - start, 2)}s')
@@ -227,8 +227,8 @@ def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, filtered_c
   return cam_info
 
 def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibration.calibrate_intrinsics(
-    cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_intrinsics(
+    calibration, cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
   cam_info['filtered_ids'] = filtered_ids
   cam_info['filtered_corners'] = filtered_corners
 
@@ -255,7 +255,7 @@ def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, rig
   # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
   # print(flags)
   flags = cv2.CALIB_FIX_INTRINSIC
-  distortion_flags = calibration.get_distortion_flags(left_distortion_model)
+  distortion_flags = get_distortion_flags(left_distortion_model)
   flags += distortion_flags
   # print(flags)
   ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
@@ -407,10 +407,10 @@ def remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info
     elif right_cam_info["name"] in calibration.extrinsic_img:
       array = calibration.extrinsic_img[left_cam_info["name"]]
 
-    left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = calibration.remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-    right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = calibration.remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = calibration.filtering_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
-    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = calibration.filtering_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info["hfov"], left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
+    left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
+    right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
+    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = filtering_features(calibration, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
+    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = filtering_features(calibration, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
   return left_cam_info, right_cam_info
 
 def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam, board_config, extrinsics):
@@ -494,6 +494,616 @@ def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charuco
   cam_info['images_path'] = images_path
   return cam_info
 
+def getting_features(self, img_path, width, height, features = None, charucos=None):
+  if charucos:
+    allCorners = []
+    allIds = []
+    for index, charuco_img in enumerate(charucos):
+      ids, charuco = charuco_img
+      allCorners.append(charuco)
+      allIds.append(ids)
+    imsize = (width, height)
+    return allCorners, allIds, imsize
+
+  elif features == None or features == "charucos":
+    allCorners, allIds, _, _, imsize, _ = analyze_charuco(self, img_path)
+    return allCorners, allIds, imsize
+
+  if features == "checker_board":
+    allCorners, allIds, _, _, imsize, _ = analyze_charuco(self, img_path)
+    return allCorners, allIds, imsize
+  ###### ADD HERE WHAT IT IS NEEDED ######
+
+def filtering_features(self, allCorners, allIds, name,imsize, cam_info, cameraMatrixInit, distCoeffsInit, distortionModel):
+
+   # check if there are any suspicious corners with high reprojection error
+  filtered_corners, filtered_ids, removed_corners = estimate_pose_and_filter(self, cam_info, allCorners, allIds)
+
+  distortion_flags = get_distortion_flags(distortionModel)
+  flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
+
+  try:
+    (ret, camera_matrix, distortion_coefficients,
+         rotation_vectors, translation_vectors,
+         stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+         perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+          charucoCorners=filtered_corners,
+          charucoIds=filtered_ids,
+          board=self._board,
+          imageSize=imsize,
+          cameraMatrix=cameraMatrixInit,
+          distCoeffs=distCoeffsInit,
+          flags=flags,
+          criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
+  except:
+    return f"First intrisic calibration failed for {name}", None, None
+
+  return removed_corners, filtered_corners, filtered_ids, camera_matrix, distortion_coefficients
+
+def remove_features(allCorners, allIds, array, img_files = None):
+  filteredCorners = allCorners.copy()
+  filteredIds = allIds.copy()
+  if img_files is not None:
+    img_path = img_files.copy()
+
+  for index in array:
+    filteredCorners.pop(index)
+    filteredIds.pop(index)
+    if img_files is not None:
+      img_path.pop(index)
+
+  return filteredCorners, filteredIds, img_path
+
+def get_distortion_flags(distortionModel):
+  def is_binary_string(s: str) -> bool:
+  # Check if all characters in the string are '0' or '1'
+    return all(char in '01' for char in s)
+  if distortionModel == None:
+    print("Use DEFAULT model")
+    flags = cv2.CALIB_RATIONAL_MODEL
+  elif is_binary_string(distortionModel):
+    flags = cv2.CALIB_RATIONAL_MODEL
+    flags += cv2.CALIB_TILTED_MODEL
+    flags += cv2.CALIB_THIN_PRISM_MODEL
+    binary_number = int(distortionModel, 2)
+    # Print the results
+    if binary_number == 0:
+      clauses_status = [True, True,True, True, True, True, True, True, True]
+    else:
+      clauses_status = [(binary_number & (1 << i)) != 0 for i in range(len(distortionModel))]
+      clauses_status = clauses_status[::-1]
+    if clauses_status[0]:
+      print("FIX_K1")
+      flags += cv2.CALIB_FIX_K1
+    if clauses_status[1]:
+      print("FIX_K2")
+      flags += cv2.CALIB_FIX_K2
+    if clauses_status[2]:
+      print("FIX_K3")
+      flags += cv2.CALIB_FIX_K3
+    if clauses_status[3]:
+      print("FIX_K4")
+      flags += cv2.CALIB_FIX_K4
+    if clauses_status[4]:
+      print("FIX_K5")
+      flags += cv2.CALIB_FIX_K5
+    if clauses_status[5]:
+      print("FIX_K6")
+      flags += cv2.CALIB_FIX_K6
+    if clauses_status[6]:
+      print("FIX_TANGENT_DISTORTION")
+      flags += cv2.CALIB_ZERO_TANGENT_DIST
+    if clauses_status[7]:
+      print("FIX_TILTED_DISTORTION")
+      flags += cv2.CALIB_FIX_TAUX_TAUY
+    if clauses_status[8]:
+      print("FIX_PRISM_DISTORTION")
+      flags += cv2.CALIB_FIX_S1_S2_S3_S4
+
+  elif isinstance(distortionModel, str):
+    if distortionModel == "NORMAL":
+      print("Using NORMAL model")
+      flags = cv2.CALIB_RATIONAL_MODEL
+      flags += cv2.CALIB_TILTED_MODEL
+
+    elif distortionModel == "TILTED":
+      print("Using TILTED model")
+      flags = cv2.CALIB_RATIONAL_MODEL
+      flags += cv2.CALIB_TILTED_MODEL
+
+    elif distortionModel == "PRISM":
+      print("Using PRISM model")
+      flags = cv2.CALIB_RATIONAL_MODEL
+      flags += cv2.CALIB_TILTED_MODEL
+      flags += cv2.CALIB_THIN_PRISM_MODEL
+
+    elif distortionModel == "THERMAL":
+      print("Using THERMAL model")
+      flags = cv2.CALIB_RATIONAL_MODEL
+      flags += cv2.CALIB_FIX_K3
+      flags += cv2.CALIB_FIX_K5
+      flags += cv2.CALIB_FIX_K6
+
+  elif isinstance(distortionModel, int):
+    print("Using CUSTOM flags")
+    flags = distortionModel
+  return flags
+
+def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
+  coverageImage = np.ones(imsize[::-1], np.uint8) * 255
+  coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
+  coverageImage = draw_corners(allCorners, coverageImage)
+  if calib_model == 'perspective':
+    if features == None or features == "charucos":
+      distortion_flags = get_distortion_flags(distortionModel)
+      ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
+        self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
+
+      return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+    else:
+      return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+    #### ADD ADDITIONAL FEATURES CALIBRATION ####
+  else:
+    if features == None or features == "charucos":
+      print('Fisheye--------------------------------------------------')
+      ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
+        self, allCorners, allIds, imsize, hfov, name)
+      print('Fisheye rotation vector', rotation_vectors[0])
+      print('Fisheye translation vector', translation_vectors[0])
+
+      # (Height, width)
+      return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+
+def draw_corners(charuco_corners, displayframe):
+  for corners in charuco_corners:
+    color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255)))
+    for corner in corners:
+      corner_int = (int(corner[0][0]), int(corner[0][1]))
+      cv2.circle(displayframe, corner_int, 4, color, -1)
+  height, width = displayframe.shape[:2]
+  start_point = (0, 0)  # top of the image
+  end_point = (0, height)
+
+  color = (0, 0, 0)  # blue in BGR
+  thickness = 4
+
+  # Draw the line on the image
+  cv2.line(displayframe, start_point, end_point, color, thickness)
+  return displayframe
+
+def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, reprojection, filtered_corners,filtered_id, camera, display = True, threshold = None, draw_quadrants = False, nx = 4, ny = 4):
+  whole_error = []
+  all_points = []
+  all_corners = []
+  all_error = []
+  all_ids = []
+  removed_corners = []
+  removed_points = []
+  removed_ids = []
+  removed_error = []
+  display_corners = []
+  display_points = []
+  circle_size = 0
+  reported_error = []
+  for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
+    if ids is not None and corners.size > 0:
+      ids = ids.flatten()  # Flatten the IDs from 2D to 1D
+      objPoints = np.array([self._board.chessboardCorners[id] for id in ids], dtype=np.float32)
+      imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
+      corners2 = corners.reshape(-1, 2)
+      imgpoints2 = imgpoints2.reshape(-1, 2)
+
+      errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
+      if threshold == None:
+        threshold = max(2*np.median(errors), 150)
+      valid_mask = errors <= threshold
+      removed_mask = ~valid_mask
+
+      # Collect valid IDs in the original format (array of arrays)
+      valid_ids = ids[valid_mask]
+      all_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+
+      # Collect data for valid points
+      reported_error.extend(errors)
+      all_error.extend(errors[valid_mask])
+      display_corners.extend(corners2)
+      display_points.extend(imgpoints2[valid_mask])
+      all_points.append(imgpoints2[valid_mask])  # Collect valid points for calibration
+      all_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+
+      removed_corners.extend(corners2[removed_mask])
+      removed_points.extend(imgpoints2[removed_mask])
+      removed_ids.extend(ids[removed_mask])
+      removed_error.extend(errors[removed_mask])
+
+      total_error_squared = np.sum(errors[valid_mask]**2)
+      total_points = len(objPoints[valid_mask])
+      rms_error = np.sqrt(total_error_squared / total_points if total_points else 0)
+      whole_error.append(rms_error)
+
+    total_error_squared = 0
+    total_points = 0
+
+  return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
+
+def detect_charuco_board(self, image: np.array):
+  arucoParams = cv2.aruco.DetectorParameters_create()
+  arucoParams.minMarkerDistanceRate = 0.01
+  corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, self._aruco_dictionary, parameters=arucoParams)  # First, detect markers
+  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, self._board, corners, ids, rejectedCorners=rejectedImgPoints)
+  # If found, add object points, image points (after refining them)
+  if len(marker_corners) > 0:
+    ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, self._board, minMarkers = 1)
+    return ret, corners, ids, marker_corners, marker_ids
+  else:
+    return None, None, None, None, None
+
+def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
+  objects = []
+  all_objects = []
+  index = 0
+  start_time = time.time()
+  while len(objects) < len(objpoints[:,0,0]) * min_inliers:
+    if ini_threshold > max_threshold:
+      break
+    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+    all_objects.append(objects)
+    imgpoints2 = objpoints.copy()
+
+    all_corners = corners.copy()
+    all_corners = np.array([all_corners[id[0]] for id in objects])
+    imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+
+    ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners, K, d)
+    imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, K, d)
+
+    ini_threshold += threshold_stepper
+    index += 1
+  if ret:
+    return rvec, tvec, objects
+  else:
+    return None
+
+def compute_reprojection_errors(obj_pts: np.array, img_pts: np.array, K: np.array, dist: np.array, rvec: np.array, tvec: np.array, fisheye = False):
+  if fisheye:
+    proj_pts, _ = cv2.fisheye.projectPoints(obj_pts, rvec, tvec, K, dist)
+  else:
+    proj_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist)
+  errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
+  return errs
+
+def charuco_ids_to_objpoints(self, ids):
+  one_pts = self._board.chessboardCorners
+  objpts = []
+  for j in range(len(ids)):
+    objpts.append(one_pts[ids[j]])
+  return np.array(objpts)
+
+def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
+  """
+  Charuco base pose estimation.
+  """
+  # print("POSE ESTIMATION STARTS:")
+  allCorners = []
+  allIds = []
+  all_marker_corners = []
+  all_marker_ids = []
+  all_recovered = []
+  # decimator = 0
+  # SUB PIXEL CORNER DETECTION CRITERION
+  criteria = (cv2.TERM_CRITERIA_EPS +
+        cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
+  count = 0
+  skip_vis = False
+  for im in images:
+    img_pth = Path(im)
+    frame = cv2.imread(im)
+    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+    expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1])
+
+    if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]):
+      if int(expected_height) == req_resolution[0]:
+        # resizing to have both stereo and rgb to have same
+        # resolution to capture extrinsics of the rgb-right camera
+        gray = cv2.resize(gray, req_resolution[::-1],
+                  interpolation=cv2.INTER_CUBIC)
+      else:
+        # resizing and cropping to have both stereo and rgb to have same resolution
+        # to calculate extrinsics of the rgb-right camera
+        scale_width = req_resolution[1]/gray.shape[1]
+        dest_res = (
+          int(gray.shape[1] * scale_width), int(gray.shape[0] * scale_width))
+        gray = cv2.resize(
+          gray, dest_res, interpolation=cv2.INTER_CUBIC)
+        if gray.shape[0] < req_resolution[0]:
+          raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
+            gray.shape[0], req_resolution[0]))
+        # print(gray.shape[0] - req_resolution[0])
+        del_height = (gray.shape[0] - req_resolution[0]) // 2
+        # gray = gray[: req_resolution[0], :]
+        gray = gray[del_height: del_height + req_resolution[0], :]
+
+      count += 1
+
+    ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = detect_charuco_board(self, gray)
+
+    if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
+
+      charuco_corners = cv2.cornerSubPix(gray, charuco_corners,
+                winSize=(5, 5),
+                zeroZone=(-1, -1),
+                criteria=criteria)
+      allCorners.append(charuco_corners)  # Charco chess corners
+      allIds.append(charuco_ids)  # charuco chess corner id's
+      all_marker_corners.append(marker_corners)
+      all_marker_ids.append(marker_ids)
+    else:
+      print(im)
+      return f'Failed to detect more than 3 markers on image {im}', None, None, None, None, None
+
+  # imsize = gray.shape[::-1]
+  return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
+
+def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
+  image_files = glob.glob(image_files + "/*")
+  image_files.sort()
+  assert len(
+    image_files) != 0, "ERROR: Images not read correctly, check directory"
+  if charucos == {}:
+    allCorners, allIds, _, _, imsize, _ = analyze_charuco(self, image_files)
+  else:
+    allCorners = []
+    allIds = []
+    for index, charuco_img in enumerate(charucos[name]):
+      ids, charucos = charuco_img
+      allCorners.append(charucos)
+      allIds.append(ids)
+    imsize = (height, width)
+
+  coverageImage = np.ones(imsize[::-1], np.uint8) * 255
+  coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
+  coverageImage = draw_corners(allCorners, coverageImage)
+  if calib_model == 'perspective':
+    distortion_flags = get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
+    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
+      self, allCorners, allIds, imsize, hfov, name, distortion_flags)
+    undistort_visualization(
+      self, image_files, camera_matrix, distortion_coefficients, imsize, name)
+
+    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+  else:
+    print('Fisheye--------------------------------------------------')
+    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
+      self, allCorners, allIds, imsize, hfov, name)
+    undistort_visualization(
+      self, image_files, camera_matrix, distortion_coefficients, imsize, name)
+    print('Fisheye rotation vector', rotation_vectors[0])
+    print('Fisheye translation vector', translation_vectors[0])
+
+    # (Height, width)
+    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+
+def undistort_visualization(self, img_list, K, D, img_size, name):
+  for index, im in enumerate(img_list):
+    # print(im)
+    img = cv2.imread(im)
+    # h, w = img.shape[:2]
+    if self._cameraModel == 'perspective':
+      kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0)
+      # print(f'K scaled is \n {kScaled} and size is \n {img_size}')
+      # print(f'D Value is \n {D}')
+      map1, map2 = cv2.initUndistortRectifyMap(
+        K, D, np.eye(3), kScaled, img_size, cv2.CV_32FC1)
+    else:
+      map1, map2 = cv2.fisheye.initUndistortRectifyMap(
+        K, D, np.eye(3), K, img_size, cv2.CV_32FC1)
+
+    undistorted_img = cv2.remap(
+      img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
+
+    if index == 0:
+      undistorted_file_path = self.data_path + '/' + name + f'_undistorted.png'
+      cv2.imwrite(undistorted_file_path, undistorted_img)
+
+def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
+  corners_removed = False
+  for i in range(len(allIds)):
+    corners = allCorners[i]
+    ids = allIds[i]
+    objpts = charuco_ids_to_objpoints(self, ids)
+    if self._cameraModel == "fisheye":
+      errs = compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
+    else:
+      errs = compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i])
+    suspicious_err_thr = max(2*np.median(errs), 100)
+    n_offending_pts = np.sum(errs > suspicious_err_thr)
+    offending_pts_idxs = np.where(errs > suspicious_err_thr)[0]
+    # check if there are offending points and if they form a minority
+    if n_offending_pts > 0 and n_offending_pts < len(corners)/5:
+      print(f"removing {n_offending_pts} offending points with errs {errs[offending_pts_idxs]}")
+      corners_removed = True
+      #remove the offending points
+      offset = 0
+      allCorners[i] = np.delete(allCorners[i],offending_pts_idxs, axis = 0)
+      allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
+  return corners_removed, allIds, allCorners
+
+def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
+  """
+  Calibrates the camera using the dected corners.
+  """
+  f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2)))
+
+  threshold = 2 * imsize[1]/800.0
+   # check if there are any suspicious corners with high reprojection error
+  rvecs = []
+  tvecs = []
+  index = 0
+  max_threshold = 10 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
+  min_inlier = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
+  for corners, ids in zip(allCorners, allIds):
+    objpts = charuco_ids_to_objpoints(self, ids)
+    rvec, tvec, newids = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
+    tvecs.append(tvec)
+    rvecs.append(rvec)
+    index += 1
+
+  # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
+  ret = 0.0
+  flags = cv2.CALIB_USE_INTRINSIC_GUESS
+  flags += distortion_flags
+
+  #   flags = (cv2.CALIB_RATIONAL_MODEL)
+  reprojection = []
+  removed_errors = []
+  num_corners = []
+  num_threshold = []
+  iterations_array = []
+  intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
+  distortion_array = {}
+  index = 0
+  camera_matrix = cameraIntrinsics
+  distortion_coefficients = distCoeff
+  rotation_vectors = rvecs
+  translation_vectors = tvecs
+  translation_array_x = []
+  translation_array_y = []
+  translation_array_z = []
+  corner_checker = 0
+  previous_ids = []
+  import time
+  try:
+    whole = time.time()
+    while True:
+      intrinsic_array['f_x'].append(camera_matrix[0][0])
+      intrinsic_array['f_y'].append(camera_matrix[1][1])
+      intrinsic_array['c_x'].append(camera_matrix[0][2])
+      intrinsic_array['c_y'].append(camera_matrix[1][2])
+      num_threshold.append(threshold)
+
+      translation_array_x.append(np.mean(np.array(translation_vectors).T[0][0]))
+      translation_array_y.append(np.mean(np.array(translation_vectors).T[0][1]))
+      translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
+
+      start = time.time()
+      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(self, rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = threshold)
+      num_corners.append(len(removed_corners))
+      iterations_array.append(index)
+      reprojection.append(ret)
+      for i in range(len(distortion_coefficients)):
+        if i not in distortion_array:
+          distortion_array[i] = []
+        distortion_array[i].append(distortion_coefficients[i][0])
+      print(f"Each filtering {time.time() - start}")
+      start = time.time()
+      try:
+        (ret, camera_matrix, distortion_coefficients,
+         rotation_vectors, translation_vectors,
+         stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+         perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+          charucoCorners=filtered_corners,
+          charucoIds=filtered_ids,
+          board=self._board,
+          imageSize=imsize,
+          cameraMatrix=cameraIntrinsics,
+          distCoeffs=distCoeff,
+          flags=flags,
+          criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
+      except:
+        raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=self.id)
+      cameraIntrinsics = camera_matrix
+      distCoeff = distortion_coefficients
+      threshold = 5 * imsize[1]/800.0
+      print(f"Each calibration {time.time()-start}")
+      index += 1
+      if  index > 5 or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
+        print(f"Whole procedure: {time.time() - whole}")
+        break
+      previous_ids = removed_ids
+  except:
+    return f"Failed to calibrate camera {name}", None, None, None, None, None, None, None ,None , None
+  return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
+
+def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
+  one_pts = self._board.chessboardCorners
+  obj_points = []
+  for i in range(len(allIds)):
+    obj_points.append(charuco_ids_to_objpoints(self, allIds[i]))
+
+  f_init = imsize[0]/np.deg2rad(hfov)*1.15
+
+  cameraMatrixInit = np.array([[f_init, 0.      , imsize[0]/2],
+                 [0.      , f_init, imsize[1]/2],
+                 [0.      , 0.      , 1.      ]])
+  distCoeffsInit = np.zeros((4,1))
+   # check if there are any suspicious corners with high reprojection error
+  rvecs = []
+  tvecs = []
+  for corners, ids in zip(allCorners, allIds):
+    objpts = charuco_ids_to_objpoints(self, ids)
+    corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
+    rvec, tvec, new_ids = camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
+    tvecs.append(tvec)
+    rvecs.append(rvec)
+  corners_removed, filtered_ids, filtered_corners = filter_corner_outliers(self, allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
+  if corners_removed:
+    obj_points = []
+    for i in range(len(filtered_ids)):
+      obj_points.append(charuco_ids_to_objpoints(self, filtered_ids[i]))
+
+  print("Camera Matrix initialization.............")
+  print(cameraMatrixInit)
+  flags = 0
+  flags |= cv2.fisheye.CALIB_CHECK_COND
+  flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS
+  flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
+  flags |= cv2.fisheye.CALIB_FIX_SKEW
+
+  term_criteria = (cv2.TERM_CRITERIA_COUNT +
+           cv2.TERM_CRITERIA_EPS, 30, 1e-9)
+  try:
+    res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
+  except:
+    # calibration failed for full FOV, let's try to limit the corners to smaller part of FOV first to find initial parameters
+    success = False
+    crop = 0.95
+    while not success:
+      print(f"trying crop factor {crop}")
+      obj_points_limited = []
+      corners_limited = []
+      for obj_pts, corners in zip(obj_points, filtered_corners):
+        obj_points_tmp = []
+        corners_tmp = []
+        for obj_pt, corner in zip(obj_pts, corners):
+          check_x = corner[0,0] > imsize[0]*(1-crop) and corner[0,0] < imsize[0]*crop
+          check_y = corner[0,1] > imsize[1]*(1-crop) and corner[0,1] < imsize[1]*crop
+          if check_x and check_y:
+            obj_points_tmp.append(obj_pt)
+            corners_tmp.append(corner)
+        obj_points_limited.append(np.array(obj_points_tmp))
+        corners_limited.append(np.array(corners_tmp))
+      try:
+        res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points_limited, corners_limited, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
+        print(f"success with crop factor {crop}")
+        success = True
+        break
+      except:
+        print(f"failed with crop factor {crop}")
+        if crop > 0.7:
+          crop -= 0.05
+        else:
+          raise Exception("Calibration failed: Tried maximum crop factor and still no success")
+    if success:
+      # trying the full FOV once more with better initial K
+      print(f"new K init {K}")
+      print(f"new d_init {d}")
+      try:
+        res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, imsize, K, distCoeffsInit, flags=flags, criteria=term_criteria)
+      except:
+        print(f"Failed the full res calib, using calibration with crop factor {crop}")
+
+  return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
+
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
@@ -623,649 +1233,4 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
       if stereoConfig.ret():
         board_config['stereo_config'].update(stereoConfig.ret())
 
-    return 1, board_config
-
-  def getting_features(self, img_path, width, height, features = None, charucos=None):
-    if charucos:
-      allCorners = []
-      allIds = []
-      for index, charuco_img in enumerate(charucos):
-        ids, charuco = charuco_img
-        allCorners.append(charuco)
-        allIds.append(ids)
-      imsize = (width, height)
-      return allCorners, allIds, imsize
-
-    elif features == None or features == "charucos":
-      allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
-      return allCorners, allIds, imsize
-
-    if features == "checker_board":
-      allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(img_path)
-      return allCorners, allIds, imsize
-    ###### ADD HERE WHAT IT IS NEEDED ######
-
-  def estimate_pose_and_filter(self, allCorners, allIds, name,imsize, hfov, K, d):
-     # check if there are any suspicious corners with high reprojection error
-    index = 0
-    max_threshold = 75 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
-    threshold_stepper = int(1.5 * (hfov / 30 + imsize[1] / 800))
-    if threshold_stepper < 1:
-      threshold_stepper = 1
-    print(threshold_stepper)
-    min_inliers = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
-    overall_pose = time.time()
-    for index, corners in enumerate(allCorners):
-      if len(corners) < 4:
-        raise RuntimeError(f"Less than 4 corners detected on {index} image.")
-    current = time.time()
-
-    filtered_corners = []
-    filtered_ids = []
-    removed_corners = []
-
-    #for corners, ids in zip(allCorners, allIds):
-    current = time.time()
-
-    with multiprocessing.Pool(processes=16) as pool:
-      results = pool.map(func=estimate_pose_and_filter_single, iterable=[(self, a, b, K, d, min_inliers, max_threshold, threshold_stepper) for a, b in zip(allCorners, allIds)])
-
-    filtered_ids, filtered_corners, removed_corners = zip(*results)
-
-    print(f"Overall pose estimation {time.time() - overall_pose}s")
-
-    if sum([len(corners) < 4 for corners in filtered_corners]) > 0.15 * len(allCorners):
-      raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {name}")
-    print(f"Filtering {time.time() -current}s")
-
-    return filtered_corners, filtered_ids, removed_corners
-
-  def filtering_features(self, allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit, distortionModel):
-
-     # check if there are any suspicious corners with high reprojection error
-    filtered_corners, filtered_ids, removed_corners = self.estimate_pose_and_filter(allCorners, allIds, name,imsize, hfov, cameraMatrixInit, distCoeffsInit)
-
-    distortion_flags = self.get_distortion_flags(distortionModel)
-    flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
-
-    try:
-      (ret, camera_matrix, distortion_coefficients,
-           rotation_vectors, translation_vectors,
-           stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-           perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-            charucoCorners=filtered_corners,
-            charucoIds=filtered_ids,
-            board=self._board,
-            imageSize=imsize,
-            cameraMatrix=cameraMatrixInit,
-            distCoeffs=distCoeffsInit,
-            flags=flags,
-            criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
-    except:
-      return f"First intrisic calibration failed for {name}", None, None
-
-    return removed_corners, filtered_corners, filtered_ids, camera_matrix, distortion_coefficients
-
-  def remove_features(self, allCorners, allIds, array, img_files = None):
-    filteredCorners = allCorners.copy()
-    filteredIds = allIds.copy()
-    if img_files is not None:
-      img_path = img_files.copy()
-
-    for index in array:
-      filteredCorners.pop(index)
-      filteredIds.pop(index)
-      if img_files is not None:
-        img_path.pop(index)
-
-    return filteredCorners, filteredIds, img_path
-
-  def get_distortion_flags(self, distortionModel):
-    def is_binary_string(s: str) -> bool:
-    # Check if all characters in the string are '0' or '1'
-      return all(char in '01' for char in s)
-    if distortionModel == None:
-      print("Use DEFAULT model")
-      flags = cv2.CALIB_RATIONAL_MODEL
-    elif is_binary_string(distortionModel):
-      flags = cv2.CALIB_RATIONAL_MODEL
-      flags += cv2.CALIB_TILTED_MODEL
-      flags += cv2.CALIB_THIN_PRISM_MODEL
-      binary_number = int(distortionModel, 2)
-      # Print the results
-      if binary_number == 0:
-        clauses_status = [True, True,True, True, True, True, True, True, True]
-      else:
-        clauses_status = [(binary_number & (1 << i)) != 0 for i in range(len(distortionModel))]
-        clauses_status = clauses_status[::-1]
-      if clauses_status[0]:
-        print("FIX_K1")
-        flags += cv2.CALIB_FIX_K1
-      if clauses_status[1]:
-        print("FIX_K2")
-        flags += cv2.CALIB_FIX_K2
-      if clauses_status[2]:
-        print("FIX_K3")
-        flags += cv2.CALIB_FIX_K3
-      if clauses_status[3]:
-        print("FIX_K4")
-        flags += cv2.CALIB_FIX_K4
-      if clauses_status[4]:
-        print("FIX_K5")
-        flags += cv2.CALIB_FIX_K5
-      if clauses_status[5]:
-        print("FIX_K6")
-        flags += cv2.CALIB_FIX_K6
-      if clauses_status[6]:
-        print("FIX_TANGENT_DISTORTION")
-        flags += cv2.CALIB_ZERO_TANGENT_DIST
-      if clauses_status[7]:
-        print("FIX_TILTED_DISTORTION")
-        flags += cv2.CALIB_FIX_TAUX_TAUY
-      if clauses_status[8]:
-        print("FIX_PRISM_DISTORTION")
-        flags += cv2.CALIB_FIX_S1_S2_S3_S4
-
-    elif isinstance(distortionModel, str):
-      if distortionModel == "NORMAL":
-        print("Using NORMAL model")
-        flags = cv2.CALIB_RATIONAL_MODEL
-        flags += cv2.CALIB_TILTED_MODEL
-
-      elif distortionModel == "TILTED":
-        print("Using TILTED model")
-        flags = cv2.CALIB_RATIONAL_MODEL
-        flags += cv2.CALIB_TILTED_MODEL
-
-      elif distortionModel == "PRISM":
-        print("Using PRISM model")
-        flags = cv2.CALIB_RATIONAL_MODEL
-        flags += cv2.CALIB_TILTED_MODEL
-        flags += cv2.CALIB_THIN_PRISM_MODEL
-
-      elif distortionModel == "THERMAL":
-        print("Using THERMAL model")
-        flags = cv2.CALIB_RATIONAL_MODEL
-        flags += cv2.CALIB_FIX_K3
-        flags += cv2.CALIB_FIX_K5
-        flags += cv2.CALIB_FIX_K6
-
-    elif isinstance(distortionModel, int):
-      print("Using CUSTOM flags")
-      flags = distortionModel
-    return flags
-
-  def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
-    coverageImage = np.ones(imsize[::-1], np.uint8) * 255
-    coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
-    coverageImage = self.draw_corners(allCorners, coverageImage)
-    if calib_model == 'perspective':
-      if features == None or features == "charucos":
-        distortion_flags = self.get_distortion_flags(distortionModel)
-        ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
-          allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
-
-        return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-      else:
-        return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-      #### ADD ADDITIONAL FEATURES CALIBRATION ####
-    else:
-      if features == None or features == "charucos":
-        print('Fisheye--------------------------------------------------')
-        ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = self.calibrate_fisheye(
-          allCorners, allIds, imsize, hfov, name)
-        print('Fisheye rotation vector', rotation_vectors[0])
-        print('Fisheye translation vector', translation_vectors[0])
-
-        # (Height, width)
-        return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-
-  def draw_corners(self, charuco_corners, displayframe):
-    for corners in charuco_corners:
-      color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255)))
-      for corner in corners:
-        corner_int = (int(corner[0][0]), int(corner[0][1]))
-        cv2.circle(displayframe, corner_int, 4, color, -1)
-    height, width = displayframe.shape[:2]
-    start_point = (0, 0)  # top of the image
-    end_point = (0, height)
-
-    color = (0, 0, 0)  # blue in BGR
-    thickness = 4
-
-    # Draw the line on the image
-    cv2.line(displayframe, start_point, end_point, color, thickness)
-    return displayframe
-
-  def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, reprojection, filtered_corners,filtered_id, camera, display = True, threshold = None, draw_quadrants = False, nx = 4, ny = 4):
-    whole_error = []
-    all_points = []
-    all_corners = []
-    all_error = []
-    all_ids = []
-    removed_corners = []
-    removed_points = []
-    removed_ids = []
-    removed_error = []
-    display_corners = []
-    display_points = []
-    circle_size = 0
-    reported_error = []
-    for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
-      if ids is not None and corners.size > 0:
-        ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-        objPoints = np.array([self._board.chessboardCorners[id] for id in ids], dtype=np.float32)
-        imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
-        corners2 = corners.reshape(-1, 2)
-        imgpoints2 = imgpoints2.reshape(-1, 2)
-
-        errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
-        if threshold == None:
-          threshold = max(2*np.median(errors), 150)
-        valid_mask = errors <= threshold
-        removed_mask = ~valid_mask
-
-        # Collect valid IDs in the original format (array of arrays)
-        valid_ids = ids[valid_mask]
-        all_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
-
-        # Collect data for valid points
-        reported_error.extend(errors)
-        all_error.extend(errors[valid_mask])
-        display_corners.extend(corners2)
-        display_points.extend(imgpoints2[valid_mask])
-        all_points.append(imgpoints2[valid_mask])  # Collect valid points for calibration
-        all_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
-
-        removed_corners.extend(corners2[removed_mask])
-        removed_points.extend(imgpoints2[removed_mask])
-        removed_ids.extend(ids[removed_mask])
-        removed_error.extend(errors[removed_mask])
-
-        total_error_squared = np.sum(errors[valid_mask]**2)
-        total_points = len(objPoints[valid_mask])
-        rms_error = np.sqrt(total_error_squared / total_points if total_points else 0)
-        whole_error.append(rms_error)
-
-      total_error_squared = 0
-      total_points = 0
-
-    return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
-
-  def detect_charuco_board(self, image: np.array):
-    arucoParams = cv2.aruco.DetectorParameters_create()
-    arucoParams.minMarkerDistanceRate = 0.01
-    corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, self._aruco_dictionary, parameters=arucoParams)  # First, detect markers
-    marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, self._board, corners, ids, rejectedCorners=rejectedImgPoints)
-    # If found, add object points, image points (after refining them)
-    if len(marker_corners) > 0:
-      ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, self._board, minMarkers = 1)
-      return ret, corners, ids, marker_corners, marker_ids
-    else:
-      return None, None, None, None, None
-
-  def camera_pose_charuco(self, objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
-    objects = []
-    all_objects = []
-    index = 0
-    start_time = time.time()
-    while len(objects) < len(objpoints[:,0,0]) * min_inliers:
-      if ini_threshold > max_threshold:
-        break
-      ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-      all_objects.append(objects)
-      imgpoints2 = objpoints.copy()
-
-      all_corners = corners.copy()
-      all_corners = np.array([all_corners[id[0]] for id in objects])
-      imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
-
-      ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners, K, d)
-      imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, K, d)
-
-      ini_threshold += threshold_stepper
-      index += 1
-    if ret:
-      return rvec, tvec, objects
-    else:
-      return None
-
-  def compute_reprojection_errors(self, obj_pts: np.array, img_pts: np.array, K: np.array, dist: np.array, rvec: np.array, tvec: np.array, fisheye = False):
-    if fisheye:
-      proj_pts, _ = cv2.fisheye.projectPoints(obj_pts, rvec, tvec, K, dist)
-    else:
-      proj_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist)
-    errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
-    return errs
-
-  def charuco_ids_to_objpoints(self, ids):
-    one_pts = self._board.chessboardCorners
-    objpts = []
-    for j in range(len(ids)):
-      objpts.append(one_pts[ids[j]])
-    return np.array(objpts)
-
-  def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
-    """
-    Charuco base pose estimation.
-    """
-    # print("POSE ESTIMATION STARTS:")
-    allCorners = []
-    allIds = []
-    all_marker_corners = []
-    all_marker_ids = []
-    all_recovered = []
-    # decimator = 0
-    # SUB PIXEL CORNER DETECTION CRITERION
-    criteria = (cv2.TERM_CRITERIA_EPS +
-          cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
-    count = 0
-    skip_vis = False
-    for im in images:
-      img_pth = Path(im)
-      frame = cv2.imread(im)
-      gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-      expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1])
-
-      if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]):
-        if int(expected_height) == req_resolution[0]:
-          # resizing to have both stereo and rgb to have same
-          # resolution to capture extrinsics of the rgb-right camera
-          gray = cv2.resize(gray, req_resolution[::-1],
-                    interpolation=cv2.INTER_CUBIC)
-        else:
-          # resizing and cropping to have both stereo and rgb to have same resolution
-          # to calculate extrinsics of the rgb-right camera
-          scale_width = req_resolution[1]/gray.shape[1]
-          dest_res = (
-            int(gray.shape[1] * scale_width), int(gray.shape[0] * scale_width))
-          gray = cv2.resize(
-            gray, dest_res, interpolation=cv2.INTER_CUBIC)
-          if gray.shape[0] < req_resolution[0]:
-            raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
-              gray.shape[0], req_resolution[0]))
-          # print(gray.shape[0] - req_resolution[0])
-          del_height = (gray.shape[0] - req_resolution[0]) // 2
-          # gray = gray[: req_resolution[0], :]
-          gray = gray[del_height: del_height + req_resolution[0], :]
-
-        count += 1
-
-      ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = self.detect_charuco_board(gray)
-
-      if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
-
-        charuco_corners = cv2.cornerSubPix(gray, charuco_corners,
-                  winSize=(5, 5),
-                  zeroZone=(-1, -1),
-                  criteria=criteria)
-        allCorners.append(charuco_corners)  # Charco chess corners
-        allIds.append(charuco_ids)  # charuco chess corner id's
-        all_marker_corners.append(marker_corners)
-        all_marker_ids.append(marker_ids)
-      else:
-        print(im)
-        return f'Failed to detect more than 3 markers on image {im}', None, None, None, None, None
-
-    # imsize = gray.shape[::-1]
-    return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
-
-  def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
-    image_files = glob.glob(image_files + "/*")
-    image_files.sort()
-    assert len(
-      image_files) != 0, "ERROR: Images not read correctly, check directory"
-    if charucos == {}:
-      allCorners, allIds, _, _, imsize, _ = self.analyze_charuco(image_files)
-    else:
-      allCorners = []
-      allIds = []
-      for index, charuco_img in enumerate(charucos[name]):
-        ids, charucos = charuco_img
-        allCorners.append(charucos)
-        allIds.append(ids)
-      imsize = (height, width)
-
-    coverageImage = np.ones(imsize[::-1], np.uint8) * 255
-    coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
-    coverageImage = self.draw_corners(allCorners, coverageImage)
-    if calib_model == 'perspective':
-      distortion_flags = self.get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
-      ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = self.calibrate_camera_charuco(
-        allCorners, allIds, imsize, hfov, name, distortion_flags)
-      self.undistort_visualization(
-        image_files, camera_matrix, distortion_coefficients, imsize, name)
-
-      return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-    else:
-      print('Fisheye--------------------------------------------------')
-      ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = self.calibrate_fisheye(
-        allCorners, allIds, imsize, hfov, name)
-      self.undistort_visualization(
-          image_files, camera_matrix, distortion_coefficients, imsize, name)
-      print('Fisheye rotation vector', rotation_vectors[0])
-      print('Fisheye translation vector', translation_vectors[0])
-
-      # (Height, width)
-      return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-
-  def undistort_visualization(self, img_list, K, D, img_size, name):
-    for index, im in enumerate(img_list):
-      # print(im)
-      img = cv2.imread(im)
-      # h, w = img.shape[:2]
-      if self._cameraModel == 'perspective':
-        kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0)
-        # print(f'K scaled is \n {kScaled} and size is \n {img_size}')
-        # print(f'D Value is \n {D}')
-        map1, map2 = cv2.initUndistortRectifyMap(
-          K, D, np.eye(3), kScaled, img_size, cv2.CV_32FC1)
-      else:
-        map1, map2 = cv2.fisheye.initUndistortRectifyMap(
-          K, D, np.eye(3), K, img_size, cv2.CV_32FC1)
-
-      undistorted_img = cv2.remap(
-        img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
-
-      if index == 0:
-        undistorted_file_path = self.data_path + '/' + name + f'_undistorted.png'
-        cv2.imwrite(undistorted_file_path, undistorted_img)
-
-  def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
-    corners_removed = False
-    for i in range(len(allIds)):
-      corners = allCorners[i]
-      ids = allIds[i]
-      objpts = self.charuco_ids_to_objpoints(ids)
-      if self._cameraModel == "fisheye":
-        errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
-      else:
-        errs = self.compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i])
-      suspicious_err_thr = max(2*np.median(errs), 100)
-      n_offending_pts = np.sum(errs > suspicious_err_thr)
-      offending_pts_idxs = np.where(errs > suspicious_err_thr)[0]
-      # check if there are offending points and if they form a minority
-      if n_offending_pts > 0 and n_offending_pts < len(corners)/5:
-        print(f"removing {n_offending_pts} offending points with errs {errs[offending_pts_idxs]}")
-        corners_removed = True
-        #remove the offending points
-        offset = 0
-        allCorners[i] = np.delete(allCorners[i],offending_pts_idxs, axis = 0)
-        allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
-    return corners_removed, allIds, allCorners
-
-  def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
-    """
-    Calibrates the camera using the dected corners.
-    """
-    f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2)))
-
-    threshold = 2 * imsize[1]/800.0
-     # check if there are any suspicious corners with high reprojection error
-    rvecs = []
-    tvecs = []
-    index = 0
-    max_threshold = 10 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
-    min_inlier = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
-    for corners, ids in zip(allCorners, allIds):
-      objpts = self.charuco_ids_to_objpoints(ids)
-      rvec, tvec, newids = self.camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
-      tvecs.append(tvec)
-      rvecs.append(rvec)
-      index += 1
-
-    # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
-    ret = 0.0
-    flags = cv2.CALIB_USE_INTRINSIC_GUESS
-    flags += distortion_flags
-
-    #   flags = (cv2.CALIB_RATIONAL_MODEL)
-    reprojection = []
-    removed_errors = []
-    num_corners = []
-    num_threshold = []
-    iterations_array = []
-    intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
-    distortion_array = {}
-    index = 0
-    camera_matrix = cameraIntrinsics
-    distortion_coefficients = distCoeff
-    rotation_vectors = rvecs
-    translation_vectors = tvecs
-    translation_array_x = []
-    translation_array_y = []
-    translation_array_z = []
-    corner_checker = 0
-    previous_ids = []
-    import time
-    try:
-      whole = time.time()
-      while True:
-        intrinsic_array['f_x'].append(camera_matrix[0][0])
-        intrinsic_array['f_y'].append(camera_matrix[1][1])
-        intrinsic_array['c_x'].append(camera_matrix[0][2])
-        intrinsic_array['c_y'].append(camera_matrix[1][2])
-        num_threshold.append(threshold)
-
-        translation_array_x.append(np.mean(np.array(translation_vectors).T[0][0]))
-        translation_array_y.append(np.mean(np.array(translation_vectors).T[0][1]))
-        translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
-
-        start = time.time()
-        filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = self.features_filtering_function(rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = threshold)
-        num_corners.append(len(removed_corners))
-        iterations_array.append(index)
-        reprojection.append(ret)
-        for i in range(len(distortion_coefficients)):
-          if i not in distortion_array:
-            distortion_array[i] = []
-          distortion_array[i].append(distortion_coefficients[i][0])
-        print(f"Each filtering {time.time() - start}")
-        start = time.time()
-        try:
-          (ret, camera_matrix, distortion_coefficients,
-           rotation_vectors, translation_vectors,
-           stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-           perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-            charucoCorners=filtered_corners,
-            charucoIds=filtered_ids,
-            board=self._board,
-            imageSize=imsize,
-            cameraMatrix=cameraIntrinsics,
-            distCoeffs=distCoeff,
-            flags=flags,
-            criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
-        except:
-          raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=self.id)
-        cameraIntrinsics = camera_matrix
-        distCoeff = distortion_coefficients
-        threshold = 5 * imsize[1]/800.0
-        print(f"Each calibration {time.time()-start}")
-        index += 1
-        if  index > 5 or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
-          print(f"Whole procedure: {time.time() - whole}")
-          break
-        previous_ids = removed_ids
-    except:
-      return f"Failed to calibrate camera {name}", None, None, None, None, None, None, None ,None , None
-    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
-
-  def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
-    one_pts = self._board.chessboardCorners
-    obj_points = []
-    for i in range(len(allIds)):
-      obj_points.append(self.charuco_ids_to_objpoints(allIds[i]))
-
-    f_init = imsize[0]/np.deg2rad(hfov)*1.15
-
-    cameraMatrixInit = np.array([[f_init, 0.      , imsize[0]/2],
-                   [0.      , f_init, imsize[1]/2],
-                   [0.      , 0.      , 1.      ]])
-    distCoeffsInit = np.zeros((4,1))
-     # check if there are any suspicious corners with high reprojection error
-    rvecs = []
-    tvecs = []
-    for corners, ids in zip(allCorners, allIds):
-      objpts = self.charuco_ids_to_objpoints(ids)
-      corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
-      rvec, tvec, new_ids = self.camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
-      tvecs.append(tvec)
-      rvecs.append(rvec)
-    corners_removed, filtered_ids, filtered_corners = self.filter_corner_outliers(allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
-    if corners_removed:
-      obj_points = []
-      for i in range(len(filtered_ids)):
-        obj_points.append(self.charuco_ids_to_objpoints(filtered_ids[i]))
-
-    print("Camera Matrix initialization.............")
-    print(cameraMatrixInit)
-    flags = 0
-    flags |= cv2.fisheye.CALIB_CHECK_COND
-    flags |= cv2.fisheye.CALIB_USE_INTRINSIC_GUESS
-    flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
-    flags |= cv2.fisheye.CALIB_FIX_SKEW
-
-    term_criteria = (cv2.TERM_CRITERIA_COUNT +
-             cv2.TERM_CRITERIA_EPS, 30, 1e-9)
-    try:
-      res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
-    except:
-      # calibration failed for full FOV, let's try to limit the corners to smaller part of FOV first to find initial parameters
-      success = False
-      crop = 0.95
-      while not success:
-        print(f"trying crop factor {crop}")
-        obj_points_limited = []
-        corners_limited = []
-        for obj_pts, corners in zip(obj_points, filtered_corners):
-          obj_points_tmp = []
-          corners_tmp = []
-          for obj_pt, corner in zip(obj_pts, corners):
-            check_x = corner[0,0] > imsize[0]*(1-crop) and corner[0,0] < imsize[0]*crop
-            check_y = corner[0,1] > imsize[1]*(1-crop) and corner[0,1] < imsize[1]*crop
-            if check_x and check_y:
-              obj_points_tmp.append(obj_pt)
-              corners_tmp.append(corner)
-          obj_points_limited.append(np.array(obj_points_tmp))
-          corners_limited.append(np.array(corners_tmp))
-        try:
-          res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points_limited, corners_limited, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
-          print(f"success with crop factor {crop}")
-          success = True
-          break
-        except:
-          print(f"failed with crop factor {crop}")
-          if crop > 0.7:
-            crop -= 0.05
-          else:
-            raise Exception("Calibration failed: Tried maximum crop factor and still no success")
-      if success:
-        # trying the full FOV once more with better initial K
-        print(f"new K init {K}")
-        print(f"new d_init {d}")
-        try:
-          res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, imsize, K, distCoeffsInit, flags=flags, criteria=term_criteria)
-        except:
-          print(f"Failed the full res calib, using calibration with crop factor {crop}")
-
-    return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
\ No newline at end of file
+    return 1, board_config
\ No newline at end of file

From c0945b57a8280b2813b9937202ff6a890103a409 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 12:11:43 +0200
Subject: [PATCH 48/87] Add config structure

---
 calibration_utils.py | 201 +++++++++++++++++++++++++------------------
 1 file changed, 118 insertions(+), 83 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index b2a4589..47fc399 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -22,7 +22,7 @@
 EXTRINSICS_PER_CCM = False
 
 class ProxyDict:
-  def __init__(self, squaresX, squaresY, squareSize, markerSize, dictSize):
+  def __init__(self, squaresX = 16, squaresY = 9, squareSize = 1.0, markerSize = 0.8, dictSize = cv2.aruco.DICT_4X4_1000):
     self.squaresX = squaresX
     self.squaresY = squaresY
     self.squareSize = squareSize
@@ -52,6 +52,36 @@ def board(self):
       self.__build()
     return self._board
 
+class CalibrationConfig:
+  def __init__(self, proxyDict = ProxyDict(), enableFiltering = True, ccmModel = '', disableCameras = [], initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
+               cameraModel = 0, stereoCalibCriteria = 0):
+    self._proxyDict = proxyDict
+    self.enableFiltering = enableFiltering
+    self.ccmModel = ccmModel # Distortion model
+    self.disableCameras = disableCameras
+    self.initialMaxThreshold = initialMaxThreshold
+    self.initialMinFiltered = initialMinFiltered
+    self.calibrationMaxThreshold = calibrationMaxThreshold
+    self.calibrationMinFiltered = calibrationMinFiltered
+    self.cameraModel = cameraModel
+    self.stereoCalibCriteria = stereoCalibCriteria
+
+  @property
+  def dictionary(self):
+    return self._proxyDict.dictionary
+
+  @property
+  def board(self):
+    return self._proxyDict.board
+
+class CalibrationData:
+
+  class CameraData:
+    ...
+
+  def __init__(self, config = CalibrationConfig()):
+    self.config = config
+
 colors = [(0, 255 , 0), (0, 0, 255)]
 
 class StereoExceptions(Exception):
@@ -67,10 +97,8 @@ def summary(self) -> str:
     """
     return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
-def estimate_pose_and_filter_single(calibration, cam_info, corners, ids):
-  board = calibration._board
-
-  objpoints = np.array([board.chessboardCorners[id] for id in ids], dtype=np.float32)
+def estimate_pose_and_filter_single(config: CalibrationConfig, cam_info, corners, ids):
+  objpoints = np.array([config.board.chessboardCorners[id] for id in ids], dtype=np.float32)
 
   ini_threshold=5
   threshold = None
@@ -116,8 +144,8 @@ def estimate_pose_and_filter_single(calibration, cam_info, corners, ids):
     #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
-def get_features(calibration, features, charucos, cam_info):
-  all_features, all_ids, imsize = getting_features(calibration, cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
+def get_features(config: CalibrationConfig, features, charucos, cam_info):
+  all_features, all_ids, imsize = getting_features(config, cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
 
   if isinstance(all_features, str) and all_ids is None:
     raise RuntimeError(f'Exception {all_features}') # TODO : Handle
@@ -133,28 +161,28 @@ def get_features(calibration, features, charucos, cam_info):
   cam_info['dist_coeff'] = distCoeff
 
    # check if there are any suspicious corners with high reprojection error
-  max_threshold = 75 + calibration.initial_max_threshold * (cam_info['hfov']/ 30 + cam_info['imsize'][1] / 800 * 0.2)
+  max_threshold = 75 + config.initialMaxThreshold * (cam_info['hfov']/ 30 + cam_info['imsize'][1] / 800 * 0.2)
   threshold_stepper = int(1.5 * (cam_info['hfov'] / 30 + cam_info['imsize'][1] / 800))
   if threshold_stepper < 1:
     threshold_stepper = 1
-  min_inliers = 1 - calibration.initial_min_filtered * (cam_info['hfov'] / 60 + cam_info['imsize'][1] / 800 * 0.2)
+  min_inliers = 1 - config.initialMinFiltered * (cam_info['hfov'] / 60 + cam_info['imsize'][1] / 800 * 0.2)
   cam_info['max_threshold'] = max_threshold
   cam_info['threshold_stepper'] = threshold_stepper
   cam_info['min_inliers'] = min_inliers
 
   return cam_info, all_features, all_ids
 
-def estimate_pose_and_filter(calibration, cam_info, allCorners, allIds):
+def estimate_pose_and_filter(config: CalibrationConfig, cam_info, allCorners, allIds):
   filtered_corners = []
   filtered_ids = []
   for a, b in zip(allCorners, allIds):
-    ids, corners, _ = estimate_pose_and_filter_single(calibration, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
+    ids, corners, _ = estimate_pose_and_filter_single(config, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
     filtered_corners.append(corners)
     filtered_ids.append(ids)
 
   return filtered_corners, filtered_ids
 
-def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
+def calibrate_charuco(config: CalibrationConfig, cam_info, filteredCorners, filteredIds):
   # TODO : If we still need this check it needs to be elsewhere
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
   #   raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
@@ -169,7 +197,7 @@ def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
        perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
         charucoCorners=filteredCorners,
         charucoIds=filteredIds,
-        board=calibration._board,
+        board=config.board,
         imageSize=cam_info['imsize'],
         cameraMatrix=cam_info['intrinsics'],
         distCoeffs=cam_info['dist_coeff'],
@@ -184,7 +212,7 @@ def calibrate_charuco(calibration, cam_info, filteredCorners, filteredIds):
   cam_info['filtered_ids'] = filteredIds
   return cam_info
 
-def filter_features_fisheye(calibration, cam_info, intrinsic_img, all_features, all_ids):
+def filter_features_fisheye(cam_info, intrinsic_img, all_features, all_ids):
   f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
   print("INTRINSIC CALIBRATION")
   cameraIntrinsics = np.array([[f,  0.0,    cam_info['imsize'][0]/2],
@@ -209,10 +237,10 @@ def filter_features_fisheye(calibration, cam_info, intrinsic_img, all_features,
 
   return cam_info
 
-def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, filtered_corners, filtered_ids):
+def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, features, cam_info, filtered_corners, filtered_ids):
   start = time.time()
   print('starting calibrate_wf')
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(calibration, cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
   if isinstance(ret, str) and all_ids is None:
     raise RuntimeError('Exception' + ret) # TODO : Handle
   print(f'calibrate_wf took {round(time.time() - start, 2)}s')
@@ -226,9 +254,9 @@ def calibrate_ccm_intrinsics_per_ccm(calibration, features, cam_info, filtered_c
 
   return cam_info
 
-def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
+def calibrate_ccm_intrinsics(config: CalibrationConfig, cam_info, charucos):
   ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_intrinsics(
-    calibration, cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
+    config, cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
   cam_info['filtered_ids'] = filtered_ids
   cam_info['filtered_corners'] = filtered_corners
 
@@ -241,7 +269,7 @@ def calibrate_ccm_intrinsics(calibration, cam_info, charucos):
 
   return cam_info
 
-def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
+def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model']
   specTranslation = left_cam_info['extrinsics']['specTranslation']
   rot = left_cam_info['extrinsics']['rotation']
@@ -261,7 +289,7 @@ def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, rig
   ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
   obj_pts, left_corners_sampled, right_corners_sampled,
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-  R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
+  R=r_in, T=t_in, criteria=config.stereoCalibCriteria, flags=flags)
 
   r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
   print(f'Epipolar error is {ret}')
@@ -283,7 +311,7 @@ def calibrate_stereo_perspective(calibration, obj_pts, left_corners_sampled, rig
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
+def calibrate_stereo_perspective_per_ccm(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
   specTranslation = left_cam_info['extrinsics']['specTranslation']
   rot = left_cam_info['extrinsics']['rotation']
@@ -297,7 +325,7 @@ def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_samp
   ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
   obj_pts, left_corners_sampled, right_corners_sampled,
   np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
-  R=r_in, T=t_in, criteria=calibration.stereocalib_criteria , flags=flags)
+  R=r_in, T=t_in, criteria=config.stereoCalibCriteria , flags=flags)
 
   r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
   scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
@@ -322,7 +350,7 @@ def calibrate_stereo_perspective_per_ccm(calibration, obj_pts, left_corners_samp
   # print(f'P_r is \n {P_r}')
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
+def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
   # make sure all images have the same *number of* points
   min_num_points = min([len(pts) for pts in obj_pts])
@@ -346,7 +374,7 @@ def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_c
   (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate(
     obj_pts_truncated, left_corners_truncated, right_corners_truncated,
     cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-    flags=flags, criteria=calibration.stereocalib_criteria), None, None
+    flags=flags, criteria=config.stereoCalibCriteria), None, None
   r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
   print(f'Reprojection error is {ret}')
   isHorizontal = np.absolute(T[0]) > np.absolute(T[1])
@@ -363,13 +391,13 @@ def calibrate_stereo_fisheye(calibration, obj_pts, left_corners_sampled, right_c
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def find_stereo_common_features(calibration, left_cam_info, right_cam_info):
+def find_stereo_common_features(config: CalibrationConfig, left_cam_info, right_cam_info):
   allIds_l, allIds_r, allCorners_l, allCorners_r = left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners']
   left_corners_sampled = []
   right_corners_sampled = []
   left_ids_sampled = []
   obj_pts = []
-  one_pts = calibration._board.chessboardCorners
+  one_pts = config.board.chessboardCorners
 
   for i, ids in enumerate(allIds_l):
     left_sub_corners = []
@@ -400,7 +428,7 @@ def undistort_points_perspective(allCorners, camInfo):
 def undistort_points_fisheye(allCorners, camInfo):
   return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
-def remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info):
+def remove_and_filter_stereo_features(calibration, config:CalibrationConfig, left_cam_info, right_cam_info):
   if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:
     if left_cam_info["name"] in calibration.extrinsic_img:
       array = calibration.extrinsic_img[left_cam_info["name"]]
@@ -409,8 +437,8 @@ def remove_and_filter_stereo_features(calibration, left_cam_info, right_cam_info
 
     left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
     right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = filtering_features(calibration, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
-    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = filtering_features(calibration, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
+    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = filtering_features(config, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
+    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = filtering_features(config, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
   return left_cam_info, right_cam_info
 
 def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam, board_config, extrinsics):
@@ -494,7 +522,7 @@ def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charuco
   cam_info['images_path'] = images_path
   return cam_info
 
-def getting_features(self, img_path, width, height, features = None, charucos=None):
+def getting_features(config: CalibrationConfig, img_path, width, height, features = None, charucos=None):
   if charucos:
     allCorners = []
     allIds = []
@@ -506,18 +534,18 @@ def getting_features(self, img_path, width, height, features = None, charucos=No
     return allCorners, allIds, imsize
 
   elif features == None or features == "charucos":
-    allCorners, allIds, _, _, imsize, _ = analyze_charuco(self, img_path)
+    allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
     return allCorners, allIds, imsize
 
   if features == "checker_board":
-    allCorners, allIds, _, _, imsize, _ = analyze_charuco(self, img_path)
+    allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
     return allCorners, allIds, imsize
   ###### ADD HERE WHAT IT IS NEEDED ######
 
-def filtering_features(self, allCorners, allIds, name,imsize, cam_info, cameraMatrixInit, distCoeffsInit, distortionModel):
+def filtering_features(config: CalibrationConfig, allCorners, allIds, name,imsize, cam_info, cameraMatrixInit, distCoeffsInit, distortionModel):
 
    # check if there are any suspicious corners with high reprojection error
-  filtered_corners, filtered_ids, removed_corners = estimate_pose_and_filter(self, cam_info, allCorners, allIds)
+  filtered_corners, filtered_ids, removed_corners = estimate_pose_and_filter(config, cam_info, allCorners, allIds)
 
   distortion_flags = get_distortion_flags(distortionModel)
   flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
@@ -529,7 +557,7 @@ def filtering_features(self, allCorners, allIds, name,imsize, cam_info, cameraMa
          perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
           charucoCorners=filtered_corners,
           charucoIds=filtered_ids,
-          board=self._board,
+          board=config.board,
           imageSize=imsize,
           cameraMatrix=cameraMatrixInit,
           distCoeffs=distCoeffsInit,
@@ -629,7 +657,7 @@ def is_binary_string(s: str) -> bool:
     flags = distortionModel
   return flags
 
-def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
+def calibrate_wf_intrinsics(config: CalibrationConfig, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
   coverageImage = np.ones(imsize[::-1], np.uint8) * 255
   coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
   coverageImage = draw_corners(allCorners, coverageImage)
@@ -637,7 +665,7 @@ def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, featur
     if features == None or features == "charucos":
       distortion_flags = get_distortion_flags(distortionModel)
       ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-        self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
+        config, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
 
       return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
     else:
@@ -647,7 +675,7 @@ def calibrate_wf_intrinsics(self, name, allCorners, allIds, imsize, hfov, featur
     if features == None or features == "charucos":
       print('Fisheye--------------------------------------------------')
       ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
-        self, allCorners, allIds, imsize, hfov, name)
+        config, allCorners, allIds, imsize, hfov, name)
       print('Fisheye rotation vector', rotation_vectors[0])
       print('Fisheye translation vector', translation_vectors[0])
 
@@ -671,7 +699,7 @@ def draw_corners(charuco_corners, displayframe):
   cv2.line(displayframe, start_point, end_point, color, thickness)
   return displayframe
 
-def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, reprojection, filtered_corners,filtered_id, camera, display = True, threshold = None, draw_quadrants = False, nx = 4, ny = 4):
+def features_filtering_function(config: CalibrationConfig, rvecs, tvecs, cameraMatrix, distCoeffs, reprojection, filtered_corners,filtered_id, camera, display = True, threshold = None, draw_quadrants = False, nx = 4, ny = 4):
   whole_error = []
   all_points = []
   all_corners = []
@@ -688,7 +716,7 @@ def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, rep
   for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
     if ids is not None and corners.size > 0:
       ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-      objPoints = np.array([self._board.chessboardCorners[id] for id in ids], dtype=np.float32)
+      objPoints = np.array([config.board.chessboardCorners[id] for id in ids], dtype=np.float32)
       imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
       corners2 = corners.reshape(-1, 2)
       imgpoints2 = imgpoints2.reshape(-1, 2)
@@ -726,14 +754,14 @@ def features_filtering_function(self,rvecs, tvecs, cameraMatrix, distCoeffs, rep
 
   return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
 
-def detect_charuco_board(self, image: np.array):
+def detect_charuco_board(config: CalibrationConfig, image: np.array):
   arucoParams = cv2.aruco.DetectorParameters_create()
   arucoParams.minMarkerDistanceRate = 0.01
-  corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, self._aruco_dictionary, parameters=arucoParams)  # First, detect markers
-  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, self._board, corners, ids, rejectedCorners=rejectedImgPoints)
+  corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, config.dictionary, parameters=arucoParams)  # First, detect markers
+  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, config.board, corners, ids, rejectedCorners=rejectedImgPoints)
   # If found, add object points, image points (after refining them)
   if len(marker_corners) > 0:
-    ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, self._board, minMarkers = 1)
+    ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, config.board, minMarkers = 1)
     return ret, corners, ids, marker_corners, marker_ids
   else:
     return None, None, None, None, None
@@ -772,14 +800,14 @@ def compute_reprojection_errors(obj_pts: np.array, img_pts: np.array, K: np.arra
   errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
   return errs
 
-def charuco_ids_to_objpoints(self, ids):
-  one_pts = self._board.chessboardCorners
+def charuco_ids_to_objpoints(config: CalibrationConfig, ids):
+  one_pts = config.board.chessboardCorners
   objpts = []
   for j in range(len(ids)):
     objpts.append(one_pts[ids[j]])
   return np.array(objpts)
 
-def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
+def analyze_charuco(config: CalibrationConfig, images, scale_req=False, req_resolution=(800, 1280)):
   """
   Charuco base pose estimation.
   """
@@ -825,7 +853,7 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
 
       count += 1
 
-    ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = detect_charuco_board(self, gray)
+    ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = detect_charuco_board(config, gray)
 
     if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
 
@@ -844,13 +872,13 @@ def analyze_charuco(self, images, scale_req=False, req_resolution=(800, 1280)):
   # imsize = gray.shape[::-1]
   return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
 
-def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
+def calibrate_intrinsics(config: CalibrationConfig, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
   image_files = glob.glob(image_files + "/*")
   image_files.sort()
   assert len(
     image_files) != 0, "ERROR: Images not read correctly, check directory"
   if charucos == {}:
-    allCorners, allIds, _, _, imsize, _ = analyze_charuco(self, image_files)
+    allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, image_files)
   else:
     allCorners = []
     allIds = []
@@ -866,17 +894,17 @@ def calibrate_intrinsics(self, image_files, hfov, name, charucos, width, height,
   if calib_model == 'perspective':
     distortion_flags = get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
     ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-      self, allCorners, allIds, imsize, hfov, name, distortion_flags)
-    undistort_visualization(
-      self, image_files, camera_matrix, distortion_coefficients, imsize, name)
+      config, allCorners, allIds, imsize, hfov, name, distortion_flags)
+    # undistort_visualization(
+    #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
 
     return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
   else:
     print('Fisheye--------------------------------------------------')
     ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
-      self, allCorners, allIds, imsize, hfov, name)
-    undistort_visualization(
-      self, image_files, camera_matrix, distortion_coefficients, imsize, name)
+      config, allCorners, allIds, imsize, hfov, name)
+    # undistort_visualization(
+    #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
     print('Fisheye rotation vector', rotation_vectors[0])
     print('Fisheye translation vector', translation_vectors[0])
 
@@ -905,13 +933,13 @@ def undistort_visualization(self, img_list, K, D, img_size, name):
       undistorted_file_path = self.data_path + '/' + name + f'_undistorted.png'
       cv2.imwrite(undistorted_file_path, undistorted_img)
 
-def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
+def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
   corners_removed = False
   for i in range(len(allIds)):
     corners = allCorners[i]
     ids = allIds[i]
-    objpts = charuco_ids_to_objpoints(self, ids)
-    if self._cameraModel == "fisheye":
+    objpts = charuco_ids_to_objpoints(config, ids)
+    if config.cameraModel == "fisheye":
       errs = compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
     else:
       errs = compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i])
@@ -928,7 +956,7 @@ def filter_corner_outliers(self, allIds, allCorners, camera_matrix, distortion_c
       allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
   return corners_removed, allIds, allCorners
 
-def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
+def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
   """
   Calibrates the camera using the dected corners.
   """
@@ -939,10 +967,10 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, disto
   rvecs = []
   tvecs = []
   index = 0
-  max_threshold = 10 + self.initial_max_threshold * (hfov / 30 + imsize[1] / 800 * 0.2)
-  min_inlier = 1 - self.initial_min_filtered * (hfov / 60 + imsize[1] / 800 * 0.2)
+  max_threshold = 10 + config.initialMaxThreshold * (hfov / 30 + imsize[1] / 800 * 0.2)
+  min_inlier = 1 - config.initialMinFiltered * (hfov / 60 + imsize[1] / 800 * 0.2)
   for corners, ids in zip(allCorners, allIds):
-    objpts = charuco_ids_to_objpoints(self, ids)
+    objpts = charuco_ids_to_objpoints(config, ids)
     rvec, tvec, newids = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
     tvecs.append(tvec)
     rvecs.append(rvec)
@@ -986,7 +1014,7 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, disto
       translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
 
       start = time.time()
-      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(self, rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = threshold)
+      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(config, rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = threshold)
       num_corners.append(len(removed_corners))
       iterations_array.append(index)
       reprojection.append(ret)
@@ -1003,14 +1031,14 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, disto
          perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
           charucoCorners=filtered_corners,
           charucoIds=filtered_ids,
-          board=self._board,
+          board=config.board,
           imageSize=imsize,
           cameraMatrix=cameraIntrinsics,
           distCoeffs=distCoeff,
           flags=flags,
           criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
       except:
-        raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=self.id)
+        raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=0)
       cameraIntrinsics = camera_matrix
       distCoeff = distortion_coefficients
       threshold = 5 * imsize[1]/800.0
@@ -1024,11 +1052,10 @@ def calibrate_camera_charuco(self, allCorners, allIds, imsize, hfov, name, disto
     return f"Failed to calibrate camera {name}", None, None, None, None, None, None, None ,None , None
   return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
-def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
-  one_pts = self._board.chessboardCorners
+def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name):
   obj_points = []
   for i in range(len(allIds)):
-    obj_points.append(charuco_ids_to_objpoints(self, allIds[i]))
+    obj_points.append(charuco_ids_to_objpoints(config, allIds[i]))
 
   f_init = imsize[0]/np.deg2rad(hfov)*1.15
 
@@ -1040,16 +1067,16 @@ def calibrate_fisheye(self, allCorners, allIds, imsize, hfov, name):
   rvecs = []
   tvecs = []
   for corners, ids in zip(allCorners, allIds):
-    objpts = charuco_ids_to_objpoints(self, ids)
+    objpts = charuco_ids_to_objpoints(config, ids)
     corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
     rvec, tvec, new_ids = camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
     tvecs.append(tvec)
     rvecs.append(rvec)
-  corners_removed, filtered_ids, filtered_corners = filter_corner_outliers(self, allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
+  corners_removed, filtered_ids, filtered_corners = filter_corner_outliers(config, allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
   if corners_removed:
     obj_points = []
     for i in range(len(filtered_ids)):
-      obj_points.append(charuco_ids_to_objpoints(self, filtered_ids[i]))
+      obj_points.append(charuco_ids_to_objpoints(config, filtered_ids[i]))
 
   print("Camera Matrix initialization.............")
   print(cameraMatrixInit)
@@ -1156,6 +1183,14 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     resizeWidth, resizeHeight = 1280, 800
 
     activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
+    
+    calibData = CalibrationData(
+      CalibrationConfig(
+        ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000),
+        self.filtering_enable, self.ccm_model, self.disableCamera, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
+        camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
+      )
+    )
 
     stereoPairs = []
     for camera, _ in activeCameras:
@@ -1179,28 +1214,28 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     pw = ParallelWorker(16)
     if PER_CCM:
       for cam, cam_info in activeCameras:
-        ret = pw.run(get_features, self, features, charucos[cam_info['name']], cam_info)
+        ret = pw.run(get_features, calibData.config, features, charucos[cam_info['name']], cam_info)
         if self._cameraModel == "fisheye":
-          ret2 = pw.run(filter_features_fisheye, self, ret[0], intrinsic_img, ret[1], ret[2])
+          ret2 = pw.run(filter_features_fisheye, ret[0], intrinsic_img, ret[1], ret[2])
         else:
-          featuresAndIds = pw.map(estimate_pose_and_filter_single, self, ret[0], ret[1], ret[2])
+          featuresAndIds = pw.map(estimate_pose_and_filter_single, calibData.config, ret[0], ret[1], ret[2])
 
-          ret2 = pw.run(calibrate_charuco, self, ret[0], featuresAndIds[0], featuresAndIds[1])
-        ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, self, features, ret2, featuresAndIds[0], featuresAndIds[1])
+          ret2 = pw.run(calibrate_charuco, calibData.config, ret[0], featuresAndIds[0], featuresAndIds[1])
+        ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, calibData.config, features, ret2, featuresAndIds[0], featuresAndIds[1])
         tasks.extend([ret, ret2, ret3, featuresAndIds])
         camInfos[cam] = ret3
     else:
       for cam, cam_info in activeCameras:
-        cam_info = calibrate_ccm_intrinsics(self, cam_info, charucos[cam_info['name']])
+        cam_info = calibrate_ccm_intrinsics(calibData.config, cam_info, charucos[cam_info['name']])
 
     for left, right in stereoPairs:
       left_cam_info = camInfos[left]
       right_cam_info = camInfos[right]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
-        left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, left_cam_info, right_cam_info)
+        left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, calibData.config, left_cam_info, right_cam_info)
 
-      ret = pw.run(find_stereo_common_features, self, left_cam_info, right_cam_info)
+      ret = pw.run(find_stereo_common_features, calibData.config, left_cam_info, right_cam_info)
       left_corners_sampled, right_corners_sampled, obj_pts = ret[0], ret[1], ret[2]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
@@ -1212,13 +1247,13 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
           right_corners_sampled = pw.run(undistort_points_fisheye, right_corners_sampled, right_cam_info)
 
         if features == None or features == "charucos":
-          extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+          extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, calibData.config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
         #### ADD OTHER CALIBRATION METHODS ###
       else:
         if self._cameraModel == 'perspective':
-          extrinsics = pw.run(calibrate_stereo_perspective, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+          extrinsics = pw.run(calibrate_stereo_perspective, calibData.config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
         elif self._cameraModel == 'fisheye':
-          extrinsics = pw.run(calibrate_stereo_fisheye, self, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+          extrinsics = pw.run(calibrate_stereo_fisheye, calibData.config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
       ret4 = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
       camInfos[left] = ret4[0]
       stereoConfigs.append(ret4[1])

From 4f593ff28c8d74d85d25d902d4f6abebf860d5cc Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 12:54:31 +0200
Subject: [PATCH 49/87] Cleanup arguments

---
 calibration_utils.py | 63 ++++++++++++++++++++++----------------------
 1 file changed, 31 insertions(+), 32 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 47fc399..6eae7ef 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -54,7 +54,7 @@ def board(self):
 
 class CalibrationConfig:
   def __init__(self, proxyDict = ProxyDict(), enableFiltering = True, ccmModel = '', disableCameras = [], initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
-               cameraModel = 0, stereoCalibCriteria = 0):
+               cameraModel = 0, stereoCalibCriteria = 0, features = None):
     self._proxyDict = proxyDict
     self.enableFiltering = enableFiltering
     self.ccmModel = ccmModel # Distortion model
@@ -65,6 +65,7 @@ def __init__(self, proxyDict = ProxyDict(), enableFiltering = True, ccmModel = '
     self.calibrationMinFiltered = calibrationMinFiltered
     self.cameraModel = cameraModel
     self.stereoCalibCriteria = stereoCalibCriteria
+    self.features = features # None | 'checker_board' | 'charuco'
 
   @property
   def dictionary(self):
@@ -144,8 +145,8 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, cam_info, corners
     #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
-def get_features(config: CalibrationConfig, features, charucos, cam_info):
-  all_features, all_ids, imsize = getting_features(config, cam_info['images_path'], cam_info['width'], cam_info['height'], features=features, charucos=charucos)
+def get_features(config: CalibrationConfig, charucos, cam_info):
+  all_features, all_ids, imsize = getting_features(config, cam_info['images_path'], cam_info['width'], cam_info['height'], charucos=charucos)
 
   if isinstance(all_features, str) and all_ids is None:
     raise RuntimeError(f'Exception {all_features}') # TODO : Handle
@@ -237,10 +238,10 @@ def filter_features_fisheye(cam_info, intrinsic_img, all_features, all_ids):
 
   return cam_info
 
-def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, features, cam_info, filtered_corners, filtered_ids):
+def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, cam_info, filtered_corners, filtered_ids):
   start = time.time()
   print('starting calibrate_wf')
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], features, cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
   if isinstance(ret, str) and all_ids is None:
     raise RuntimeError('Exception' + ret) # TODO : Handle
   print(f'calibrate_wf took {round(time.time() - start, 2)}s')
@@ -522,22 +523,21 @@ def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charuco
   cam_info['images_path'] = images_path
   return cam_info
 
-def getting_features(config: CalibrationConfig, img_path, width, height, features = None, charucos=None):
+def getting_features(config: CalibrationConfig, img_path, width, height, charucos=None):
   if charucos:
     allCorners = []
     allIds = []
-    for index, charuco_img in enumerate(charucos):
-      ids, charuco = charuco_img
+    for ids, charuco in charucos:
       allCorners.append(charuco)
       allIds.append(ids)
     imsize = (width, height)
     return allCorners, allIds, imsize
 
-  elif features == None or features == "charucos":
+  elif config.features == None or config.features == "charucos":
     allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
     return allCorners, allIds, imsize
 
-  if features == "checker_board":
+  if config.features == "checker_board":
     allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
     return allCorners, allIds, imsize
   ###### ADD HERE WHAT IT IS NEEDED ######
@@ -657,12 +657,12 @@ def is_binary_string(s: str) -> bool:
     flags = distortionModel
   return flags
 
-def calibrate_wf_intrinsics(config: CalibrationConfig, name, allCorners, allIds, imsize, hfov, features, calib_model, distortionModel, cameraIntrinsics, distCoeff):
+def calibrate_wf_intrinsics(config: CalibrationConfig, name, allCorners, allIds, imsize, hfov, calib_model, distortionModel, cameraIntrinsics, distCoeff):
   coverageImage = np.ones(imsize[::-1], np.uint8) * 255
   coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
   coverageImage = draw_corners(allCorners, coverageImage)
   if calib_model == 'perspective':
-    if features == None or features == "charucos":
+    if config.features == None or config.features == "charucos":
       distortion_flags = get_distortion_flags(distortionModel)
       ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
         config, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
@@ -672,7 +672,7 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, name, allCorners, allIds,
       return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
     #### ADD ADDITIONAL FEATURES CALIBRATION ####
   else:
-    if features == None or features == "charucos":
+    if config.features == None or config.features == "charucos":
       print('Fisheye--------------------------------------------------')
       ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
         config, allCorners, allIds, imsize, hfov, name)
@@ -1185,11 +1185,11 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
     
     calibData = CalibrationData(
-      CalibrationConfig(
-        ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000),
-        self.filtering_enable, self.ccm_model, self.disableCamera, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
-        camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
-      )
+    )
+    config = CalibrationConfig(
+      ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000),
+      self.filtering_enable, self.ccm_model, self.disableCamera, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
+      camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9), None
     )
 
     stereoPairs = []
@@ -1212,30 +1212,29 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     camInfos = {}
     stereoConfigs = []
     pw = ParallelWorker(16)
-    if PER_CCM:
-      for cam, cam_info in activeCameras:
-        ret = pw.run(get_features, calibData.config, features, charucos[cam_info['name']], cam_info)
+    for cam, cam_info in activeCameras:
+      if PER_CCM:
+        ret = pw.run(get_features, config, charucos[cam_info['name']], cam_info)
         if self._cameraModel == "fisheye":
           ret2 = pw.run(filter_features_fisheye, ret[0], intrinsic_img, ret[1], ret[2])
         else:
-          featuresAndIds = pw.map(estimate_pose_and_filter_single, calibData.config, ret[0], ret[1], ret[2])
+          featuresAndIds = pw.map(estimate_pose_and_filter_single, config, ret[0], ret[1], ret[2])
 
-          ret2 = pw.run(calibrate_charuco, calibData.config, ret[0], featuresAndIds[0], featuresAndIds[1])
-        ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, calibData.config, features, ret2, featuresAndIds[0], featuresAndIds[1])
+          ret2 = pw.run(calibrate_charuco, config, ret[0], featuresAndIds[0], featuresAndIds[1])
+        ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, config, ret2, featuresAndIds[0], featuresAndIds[1])
         tasks.extend([ret, ret2, ret3, featuresAndIds])
         camInfos[cam] = ret3
-    else:
-      for cam, cam_info in activeCameras:
-        cam_info = calibrate_ccm_intrinsics(calibData.config, cam_info, charucos[cam_info['name']])
+      else:
+        cam_info = calibrate_ccm_intrinsics(config, cam_info, charucos[cam_info['name']])
 
     for left, right in stereoPairs:
       left_cam_info = camInfos[left]
       right_cam_info = camInfos[right]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
-        left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, calibData.config, left_cam_info, right_cam_info)
+        left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, config, left_cam_info, right_cam_info)
 
-      ret = pw.run(find_stereo_common_features, calibData.config, left_cam_info, right_cam_info)
+      ret = pw.run(find_stereo_common_features, config, left_cam_info, right_cam_info)
       left_corners_sampled, right_corners_sampled, obj_pts = ret[0], ret[1], ret[2]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
@@ -1247,13 +1246,13 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
           right_corners_sampled = pw.run(undistort_points_fisheye, right_corners_sampled, right_cam_info)
 
         if features == None or features == "charucos":
-          extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, calibData.config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+          extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
         #### ADD OTHER CALIBRATION METHODS ###
       else:
         if self._cameraModel == 'perspective':
-          extrinsics = pw.run(calibrate_stereo_perspective, calibData.config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+          extrinsics = pw.run(calibrate_stereo_perspective, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
         elif self._cameraModel == 'fisheye':
-          extrinsics = pw.run(calibrate_stereo_fisheye, calibData.config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+          extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
       ret4 = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
       camInfos[left] = ret4[0]
       stereoConfigs.append(ret4[1])

From 9aa475bf63abb6408d8c82c89268d1a38a28021d Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 15:36:23 +0200
Subject: [PATCH 50/87] Cleaner return values from ParallelTask

---
 calibration_utils.py | 23 ++++++++++-------------
 worker.py            | 12 ++++++++++++
 2 files changed, 22 insertions(+), 13 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 6eae7ef..ea4009a 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1208,22 +1208,20 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     for cam, cam_info in activeCameras:
       cam_info = load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
 
-    tasks = []
     camInfos = {}
     stereoConfigs = []
     pw = ParallelWorker(16)
     for cam, cam_info in activeCameras:
       if PER_CCM:
-        ret = pw.run(get_features, config, charucos[cam_info['name']], cam_info)
+        cam_info, features, ids = pw.run(get_features, config, charucos[cam_info['name']], cam_info)[:3]
         if self._cameraModel == "fisheye":
-          ret2 = pw.run(filter_features_fisheye, ret[0], intrinsic_img, ret[1], ret[2])
+          ret2 = pw.run(filter_features_fisheye, cam_info, intrinsic_img, features, ids)
         else:
-          featuresAndIds = pw.map(estimate_pose_and_filter_single, config, ret[0], ret[1], ret[2])
+          features, ids = pw.map(estimate_pose_and_filter_single, config, cam_info, features, ids)[:2]
 
-          ret2 = pw.run(calibrate_charuco, config, ret[0], featuresAndIds[0], featuresAndIds[1])
-        ret3 = pw.run(calibrate_ccm_intrinsics_per_ccm, config, ret2, featuresAndIds[0], featuresAndIds[1])
-        tasks.extend([ret, ret2, ret3, featuresAndIds])
-        camInfos[cam] = ret3
+          cam_info = pw.run(calibrate_charuco, config, cam_info, features, ids)
+        cam_info = pw.run(calibrate_ccm_intrinsics_per_ccm, config, cam_info, features, ids)
+        camInfos[cam] = cam_info
       else:
         cam_info = calibrate_ccm_intrinsics(config, cam_info, charucos[cam_info['name']])
 
@@ -1234,8 +1232,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
       if PER_CCM and EXTRINSICS_PER_CCM:
         left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, config, left_cam_info, right_cam_info)
 
-      ret = pw.run(find_stereo_common_features, config, left_cam_info, right_cam_info)
-      left_corners_sampled, right_corners_sampled, obj_pts = ret[0], ret[1], ret[2]
+      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, config, left_cam_info, right_cam_info)[:3]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
         if left_cam_info['calib_model'] == "perspective":
@@ -1253,9 +1250,9 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
           extrinsics = pw.run(calibrate_stereo_perspective, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
         elif self._cameraModel == 'fisheye':
           extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-      ret4 = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)
-      camInfos[left] = ret4[0]
-      stereoConfigs.append(ret4[1])
+      cam_info, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
+      camInfos[left] = cam_info
+      stereoConfigs.append(stereo_config)
 
     pw.execute()
 
diff --git a/worker.py b/worker.py
index b48e944..c4ec222 100644
--- a/worker.py
+++ b/worker.py
@@ -17,6 +17,18 @@ def __init__(self, taskOrGroup: 'ParallelTask', key: slice | tuple | int):
     self._taskOrGroup = taskOrGroup
     self._key = key
 
+  def __iter__(self): # Allow iterating over slice or list retvals
+    if isinstance(self._key, slice):
+      if not self._key.stop:
+        raise RuntimeError('Cannot iterate over an unknown length Retvals')
+      for i in range(self._key.start or 0, self._key.stop, self._key.step or 1):
+        yield Retvals(self._taskOrGroup, i)
+    elif isinstance(self._key, list | tuple):
+      for i in self._key:
+        yield Retvals(self._taskOrGroup, i)
+    else:
+      yield Retvals(self._taskOrGroup, self._key)
+
   def ret(self):
     if isinstance(self._key, list | tuple):
       ret = self._taskOrGroup.ret()

From b04704a39dd38733bc8874b67b729628237a83bc Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 16:58:25 +0200
Subject: [PATCH 51/87] Slightly better typehints

---
 worker.py | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/worker.py b/worker.py
index c4ec222..9abc877 100644
--- a/worker.py
+++ b/worker.py
@@ -1,4 +1,4 @@
-from typing import Dict, Any, Callable, Iterable, Tuple, TypeVar, Generic, List
+from typing import Dict, Any, Callable, Iterable, Tuple, TypeVar, Generic, Sequence, List
 from collections import abc
 import multiprocessing
 import random
@@ -12,12 +12,12 @@ def allArgs(args, kwargs):
   for kwarg in kwargs.values():
     yield kwarg
 
-class Retvals:
+class Retvals(Sequence[T]):
   def __init__(self, taskOrGroup: 'ParallelTask', key: slice | tuple | int):
     self._taskOrGroup = taskOrGroup
     self._key = key
 
-  def __iter__(self): # Allow iterating over slice or list retvals
+  def __iter__(self) -> T: # Allow iterating over slice or list retvals
     if isinstance(self._key, slice):
       if not self._key.stop:
         raise RuntimeError('Cannot iterate over an unknown length Retvals')
@@ -29,16 +29,16 @@ def __iter__(self): # Allow iterating over slice or list retvals
     else:
       yield Retvals(self._taskOrGroup, self._key)
 
-  def ret(self):
+  def ret(self) -> T:
     if isinstance(self._key, list | tuple):
       ret = self._taskOrGroup.ret()
       return [ret[i] for i in self._key]
     return self._taskOrGroup.ret()[self._key]
 
-  def finished(self):
+  def finished(self) -> bool:
     return self._taskOrGroup.finished()
 
-class ParallelTask(Generic[T]):
+class ParallelTask(Sequence[T]):
   def __init__(self, worker: 'ParallelWorker', fun, args, kwargs):
     self._worker = worker
     self._fun = fun
@@ -49,7 +49,7 @@ def __init__(self, worker: 'ParallelWorker', fun, args, kwargs):
     self._id = random.getrandbits(64)
     self._finished = False
 
-  def __getitem__(self, key):
+  def __getitem__(self, key) -> T:
     return Retvals(self, key)
 
   def __repr__(self):
@@ -175,7 +175,7 @@ def worker_controller(_stop: multiprocessing.Event, _in: multiprocessing.Queue,
     #finally:
     _out.put((task._id, ret, exc))
 
-class ParallelWorker(Generic[T]):
+class ParallelWorker:
   def __init__(self, workers: int = 16):
     self._workers = workers
     self._tasks: List[ParallelTask] = []

From 61fba818fbc6a1af339a02f174004583c07ef36c Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 18:24:18 +0200
Subject: [PATCH 52/87] Annotate camData

---
 calibration_utils.py | 291 ++++++++++++++++++++++---------------------
 worker.py            |   4 +-
 2 files changed, 154 insertions(+), 141 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index ea4009a..cc0fcd2 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1,13 +1,10 @@
-#!/usr/bin/env python3
-
 from scipy.spatial.transform import Rotation
+from typing import TypedDict, List, Tuple
 import matplotlib.colors as colors
 from .worker import ParallelWorker
 import matplotlib.pyplot as plt
-from collections import deque
 import cv2.aruco as aruco
 from pathlib import Path
-import multiprocessing
 import numpy as np
 import logging
 import time
@@ -75,13 +72,33 @@ def dictionary(self):
   def board(self):
     return self._proxyDict.board
 
-class CalibrationData:
-
-  class CameraData:
-    ...
-
-  def __init__(self, config = CalibrationConfig()):
-    self.config = config
+class CameraData(TypedDict):
+  calib_model: str
+  dist_coeff: str
+  distortion_model: str
+  extrinsics: str
+  to_cam: str
+  filtered_corners: str
+  type: str
+  filtered_ids: str
+  height: str
+  width: str
+  hfov: str
+  socket: str
+  images_path: str
+  imsize: str
+  intrinsics: str
+  sensorName: str
+  hasAutofocus: str
+  model: str
+  max_threshold: str
+  min_inliers: str
+  name: str
+  reprojection_error: str
+  size: str
+  threshold_stepper: str
+  features: str
+  ids: str
 
 colors = [(0, 255 , 0), (0, 0, 255)]
 
@@ -98,7 +115,7 @@ def summary(self) -> str:
     """
     return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
-def estimate_pose_and_filter_single(config: CalibrationConfig, cam_info, corners, ids):
+def estimate_pose_and_filter_single(config: CalibrationConfig, camData: CameraData, corners, ids):
   objpoints = np.array([config.board.chessboardCorners[id] for id in ids], dtype=np.float32)
 
   ini_threshold=5
@@ -106,10 +123,10 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, cam_info, corners
 
   objects = []
   all_objects = []
-  while len(objects) < len(objpoints[:,0,0]) * cam_info['min_inliers']:
-    if ini_threshold > cam_info['max_threshold']:
+  while len(objects) < len(objpoints[:,0,0]) * camData['min_inliers']:
+    if ini_threshold > camData['max_threshold']:
       break
-    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, cam_info['intrinsics'], cam_info['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, camData['intrinsics'], camData['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
     all_objects.append(objects)
     imgpoints2 = objpoints.copy()
 
@@ -117,15 +134,15 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, cam_info, corners
     all_corners2 = np.array([all_corners2[id[0]] for id in objects])
     imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
 
-    ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, cam_info['intrinsics'], cam_info['dist_coeff'])
+    ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, camData['intrinsics'], camData['dist_coeff'])
 
-    ini_threshold += cam_info['threshold_stepper']
+    ini_threshold += camData['threshold_stepper']
   if not ret:
     raise RuntimeError('Exception') # TODO : Handle
 
   if ids is not None and corners.size > 0:
     ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-    imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, cam_info['intrinsics'], cam_info['dist_coeff'])
+    imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, camData['intrinsics'], camData['dist_coeff'])
     corners2 = corners.reshape(-1, 2)
     imgpoints2 = imgpoints2.reshape(-1, 2)
 
@@ -145,33 +162,33 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, cam_info, corners
     #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
-def get_features(config: CalibrationConfig, charucos, cam_info):
-  all_features, all_ids, imsize = getting_features(config, cam_info['images_path'], cam_info['width'], cam_info['height'], charucos=charucos)
+def get_features(config: CalibrationConfig, camData: CameraData, charucos) -> CameraData:
+  all_features, all_ids, imsize = getting_features(config, camData['images_path'], camData['width'], camData['height'], charucos=charucos)
 
   if isinstance(all_features, str) and all_ids is None:
     raise RuntimeError(f'Exception {all_features}') # TODO : Handle
-  cam_info["imsize"] = imsize
-  f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+  camData["imsize"] = imsize
+  f = camData['imsize'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
   print("INTRINSIC CALIBRATION")
-  cameraIntrinsics = np.array([[f,  0.0,    cam_info['imsize'][0]/2],
-                 [0.0,   f,    cam_info['imsize'][1]/2],
+  cameraIntrinsics = np.array([[f,  0.0,    camData['imsize'][0]/2],
+                 [0.0,   f,    camData['imsize'][1]/2],
                  [0.0,   0.0,    1.0]])
 
   distCoeff = np.zeros((12, 1))
-  cam_info['intrinsics'] = cameraIntrinsics
-  cam_info['dist_coeff'] = distCoeff
+  camData['intrinsics'] = cameraIntrinsics
+  camData['dist_coeff'] = distCoeff
 
    # check if there are any suspicious corners with high reprojection error
-  max_threshold = 75 + config.initialMaxThreshold * (cam_info['hfov']/ 30 + cam_info['imsize'][1] / 800 * 0.2)
-  threshold_stepper = int(1.5 * (cam_info['hfov'] / 30 + cam_info['imsize'][1] / 800))
+  max_threshold = 75 + config.initialMaxThreshold * (camData['hfov']/ 30 + camData['imsize'][1] / 800 * 0.2)
+  threshold_stepper = int(1.5 * (camData['hfov'] / 30 + camData['imsize'][1] / 800))
   if threshold_stepper < 1:
     threshold_stepper = 1
-  min_inliers = 1 - config.initialMinFiltered * (cam_info['hfov'] / 60 + cam_info['imsize'][1] / 800 * 0.2)
-  cam_info['max_threshold'] = max_threshold
-  cam_info['threshold_stepper'] = threshold_stepper
-  cam_info['min_inliers'] = min_inliers
+  min_inliers = 1 - config.initialMinFiltered * (camData['hfov'] / 60 + camData['imsize'][1] / 800 * 0.2)
+  camData['max_threshold'] = max_threshold
+  camData['threshold_stepper'] = threshold_stepper
+  camData['min_inliers'] = min_inliers
 
-  return cam_info, all_features, all_ids
+  return camData, all_features, all_ids
 
 def estimate_pose_and_filter(config: CalibrationConfig, cam_info, allCorners, allIds):
   filtered_corners = []
@@ -183,12 +200,12 @@ def estimate_pose_and_filter(config: CalibrationConfig, cam_info, allCorners, al
 
   return filtered_corners, filtered_ids
 
-def calibrate_charuco(config: CalibrationConfig, cam_info, filteredCorners, filteredIds):
+def calibrate_charuco(config: CalibrationConfig, camData: CameraData, corners, ids):
   # TODO : If we still need this check it needs to be elsewhere
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
   #   raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
 
-  distortion_flags = get_distortion_flags(cam_info['distortion_model'])
+  distortion_flags = get_distortion_flags(camData['distortion_model'])
   flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
 
   #try:
@@ -196,22 +213,22 @@ def calibrate_charuco(config: CalibrationConfig, cam_info, filteredCorners, filt
        rotation_vectors, translation_vectors,
        stdDeviationsIntrinsics, stdDeviationsExtrinsics,
        perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-        charucoCorners=filteredCorners,
-        charucoIds=filteredIds,
+        charucoCorners=corners,
+        charucoIds=ids,
         board=config.board,
-        imageSize=cam_info['imsize'],
-        cameraMatrix=cam_info['intrinsics'],
-        distCoeffs=cam_info['dist_coeff'],
+        imageSize=camData['imsize'],
+        cameraMatrix=camData['intrinsics'],
+        distCoeffs=camData['dist_coeff'],
         flags=flags,
         criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
   #except:
   #  return f"First intrisic calibration failed for {cam_info['imsize']}", None, None
 
-  cam_info['intrinsics'] = camera_matrix
-  cam_info['dist_coeff'] = distortion_coefficients
-  cam_info['filtered_corners'] = filteredCorners
-  cam_info['filtered_ids'] = filteredIds
-  return cam_info
+  camData['intrinsics'] = camera_matrix
+  camData['dist_coeff'] = distortion_coefficients
+  camData['filtered_corners'] = corners
+  camData['filtered_ids'] = ids
+  return camData
 
 def filter_features_fisheye(cam_info, intrinsic_img, all_features, all_ids):
   f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
@@ -238,37 +255,63 @@ def filter_features_fisheye(cam_info, intrinsic_img, all_features, all_ids):
 
   return cam_info
 
-def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, cam_info, filtered_corners, filtered_ids):
+def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraData):
   start = time.time()
   print('starting calibrate_wf')
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, cam_info["name"], filtered_corners, filtered_ids, cam_info["imsize"], cam_info["hfov"], cam_info['calib_model'], cam_info['distortion_model'], cam_info['intrinsics'], cam_info['dist_coeff'])
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, camData)
   if isinstance(ret, str) and all_ids is None:
     raise RuntimeError('Exception' + ret) # TODO : Handle
   print(f'calibrate_wf took {round(time.time() - start, 2)}s')
 
-  cam_info['intrinsics'] = cameraIntrinsics
-  cam_info['dist_coeff'] = distCoeff
-  cam_info['size'] = size # (Width, height)
-  cam_info['reprojection_error'] = ret
+  camData['intrinsics'] = cameraIntrinsics
+  camData['dist_coeff'] = distCoeff
+  camData['size'] = size # (Width, height)
+  camData['reprojection_error'] = ret
   print("Reprojection error of {0}: {1}".format(
-    cam_info['name'], ret))
-
-  return cam_info
-
-def calibrate_ccm_intrinsics(config: CalibrationConfig, cam_info, charucos):
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_intrinsics(
-    config, cam_info['images_path'], cam_info['hfov'], cam_info["name"], charucos, cam_info['width'], cam_info['height'], cam_info['calib_model'], cam_info['distortion_model'])
-  cam_info['filtered_ids'] = filtered_ids
-  cam_info['filtered_corners'] = filtered_corners
+    camData['name'], ret))
+
+  return camData
+
+def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData, charucos):
+  imsize = camData['imsize']
+  hfov = camData['hfov']
+  name = camData['name']
+  allCorners = camData['filtered_corners']
+  allIds = camData['filtered_ids']
+  calib_model = camData['calib_model']
+  distortionModel = camData['distortion_model']
+  
+  coverageImage = np.ones(imsize[::-1], np.uint8) * 255
+  coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
+  coverageImage = draw_corners(allCorners, coverageImage)
+  if calib_model == 'perspective':
+    distortion_flags = get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
+    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
+      config, allCorners, allIds, imsize, hfov, distortion_flags, camData['intrinsics'], camData['dist_coeff'])
+    # undistort_visualization(
+    #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
 
-  cam_info['intrinsics'] = cameraIntrinsics
-  cam_info['dist_coeff'] = distCoeff
-  cam_info['size'] = size # (Width, height)
-  cam_info['reprojection_error'] = ret
+    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+  else:
+    print('Fisheye--------------------------------------------------')
+    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
+      config, allCorners, allIds, imsize, hfov, name)
+    # undistort_visualization(
+    #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
+    print('Fisheye rotation vector', rotation_vectors[0])
+    print('Fisheye translation vector', translation_vectors[0])
+  
+  camData['filtered_ids'] = filtered_ids
+  camData['filtered_corners'] = filtered_corners
+
+  camData['intrinsics'] = camera_matrix
+  camData['dist_coeff'] = distortion_coefficients
+  camData['size'] = size # (Width, height)
+  camData['reprojection_error'] = ret
   print("Reprojection error of {0}: {1}".format(
-    cam_info['name'], ret))
+    camData['name'], ret))
 
-  return cam_info
+  return camData
 
 def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model']
@@ -657,7 +700,17 @@ def is_binary_string(s: str) -> bool:
     flags = distortionModel
   return flags
 
-def calibrate_wf_intrinsics(config: CalibrationConfig, name, allCorners, allIds, imsize, hfov, calib_model, distortionModel, cameraIntrinsics, distCoeff):
+def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData):
+  name = camData['name']
+  allCorners = camData['filtered_corners']
+  allIds = camData['filtered_ids']
+  imsize = camData['imsize']
+  hfov = camData['hfov']
+  calib_model = camData['calib_model']
+  distortionModel = camData['distortion_model']
+  cameraIntrinsics = camData['intrinsics']
+  distCoeff = camData['dist_coeff']
+  
   coverageImage = np.ones(imsize[::-1], np.uint8) * 255
   coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
   coverageImage = draw_corners(allCorners, coverageImage)
@@ -665,7 +718,7 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, name, allCorners, allIds,
     if config.features == None or config.features == "charucos":
       distortion_flags = get_distortion_flags(distortionModel)
       ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-        config, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff)
+        config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff)
 
       return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
     else:
@@ -699,7 +752,7 @@ def draw_corners(charuco_corners, displayframe):
   cv2.line(displayframe, start_point, end_point, color, thickness)
   return displayframe
 
-def features_filtering_function(config: CalibrationConfig, rvecs, tvecs, cameraMatrix, distCoeffs, reprojection, filtered_corners,filtered_id, camera, display = True, threshold = None, draw_quadrants = False, nx = 4, ny = 4):
+def features_filtering_function(config: CalibrationConfig, rvecs, tvecs, cameraMatrix, distCoeffs, filtered_corners,filtered_id, threshold = None):
   whole_error = []
   all_points = []
   all_corners = []
@@ -872,45 +925,6 @@ def analyze_charuco(config: CalibrationConfig, images, scale_req=False, req_reso
   # imsize = gray.shape[::-1]
   return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
 
-def calibrate_intrinsics(config: CalibrationConfig, image_files, hfov, name, charucos, width, height, calib_model, distortionModel):
-  image_files = glob.glob(image_files + "/*")
-  image_files.sort()
-  assert len(
-    image_files) != 0, "ERROR: Images not read correctly, check directory"
-  if charucos == {}:
-    allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, image_files)
-  else:
-    allCorners = []
-    allIds = []
-    for index, charuco_img in enumerate(charucos[name]):
-      ids, charucos = charuco_img
-      allCorners.append(charucos)
-      allIds.append(ids)
-    imsize = (height, width)
-
-  coverageImage = np.ones(imsize[::-1], np.uint8) * 255
-  coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
-  coverageImage = draw_corners(allCorners, coverageImage)
-  if calib_model == 'perspective':
-    distortion_flags = get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
-    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-      config, allCorners, allIds, imsize, hfov, name, distortion_flags)
-    # undistort_visualization(
-    #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
-
-    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-  else:
-    print('Fisheye--------------------------------------------------')
-    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
-      config, allCorners, allIds, imsize, hfov, name)
-    # undistort_visualization(
-    #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
-    print('Fisheye rotation vector', rotation_vectors[0])
-    print('Fisheye translation vector', translation_vectors[0])
-
-    # (Height, width)
-    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-
 def undistort_visualization(self, img_list, K, D, img_size, name):
   for index, im in enumerate(img_list):
     # print(im)
@@ -956,7 +970,7 @@ def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners, camera
       allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
   return corners_removed, allIds, allCorners
 
-def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name, distortion_flags, cameraIntrinsics, distCoeff):
+def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff):
   """
   Calibrates the camera using the dected corners.
   """
@@ -1014,7 +1028,7 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
       translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
 
       start = time.time()
-      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(config, rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, ret, allCorners, allIds, camera = name, threshold = threshold)
+      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(config, rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, allCorners, allIds, threshold = threshold)
       num_corners.append(len(removed_corners))
       iterations_array.append(index)
       reprojection.append(ret)
@@ -1038,7 +1052,7 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
           flags=flags,
           criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
       except:
-        raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element=name, id=0)
+        raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element='', id=0)
       cameraIntrinsics = camera_matrix
       distCoeff = distortion_coefficients
       threshold = 5 * imsize[1]/800.0
@@ -1049,7 +1063,7 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
         break
       previous_ids = removed_ids
   except:
-    return f"Failed to calibrate camera {name}", None, None, None, None, None, None, None ,None , None
+    return f"Failed to calibrate camera", None, None, None, None, None, None, None ,None , None
   return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
 def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name):
@@ -1182,10 +1196,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
     resizeWidth, resizeHeight = 1280, 800
 
-    activeCameras = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
+    activeCameras: List[Tuple[str, CameraData]] = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
     
-    calibData = CalibrationData(
-    )
     config = CalibrationConfig(
       ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000),
       self.filtering_enable, self.ccm_model, self.disableCamera, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
@@ -1193,44 +1205,45 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
     )
 
     stereoPairs = []
-    for camera, _ in activeCameras:
-      left_cam_info = board_config['cameras'][camera]
-      if str(left_cam_info["name"]) in self.disableCamera:
-        continue
-      if not 'extrinsics' in left_cam_info:
+    for camera, camInfo in activeCameras:
+      if str(camInfo['name']) in self.disableCamera \
+        or not 'extrinsics' in camInfo \
+        or not 'to_cam' in camInfo['extrinsics'] \
+        or str(board_config['cameras'][camInfo['extrinsics']['to_cam']]['name']) in self.disableCamera:
         continue
-      if not 'to_cam' in left_cam_info['extrinsics']:
-        continue
-      if str(board_config['cameras'][left_cam_info['extrinsics']['to_cam']]['name']) in self.disableCamera:
-        continue
-      stereoPairs.append([camera, left_cam_info['extrinsics']['to_cam']])
+      stereoPairs.append([camera, camInfo['extrinsics']['to_cam']])
 
-    for cam, cam_info in activeCameras:
-      cam_info = load_camera_data(filepath, cam_info, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
+    for cam, camData in activeCameras:
+      camData = load_camera_data(filepath, camData, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
 
     camInfos = {}
     stereoConfigs = []
     pw = ParallelWorker(16)
-    for cam, cam_info in activeCameras:
+
+    for cam, camData in activeCameras:
       if PER_CCM:
-        cam_info, features, ids = pw.run(get_features, config, charucos[cam_info['name']], cam_info)[:3]
+        camData, features, ids = pw.run(get_features, config, camData, charucos[camData['name']])[:3]
         if self._cameraModel == "fisheye":
-          ret2 = pw.run(filter_features_fisheye, cam_info, intrinsic_img, features, ids)
+          camData = pw.run(filter_features_fisheye, camData, intrinsic_img) # TODO : Input types are wrong
         else:
-          features, ids = pw.map(estimate_pose_and_filter_single, config, cam_info, features, ids)[:2]
-
-          cam_info = pw.run(calibrate_charuco, config, cam_info, features, ids)
-        cam_info = pw.run(calibrate_ccm_intrinsics_per_ccm, config, cam_info, features, ids)
-        camInfos[cam] = cam_info
+          features, ids = pw.map(estimate_pose_and_filter_single, config, camData, features, ids)[:2]
+          
+    # camData['features'] = corners2[valid_mask].reshape(-1, 1, 2)
+    # camData['ids'] = valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
+    # return camData
+
+          camData = pw.run(calibrate_charuco, config, camData, features, ids)
+        camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData)
+        camInfos[cam] = camData
       else:
-        cam_info = calibrate_ccm_intrinsics(config, cam_info, charucos[cam_info['name']])
+        camData = calibrate_ccm_intrinsics(config, camData, charucos[camData['name']])
 
     for left, right in stereoPairs:
       left_cam_info = camInfos[left]
       right_cam_info = camInfos[right]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
-        left_cam_info_and_right_cam_info = pw.run(remove_and_filter_stereo_features, self, config, left_cam_info, right_cam_info)
+        left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, self, config, left_cam_info, right_cam_info)[:2]
 
       left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, config, left_cam_info, right_cam_info)[:3]
 
@@ -1250,8 +1263,8 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
           extrinsics = pw.run(calibrate_stereo_perspective, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
         elif self._cameraModel == 'fisheye':
           extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-      cam_info, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
-      camInfos[left] = cam_info
+      camData, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
+      camInfos[left] = camData
       stereoConfigs.append(stereo_config)
 
     pw.execute()
diff --git a/worker.py b/worker.py
index 9abc877..5ad5b3c 100644
--- a/worker.py
+++ b/worker.py
@@ -12,7 +12,7 @@ def allArgs(args, kwargs):
   for kwarg in kwargs.values():
     yield kwarg
 
-class Retvals(Sequence[T]):
+class Retvals(Generic[T]):
   def __init__(self, taskOrGroup: 'ParallelTask', key: slice | tuple | int):
     self._taskOrGroup = taskOrGroup
     self._key = key
@@ -38,7 +38,7 @@ def ret(self) -> T:
   def finished(self) -> bool:
     return self._taskOrGroup.finished()
 
-class ParallelTask(Sequence[T]):
+class ParallelTask(Generic[T]):
   def __init__(self, worker: 'ParallelWorker', fun, args, kwargs):
     self._worker = worker
     self._fun = fun

From 5e1e4fba86b32488e444e1ae93915ff4e1274058 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 18:35:14 +0200
Subject: [PATCH 53/87] Remove charuco_ids_to_objpoints

---
 calibration_utils.py | 29 ++++++++++-------------------
 1 file changed, 10 insertions(+), 19 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index cc0fcd2..9970241 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -116,7 +116,7 @@ def summary(self) -> str:
     return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
 def estimate_pose_and_filter_single(config: CalibrationConfig, camData: CameraData, corners, ids):
-  objpoints = np.array([config.board.chessboardCorners[id] for id in ids], dtype=np.float32)
+  objpoints = config.board.chessboardCorners[ids]
 
   ini_threshold=5
   threshold = None
@@ -853,13 +853,6 @@ def compute_reprojection_errors(obj_pts: np.array, img_pts: np.array, K: np.arra
   errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
   return errs
 
-def charuco_ids_to_objpoints(config: CalibrationConfig, ids):
-  one_pts = config.board.chessboardCorners
-  objpts = []
-  for j in range(len(ids)):
-    objpts.append(one_pts[ids[j]])
-  return np.array(objpts)
-
 def analyze_charuco(config: CalibrationConfig, images, scale_req=False, req_resolution=(800, 1280)):
   """
   Charuco base pose estimation.
@@ -952,7 +945,7 @@ def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners, camera
   for i in range(len(allIds)):
     corners = allCorners[i]
     ids = allIds[i]
-    objpts = charuco_ids_to_objpoints(config, ids)
+    objpts = config.board.chessboardCorners[ids]
     if config.cameraModel == "fisheye":
       errs = compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
     else:
@@ -984,7 +977,7 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
   max_threshold = 10 + config.initialMaxThreshold * (hfov / 30 + imsize[1] / 800 * 0.2)
   min_inlier = 1 - config.initialMinFiltered * (hfov / 60 + imsize[1] / 800 * 0.2)
   for corners, ids in zip(allCorners, allIds):
-    objpts = charuco_ids_to_objpoints(config, ids)
+    objpts = config.board.chessboardCorners[ids]
     rvec, tvec, newids = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
     tvecs.append(tvec)
     rvecs.append(rvec)
@@ -1067,10 +1060,6 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
   return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
 def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name):
-  obj_points = []
-  for i in range(len(allIds)):
-    obj_points.append(charuco_ids_to_objpoints(config, allIds[i]))
-
   f_init = imsize[0]/np.deg2rad(hfov)*1.15
 
   cameraMatrixInit = np.array([[f_init, 0.      , imsize[0]/2],
@@ -1081,16 +1070,18 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
   rvecs = []
   tvecs = []
   for corners, ids in zip(allCorners, allIds):
-    objpts = charuco_ids_to_objpoints(config, ids)
+    objpts = config.board.chessboardCorners[ids]
     corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
     rvec, tvec, new_ids = camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
     tvecs.append(tvec)
     rvecs.append(rvec)
+
   corners_removed, filtered_ids, filtered_corners = filter_corner_outliers(config, allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
-  if corners_removed:
-    obj_points = []
-    for i in range(len(filtered_ids)):
-      obj_points.append(charuco_ids_to_objpoints(config, filtered_ids[i]))
+
+  obj_points = []
+  for ids in filtered_ids:
+    obj_points.append(config.board.chessboardCorners[ids])
+  # TODO :Maybe this can be obj_points = config.board.chessboardCorners[filtered_ids]
 
   print("Camera Matrix initialization.............")
   print(cameraMatrixInit)

From 1df473d13cf616920031146763395722f1aa6986 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 6 Aug 2024 19:26:27 +0200
Subject: [PATCH 54/87] Refactor find_stereo_common_features

---
 calibration_utils.py | 27 ++++++++-------------------
 1 file changed, 8 insertions(+), 19 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 9970241..d434fd7 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -439,29 +439,18 @@ def find_stereo_common_features(config: CalibrationConfig, left_cam_info, right_
   allIds_l, allIds_r, allCorners_l, allCorners_r = left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners']
   left_corners_sampled = []
   right_corners_sampled = []
-  left_ids_sampled = []
   obj_pts = []
-  one_pts = config.board.chessboardCorners
 
-  for i, ids in enumerate(allIds_l):
-    left_sub_corners = []
-    right_sub_corners = []
-    obj_pts_sub = []
+  for i, ids in enumerate(allIds_l): # For ids in all images
+    commonIds = np.intersect1d(allIds_l[i], allIds_r[i])
+    left_sub_corners = allCorners_l[i][np.isin(allIds_l[i], commonIds)]
+    right_sub_corners = allCorners_r[i][np.isin(allIds_r[i], commonIds)]
+    obj_pts_sub = config.board.chessboardCorners[commonIds]
 
-    for j, id in enumerate(ids):
-      idx = np.where(allIds_r[i] == id)
-      if idx[0].size == 0:
-        continue
-      left_sub_corners.append(allCorners_l[i][j]) # TODO : This copies even idxs that don't match
-      right_sub_corners.append(allCorners_r[i][idx])
-      obj_pts_sub.append(one_pts[id])
     if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
-      obj_pts.append(np.array(obj_pts_sub, dtype=np.float32))
-      left_corners_sampled.append(
-        np.array(left_sub_corners, dtype=np.float32))
-      left_ids_sampled.append(np.array(ids, dtype=np.int32))
-      right_corners_sampled.append(
-        np.array(right_sub_corners, dtype=np.float32))
+      obj_pts.append(obj_pts_sub)
+      left_corners_sampled.append(left_sub_corners)
+      right_corners_sampled.append(right_sub_corners)
     else:
       return -1, "Stereo Calib failed due to less common features"
   return left_corners_sampled, right_corners_sampled, obj_pts

From bc24df049fa19785f1584c3fd9ff23d710f73812 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Wed, 14 Aug 2024 12:59:52 +0200
Subject: [PATCH 55/87] Add __repr__, don't unpack dict, str and bytes in
 TaskGroup arguments

---
 worker.py | 12 +++++++++---
 1 file changed, 9 insertions(+), 3 deletions(-)

diff --git a/worker.py b/worker.py
index 5ad5b3c..a7d2b22 100644
--- a/worker.py
+++ b/worker.py
@@ -29,6 +29,9 @@ def __iter__(self) -> T: # Allow iterating over slice or list retvals
     else:
       yield Retvals(self._taskOrGroup, self._key)
 
+  def __repr__(self):
+    return f'<Retvals of {self._taskOrGroup}>'
+
   def ret(self) -> T:
     if isinstance(self._key, list | tuple):
       ret = self._taskOrGroup.ret()
@@ -96,6 +99,9 @@ def __init__(self, fun, args, kwargs):
   def __getitem__(self, key):
     return Retvals(self, key)
 
+  def __repr__(self):
+    return f'<TaskGroup {self._fun}>'
+
   def finished(self) -> bool:
     for task in self._tasks:
       if not task.finished():
@@ -108,10 +114,10 @@ def exc(self) -> List[BaseException | None]:
   def tasks(self) -> List[ParallelTask]:
     nTasks = 1
     for arg in allArgs(self._args, self._kwargs):
-      if isinstance(arg, abc.Sized):
+      if isinstance(arg, ParallelTask | Retvals):
+        arg = arg.ret()
+      if isinstance(arg, abc.Sized) and not isinstance(arg, str | bytes | dict):
         nTasks = max(nTasks, len(arg))
-      elif isinstance(arg, ParallelTask | Retvals):
-        nTasks = max(nTasks, len(arg.ret()))
     self._tasks = []
     for arg in allArgs(self._args, self._kwargs):
       if isinstance(arg, abc.Sized) and len(arg) != nTasks:

From a2c5fef0330be3c13425ed7e8e561c4e4cd5261e Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 15 Aug 2024 13:03:11 +0200
Subject: [PATCH 56/87] Thermal WIP

---
 calibration_utils.py | 212 ++++++++++++++++---------------------------
 1 file changed, 80 insertions(+), 132 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index d434fd7..94bdb15 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1,17 +1,17 @@
+import logging
+logging.getLogger('matplotlib').setLevel(logging.WARNING)
+
 from scipy.spatial.transform import Rotation
 from typing import TypedDict, List, Tuple
-import matplotlib.colors as colors
 from .worker import ParallelWorker
 import matplotlib.pyplot as plt
 import cv2.aruco as aruco
 from pathlib import Path
 import numpy as np
-import logging
 import time
 import glob
 import cv2
 
-logging.getLogger('matplotlib').setLevel(logging.WARNING)
 
 plt.rcParams.update({'font.size': 16})
 
@@ -126,13 +126,18 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, camData: CameraDa
   while len(objects) < len(objpoints[:,0,0]) * camData['min_inliers']:
     if ini_threshold > camData['max_threshold']:
       break
-    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, camData['intrinsics'], camData['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-    all_objects.append(objects)
-    imgpoints2 = objpoints.copy()
+    if camData['name'] == 'thermal': # TODO : It doesn't make sense to rerun this loop if Ransac is dsabled
+      imgpoints2 = objpoints.copy()
+
+      all_corners2 = corners.copy()
+    else:
+      ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, camData['intrinsics'], camData['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+      all_objects.append(objects)
+      imgpoints2 = objpoints.copy()
 
-    all_corners2 = corners.copy()
-    all_corners2 = np.array([all_corners2[id[0]] for id in objects])
-    imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+      all_corners2 = corners.copy()
+      all_corners2 = np.array([all_corners2[id[0]] for id in objects])
+      imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
 
     ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, camData['intrinsics'], camData['dist_coeff'])
 
@@ -140,7 +145,7 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, camData: CameraDa
   if not ret:
     raise RuntimeError('Exception') # TODO : Handle
 
-  if ids is not None and corners.size > 0:
+  if ids is not None and corners.size > 0: # TODO : Try to remove the np reshaping
     ids = ids.flatten()  # Flatten the IDs from 2D to 1D
     imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, camData['intrinsics'], camData['dist_coeff'])
     corners2 = corners.reshape(-1, 2)
@@ -162,21 +167,25 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, camData: CameraDa
     #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
-def get_features(config: CalibrationConfig, camData: CameraData, charucos) -> CameraData:
-  all_features, all_ids, imsize = getting_features(config, camData['images_path'], camData['width'], camData['height'], charucos=charucos)
+def get_features(config: CalibrationConfig, camData: CameraData, charucos) -> Tuple[CameraData, list, list]:
+  #all_features, all_ids, imsize = getting_features(config, camData['images_path'], camData['width'], camData['height'], charucos=charucos)
+  
+  allCorners = []
+  allIds = []
+  for ids, charuco in charucos:
+    allCorners.append(charuco)
+    allIds.append(ids)
 
-  if isinstance(all_features, str) and all_ids is None:
-    raise RuntimeError(f'Exception {all_features}') # TODO : Handle
-  camData["imsize"] = imsize
+#   if isinstance(all_features, str) and all_ids is None:
+#     raise RuntimeError(f'Exception {all_features}') # TODO : Handle
   f = camData['imsize'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
-  print("INTRINSIC CALIBRATION")
-  cameraIntrinsics = np.array([[f,  0.0,    camData['imsize'][0]/2],
-                 [0.0,   f,    camData['imsize'][1]/2],
-                 [0.0,   0.0,    1.0]])
 
-  distCoeff = np.zeros((12, 1))
-  camData['intrinsics'] = cameraIntrinsics
-  camData['dist_coeff'] = distCoeff
+  camData['intrinsics'] = np.array([
+     [f,     0.0,    camData['width']/2],
+     [0.0,   f,      camData['height']/2],
+     [0.0,   0.0,    1.0]
+  ])
+  camData['dist_coeff'] = np.zeros((12, 1))
 
    # check if there are any suspicious corners with high reprojection error
   max_threshold = 75 + config.initialMaxThreshold * (camData['hfov']/ 30 + camData['imsize'][1] / 800 * 0.2)
@@ -188,13 +197,13 @@ def get_features(config: CalibrationConfig, camData: CameraData, charucos) -> Ca
   camData['threshold_stepper'] = threshold_stepper
   camData['min_inliers'] = min_inliers
 
-  return camData, all_features, all_ids
+  return camData, allCorners, allIds
 
-def estimate_pose_and_filter(config: CalibrationConfig, cam_info, allCorners, allIds):
+def estimate_pose_and_filter(config: CalibrationConfig, camData: CameraData, allCornersAndIds):
   filtered_corners = []
   filtered_ids = []
-  for a, b in zip(allCorners, allIds):
-    ids, corners, _ = estimate_pose_and_filter_single(config, a, b, cam_info['intrinsics'], cam_info['dist_coeff'], cam_info['min_inliers'], cam_info['max_threshold'], cam_info['threshold_stepper'])
+  for corners, ids in allCornersAndIds:
+    corners, ids, _ = estimate_pose_and_filter_single(config, camData, corners, ids)
     filtered_corners.append(corners)
     filtered_ids.append(ids)
 
@@ -272,11 +281,11 @@ def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraD
 
   return camData
 
-def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData, charucos):
+def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
   imsize = camData['imsize']
   hfov = camData['hfov']
   name = camData['name']
-  allCorners = camData['filtered_corners']
+  allCorners = camData['filtered_corners'] # TODO : I don't think this has a way to get here from one of the codepaths in matin in the else:
   allIds = camData['filtered_ids']
   calib_model = camData['calib_model']
   distortionModel = camData['distortion_model']
@@ -285,9 +294,9 @@ def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData, cha
   coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
   coverageImage = draw_corners(allCorners, coverageImage)
   if calib_model == 'perspective':
-    distortion_flags = get_distortion_flags(distortionModel) # TODO : The call to calibrate_camera_charuco has different parameters than it should
+    distortion_flags = get_distortion_flags(distortionModel)
     ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-      config, allCorners, allIds, imsize, hfov, distortion_flags, camData['intrinsics'], camData['dist_coeff'])
+      config, allCorners, allIds, imsize, hfov, distortion_flags, camData['intrinsics'], camData['dist_coeff'], camData['name'] == 'thermal')
     # undistort_visualization(
     #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
 
@@ -461,18 +470,11 @@ def undistort_points_perspective(allCorners, camInfo):
 def undistort_points_fisheye(allCorners, camInfo):
   return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
-def remove_and_filter_stereo_features(calibration, config:CalibrationConfig, left_cam_info, right_cam_info):
-  if left_cam_info["name"] in calibration.extrinsic_img or right_cam_info["name"] in calibration.extrinsic_img:
-    if left_cam_info["name"] in calibration.extrinsic_img:
-      array = calibration.extrinsic_img[left_cam_info["name"]]
-    elif right_cam_info["name"] in calibration.extrinsic_img:
-      array = calibration.extrinsic_img[left_cam_info["name"]]
+def remove_and_filter_stereo_features(config:CalibrationConfig, leftCamData: CameraData, rightCamData: CameraData, allCornersAndIds):
+  leftCamData['filtered_corners'], leftCamData['filtered_ids'] = estimate_pose_and_filter(config, leftCamData, allCornersAndIds[leftCamData['name']])
+  rightCamData['filtered_corners'], rightCamData['filtered_ids'] = estimate_pose_and_filter(config, rightCamData, allCornersAndIds[rightCamData['name']])
 
-    left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], filtered_images = remove_features(left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], array)
-    right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], filtered_images = remove_features(right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], array)
-    removed_features, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], _, _ = filtering_features(config, left_cam_info['filtered_corners'], left_cam_info['filtered_ids'], left_cam_info["name"],left_cam_info["imsize"],left_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'],  left_cam_info['distortion_model'])
-    removed_features, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], _, _ = filtering_features(config, right_cam_info['filtered_corners'], right_cam_info['filtered_ids'], right_cam_info["name"], right_cam_info["imsize"], right_cam_info, left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['distortion_model'])
-  return left_cam_info, right_cam_info
+  return leftCamData, rightCamData
 
 def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam, board_config, extrinsics):
 
@@ -541,17 +543,11 @@ def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charuco
     else:
       distortion_model = model
 
-  img_path = glob.glob(images_path + "/*")
-  if charucos == {}:
-    img_path = sorted(img_path, key=lambda x: int(x.split('_')[1]))
-  else:
-    img_path.sort()
-
   cam_info['width'] = width
   cam_info['height'] = height
+  cam_info['imsize'] = (width, height)
   cam_info['calib_model'] = calib_model
   cam_info['distortion_model'] = distortion_model
-  cam_info["img_path"] = img_path
   cam_info['images_path'] = images_path
   return cam_info
 
@@ -565,55 +561,15 @@ def getting_features(config: CalibrationConfig, img_path, width, height, charuco
     imsize = (width, height)
     return allCorners, allIds, imsize
 
-  elif config.features == None or config.features == "charucos":
-    allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
-    return allCorners, allIds, imsize
+  # elif config.features == None or config.features == "charucos":
+  #   allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
+  #   return allCorners, allIds, imsize
 
-  if config.features == "checker_board":
-    allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
-    return allCorners, allIds, imsize
+  # if config.features == "checker_board":
+  #   allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
+  #   return allCorners, allIds, imsize
   ###### ADD HERE WHAT IT IS NEEDED ######
 
-def filtering_features(config: CalibrationConfig, allCorners, allIds, name,imsize, cam_info, cameraMatrixInit, distCoeffsInit, distortionModel):
-
-   # check if there are any suspicious corners with high reprojection error
-  filtered_corners, filtered_ids, removed_corners = estimate_pose_and_filter(config, cam_info, allCorners, allIds)
-
-  distortion_flags = get_distortion_flags(distortionModel)
-  flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
-
-  try:
-    (ret, camera_matrix, distortion_coefficients,
-         rotation_vectors, translation_vectors,
-         stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-         perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-          charucoCorners=filtered_corners,
-          charucoIds=filtered_ids,
-          board=config.board,
-          imageSize=imsize,
-          cameraMatrix=cameraMatrixInit,
-          distCoeffs=distCoeffsInit,
-          flags=flags,
-          criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
-  except:
-    return f"First intrisic calibration failed for {name}", None, None
-
-  return removed_corners, filtered_corners, filtered_ids, camera_matrix, distortion_coefficients
-
-def remove_features(allCorners, allIds, array, img_files = None):
-  filteredCorners = allCorners.copy()
-  filteredIds = allIds.copy()
-  if img_files is not None:
-    img_path = img_files.copy()
-
-  for index in array:
-    filteredCorners.pop(index)
-    filteredIds.pop(index)
-    if img_files is not None:
-      img_path.pop(index)
-
-  return filteredCorners, filteredIds, img_path
-
 def get_distortion_flags(distortionModel):
   def is_binary_string(s: str) -> bool:
   # Check if all characters in the string are '0' or '1'
@@ -707,7 +663,7 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData):
     if config.features == None or config.features == "charucos":
       distortion_flags = get_distortion_flags(distortionModel)
       ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-        config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff)
+        config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, camData['name'] == 'thermal')
 
       return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
     else:
@@ -808,31 +764,30 @@ def detect_charuco_board(config: CalibrationConfig, image: np.array):
   else:
     return None, None, None, None, None
 
-def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
+def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50, skipRANSAC=False):
   objects = []
-  all_objects = []
-  index = 0
-  start_time = time.time()
   while len(objects) < len(objpoints[:,0,0]) * min_inliers:
     if ini_threshold > max_threshold:
       break
-    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-    all_objects.append(objects)
-    imgpoints2 = objpoints.copy()
+    if not skipRANSAC:
+      ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+      imgpoints2 = objpoints.copy()
 
-    all_corners = corners.copy()
-    all_corners = np.array([all_corners[id[0]] for id in objects])
-    imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+      all_corners = corners.copy()
+      all_corners = np.array([all_corners[id[0]] for id in objects])
+      imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+    else:
+      imgpoints2 = objpoints.copy()
+      all_corners = corners.copy()
 
     ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners, K, d)
     imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, K, d)
 
     ini_threshold += threshold_stepper
-    index += 1
   if ret:
-    return rvec, tvec, objects
+    return rvec, tvec
   else:
-    return None
+    raise RuntimeError() # TODO : Handle
 
 def compute_reprojection_errors(obj_pts: np.array, img_pts: np.array, K: np.array, dist: np.array, rvec: np.array, tvec: np.array, fisheye = False):
   if fisheye:
@@ -952,7 +907,7 @@ def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners, camera
       allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
   return corners_removed, allIds, allCorners
 
-def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff):
+def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, skipRANSAC):
   """
   Calibrates the camera using the dected corners.
   """
@@ -967,10 +922,12 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
   min_inlier = 1 - config.initialMinFiltered * (hfov / 60 + imsize[1] / 800 * 0.2)
   for corners, ids in zip(allCorners, allIds):
     objpts = config.board.chessboardCorners[ids]
-    rvec, tvec, newids = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
+    rvec, tvec = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff, skipRANSAC=skipRANSAC)
     tvecs.append(tvec)
     rvecs.append(rvec)
     index += 1
+  if skipRANSAC:
+    threshold = 60
 
   # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
   ret = 0.0
@@ -980,7 +937,6 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
   #   flags = (cv2.CALIB_RATIONAL_MODEL)
   reprojection = []
   removed_errors = []
-  num_corners = []
   num_threshold = []
   iterations_array = []
   intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
@@ -996,7 +952,7 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
   corner_checker = 0
   previous_ids = []
   import time
-  try:
+  if True:
     whole = time.time()
     while True:
       intrinsic_array['f_x'].append(camera_matrix[0][0])
@@ -1010,8 +966,10 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
       translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
 
       start = time.time()
+      #if not skipRANSAC:
       filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(config, rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, allCorners, allIds, threshold = threshold)
-      num_corners.append(len(removed_corners))
+      #else:
+      #  filtered_corners, filtered_ids = allCorners, allIds
       iterations_array.append(index)
       reprojection.append(ret)
       for i in range(len(distortion_coefficients)):
@@ -1040,12 +998,10 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
       threshold = 5 * imsize[1]/800.0
       print(f"Each calibration {time.time()-start}")
       index += 1
-      if  index > 5 or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
+      if  index > 5: #or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
         print(f"Whole procedure: {time.time() - whole}")
         break
-      previous_ids = removed_ids
-  except:
-    return f"Failed to calibrate camera", None, None, None, None, None, None, None ,None , None
+      #previous_ids = removed_ids
   return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
 def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name):
@@ -1061,7 +1017,7 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
   for corners, ids in zip(allCorners, allIds):
     objpts = config.board.chessboardCorners[ids]
     corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
-    rvec, tvec, new_ids = camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
+    rvec, tvec = camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
     tvecs.append(tvec)
     rvecs.append(rvec)
 
@@ -1149,18 +1105,10 @@ def _aruco_dictionary(self):
   def _board(self):
     return self._proxyDict.board
 
-  def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify, charucos = {}, intrinsic_img = {}, extrinsic_img = []):
+  def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify, charucos = {}, intrinsic_img = {}, extrinsic_img = {}):
     """Function to calculate calibration for stereo camera."""
     start_time = time.time()
     # init object data
-    if intrinsic_img != {}:
-      for cam in intrinsic_img:
-        intrinsic_img[cam].sort(reverse=True)
-    if extrinsic_img != {}:
-      for cam in extrinsic_img:
-        extrinsic_img[cam].sort(reverse=True)
-    self.intrinsic_img = intrinsic_img
-    self.extrinsic_img = extrinsic_img
     self._cameraModel = camera_model
     self._data_path = filepath
     self._proxyDict = ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000)
@@ -1176,6 +1124,7 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
     resizeWidth, resizeHeight = 1280, 800
 
+    self.disableCamera = []
     activeCameras: List[Tuple[str, CameraData]] = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
     
     config = CalibrationConfig(
@@ -1198,16 +1147,15 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
     camInfos = {}
     stereoConfigs = []
-    pw = ParallelWorker(16)
+    pw = ParallelWorker(1)
 
     for cam, camData in activeCameras:
       if PER_CCM:
-        camData, features, ids = pw.run(get_features, config, camData, charucos[camData['name']])[:3]
+        camData, features, ids = pw.run(get_features, config, camData, intrinsic_img[camData['name']])[:3]
         if self._cameraModel == "fisheye":
           camData = pw.run(filter_features_fisheye, camData, intrinsic_img) # TODO : Input types are wrong
         else:
           features, ids = pw.map(estimate_pose_and_filter_single, config, camData, features, ids)[:2]
-          
     # camData['features'] = corners2[valid_mask].reshape(-1, 1, 2)
     # camData['ids'] = valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
     # return camData
@@ -1216,14 +1164,14 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
         camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData)
         camInfos[cam] = camData
       else:
-        camData = calibrate_ccm_intrinsics(config, camData, charucos[camData['name']])
+        camData = calibrate_ccm_intrinsics(config, camData)
 
     for left, right in stereoPairs:
       left_cam_info = camInfos[left]
       right_cam_info = camInfos[right]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
-        left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, self, config, left_cam_info, right_cam_info)[:2]
+        left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, config, left_cam_info, right_cam_info, extrinsic_img)[:2]
 
       left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, config, left_cam_info, right_cam_info)[:3]
 

From 790731b9a790b62934cb0b54e88c4f7b51cda4ea Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 15 Aug 2024 13:47:05 +0200
Subject: [PATCH 57/87] Count TaskGroup as not finished if no tasks were
 created

---
 worker.py | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/worker.py b/worker.py
index a7d2b22..61d4656 100644
--- a/worker.py
+++ b/worker.py
@@ -95,6 +95,7 @@ def __init__(self, fun, args, kwargs):
     self._fun = fun
     self._args = args
     self._kwargs = kwargs
+    self._tasks = None
 
   def __getitem__(self, key):
     return Retvals(self, key)
@@ -103,6 +104,8 @@ def __repr__(self):
     return f'<TaskGroup {self._fun}>'
 
   def finished(self) -> bool:
+    if not self._tasks:
+      return False
     for task in self._tasks:
       if not task.finished():
         return False

From 9e8fc7310c3856061a2985f51223c0f9bce819f6 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 15 Aug 2024 18:44:08 +0200
Subject: [PATCH 58/87] Calibration with multiple charuco boards

---
 calibration_utils.py | 321 ++++++++++++++++++-------------------------
 1 file changed, 137 insertions(+), 184 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 94bdb15..2d74feb 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -9,7 +9,6 @@
 from pathlib import Path
 import numpy as np
 import time
-import glob
 import cv2
 
 
@@ -49,10 +48,52 @@ def board(self):
       self.__build()
     return self._board
 
+CharucoBoard = ProxyDict # TODO : Rename the class
+
+class Dataset:
+  class Images:
+    def __init__(self, images: List[np.ndarray | str]):
+      self._images = images
+
+    def at(self, key):
+      if isinstance(self._images[key], str):
+        self._images[key] = cv2.imread(self._images[key], cv2.IMREAD_UNCHANGED)
+      return self._images[key]
+
+    def __len__(self):
+      return len(self._images)
+
+    def __iter__(self):
+      for i in len(self._images):
+        yield self.at(i)
+
+    def __getitem__(self, key):
+      return self.at(key)
+
+    def __repr__(self):
+      return self._images.__repr__()
+
+  def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str] = [], allCorners: List[np.ndarray] = [], allIds: List[np.ndarray] = [], imageSize: Tuple[float, float] = ()):
+    """Create a dataset for camera calibration
+
+    Args:
+        name (str): Name of the camera for and the key for output data
+        board (CharucoBoard): Charuco board configuration used in the dataset
+        images (List[np.ndarray  |  str], optional): Set of images for calibration, can be left empty if `allCorners`, `allIds` and `imageSize` is provided. Defaults to [].
+        allCorners (List[np.ndarray], optional): Set of corners used for intrinsic calibration. Defaults to [].
+        allIds (List[np.ndarray], optional): Set of ids used for intrinsic calibration. Defaults to [].
+        imageSize (Tuple[float, float], optional): Size of the images captured during calibration, must be provided if `images` is empty. Defaults to ().
+    """
+    self.name = name
+    self.images = Dataset.Images(images)
+    self.allCorners = allCorners
+    self.allIds = allIds
+    self.imageSize = imageSize
+    self.board = board
+
 class CalibrationConfig:
-  def __init__(self, proxyDict = ProxyDict(), enableFiltering = True, ccmModel = '', disableCameras = [], initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
+  def __init__(self, enableFiltering = True, ccmModel = '', disableCameras = [], initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
                cameraModel = 0, stereoCalibCriteria = 0, features = None):
-    self._proxyDict = proxyDict
     self.enableFiltering = enableFiltering
     self.ccmModel = ccmModel # Distortion model
     self.disableCameras = disableCameras
@@ -64,14 +105,6 @@ def __init__(self, proxyDict = ProxyDict(), enableFiltering = True, ccmModel = '
     self.stereoCalibCriteria = stereoCalibCriteria
     self.features = features # None | 'checker_board' | 'charuco'
 
-  @property
-  def dictionary(self):
-    return self._proxyDict.dictionary
-
-  @property
-  def board(self):
-    return self._proxyDict.board
-
 class CameraData(TypedDict):
   calib_model: str
   dist_coeff: str
@@ -115,8 +148,8 @@ def summary(self) -> str:
     """
     return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
-def estimate_pose_and_filter_single(config: CalibrationConfig, camData: CameraData, corners, ids):
-  objpoints = config.board.chessboardCorners[ids]
+def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBoard):
+  objpoints = charucoBoard.chessboardCorners[ids]
 
   ini_threshold=5
   threshold = None
@@ -167,22 +200,12 @@ def estimate_pose_and_filter_single(config: CalibrationConfig, camData: CameraDa
     #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
-def get_features(config: CalibrationConfig, camData: CameraData, charucos) -> Tuple[CameraData, list, list]:
-  #all_features, all_ids, imsize = getting_features(config, camData['images_path'], camData['width'], camData['height'], charucos=charucos)
-  
-  allCorners = []
-  allIds = []
-  for ids, charuco in charucos:
-    allCorners.append(charuco)
-    allIds.append(ids)
-
-#   if isinstance(all_features, str) and all_ids is None:
-#     raise RuntimeError(f'Exception {all_features}') # TODO : Handle
+def get_features(config: CalibrationConfig, camData: CameraData, dataset: Dataset) -> Tuple[CameraData, list, list]:
   f = camData['imsize'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
 
   camData['intrinsics'] = np.array([
-     [f,     0.0,    camData['width']/2],
-     [0.0,   f,      camData['height']/2],
+     [f,     0.0,    camData['imsize'][0]/2],
+     [0.0,   f,      camData['imsize'][1]/2],
      [0.0,   0.0,    1.0]
   ])
   camData['dist_coeff'] = np.zeros((12, 1))
@@ -197,19 +220,19 @@ def get_features(config: CalibrationConfig, camData: CameraData, charucos) -> Tu
   camData['threshold_stepper'] = threshold_stepper
   camData['min_inliers'] = min_inliers
 
-  return camData, allCorners, allIds
+  return camData, dataset.allCorners, dataset.allIds
 
-def estimate_pose_and_filter(config: CalibrationConfig, camData: CameraData, allCornersAndIds):
+def estimate_pose_and_filter(camData: CameraData, allCorners, allIds, charucoBoard):
   filtered_corners = []
   filtered_ids = []
-  for corners, ids in allCornersAndIds:
-    corners, ids, _ = estimate_pose_and_filter_single(config, camData, corners, ids)
+  for corners, ids in zip(allCorners, allIds):
+    corners, ids, _ = estimate_pose_and_filter_single(camData, corners, ids, charucoBoard)
     filtered_corners.append(corners)
     filtered_ids.append(ids)
 
   return filtered_corners, filtered_ids
 
-def calibrate_charuco(config: CalibrationConfig, camData: CameraData, corners, ids):
+def calibrate_charuco(camData: CameraData, corners, ids, dataset: Dataset):
   # TODO : If we still need this check it needs to be elsewhere
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
   #   raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
@@ -224,7 +247,7 @@ def calibrate_charuco(config: CalibrationConfig, camData: CameraData, corners, i
        perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
         charucoCorners=corners,
         charucoIds=ids,
-        board=config.board,
+        board=dataset.board.board,
         imageSize=camData['imsize'],
         cameraMatrix=camData['intrinsics'],
         distCoeffs=camData['dist_coeff'],
@@ -264,10 +287,10 @@ def filter_features_fisheye(cam_info, intrinsic_img, all_features, all_ids):
 
   return cam_info
 
-def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraData):
+def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraData, dataset: Dataset):
   start = time.time()
   print('starting calibrate_wf')
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, camData)
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, camData, dataset)
   if isinstance(ret, str) and all_ids is None:
     raise RuntimeError('Exception' + ret) # TODO : Handle
   print(f'calibrate_wf took {round(time.time() - start, 2)}s')
@@ -444,24 +467,23 @@ def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sa
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def find_stereo_common_features(config: CalibrationConfig, left_cam_info, right_cam_info):
-  allIds_l, allIds_r, allCorners_l, allCorners_r = left_cam_info['filtered_ids'], right_cam_info['filtered_ids'], left_cam_info['filtered_corners'], right_cam_info['filtered_corners']
+def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset):
   left_corners_sampled = []
   right_corners_sampled = []
   obj_pts = []
 
-  for i, ids in enumerate(allIds_l): # For ids in all images
-    commonIds = np.intersect1d(allIds_l[i], allIds_r[i])
-    left_sub_corners = allCorners_l[i][np.isin(allIds_l[i], commonIds)]
-    right_sub_corners = allCorners_r[i][np.isin(allIds_r[i], commonIds)]
-    obj_pts_sub = config.board.chessboardCorners[commonIds]
+  for i, _ in enumerate(leftDataset.allIds): # For ids in all images
+    commonIds = np.intersect1d(leftDataset.allIds[i], rightDataset.allIds[i])
+    left_sub_corners = leftDataset.allCorners[i][np.isin(leftDataset.allIds[i], commonIds)]
+    right_sub_corners = rightDataset.allCorners[i][np.isin(rightDataset.allIds[i], commonIds)]
+    obj_pts_sub = leftDataset.board.board.chessboardCorners[commonIds]
 
     if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
       obj_pts.append(obj_pts_sub)
       left_corners_sampled.append(left_sub_corners)
       right_corners_sampled.append(right_sub_corners)
     else:
-      return -1, "Stereo Calib failed due to less common features"
+      raise RuntimeError('Less than 3 common features found')
   return left_corners_sampled, right_corners_sampled, obj_pts
 
 def undistort_points_perspective(allCorners, camInfo):
@@ -470,27 +492,27 @@ def undistort_points_perspective(allCorners, camInfo):
 def undistort_points_fisheye(allCorners, camInfo):
   return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
-def remove_and_filter_stereo_features(config:CalibrationConfig, leftCamData: CameraData, rightCamData: CameraData, allCornersAndIds):
-  leftCamData['filtered_corners'], leftCamData['filtered_ids'] = estimate_pose_and_filter(config, leftCamData, allCornersAndIds[leftCamData['name']])
-  rightCamData['filtered_corners'], rightCamData['filtered_ids'] = estimate_pose_and_filter(config, rightCamData, allCornersAndIds[rightCamData['name']])
+def remove_and_filter_stereo_features(leftCamData: CameraData, rightCamData: CameraData, leftDataset: Dataset, rightDataset: Dataset):
+  leftCamData['filtered_corners'], leftCamData['filtered_ids'] = estimate_pose_and_filter(leftCamData, leftDataset.allCorners, leftDataset.allIds, leftDataset.board.board)
+  rightCamData['filtered_corners'], rightCamData['filtered_ids'] = estimate_pose_and_filter(rightCamData, rightDataset.allCorners, rightDataset.allIds, leftDataset.board.board)
 
   return leftCamData, rightCamData
 
 def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam, board_config, extrinsics):
-
   if extrinsics[0] == -1:
     return -1, extrinsics[1]
   stereoConfig = None
-  if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: # TODO : Is this supposed to take the last camera pair?
-    stereoConfig = {
-      'rectification_left': extrinsics[3],
-      'rectification_right': extrinsics[4]
-    }
-  elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
-    stereoConfig = {
-      'rectification_left': extrinsics[4],
-      'rectification_right': extrinsics[3]
-    }
+  if 'stereo_config' in board_config:
+    if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: # TODO : Is this supposed to take the last camera pair?
+      stereoConfig = {
+        'rectification_left': extrinsics[3],
+        'rectification_right': extrinsics[4]
+      }
+    elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+      stereoConfig = {
+        'rectification_left': extrinsics[4],
+        'rectification_right': extrinsics[3]
+      }
 
   print('<-------------Epipolar error of {} and {} ------------>'.format(
     left_cam_info['name'], right_cam_info['name']))
@@ -515,61 +537,6 @@ def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam,
 
   return left_cam_info, stereoConfig
 
-def load_camera_data(filepath, cam_info, _cameraModel, ccm_model, model, charucos, resizeWidth, resizeHeight):
-  images_path = filepath + '/' + cam_info['name']
-  image_files = glob.glob(images_path + "/*")
-  image_files.sort()
-  for im in image_files:
-    frame = cv2.imread(im)
-    height, width, _ = frame.shape
-    widthRatio = resizeWidth / width
-    heightRatio = resizeHeight / height
-    if (widthRatio > 0.8 and heightRatio > 0.8 and widthRatio <= 1.0 and heightRatio <= 1.0) or (widthRatio > 1.2 and heightRatio > 1.2) or (resizeHeight == 0):
-      resizeWidth = width
-      resizeHeight = height
-    break
-
-  images_path = filepath + '/' + cam_info['name']
-  if "calib_model" in cam_info:
-    cameraModel_ccm, model_ccm = cam_info["calib_model"].split("_")
-    if cameraModel_ccm == "fisheye":
-      model_ccm == None
-    calib_model = cameraModel_ccm
-    distortion_model = model_ccm
-  else:
-    calib_model = _cameraModel
-    if cam_info["name"] in ccm_model:
-      distortion_model = ccm_model[cam_info["name"]]
-    else:
-      distortion_model = model
-
-  cam_info['width'] = width
-  cam_info['height'] = height
-  cam_info['imsize'] = (width, height)
-  cam_info['calib_model'] = calib_model
-  cam_info['distortion_model'] = distortion_model
-  cam_info['images_path'] = images_path
-  return cam_info
-
-def getting_features(config: CalibrationConfig, img_path, width, height, charucos=None):
-  if charucos:
-    allCorners = []
-    allIds = []
-    for ids, charuco in charucos:
-      allCorners.append(charuco)
-      allIds.append(ids)
-    imsize = (width, height)
-    return allCorners, allIds, imsize
-
-  # elif config.features == None or config.features == "charucos":
-  #   allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
-  #   return allCorners, allIds, imsize
-
-  # if config.features == "checker_board":
-  #   allCorners, allIds, _, _, imsize, _ = analyze_charuco(config, img_path)
-  #   return allCorners, allIds, imsize
-  ###### ADD HERE WHAT IT IS NEEDED ######
-
 def get_distortion_flags(distortionModel):
   def is_binary_string(s: str) -> bool:
   # Check if all characters in the string are '0' or '1'
@@ -645,7 +612,7 @@ def is_binary_string(s: str) -> bool:
     flags = distortionModel
   return flags
 
-def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData):
+def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData, dataset: Dataset):
   name = camData['name']
   allCorners = camData['filtered_corners']
   allIds = camData['filtered_ids']
@@ -663,7 +630,7 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData):
     if config.features == None or config.features == "charucos":
       distortion_flags = get_distortion_flags(distortionModel)
       ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-        config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, camData['name'] == 'thermal')
+        config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, dataset)
 
       return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
     else:
@@ -697,7 +664,7 @@ def draw_corners(charuco_corners, displayframe):
   cv2.line(displayframe, start_point, end_point, color, thickness)
   return displayframe
 
-def features_filtering_function(config: CalibrationConfig, rvecs, tvecs, cameraMatrix, distCoeffs, filtered_corners,filtered_id, threshold = None):
+def features_filtering_function(rvecs, tvecs, cameraMatrix, distCoeffs, filtered_corners,filtered_id, dataset: Dataset, threshold = None):
   whole_error = []
   all_points = []
   all_corners = []
@@ -714,7 +681,7 @@ def features_filtering_function(config: CalibrationConfig, rvecs, tvecs, cameraM
   for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
     if ids is not None and corners.size > 0:
       ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-      objPoints = np.array([config.board.chessboardCorners[id] for id in ids], dtype=np.float32)
+      objPoints = np.array([dataset.board.board.chessboardCorners[id] for id in ids], dtype=np.float32)
       imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
       corners2 = corners.reshape(-1, 2)
       imgpoints2 = imgpoints2.reshape(-1, 2)
@@ -764,21 +731,17 @@ def detect_charuco_board(config: CalibrationConfig, image: np.array):
   else:
     return None, None, None, None, None
 
-def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50, skipRANSAC=False):
+def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
   objects = []
   while len(objects) < len(objpoints[:,0,0]) * min_inliers:
     if ini_threshold > max_threshold:
       break
-    if not skipRANSAC:
-      ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-      imgpoints2 = objpoints.copy()
+    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+    imgpoints2 = objpoints.copy()
 
-      all_corners = corners.copy()
-      all_corners = np.array([all_corners[id[0]] for id in objects])
-      imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
-    else:
-      imgpoints2 = objpoints.copy()
-      all_corners = corners.copy()
+    all_corners = corners.copy()
+    all_corners = np.array([all_corners[id[0]] for id in objects])
+    imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
 
     ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners, K, d)
     imgpoints2, _ = cv2.projectPoints(imgpoints2, rvec, tvec, K, d)
@@ -907,27 +870,21 @@ def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners, camera
       allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
   return corners_removed, allIds, allCorners
 
-def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, skipRANSAC):
+def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, dataset: Dataset):
   """
   Calibrates the camera using the dected corners.
   """
-  f = imsize[0] / (2 * np.tan(np.deg2rad(hfov/2)))
-
   threshold = 2 * imsize[1]/800.0
    # check if there are any suspicious corners with high reprojection error
   rvecs = []
   tvecs = []
   index = 0
-  max_threshold = 10 + config.initialMaxThreshold * (hfov / 30 + imsize[1] / 800 * 0.2)
-  min_inlier = 1 - config.initialMinFiltered * (hfov / 60 + imsize[1] / 800 * 0.2)
   for corners, ids in zip(allCorners, allIds):
-    objpts = config.board.chessboardCorners[ids]
-    rvec, tvec = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff, skipRANSAC=skipRANSAC)
+    objpts = dataset.board.board.chessboardCorners[ids]
+    rvec, tvec = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
     tvecs.append(tvec)
     rvecs.append(rvec)
     index += 1
-  if skipRANSAC:
-    threshold = 60
 
   # Here we need to get initialK and parameters for each camera ready and fill them inside reconstructed reprojection error per point
   ret = 0.0
@@ -936,7 +893,6 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
 
   #   flags = (cv2.CALIB_RATIONAL_MODEL)
   reprojection = []
-  removed_errors = []
   num_threshold = []
   iterations_array = []
   intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
@@ -949,8 +905,6 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
   translation_array_x = []
   translation_array_y = []
   translation_array_z = []
-  corner_checker = 0
-  previous_ids = []
   import time
   if True:
     whole = time.time()
@@ -966,10 +920,7 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
       translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
 
       start = time.time()
-      #if not skipRANSAC:
-      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(config, rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, allCorners, allIds, threshold = threshold)
-      #else:
-      #  filtered_corners, filtered_ids = allCorners, allIds
+      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, allCorners, allIds, threshold = threshold, dataset=dataset)
       iterations_array.append(index)
       reprojection.append(ret)
       for i in range(len(distortion_coefficients)):
@@ -985,7 +936,7 @@ def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsi
          perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
           charucoCorners=filtered_corners,
           charucoIds=filtered_ids,
-          board=config.board,
+          board=dataset.board.board,
           imageSize=imsize,
           cameraMatrix=cameraIntrinsics,
           distCoeffs=distCoeff,
@@ -1081,6 +1032,8 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
 
   return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
 
+def proxy_estimate_pose_and_filter_single(camData, corners, ids, dataset):
+  return estimate_pose_and_filter_single(camData, corners, ids, dataset.board.board)
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
@@ -1105,17 +1058,12 @@ def _aruco_dictionary(self):
   def _board(self):
     return self._proxyDict.board
 
-  def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squaresY, camera_model, enable_disp_rectify, charucos = {}, intrinsic_img = {}, extrinsic_img = {}):
+  def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Dataset] = [], extrinsics: List[Tuple[Dataset, Dataset]] = []):
     """Function to calculate calibration for stereo camera."""
     start_time = time.time()
     # init object data
     self._cameraModel = camera_model
     self._data_path = filepath
-    self._proxyDict = ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000)
-    self.squaresX = squaresX
-    self.squaresY = squaresY
-    self.squareSize = square_size
-    self.markerSize = mrk_size
     self.stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT +
                    cv2.TERM_CRITERIA_EPS, 300, 1e-9)
 
@@ -1124,56 +1072,59 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
 
     resizeWidth, resizeHeight = 1280, 800
 
-    self.disableCamera = []
-    activeCameras: List[Tuple[str, CameraData]] = [(cam, cam_info) for cam, cam_info in board_config['cameras'].items() if not cam_info['name'] in self.disableCamera]
     
     config = CalibrationConfig(
-      ProxyDict(squaresX, squaresY, square_size, mrk_size, aruco.DICT_4X4_1000),
       self.filtering_enable, self.ccm_model, self.disableCamera, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
       camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9), None
     )
 
-    stereoPairs = []
-    for camera, camInfo in activeCameras:
-      if str(camInfo['name']) in self.disableCamera \
-        or not 'extrinsics' in camInfo \
-        or not 'to_cam' in camInfo['extrinsics'] \
-        or str(board_config['cameras'][camInfo['extrinsics']['to_cam']]['name']) in self.disableCamera:
-        continue
-      stereoPairs.append([camera, camInfo['extrinsics']['to_cam']])
-
-    for cam, camData in activeCameras:
-      camData = load_camera_data(filepath, camData, self._cameraModel, self.ccm_model, self.model, charucos, resizeWidth, resizeHeight)
-
+    pw = ParallelWorker(1)
     camInfos = {}
     stereoConfigs = []
-    pw = ParallelWorker(1)
 
-    for cam, camData in activeCameras:
+    # Calibrate camera intrinsics for all provided datasets
+    for dataset in intrinsics:
+      camData = [c for c in board_config['cameras'].values() if c['name'] == dataset.name][0]
+      
+      if "calib_model" in camData:
+        cameraModel_ccm, model_ccm = camData["calib_model"].split("_")
+        if cameraModel_ccm == "fisheye":
+          model_ccm == None
+        calib_model = cameraModel_ccm
+        distortion_model = model_ccm
+      else:
+        calib_model = self._cameraModel
+        if camData["name"] in self.ccm_model:
+          distortion_model = self.ccm_model[camData["name"]]
+        else:
+          distortion_model = self.model
+
+      camData['imsize'] = dataset.imageSize
+      camData['calib_model'] = calib_model
+      camData['distortion_model'] = distortion_model
+
       if PER_CCM:
-        camData, features, ids = pw.run(get_features, config, camData, intrinsic_img[camData['name']])[:3]
+        camData, corners, ids = pw.run(get_features, config, camData, dataset)[:3]
         if self._cameraModel == "fisheye":
-          camData = pw.run(filter_features_fisheye, camData, intrinsic_img) # TODO : Input types are wrong
+          camData = pw.run(filter_features_fisheye, camData, corners, ids) # TODO : Input types are wrong
         else:
-          features, ids = pw.map(estimate_pose_and_filter_single, config, camData, features, ids)[:2]
-    # camData['features'] = corners2[valid_mask].reshape(-1, 1, 2)
-    # camData['ids'] = valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
-    # return camData
-
-          camData = pw.run(calibrate_charuco, config, camData, features, ids)
-        camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData)
-        camInfos[cam] = camData
+          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, corners, ids, dataset)[:2] # TODO : Skip for thermal
+
+          camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
+        camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)
+        camInfos[dataset.name] = camData
       else:
         camData = calibrate_ccm_intrinsics(config, camData)
 
-    for left, right in stereoPairs:
-      left_cam_info = camInfos[left]
-      right_cam_info = camInfos[right]
+    for left, right in extrinsics:
+      left_cam_info = camInfos[left.name]
+      right_cam_info = camInfos[right.name]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
-        left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, config, left_cam_info, right_cam_info, extrinsic_img)[:2]
+        # TODO : Shouldn't refilter if it's already been filtered in intrinsic calibration
+        left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, left_cam_info, right_cam_info, left, right)[:2]
 
-      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, config, left_cam_info, right_cam_info)[:3]
+      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, left, right)[:3]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
         if left_cam_info['calib_model'] == "perspective":
@@ -1191,18 +1142,20 @@ def calibrate(self, board_config, filepath, square_size, mrk_size, squaresX, squ
           extrinsics = pw.run(calibrate_stereo_perspective, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
         elif self._cameraModel == 'fisheye':
           extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-      camData, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
-      camInfos[left] = camData
+      left_cam_info, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
+      camInfos[left.name] = left_cam_info
       stereoConfigs.append(stereo_config)
 
     pw.execute()
 
     # Extract camera info structs and stereo config
     for cam, camInfo in camInfos.items():
-      board_config['cameras'][cam] = camInfo.ret()
+      for socket in board_config['cameras']:
+        if board_config['cameras'][socket]['name'] == cam:
+          board_config['cameras'][socket] = camInfo.ret()
 
     for stereoConfig in stereoConfigs:
       if stereoConfig.ret():
         board_config['stereo_config'].update(stereoConfig.ret())
 
-    return 1, board_config
\ No newline at end of file
+    return  board_config
\ No newline at end of file

From eec08f26c3ba63161ed78de51d753a850f94d26f Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 26 Aug 2024 10:17:06 +0200
Subject: [PATCH 59/87] Comments and enums

---
 calibration_utils.py | 197 ++++++++++++++++++++++++-------------------
 1 file changed, 108 insertions(+), 89 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 2d74feb..5673a36 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -7,6 +7,7 @@
 import matplotlib.pyplot as plt
 import cv2.aruco as aruco
 from pathlib import Path
+from enum import Enum
 import numpy as np
 import time
 import cv2
@@ -17,15 +18,41 @@
 PER_CCM = True
 EXTRINSICS_PER_CCM = False
 
-class ProxyDict:
+class StrEnum(Enum): # Doesn't exist in python 3.10
+  def __eq__(self, other):
+    if isinstance(other, str):
+      print('compare string')
+      return self.value == other
+    return super().__eq__(other)
+
+class CalibrationModel(StrEnum):
+  Perspective = 'perspective'
+  Fisheye = 'fisheye'
+
+class DistortionModel(StrEnum):
+  Normal = 'NORMAL'
+  Tilted = 'TILTED'
+  Prism = 'PRISM'
+  Thermal = 'THERMAL' # TODO : Is this even a distortion model
+
+class CharucoBoard:
   def __init__(self, squaresX = 16, squaresY = 9, squareSize = 1.0, markerSize = 0.8, dictSize = cv2.aruco.DICT_4X4_1000):
+    """Charuco board configuration used in a captured dataset
+
+    Args:
+        squaresX (int, optional): Number of squares horizontally. Defaults to 16.
+        squaresY (int, optional): Number of squares vertically. Defaults to 9.
+        squareSize (float, optional): Length of the side of one square (cm). Defaults to 1.0.
+        markerSize (float, optional): Length of the side of one marker (cm). Defaults to 0.8.
+        dictSize (_type_, optional): cv2 aruco dictionary size. Defaults to cv2.aruco.DICT_4X4_1000.
+    """
     self.squaresX = squaresX
     self.squaresY = squaresY
     self.squareSize = squareSize
     self.markerSize = markerSize
     self.dictSize = dictSize
 
-  def __getstate__(self):
+  def __getstate__(self): # Magic to allow pickling the instance without pickling the cv2 dictionary
     state = self.__dict__.copy()
     for hidden in ['_board', '_dictionary']:
       if hidden in state:
@@ -38,18 +65,18 @@ def __build(self):
 
   @property
   def dictionary(self):
+    """cv2 aruco dictionary"""
     if not hasattr(self, '_dictionary'):
       self.__build()
     return self._dictionary
 
   @property
   def board(self):
+    """cv2 aruco board"""
     if not hasattr(self, '_board'):
       self.__build()
     return self._board
 
-CharucoBoard = ProxyDict # TODO : Rename the class
-
 class Dataset:
   class Images:
     def __init__(self, images: List[np.ndarray | str]):
@@ -92,11 +119,23 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
     self.board = board
 
 class CalibrationConfig:
-  def __init__(self, enableFiltering = True, ccmModel = '', disableCameras = [], initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
+  def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
                cameraModel = 0, stereoCalibCriteria = 0, features = None):
+    """Calibration configuration options
+
+    Args:
+        enableFiltering (bool, optional): Whether corners should be filtered for outliers. Defaults to True.
+        ccmModel (str, optional): _description_. Defaults to ''.
+        initialMaxThreshold (int, optional): _description_. Defaults to 0.
+        initialMinFiltered (int, optional): _description_. Defaults to 0.
+        calibrationMaxThreshold (int, optional): _description_. Defaults to 0.
+        calibrationMinFiltered (int, optional): _description_. Defaults to 0.
+        cameraModel (int, optional): _description_. Defaults to 0.
+        stereoCalibCriteria (int, optional): _description_. Defaults to 0.
+        features (_type_, optional): _description_. Defaults to None.
+    """
     self.enableFiltering = enableFiltering
     self.ccmModel = ccmModel # Distortion model
-    self.disableCameras = disableCameras
     self.initialMaxThreshold = initialMaxThreshold
     self.initialMinFiltered = initialMinFiltered
     self.calibrationMaxThreshold = calibrationMaxThreshold
@@ -106,9 +145,9 @@ def __init__(self, enableFiltering = True, ccmModel = '', disableCameras = [], i
     self.features = features # None | 'checker_board' | 'charuco'
 
 class CameraData(TypedDict):
-  calib_model: str
+  calib_model: CalibrationModel
   dist_coeff: str
-  distortion_model: str
+  distortion_model: DistortionModel
   extrinsics: str
   to_cam: str
   filtered_corners: str
@@ -232,7 +271,7 @@ def estimate_pose_and_filter(camData: CameraData, allCorners, allIds, charucoBoa
 
   return filtered_corners, filtered_ids
 
-def calibrate_charuco(camData: CameraData, corners, ids, dataset: Dataset):
+def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset):
   # TODO : If we still need this check it needs to be elsewhere
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
   #   raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
@@ -245,8 +284,8 @@ def calibrate_charuco(camData: CameraData, corners, ids, dataset: Dataset):
        rotation_vectors, translation_vectors,
        stdDeviationsIntrinsics, stdDeviationsExtrinsics,
        perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-        charucoCorners=corners,
-        charucoIds=ids,
+        charucoCorners=allCorners,
+        charucoIds=allIds,
         board=dataset.board.board,
         imageSize=camData['imsize'],
         cameraMatrix=camData['intrinsics'],
@@ -258,34 +297,34 @@ def calibrate_charuco(camData: CameraData, corners, ids, dataset: Dataset):
 
   camData['intrinsics'] = camera_matrix
   camData['dist_coeff'] = distortion_coefficients
-  camData['filtered_corners'] = corners
-  camData['filtered_ids'] = ids
+  camData['filtered_corners'] = allCorners
+  camData['filtered_ids'] = allIds
   return camData
 
-def filter_features_fisheye(cam_info, intrinsic_img, all_features, all_ids):
-  f = cam_info['imsize'][0] / (2 * np.tan(np.deg2rad(cam_info["hfov"]/2)))
+def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features, all_ids):
+  f = camData['imsize'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
   print("INTRINSIC CALIBRATION")
-  cameraIntrinsics = np.array([[f,  0.0,    cam_info['imsize'][0]/2],
-                 [0.0,   f,    cam_info['imsize'][1]/2],
+  cameraIntrinsics = np.array([[f,  0.0,    camData['imsize'][0]/2],
+                 [0.0,   f,    camData['imsize'][1]/2],
                  [0.0,   0.0,    1.0]])
 
   distCoeff = np.zeros((12, 1))
 
-  if cam_info["name"] in intrinsic_img:
+  if camData["name"] in intrinsic_img:
     raise RuntimeError('This is broken')
-    all_features, all_ids, filtered_images = remove_features(filtered_features, filtered_ids, intrinsic_img[cam_info["name"]], image_files)
+    all_features, all_ids, filtered_images = remove_features(filtered_features, filtered_ids, intrinsic_img[camData["name"]], image_files)
   else:
-    filtered_images = cam_info['images_path']
+    filtered_images = camData['images_path']
 
   filtered_features = all_features
   filtered_ids = all_ids
 
-  cam_info['filtered_ids'] = filtered_ids
-  cam_info['filtered_corners'] = filtered_features
-  cam_info['intrinsics'] = cameraIntrinsics
-  cam_info['dist_coeff'] = distCoeff
+  camData['filtered_ids'] = filtered_ids
+  camData['filtered_corners'] = filtered_features
+  camData['intrinsics'] = cameraIntrinsics
+  camData['dist_coeff'] = distCoeff
 
-  return cam_info
+  return camData
 
 def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraData, dataset: Dataset):
   start = time.time()
@@ -345,10 +384,10 @@ def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
 
   return camData
 
-def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
-  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff'], left_cam_info['distortion_model']
-  specTranslation = left_cam_info['extrinsics']['specTranslation']
-  rot = left_cam_info['extrinsics']['rotation']
+def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, allLeftCorners, allRightCorners, leftCamData: CameraData, rightCamData: CameraData):
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = leftCamData['intrinsics'], leftCamData['dist_coeff'], rightCamData['intrinsics'], rightCamData['dist_coeff'], leftCamData['distortion_model']
+  specTranslation = leftCamData['extrinsics']['specTranslation']
+  rot = leftCamData['extrinsics']['rotation']
 
   t_in = np.array(
     [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
@@ -363,7 +402,7 @@ def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, left_corner
   flags += distortion_flags
   # print(flags)
   ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-  obj_pts, left_corners_sampled, right_corners_sampled,
+  obj_pts, allLeftCorners, allRightCorners,
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
   R=r_in, T=t_in, criteria=config.stereoCalibCriteria, flags=flags)
 
@@ -526,86 +565,76 @@ def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam,
     scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
     print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
     left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
-    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0]*np.sqrt(scale) # TODO :Remove one of these
   else:
     print(f"Epipolar error {extrinsics[0]}")
     left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
-    left_cam_info['extrinsics']['stereo_error'] = extrinsics[0] # TODO : Remove one of these
 
   left_cam_info['extrinsics']['rotation_matrix'] = extrinsics[1]
   left_cam_info['extrinsics']['translation'] = extrinsics[2]
 
   return left_cam_info, stereoConfig
 
-def get_distortion_flags(distortionModel):
-  def is_binary_string(s: str) -> bool:
-  # Check if all characters in the string are '0' or '1'
-    return all(char in '01' for char in s)
+def get_distortion_flags(distortionModel: DistortionModel):
   if distortionModel == None:
     print("Use DEFAULT model")
     flags = cv2.CALIB_RATIONAL_MODEL
-  elif is_binary_string(distortionModel):
+
+  elif all(char in '01' for char in distortionModel):
     flags = cv2.CALIB_RATIONAL_MODEL
     flags += cv2.CALIB_TILTED_MODEL
     flags += cv2.CALIB_THIN_PRISM_MODEL
-    binary_number = int(distortionModel, 2)
-    # Print the results
-    if binary_number == 0:
-      clauses_status = [True, True,True, True, True, True, True, True, True]
-    else:
-      clauses_status = [(binary_number & (1 << i)) != 0 for i in range(len(distortionModel))]
-      clauses_status = clauses_status[::-1]
-    if clauses_status[0]:
+    distFlags = int(distortionModel, 2)
+
+    if distFlags & (1 << 0):
       print("FIX_K1")
       flags += cv2.CALIB_FIX_K1
-    if clauses_status[1]:
+    if distFlags & (1 << 1):
       print("FIX_K2")
       flags += cv2.CALIB_FIX_K2
-    if clauses_status[2]:
+    if distFlags & (1 << 2):
       print("FIX_K3")
       flags += cv2.CALIB_FIX_K3
-    if clauses_status[3]:
+    if distFlags & (1 << 3):
       print("FIX_K4")
       flags += cv2.CALIB_FIX_K4
-    if clauses_status[4]:
+    if distFlags & (1 << 4):
       print("FIX_K5")
       flags += cv2.CALIB_FIX_K5
-    if clauses_status[5]:
+    if distFlags & (1 << 5):
       print("FIX_K6")
       flags += cv2.CALIB_FIX_K6
-    if clauses_status[6]:
+    if distFlags & (1 << 6):
       print("FIX_TANGENT_DISTORTION")
       flags += cv2.CALIB_ZERO_TANGENT_DIST
-    if clauses_status[7]:
+    if distFlags & (1 << 7):
       print("FIX_TILTED_DISTORTION")
       flags += cv2.CALIB_FIX_TAUX_TAUY
-    if clauses_status[8]:
+    if distFlags & (1 << 8):
       print("FIX_PRISM_DISTORTION")
       flags += cv2.CALIB_FIX_S1_S2_S3_S4
 
-  elif isinstance(distortionModel, str):
-    if distortionModel == "NORMAL":
-      print("Using NORMAL model")
-      flags = cv2.CALIB_RATIONAL_MODEL
-      flags += cv2.CALIB_TILTED_MODEL
-
-    elif distortionModel == "TILTED":
-      print("Using TILTED model")
-      flags = cv2.CALIB_RATIONAL_MODEL
-      flags += cv2.CALIB_TILTED_MODEL
-
-    elif distortionModel == "PRISM":
-      print("Using PRISM model")
-      flags = cv2.CALIB_RATIONAL_MODEL
-      flags += cv2.CALIB_TILTED_MODEL
-      flags += cv2.CALIB_THIN_PRISM_MODEL
-
-    elif distortionModel == "THERMAL":
-      print("Using THERMAL model")
-      flags = cv2.CALIB_RATIONAL_MODEL
-      flags += cv2.CALIB_FIX_K3
-      flags += cv2.CALIB_FIX_K5
-      flags += cv2.CALIB_FIX_K6
+  elif distortionModel == DistortionModel.Normal:
+    print("Using NORMAL model")
+    flags = cv2.CALIB_RATIONAL_MODEL
+    flags += cv2.CALIB_TILTED_MODEL
+
+  elif distortionModel == DistortionModel.Tilted:
+    print("Using TILTED model")
+    flags = cv2.CALIB_RATIONAL_MODEL
+    flags += cv2.CALIB_TILTED_MODEL
+
+  elif distortionModel == DistortionModel.Prism:
+    print("Using PRISM model")
+    flags = cv2.CALIB_RATIONAL_MODEL
+    flags += cv2.CALIB_TILTED_MODEL
+    flags += cv2.CALIB_THIN_PRISM_MODEL
+
+  elif distortionModel == DistortionModel.Thermal:
+    print("Using THERMAL model")
+    flags = cv2.CALIB_RATIONAL_MODEL
+    flags += cv2.CALIB_FIX_K3
+    flags += cv2.CALIB_FIX_K5
+    flags += cv2.CALIB_FIX_K6
 
   elif isinstance(distortionModel, int):
     print("Using CUSTOM flags")
@@ -1037,12 +1066,10 @@ def proxy_estimate_pose_and_filter_single(camData, corners, ids, dataset):
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
-  def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, disableCamera: list = [], model = None,distortion_model = {}, filtering_enable = False, initial_max_threshold = 15, initial_min_filtered = 0.05, calibration_max_threshold = 10):
+  def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, model = None,distortion_model = {}, filtering_enable = False, initial_max_threshold = 15, initial_min_filtered = 0.05, calibration_max_threshold = 10):
     self.filtering_enable = filtering_enable
     self.ccm_model = distortion_model
-    self.model = model
     self.output_scale_factor = outputScaleFactor
-    self.disableCamera = disableCamera
     self.initial_max_threshold = initial_max_threshold
     self.initial_min_filtered = initial_min_filtered
     self.calibration_max_threshold = calibration_max_threshold
@@ -1050,14 +1077,6 @@ def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, disa
 
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
-  @property
-  def _aruco_dictionary(self):
-    return self._proxyDict.dictionary
-
-  @property
-  def _board(self):
-    return self._proxyDict.board
-
   def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Dataset] = [], extrinsics: List[Tuple[Dataset, Dataset]] = []):
     """Function to calculate calibration for stereo camera."""
     start_time = time.time()
@@ -1074,7 +1093,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
 
     
     config = CalibrationConfig(
-      self.filtering_enable, self.ccm_model, self.disableCamera, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
+      self.filtering_enable, self.ccm_model, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
       camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9), None
     )
 
@@ -1097,7 +1116,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
         if camData["name"] in self.ccm_model:
           distortion_model = self.ccm_model[camData["name"]]
         else:
-          distortion_model = self.model
+          distortion_model = DistortionModel.Tilted # Use the tilted model by default
 
       camData['imsize'] = dataset.imageSize
       camData['calib_model'] = calib_model

From 9a027e1598799ab9349f547158bec7aa56a3f95f Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 26 Aug 2024 10:22:32 +0200
Subject: [PATCH 60/87] Remove unused features selection

---
 calibration_utils.py | 40 +++++++++++++---------------------------
 1 file changed, 13 insertions(+), 27 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 5673a36..7a1fca1 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -120,7 +120,7 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
 
 class CalibrationConfig:
   def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
-               cameraModel = 0, stereoCalibCriteria = 0, features = None):
+               cameraModel = 0, stereoCalibCriteria = 0):
     """Calibration configuration options
 
     Args:
@@ -132,7 +132,6 @@ def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold =
         calibrationMinFiltered (int, optional): _description_. Defaults to 0.
         cameraModel (int, optional): _description_. Defaults to 0.
         stereoCalibCriteria (int, optional): _description_. Defaults to 0.
-        features (_type_, optional): _description_. Defaults to None.
     """
     self.enableFiltering = enableFiltering
     self.ccmModel = ccmModel # Distortion model
@@ -142,7 +141,6 @@ def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold =
     self.calibrationMinFiltered = calibrationMinFiltered
     self.cameraModel = cameraModel
     self.stereoCalibCriteria = stereoCalibCriteria
-    self.features = features # None | 'checker_board' | 'charuco'
 
 class CameraData(TypedDict):
   calib_model: CalibrationModel
@@ -169,7 +167,6 @@ class CameraData(TypedDict):
   reprojection_error: str
   size: str
   threshold_stepper: str
-  features: str
   ids: str
 
 colors = [(0, 255 , 0), (0, 0, 255)]
@@ -656,25 +653,20 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData, data
   coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
   coverageImage = draw_corners(allCorners, coverageImage)
   if calib_model == 'perspective':
-    if config.features == None or config.features == "charucos":
-      distortion_flags = get_distortion_flags(distortionModel)
-      ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-        config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, dataset)
+    distortion_flags = get_distortion_flags(distortionModel)
+    ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
+      config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, dataset)
 
-      return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-    else:
-      return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
-    #### ADD ADDITIONAL FEATURES CALIBRATION ####
+    return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
   else:
-    if config.features == None or config.features == "charucos":
-      print('Fisheye--------------------------------------------------')
-      ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
-        config, allCorners, allIds, imsize, hfov, name)
-      print('Fisheye rotation vector', rotation_vectors[0])
-      print('Fisheye translation vector', translation_vectors[0])
+    print('Fisheye--------------------------------------------------')
+    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
+      config, allCorners, allIds, imsize, hfov, name)
+    print('Fisheye rotation vector', rotation_vectors[0])
+    print('Fisheye translation vector', translation_vectors[0])
 
-      # (Height, width)
-      return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
+    # (Height, width)
+    return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
 
 def draw_corners(charuco_corners, displayframe):
   for corners in charuco_corners:
@@ -1079,7 +1071,6 @@ def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, mode
 
   def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Dataset] = [], extrinsics: List[Tuple[Dataset, Dataset]] = []):
     """Function to calculate calibration for stereo camera."""
-    start_time = time.time()
     # init object data
     self._cameraModel = camera_model
     self._data_path = filepath
@@ -1087,9 +1078,6 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
                    cv2.TERM_CRITERIA_EPS, 300, 1e-9)
 
     self.cams = []
-    features = None
-
-    resizeWidth, resizeHeight = 1280, 800
 
     
     config = CalibrationConfig(
@@ -1153,9 +1141,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
           left_corners_sampled = pw.run(undistort_points_fisheye, left_corners_sampled, left_cam_info)
           right_corners_sampled = pw.run(undistort_points_fisheye, right_corners_sampled, right_cam_info)
 
-        if features == None or features == "charucos":
-          extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-        #### ADD OTHER CALIBRATION METHODS ###
+        extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
       else:
         if self._cameraModel == 'perspective':
           extrinsics = pw.run(calibrate_stereo_perspective, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)

From 896338d960822a63f63daa6bce1195a60354b6d5 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 26 Aug 2024 10:28:24 +0200
Subject: [PATCH 61/87] Fix conversion bug

---
 calibration_utils.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 7a1fca1..9a4622b 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -576,7 +576,7 @@ def get_distortion_flags(distortionModel: DistortionModel):
     print("Use DEFAULT model")
     flags = cv2.CALIB_RATIONAL_MODEL
 
-  elif all(char in '01' for char in distortionModel):
+  elif all(char in '01' for char in str(distortionModel)):
     flags = cv2.CALIB_RATIONAL_MODEL
     flags += cv2.CALIB_TILTED_MODEL
     flags += cv2.CALIB_THIN_PRISM_MODEL

From dc58edb1a96264c6a54dba3b4334056c6178466d Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 26 Aug 2024 10:48:58 +0200
Subject: [PATCH 62/87] Remove explicit thermal switch cases, remove duplicate
 'size' value, allow disabling filtering

---
 calibration_utils.py | 63 ++++++++++++++++++++------------------------
 1 file changed, 29 insertions(+), 34 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 9a4622b..593b4e4 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -100,7 +100,7 @@ def __getitem__(self, key):
     def __repr__(self):
       return self._images.__repr__()
 
-  def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str] = [], allCorners: List[np.ndarray] = [], allIds: List[np.ndarray] = [], imageSize: Tuple[float, float] = ()):
+  def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str] = [], allCorners: List[np.ndarray] = [], allIds: List[np.ndarray] = [], imageSize: Tuple[float, float] = (), enableFiltering: bool = True):
     """Create a dataset for camera calibration
 
     Args:
@@ -110,6 +110,7 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
         allCorners (List[np.ndarray], optional): Set of corners used for intrinsic calibration. Defaults to [].
         allIds (List[np.ndarray], optional): Set of ids used for intrinsic calibration. Defaults to [].
         imageSize (Tuple[float, float], optional): Size of the images captured during calibration, must be provided if `images` is empty. Defaults to ().
+        enableFiltering (bool, optional): Whether to filter provided corners, or use all of them as is. Defaults to True.
     """
     self.name = name
     self.images = Dataset.Images(images)
@@ -117,6 +118,7 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
     self.allIds = allIds
     self.imageSize = imageSize
     self.board = board
+    self.enableFiltering = enableFiltering
 
 class CalibrationConfig:
   def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
@@ -195,18 +197,13 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
   while len(objects) < len(objpoints[:,0,0]) * camData['min_inliers']:
     if ini_threshold > camData['max_threshold']:
       break
-    if camData['name'] == 'thermal': # TODO : It doesn't make sense to rerun this loop if Ransac is dsabled
-      imgpoints2 = objpoints.copy()
-
-      all_corners2 = corners.copy()
-    else:
-      ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, camData['intrinsics'], camData['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
-      all_objects.append(objects)
-      imgpoints2 = objpoints.copy()
+    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, camData['intrinsics'], camData['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+    all_objects.append(objects)
+    imgpoints2 = objpoints.copy()
 
-      all_corners2 = corners.copy()
-      all_corners2 = np.array([all_corners2[id[0]] for id in objects])
-      imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
+    all_corners2 = corners.copy()
+    all_corners2 = np.array([all_corners2[id[0]] for id in objects])
+    imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
 
     ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, camData['intrinsics'], camData['dist_coeff'])
 
@@ -237,21 +234,21 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
 def get_features(config: CalibrationConfig, camData: CameraData, dataset: Dataset) -> Tuple[CameraData, list, list]:
-  f = camData['imsize'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
+  f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
 
   camData['intrinsics'] = np.array([
-     [f,     0.0,    camData['imsize'][0]/2],
-     [0.0,   f,      camData['imsize'][1]/2],
+     [f,     0.0,    camData['size'][0]/2],
+     [0.0,   f,      camData['size'][1]/2],
      [0.0,   0.0,    1.0]
   ])
   camData['dist_coeff'] = np.zeros((12, 1))
 
    # check if there are any suspicious corners with high reprojection error
-  max_threshold = 75 + config.initialMaxThreshold * (camData['hfov']/ 30 + camData['imsize'][1] / 800 * 0.2)
-  threshold_stepper = int(1.5 * (camData['hfov'] / 30 + camData['imsize'][1] / 800))
+  max_threshold = 75 + config.initialMaxThreshold * (camData['hfov']/ 30 + camData['size'][1] / 800 * 0.2)
+  threshold_stepper = int(1.5 * (camData['hfov'] / 30 + camData['size'][1] / 800))
   if threshold_stepper < 1:
     threshold_stepper = 1
-  min_inliers = 1 - config.initialMinFiltered * (camData['hfov'] / 60 + camData['imsize'][1] / 800 * 0.2)
+  min_inliers = 1 - config.initialMinFiltered * (camData['hfov'] / 60 + camData['size'][1] / 800 * 0.2)
   camData['max_threshold'] = max_threshold
   camData['threshold_stepper'] = threshold_stepper
   camData['min_inliers'] = min_inliers
@@ -284,13 +281,13 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
         charucoCorners=allCorners,
         charucoIds=allIds,
         board=dataset.board.board,
-        imageSize=camData['imsize'],
+        imageSize=camData['size'],
         cameraMatrix=camData['intrinsics'],
         distCoeffs=camData['dist_coeff'],
         flags=flags,
         criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
   #except:
-  #  return f"First intrisic calibration failed for {cam_info['imsize']}", None, None
+  #  return f"First intrisic calibration failed for {cam_info['size']}", None, None
 
   camData['intrinsics'] = camera_matrix
   camData['dist_coeff'] = distortion_coefficients
@@ -299,10 +296,10 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
   return camData
 
 def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features, all_ids):
-  f = camData['imsize'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
+  f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
   print("INTRINSIC CALIBRATION")
-  cameraIntrinsics = np.array([[f,  0.0,    camData['imsize'][0]/2],
-                 [0.0,   f,    camData['imsize'][1]/2],
+  cameraIntrinsics = np.array([[f,  0.0,    camData['size'][0]/2],
+                 [0.0,   f,    camData['size'][1]/2],
                  [0.0,   0.0,    1.0]])
 
   distCoeff = np.zeros((12, 1))
@@ -333,7 +330,6 @@ def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraD
 
   camData['intrinsics'] = cameraIntrinsics
   camData['dist_coeff'] = distCoeff
-  camData['size'] = size # (Width, height)
   camData['reprojection_error'] = ret
   print("Reprojection error of {0}: {1}".format(
     camData['name'], ret))
@@ -341,7 +337,7 @@ def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraD
   return camData
 
 def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
-  imsize = camData['imsize']
+  imsize = camData['size']
   hfov = camData['hfov']
   name = camData['name']
   allCorners = camData['filtered_corners'] # TODO : I don't think this has a way to get here from one of the codepaths in matin in the else:
@@ -355,7 +351,7 @@ def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
   if calib_model == 'perspective':
     distortion_flags = get_distortion_flags(distortionModel)
     ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-      config, allCorners, allIds, imsize, hfov, distortion_flags, camData['intrinsics'], camData['dist_coeff'], camData['name'] == 'thermal')
+      allCorners, allIds, imsize, distortion_flags, camData['intrinsics'], camData['dist_coeff'])
     # undistort_visualization(
     #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
 
@@ -374,7 +370,6 @@ def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
 
   camData['intrinsics'] = camera_matrix
   camData['dist_coeff'] = distortion_coefficients
-  camData['size'] = size # (Width, height)
   camData['reprojection_error'] = ret
   print("Reprojection error of {0}: {1}".format(
     camData['name'], ret))
@@ -642,7 +637,7 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData, data
   name = camData['name']
   allCorners = camData['filtered_corners']
   allIds = camData['filtered_ids']
-  imsize = camData['imsize']
+  imsize = camData['size']
   hfov = camData['hfov']
   calib_model = camData['calib_model']
   distortionModel = camData['distortion_model']
@@ -655,7 +650,7 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData, data
   if calib_model == 'perspective':
     distortion_flags = get_distortion_flags(distortionModel)
     ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-      config, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, dataset)
+      allCorners, allIds, imsize, distortion_flags, cameraIntrinsics, distCoeff, dataset)
 
     return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
   else:
@@ -891,7 +886,7 @@ def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners, camera
       allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
   return corners_removed, allIds, allCorners
 
-def calibrate_camera_charuco(config: CalibrationConfig, allCorners, allIds, imsize, hfov, distortion_flags, cameraIntrinsics, distCoeff, dataset: Dataset):
+def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags, cameraIntrinsics, distCoeff, dataset: Dataset):
   """
   Calibrates the camera using the dected corners.
   """
@@ -1082,7 +1077,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
     
     config = CalibrationConfig(
       self.filtering_enable, self.ccm_model, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
-      camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9), None
+      camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
     )
 
     pw = ParallelWorker(1)
@@ -1106,7 +1101,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
         else:
           distortion_model = DistortionModel.Tilted # Use the tilted model by default
 
-      camData['imsize'] = dataset.imageSize
+      camData['size'] = dataset.imageSize
       camData['calib_model'] = calib_model
       camData['distortion_model'] = distortion_model
 
@@ -1114,8 +1109,8 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
         camData, corners, ids = pw.run(get_features, config, camData, dataset)[:3]
         if self._cameraModel == "fisheye":
           camData = pw.run(filter_features_fisheye, camData, corners, ids) # TODO : Input types are wrong
-        else:
-          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, corners, ids, dataset)[:2] # TODO : Skip for thermal
+        elif dataset.enableFiltering:
+          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, corners, ids, dataset)[:2]
 
           camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
         camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)

From a7eeca1684dd6f7a8d01f9f1f64d4f49042efee4 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 30 Aug 2024 17:32:17 +0200
Subject: [PATCH 63/87] Adjust filterint so thermal cameras pass

---
 calibration_utils.py | 56 ++++++++++++++++++++++++++++++++------------
 1 file changed, 41 insertions(+), 15 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 593b4e4..12dbc59 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -198,6 +198,10 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
     if ini_threshold > camData['max_threshold']:
       break
     ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, camData['intrinsics'], camData['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+
+    if not ret:
+      raise RuntimeError('Exception') # TODO : Handle
+
     all_objects.append(objects)
     imgpoints2 = objpoints.copy()
 
@@ -208,8 +212,6 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
     ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, camData['intrinsics'], camData['dist_coeff'])
 
     ini_threshold += camData['threshold_stepper']
-  if not ret:
-    raise RuntimeError('Exception') # TODO : Handle
 
   if ids is not None and corners.size > 0: # TODO : Try to remove the np reshaping
     ids = ids.flatten()  # Flatten the IDs from 2D to 1D
@@ -233,7 +235,7 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
     #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
-def get_features(config: CalibrationConfig, camData: CameraData, dataset: Dataset) -> Tuple[CameraData, list, list]:
+def get_features(config: CalibrationConfig, camData: CameraData) -> CameraData:
   f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
 
   camData['intrinsics'] = np.array([
@@ -253,7 +255,7 @@ def get_features(config: CalibrationConfig, camData: CameraData, dataset: Datase
   camData['threshold_stepper'] = threshold_stepper
   camData['min_inliers'] = min_inliers
 
-  return camData, dataset.allCorners, dataset.allIds
+  return camData
 
 def estimate_pose_and_filter(camData: CameraData, allCorners, allIds, charucoBoard):
   filtered_corners = []
@@ -272,14 +274,27 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
 
   distortion_flags = get_distortion_flags(camData['distortion_model'])
   flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
+  
+  # Convert to int32 from uint32 # TODO : Shouldn't be necessary
+  for i, ids in enumerate(allIds): allIds[i] = ids.reshape(-1, 1).astype(np.int32)
+  
+  # Filter for only images with >6 corners # TODO : Shouldn't be in here, should be in a separate necessary filtering function
+  allCorners2 = []
+  allIds2 = []
+  
+  for corners, ids in zip(allCorners, allIds):
+    if len(ids) < 6:
+      continue
+    allCorners2.append(corners)
+    allIds2.append(ids)
 
   #try:
   (ret, camera_matrix, distortion_coefficients,
        rotation_vectors, translation_vectors,
        stdDeviationsIntrinsics, stdDeviationsExtrinsics,
        perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-        charucoCorners=allCorners,
-        charucoIds=allIds,
+        charucoCorners=allCorners2,
+        charucoIds=allIds2,
         board=dataset.board.board,
         imageSize=camData['size'],
         cameraMatrix=camData['intrinsics'],
@@ -291,8 +306,8 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
 
   camData['intrinsics'] = camera_matrix
   camData['dist_coeff'] = distortion_coefficients
-  camData['filtered_corners'] = allCorners
-  camData['filtered_ids'] = allIds
+  camData['filtered_corners'] = allCorners2
+  camData['filtered_ids'] = allIds2
   return camData
 
 def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features, all_ids):
@@ -502,6 +517,7 @@ def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset):
   left_corners_sampled = []
   right_corners_sampled = []
   obj_pts = []
+  failed = 0
 
   for i, _ in enumerate(leftDataset.allIds): # For ids in all images
     commonIds = np.intersect1d(leftDataset.allIds[i], rightDataset.allIds[i])
@@ -509,12 +525,16 @@ def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset):
     right_sub_corners = rightDataset.allCorners[i][np.isin(rightDataset.allIds[i], commonIds)]
     obj_pts_sub = leftDataset.board.board.chessboardCorners[commonIds]
 
-    if len(left_sub_corners) > 3 and len(right_sub_corners) > 3:
+    if len(commonIds) > 6:
       obj_pts.append(obj_pts_sub)
       left_corners_sampled.append(left_sub_corners)
       right_corners_sampled.append(right_sub_corners)
     else:
-      raise RuntimeError('Less than 3 common features found')
+      failed += 1
+
+  if failed > len(leftDataset.allIds) / 3:
+    raise RuntimeError('More than 1/3 of images had less than 6 common features found')
+
   return left_corners_sampled, right_corners_sampled, obj_pts
 
 def undistort_points_perspective(allCorners, camInfo):
@@ -1079,6 +1099,10 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
       self.filtering_enable, self.ccm_model, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
       camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
     )
+    
+    for a, b in extrinsics:
+      if len(a.allIds) != len(b.allIds):
+        raise RuntimeError('Not all dataset for extrinsic calibration have the same number of images') # TODO : This isn't thorough enough
 
     pw = ParallelWorker(1)
     camInfos = {}
@@ -1106,13 +1130,15 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
       camData['distortion_model'] = distortion_model
 
       if PER_CCM:
-        camData, corners, ids = pw.run(get_features, config, camData, dataset)[:3]
+        camData = pw.run(get_features, config, camData)
         if self._cameraModel == "fisheye":
-          camData = pw.run(filter_features_fisheye, camData, corners, ids) # TODO : Input types are wrong
+          camData = pw.run(filter_features_fisheye, camData, dataset.allCorners, dataset.allIds) # TODO : Input types are wrong
         elif dataset.enableFiltering:
-          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, corners, ids, dataset)[:2]
+          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, dataset.allCorners, dataset.allIds, dataset)[:2]
+        else:
+          corners, ids = dataset.allCorners, dataset.allIds
 
-          camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
+        camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
         camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)
         camInfos[dataset.name] = camData
       else:
@@ -1123,7 +1149,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
       right_cam_info = camInfos[right.name]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
-        # TODO : Shouldn't refilter if it's already been filtered in intrinsic calibration
+        # TODO : Shouldn't refilter if it's already been filtered in intrinsic calibration, or should at least use two calls to filter_single
         left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, left_cam_info, right_cam_info, left, right)[:2]
 
       left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, left, right)[:3]

From 9fe41f97ce0cec3e2cae0f30b261362259ad361c Mon Sep 17 00:00:00 2001
From: Matic Tonin <matictonin@gmail.com>
Date: Thu, 12 Sep 2024 13:41:11 +0200
Subject: [PATCH 64/87] Stereo fix for OAK-D-LITE in speed branch, needs
 testing

---
 calibration_utils.py | 23 ++++++++++++++++++++---
 1 file changed, 20 insertions(+), 3 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 12dbc59..152d604 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -513,12 +513,29 @@ def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sa
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset):
+def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset, leftCamData: CameraData, rightCamData: CameraData):
   left_corners_sampled = []
   right_corners_sampled = []
   obj_pts = []
   failed = 0
-
+  rvecs = []
+  tvecs = []
+  for corners, ids in zip(leftDataset.allCorners, leftDataset.allIds):
+    objpts = leftDataset.board.board.chessboardCorners[ids]
+    rvec, tvec = camera_pose_charuco(objpts, corners, ids, leftCamData['intrinsics'], leftCamData['dist_coeff'])
+    tvecs.append(tvec)
+    rvecs.append(rvec)
+  leftDataset.allCorners, leftDataset.allIds, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(rvec, tvec, leftCamData['intrinsics'], leftCamData['dist_coeff'], leftDataset.allCorners, leftDataset.allIds, threshold = 2, dataset=leftDataset)
+  
+  rvecs = []
+  tvecs = []
+  for corners, ids in zip(rightDataset.allCorners, rightDataset.allIds):
+    objpts = rightDataset.board.board.chessboardCorners[ids]
+    rvec, tvec = camera_pose_charuco(objpts, corners, ids, rightCamData['intrinsics'], rightCamData['dist_coeff'])
+    tvecs.append(tvec)
+    rvecs.append(rvec)
+  rightDataset.allCorners, rightDataset.allIds, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(rvec, tvec, rightCamData['intrinsics'], rightCamData['dist_coeff'], rightDataset.allCorners, rightDataset.allIds, threshold = 2, dataset=rightDataset)
+  
   for i, _ in enumerate(leftDataset.allIds): # For ids in all images
     commonIds = np.intersect1d(leftDataset.allIds[i], rightDataset.allIds[i])
     left_sub_corners = leftDataset.allCorners[i][np.isin(leftDataset.allIds[i], commonIds)]
@@ -1152,7 +1169,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
         # TODO : Shouldn't refilter if it's already been filtered in intrinsic calibration, or should at least use two calls to filter_single
         left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, left_cam_info, right_cam_info, left, right)[:2]
 
-      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, left, right)[:3]
+      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, left, right, left_cam_info, right_cam_info)[:3]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
         if left_cam_info['calib_model'] == "perspective":

From ad88194ec101b2ca7b0fb4086c19523cdd6e2245 Mon Sep 17 00:00:00 2001
From: Matic Tonin <77620080+MaticTonin@users.noreply.github.com>
Date: Fri, 27 Sep 2024 15:42:53 +0200
Subject: [PATCH 65/87] Revert "Stereo fix for OAK-D-LITE in speed branch,
 needs testing"

---
 calibration_utils.py | 23 +++--------------------
 1 file changed, 3 insertions(+), 20 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 152d604..12dbc59 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -513,29 +513,12 @@ def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sa
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset, leftCamData: CameraData, rightCamData: CameraData):
+def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset):
   left_corners_sampled = []
   right_corners_sampled = []
   obj_pts = []
   failed = 0
-  rvecs = []
-  tvecs = []
-  for corners, ids in zip(leftDataset.allCorners, leftDataset.allIds):
-    objpts = leftDataset.board.board.chessboardCorners[ids]
-    rvec, tvec = camera_pose_charuco(objpts, corners, ids, leftCamData['intrinsics'], leftCamData['dist_coeff'])
-    tvecs.append(tvec)
-    rvecs.append(rvec)
-  leftDataset.allCorners, leftDataset.allIds, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(rvec, tvec, leftCamData['intrinsics'], leftCamData['dist_coeff'], leftDataset.allCorners, leftDataset.allIds, threshold = 2, dataset=leftDataset)
-  
-  rvecs = []
-  tvecs = []
-  for corners, ids in zip(rightDataset.allCorners, rightDataset.allIds):
-    objpts = rightDataset.board.board.chessboardCorners[ids]
-    rvec, tvec = camera_pose_charuco(objpts, corners, ids, rightCamData['intrinsics'], rightCamData['dist_coeff'])
-    tvecs.append(tvec)
-    rvecs.append(rvec)
-  rightDataset.allCorners, rightDataset.allIds, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(rvec, tvec, rightCamData['intrinsics'], rightCamData['dist_coeff'], rightDataset.allCorners, rightDataset.allIds, threshold = 2, dataset=rightDataset)
-  
+
   for i, _ in enumerate(leftDataset.allIds): # For ids in all images
     commonIds = np.intersect1d(leftDataset.allIds[i], rightDataset.allIds[i])
     left_sub_corners = leftDataset.allCorners[i][np.isin(leftDataset.allIds[i], commonIds)]
@@ -1169,7 +1152,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
         # TODO : Shouldn't refilter if it's already been filtered in intrinsic calibration, or should at least use two calls to filter_single
         left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, left_cam_info, right_cam_info, left, right)[:2]
 
-      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, left, right, left_cam_info, right_cam_info)[:3]
+      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, left, right)[:3]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
         if left_cam_info['calib_model'] == "perspective":

From d7bb7172e624c7fd4dfe89c3c415db8dfab1575c Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 11 Nov 2024 15:19:57 +0100
Subject: [PATCH 66/87] Move types to separate file

---
 calibration_utils.py | 166 +------------------------------------------
 types.py             | 158 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 160 insertions(+), 164 deletions(-)
 create mode 100644 types.py

diff --git a/calibration_utils.py b/calibration_utils.py
index 12dbc59..876bd51 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1,176 +1,14 @@
-import logging
-logging.getLogger('matplotlib').setLevel(logging.WARNING)
-
 from scipy.spatial.transform import Rotation
-from typing import TypedDict, List, Tuple
 from .worker import ParallelWorker
-import matplotlib.pyplot as plt
-import cv2.aruco as aruco
+from typing import List, Tuple
 from pathlib import Path
-from enum import Enum
+from .types import *
 import numpy as np
 import time
 import cv2
 
-
-plt.rcParams.update({'font.size': 16})
-
 PER_CCM = True
 EXTRINSICS_PER_CCM = False
-
-class StrEnum(Enum): # Doesn't exist in python 3.10
-  def __eq__(self, other):
-    if isinstance(other, str):
-      print('compare string')
-      return self.value == other
-    return super().__eq__(other)
-
-class CalibrationModel(StrEnum):
-  Perspective = 'perspective'
-  Fisheye = 'fisheye'
-
-class DistortionModel(StrEnum):
-  Normal = 'NORMAL'
-  Tilted = 'TILTED'
-  Prism = 'PRISM'
-  Thermal = 'THERMAL' # TODO : Is this even a distortion model
-
-class CharucoBoard:
-  def __init__(self, squaresX = 16, squaresY = 9, squareSize = 1.0, markerSize = 0.8, dictSize = cv2.aruco.DICT_4X4_1000):
-    """Charuco board configuration used in a captured dataset
-
-    Args:
-        squaresX (int, optional): Number of squares horizontally. Defaults to 16.
-        squaresY (int, optional): Number of squares vertically. Defaults to 9.
-        squareSize (float, optional): Length of the side of one square (cm). Defaults to 1.0.
-        markerSize (float, optional): Length of the side of one marker (cm). Defaults to 0.8.
-        dictSize (_type_, optional): cv2 aruco dictionary size. Defaults to cv2.aruco.DICT_4X4_1000.
-    """
-    self.squaresX = squaresX
-    self.squaresY = squaresY
-    self.squareSize = squareSize
-    self.markerSize = markerSize
-    self.dictSize = dictSize
-
-  def __getstate__(self): # Magic to allow pickling the instance without pickling the cv2 dictionary
-    state = self.__dict__.copy()
-    for hidden in ['_board', '_dictionary']:
-      if hidden in state:
-        del state[hidden]
-    return state
-
-  def __build(self):
-    self._dictionary = aruco.Dictionary_get(self.dictSize)
-    self._board = aruco.CharucoBoard_create(self.squaresX, self.squaresY, self.squareSize, self.markerSize, self._dictionary)
-
-  @property
-  def dictionary(self):
-    """cv2 aruco dictionary"""
-    if not hasattr(self, '_dictionary'):
-      self.__build()
-    return self._dictionary
-
-  @property
-  def board(self):
-    """cv2 aruco board"""
-    if not hasattr(self, '_board'):
-      self.__build()
-    return self._board
-
-class Dataset:
-  class Images:
-    def __init__(self, images: List[np.ndarray | str]):
-      self._images = images
-
-    def at(self, key):
-      if isinstance(self._images[key], str):
-        self._images[key] = cv2.imread(self._images[key], cv2.IMREAD_UNCHANGED)
-      return self._images[key]
-
-    def __len__(self):
-      return len(self._images)
-
-    def __iter__(self):
-      for i in len(self._images):
-        yield self.at(i)
-
-    def __getitem__(self, key):
-      return self.at(key)
-
-    def __repr__(self):
-      return self._images.__repr__()
-
-  def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str] = [], allCorners: List[np.ndarray] = [], allIds: List[np.ndarray] = [], imageSize: Tuple[float, float] = (), enableFiltering: bool = True):
-    """Create a dataset for camera calibration
-
-    Args:
-        name (str): Name of the camera for and the key for output data
-        board (CharucoBoard): Charuco board configuration used in the dataset
-        images (List[np.ndarray  |  str], optional): Set of images for calibration, can be left empty if `allCorners`, `allIds` and `imageSize` is provided. Defaults to [].
-        allCorners (List[np.ndarray], optional): Set of corners used for intrinsic calibration. Defaults to [].
-        allIds (List[np.ndarray], optional): Set of ids used for intrinsic calibration. Defaults to [].
-        imageSize (Tuple[float, float], optional): Size of the images captured during calibration, must be provided if `images` is empty. Defaults to ().
-        enableFiltering (bool, optional): Whether to filter provided corners, or use all of them as is. Defaults to True.
-    """
-    self.name = name
-    self.images = Dataset.Images(images)
-    self.allCorners = allCorners
-    self.allIds = allIds
-    self.imageSize = imageSize
-    self.board = board
-    self.enableFiltering = enableFiltering
-
-class CalibrationConfig:
-  def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
-               cameraModel = 0, stereoCalibCriteria = 0):
-    """Calibration configuration options
-
-    Args:
-        enableFiltering (bool, optional): Whether corners should be filtered for outliers. Defaults to True.
-        ccmModel (str, optional): _description_. Defaults to ''.
-        initialMaxThreshold (int, optional): _description_. Defaults to 0.
-        initialMinFiltered (int, optional): _description_. Defaults to 0.
-        calibrationMaxThreshold (int, optional): _description_. Defaults to 0.
-        calibrationMinFiltered (int, optional): _description_. Defaults to 0.
-        cameraModel (int, optional): _description_. Defaults to 0.
-        stereoCalibCriteria (int, optional): _description_. Defaults to 0.
-    """
-    self.enableFiltering = enableFiltering
-    self.ccmModel = ccmModel # Distortion model
-    self.initialMaxThreshold = initialMaxThreshold
-    self.initialMinFiltered = initialMinFiltered
-    self.calibrationMaxThreshold = calibrationMaxThreshold
-    self.calibrationMinFiltered = calibrationMinFiltered
-    self.cameraModel = cameraModel
-    self.stereoCalibCriteria = stereoCalibCriteria
-
-class CameraData(TypedDict):
-  calib_model: CalibrationModel
-  dist_coeff: str
-  distortion_model: DistortionModel
-  extrinsics: str
-  to_cam: str
-  filtered_corners: str
-  type: str
-  filtered_ids: str
-  height: str
-  width: str
-  hfov: str
-  socket: str
-  images_path: str
-  imsize: str
-  intrinsics: str
-  sensorName: str
-  hasAutofocus: str
-  model: str
-  max_threshold: str
-  min_inliers: str
-  name: str
-  reprojection_error: str
-  size: str
-  threshold_stepper: str
-  ids: str
-
 colors = [(0, 255 , 0), (0, 0, 255)]
 
 class StereoExceptions(Exception):
diff --git a/types.py b/types.py
new file mode 100644
index 0000000..200ff51
--- /dev/null
+++ b/types.py
@@ -0,0 +1,158 @@
+from typing import TypedDict, List, Tuple
+import cv2.aruco as aruco
+from enum import Enum
+import numpy as np
+import cv2
+
+class StrEnum(Enum): # Doesn't exist in python 3.10
+  def __eq__(self, other):
+    if isinstance(other, str):
+      print('compare string')
+      return self.value == other
+    return super().__eq__(other)
+
+class CalibrationModel(StrEnum):
+  Perspective = 'perspective'
+  Fisheye = 'fisheye'
+
+class DistortionModel(StrEnum):
+  Normal = 'NORMAL'
+  Tilted = 'TILTED'
+  Prism = 'PRISM'
+  Thermal = 'THERMAL' # TODO : Is this even a distortion model
+
+class CharucoBoard:
+  def __init__(self, squaresX = 16, squaresY = 9, squareSize = 1.0, markerSize = 0.8, dictSize = cv2.aruco.DICT_4X4_1000):
+    """Charuco board configuration used in a captured dataset
+
+    Args:
+        squaresX (int, optional): Number of squares horizontally. Defaults to 16.
+        squaresY (int, optional): Number of squares vertically. Defaults to 9.
+        squareSize (float, optional): Length of the side of one square (cm). Defaults to 1.0.
+        markerSize (float, optional): Length of the side of one marker (cm). Defaults to 0.8.
+        dictSize (_type_, optional): cv2 aruco dictionary size. Defaults to cv2.aruco.DICT_4X4_1000.
+    """
+    self.squaresX = squaresX
+    self.squaresY = squaresY
+    self.squareSize = squareSize
+    self.markerSize = markerSize
+    self.dictSize = dictSize
+
+  def __getstate__(self): # Magic to allow pickling the instance without pickling the cv2 dictionary
+    state = self.__dict__.copy()
+    for hidden in ['_board', '_dictionary']:
+      if hidden in state:
+        del state[hidden]
+    return state
+
+  def __build(self):
+    self._dictionary = aruco.Dictionary_get(self.dictSize)
+    self._board = aruco.CharucoBoard_create(self.squaresX, self.squaresY, self.squareSize, self.markerSize, self._dictionary)
+
+  @property
+  def dictionary(self):
+    """cv2 aruco dictionary"""
+    if not hasattr(self, '_dictionary'):
+      self.__build()
+    return self._dictionary
+
+  @property
+  def board(self):
+    """cv2 aruco board"""
+    if not hasattr(self, '_board'):
+      self.__build()
+    return self._board
+
+class Dataset:
+  class Images:
+    def __init__(self, images: List[np.ndarray | str]):
+      self._images = images
+
+    def at(self, key):
+      if isinstance(self._images[key], str):
+        self._images[key] = cv2.imread(self._images[key], cv2.IMREAD_UNCHANGED)
+      return self._images[key]
+
+    def __len__(self):
+      return len(self._images)
+
+    def __iter__(self):
+      for i in len(self._images):
+        yield self.at(i)
+
+    def __getitem__(self, key):
+      return self.at(key)
+
+    def __repr__(self):
+      return self._images.__repr__()
+
+  def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str] = [], allCorners: List[np.ndarray] = [], allIds: List[np.ndarray] = [], imageSize: Tuple[float, float] = (), enableFiltering: bool = True):
+    """Create a dataset for camera calibration
+
+    Args:
+        name (str): Name of the camera for and the key for output data
+        board (CharucoBoard): Charuco board configuration used in the dataset
+        images (List[np.ndarray  |  str], optional): Set of images for calibration, can be left empty if `allCorners`, `allIds` and `imageSize` is provided. Defaults to [].
+        allCorners (List[np.ndarray], optional): Set of corners used for intrinsic calibration. Defaults to [].
+        allIds (List[np.ndarray], optional): Set of ids used for intrinsic calibration. Defaults to [].
+        imageSize (Tuple[float, float], optional): Size of the images captured during calibration, must be provided if `images` is empty. Defaults to ().
+        enableFiltering (bool, optional): Whether to filter provided corners, or use all of them as is. Defaults to True.
+    """
+    self.name = name
+    self.images = Dataset.Images(images)
+    self.allCorners = allCorners
+    self.allIds = allIds
+    self.imageSize = imageSize
+    self.board = board
+    self.enableFiltering = enableFiltering
+
+class CalibrationConfig:
+  def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
+               cameraModel = 0, stereoCalibCriteria = 0):
+    """Calibration configuration options
+
+    Args:
+        enableFiltering (bool, optional): Whether corners should be filtered for outliers. Defaults to True.
+        ccmModel (str, optional): _description_. Defaults to ''.
+        initialMaxThreshold (int, optional): _description_. Defaults to 0.
+        initialMinFiltered (int, optional): _description_. Defaults to 0.
+        calibrationMaxThreshold (int, optional): _description_. Defaults to 0.
+        calibrationMinFiltered (int, optional): _description_. Defaults to 0.
+        cameraModel (int, optional): _description_. Defaults to 0.
+        stereoCalibCriteria (int, optional): _description_. Defaults to 0.
+    """
+    self.enableFiltering = enableFiltering
+    self.ccmModel = ccmModel # Distortion model
+    self.initialMaxThreshold = initialMaxThreshold
+    self.initialMinFiltered = initialMinFiltered
+    self.calibrationMaxThreshold = calibrationMaxThreshold
+    self.calibrationMinFiltered = calibrationMinFiltered
+    self.cameraModel = cameraModel
+    self.stereoCalibCriteria = stereoCalibCriteria
+
+class CameraData(TypedDict):
+  calib_model: CalibrationModel
+  dist_coeff: str
+  distortion_model: DistortionModel
+  extrinsics: str
+  to_cam: str
+  filtered_corners: str
+  type: str
+  filtered_ids: str
+  height: str
+  width: str
+  hfov: str
+  socket: str
+  images_path: str
+  imsize: str
+  intrinsics: str
+  sensorName: str
+  hasAutofocus: str
+  model: str
+  max_threshold: str
+  min_inliers: str
+  name: str
+  reprojection_error: str
+  size: str
+  threshold_stepper: str
+  ids: str
\ No newline at end of file

From 890d0375b3f0e1827e2cb1a4427ca8528685155a Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 11 Nov 2024 15:28:20 +0100
Subject: [PATCH 67/87] Fix kwargs bug, remove unused functions, add debugging
 file

---
 calibration_debug.py | 285 +++++++++++++++++++++++++++++++++++++++++++
 calibration_utils.py | 112 ++++-------------
 2 files changed, 312 insertions(+), 85 deletions(-)
 create mode 100644 calibration_debug.py

diff --git a/calibration_debug.py b/calibration_debug.py
new file mode 100644
index 0000000..6601e7b
--- /dev/null
+++ b/calibration_debug.py
@@ -0,0 +1,285 @@
+import logging
+logging.getLogger('matplotlib').setLevel(logging.WARNING)
+
+import matplotlib.pyplot as plt
+from pathlib import Path
+import numpy as np
+from collections import deque
+import glob
+import cv2
+
+# Helper functions
+
+def scale_intrinsics(intrinsics, originalShape, destShape):
+    scale = destShape[1] / originalShape[1] # scale on width
+    scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]])
+    scaled_intrinsics = np.matmul(scale_mat, intrinsics)
+    """ print("Scaled height offset : {}".format(
+        (originalShape[0] * scale - destShape[0]) / 2))
+    print("Scaled width offset : {}".format(
+        (originalShape[1] * scale - destShape[1]) / 2)) """
+    scaled_intrinsics[1][2] -= (originalShape[0]      # c_y - along height of the image
+                                * scale - destShape[0]) / 2
+    scaled_intrinsics[0][2] -= (originalShape[1]     # c_x width of the image
+                                * scale - destShape[1]) / 2
+    #if self.traceLevel == 3 or self.traceLevel == 10:
+    print('original_intrinsics')
+    print(intrinsics)
+    print('scaled_intrinsics')
+    print(scaled_intrinsics)
+
+    return scaled_intrinsics
+
+def scale_image(img, scaled_res):
+    expected_height = img.shape[0]*(scaled_res[1]/img.shape[1])
+    #if self.traceLevel == 2 or self.traceLevel == 10:
+    print("Expected Height: {}".format(expected_height))
+
+    if not (img.shape[0] == scaled_res[0] and img.shape[1] == scaled_res[1]):
+        if int(expected_height) == scaled_res[0]:
+            # resizing to have both stereo and rgb to have same
+            # resolution to capture extrinsics of the rgb-right camera
+            img = cv2.resize(img, (scaled_res[1], scaled_res[0]),
+                              interpolation=cv2.INTER_CUBIC)
+            return img
+        else:
+            # resizing and cropping to have both stereo and rgb to have same resolution
+            # to calculate extrinsics of the rgb-right camera
+            scale_width = scaled_res[1]/img.shape[1]
+            dest_res = (
+                int(img.shape[1] * scale_width), int(img.shape[0] * scale_width))
+            img = cv2.resize(
+                img, dest_res, interpolation=cv2.INTER_CUBIC)
+            if img.shape[0] < scaled_res[0]:
+                raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
+                    img.shape[0], scaled_res[0]))
+            # print(gray.shape[0] - req_resolution[0])
+            del_height = (img.shape[0] - scaled_res[0]) // 2
+            # gray = gray[: req_resolution[0], :]
+            img = img[del_height: del_height + scaled_res[0], :]
+            return img
+    else:
+        return img
+
+# Debugging functions
+
+def debug_epipolar_charuco(left_cam_info: CameraData, right_cam_info: CameraData, left_board: CharucoBoard, right_board: CharucoBoard, t, r_l, r_r):
+    images_left = glob.glob(left_cam_info['images_path'] + '/*.png')
+    images_right = glob.glob(right_cam_info['images_path'] + '/*.png')
+    images_left.sort()
+    print(images_left)
+    images_right.sort()
+    assert len(images_left) != 0, "ERROR: Images not read correctly"
+    assert len(images_right) != 0, "ERROR: Images not read correctly"
+    isHorizontal = np.absolute(t[0]) > np.absolute(t[1])
+
+    scale = None
+    scale_req = False
+    frame_left_shape = cv2.imread(images_left[0], 0).shape
+    frame_right_shape = cv2.imread(images_right[0], 0).shape
+    scalable_res = frame_left_shape
+    scaled_res = frame_right_shape
+    if frame_right_shape[0] < frame_left_shape[0] and frame_right_shape[1] < frame_left_shape[1]:
+        scale_req = True
+        scale = frame_right_shape[1] / frame_left_shape[1]
+    elif frame_right_shape[0] > frame_left_shape[0] and frame_right_shape[1] > frame_left_shape[1]:
+        scale_req = True
+        scale = frame_left_shape[1] / frame_right_shape[1]
+        scalable_res = frame_right_shape
+        scaled_res = frame_left_shape
+
+    if scale_req:
+        scaled_height = scale * scalable_res[0]
+        diff = scaled_height - scaled_res[0]
+        if diff < 0:
+            scaled_res = (int(scaled_height), scaled_res[1])
+    #if self.traceLevel == 3 or self.traceLevel == 10:
+    print(
+        f'Is scale Req: {scale_req}\n scale value: {scale} \n scalable Res: {scalable_res} \n scale Res: {scaled_res}')
+    print("Original res Left :{}".format(frame_left_shape))
+    print("Original res Right :{}".format(frame_right_shape))
+    # print("Scale res :{}".format(scaled_res))
+
+    M_l = left_cam_info['intrinsics']
+    M_r = right_cam_info['intrinsics']
+    M_lp = scale_intrinsics(M_l, frame_left_shape, scaled_res)
+    M_rp = scale_intrinsics(M_r, frame_right_shape, scaled_res)
+
+    criteria = (cv2.TERM_CRITERIA_EPS +
+                cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
+
+    # TODO(Sachin): Observe Images by adding visualization 
+    # TODO(Sachin): Check if the stetch is only in calibration Images
+    print('Original intrinsics ....')
+    print(f"L {M_lp}")
+    print(f"R: {M_rp}")
+    #if self.traceLevel == 3 or self.traceLevel == 10:
+    print(f'Width and height is {scaled_res[::-1]}')
+    # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
+    # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0)
+    # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
+    kScaledR = kScaledL = M_rp
+
+    # if self.cameraModel != 'perspective':
+    #     kScaledR = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(M_r, d_r, scaled_res[::-1], np.eye(3), fov_scale=1.1)
+    #     kScaledL = kScaledR
+
+        
+    print('Intrinsics from the getOptimalNewCameraMatrix/Original ....')
+    print(f"L: {kScaledL}")
+    print(f"R: {kScaledR}")
+    oldEpipolarError = None
+    epQueue = deque()
+    movePos = True
+
+    left_calib_model = left_cam_info['calib_model']
+    right_calib_model = right_cam_info['calib_model']
+    d_l = left_cam_info['dist_coeff']
+    d_r = left_cam_info['dist_coeff']
+    print(left_calib_model, right_calib_model)
+    if left_calib_model == 'perspective':
+        mapx_l, mapy_l = cv2.initUndistortRectifyMap(
+            M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
+    else:
+        mapx_l, mapy_l = cv2.fisheye.initUndistortRectifyMap(
+            M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
+    if right_calib_model == "perspective":
+        mapx_r, mapy_r = cv2.initUndistortRectifyMap(
+            M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
+    else:
+        mapx_r, mapy_r = cv2.fisheye.initUndistortRectifyMap(
+            M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
+
+    image_data_pairs = []
+    for image_left, image_right in zip(images_left, images_right):
+        # read images
+        img_l = cv2.imread(image_left, 0)
+        img_r = cv2.imread(image_right, 0)
+
+        img_l = scale_image(img_l, scaled_res)
+        img_r = scale_image(img_r, scaled_res)
+        # print(img_l.shape)
+        # print(img_r.shape)
+
+        # warp right image
+        # img_l = cv2.warpPerspective(img_l, self.H1, img_l.shape[::-1],
+        #                             cv2.INTER_CUBIC +
+        #                             cv2.WARP_FILL_OUTLIERS +
+        #                             cv2.WARP_INVERSE_MAP)
+
+        # img_r = cv2.warpPerspective(img_r, self.H2, img_r.shape[::-1],
+        #                             cv2.INTER_CUBIC +
+        #                             cv2.WARP_FILL_OUTLIERS +
+        #                             cv2.WARP_INVERSE_MAP)
+
+        img_l = cv2.remap(img_l, mapx_l, mapy_l, cv2.INTER_LINEAR)
+        img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR)
+
+        image_data_pairs.append((img_l, img_r))
+        
+        #if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
+            #cv2.imshow("undistorted-Left", img_l)
+            #cv2.imshow("undistorted-right", img_r)
+            # print(f'image path - {im}')
+            # print(f'Image Undistorted Size {undistorted_img.shape}')
+        #    k = cv2.waitKey(0)
+        #    if k == 27:  # Esc key to stop
+        #        break
+    #if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
+    #  cv2.destroyWindow("undistorted-Left")
+    #  cv2.destroyWindow("undistorted-right")  
+    # compute metrics
+    imgpoints_r = []
+    imgpoints_l = []
+    image_epipolar_color = []
+    # new_imagePairs = [])
+    for i, image_data_pair in enumerate(image_data_pairs):
+        res2_l = detect_charuco_board(left_board, image_data_pair[0])
+        res2_r = detect_charuco_board(right_board, image_data_pair[1])
+        
+        # if self.traceLevel == 2 or self.traceLevel == 4 or self.traceLevel == 10:
+        
+
+        img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]])
+        img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB)
+        line_row = 0
+        while line_row < img_concat.shape[0]:
+            cv2.line(img_concat,
+                     (0, line_row), (img_concat.shape[1], line_row),
+                     (0, 255, 0), 1)
+            line_row += 30
+
+        cv2.imshow('Stereo Pair', img_concat)
+        k = cv2.waitKey(1)
+        
+        if res2_l[1] is not None and res2_r[2] is not None and len(res2_l[1]) > 3 and len(res2_r[1]) > 3:
+
+            cv2.cornerSubPix(image_data_pair[0], res2_l[1],
+                             winSize=(5, 5),
+                             zeroZone=(-1, -1),
+                             criteria=criteria)
+            cv2.cornerSubPix(image_data_pair[1], res2_r[1],
+                             winSize=(5, 5),
+                             zeroZone=(-1, -1),
+                             criteria=criteria)
+
+            # termination criteria
+            img_pth_right = Path(images_right[i])
+            img_pth_left = Path(images_left[i])
+            org = (100, 50)
+            # cv2.imshow('ltext', lText)
+            # cv2.waitKey(0)
+            localError = 0
+            corners_l = []
+            corners_r = []
+            for j in range(len(res2_l[2])):
+                idx = np.where(res2_r[2] == res2_l[2][j])
+                if idx[0].size == 0:
+                    continue
+                corners_l.append(res2_l[1][j])
+                corners_r.append(res2_r[1][idx])
+
+            imgpoints_l.append(corners_l)
+            imgpoints_r.append(corners_r)
+            epi_error_sum = 0
+            corner_epipolar_color = []
+            for l_pt, r_pt in zip(corners_l, corners_r):
+                if isHorizontal:
+                    curr_epipolar_error = abs(l_pt[0][1] - r_pt[0][1])
+                else:
+                    curr_epipolar_error = abs(l_pt[0][0] - r_pt[0][0])
+                if curr_epipolar_error >= 1:
+                    corner_epipolar_color.append(1)
+                else:
+                    corner_epipolar_color.append(0)
+                epi_error_sum += curr_epipolar_error
+            localError = epi_error_sum / len(corners_l)
+            image_epipolar_color.append(corner_epipolar_color)
+            #if self.traceLevel == 2 or self.traceLevel == 3 or self.traceLevel == 4 or self.traceLevel == 10:
+            print("Epipolar Error per image on host in " + img_pth_right.name + " : " +
+                    str(localError))
+        else:
+            print('Numer of corners is in left -> and right ->')
+            return -1
+        lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
+        rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
+        image_data_pairs[i] = (lText, rText)
+
+
+    epi_error_sum = 0
+    total_corners = 0
+    for corners_l, corners_r in zip(imgpoints_l, imgpoints_r):
+        total_corners += len(corners_l)
+        for l_pt, r_pt in zip(corners_l, corners_r):
+            if isHorizontal:
+                epi_error_sum += abs(l_pt[0][1] - r_pt[0][1])
+            else:
+                epi_error_sum += abs(l_pt[0][0] - r_pt[0][0])
+
+    avg_epipolar = epi_error_sum / total_corners
+    print("Average Epipolar Error is : " + str(avg_epipolar))
+
+    #if self.enable_rectification_disp:
+    #    self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal)
+
+    return avg_epipolar
diff --git a/calibration_utils.py b/calibration_utils.py
index 876bd51..3890b15 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -15,7 +15,7 @@ class StereoExceptions(Exception):
   def __init__(self, message, stage, path=None, *args, **kwargs) -> None:
     self.stage = stage
     self.path = path
-    super().__init__(message, *args, **kwargs)
+    super().__init__(message, *args)
 
   @property
   def summary(self) -> str:
@@ -387,7 +387,7 @@ def remove_and_filter_stereo_features(leftCamData: CameraData, rightCamData: Cam
 
   return leftCamData, rightCamData
 
-def calculate_epipolar_error(left_cam_info, right_cam_info, left_cam, right_cam, board_config, extrinsics):
+def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraData, left_cam, right_cam, board_config, extrinsics):
   if extrinsics[0] == -1:
     return -1, extrinsics[1]
   stereoConfig = None
@@ -593,24 +593,16 @@ def features_filtering_function(rvecs, tvecs, cameraMatrix, distCoeffs, filtered
 
   return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
 
-def detect_charuco_board(config: CalibrationConfig, image: np.array):
-  arucoParams = cv2.aruco.DetectorParameters_create()
-  arucoParams.minMarkerDistanceRate = 0.01
-  corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, config.dictionary, parameters=arucoParams)  # First, detect markers
-  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, config.board, corners, ids, rejectedCorners=rejectedImgPoints)
-  # If found, add object points, image points (after refining them)
-  if len(marker_corners) > 0:
-    ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, config.board, minMarkers = 1)
-    return ret, corners, ids, marker_corners, marker_ids
-  else:
-    return None, None, None, None, None
-
 def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
   objects = []
   while len(objects) < len(objpoints[:,0,0]) * min_inliers:
     if ini_threshold > max_threshold:
       break
     ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+
+    if not ret:
+      break
+
     imgpoints2 = objpoints.copy()
 
     all_corners = corners.copy()
@@ -634,71 +626,6 @@ def compute_reprojection_errors(obj_pts: np.array, img_pts: np.array, K: np.arra
   errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
   return errs
 
-def analyze_charuco(config: CalibrationConfig, images, scale_req=False, req_resolution=(800, 1280)):
-  """
-  Charuco base pose estimation.
-  """
-  # print("POSE ESTIMATION STARTS:")
-  allCorners = []
-  allIds = []
-  all_marker_corners = []
-  all_marker_ids = []
-  all_recovered = []
-  # decimator = 0
-  # SUB PIXEL CORNER DETECTION CRITERION
-  criteria = (cv2.TERM_CRITERIA_EPS +
-        cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
-  count = 0
-  skip_vis = False
-  for im in images:
-    img_pth = Path(im)
-    frame = cv2.imread(im)
-    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-    expected_height = gray.shape[0]*(req_resolution[1]/gray.shape[1])
-
-    if scale_req and not (gray.shape[0] == req_resolution[0] and gray.shape[1] == req_resolution[1]):
-      if int(expected_height) == req_resolution[0]:
-        # resizing to have both stereo and rgb to have same
-        # resolution to capture extrinsics of the rgb-right camera
-        gray = cv2.resize(gray, req_resolution[::-1],
-                  interpolation=cv2.INTER_CUBIC)
-      else:
-        # resizing and cropping to have both stereo and rgb to have same resolution
-        # to calculate extrinsics of the rgb-right camera
-        scale_width = req_resolution[1]/gray.shape[1]
-        dest_res = (
-          int(gray.shape[1] * scale_width), int(gray.shape[0] * scale_width))
-        gray = cv2.resize(
-          gray, dest_res, interpolation=cv2.INTER_CUBIC)
-        if gray.shape[0] < req_resolution[0]:
-          raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
-            gray.shape[0], req_resolution[0]))
-        # print(gray.shape[0] - req_resolution[0])
-        del_height = (gray.shape[0] - req_resolution[0]) // 2
-        # gray = gray[: req_resolution[0], :]
-        gray = gray[del_height: del_height + req_resolution[0], :]
-
-      count += 1
-
-    ret, charuco_corners, charuco_ids, marker_corners, marker_ids  = detect_charuco_board(config, gray)
-
-    if charuco_corners is not None and charuco_ids is not None and len(charuco_corners) > 3:
-
-      charuco_corners = cv2.cornerSubPix(gray, charuco_corners,
-                winSize=(5, 5),
-                zeroZone=(-1, -1),
-                criteria=criteria)
-      allCorners.append(charuco_corners)  # Charco chess corners
-      allIds.append(charuco_ids)  # charuco chess corner id's
-      all_marker_corners.append(marker_corners)
-      all_marker_ids.append(marker_ids)
-    else:
-      print(im)
-      return f'Failed to detect more than 3 markers on image {im}', None, None, None, None, None
-
-  # imsize = gray.shape[::-1]
-  return allCorners, allIds, all_marker_corners, all_marker_ids, gray.shape[::-1], all_recovered
-
 def undistort_visualization(self, img_list, K, D, img_size, name):
   for index, im in enumerate(img_list):
     # print(im)
@@ -922,7 +849,7 @@ def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, mode
 
     """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
-  def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Dataset] = [], extrinsics: List[Tuple[Dataset, Dataset]] = []):
+  def calibrate(self, board_config, filepath, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = []):
     """Function to calculate calibration for stereo camera."""
     # init object data
     self._cameraModel = camera_model
@@ -932,14 +859,14 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
 
     self.cams = []
 
-    
     config = CalibrationConfig(
       self.filtering_enable, self.ccm_model, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
       camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
     )
     
-    for a, b in extrinsics:
+    for a, b in extrinsicPairs:
       if len(a.allIds) != len(b.allIds):
+        print('Not all dataset for extrinsic calibration have the same number of images')
         raise RuntimeError('Not all dataset for extrinsic calibration have the same number of images') # TODO : This isn't thorough enough
 
     pw = ParallelWorker(1)
@@ -947,7 +874,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
     stereoConfigs = []
 
     # Calibrate camera intrinsics for all provided datasets
-    for dataset in intrinsics:
+    for dataset in intrinsicCameras:
       camData = [c for c in board_config['cameras'].values() if c['name'] == dataset.name][0]
       
       if "calib_model" in camData:
@@ -982,7 +909,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
       else:
         camData = calibrate_ccm_intrinsics(config, camData)
 
-    for left, right in extrinsics:
+    for left, right in extrinsicPairs:
       left_cam_info = camInfos[left.name]
       right_cam_info = camInfos[right.name]
 
@@ -1011,6 +938,21 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
       stereoConfigs.append(stereo_config)
 
     pw.execute()
+    
+    # def get_board_for_cam(cam_info: CameraData):
+    #   for pair in extrinsicPairs:
+    #     for cam in pair:
+    #       if cam.name == cam_info['name']:
+    #         return cam.board
+    
+    # test_epipolar_charuco(
+    #   {**left_cam_info.ret(), 'images_path': '/home/developer/debug_session/14442C10111E19D000/2024_11_08_18-06-05/thermal/capture/color'}, 
+    #   {**right_cam_info.ret(), 'images_path': '/home/developer/debug_session/14442C10111E19D000/2024_11_08_18-06-05/thermal/capture/thermal'},
+    #   get_board_for_cam(left_cam_info.ret()),
+    #   get_board_for_cam(right_cam_info.ret()),
+    #   extrinsics.ret()[2], # Translation between left and right Cameras
+    #   extrinsics.ret()[3], # Left Rectification rotation 
+    #   extrinsics.ret()[4]) # Right Rectification rotation
 
     # Extract camera info structs and stereo config
     for cam, camInfo in camInfos.items():
@@ -1022,4 +964,4 @@ def calibrate(self, board_config, filepath, camera_model, intrinsics: List[Datas
       if stereoConfig.ret():
         board_config['stereo_config'].update(stereoConfig.ret())
 
-    return  board_config
\ No newline at end of file
+    return board_config
\ No newline at end of file

From 6c9409cf1bd703ba3aaa6af71b9404858952f1b4 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 11 Nov 2024 15:28:35 +0100
Subject: [PATCH 68/87] Add debugging file

---
 calibration_debug.py | 541 ++++++++++++++++++++++---------------------
 1 file changed, 277 insertions(+), 264 deletions(-)

diff --git a/calibration_debug.py b/calibration_debug.py
index 6601e7b..870a882 100644
--- a/calibration_debug.py
+++ b/calibration_debug.py
@@ -2,284 +2,297 @@
 logging.getLogger('matplotlib').setLevel(logging.WARNING)
 
 import matplotlib.pyplot as plt
+from collections import deque
 from pathlib import Path
+from .types import *
 import numpy as np
-from collections import deque
 import glob
 import cv2
 
 # Helper functions
 
 def scale_intrinsics(intrinsics, originalShape, destShape):
-    scale = destShape[1] / originalShape[1] # scale on width
-    scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]])
-    scaled_intrinsics = np.matmul(scale_mat, intrinsics)
-    """ print("Scaled height offset : {}".format(
-        (originalShape[0] * scale - destShape[0]) / 2))
-    print("Scaled width offset : {}".format(
-        (originalShape[1] * scale - destShape[1]) / 2)) """
-    scaled_intrinsics[1][2] -= (originalShape[0]      # c_y - along height of the image
-                                * scale - destShape[0]) / 2
-    scaled_intrinsics[0][2] -= (originalShape[1]     # c_x width of the image
-                                * scale - destShape[1]) / 2
-    #if self.traceLevel == 3 or self.traceLevel == 10:
-    print('original_intrinsics')
-    print(intrinsics)
-    print('scaled_intrinsics')
-    print(scaled_intrinsics)
-
-    return scaled_intrinsics
+  scale = destShape[1] / originalShape[1] # scale on width
+  scale_mat = np.array([[scale, 0, 0], [0, scale, 0], [0, 0, 1]])
+  scaled_intrinsics = np.matmul(scale_mat, intrinsics)
+  """ print("Scaled height offset : {}".format(
+    (originalShape[0] * scale - destShape[0]) / 2))
+  print("Scaled width offset : {}".format(
+    (originalShape[1] * scale - destShape[1]) / 2)) """
+  scaled_intrinsics[1][2] -= (originalShape[0]    # c_y - along height of the image
+                * scale - destShape[0]) / 2
+  scaled_intrinsics[0][2] -= (originalShape[1]   # c_x width of the image
+                * scale - destShape[1]) / 2
+  #if self.traceLevel == 3 or self.traceLevel == 10:
+  print('original_intrinsics')
+  print(intrinsics)
+  print('scaled_intrinsics')
+  print(scaled_intrinsics)
+
+  return scaled_intrinsics
 
 def scale_image(img, scaled_res):
-    expected_height = img.shape[0]*(scaled_res[1]/img.shape[1])
-    #if self.traceLevel == 2 or self.traceLevel == 10:
-    print("Expected Height: {}".format(expected_height))
-
-    if not (img.shape[0] == scaled_res[0] and img.shape[1] == scaled_res[1]):
-        if int(expected_height) == scaled_res[0]:
-            # resizing to have both stereo and rgb to have same
-            # resolution to capture extrinsics of the rgb-right camera
-            img = cv2.resize(img, (scaled_res[1], scaled_res[0]),
-                              interpolation=cv2.INTER_CUBIC)
-            return img
-        else:
-            # resizing and cropping to have both stereo and rgb to have same resolution
-            # to calculate extrinsics of the rgb-right camera
-            scale_width = scaled_res[1]/img.shape[1]
-            dest_res = (
-                int(img.shape[1] * scale_width), int(img.shape[0] * scale_width))
-            img = cv2.resize(
-                img, dest_res, interpolation=cv2.INTER_CUBIC)
-            if img.shape[0] < scaled_res[0]:
-                raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
-                    img.shape[0], scaled_res[0]))
-            # print(gray.shape[0] - req_resolution[0])
-            del_height = (img.shape[0] - scaled_res[0]) // 2
-            # gray = gray[: req_resolution[0], :]
-            img = img[del_height: del_height + scaled_res[0], :]
-            return img
+  expected_height = img.shape[0]*(scaled_res[1]/img.shape[1])
+  #if self.traceLevel == 2 or self.traceLevel == 10:
+  print("Expected Height: {}".format(expected_height))
+
+  if not (img.shape[0] == scaled_res[0] and img.shape[1] == scaled_res[1]):
+    if int(expected_height) == scaled_res[0]:
+      # resizing to have both stereo and rgb to have same
+      # resolution to capture extrinsics of the rgb-right camera
+      img = cv2.resize(img, (scaled_res[1], scaled_res[0]),
+                interpolation=cv2.INTER_CUBIC)
+      return img
     else:
-        return img
+      # resizing and cropping to have both stereo and rgb to have same resolution
+      # to calculate extrinsics of the rgb-right camera
+      scale_width = scaled_res[1]/img.shape[1]
+      dest_res = (
+        int(img.shape[1] * scale_width), int(img.shape[0] * scale_width))
+      img = cv2.resize(
+        img, dest_res, interpolation=cv2.INTER_CUBIC)
+      if img.shape[0] < scaled_res[0]:
+        raise RuntimeError("resizeed height of rgb is smaller than required. {0} < {1}".format(
+          img.shape[0], scaled_res[0]))
+      # print(gray.shape[0] - req_resolution[0])
+      del_height = (img.shape[0] - scaled_res[0]) // 2
+      # gray = gray[: req_resolution[0], :]
+      img = img[del_height: del_height + scaled_res[0], :]
+      return img
+  else:
+    return img
+
+def detect_charuco_board(config: CharucoBoard, image: np.array):
+  arucoParams = cv2.aruco.DetectorParameters_create()
+  arucoParams.minMarkerDistanceRate = 0.01
+  corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, config.dictionary, parameters=arucoParams)  # First, detect markers
+  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, config.board, corners, ids, rejectedCorners=rejectedImgPoints)
+  # If found, add object points, image points (after refining them)
+  if len(marker_corners) > 0:
+    ret, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids,image, config.board, minMarkers = 1)
+    return ret, corners, ids, marker_corners, marker_ids
+  else:
+    return None, None, None, None, None
 
 # Debugging functions
 
 def debug_epipolar_charuco(left_cam_info: CameraData, right_cam_info: CameraData, left_board: CharucoBoard, right_board: CharucoBoard, t, r_l, r_r):
-    images_left = glob.glob(left_cam_info['images_path'] + '/*.png')
-    images_right = glob.glob(right_cam_info['images_path'] + '/*.png')
-    images_left.sort()
-    print(images_left)
-    images_right.sort()
-    assert len(images_left) != 0, "ERROR: Images not read correctly"
-    assert len(images_right) != 0, "ERROR: Images not read correctly"
-    isHorizontal = np.absolute(t[0]) > np.absolute(t[1])
-
-    scale = None
-    scale_req = False
-    frame_left_shape = cv2.imread(images_left[0], 0).shape
-    frame_right_shape = cv2.imread(images_right[0], 0).shape
-    scalable_res = frame_left_shape
-    scaled_res = frame_right_shape
-    if frame_right_shape[0] < frame_left_shape[0] and frame_right_shape[1] < frame_left_shape[1]:
-        scale_req = True
-        scale = frame_right_shape[1] / frame_left_shape[1]
-    elif frame_right_shape[0] > frame_left_shape[0] and frame_right_shape[1] > frame_left_shape[1]:
-        scale_req = True
-        scale = frame_left_shape[1] / frame_right_shape[1]
-        scalable_res = frame_right_shape
-        scaled_res = frame_left_shape
-
-    if scale_req:
-        scaled_height = scale * scalable_res[0]
-        diff = scaled_height - scaled_res[0]
-        if diff < 0:
-            scaled_res = (int(scaled_height), scaled_res[1])
-    #if self.traceLevel == 3 or self.traceLevel == 10:
-    print(
-        f'Is scale Req: {scale_req}\n scale value: {scale} \n scalable Res: {scalable_res} \n scale Res: {scaled_res}')
-    print("Original res Left :{}".format(frame_left_shape))
-    print("Original res Right :{}".format(frame_right_shape))
-    # print("Scale res :{}".format(scaled_res))
-
-    M_l = left_cam_info['intrinsics']
-    M_r = right_cam_info['intrinsics']
-    M_lp = scale_intrinsics(M_l, frame_left_shape, scaled_res)
-    M_rp = scale_intrinsics(M_r, frame_right_shape, scaled_res)
-
-    criteria = (cv2.TERM_CRITERIA_EPS +
-                cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
-
-    # TODO(Sachin): Observe Images by adding visualization 
-    # TODO(Sachin): Check if the stetch is only in calibration Images
-    print('Original intrinsics ....')
-    print(f"L {M_lp}")
-    print(f"R: {M_rp}")
-    #if self.traceLevel == 3 or self.traceLevel == 10:
-    print(f'Width and height is {scaled_res[::-1]}')
-    # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
-    # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0)
-    # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
-    kScaledR = kScaledL = M_rp
-
-    # if self.cameraModel != 'perspective':
-    #     kScaledR = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(M_r, d_r, scaled_res[::-1], np.eye(3), fov_scale=1.1)
-    #     kScaledL = kScaledR
-
-        
-    print('Intrinsics from the getOptimalNewCameraMatrix/Original ....')
-    print(f"L: {kScaledL}")
-    print(f"R: {kScaledR}")
-    oldEpipolarError = None
-    epQueue = deque()
-    movePos = True
-
-    left_calib_model = left_cam_info['calib_model']
-    right_calib_model = right_cam_info['calib_model']
-    d_l = left_cam_info['dist_coeff']
-    d_r = left_cam_info['dist_coeff']
-    print(left_calib_model, right_calib_model)
-    if left_calib_model == 'perspective':
-        mapx_l, mapy_l = cv2.initUndistortRectifyMap(
-            M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
-    else:
-        mapx_l, mapy_l = cv2.fisheye.initUndistortRectifyMap(
-            M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
-    if right_calib_model == "perspective":
-        mapx_r, mapy_r = cv2.initUndistortRectifyMap(
-            M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
-    else:
-        mapx_r, mapy_r = cv2.fisheye.initUndistortRectifyMap(
-            M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
-
-    image_data_pairs = []
-    for image_left, image_right in zip(images_left, images_right):
-        # read images
-        img_l = cv2.imread(image_left, 0)
-        img_r = cv2.imread(image_right, 0)
-
-        img_l = scale_image(img_l, scaled_res)
-        img_r = scale_image(img_r, scaled_res)
-        # print(img_l.shape)
-        # print(img_r.shape)
-
-        # warp right image
-        # img_l = cv2.warpPerspective(img_l, self.H1, img_l.shape[::-1],
-        #                             cv2.INTER_CUBIC +
-        #                             cv2.WARP_FILL_OUTLIERS +
-        #                             cv2.WARP_INVERSE_MAP)
-
-        # img_r = cv2.warpPerspective(img_r, self.H2, img_r.shape[::-1],
-        #                             cv2.INTER_CUBIC +
-        #                             cv2.WARP_FILL_OUTLIERS +
-        #                             cv2.WARP_INVERSE_MAP)
-
-        img_l = cv2.remap(img_l, mapx_l, mapy_l, cv2.INTER_LINEAR)
-        img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR)
-
-        image_data_pairs.append((img_l, img_r))
-        
-        #if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
-            #cv2.imshow("undistorted-Left", img_l)
-            #cv2.imshow("undistorted-right", img_r)
-            # print(f'image path - {im}')
-            # print(f'Image Undistorted Size {undistorted_img.shape}')
-        #    k = cv2.waitKey(0)
-        #    if k == 27:  # Esc key to stop
-        #        break
+  images_left = glob.glob(left_cam_info['images_path'] + '/*.png')
+  images_right = glob.glob(right_cam_info['images_path'] + '/*.png')
+  images_left.sort()
+  print(images_left)
+  images_right.sort()
+  assert len(images_left) != 0, "ERROR: Images not read correctly"
+  assert len(images_right) != 0, "ERROR: Images not read correctly"
+  isHorizontal = np.absolute(t[0]) > np.absolute(t[1])
+
+  scale = None
+  scale_req = False
+  frame_left_shape = cv2.imread(images_left[0], 0).shape
+  frame_right_shape = cv2.imread(images_right[0], 0).shape
+  scalable_res = frame_left_shape
+  scaled_res = frame_right_shape
+  if frame_right_shape[0] < frame_left_shape[0] and frame_right_shape[1] < frame_left_shape[1]:
+    scale_req = True
+    scale = frame_right_shape[1] / frame_left_shape[1]
+  elif frame_right_shape[0] > frame_left_shape[0] and frame_right_shape[1] > frame_left_shape[1]:
+    scale_req = True
+    scale = frame_left_shape[1] / frame_right_shape[1]
+    scalable_res = frame_right_shape
+    scaled_res = frame_left_shape
+
+  if scale_req:
+    scaled_height = scale * scalable_res[0]
+    diff = scaled_height - scaled_res[0]
+    if diff < 0:
+      scaled_res = (int(scaled_height), scaled_res[1])
+  #if self.traceLevel == 3 or self.traceLevel == 10:
+  print(
+    f'Is scale Req: {scale_req}\n scale value: {scale} \n scalable Res: {scalable_res} \n scale Res: {scaled_res}')
+  print("Original res Left :{}".format(frame_left_shape))
+  print("Original res Right :{}".format(frame_right_shape))
+  # print("Scale res :{}".format(scaled_res))
+
+  M_l = left_cam_info['intrinsics']
+  M_r = right_cam_info['intrinsics']
+  M_lp = scale_intrinsics(M_l, frame_left_shape, scaled_res)
+  M_rp = scale_intrinsics(M_r, frame_right_shape, scaled_res)
+
+  criteria = (cv2.TERM_CRITERIA_EPS +
+        cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
+
+  # TODO(Sachin): Observe Images by adding visualization 
+  # TODO(Sachin): Check if the stetch is only in calibration Images
+  print('Original intrinsics ....')
+  print(f"L {M_lp}")
+  print(f"R: {M_rp}")
+  #if self.traceLevel == 3 or self.traceLevel == 10:
+  print(f'Width and height is {scaled_res[::-1]}')
+  # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
+  # kScaledL, _ = cv2.getOptimalNewCameraMatrix(M_r, d_l, scaled_res[::-1], 0)
+  # kScaledR, _ = cv2.getOptimalNewCameraMatrix(M_r, d_r, scaled_res[::-1], 0)
+  kScaledR = kScaledL = M_rp
+
+  # if self.cameraModel != 'perspective':
+  #   kScaledR = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(M_r, d_r, scaled_res[::-1], np.eye(3), fov_scale=1.1)
+  #   kScaledL = kScaledR
+
+    
+  print('Intrinsics from the getOptimalNewCameraMatrix/Original ....')
+  print(f"L: {kScaledL}")
+  print(f"R: {kScaledR}")
+  oldEpipolarError = None
+  epQueue = deque()
+  movePos = True
+
+  left_calib_model = left_cam_info['calib_model']
+  right_calib_model = right_cam_info['calib_model']
+  d_l = left_cam_info['dist_coeff']
+  d_r = left_cam_info['dist_coeff']
+  print(left_calib_model, right_calib_model)
+  if left_calib_model == 'perspective':
+    mapx_l, mapy_l = cv2.initUndistortRectifyMap(
+      M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
+  else:
+    mapx_l, mapy_l = cv2.fisheye.initUndistortRectifyMap(
+      M_lp, d_l, r_l, kScaledL, scaled_res[::-1], cv2.CV_32FC1)
+  if right_calib_model == "perspective":
+    mapx_r, mapy_r = cv2.initUndistortRectifyMap(
+      M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
+  else:
+    mapx_r, mapy_r = cv2.fisheye.initUndistortRectifyMap(
+      M_rp, d_r, r_r, kScaledR, scaled_res[::-1], cv2.CV_32FC1)
+
+  image_data_pairs = []
+  for image_left, image_right in zip(images_left, images_right):
+    # read images
+    img_l = cv2.imread(image_left, 0)
+    img_r = cv2.imread(image_right, 0)
+
+    img_l = scale_image(img_l, scaled_res)
+    img_r = scale_image(img_r, scaled_res)
+    # print(img_l.shape)
+    # print(img_r.shape)
+
+    # warp right image
+    # img_l = cv2.warpPerspective(img_l, self.H1, img_l.shape[::-1],
+    #               cv2.INTER_CUBIC +
+    #               cv2.WARP_FILL_OUTLIERS +
+    #               cv2.WARP_INVERSE_MAP)
+
+    # img_r = cv2.warpPerspective(img_r, self.H2, img_r.shape[::-1],
+    #               cv2.INTER_CUBIC +
+    #               cv2.WARP_FILL_OUTLIERS +
+    #               cv2.WARP_INVERSE_MAP)
+
+    img_l = cv2.remap(img_l, mapx_l, mapy_l, cv2.INTER_LINEAR)
+    img_r = cv2.remap(img_r, mapx_r, mapy_r, cv2.INTER_LINEAR)
+
+    image_data_pairs.append((img_l, img_r))
+    
     #if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
-    #  cv2.destroyWindow("undistorted-Left")
-    #  cv2.destroyWindow("undistorted-right")  
-    # compute metrics
-    imgpoints_r = []
-    imgpoints_l = []
-    image_epipolar_color = []
-    # new_imagePairs = [])
-    for i, image_data_pair in enumerate(image_data_pairs):
-        res2_l = detect_charuco_board(left_board, image_data_pair[0])
-        res2_r = detect_charuco_board(right_board, image_data_pair[1])
-        
-        # if self.traceLevel == 2 or self.traceLevel == 4 or self.traceLevel == 10:
-        
-
-        img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]])
-        img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB)
-        line_row = 0
-        while line_row < img_concat.shape[0]:
-            cv2.line(img_concat,
-                     (0, line_row), (img_concat.shape[1], line_row),
-                     (0, 255, 0), 1)
-            line_row += 30
-
-        cv2.imshow('Stereo Pair', img_concat)
-        k = cv2.waitKey(1)
-        
-        if res2_l[1] is not None and res2_r[2] is not None and len(res2_l[1]) > 3 and len(res2_r[1]) > 3:
-
-            cv2.cornerSubPix(image_data_pair[0], res2_l[1],
-                             winSize=(5, 5),
-                             zeroZone=(-1, -1),
-                             criteria=criteria)
-            cv2.cornerSubPix(image_data_pair[1], res2_r[1],
-                             winSize=(5, 5),
-                             zeroZone=(-1, -1),
-                             criteria=criteria)
-
-            # termination criteria
-            img_pth_right = Path(images_right[i])
-            img_pth_left = Path(images_left[i])
-            org = (100, 50)
-            # cv2.imshow('ltext', lText)
-            # cv2.waitKey(0)
-            localError = 0
-            corners_l = []
-            corners_r = []
-            for j in range(len(res2_l[2])):
-                idx = np.where(res2_r[2] == res2_l[2][j])
-                if idx[0].size == 0:
-                    continue
-                corners_l.append(res2_l[1][j])
-                corners_r.append(res2_r[1][idx])
-
-            imgpoints_l.append(corners_l)
-            imgpoints_r.append(corners_r)
-            epi_error_sum = 0
-            corner_epipolar_color = []
-            for l_pt, r_pt in zip(corners_l, corners_r):
-                if isHorizontal:
-                    curr_epipolar_error = abs(l_pt[0][1] - r_pt[0][1])
-                else:
-                    curr_epipolar_error = abs(l_pt[0][0] - r_pt[0][0])
-                if curr_epipolar_error >= 1:
-                    corner_epipolar_color.append(1)
-                else:
-                    corner_epipolar_color.append(0)
-                epi_error_sum += curr_epipolar_error
-            localError = epi_error_sum / len(corners_l)
-            image_epipolar_color.append(corner_epipolar_color)
-            #if self.traceLevel == 2 or self.traceLevel == 3 or self.traceLevel == 4 or self.traceLevel == 10:
-            print("Epipolar Error per image on host in " + img_pth_right.name + " : " +
-                    str(localError))
+      #cv2.imshow("undistorted-Left", img_l)
+      #cv2.imshow("undistorted-right", img_r)
+      # print(f'image path - {im}')
+      # print(f'Image Undistorted Size {undistorted_img.shape}')
+    #  k = cv2.waitKey(0)
+    #  if k == 27:  # Esc key to stop
+    #    break
+  #if self.traceLevel == 4 or self.traceLevel == 5 or self.traceLevel == 10:
+  #  cv2.destroyWindow("undistorted-Left")
+  #  cv2.destroyWindow("undistorted-right")  
+  # compute metrics
+  imgpoints_r = []
+  imgpoints_l = []
+  image_epipolar_color = []
+  # new_imagePairs = [])
+  for i, image_data_pair in enumerate(image_data_pairs):
+    res2_l = detect_charuco_board(left_board, image_data_pair[0])
+    res2_r = detect_charuco_board(right_board, image_data_pair[1])
+    
+    # if self.traceLevel == 2 or self.traceLevel == 4 or self.traceLevel == 10:
+    
+
+    img_concat = cv2.hconcat([image_data_pair[0], image_data_pair[1]])
+    img_concat = cv2.cvtColor(img_concat, cv2.COLOR_GRAY2RGB)
+    line_row = 0
+    while line_row < img_concat.shape[0]:
+      cv2.line(img_concat,
+           (0, line_row), (img_concat.shape[1], line_row),
+           (0, 255, 0), 1)
+      line_row += 30
+
+    cv2.imshow('Stereo Pair', img_concat)
+    k = cv2.waitKey(1)
+    
+    if res2_l[1] is not None and res2_r[2] is not None and len(res2_l[1]) > 3 and len(res2_r[1]) > 3:
+
+      cv2.cornerSubPix(image_data_pair[0], res2_l[1],
+               winSize=(5, 5),
+               zeroZone=(-1, -1),
+               criteria=criteria)
+      cv2.cornerSubPix(image_data_pair[1], res2_r[1],
+               winSize=(5, 5),
+               zeroZone=(-1, -1),
+               criteria=criteria)
+
+      # termination criteria
+      img_pth_right = Path(images_right[i])
+      img_pth_left = Path(images_left[i])
+      org = (100, 50)
+      # cv2.imshow('ltext', lText)
+      # cv2.waitKey(0)
+      localError = 0
+      corners_l = []
+      corners_r = []
+      for j in range(len(res2_l[2])):
+        idx = np.where(res2_r[2] == res2_l[2][j])
+        if idx[0].size == 0:
+          continue
+        corners_l.append(res2_l[1][j])
+        corners_r.append(res2_r[1][idx])
+
+      imgpoints_l.append(corners_l)
+      imgpoints_r.append(corners_r)
+      epi_error_sum = 0
+      corner_epipolar_color = []
+      for l_pt, r_pt in zip(corners_l, corners_r):
+        if isHorizontal:
+          curr_epipolar_error = abs(l_pt[0][1] - r_pt[0][1])
+        else:
+          curr_epipolar_error = abs(l_pt[0][0] - r_pt[0][0])
+        if curr_epipolar_error >= 1:
+          corner_epipolar_color.append(1)
         else:
-            print('Numer of corners is in left -> and right ->')
-            return -1
-        lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
-        rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
-        image_data_pairs[i] = (lText, rText)
-
-
-    epi_error_sum = 0
-    total_corners = 0
-    for corners_l, corners_r in zip(imgpoints_l, imgpoints_r):
-        total_corners += len(corners_l)
-        for l_pt, r_pt in zip(corners_l, corners_r):
-            if isHorizontal:
-                epi_error_sum += abs(l_pt[0][1] - r_pt[0][1])
-            else:
-                epi_error_sum += abs(l_pt[0][0] - r_pt[0][0])
-
-    avg_epipolar = epi_error_sum / total_corners
-    print("Average Epipolar Error is : " + str(avg_epipolar))
-
-    #if self.enable_rectification_disp:
-    #    self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal)
-
-    return avg_epipolar
+          corner_epipolar_color.append(0)
+        epi_error_sum += curr_epipolar_error
+      localError = epi_error_sum / len(corners_l)
+      image_epipolar_color.append(corner_epipolar_color)
+      #if self.traceLevel == 2 or self.traceLevel == 3 or self.traceLevel == 4 or self.traceLevel == 10:
+      print("Epipolar Error per image on host in " + img_pth_right.name + " : " +
+          str(localError))
+    else:
+      print('Numer of corners is in left -> and right ->')
+      return -1
+    lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
+    rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
+    image_data_pairs[i] = (lText, rText)
+
+
+  epi_error_sum = 0
+  total_corners = 0
+  for corners_l, corners_r in zip(imgpoints_l, imgpoints_r):
+    total_corners += len(corners_l)
+    for l_pt, r_pt in zip(corners_l, corners_r):
+      if isHorizontal:
+        epi_error_sum += abs(l_pt[0][1] - r_pt[0][1])
+      else:
+        epi_error_sum += abs(l_pt[0][0] - r_pt[0][0])
+
+  avg_epipolar = epi_error_sum / total_corners
+  print("Average Epipolar Error is : " + str(avg_epipolar))
+
+  #if self.enable_rectification_disp:
+  #  self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal)
+
+  return avg_epipolar

From bd886dc3566dec2f70788a3cbc7a1b6bf68af7ce Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 11 Nov 2024 17:27:26 +0100
Subject: [PATCH 69/87] Remove unused variables

---
 calibration_utils.py | 46 ++++++--------------------------------------
 1 file changed, 6 insertions(+), 40 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 3890b15..1ef6df0 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -838,29 +838,10 @@ def proxy_estimate_pose_and_filter_single(camData, corners, ids, dataset):
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
-  def __init__(self, traceLevel: float = 1.0, outputScaleFactor: float = 0.5, model = None,distortion_model = {}, filtering_enable = False, initial_max_threshold = 15, initial_min_filtered = 0.05, calibration_max_threshold = 10):
-    self.filtering_enable = filtering_enable
-    self.ccm_model = distortion_model
-    self.output_scale_factor = outputScaleFactor
-    self.initial_max_threshold = initial_max_threshold
-    self.initial_min_filtered = initial_min_filtered
-    self.calibration_max_threshold = calibration_max_threshold
-    self.calibration_min_filtered = initial_min_filtered
-
-    """Class to Calculate Calibration and Rectify a Stereo Camera."""
-
-  def calibrate(self, board_config, filepath, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = []):
+  def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = [], initial_max_threshold = 15, initial_min_filtered = 0.05):
     """Function to calculate calibration for stereo camera."""
-    # init object data
-    self._cameraModel = camera_model
-    self._data_path = filepath
-    self.stereocalib_criteria = (cv2.TERM_CRITERIA_COUNT +
-                   cv2.TERM_CRITERIA_EPS, 300, 1e-9)
-
-    self.cams = []
-
     config = CalibrationConfig(
-      self.filtering_enable, self.ccm_model, self.initial_max_threshold, self.initial_min_filtered, self.calibration_max_threshold, self.calibration_min_filtered,
+      initial_max_threshold, initial_min_filtered,
       camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
     )
     
@@ -884,7 +865,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsicCameras: List
         calib_model = cameraModel_ccm
         distortion_model = model_ccm
       else:
-        calib_model = self._cameraModel
+        calib_model = camera_model
         if camData["name"] in self.ccm_model:
           distortion_model = self.ccm_model[camData["name"]]
         else:
@@ -896,7 +877,7 @@ def calibrate(self, board_config, filepath, camera_model, intrinsicCameras: List
 
       if PER_CCM:
         camData = pw.run(get_features, config, camData)
-        if self._cameraModel == "fisheye":
+        if camera_model== "fisheye":
           camData = pw.run(filter_features_fisheye, camData, dataset.allCorners, dataset.allIds) # TODO : Input types are wrong
         elif dataset.enableFiltering:
           corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, dataset.allCorners, dataset.allIds, dataset)[:2]
@@ -929,30 +910,15 @@ def calibrate(self, board_config, filepath, camera_model, intrinsicCameras: List
 
         extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
       else:
-        if self._cameraModel == 'perspective':
+        if camera_model == 'perspective':
           extrinsics = pw.run(calibrate_stereo_perspective, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-        elif self._cameraModel == 'fisheye':
+        elif camera_model == 'fisheye':
           extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
       left_cam_info, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
       camInfos[left.name] = left_cam_info
       stereoConfigs.append(stereo_config)
 
     pw.execute()
-    
-    # def get_board_for_cam(cam_info: CameraData):
-    #   for pair in extrinsicPairs:
-    #     for cam in pair:
-    #       if cam.name == cam_info['name']:
-    #         return cam.board
-    
-    # test_epipolar_charuco(
-    #   {**left_cam_info.ret(), 'images_path': '/home/developer/debug_session/14442C10111E19D000/2024_11_08_18-06-05/thermal/capture/color'}, 
-    #   {**right_cam_info.ret(), 'images_path': '/home/developer/debug_session/14442C10111E19D000/2024_11_08_18-06-05/thermal/capture/thermal'},
-    #   get_board_for_cam(left_cam_info.ret()),
-    #   get_board_for_cam(right_cam_info.ret()),
-    #   extrinsics.ret()[2], # Translation between left and right Cameras
-    #   extrinsics.ret()[3], # Left Rectification rotation 
-    #   extrinsics.ret()[4]) # Right Rectification rotation
 
     # Extract camera info structs and stereo config
     for cam, camInfo in camInfos.items():

From 53d7beed4b14146ba3bcd2f7dd002932089f6722 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 11 Nov 2024 17:27:44 +0100
Subject: [PATCH 70/87] Remove unused variables

---
 types.py | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/types.py b/types.py
index 200ff51..a401b93 100644
--- a/types.py
+++ b/types.py
@@ -107,26 +107,18 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
     self.enableFiltering = enableFiltering
 
 class CalibrationConfig:
-  def __init__(self, enableFiltering = True, ccmModel = '', initialMaxThreshold = 0, initialMinFiltered = 0, calibrationMaxThreshold = 0, calibrationMinFiltered = 0,
+  def __init__(self, initialMaxThreshold = 0, initialMinFiltered = 0, ,
                cameraModel = 0, stereoCalibCriteria = 0):
     """Calibration configuration options
 
     Args:
-        enableFiltering (bool, optional): Whether corners should be filtered for outliers. Defaults to True.
-        ccmModel (str, optional): _description_. Defaults to ''.
         initialMaxThreshold (int, optional): _description_. Defaults to 0.
         initialMinFiltered (int, optional): _description_. Defaults to 0.
-        calibrationMaxThreshold (int, optional): _description_. Defaults to 0.
-        calibrationMinFiltered (int, optional): _description_. Defaults to 0.
         cameraModel (int, optional): _description_. Defaults to 0.
         stereoCalibCriteria (int, optional): _description_. Defaults to 0.
     """
-    self.enableFiltering = enableFiltering
-    self.ccmModel = ccmModel # Distortion model
     self.initialMaxThreshold = initialMaxThreshold
     self.initialMinFiltered = initialMinFiltered
-    self.calibrationMaxThreshold = calibrationMaxThreshold
-    self.calibrationMinFiltered = calibrationMinFiltered
     self.cameraModel = cameraModel
     self.stereoCalibCriteria = stereoCalibCriteria
 

From 41b38284b052413e0e6827bd90e6d2312132f806 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 11 Nov 2024 17:33:04 +0100
Subject: [PATCH 71/87] Typo

---
 types.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/types.py b/types.py
index a401b93..40ef6e0 100644
--- a/types.py
+++ b/types.py
@@ -107,7 +107,7 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
     self.enableFiltering = enableFiltering
 
 class CalibrationConfig:
-  def __init__(self, initialMaxThreshold = 0, initialMinFiltered = 0, ,
+  def __init__(self, initialMaxThreshold = 0, initialMinFiltered = 0,
                cameraModel = 0, stereoCalibCriteria = 0):
     """Calibration configuration options
 

From b667263b0b0d9e91ff746f14b7a900edc824bd4e Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 11 Nov 2024 17:43:47 +0100
Subject: [PATCH 72/87] Remove unused variable

---
 calibration_utils.py | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 1ef6df0..f057c2e 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -866,10 +866,7 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
         distortion_model = model_ccm
       else:
         calib_model = camera_model
-        if camData["name"] in self.ccm_model:
-          distortion_model = self.ccm_model[camData["name"]]
-        else:
-          distortion_model = DistortionModel.Tilted # Use the tilted model by default
+        distortion_model = DistortionModel.Tilted # Use the tilted model by default
 
       camData['size'] = dataset.imageSize
       camData['calib_model'] = calib_model

From faf8fec6b3f940e54322cf6417852e50759c7993 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 12 Nov 2024 15:49:35 +0100
Subject: [PATCH 73/87] Fix segfault bug

---
 worker.py | 68 +++++++++++++++++++++++++++++++------------------------
 1 file changed, 38 insertions(+), 30 deletions(-)

diff --git a/worker.py b/worker.py
index 61d4656..2a7df1c 100644
--- a/worker.py
+++ b/worker.py
@@ -174,15 +174,18 @@ def worker_controller(_stop: multiprocessing.Event, _in: multiprocessing.Queue,
   while not _stop.is_set():
     try:
       task: ParallelTask | None = _in.get(timeout=0.01)
+      
+      #try:
+      ret, exc = None, None
+      ret = task._fun(*task._args, **task._kwargs)
+      #except BaseException as e:
+        #exc = e
+      #finally:
+      _out.put((task._id, ret, exc))
     except queue.Empty:
       continue
-    #try:
-    ret, exc = None, None
-    ret = task._fun(*task._args, **task._kwargs)
-    #except BaseException as e:
-      #exc = e
-    #finally:
-    _out.put((task._id, ret, exc))
+    except:
+      break
 
 class ParallelWorker:
   def __init__(self, workers: int = 16):
@@ -218,26 +221,31 @@ def execute(self):
     doneTasks = []
     remaining = len(self._tasks)
 
-    while remaining > 0:
-      for task in self._tasks:
-        if task.isExecutable():
-          if isinstance(task, ParallelTask):
-            task.resolveArguments()
-            workerIn.put(task)
-            doneTasks.append(task)
-            self._tasks.remove(task)
-          else:
-            newTasks = task.tasks()
-            remaining += len(newTasks) - 1
-            self._tasks.extend(newTasks)
-            self._tasks.remove(task)
-
-      if not workerOut.empty():
-        tId, ret, exc = workerOut.get()
-        for task in doneTasks:
-          if task._id == tId:
-            task.finish(ret, exc)
-            remaining -= 1
-    stop.set()
-    for p in processes:
-      p.join()
\ No newline at end of file
+    try:
+      while remaining > 0:
+        for task in self._tasks:
+          if task.isExecutable():
+            if isinstance(task, ParallelTask):
+              task.resolveArguments()
+              #workerIn.put(task)
+              ret = task._fun(*task._args, **task._kwargs)
+              workerOut.put((task._id, ret, None))
+              
+              doneTasks.append(task)
+              self._tasks.remove(task)
+            else:
+              newTasks = task.tasks()
+              remaining += len(newTasks) - 1
+              self._tasks.extend(newTasks)
+              self._tasks.remove(task)
+
+        if not workerOut.empty():
+          tId, ret, exc = workerOut.get()
+          for task in doneTasks:
+            if task._id == tId:
+              task.finish(ret, exc)
+              remaining -= 1
+    finally:
+      stop.set()
+      for p in processes:
+        p.join()
\ No newline at end of file

From 2231d56eb949075ec88d51a39fb06777cc307ad9 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 14 Nov 2024 16:54:24 +0100
Subject: [PATCH 74/87] Fix stereo calib not being produced

---
 calibration_utils.py | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index f057c2e..1542acf 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -387,17 +387,19 @@ def remove_and_filter_stereo_features(leftCamData: CameraData, rightCamData: Cam
 
   return leftCamData, rightCamData
 
-def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraData, left_cam, right_cam, board_config, extrinsics):
+def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraData, left_cam: Dataset, right_cam: Dataset, board_config, extrinsics):
   if extrinsics[0] == -1:
     return -1, extrinsics[1]
   stereoConfig = None
   if 'stereo_config' in board_config:
-    if board_config['stereo_config']['left_cam'] == left_cam and board_config['stereo_config']['right_cam'] == right_cam: # TODO : Is this supposed to take the last camera pair?
+    leftCamName = board_config['cameras'][board_config['stereo_config']['left_cam']]['name']
+    rightCamName = board_config['cameras'][board_config['stereo_config']['right_cam']]['name']
+    if leftCamName == left_cam.name and rightCamName == right_cam.name: # TODO : Is this supposed to take the last camera pair?
       stereoConfig = {
         'rectification_left': extrinsics[3],
         'rectification_right': extrinsics[4]
       }
-    elif board_config['stereo_config']['left_cam'] == right_cam and board_config['stereo_config']['right_cam'] == left_cam:
+    elif leftCamName == right_cam and rightCamName == left_cam:
       stereoConfig = {
         'rectification_left': extrinsics[4],
         'rectification_right': extrinsics[3]

From cec3b6e0f3a7c748a4adf00df6cd2c0cd6d002f4 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 14 Nov 2024 16:54:43 +0100
Subject: [PATCH 75/87] Add debug output parameter

---
 calibration_utils.py | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 1542acf..d88ee59 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -840,7 +840,7 @@ def proxy_estimate_pose_and_filter_single(camData, corners, ids, dataset):
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
-  def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = [], initial_max_threshold = 15, initial_min_filtered = 0.05):
+  def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = [], initial_max_threshold = 15, initial_min_filtered = 0.05, debug: bool = False):
     """Function to calculate calibration for stereo camera."""
     config = CalibrationConfig(
       initial_max_threshold, initial_min_filtered,
@@ -855,6 +855,7 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
     pw = ParallelWorker(1)
     camInfos = {}
     stereoConfigs = []
+    allExtrinsics = []
 
     # Calibrate camera intrinsics for all provided datasets
     for dataset in intrinsicCameras:
@@ -914,6 +915,7 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
         elif camera_model == 'fisheye':
           extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
       left_cam_info, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
+      allExtrinsics.append(extrinsics)
       camInfos[left.name] = left_cam_info
       stereoConfigs.append(stereo_config)
 
@@ -929,4 +931,7 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
       if stereoConfig.ret():
         board_config['stereo_config'].update(stereoConfig.ret())
 
+    if debug:
+      return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}
+
     return board_config
\ No newline at end of file

From c79f3de6248d292fe5b9cdb6ba463225693c19bf Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 15 Nov 2024 16:56:49 +0100
Subject: [PATCH 76/87] Stereo filter features

---
 calibration_utils.py | 32 +++++++++++++++++---------------
 1 file changed, 17 insertions(+), 15 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index d88ee59..1b4aad1 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -351,17 +351,17 @@ def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sa
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
-def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset):
+def find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds, board: CharucoBoard):
   left_corners_sampled = []
   right_corners_sampled = []
   obj_pts = []
   failed = 0
 
-  for i, _ in enumerate(leftDataset.allIds): # For ids in all images
-    commonIds = np.intersect1d(leftDataset.allIds[i], rightDataset.allIds[i])
-    left_sub_corners = leftDataset.allCorners[i][np.isin(leftDataset.allIds[i], commonIds)]
-    right_sub_corners = rightDataset.allCorners[i][np.isin(rightDataset.allIds[i], commonIds)]
-    obj_pts_sub = leftDataset.board.board.chessboardCorners[commonIds]
+  for i, _ in enumerate(leftIds): # For ids in all images
+    commonIds = np.intersect1d(leftIds[i], rightIds[i])
+    left_sub_corners = leftCorners[i][np.isin(leftIds[i], commonIds)]
+    right_sub_corners = rightCorners[i][np.isin(rightIds[i], commonIds)]
+    obj_pts_sub = board.board.chessboardCorners[commonIds]
 
     if len(commonIds) > 6:
       obj_pts.append(obj_pts_sub)
@@ -370,7 +370,7 @@ def find_stereo_common_features(leftDataset: Dataset, rightDataset: Dataset):
     else:
       failed += 1
 
-  if failed > len(leftDataset.allIds) / 3:
+  if failed > len(leftIds) / 3:
     raise RuntimeError('More than 1/3 of images had less than 6 common features found')
 
   return left_corners_sampled, right_corners_sampled, obj_pts
@@ -382,10 +382,10 @@ def undistort_points_fisheye(allCorners, camInfo):
   return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
 def remove_and_filter_stereo_features(leftCamData: CameraData, rightCamData: CameraData, leftDataset: Dataset, rightDataset: Dataset):
-  leftCamData['filtered_corners'], leftCamData['filtered_ids'] = estimate_pose_and_filter(leftCamData, leftDataset.allCorners, leftDataset.allIds, leftDataset.board.board)
-  rightCamData['filtered_corners'], rightCamData['filtered_ids'] = estimate_pose_and_filter(rightCamData, rightDataset.allCorners, rightDataset.allIds, leftDataset.board.board)
+  leftCorners, leftIds = estimate_pose_and_filter(leftCamData, leftDataset.allCorners, leftDataset.allIds, leftDataset.board.board)
+  rightCorners, rightIds = estimate_pose_and_filter(rightCamData, rightDataset.allCorners, rightDataset.allIds, leftDataset.board.board)
 
-  return leftCamData, rightCamData
+  return leftCorners, leftIds, rightCorners, rightIds
 
 def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraData, left_cam: Dataset, right_cam: Dataset, board_config, extrinsics):
   if extrinsics[0] == -1:
@@ -746,6 +746,7 @@ def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags, camer
           flags=flags,
           criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
       except:
+        print('failed on dataset', dataset.name)
         raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element='', id=0)
       cameraIntrinsics = camera_matrix
       distCoeff = distortion_coefficients
@@ -851,8 +852,11 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
       if len(a.allIds) != len(b.allIds):
         print('Not all dataset for extrinsic calibration have the same number of images')
         raise RuntimeError('Not all dataset for extrinsic calibration have the same number of images') # TODO : This isn't thorough enough
+      if a.board is not b.board:
+        print('Extrinsic pair has different dataset board')
+        raise RuntimeError('Extrinsic pair has different dataset board')
 
-    pw = ParallelWorker(1)
+    pw = ParallelWorker(10)
     camInfos = {}
     stereoConfigs = []
     allExtrinsics = []
@@ -894,11 +898,9 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
       left_cam_info = camInfos[left.name]
       right_cam_info = camInfos[right.name]
 
-      if PER_CCM and EXTRINSICS_PER_CCM:
-        # TODO : Shouldn't refilter if it's already been filtered in intrinsic calibration, or should at least use two calls to filter_single
-        left_cam_info, right_cam_info = pw.run(remove_and_filter_stereo_features, left_cam_info, right_cam_info, left, right)[:2]
+      leftCorners, leftIds, rightCorners, rightIds = pw.run(remove_and_filter_stereo_features, left_cam_info, right_cam_info, left, right)[:4]
 
-      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, left, right)[:3]
+      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, leftCorners, leftIds, rightCorners, rightIds, left.board)[:3]
 
       if PER_CCM and EXTRINSICS_PER_CCM:
         if left_cam_info['calib_model'] == "perspective":

From 792e6eea7757f40f478599261488667b2f2abc74 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 25 Nov 2024 15:58:45 +0100
Subject: [PATCH 77/87] Charuco board detection, reuse filtered images

---
 calibration_utils.py | 44 +++++++++++++++++++++++++++++++++-----------
 1 file changed, 33 insertions(+), 11 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 1b4aad1..a6435cb 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -73,6 +73,25 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
     #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
+def detect_charuco_board(image: np.array, board: CharucoBoard):
+  arucoParams = cv2.aruco.DetectorParameters_create()
+  arucoParams.minMarkerDistanceRate = 0.01
+  markers, marker_ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, board.dictionary, parameters=arucoParams)  # First, detect markers
+  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, board.board, markers, marker_ids, rejectedCorners=rejectedImgPoints)
+  criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
+
+  # If found, add object points, image points (after refining them)
+  if len(marker_corners)>0:
+    num_corners, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids, image, board.board, minMarkers = 1)
+    if corners is not None and ids is not None and len(corners) > 3:
+      corners = cv2.cornerSubPix(image, corners,
+                              winSize=(5, 5),
+                              zeroZone=(-1, -1),
+                              criteria=criteria)
+    return corners, ids
+  else:
+    raise RuntimeError('Failed to detect corners on image')
+
 def get_features(config: CalibrationConfig, camData: CameraData) -> CameraData:
   f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
 
@@ -381,12 +400,6 @@ def undistort_points_perspective(allCorners, camInfo):
 def undistort_points_fisheye(allCorners, camInfo):
   return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
-def remove_and_filter_stereo_features(leftCamData: CameraData, rightCamData: CameraData, leftDataset: Dataset, rightDataset: Dataset):
-  leftCorners, leftIds = estimate_pose_and_filter(leftCamData, leftDataset.allCorners, leftDataset.allIds, leftDataset.board.board)
-  rightCorners, rightIds = estimate_pose_and_filter(rightCamData, rightDataset.allCorners, rightDataset.allIds, leftDataset.board.board)
-
-  return leftCorners, leftIds, rightCorners, rightIds
-
 def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraData, left_cam: Dataset, right_cam: Dataset, board_config, extrinsics):
   if extrinsics[0] == -1:
     return -1, extrinsics[1]
@@ -860,6 +873,7 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
     camInfos = {}
     stereoConfigs = []
     allExtrinsics = []
+    filteredCharucos = {}
 
     # Calibrate camera intrinsics for all provided datasets
     for dataset in intrinsicCameras:
@@ -879,26 +893,34 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
       camData['calib_model'] = calib_model
       camData['distortion_model'] = distortion_model
 
+      if len(dataset.allCorners) and len(dataset.allIds):
+        allCorners, allIds = dataset.allCorners, dataset.allIds
+      elif len(dataset.images):
+        allCorners, allIds = pw.map(detect_charuco_board, list(dataset.images), dataset.board)[:2]
+      else:
+        raise RuntimeError(f'Dataset \'{dataset.name}\' doesn\'t contain corners or images')
+
       if PER_CCM:
         camData = pw.run(get_features, config, camData)
         if camera_model== "fisheye":
-          camData = pw.run(filter_features_fisheye, camData, dataset.allCorners, dataset.allIds) # TODO : Input types are wrong
+          camData = pw.run(filter_features_fisheye, camData, allCorners, allIds) # TODO : Input types are wrong
         elif dataset.enableFiltering:
-          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, dataset.allCorners, dataset.allIds, dataset)[:2]
+          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, allCorners, allIds, dataset)[:2]
         else:
-          corners, ids = dataset.allCorners, dataset.allIds
+          corners, ids = allCorners, allIds
 
         camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
         camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)
         camInfos[dataset.name] = camData
+        filteredCharucos[dataset.name] = [corners, ids]
       else:
         camData = calibrate_ccm_intrinsics(config, camData)
 
     for left, right in extrinsicPairs:
       left_cam_info = camInfos[left.name]
       right_cam_info = camInfos[right.name]
-
-      leftCorners, leftIds, rightCorners, rightIds = pw.run(remove_and_filter_stereo_features, left_cam_info, right_cam_info, left, right)[:4]
+      leftCorners, leftIds = filteredCharucos[left.name]
+      rightCorners, rightIds = filteredCharucos[right.name]
 
       left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, leftCorners, leftIds, rightCorners, rightIds, left.board)[:3]
 

From 551f6a1c1a127510c916608140f4f9df33684b0e Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 25 Nov 2024 16:46:59 +0100
Subject: [PATCH 78/87] Fix bug in image loader

---
 types.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/types.py b/types.py
index 40ef6e0..4ebcab6 100644
--- a/types.py
+++ b/types.py
@@ -1,4 +1,4 @@
-from typing import TypedDict, List, Tuple
+from typing import TypedDict, List, Tuple, Iterable
 import cv2.aruco as aruco
 from enum import Enum
 import numpy as np
@@ -65,8 +65,8 @@ def board(self):
 
 class Dataset:
   class Images:
-    def __init__(self, images: List[np.ndarray | str]):
-      self._images = images
+    def __init__(self, images: Iterable[np.ndarray | str]):
+      self._images = list(images)
 
     def at(self, key):
       if isinstance(self._images[key], str):
@@ -77,7 +77,7 @@ def __len__(self):
       return len(self._images)
 
     def __iter__(self):
-      for i in len(self._images):
+      for i, _ in enumerate(self._images):
         yield self.at(i)
 
     def __getitem__(self, key):
@@ -99,7 +99,7 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
         enableFiltering (bool, optional): Whether to filter provided corners, or use all of them as is. Defaults to True.
     """
     self.name = name
-    self.images = Dataset.Images(images)
+    self.images = images if isinstance(images, Dataset.Images) else Dataset.Images(images)
     self.allCorners = allCorners
     self.allIds = allIds
     self.imageSize = imageSize

From dc16bd779a0956b381437993e0f83a5eb2f0234d Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Thu, 28 Nov 2024 17:46:06 +0100
Subject: [PATCH 79/87] Add rectification display in debugging, enable
 multithreading

---
 calibration_debug.py | 44 +++++++++++++++++++++++++++++++++++++++++---
 worker.py            |  5 +----
 2 files changed, 42 insertions(+), 7 deletions(-)

diff --git a/calibration_debug.py b/calibration_debug.py
index 870a882..236c1dd 100644
--- a/calibration_debug.py
+++ b/calibration_debug.py
@@ -74,6 +74,36 @@ def detect_charuco_board(config: CharucoBoard, image: np.array):
   else:
     return None, None, None, None, None
 
+def display_rectification(image_data_pairs, images_corners_l, images_corners_r, image_epipolar_color, isHorizontal):
+  print("Displaying Stereo Pair for visual inspection. Press the [ESC] key to exit.")
+  colors = [(0, 255 , 0), (0, 0, 255)]
+  for idx, image_data_pair in enumerate(image_data_pairs):
+    if isHorizontal:
+      img_concat = cv2.hconcat(
+        [image_data_pair[0], image_data_pair[1]])
+      for left_pt, right_pt, colorMode in zip(images_corners_l[idx], images_corners_r[idx], image_epipolar_color[idx]):
+        cv2.line(img_concat,
+                 (int(left_pt[0][0]), int(left_pt[0][1])), (int(right_pt[0][0]) + image_data_pair[0].shape[1], int(right_pt[0][1])),
+                 colors[colorMode], 1)
+    else:
+      img_concat = cv2.vconcat(
+        [image_data_pair[0], image_data_pair[1]])
+      for left_pt, right_pt, colorMode in zip(images_corners_l[idx], images_corners_r[idx], image_epipolar_color[idx]):
+        cv2.line(img_concat,
+               (int(left_pt[0][0]), int(left_pt[0][1])), (int(right_pt[0][0]), int(right_pt[0][1])  + image_data_pair[0].shape[0]),
+               colors[colorMode], 1)
+
+    img_concat = cv2.resize(img_concat, (0, 0), fx=0.8, fy=0.8)
+
+    # show image
+    cv2.imshow('Stereo Pair', img_concat)
+    while True:
+      k = cv2.waitKey(1)
+      if k == 27:  # Esc key to stop
+        break
+
+  cv2.destroyWindow('Stereo Pair')
+
 # Debugging functions
 
 def debug_epipolar_charuco(left_cam_info: CameraData, right_cam_info: CameraData, left_board: CharucoBoard, right_board: CharucoBoard, t, r_l, r_r):
@@ -223,7 +253,10 @@ def debug_epipolar_charuco(left_cam_info: CameraData, right_cam_info: CameraData
       line_row += 30
 
     cv2.imshow('Stereo Pair', img_concat)
-    k = cv2.waitKey(1)
+    while True:
+      k = cv2.waitKey(1)
+      if k == 27:  # Esc key to stop
+        break
     
     if res2_l[1] is not None and res2_r[2] is not None and len(res2_l[1]) > 3 and len(res2_r[1]) > 3:
 
@@ -251,6 +284,8 @@ def debug_epipolar_charuco(left_cam_info: CameraData, right_cam_info: CameraData
           continue
         corners_l.append(res2_l[1][j])
         corners_r.append(res2_r[1][idx])
+      #if len(corners_l) == 0 or len(corners_r) == 0:
+        #continue
 
       imgpoints_l.append(corners_l)
       imgpoints_r.append(corners_r)
@@ -273,6 +308,9 @@ def debug_epipolar_charuco(left_cam_info: CameraData, right_cam_info: CameraData
           str(localError))
     else:
       print('Numer of corners is in left -> and right ->')
+      imgpoints_l.append([])
+      imgpoints_r.append([])
+      continue
       return -1
     lText = cv2.putText(cv2.cvtColor(image_data_pair[0],cv2.COLOR_GRAY2RGB), img_pth_left.name, org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
     rText = cv2.putText(cv2.cvtColor(image_data_pair[1],cv2.COLOR_GRAY2RGB), img_pth_right.name + " Error: " + str(localError), org, cv2.FONT_HERSHEY_SIMPLEX, 1, (2, 0, 255), 2, cv2.LINE_AA)
@@ -292,7 +330,7 @@ def debug_epipolar_charuco(left_cam_info: CameraData, right_cam_info: CameraData
   avg_epipolar = epi_error_sum / total_corners
   print("Average Epipolar Error is : " + str(avg_epipolar))
 
-  #if self.enable_rectification_disp:
-  #  self.display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal)
+  if True or enable_rectification_disp:
+    display_rectification(image_data_pairs, imgpoints_l, imgpoints_r, image_epipolar_color, isHorizontal)
 
   return avg_epipolar
diff --git a/worker.py b/worker.py
index 2a7df1c..e82b2b9 100644
--- a/worker.py
+++ b/worker.py
@@ -227,10 +227,7 @@ def execute(self):
           if task.isExecutable():
             if isinstance(task, ParallelTask):
               task.resolveArguments()
-              #workerIn.put(task)
-              ret = task._fun(*task._args, **task._kwargs)
-              workerOut.put((task._id, ret, None))
-              
+              workerIn.put(task)
               doneTasks.append(task)
               self._tasks.remove(task)
             else:

From e0f5f50ad770d543b952cea6b948207a582af86c Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 2 Dec 2024 18:06:35 +0100
Subject: [PATCH 80/87] Fix incorrect corner passing when multiple calibration
 boards are used

---
 calibration_utils.py | 42 +++++++++++++++++++++++++++---------------
 types.py             |  1 +
 2 files changed, 28 insertions(+), 15 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index a6435cb..e79f046 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1,6 +1,7 @@
 from scipy.spatial.transform import Rotation
 from .worker import ParallelWorker
 from typing import List, Tuple
+from itertools import chain
 from pathlib import Path
 from .types import *
 import numpy as np
@@ -860,7 +861,7 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
       initial_max_threshold, initial_min_filtered,
       camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
     )
-    
+
     for a, b in extrinsicPairs:
       if len(a.allIds) != len(b.allIds):
         print('Not all dataset for extrinsic calibration have the same number of images')
@@ -875,11 +876,17 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
     allExtrinsics = []
     filteredCharucos = {}
 
+    # Compile a list of unique datasets
+    uniqueDatasets = intrinsicCameras
+    for dataset in chain(*extrinsicPairs):
+      if dataset not in uniqueDatasets:
+        uniqueDatasets.append(dataset)
+
     # Calibrate camera intrinsics for all provided datasets
-    for dataset in intrinsicCameras:
+    for dataset in uniqueDatasets:
       camData = [c for c in board_config['cameras'].values() if c['name'] == dataset.name][0]
-      
-      if "calib_model" in camData:
+
+      if "calib_model" in camData and len(camData["calib_model"].split("_")) > 1:
         cameraModel_ccm, model_ccm = camData["calib_model"].split("_")
         if cameraModel_ccm == "fisheye":
           model_ccm == None
@@ -911,16 +918,16 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
 
         camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
         camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)
-        camInfos[dataset.name] = camData
-        filteredCharucos[dataset.name] = [corners, ids]
+        camInfos[dataset.id] = camData
+        filteredCharucos[dataset.id] = [corners, ids]
       else:
         camData = calibrate_ccm_intrinsics(config, camData)
 
     for left, right in extrinsicPairs:
-      left_cam_info = camInfos[left.name]
-      right_cam_info = camInfos[right.name]
-      leftCorners, leftIds = filteredCharucos[left.name]
-      rightCorners, rightIds = filteredCharucos[right.name]
+      left_cam_info = camInfos[left.id]
+      right_cam_info = camInfos[right.id]
+      leftCorners, leftIds = filteredCharucos[left.id]
+      rightCorners, rightIds = filteredCharucos[right.id]
 
       left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, leftCorners, leftIds, rightCorners, rightIds, left.board)[:3]
 
@@ -940,16 +947,21 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
           extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
       left_cam_info, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
       allExtrinsics.append(extrinsics)
-      camInfos[left.name] = left_cam_info
+      camInfos[left.id] = left_cam_info
       stereoConfigs.append(stereo_config)
 
     pw.execute()
 
-    # Extract camera info structs and stereo config
-    for cam, camInfo in camInfos.items():
+    # Construct board config from calibrated cam infos
+    for dataset in intrinsicCameras:
+      for socket in board_config['cameras']:
+        if board_config['cameras'][socket]['name'] == dataset.name:
+          board_config['cameras'][socket] = camInfos[dataset.id].ret()
+
+    for left, _ in extrinsicPairs:
       for socket in board_config['cameras']:
-        if board_config['cameras'][socket]['name'] == cam:
-          board_config['cameras'][socket] = camInfo.ret()
+        if board_config['cameras'][socket]['name'] == left.name:
+          board_config['cameras'][socket]['extrinsics'] = camInfos[left.id].ret()['extrinsics']
 
     for stereoConfig in stereoConfigs:
       if stereoConfig.ret():
diff --git a/types.py b/types.py
index 4ebcab6..3886ea7 100644
--- a/types.py
+++ b/types.py
@@ -105,6 +105,7 @@ def __init__(self, name: str, board: CharucoBoard, images: List[np.ndarray | str
     self.imageSize = imageSize
     self.board = board
     self.enableFiltering = enableFiltering
+    self.id = hash(self)
 
 class CalibrationConfig:
   def __init__(self, initialMaxThreshold = 0, initialMinFiltered = 0,

From 1b3868919bf0d17f924350164aedd0a08af282cd Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 2 Dec 2024 18:06:49 +0100
Subject: [PATCH 81/87] Pass errors

---
 worker.py | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

diff --git a/worker.py b/worker.py
index e82b2b9..65f29f0 100644
--- a/worker.py
+++ b/worker.py
@@ -174,13 +174,12 @@ def worker_controller(_stop: multiprocessing.Event, _in: multiprocessing.Queue,
   while not _stop.is_set():
     try:
       task: ParallelTask | None = _in.get(timeout=0.01)
-      
-      #try:
+
       ret, exc = None, None
-      ret = task._fun(*task._args, **task._kwargs)
-      #except BaseException as e:
-        #exc = e
-      #finally:
+      try:
+        ret = task._fun(*task._args, **task._kwargs)
+      except BaseException as e:
+        exc = e
       _out.put((task._id, ret, exc))
     except queue.Empty:
       continue
@@ -240,6 +239,8 @@ def execute(self):
           tId, ret, exc = workerOut.get()
           for task in doneTasks:
             if task._id == tId:
+              if exc:
+                raise Exception(f'Calibration pipeline failed during \'{task}\'') from exc
               task.finish(ret, exc)
               remaining -= 1
     finally:

From 3f510d5749563d126c7ca6d05876795169f13509 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Mon, 16 Dec 2024 15:53:06 +0100
Subject: [PATCH 82/87] Return filtered charucos

---
 calibration_utils.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index e79f046..8502e75 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -968,6 +968,6 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
         board_config['stereo_config'].update(stereoConfig.ret())
 
     if debug:
-      return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}
+      return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}, filteredCharucos
 
-    return board_config
\ No newline at end of file
+    return board_config, filteredCharucos
\ No newline at end of file

From b06527a483a628c03419e551af7946c6a30de504 Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Tue, 17 Dec 2024 14:45:16 +0100
Subject: [PATCH 83/87] Fix filtered corner saving

---
 calibration_utils.py | 41 +++++++++++++----------------------------
 1 file changed, 13 insertions(+), 28 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 8502e75..f2e53c3 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -2,7 +2,6 @@
 from .worker import ParallelWorker
 from typing import List, Tuple
 from itertools import chain
-from pathlib import Path
 from .types import *
 import numpy as np
 import time
@@ -25,8 +24,10 @@ def summary(self) -> str:
     """
     return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
-def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBoard):
-  objpoints = charucoBoard.chessboardCorners[ids]
+def estimate_pose_and_filter(camData: CameraData, corners, ids, charucoBoard: CharucoBoard):
+  """Very rough corner filtering on a single image"""
+
+  objpoints = charucoBoard.board.chessboardCorners[ids]
 
   ini_threshold=5
   threshold = None
@@ -52,7 +53,7 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
 
     ini_threshold += camData['threshold_stepper']
 
-  if ids is not None and corners.size > 0: # TODO : Try to remove the np reshaping
+  if ids.size > 0 and corners.size > 0: # TODO : Try to remove the np reshaping
     ids = ids.flatten()  # Flatten the IDs from 2D to 1D
     imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, camData['intrinsics'], camData['dist_coeff'])
     corners2 = corners.reshape(-1, 2)
@@ -64,14 +65,8 @@ def estimate_pose_and_filter_single(camData: CameraData, corners, ids, charucoBo
     valid_mask = errors <= threshold
     removed_mask = ~valid_mask
 
-    # Collect valid IDs in the original format (array of arrays)
     valid_ids = ids[valid_mask]
-    #filtered_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
-
-    # Collect data for valid points
-    #filtered_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
 
-    #removed_corners.extend(corners2[removed_mask])
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
 def detect_charuco_board(image: np.array, board: CharucoBoard):
@@ -115,16 +110,6 @@ def get_features(config: CalibrationConfig, camData: CameraData) -> CameraData:
 
   return camData
 
-def estimate_pose_and_filter(camData: CameraData, allCorners, allIds, charucoBoard):
-  filtered_corners = []
-  filtered_ids = []
-  for corners, ids in zip(allCorners, allIds):
-    corners, ids, _ = estimate_pose_and_filter_single(camData, corners, ids, charucoBoard)
-    filtered_corners.append(corners)
-    filtered_ids.append(ids)
-
-  return filtered_corners, filtered_ids
-
 def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset):
   # TODO : If we still need this check it needs to be elsewhere
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
@@ -135,11 +120,11 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
   
   # Convert to int32 from uint32 # TODO : Shouldn't be necessary
   for i, ids in enumerate(allIds): allIds[i] = ids.reshape(-1, 1).astype(np.int32)
-  
+
   # Filter for only images with >6 corners # TODO : Shouldn't be in here, should be in a separate necessary filtering function
   allCorners2 = []
   allIds2 = []
-  
+
   for corners, ids in zip(allCorners, allIds):
     if len(ids) < 6:
       continue
@@ -206,8 +191,7 @@ def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraD
   camData['reprojection_error'] = ret
   print("Reprojection error of {0}: {1}".format(
     camData['name'], ret))
-
-  return camData
+  return camData, filtered_corners, filtered_ids
 
 def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
   imsize = camData['size']
@@ -850,8 +834,6 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
 
   return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
 
-def proxy_estimate_pose_and_filter_single(camData, corners, ids, dataset):
-  return estimate_pose_and_filter_single(camData, corners, ids, dataset.board.board)
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
@@ -912,12 +894,12 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
         if camera_model== "fisheye":
           camData = pw.run(filter_features_fisheye, camData, allCorners, allIds) # TODO : Input types are wrong
         elif dataset.enableFiltering:
-          corners, ids = pw.map(proxy_estimate_pose_and_filter_single, camData, allCorners, allIds, dataset)[:2]
+          corners, ids = pw.map(estimate_pose_and_filter, camData, allCorners, allIds, dataset.board)[:2]
         else:
           corners, ids = allCorners, allIds
 
         camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
-        camData = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)
+        camData, corners, ids = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)[:3]
         camInfos[dataset.id] = camData
         filteredCharucos[dataset.id] = [corners, ids]
       else:
@@ -967,6 +949,9 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
       if stereoConfig.ret():
         board_config['stereo_config'].update(stereoConfig.ret())
 
+    for key in filteredCharucos.keys():
+      filteredCharucos[key] = [e.ret() for e in filteredCharucos[key]]
+
     if debug:
       return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}, filteredCharucos
 

From fab3b5b3c53af5ae767c413daa01fedcc8deba0c Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 20 Dec 2024 18:21:05 +0100
Subject: [PATCH 84/87] Automatically generate parallel worker calls by
 modifying the function AST

---
 calibration_utils.py | 214 +++++++++++++++++++++----------------
 worker.py            | 249 +++++++++++++++++++++++++++++++++++++++++--
 2 files changed, 366 insertions(+), 97 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index f2e53c3..b15bd88 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1,5 +1,5 @@
 from scipy.spatial.transform import Rotation
-from .worker import ParallelWorker
+from .worker import parallel_function
 from typing import List, Tuple
 from itertools import chain
 from .types import *
@@ -24,6 +24,7 @@ def summary(self) -> str:
     """
     return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
+@parallel_function
 def estimate_pose_and_filter(camData: CameraData, corners, ids, charucoBoard: CharucoBoard):
   """Very rough corner filtering on a single image"""
 
@@ -69,6 +70,7 @@ def estimate_pose_and_filter(camData: CameraData, corners, ids, charucoBoard: Ch
 
     return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
 
+@parallel_function
 def detect_charuco_board(image: np.array, board: CharucoBoard):
   arucoParams = cv2.aruco.DetectorParameters_create()
   arucoParams.minMarkerDistanceRate = 0.01
@@ -88,6 +90,7 @@ def detect_charuco_board(image: np.array, board: CharucoBoard):
   else:
     raise RuntimeError('Failed to detect corners on image')
 
+@parallel_function
 def get_features(config: CalibrationConfig, camData: CameraData) -> CameraData:
   f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
 
@@ -110,6 +113,7 @@ def get_features(config: CalibrationConfig, camData: CameraData) -> CameraData:
 
   return camData
 
+@parallel_function
 def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset):
   # TODO : If we still need this check it needs to be elsewhere
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
@@ -153,6 +157,7 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
   camData['filtered_ids'] = allIds2
   return camData
 
+@parallel_function
 def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features, all_ids):
   f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
   print("INTRINSIC CALIBRATION")
@@ -178,6 +183,7 @@ def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features, al
 
   return camData
 
+@parallel_function
 def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraData, dataset: Dataset):
   start = time.time()
   print('starting calibrate_wf')
@@ -233,6 +239,7 @@ def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
 
   return camData
 
+@parallel_function
 def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, allLeftCorners, allRightCorners, leftCamData: CameraData, rightCamData: CameraData):
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = leftCamData['intrinsics'], leftCamData['dist_coeff'], rightCamData['intrinsics'], rightCamData['dist_coeff'], leftCamData['distortion_model']
   specTranslation = leftCamData['extrinsics']['specTranslation']
@@ -275,6 +282,7 @@ def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, allLeftCorn
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
+@parallel_function
 def calibrate_stereo_perspective_per_ccm(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
   specTranslation = left_cam_info['extrinsics']['specTranslation']
@@ -314,6 +322,7 @@ def calibrate_stereo_perspective_per_ccm(config: CalibrationConfig, obj_pts, lef
   # print(f'P_r is \n {P_r}')
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
+@parallel_function
 def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
   cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
   # make sure all images have the same *number of* points
@@ -355,6 +364,7 @@ def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sa
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
+@parallel_function
 def find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds, board: CharucoBoard):
   left_corners_sampled = []
   right_corners_sampled = []
@@ -379,12 +389,15 @@ def find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds, bo
 
   return left_corners_sampled, right_corners_sampled, obj_pts
 
+@parallel_function
 def undistort_points_perspective(allCorners, camInfo):
   return [cv2.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
+@parallel_function
 def undistort_points_fisheye(allCorners, camInfo):
   return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
 
+@parallel_function
 def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraData, left_cam: Dataset, right_cam: Dataset, board_config, extrinsics):
   if extrinsics[0] == -1:
     return -1, extrinsics[1]
@@ -757,6 +770,7 @@ def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags, camer
       #previous_ids = removed_ids
   return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
+@parallel_function
 def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name):
   f_init = imsize[0]/np.deg2rad(hfov)*1.15
 
@@ -834,6 +848,102 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
 
   return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
 
+@parallel_function
+def calibrate_camera(config, board_config, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = []):
+  camInfos = {}
+  stereoConfigs = []
+  allExtrinsics = []
+  filteredCharucos = {}
+
+  # Compile a list of unique datasets
+  uniqueDatasets = intrinsicCameras
+  for dataset in chain(*extrinsicPairs):
+    if dataset not in uniqueDatasets:
+      uniqueDatasets.append(dataset)
+
+  # Calibrate camera intrinsics for all provided datasets
+  for dataset in uniqueDatasets:
+    camData: CameraData = [c for c in board_config['cameras'].values() if c['name'] == dataset.name][0]
+
+    if "calib_model" in camData and len(camData["calib_model"].split("_")) > 1:
+      cameraModel_ccm, model_ccm = camData["calib_model"].split("_")
+      if cameraModel_ccm == "fisheye":
+        model_ccm == None
+      calib_model = cameraModel_ccm
+      distortion_model = model_ccm
+    else:
+      calib_model = camera_model
+      distortion_model = DistortionModel.Tilted # Use the tilted model by default
+
+    camData['size'] = dataset.imageSize
+    camData['calib_model'] = calib_model
+    camData['distortion_model'] = distortion_model
+
+    if len(dataset.allCorners) and len(dataset.allIds):
+      allCorners, allIds = dataset.allCorners, dataset.allIds
+    elif len(dataset.images):
+      #allCorners, allIds = detect_charuco_board(list(dataset.images), dataset.board)
+      
+      allCorners, allids = [], []
+      for image in list(dataset.images):
+        corners, ids = detect_charuco_board(image, dataset.board)
+        allCorners.append(corners)
+        allIds.append(ids)
+    else:
+      raise RuntimeError(f'Dataset \'{dataset.name}\' doesn\'t contain corners or images')
+
+    if PER_CCM:
+      camData = get_features(config, camData)
+      if camera_model== "fisheye":
+        camData = filter_features_fisheye(camData, allCorners, allIds) # TODO : Input types are wrong
+      elif dataset.enableFiltering:
+        filteredCorners, filteredIds = [], []
+        for corners, ids in zip(allCorners, allIds):
+          corners, ids, _ = estimate_pose_and_filter(camData, corners, ids, dataset.board)
+          filteredCorners.append(corners)
+          filteredIds.append(ids)
+        corners, ids = filteredCorners, filteredIds
+
+        #corners, ids = estimate_pose_and_filter(camData, allCorners, allIds, dataset.board)
+      else:
+        corners, ids = allCorners, allIds
+
+      camData = calibrate_charuco(camData, corners, ids, dataset)
+      camData, corners, ids = calibrate_ccm_intrinsics_per_ccm(config, camData, dataset)
+      camInfos[dataset.id] = camData
+      filteredCharucos[dataset.id] = [corners, ids]
+    else:
+      camData = calibrate_ccm_intrinsics(config, camData) # TODO : Not a parallel function
+
+  for left, right in extrinsicPairs:
+    left_cam_info = camInfos[left.id]
+    right_cam_info = camInfos[right.id]
+    leftCorners, leftIds = filteredCharucos[left.id]
+    rightCorners, rightIds = filteredCharucos[right.id]
+
+    left_corners_sampled, right_corners_sampled, obj_pts= find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds, left.board)
+
+    if PER_CCM and EXTRINSICS_PER_CCM:
+      if left_cam_info['calib_model'] == "perspective":
+        left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info)
+        right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info)
+      else:
+        left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info)
+        right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info)
+
+      extrinsics = calibrate_stereo_perspective_per_ccm(config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+    else:
+      if camera_model == 'perspective':
+        extrinsics = calibrate_stereo_perspective(config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+      elif camera_model == 'fisheye':
+        extrinsics = calibrate_stereo_fisheye(config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+    left_cam_info, stereo_config = calculate_epipolar_error(left_cam_info, right_cam_info, left, right, board_config, extrinsics)
+    allExtrinsics.append(extrinsics)
+    camInfos[left.id] = left_cam_info
+    stereoConfigs.append(stereo_config)
+
+  return board_config, filteredCharucos, allExtrinsics, camInfos, stereoConfigs
+
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
@@ -852,107 +962,33 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
         print('Extrinsic pair has different dataset board')
         raise RuntimeError('Extrinsic pair has different dataset board')
 
-    pw = ParallelWorker(10)
-    camInfos = {}
-    stereoConfigs = []
-    allExtrinsics = []
-    filteredCharucos = {}
-
-    # Compile a list of unique datasets
-    uniqueDatasets = intrinsicCameras
-    for dataset in chain(*extrinsicPairs):
-      if dataset not in uniqueDatasets:
-        uniqueDatasets.append(dataset)
-
-    # Calibrate camera intrinsics for all provided datasets
-    for dataset in uniqueDatasets:
-      camData = [c for c in board_config['cameras'].values() if c['name'] == dataset.name][0]
-
-      if "calib_model" in camData and len(camData["calib_model"].split("_")) > 1:
-        cameraModel_ccm, model_ccm = camData["calib_model"].split("_")
-        if cameraModel_ccm == "fisheye":
-          model_ccm == None
-        calib_model = cameraModel_ccm
-        distortion_model = model_ccm
-      else:
-        calib_model = camera_model
-        distortion_model = DistortionModel.Tilted # Use the tilted model by default
-
-      camData['size'] = dataset.imageSize
-      camData['calib_model'] = calib_model
-      camData['distortion_model'] = distortion_model
-
-      if len(dataset.allCorners) and len(dataset.allIds):
-        allCorners, allIds = dataset.allCorners, dataset.allIds
-      elif len(dataset.images):
-        allCorners, allIds = pw.map(detect_charuco_board, list(dataset.images), dataset.board)[:2]
-      else:
-        raise RuntimeError(f'Dataset \'{dataset.name}\' doesn\'t contain corners or images')
-
-      if PER_CCM:
-        camData = pw.run(get_features, config, camData)
-        if camera_model== "fisheye":
-          camData = pw.run(filter_features_fisheye, camData, allCorners, allIds) # TODO : Input types are wrong
-        elif dataset.enableFiltering:
-          corners, ids = pw.map(estimate_pose_and_filter, camData, allCorners, allIds, dataset.board)[:2]
-        else:
-          corners, ids = allCorners, allIds
-
-        camData = pw.run(calibrate_charuco, camData, corners, ids, dataset)
-        camData, corners, ids = pw.run(calibrate_ccm_intrinsics_per_ccm, config, camData, dataset)[:3]
-        camInfos[dataset.id] = camData
-        filteredCharucos[dataset.id] = [corners, ids]
-      else:
-        camData = calibrate_ccm_intrinsics(config, camData)
-
-    for left, right in extrinsicPairs:
-      left_cam_info = camInfos[left.id]
-      right_cam_info = camInfos[right.id]
-      leftCorners, leftIds = filteredCharucos[left.id]
-      rightCorners, rightIds = filteredCharucos[right.id]
-
-      left_corners_sampled, right_corners_sampled, obj_pts= pw.run(find_stereo_common_features, leftCorners, leftIds, rightCorners, rightIds, left.board)[:3]
-
-      if PER_CCM and EXTRINSICS_PER_CCM:
-        if left_cam_info['calib_model'] == "perspective":
-          left_corners_sampled = pw.run(undistort_points_perspective, left_corners_sampled, left_cam_info)
-          right_corners_sampled = pw.run(undistort_points_perspective, right_corners_sampled, right_cam_info)
-        else:
-          left_corners_sampled = pw.run(undistort_points_fisheye, left_corners_sampled, left_cam_info)
-          right_corners_sampled = pw.run(undistort_points_fisheye, right_corners_sampled, right_cam_info)
-
-        extrinsics = pw.run(calibrate_stereo_perspective_per_ccm, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-      else:
-        if camera_model == 'perspective':
-          extrinsics = pw.run(calibrate_stereo_perspective, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-        elif camera_model == 'fisheye':
-          extrinsics = pw.run(calibrate_stereo_fisheye, config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-      left_cam_info, stereo_config = pw.run(calculate_epipolar_error, left_cam_info, right_cam_info, left, right, board_config, extrinsics)[:2]
-      allExtrinsics.append(extrinsics)
-      camInfos[left.id] = left_cam_info
-      stereoConfigs.append(stereo_config)
-
-    pw.execute()
+    #board_config, filteredCharucos, allExtrinsics, camInfos, stereoConfigs = calibrate_camera.run_parallel(10, config, board_config, camera_model, intrinsicCameras, extrinsicPairs)
+    board_config, filteredCharucos, allExtrinsics, camInfos, stereoConfigs = calibrate_camera.run_parallel(10, config, board_config, camera_model, intrinsicCameras, extrinsicPairs)
 
     # Construct board config from calibrated cam infos
     for dataset in intrinsicCameras:
       for socket in board_config['cameras']:
         if board_config['cameras'][socket]['name'] == dataset.name:
-          board_config['cameras'][socket] = camInfos[dataset.id].ret()
+          #board_config['cameras'][socket] = camInfos[dataset.id].ret()
+          board_config['cameras'][socket] = camInfos[dataset.id]
 
     for left, _ in extrinsicPairs:
       for socket in board_config['cameras']:
         if board_config['cameras'][socket]['name'] == left.name:
-          board_config['cameras'][socket]['extrinsics'] = camInfos[left.id].ret()['extrinsics']
+          #board_config['cameras'][socket]['extrinsics'] = camInfos[left.id].ret()['extrinsics']
+          board_config['cameras'][socket]['extrinsics'] = camInfos[left.id]['extrinsics']
 
     for stereoConfig in stereoConfigs:
-      if stereoConfig.ret():
-        board_config['stereo_config'].update(stereoConfig.ret())
+      #if stereoConfig.ret():
+      #  board_config['stereo_config'].update(stereoConfig.ret())
+      if stereoConfig:
+        board_config['stereo_config'].update(stereoConfig)
 
-    for key in filteredCharucos.keys():
-      filteredCharucos[key] = [e.ret() for e in filteredCharucos[key]]
+    #for key in filteredCharucos.keys():
+      #filteredCharucos[key] = [e.ret() for e in filteredCharucos[key]]
 
     if debug:
-      return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}, filteredCharucos
+      #return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}, filteredCharucos
+      return stereoConfigs, allExtrinsics, board_config, camInfos, filteredCharucos
 
     return board_config, filteredCharucos
\ No newline at end of file
diff --git a/worker.py b/worker.py
index 65f29f0..79fe429 100644
--- a/worker.py
+++ b/worker.py
@@ -1,8 +1,11 @@
-from typing import Dict, Any, Callable, Iterable, Tuple, TypeVar, Generic, Sequence, List
+from typing import Dict, Any, Callable, Iterable, Tuple, TypeVar, Generic, List
 from collections import abc
-import multiprocessing
+import multiprocess
+import inspect
 import random
 import queue
+import copy
+import ast
 
 T = TypeVar('T')
 
@@ -170,7 +173,7 @@ def isExecutable(self) -> bool:
         return False
     return True
 
-def worker_controller(_stop: multiprocessing.Event, _in: multiprocessing.Queue, _out: multiprocessing.Queue, _wId: int) -> None:
+def worker_controller(_stop: multiprocess.Event, _in: multiprocess.Queue, _out: multiprocess.Queue, _wId: int) -> None:
   while not _stop.is_set():
     try:
       task: ParallelTask | None = _in.get(timeout=0.01)
@@ -208,12 +211,12 @@ def map(self, fun: Callable[[Any], T], *args: Tuple[Iterable[Any]], **kwargs: Di
     return taskGroup
 
   def execute(self):
-    workerIn = multiprocessing.Manager().Queue()
-    workerOut = multiprocessing.Manager().Queue()
-    stop = multiprocessing.Event()
+    workerIn = multiprocess.Manager().Queue()
+    workerOut = multiprocess.Manager().Queue()
+    stop = multiprocess.Event()
     processes = []
     for i in range(self._workers):
-      p = multiprocessing.Process(target=worker_controller, args=(stop, workerIn, workerOut, i))
+      p = multiprocess.Process(target=worker_controller, args=(stop, workerIn, workerOut, i))
       processes.append(p)
       p.start()
 
@@ -246,4 +249,234 @@ def execute(self):
     finally:
       stop.set()
       for p in processes:
-        p.join()
\ No newline at end of file
+        p.join()
+
+class ParallelFunctionTransformer(ast.NodeTransformer):
+  """AST Transformer for parsing ParallelFunctions and replacing calls to other parallel functions with pw.run(fun)"""
+
+  def __init__(self, pw: ParallelWorker):
+    self._pw = pw
+
+  def visit_Expr(self, node):
+    """Finds all standalone expressions and converts them to __ParallelWorker_run__"""
+    if isinstance(node.value, ast.Call):
+      call_node = node.value
+      new_node = ast.Expr(
+        value=ast.Call(
+          func=ast.Name(id='__ParallelWorker_run__', ctx=ast.Load()),
+          args=[
+            ast.Constant(0),
+            call_node.func,
+            *call_node.args
+          ],
+          keywords=call_node.keywords
+        )
+      )
+      ast.fix_missing_locations(new_node)
+      return ast.copy_location(new_node, node)
+    return node
+
+  def visit_Assign(self, node):
+    """
+    Finds all assignments and converts them to __ParallelWorker_run__
+    """
+    targets = node.targets
+    target_names = []
+    for target in targets:
+      if isinstance(target, ast.Tuple):
+        target_names.extend([elt.id for elt in target.elts if isinstance(elt, ast.Name)])
+      elif isinstance(target, ast.Name):
+        target_names.append(target.id)
+
+    # Modify the assignment to include print calls
+    if isinstance(node.value, ast.Call):
+      assign_call = node.value
+      new_assign = ast.Assign(
+        targets=node.targets,
+        value=ast.Call(
+          func=ast.Name(id='__ParallelWorker_run__', ctx=ast.Load()),
+          args=[
+            ast.Constant(len(target_names)),
+            assign_call.func,
+            *assign_call.args
+          ],
+          keywords=assign_call.keywords
+        )
+      )
+      ast.fix_missing_locations(new_assign)
+      return new_assign
+    return node
+
+  def visit_For(self, node: ast.For):
+    """
+    Finds all for loops that append to arrays, converting them to __ParallelWorker_map__
+    """
+    if isinstance(node.iter, ast.Name):
+      being_iterated = copy.deepcopy([node.iter])
+    else:
+      being_iterated = copy.deepcopy(node.iter.args)
+
+    if isinstance(node.target, ast.Name):
+      itElNames = [node.target.id]
+    else:
+      itElNames = [n.id for n in node.target.elts]
+
+    appending = {}
+    funName = None
+
+    for el in node.body:
+      match el:
+        case ast.Expr():
+          # Filter for only '.append' functions
+          if not isinstance(el.value, ast.Call) or el.value.func.attr != 'append':
+            continue
+          appending[el.value.args[0].id] = copy.deepcopy(el.value.func.value)
+          appending[el.value.args[0].id].ctx = ast.Load()
+        case ast.Assign():
+          if funName: # If there already was a function, then it can't be valid
+            funName = None
+            break
+          if not isinstance(el.value, ast.Call):
+            continue
+          funName = copy.deepcopy(el.value.func)
+          args = copy.deepcopy(el.value.args)
+          if isinstance(el.targets[0], ast.Name):
+            assigns = el.targets[0]
+          else:
+            assigns = el.targets[0].elts
+
+    # If the signature doesn't match at all then skip
+    if not appending or not funName or len(node.body) > 10:
+      return self.generic_visit(node)
+
+    argMap = {a: b for a, b in zip(itElNames, being_iterated)}
+    assigns = [appending[a.id] for a in assigns if a.id in appending]
+
+    args =  [ast.Constant(len(assigns))] + [funName] + [argMap.get(a.id if isinstance(a, ast.Name) else 0, a) for a in args]
+
+    for arg in args:
+      arg.ctx = ast.Load()
+
+    for assign in assigns:
+      assign.ctx = ast.Store()
+
+    funName.ctx = ast.Load()
+
+    # Create the map call node and return it
+    if len(assigns) != 1:
+      assigns = [ast.Tuple(
+        elts=assigns,
+        ctx=ast.Store()
+      )]
+
+    workerMap = ast.Assign(
+      targets=assigns,
+      value=ast.Call(
+        func=ast.Name(id="__ParallelWorker_map__", ctx=ast.Load()),
+        args=args,
+        keywords=[]
+      )
+    )
+
+    if_condition=ast.Call(
+      func=ast.Name(id="isinstance", ctx=ast.Load()),
+      args=[
+        funName,
+        ast.Name(id="ParallelFunction", ctx=ast.Load())
+      ],
+      keywords=[]
+    )
+    if_node = ast.If(
+      test=if_condition,
+      body=[workerMap],
+      orelse=[node]
+    )
+
+    ast.fix_missing_locations(if_node)
+    return if_node
+
+class ParallelFunction:
+  """Decorator for a parallel function"""
+  def __init__(self, fun):
+    self._fun = fun
+
+  def __call__(self, *args, **kwargs):
+    return self._fun(*args, **kwargs)
+
+  @staticmethod
+  def _run_function(fun, *args, **kwargs):
+    return fun(*args, **kwargs)
+
+  def run_parallel(self, workers: int = 8, *args, **kwargs):
+    """Run the function in parallel
+
+    Args:
+        workers (int, optional): Number of worker processes to use. Defaults to 8.
+    """
+    pw = ParallelWorker(workers)
+
+    # Recurse through the function and find all parallelizable functions
+    func_globals = self._convert_to_pw(pw)
+    ret = func_globals[self._fun.__name__](*args, **kwargs)
+
+    # Execute all parallel functions
+    pw.execute()
+    
+    # Unwind and replace retvals
+    def replace_retvals(el):
+      if isinstance(el, Retvals):
+        return el.ret()
+      elif isinstance(el, dict):
+        return {k: replace_retvals(v) for k, v in el.items()}
+      elif isinstance(el, list):
+        return list(map(replace_retvals, el))
+      elif isinstance(el, tuple):
+        return tuple(map(replace_retvals, el))
+      return el
+    
+    return replace_retvals(ret)
+
+  def _convert_to_pw(self, pw: ParallelWorker):
+    source_lines = inspect.getsourcelines(self._fun)[0]
+    source = ''.join([line for line in source_lines if not line.startswith('@')])
+
+    tree = ast.parse(source)
+
+    # Walk the parsed AST and add calls to ParallelWorkers
+    transformer = ParallelFunctionTransformer(pw)
+    transformed_tree = transformer.visit(tree)
+
+    ast.fix_missing_locations(transformed_tree)
+
+    # Recompile to bytecode
+    compiled_code = compile(transformed_tree, filename="<ast>", mode="exec")
+    func_globals = self._fun.__globals__.copy()
+
+    def worker_run(nret, fun, *args):
+      # Check whether it's a parallel function, otherwise treat it normally
+      if isinstance(fun, ParallelFunction):
+        retvals = pw.run(fun, *args)
+        if nret > 1:
+          return retvals[:nret]
+        return retvals
+      return fun(*args)
+
+    def worker_map(nret, fun, *args):
+      # Check whether it's a parallel function, otherwise treat it normally
+      if isinstance(fun, ParallelFunction):
+        retvals = pw.map(fun, *args)
+        if nret > 1:
+          return retvals[:nret]
+        return retvals
+      return fun(*args)
+    
+    # Inject __ParallelWorker... definitions
+    func_globals = {**func_globals, '__ParallelWorker_run__': worker_run, '__ParallelWorker_map__': worker_map}
+    exec(compiled_code, func_globals)
+
+    return func_globals
+
+T = TypeVar("T")
+
+def parallel_function(fun: T) -> T:
+  return ParallelFunction(fun)
\ No newline at end of file

From 280ec356a327a8ccee9e176bd0545d056fb4957e Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 20 Dec 2024 18:21:51 +0100
Subject: [PATCH 85/87] Formatting

---
 calibration_utils.py | 715 +++++++++++++++++++++++++++++--------------
 worker.py            | 159 +++++-----
 2 files changed, 575 insertions(+), 299 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index b15bd88..7261be5 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -9,9 +9,11 @@
 
 PER_CCM = True
 EXTRINSICS_PER_CCM = False
-colors = [(0, 255 , 0), (0, 0, 255)]
+colors = [(0, 255, 0), (0, 0, 255)]
+
 
 class StereoExceptions(Exception):
+
   def __init__(self, message, stage, path=None, *args, **kwargs) -> None:
     self.stage = stage
     self.path = path
@@ -24,24 +26,34 @@ def summary(self) -> str:
     """
     return f"'{self.args[0]}' (occured during stage '{self.stage}')"
 
+
 @parallel_function
-def estimate_pose_and_filter(camData: CameraData, corners, ids, charucoBoard: CharucoBoard):
+def estimate_pose_and_filter(camData: CameraData, corners, ids,
+                             charucoBoard: CharucoBoard):
   """Very rough corner filtering on a single image"""
 
   objpoints = charucoBoard.board.chessboardCorners[ids]
 
-  ini_threshold=5
+  ini_threshold = 5
   threshold = None
 
   objects = []
   all_objects = []
-  while len(objects) < len(objpoints[:,0,0]) * camData['min_inliers']:
+  while len(objects) < len(objpoints[:, 0, 0]) * camData['min_inliers']:
     if ini_threshold > camData['max_threshold']:
       break
-    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, camData['intrinsics'], camData['dist_coeff'], flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+    ret, rvec, tvec, objects = cv2.solvePnPRansac(
+        objpoints,
+        corners,
+        camData['intrinsics'],
+        camData['dist_coeff'],
+        flags=cv2.SOLVEPNP_P3P,
+        reprojectionError=ini_threshold,
+        iterationsCount=10000,
+        confidence=0.9)
 
     if not ret:
-      raise RuntimeError('Exception') # TODO : Handle
+      raise RuntimeError('Exception')  # TODO : Handle
 
     all_objects.append(objects)
     imgpoints2 = objpoints.copy()
@@ -50,80 +62,100 @@ def estimate_pose_and_filter(camData: CameraData, corners, ids, charucoBoard: Ch
     all_corners2 = np.array([all_corners2[id[0]] for id in objects])
     imgpoints2 = np.array([imgpoints2[id[0]] for id in objects])
 
-    ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2, camData['intrinsics'], camData['dist_coeff'])
+    ret, rvec, tvec = cv2.solvePnP(imgpoints2, all_corners2,
+                                   camData['intrinsics'],
+                                   camData['dist_coeff'])
 
     ini_threshold += camData['threshold_stepper']
 
-  if ids.size > 0 and corners.size > 0: # TODO : Try to remove the np reshaping
+  if ids.size > 0 and corners.size > 0:  # TODO : Try to remove the np reshaping
     ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-    imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec, camData['intrinsics'], camData['dist_coeff'])
+    imgpoints2, _ = cv2.projectPoints(objpoints, rvec, tvec,
+                                      camData['intrinsics'],
+                                      camData['dist_coeff'])
     corners2 = corners.reshape(-1, 2)
     imgpoints2 = imgpoints2.reshape(-1, 2)
 
     errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
     if threshold == None:
-      threshold = max(2*np.median(errors), 150)
+      threshold = max(2 * np.median(errors), 150)
     valid_mask = errors <= threshold
     removed_mask = ~valid_mask
 
     valid_ids = ids[valid_mask]
 
-    return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(-1, 1).astype(np.int32), corners2[removed_mask]
+    return corners2[valid_mask].reshape(-1, 1, 2), valid_ids.reshape(
+        -1, 1).astype(np.int32), corners2[removed_mask]
+
 
 @parallel_function
 def detect_charuco_board(image: np.array, board: CharucoBoard):
   arucoParams = cv2.aruco.DetectorParameters_create()
   arucoParams.minMarkerDistanceRate = 0.01
-  markers, marker_ids, rejectedImgPoints = cv2.aruco.detectMarkers(image, board.dictionary, parameters=arucoParams)  # First, detect markers
-  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(image, board.board, markers, marker_ids, rejectedCorners=rejectedImgPoints)
-  criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000, 0.00001)
+  markers, marker_ids, rejectedImgPoints = cv2.aruco.detectMarkers(
+      image, board.dictionary, parameters=arucoParams)  # First, detect markers
+  marker_corners, marker_ids, refusd, recoverd = cv2.aruco.refineDetectedMarkers(
+      image,
+      board.board,
+      markers,
+      marker_ids,
+      rejectedCorners=rejectedImgPoints)
+  criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 10000,
+              0.00001)
 
   # If found, add object points, image points (after refining them)
-  if len(marker_corners)>0:
-    num_corners, corners, ids = cv2.aruco.interpolateCornersCharuco(marker_corners,marker_ids, image, board.board, minMarkers = 1)
+  if len(marker_corners) > 0:
+    num_corners, corners, ids = cv2.aruco.interpolateCornersCharuco(
+        marker_corners, marker_ids, image, board.board, minMarkers=1)
     if corners is not None and ids is not None and len(corners) > 3:
-      corners = cv2.cornerSubPix(image, corners,
-                              winSize=(5, 5),
-                              zeroZone=(-1, -1),
-                              criteria=criteria)
+      corners = cv2.cornerSubPix(image,
+                                 corners,
+                                 winSize=(5, 5),
+                                 zeroZone=(-1, -1),
+                                 criteria=criteria)
     return corners, ids
   else:
     raise RuntimeError('Failed to detect corners on image')
 
+
 @parallel_function
 def get_features(config: CalibrationConfig, camData: CameraData) -> CameraData:
-  f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
+  f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"] / 2)))
 
-  camData['intrinsics'] = np.array([
-     [f,     0.0,    camData['size'][0]/2],
-     [0.0,   f,      camData['size'][1]/2],
-     [0.0,   0.0,    1.0]
-  ])
+  camData['intrinsics'] = np.array([[f, 0.0, camData['size'][0] / 2],
+                                    [0.0, f, camData['size'][1] / 2],
+                                    [0.0, 0.0, 1.0]])
   camData['dist_coeff'] = np.zeros((12, 1))
 
-   # check if there are any suspicious corners with high reprojection error
-  max_threshold = 75 + config.initialMaxThreshold * (camData['hfov']/ 30 + camData['size'][1] / 800 * 0.2)
-  threshold_stepper = int(1.5 * (camData['hfov'] / 30 + camData['size'][1] / 800))
+  # check if there are any suspicious corners with high reprojection error
+  max_threshold = 75 + config.initialMaxThreshold * (
+      camData['hfov'] / 30 + camData['size'][1] / 800 * 0.2)
+  threshold_stepper = int(1.5 *
+                          (camData['hfov'] / 30 + camData['size'][1] / 800))
   if threshold_stepper < 1:
     threshold_stepper = 1
-  min_inliers = 1 - config.initialMinFiltered * (camData['hfov'] / 60 + camData['size'][1] / 800 * 0.2)
+  min_inliers = 1 - config.initialMinFiltered * (
+      camData['hfov'] / 60 + camData['size'][1] / 800 * 0.2)
   camData['max_threshold'] = max_threshold
   camData['threshold_stepper'] = threshold_stepper
   camData['min_inliers'] = min_inliers
 
   return camData
 
+
 @parallel_function
-def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset):
+def calibrate_charuco(camData: CameraData, allCorners, allIds,
+                      dataset: Dataset):
   # TODO : If we still need this check it needs to be elsewhere
   # if sum([len(corners) < 4 for corners in filteredCorners]) > 0.15 * len(filteredCorners):
   #   raise RuntimeError(f"More than 1/4 of images has less than 4 corners for {cam_info['name']}")
 
   distortion_flags = get_distortion_flags(camData['distortion_model'])
   flags = cv2.CALIB_USE_INTRINSIC_GUESS + distortion_flags
-  
+
   # Convert to int32 from uint32 # TODO : Shouldn't be necessary
-  for i, ids in enumerate(allIds): allIds[i] = ids.reshape(-1, 1).astype(np.int32)
+  for i, ids in enumerate(allIds):
+    allIds[i] = ids.reshape(-1, 1).astype(np.int32)
 
   # Filter for only images with >6 corners # TODO : Shouldn't be in here, should be in a separate necessary filtering function
   allCorners2 = []
@@ -136,18 +168,17 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
     allIds2.append(ids)
 
   #try:
-  (ret, camera_matrix, distortion_coefficients,
-       rotation_vectors, translation_vectors,
-       stdDeviationsIntrinsics, stdDeviationsExtrinsics,
-       perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-        charucoCorners=allCorners2,
-        charucoIds=allIds2,
-        board=dataset.board.board,
-        imageSize=camData['size'],
-        cameraMatrix=camData['intrinsics'],
-        distCoeffs=camData['dist_coeff'],
-        flags=flags,
-        criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
+  (ret, camera_matrix, distortion_coefficients, rotation_vectors,
+   translation_vectors, stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+   perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
+       charucoCorners=allCorners2,
+       charucoIds=allIds2,
+       board=dataset.board.board,
+       imageSize=camData['size'],
+       cameraMatrix=camData['intrinsics'],
+       distCoeffs=camData['dist_coeff'],
+       flags=flags,
+       criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 1000, 1e-6))
   #except:
   #  return f"First intrisic calibration failed for {cam_info['size']}", None, None
 
@@ -157,19 +188,23 @@ def calibrate_charuco(camData: CameraData, allCorners, allIds, dataset: Dataset)
   camData['filtered_ids'] = allIds2
   return camData
 
+
 @parallel_function
-def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features, all_ids):
-  f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"]/2)))
+def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features,
+                            all_ids):
+  f = camData['size'][0] / (2 * np.tan(np.deg2rad(camData["hfov"] / 2)))
   print("INTRINSIC CALIBRATION")
-  cameraIntrinsics = np.array([[f,  0.0,    camData['size'][0]/2],
-                 [0.0,   f,    camData['size'][1]/2],
-                 [0.0,   0.0,    1.0]])
+  cameraIntrinsics = np.array([[f, 0.0, camData['size'][0] / 2],
+                               [0.0, f, camData['size'][1] / 2],
+                               [0.0, 0.0, 1.0]])
 
   distCoeff = np.zeros((12, 1))
 
   if camData["name"] in intrinsic_img:
     raise RuntimeError('This is broken')
-    all_features, all_ids, filtered_images = remove_features(filtered_features, filtered_ids, intrinsic_img[camData["name"]], image_files)
+    all_features, all_ids, filtered_images = remove_features(
+        filtered_features, filtered_ids, intrinsic_img[camData["name"]],
+        image_files)
   else:
     filtered_images = camData['images_path']
 
@@ -183,38 +218,43 @@ def filter_features_fisheye(camData: CameraData, intrinsic_img, all_features, al
 
   return camData
 
+
 @parallel_function
-def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig, camData: CameraData, dataset: Dataset):
+def calibrate_ccm_intrinsics_per_ccm(config: CalibrationConfig,
+                                     camData: CameraData, dataset: Dataset):
   start = time.time()
   print('starting calibrate_wf')
-  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(config, camData, dataset)
+  ret, cameraIntrinsics, distCoeff, _, _, filtered_ids, filtered_corners, size, coverageImage, all_corners, all_ids = calibrate_wf_intrinsics(
+      config, camData, dataset)
   if isinstance(ret, str) and all_ids is None:
-    raise RuntimeError('Exception' + ret) # TODO : Handle
+    raise RuntimeError('Exception' + ret)  # TODO : Handle
   print(f'calibrate_wf took {round(time.time() - start, 2)}s')
 
   camData['intrinsics'] = cameraIntrinsics
   camData['dist_coeff'] = distCoeff
   camData['reprojection_error'] = ret
-  print("Reprojection error of {0}: {1}".format(
-    camData['name'], ret))
+  print("Reprojection error of {0}: {1}".format(camData['name'], ret))
   return camData, filtered_corners, filtered_ids
 
+
 def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
   imsize = camData['size']
   hfov = camData['hfov']
   name = camData['name']
-  allCorners = camData['filtered_corners'] # TODO : I don't think this has a way to get here from one of the codepaths in matin in the else:
+  allCorners = camData[
+      'filtered_corners']  # TODO : I don't think this has a way to get here from one of the codepaths in matin in the else:
   allIds = camData['filtered_ids']
   calib_model = camData['calib_model']
   distortionModel = camData['distortion_model']
-  
+
   coverageImage = np.ones(imsize[::-1], np.uint8) * 255
   coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
   coverageImage = draw_corners(allCorners, coverageImage)
   if calib_model == 'perspective':
     distortion_flags = get_distortion_flags(distortionModel)
-    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-      allCorners, allIds, imsize, distortion_flags, camData['intrinsics'], camData['dist_coeff'])
+    ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds = calibrate_camera_charuco(
+        allCorners, allIds, imsize, distortion_flags, camData['intrinsics'],
+        camData['dist_coeff'])
     # undistort_visualization(
     #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
 
@@ -222,33 +262,40 @@ def calibrate_ccm_intrinsics(config: CalibrationConfig, camData: CameraData):
   else:
     print('Fisheye--------------------------------------------------')
     ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
-      config, allCorners, allIds, imsize, hfov, name)
+        config, allCorners, allIds, imsize, hfov, name)
     # undistort_visualization(
     #   self, image_files, camera_matrix, distortion_coefficients, imsize, name)
     print('Fisheye rotation vector', rotation_vectors[0])
     print('Fisheye translation vector', translation_vectors[0])
-  
+
   camData['filtered_ids'] = filtered_ids
   camData['filtered_corners'] = filtered_corners
 
   camData['intrinsics'] = camera_matrix
   camData['dist_coeff'] = distortion_coefficients
   camData['reprojection_error'] = ret
-  print("Reprojection error of {0}: {1}".format(
-    camData['name'], ret))
+  print("Reprojection error of {0}: {1}".format(camData['name'], ret))
 
   return camData
 
+
 @parallel_function
-def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, allLeftCorners, allRightCorners, leftCamData: CameraData, rightCamData: CameraData):
-  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = leftCamData['intrinsics'], leftCamData['dist_coeff'], rightCamData['intrinsics'], rightCamData['dist_coeff'], leftCamData['distortion_model']
+def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts,
+                                 allLeftCorners, allRightCorners,
+                                 leftCamData: CameraData,
+                                 rightCamData: CameraData):
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, left_distortion_model = leftCamData[
+      'intrinsics'], leftCamData['dist_coeff'], rightCamData[
+          'intrinsics'], rightCamData['dist_coeff'], leftCamData[
+              'distortion_model']
   specTranslation = leftCamData['extrinsics']['specTranslation']
   rot = leftCamData['extrinsics']['rotation']
 
   t_in = np.array(
-    [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-  r_in = Rotation.from_euler(
-    'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+      [specTranslation['x'], specTranslation['y'], specTranslation['z']],
+      dtype=np.float32)
+  r_in = Rotation.from_euler('xyz', [rot['r'], rot['p'], rot['y']],
+                             degrees=True).as_matrix().astype(np.float32)
 
   flags = 0
   # flags |= cv2.CALIB_USE_EXTRINSIC_GUESS
@@ -258,9 +305,18 @@ def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, allLeftCorn
   flags += distortion_flags
   # print(flags)
   ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-  obj_pts, allLeftCorners, allRightCorners,
-  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-  R=r_in, T=t_in, criteria=config.stereoCalibCriteria, flags=flags)
+      obj_pts,
+      allLeftCorners,
+      allRightCorners,
+      cameraMatrix_l,
+      distCoeff_l,
+      cameraMatrix_r,
+      distCoeff_r,
+      None,
+      R=r_in,
+      T=t_in,
+      criteria=config.stereoCalibCriteria,
+      flags=flags)
 
   r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
   print(f'Epipolar error is {ret}')
@@ -270,11 +326,8 @@ def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, allLeftCorn
   print(f'Euler angles in XYZ {r_euler} degs')
 
   R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
-    cameraMatrix_l,
-    distCoeff_l,
-    cameraMatrix_r,
-    distCoeff_r,
-    None, R, T) # , alpha=0.1
+      cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None, R,
+      T)  # , alpha=0.1
   # self.P_l = P_l
   # self.P_r = P_r
   r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
@@ -282,25 +335,41 @@ def calibrate_stereo_perspective(config: CalibrationConfig, obj_pts, allLeftCorn
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
+
 @parallel_function
-def calibrate_stereo_perspective_per_ccm(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
-  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
+def calibrate_stereo_perspective_per_ccm(config: CalibrationConfig, obj_pts,
+                                         left_corners_sampled,
+                                         right_corners_sampled, left_cam_info,
+                                         right_cam_info):
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info[
+      'intrinsics'], left_cam_info['dist_coeff'], right_cam_info[
+          'intrinsics'], right_cam_info['dist_coeff']
   specTranslation = left_cam_info['extrinsics']['specTranslation']
   rot = left_cam_info['extrinsics']['rotation']
 
   t_in = np.array(
-    [specTranslation['x'], specTranslation['y'], specTranslation['z']], dtype=np.float32)
-  r_in = Rotation.from_euler(
-    'xyz', [rot['r'], rot['p'], rot['y']], degrees=True).as_matrix().astype(np.float32)
+      [specTranslation['x'], specTranslation['y'], specTranslation['z']],
+      dtype=np.float32)
+  r_in = Rotation.from_euler('xyz', [rot['r'], rot['p'], rot['y']],
+                             degrees=True).as_matrix().astype(np.float32)
 
   flags = cv2.CALIB_FIX_INTRINSIC
   ret, M1, d1, M2, d2, R, T, E, F, _ = cv2.stereoCalibrateExtended(
-  obj_pts, left_corners_sampled, right_corners_sampled,
-  np.eye(3), np.zeros(12), np.eye(3), np.zeros(12), None,
-  R=r_in, T=t_in, criteria=config.stereoCalibCriteria , flags=flags)
+      obj_pts,
+      left_corners_sampled,
+      right_corners_sampled,
+      np.eye(3),
+      np.zeros(12),
+      np.eye(3),
+      np.zeros(12),
+      None,
+      R=r_in,
+      T=t_in,
+      criteria=config.stereoCalibCriteria,
+      flags=flags)
 
   r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
-  scale = ((cameraMatrix_l[0][0]*cameraMatrix_r[0][0]))
+  scale = ((cameraMatrix_l[0][0] * cameraMatrix_r[0][0]))
   print(f'Epipolar error without scale: {ret}')
   print(f'Epipolar error with scale: {ret*np.sqrt(scale)}')
   print('Printing Extrinsics res...')
@@ -308,11 +377,8 @@ def calibrate_stereo_perspective_per_ccm(config: CalibrationConfig, obj_pts, lef
   print(T)
   print(f'Euler angles in XYZ {r_euler} degs')
   R_l, R_r, P_l, P_r, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
-    cameraMatrix_l,
-    distCoeff_l,
-    cameraMatrix_r,
-    distCoeff_r,
-    None, R, T) # , alpha=0.1
+      cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None, R,
+      T)  # , alpha=0.1
   # self.P_l = P_l
   # self.P_r = P_r
   r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
@@ -322,14 +388,23 @@ def calibrate_stereo_perspective_per_ccm(config: CalibrationConfig, obj_pts, lef
   # print(f'P_r is \n {P_r}')
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
+
 @parallel_function
-def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info):
-  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info['intrinsics'], left_cam_info['dist_coeff'], right_cam_info['intrinsics'], right_cam_info['dist_coeff']
+def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts,
+                             left_corners_sampled, right_corners_sampled,
+                             left_cam_info, right_cam_info):
+  cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r = left_cam_info[
+      'intrinsics'], left_cam_info['dist_coeff'], right_cam_info[
+          'intrinsics'], right_cam_info['dist_coeff']
   # make sure all images have the same *number of* points
   min_num_points = min([len(pts) for pts in obj_pts])
   obj_pts_truncated = [pts[:min_num_points] for pts in obj_pts]
-  left_corners_truncated = [pts[:min_num_points] for pts in left_corners_sampled]
-  right_corners_truncated = [pts[:min_num_points] for pts in right_corners_sampled]
+  left_corners_truncated = [
+      pts[:min_num_points] for pts in left_corners_sampled
+  ]
+  right_corners_truncated = [
+      pts[:min_num_points] for pts in right_corners_sampled
+  ]
 
   flags = 0
   # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
@@ -345,33 +420,44 @@ def calibrate_stereo_fisheye(config: CalibrationConfig, obj_pts, left_corners_sa
   # flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
   # flags = cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW
   (ret, M1, d1, M2, d2, R, T), E, F = cv2.fisheye.stereoCalibrate(
-    obj_pts_truncated, left_corners_truncated, right_corners_truncated,
-    cameraMatrix_l, distCoeff_l, cameraMatrix_r, distCoeff_r, None,
-    flags=flags, criteria=config.stereoCalibCriteria), None, None
+      obj_pts_truncated,
+      left_corners_truncated,
+      right_corners_truncated,
+      cameraMatrix_l,
+      distCoeff_l,
+      cameraMatrix_r,
+      distCoeff_r,
+      None,
+      flags=flags,
+      criteria=config.stereoCalibCriteria), None, None
   r_euler = Rotation.from_matrix(R).as_euler('xyz', degrees=True)
   print(f'Reprojection error is {ret}')
   isHorizontal = np.absolute(T[0]) > np.absolute(T[1])
 
-  R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify(
-    cameraMatrix_l,
-    distCoeff_l,
-    cameraMatrix_r,
-    distCoeff_r,
-    None, R, T, flags=0)
+  R_l, R_r, P_l, P_r, Q = cv2.fisheye.stereoRectify(cameraMatrix_l,
+                                                    distCoeff_l,
+                                                    cameraMatrix_r,
+                                                    distCoeff_r,
+                                                    None,
+                                                    R,
+                                                    T,
+                                                    flags=0)
 
   r_euler = Rotation.from_matrix(R_l).as_euler('xyz', degrees=True)
   r_euler = Rotation.from_matrix(R_r).as_euler('xyz', degrees=True)
 
   return [ret, R, T, R_l, R_r, P_l, P_r]
 
+
 @parallel_function
-def find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds, board: CharucoBoard):
+def find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds,
+                                board: CharucoBoard):
   left_corners_sampled = []
   right_corners_sampled = []
   obj_pts = []
   failed = 0
 
-  for i, _ in enumerate(leftIds): # For ids in all images
+  for i, _ in enumerate(leftIds):  # For ids in all images
     commonIds = np.intersect1d(leftIds[i], rightIds[i])
     left_sub_corners = leftCorners[i][np.isin(leftIds[i], commonIds)]
     right_sub_corners = rightCorners[i][np.isin(rightIds[i], commonIds)]
@@ -385,39 +471,58 @@ def find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds, bo
       failed += 1
 
   if failed > len(leftIds) / 3:
-    raise RuntimeError('More than 1/3 of images had less than 6 common features found')
+    raise RuntimeError(
+        'More than 1/3 of images had less than 6 common features found')
 
   return left_corners_sampled, right_corners_sampled, obj_pts
 
+
 @parallel_function
 def undistort_points_perspective(allCorners, camInfo):
-  return [cv2.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
+  return [
+      cv2.undistortPoints(np.array(corners),
+                          camInfo['intrinsics'],
+                          camInfo['dist_coeff'],
+                          P=camInfo['intrinsics']) for corners in allCorners
+  ]
+
 
 @parallel_function
 def undistort_points_fisheye(allCorners, camInfo):
-  return [cv2.fisheye.undistortPoints(np.array(corners), camInfo['intrinsics'], camInfo['dist_coeff'], P=camInfo['intrinsics']) for corners in allCorners]
+  return [
+      cv2.fisheye.undistortPoints(np.array(corners),
+                                  camInfo['intrinsics'],
+                                  camInfo['dist_coeff'],
+                                  P=camInfo['intrinsics'])
+      for corners in allCorners
+  ]
+
 
 @parallel_function
-def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraData, left_cam: Dataset, right_cam: Dataset, board_config, extrinsics):
+def calculate_epipolar_error(left_cam_info: CameraData,
+                             right_cam_info: CameraData, left_cam: Dataset,
+                             right_cam: Dataset, board_config, extrinsics):
   if extrinsics[0] == -1:
     return -1, extrinsics[1]
   stereoConfig = None
   if 'stereo_config' in board_config:
-    leftCamName = board_config['cameras'][board_config['stereo_config']['left_cam']]['name']
-    rightCamName = board_config['cameras'][board_config['stereo_config']['right_cam']]['name']
-    if leftCamName == left_cam.name and rightCamName == right_cam.name: # TODO : Is this supposed to take the last camera pair?
+    leftCamName = board_config['cameras'][board_config['stereo_config']
+                                          ['left_cam']]['name']
+    rightCamName = board_config['cameras'][board_config['stereo_config']
+                                           ['right_cam']]['name']
+    if leftCamName == left_cam.name and rightCamName == right_cam.name:  # TODO : Is this supposed to take the last camera pair?
       stereoConfig = {
-        'rectification_left': extrinsics[3],
-        'rectification_right': extrinsics[4]
+          'rectification_left': extrinsics[3],
+          'rectification_right': extrinsics[4]
       }
     elif leftCamName == right_cam and rightCamName == left_cam:
       stereoConfig = {
-        'rectification_left': extrinsics[4],
-        'rectification_right': extrinsics[3]
+          'rectification_left': extrinsics[4],
+          'rectification_right': extrinsics[3]
       }
 
   print('<-------------Epipolar error of {} and {} ------------>'.format(
-    left_cam_info['name'], right_cam_info['name']))
+      left_cam_info['name'], right_cam_info['name']))
   #print(f"dist {left_cam_info['name']}: {left_cam_info['dist_coeff']}")
   #print(f"dist {right_cam_info['name']}: {right_cam_info['dist_coeff']}")
   if left_cam_info['intrinsics'][0][0] < right_cam_info['intrinsics'][0][0]:
@@ -425,9 +530,13 @@ def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraDa
   else:
     scale = left_cam_info['intrinsics'][0][0]
   if PER_CCM and EXTRINSICS_PER_CCM:
-    scale = ((left_cam_info['intrinsics'][0][0]*right_cam_info['intrinsics'][0][0] + left_cam_info['intrinsics'][1][1]*right_cam_info['intrinsics'][1][1])/2)
+    scale = (
+        (left_cam_info['intrinsics'][0][0] * right_cam_info['intrinsics'][0][0]
+         + left_cam_info['intrinsics'][1][1] *
+         right_cam_info['intrinsics'][1][1]) / 2)
     print(f"Epipolar error {extrinsics[0]*np.sqrt(scale)}")
-    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]*np.sqrt(scale)
+    left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0] * np.sqrt(
+        scale)
   else:
     print(f"Epipolar error {extrinsics[0]}")
     left_cam_info['extrinsics']['epipolar_error'] = extrinsics[0]
@@ -437,6 +546,7 @@ def calculate_epipolar_error(left_cam_info: CameraData, right_cam_info: CameraDa
 
   return left_cam_info, stereoConfig
 
+
 def get_distortion_flags(distortionModel: DistortionModel):
   if distortionModel == None:
     print("Use DEFAULT model")
@@ -504,7 +614,9 @@ def get_distortion_flags(distortionModel: DistortionModel):
     flags = distortionModel
   return flags
 
-def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData, dataset: Dataset):
+
+def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData,
+                            dataset: Dataset):
   name = camData['name']
   allCorners = camData['filtered_corners']
   allIds = camData['filtered_ids']
@@ -514,29 +626,32 @@ def calibrate_wf_intrinsics(config: CalibrationConfig, camData: CameraData, data
   distortionModel = camData['distortion_model']
   cameraIntrinsics = camData['intrinsics']
   distCoeff = camData['dist_coeff']
-  
+
   coverageImage = np.ones(imsize[::-1], np.uint8) * 255
   coverageImage = cv2.cvtColor(coverageImage, cv2.COLOR_GRAY2BGR)
   coverageImage = draw_corners(allCorners, coverageImage)
   if calib_model == 'perspective':
     distortion_flags = get_distortion_flags(distortionModel)
-    ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds  = calibrate_camera_charuco(
-      allCorners, allIds, imsize, distortion_flags, cameraIntrinsics, distCoeff, dataset)
+    ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds = calibrate_camera_charuco(
+        allCorners, allIds, imsize, distortion_flags, cameraIntrinsics,
+        distCoeff, dataset)
 
     return ret, cameraIntrinsics, distCoeff, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
   else:
     print('Fisheye--------------------------------------------------')
     ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners = calibrate_fisheye(
-      config, allCorners, allIds, imsize, hfov, name)
+        config, allCorners, allIds, imsize, hfov, name)
     print('Fisheye rotation vector', rotation_vectors[0])
     print('Fisheye translation vector', translation_vectors[0])
 
     # (Height, width)
     return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, imsize, coverageImage, allCorners, allIds
 
+
 def draw_corners(charuco_corners, displayframe):
   for corners in charuco_corners:
-    color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)), int(np.random.randint(0, 255)))
+    color = (int(np.random.randint(0, 255)), int(np.random.randint(0, 255)),
+             int(np.random.randint(0, 255)))
     for corner in corners:
       corner_int = (int(corner[0][0]), int(corner[0][1]))
       cv2.circle(displayframe, corner_int, 4, color, -1)
@@ -551,7 +666,15 @@ def draw_corners(charuco_corners, displayframe):
   cv2.line(displayframe, start_point, end_point, color, thickness)
   return displayframe
 
-def features_filtering_function(rvecs, tvecs, cameraMatrix, distCoeffs, filtered_corners,filtered_id, dataset: Dataset, threshold = None):
+
+def features_filtering_function(rvecs,
+                                tvecs,
+                                cameraMatrix,
+                                distCoeffs,
+                                filtered_corners,
+                                filtered_id,
+                                dataset: Dataset,
+                                threshold=None):
   whole_error = []
   all_points = []
   all_corners = []
@@ -568,28 +691,34 @@ def features_filtering_function(rvecs, tvecs, cameraMatrix, distCoeffs, filtered
   for i, (corners, ids) in enumerate(zip(filtered_corners, filtered_id)):
     if ids is not None and corners.size > 0:
       ids = ids.flatten()  # Flatten the IDs from 2D to 1D
-      objPoints = np.array([dataset.board.board.chessboardCorners[id] for id in ids], dtype=np.float32)
-      imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i], cameraMatrix, distCoeffs)
+      objPoints = np.array(
+          [dataset.board.board.chessboardCorners[id] for id in ids],
+          dtype=np.float32)
+      imgpoints2, _ = cv2.projectPoints(objPoints, rvecs[i], tvecs[i],
+                                        cameraMatrix, distCoeffs)
       corners2 = corners.reshape(-1, 2)
       imgpoints2 = imgpoints2.reshape(-1, 2)
 
       errors = np.linalg.norm(corners2 - imgpoints2, axis=1)
       if threshold == None:
-        threshold = max(2*np.median(errors), 150)
+        threshold = max(2 * np.median(errors), 150)
       valid_mask = errors <= threshold
       removed_mask = ~valid_mask
 
       # Collect valid IDs in the original format (array of arrays)
       valid_ids = ids[valid_mask]
-      all_ids.append(valid_ids.reshape(-1, 1).astype(np.int32))  # Reshape and store as array of arrays
+      all_ids.append(valid_ids.reshape(-1, 1).astype(
+          np.int32))  # Reshape and store as array of arrays
 
       # Collect data for valid points
       reported_error.extend(errors)
       all_error.extend(errors[valid_mask])
       display_corners.extend(corners2)
       display_points.extend(imgpoints2[valid_mask])
-      all_points.append(imgpoints2[valid_mask])  # Collect valid points for calibration
-      all_corners.append(corners2[valid_mask].reshape(-1, 1, 2))   # Collect valid corners for calibration
+      all_points.append(
+          imgpoints2[valid_mask])  # Collect valid points for calibration
+      all_corners.append(corners2[valid_mask].reshape(
+          -1, 1, 2))  # Collect valid corners for calibration
 
       removed_corners.extend(corners2[removed_mask])
       removed_points.extend(imgpoints2[removed_mask])
@@ -598,20 +727,38 @@ def features_filtering_function(rvecs, tvecs, cameraMatrix, distCoeffs, filtered
 
       total_error_squared = np.sum(errors[valid_mask]**2)
       total_points = len(objPoints[valid_mask])
-      rms_error = np.sqrt(total_error_squared / total_points if total_points else 0)
+      rms_error = np.sqrt(total_error_squared /
+                          total_points if total_points else 0)
       whole_error.append(rms_error)
 
     total_error_squared = 0
     total_points = 0
 
-  return all_corners ,all_ids, all_error, removed_corners, removed_ids, removed_error
+  return all_corners, all_ids, all_error, removed_corners, removed_ids, removed_error
 
-def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K: np.array, d: np.array, ini_threshold = 2, min_inliers = 0.95, threshold_stepper = 1, max_threshold = 50):
+
+def camera_pose_charuco(objpoints: np.array,
+                        corners: np.array,
+                        ids: np.array,
+                        K: np.array,
+                        d: np.array,
+                        ini_threshold=2,
+                        min_inliers=0.95,
+                        threshold_stepper=1,
+                        max_threshold=50):
   objects = []
-  while len(objects) < len(objpoints[:,0,0]) * min_inliers:
+  while len(objects) < len(objpoints[:, 0, 0]) * min_inliers:
     if ini_threshold > max_threshold:
       break
-    ret, rvec, tvec, objects  = cv2.solvePnPRansac(objpoints, corners, K, d, flags = cv2.SOLVEPNP_P3P, reprojectionError = ini_threshold,  iterationsCount = 10000, confidence = 0.9)
+    ret, rvec, tvec, objects = cv2.solvePnPRansac(
+        objpoints,
+        corners,
+        K,
+        d,
+        flags=cv2.SOLVEPNP_P3P,
+        reprojectionError=ini_threshold,
+        iterationsCount=10000,
+        confidence=0.9)
 
     if not ret:
       break
@@ -629,16 +776,24 @@ def camera_pose_charuco(objpoints: np.array, corners: np.array, ids: np.array, K
   if ret:
     return rvec, tvec
   else:
-    raise RuntimeError() # TODO : Handle
+    raise RuntimeError()  # TODO : Handle
+
 
-def compute_reprojection_errors(obj_pts: np.array, img_pts: np.array, K: np.array, dist: np.array, rvec: np.array, tvec: np.array, fisheye = False):
+def compute_reprojection_errors(obj_pts: np.array,
+                                img_pts: np.array,
+                                K: np.array,
+                                dist: np.array,
+                                rvec: np.array,
+                                tvec: np.array,
+                                fisheye=False):
   if fisheye:
     proj_pts, _ = cv2.fisheye.projectPoints(obj_pts, rvec, tvec, K, dist)
   else:
     proj_pts, _ = cv2.projectPoints(obj_pts, rvec, tvec, K, dist)
-  errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis = 1)
+  errs = np.linalg.norm(np.squeeze(proj_pts) - np.squeeze(img_pts), axis=1)
   return errs
 
+
 def undistort_visualization(self, img_list, K, D, img_size, name):
   for index, im in enumerate(img_list):
     # print(im)
@@ -648,54 +803,74 @@ def undistort_visualization(self, img_list, K, D, img_size, name):
       kScaled, _ = cv2.getOptimalNewCameraMatrix(K, D, img_size, 0)
       # print(f'K scaled is \n {kScaled} and size is \n {img_size}')
       # print(f'D Value is \n {D}')
-      map1, map2 = cv2.initUndistortRectifyMap(
-        K, D, np.eye(3), kScaled, img_size, cv2.CV_32FC1)
+      map1, map2 = cv2.initUndistortRectifyMap(K, D, np.eye(3), kScaled,
+                                               img_size, cv2.CV_32FC1)
     else:
-      map1, map2 = cv2.fisheye.initUndistortRectifyMap(
-        K, D, np.eye(3), K, img_size, cv2.CV_32FC1)
+      map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K,
+                                                       img_size, cv2.CV_32FC1)
 
-    undistorted_img = cv2.remap(
-      img, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT)
+    undistorted_img = cv2.remap(img,
+                                map1,
+                                map2,
+                                interpolation=cv2.INTER_LINEAR,
+                                borderMode=cv2.BORDER_CONSTANT)
 
     if index == 0:
       undistorted_file_path = self.data_path + '/' + name + f'_undistorted.png'
       cv2.imwrite(undistorted_file_path, undistorted_img)
 
-def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors):
+
+def filter_corner_outliers(config: CalibrationConfig, allIds, allCorners,
+                           camera_matrix, distortion_coefficients,
+                           rotation_vectors, translation_vectors):
   corners_removed = False
   for i in range(len(allIds)):
     corners = allCorners[i]
     ids = allIds[i]
     objpts = config.board.chessboardCorners[ids]
     if config.cameraModel == "fisheye":
-      errs = compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i], fisheye = True)
+      errs = compute_reprojection_errors(objpts,
+                                         corners,
+                                         camera_matrix,
+                                         distortion_coefficients,
+                                         rotation_vectors[i],
+                                         translation_vectors[i],
+                                         fisheye=True)
     else:
-      errs = compute_reprojection_errors(objpts, corners, camera_matrix, distortion_coefficients, rotation_vectors[i], translation_vectors[i])
-    suspicious_err_thr = max(2*np.median(errs), 100)
+      errs = compute_reprojection_errors(objpts, corners, camera_matrix,
+                                         distortion_coefficients,
+                                         rotation_vectors[i],
+                                         translation_vectors[i])
+    suspicious_err_thr = max(2 * np.median(errs), 100)
     n_offending_pts = np.sum(errs > suspicious_err_thr)
     offending_pts_idxs = np.where(errs > suspicious_err_thr)[0]
     # check if there are offending points and if they form a minority
-    if n_offending_pts > 0 and n_offending_pts < len(corners)/5:
-      print(f"removing {n_offending_pts} offending points with errs {errs[offending_pts_idxs]}")
+    if n_offending_pts > 0 and n_offending_pts < len(corners) / 5:
+      print(
+          f"removing {n_offending_pts} offending points with errs {errs[offending_pts_idxs]}"
+      )
       corners_removed = True
       #remove the offending points
       offset = 0
-      allCorners[i] = np.delete(allCorners[i],offending_pts_idxs, axis = 0)
-      allIds[i] = np.delete(allIds[i],offending_pts_idxs, axis = 0)
+      allCorners[i] = np.delete(allCorners[i], offending_pts_idxs, axis=0)
+      allIds[i] = np.delete(allIds[i], offending_pts_idxs, axis=0)
   return corners_removed, allIds, allCorners
 
-def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags, cameraIntrinsics, distCoeff, dataset: Dataset):
+
+def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags,
+                             cameraIntrinsics, distCoeff, dataset: Dataset):
   """
   Calibrates the camera using the dected corners.
   """
-  threshold = 2 * imsize[1]/800.0
-   # check if there are any suspicious corners with high reprojection error
+  threshold = 2 * imsize[1] / 800.0
+  # check if there are any suspicious corners with high reprojection error
   rvecs = []
   tvecs = []
   index = 0
   for corners, ids in zip(allCorners, allIds):
     objpts = dataset.board.board.chessboardCorners[ids]
-    rvec, tvec = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics, distCoeff)
+    rvec, tvec = camera_pose_charuco(objpts, corners, ids, cameraIntrinsics,
+                                     distCoeff)
     tvecs.append(tvec)
     rvecs.append(rvec)
     index += 1
@@ -709,7 +884,7 @@ def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags, camer
   reprojection = []
   num_threshold = []
   iterations_array = []
-  intrinsic_array = {"f_x": [], "f_y": [], "c_x": [],"c_y": []}
+  intrinsic_array = {"f_x": [], "f_y": [], "c_x": [], "c_y": []}
   distortion_array = {}
   index = 0
   camera_matrix = cameraIntrinsics
@@ -729,12 +904,23 @@ def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags, camer
       intrinsic_array['c_y'].append(camera_matrix[1][2])
       num_threshold.append(threshold)
 
-      translation_array_x.append(np.mean(np.array(translation_vectors).T[0][0]))
-      translation_array_y.append(np.mean(np.array(translation_vectors).T[0][1]))
-      translation_array_z.append(np.mean(np.array(translation_vectors).T[0][2]))
+      translation_array_x.append(np.mean(
+          np.array(translation_vectors).T[0][0]))
+      translation_array_y.append(np.mean(
+          np.array(translation_vectors).T[0][1]))
+      translation_array_z.append(np.mean(
+          np.array(translation_vectors).T[0][2]))
 
       start = time.time()
-      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(rotation_vectors, translation_vectors, camera_matrix, distortion_coefficients, allCorners, allIds, threshold = threshold, dataset=dataset)
+      filtered_corners, filtered_ids, all_error, removed_corners, removed_ids, removed_error = features_filtering_function(
+          rotation_vectors,
+          translation_vectors,
+          camera_matrix,
+          distortion_coefficients,
+          allCorners,
+          allIds,
+          threshold=threshold,
+          dataset=dataset)
       iterations_array.append(index)
       reprojection.append(ret)
       for i in range(len(distortion_coefficients)):
@@ -744,51 +930,59 @@ def calibrate_camera_charuco(allCorners, allIds, imsize, distortion_flags, camer
       print(f"Each filtering {time.time() - start}")
       start = time.time()
       try:
-        (ret, camera_matrix, distortion_coefficients,
-         rotation_vectors, translation_vectors,
-         stdDeviationsIntrinsics, stdDeviationsExtrinsics,
+        (ret, camera_matrix, distortion_coefficients, rotation_vectors,
+         translation_vectors, stdDeviationsIntrinsics, stdDeviationsExtrinsics,
          perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
-          charucoCorners=filtered_corners,
-          charucoIds=filtered_ids,
-          board=dataset.board.board,
-          imageSize=imsize,
-          cameraMatrix=cameraIntrinsics,
-          distCoeffs=distCoeff,
-          flags=flags,
-          criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000, 1e-9))
+             charucoCorners=filtered_corners,
+             charucoIds=filtered_ids,
+             board=dataset.board.board,
+             imageSize=imsize,
+             cameraMatrix=cameraIntrinsics,
+             distCoeffs=distCoeff,
+             flags=flags,
+             criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 50000,
+                       1e-9))
       except:
         print('failed on dataset', dataset.name)
-        raise StereoExceptions(message="Intrisic calibration failed", stage="intrinsic_calibration", element='', id=0)
+        raise StereoExceptions(message="Intrisic calibration failed",
+                               stage="intrinsic_calibration",
+                               element='',
+                               id=0)
       cameraIntrinsics = camera_matrix
       distCoeff = distortion_coefficients
-      threshold = 5 * imsize[1]/800.0
+      threshold = 5 * imsize[1] / 800.0
       print(f"Each calibration {time.time()-start}")
       index += 1
-      if  index > 5: #or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
+      if index > 5:  #or (previous_ids == removed_ids and len(previous_ids) >= len(removed_ids) and index > 2):
         print(f"Whole procedure: {time.time() - whole}")
         break
       #previous_ids = removed_ids
   return ret, camera_matrix, distortion_coefficients, rotation_vectors, translation_vectors, filtered_ids, filtered_corners, allCorners, allIds
 
+
 @parallel_function
-def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfov, name):
-  f_init = imsize[0]/np.deg2rad(hfov)*1.15
-
-  cameraMatrixInit = np.array([[f_init, 0.      , imsize[0]/2],
-                 [0.      , f_init, imsize[1]/2],
-                 [0.      , 0.      , 1.      ]])
-  distCoeffsInit = np.zeros((4,1))
-   # check if there are any suspicious corners with high reprojection error
+def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize,
+                      hfov, name):
+  f_init = imsize[0] / np.deg2rad(hfov) * 1.15
+
+  cameraMatrixInit = np.array([[f_init, 0., imsize[0] / 2],
+                               [0., f_init, imsize[1] / 2], [0., 0., 1.]])
+  distCoeffsInit = np.zeros((4, 1))
+  # check if there are any suspicious corners with high reprojection error
   rvecs = []
   tvecs = []
   for corners, ids in zip(allCorners, allIds):
     objpts = config.board.chessboardCorners[ids]
-    corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit, distCoeffsInit)
-    rvec, tvec = camera_pose_charuco(objpts, corners_undist,ids, np.eye(3), np.array((0.0,0,0,0)))
+    corners_undist = cv2.fisheye.undistortPoints(corners, cameraMatrixInit,
+                                                 distCoeffsInit)
+    rvec, tvec = camera_pose_charuco(objpts, corners_undist, ids, np.eye(3),
+                                     np.array((0.0, 0, 0, 0)))
     tvecs.append(tvec)
     rvecs.append(rvec)
 
-  corners_removed, filtered_ids, filtered_corners = filter_corner_outliers(config, allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs, tvecs)
+  corners_removed, filtered_ids, filtered_corners = filter_corner_outliers(
+      config, allIds, allCorners, cameraMatrixInit, distCoeffsInit, rvecs,
+      tvecs)
 
   obj_points = []
   for ids in filtered_ids:
@@ -803,10 +997,15 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
   flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
   flags |= cv2.fisheye.CALIB_FIX_SKEW
 
-  term_criteria = (cv2.TERM_CRITERIA_COUNT +
-           cv2.TERM_CRITERIA_EPS, 30, 1e-9)
+  term_criteria = (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 30, 1e-9)
   try:
-    res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
+    res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points,
+                                                    filtered_corners,
+                                                    None,
+                                                    cameraMatrixInit,
+                                                    distCoeffsInit,
+                                                    flags=flags,
+                                                    criteria=term_criteria)
   except:
     # calibration failed for full FOV, let's try to limit the corners to smaller part of FOV first to find initial parameters
     success = False
@@ -819,15 +1018,23 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
         obj_points_tmp = []
         corners_tmp = []
         for obj_pt, corner in zip(obj_pts, corners):
-          check_x = corner[0,0] > imsize[0]*(1-crop) and corner[0,0] < imsize[0]*crop
-          check_y = corner[0,1] > imsize[1]*(1-crop) and corner[0,1] < imsize[1]*crop
+          check_x = corner[0, 0] > imsize[0] * (1 - crop) and corner[
+              0, 0] < imsize[0] * crop
+          check_y = corner[0, 1] > imsize[1] * (1 - crop) and corner[
+              0, 1] < imsize[1] * crop
           if check_x and check_y:
             obj_points_tmp.append(obj_pt)
             corners_tmp.append(corner)
         obj_points_limited.append(np.array(obj_points_tmp))
         corners_limited.append(np.array(corners_tmp))
       try:
-        res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points_limited, corners_limited, None, cameraMatrixInit, distCoeffsInit, flags=flags, criteria=term_criteria)
+        res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points_limited,
+                                                        corners_limited,
+                                                        None,
+                                                        cameraMatrixInit,
+                                                        distCoeffsInit,
+                                                        flags=flags,
+                                                        criteria=term_criteria)
         print(f"success with crop factor {crop}")
         success = True
         break
@@ -836,20 +1043,35 @@ def calibrate_fisheye(config: CalibrationConfig, allCorners, allIds, imsize, hfo
         if crop > 0.7:
           crop -= 0.05
         else:
-          raise Exception("Calibration failed: Tried maximum crop factor and still no success")
+          raise Exception(
+              "Calibration failed: Tried maximum crop factor and still no success"
+          )
     if success:
       # trying the full FOV once more with better initial K
       print(f"new K init {K}")
       print(f"new d_init {d}")
       try:
-        res, K, d, rvecs, tvecs =  cv2.fisheye.calibrate(obj_points, filtered_corners, imsize, K, distCoeffsInit, flags=flags, criteria=term_criteria)
+        res, K, d, rvecs, tvecs = cv2.fisheye.calibrate(obj_points,
+                                                        filtered_corners,
+                                                        imsize,
+                                                        K,
+                                                        distCoeffsInit,
+                                                        flags=flags,
+                                                        criteria=term_criteria)
       except:
-        print(f"Failed the full res calib, using calibration with crop factor {crop}")
+        print(
+            f"Failed the full res calib, using calibration with crop factor {crop}"
+        )
 
   return res, K, d, rvecs, tvecs, filtered_ids, filtered_corners
 
+
 @parallel_function
-def calibrate_camera(config, board_config, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = []):
+def calibrate_camera(config,
+                     board_config,
+                     camera_model,
+                     intrinsicCameras: List[Dataset] = [],
+                     extrinsicPairs: List[Tuple[Dataset, Dataset]] = []):
   camInfos = {}
   stereoConfigs = []
   allExtrinsics = []
@@ -863,7 +1085,10 @@ def calibrate_camera(config, board_config, camera_model, intrinsicCameras: List[
 
   # Calibrate camera intrinsics for all provided datasets
   for dataset in uniqueDatasets:
-    camData: CameraData = [c for c in board_config['cameras'].values() if c['name'] == dataset.name][0]
+    camData: CameraData = [
+        c for c in board_config['cameras'].values()
+        if c['name'] == dataset.name
+    ][0]
 
     if "calib_model" in camData and len(camData["calib_model"].split("_")) > 1:
       cameraModel_ccm, model_ccm = camData["calib_model"].split("_")
@@ -873,7 +1098,7 @@ def calibrate_camera(config, board_config, camera_model, intrinsicCameras: List[
       distortion_model = model_ccm
     else:
       calib_model = camera_model
-      distortion_model = DistortionModel.Tilted # Use the tilted model by default
+      distortion_model = DistortionModel.Tilted  # Use the tilted model by default
 
     camData['size'] = dataset.imageSize
     camData['calib_model'] = calib_model
@@ -883,23 +1108,26 @@ def calibrate_camera(config, board_config, camera_model, intrinsicCameras: List[
       allCorners, allIds = dataset.allCorners, dataset.allIds
     elif len(dataset.images):
       #allCorners, allIds = detect_charuco_board(list(dataset.images), dataset.board)
-      
+
       allCorners, allids = [], []
       for image in list(dataset.images):
         corners, ids = detect_charuco_board(image, dataset.board)
         allCorners.append(corners)
         allIds.append(ids)
     else:
-      raise RuntimeError(f'Dataset \'{dataset.name}\' doesn\'t contain corners or images')
+      raise RuntimeError(
+          f'Dataset \'{dataset.name}\' doesn\'t contain corners or images')
 
     if PER_CCM:
       camData = get_features(config, camData)
-      if camera_model== "fisheye":
-        camData = filter_features_fisheye(camData, allCorners, allIds) # TODO : Input types are wrong
+      if camera_model == "fisheye":
+        camData = filter_features_fisheye(
+            camData, allCorners, allIds)  # TODO : Input types are wrong
       elif dataset.enableFiltering:
         filteredCorners, filteredIds = [], []
         for corners, ids in zip(allCorners, allIds):
-          corners, ids, _ = estimate_pose_and_filter(camData, corners, ids, dataset.board)
+          corners, ids, _ = estimate_pose_and_filter(camData, corners, ids,
+                                                     dataset.board)
           filteredCorners.append(corners)
           filteredIds.append(ids)
         corners, ids = filteredCorners, filteredIds
@@ -909,11 +1137,13 @@ def calibrate_camera(config, board_config, camera_model, intrinsicCameras: List[
         corners, ids = allCorners, allIds
 
       camData = calibrate_charuco(camData, corners, ids, dataset)
-      camData, corners, ids = calibrate_ccm_intrinsics_per_ccm(config, camData, dataset)
+      camData, corners, ids = calibrate_ccm_intrinsics_per_ccm(
+          config, camData, dataset)
       camInfos[dataset.id] = camData
       filteredCharucos[dataset.id] = [corners, ids]
     else:
-      camData = calibrate_ccm_intrinsics(config, camData) # TODO : Not a parallel function
+      camData = calibrate_ccm_intrinsics(
+          config, camData)  # TODO : Not a parallel function
 
   for left, right in extrinsicPairs:
     left_cam_info = camInfos[left.id]
@@ -921,49 +1151,77 @@ def calibrate_camera(config, board_config, camera_model, intrinsicCameras: List[
     leftCorners, leftIds = filteredCharucos[left.id]
     rightCorners, rightIds = filteredCharucos[right.id]
 
-    left_corners_sampled, right_corners_sampled, obj_pts= find_stereo_common_features(leftCorners, leftIds, rightCorners, rightIds, left.board)
+    left_corners_sampled, right_corners_sampled, obj_pts = find_stereo_common_features(
+        leftCorners, leftIds, rightCorners, rightIds, left.board)
 
     if PER_CCM and EXTRINSICS_PER_CCM:
       if left_cam_info['calib_model'] == "perspective":
-        left_corners_sampled = undistort_points_perspective(left_corners_sampled, left_cam_info)
-        right_corners_sampled = undistort_points_perspective(right_corners_sampled, right_cam_info)
+        left_corners_sampled = undistort_points_perspective(
+            left_corners_sampled, left_cam_info)
+        right_corners_sampled = undistort_points_perspective(
+            right_corners_sampled, right_cam_info)
       else:
-        left_corners_sampled = undistort_points_fisheye(left_corners_sampled, left_cam_info)
-        right_corners_sampled = undistort_points_fisheye(right_corners_sampled, right_cam_info)
-
-      extrinsics = calibrate_stereo_perspective_per_ccm(config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+        left_corners_sampled = undistort_points_fisheye(
+            left_corners_sampled, left_cam_info)
+        right_corners_sampled = undistort_points_fisheye(
+            right_corners_sampled, right_cam_info)
+
+      extrinsics = calibrate_stereo_perspective_per_ccm(
+          config, obj_pts, left_corners_sampled, right_corners_sampled,
+          left_cam_info, right_cam_info)
     else:
       if camera_model == 'perspective':
-        extrinsics = calibrate_stereo_perspective(config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
+        extrinsics = calibrate_stereo_perspective(config, obj_pts,
+                                                  left_corners_sampled,
+                                                  right_corners_sampled,
+                                                  left_cam_info,
+                                                  right_cam_info)
       elif camera_model == 'fisheye':
-        extrinsics = calibrate_stereo_fisheye(config, obj_pts, left_corners_sampled, right_corners_sampled, left_cam_info, right_cam_info)
-    left_cam_info, stereo_config = calculate_epipolar_error(left_cam_info, right_cam_info, left, right, board_config, extrinsics)
+        extrinsics = calibrate_stereo_fisheye(config, obj_pts,
+                                              left_corners_sampled,
+                                              right_corners_sampled,
+                                              left_cam_info, right_cam_info)
+    left_cam_info, stereo_config = calculate_epipolar_error(
+        left_cam_info, right_cam_info, left, right, board_config, extrinsics)
     allExtrinsics.append(extrinsics)
     camInfos[left.id] = left_cam_info
     stereoConfigs.append(stereo_config)
 
   return board_config, filteredCharucos, allExtrinsics, camInfos, stereoConfigs
 
+
 class StereoCalibration(object):
   """Class to Calculate Calibration and Rectify a Stereo Camera."""
 
-  def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset] = [], extrinsicPairs: List[Tuple[Dataset, Dataset]] = [], initial_max_threshold = 15, initial_min_filtered = 0.05, debug: bool = False):
+  def calibrate(self,
+                board_config,
+                camera_model,
+                intrinsicCameras: List[Dataset] = [],
+                extrinsicPairs: List[Tuple[Dataset, Dataset]] = [],
+                initial_max_threshold=15,
+                initial_min_filtered=0.05,
+                debug: bool = False):
     """Function to calculate calibration for stereo camera."""
     config = CalibrationConfig(
-      initial_max_threshold, initial_min_filtered,
-      camera_model, (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9)
-    )
+        initial_max_threshold, initial_min_filtered, camera_model,
+        (cv2.TERM_CRITERIA_COUNT + cv2.TERM_CRITERIA_EPS, 300, 1e-9))
 
     for a, b in extrinsicPairs:
       if len(a.allIds) != len(b.allIds):
-        print('Not all dataset for extrinsic calibration have the same number of images')
-        raise RuntimeError('Not all dataset for extrinsic calibration have the same number of images') # TODO : This isn't thorough enough
+        print(
+            'Not all dataset for extrinsic calibration have the same number of images'
+        )
+        raise RuntimeError(
+            'Not all dataset for extrinsic calibration have the same number of images'
+        )  # TODO : This isn't thorough enough
       if a.board is not b.board:
         print('Extrinsic pair has different dataset board')
         raise RuntimeError('Extrinsic pair has different dataset board')
 
     #board_config, filteredCharucos, allExtrinsics, camInfos, stereoConfigs = calibrate_camera.run_parallel(10, config, board_config, camera_model, intrinsicCameras, extrinsicPairs)
-    board_config, filteredCharucos, allExtrinsics, camInfos, stereoConfigs = calibrate_camera.run_parallel(10, config, board_config, camera_model, intrinsicCameras, extrinsicPairs)
+    board_config, filteredCharucos, allExtrinsics, camInfos, stereoConfigs = calibrate_camera.run_parallel(
+        10, config, board_config, camera_model, intrinsicCameras,
+        extrinsicPairs)
 
     # Construct board config from calibrated cam infos
     for dataset in intrinsicCameras:
@@ -976,7 +1234,8 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
       for socket in board_config['cameras']:
         if board_config['cameras'][socket]['name'] == left.name:
           #board_config['cameras'][socket]['extrinsics'] = camInfos[left.id].ret()['extrinsics']
-          board_config['cameras'][socket]['extrinsics'] = camInfos[left.id]['extrinsics']
+          board_config['cameras'][socket]['extrinsics'] = camInfos[
+              left.id]['extrinsics']
 
     for stereoConfig in stereoConfigs:
       #if stereoConfig.ret():
@@ -985,10 +1244,10 @@ def calibrate(self, board_config, camera_model, intrinsicCameras: List[Dataset]
         board_config['stereo_config'].update(stereoConfig)
 
     #for key in filteredCharucos.keys():
-      #filteredCharucos[key] = [e.ret() for e in filteredCharucos[key]]
+    #filteredCharucos[key] = [e.ret() for e in filteredCharucos[key]]
 
     if debug:
       #return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}, filteredCharucos
       return stereoConfigs, allExtrinsics, board_config, camInfos, filteredCharucos
 
-    return board_config, filteredCharucos
\ No newline at end of file
+    return board_config, filteredCharucos
diff --git a/worker.py b/worker.py
index 79fe429..cbce9ee 100644
--- a/worker.py
+++ b/worker.py
@@ -9,22 +9,26 @@
 
 T = TypeVar('T')
 
+
 def allArgs(args, kwargs):
   for arg in args:
     yield arg
   for kwarg in kwargs.values():
     yield kwarg
 
+
 class Retvals(Generic[T]):
+
   def __init__(self, taskOrGroup: 'ParallelTask', key: slice | tuple | int):
     self._taskOrGroup = taskOrGroup
     self._key = key
 
-  def __iter__(self) -> T: # Allow iterating over slice or list retvals
+  def __iter__(self) -> T:  # Allow iterating over slice or list retvals
     if isinstance(self._key, slice):
       if not self._key.stop:
         raise RuntimeError('Cannot iterate over an unknown length Retvals')
-      for i in range(self._key.start or 0, self._key.stop, self._key.step or 1):
+      for i in range(self._key.start or 0, self._key.stop, self._key.step
+                     or 1):
         yield Retvals(self._taskOrGroup, i)
     elif isinstance(self._key, list | tuple):
       for i in self._key:
@@ -44,7 +48,9 @@ def ret(self) -> T:
   def finished(self) -> bool:
     return self._taskOrGroup.finished()
 
+
 class ParallelTask(Generic[T]):
+
   def __init__(self, worker: 'ParallelWorker', fun, args, kwargs):
     self._worker = worker
     self._fun = fun
@@ -62,12 +68,14 @@ def __repr__(self):
     return f'<Task {self._fun} {self._id}>'
 
   def resolveArguments(self) -> None:
+
     def _replace(args):
       for arg in args:
         if isinstance(arg, ParallelTask | ParallelTaskGroup | Retvals):
           yield arg.ret()
         else:
           yield arg
+
     self._args = list(_replace(self._args))
     for key, value in self._kwargs:
       if isinstance(value, ParallelTask | ParallelTaskGroup | Retvals):
@@ -75,7 +83,8 @@ def _replace(args):
 
   def isExecutable(self) -> bool:
     for arg in allArgs(self._args, self._kwargs):
-      if isinstance(arg, ParallelTask | ParallelTaskGroup | Retvals) and not arg.finished():
+      if isinstance(arg, ParallelTask | ParallelTaskGroup
+                    | Retvals) and not arg.finished():
         return False
     return True
 
@@ -93,7 +102,9 @@ def exc(self) -> BaseException | None:
   def ret(self) -> T | None:
     return self._ret
 
+
 class ParallelTaskGroup(Generic[T]):
+
   def __init__(self, fun, args, kwargs):
     self._fun = fun
     self._args = args
@@ -122,21 +133,26 @@ def tasks(self) -> List[ParallelTask]:
     for arg in allArgs(self._args, self._kwargs):
       if isinstance(arg, ParallelTask | Retvals):
         arg = arg.ret()
-      if isinstance(arg, abc.Sized) and not isinstance(arg, str | bytes | dict):
+      if isinstance(arg,
+                    abc.Sized) and not isinstance(arg, str | bytes | dict):
         nTasks = max(nTasks, len(arg))
     self._tasks = []
     for arg in allArgs(self._args, self._kwargs):
       if isinstance(arg, abc.Sized) and len(arg) != nTasks:
-        raise RuntimeError('All sized arguments must have the same length or 1')
-    
+        raise RuntimeError(
+            'All sized arguments must have the same length or 1')
+
     def argsAtI(i: int):
       for arg in self._args:
         if isinstance(arg, list):
           yield arg[i]
-        elif isinstance(arg, ParallelTask | Retvals) and isinstance(arg.ret(), abc.Iterable) and not isinstance(arg.ret(), (str, bytes, dict)):
+        elif isinstance(arg, ParallelTask | Retvals) and isinstance(
+            arg.ret(),
+            abc.Iterable) and not isinstance(arg.ret(), (str, bytes, dict)):
           yield arg.ret()[i]
         else:
           yield arg
+
     def kwargsAtI(i: int):
       newKwargs = {}
       for key, value in self._kwargs:
@@ -152,7 +168,9 @@ def kwargsAtI(i: int):
     return self._tasks
 
   def ret(self) -> List[T | None]:
+
     def zip_retvals(tasks):
+
       def _iRet(i):
         for task in tasks:
           yield task.ret()[i]
@@ -169,11 +187,14 @@ def _iRet(i):
 
   def isExecutable(self) -> bool:
     for arg in allArgs(self._args, self._kwargs):
-      if isinstance(arg, ParallelTask | ParallelTaskGroup | Retvals) and not arg.finished():
+      if isinstance(arg, ParallelTask | ParallelTaskGroup
+                    | Retvals) and not arg.finished():
         return False
     return True
 
-def worker_controller(_stop: multiprocess.Event, _in: multiprocess.Queue, _out: multiprocess.Queue, _wId: int) -> None:
+
+def worker_controller(_stop: multiprocess.Event, _in: multiprocess.Queue,
+                      _out: multiprocess.Queue, _wId: int) -> None:
   while not _stop.is_set():
     try:
       task: ParallelTask | None = _in.get(timeout=0.01)
@@ -189,7 +210,9 @@ def worker_controller(_stop: multiprocess.Event, _in: multiprocess.Queue, _out:
     except:
       break
 
+
 class ParallelWorker:
+
   def __init__(self, workers: int = 16):
     self._workers = workers
     self._tasks: List[ParallelTask] = []
@@ -200,12 +223,14 @@ def __enter__(self):
   def __exit__(self, exc_type, exc_value, exc_traceback):
     self.execute()
 
-  def run(self, fun: Callable[[Any], T], *args: Tuple[Any], **kwargs: Dict[str, Any]) -> ParallelTask[T]:
+  def run(self, fun: Callable[[Any], T], *args: Tuple[Any],
+          **kwargs: Dict[str, Any]) -> ParallelTask[T]:
     task = ParallelTask(self, fun, args, kwargs)
     self._tasks.append(task)
     return task
 
-  def map(self, fun: Callable[[Any], T], *args: Tuple[Iterable[Any]], **kwargs: Dict[str, Iterable[Any]]) -> ParallelTaskGroup[T]:
+  def map(self, fun: Callable[[Any], T], *args: Tuple[Iterable[Any]],
+          **kwargs: Dict[str, Iterable[Any]]) -> ParallelTaskGroup[T]:
     taskGroup = ParallelTaskGroup(fun, args, kwargs)
     self._tasks.append(taskGroup)
     return taskGroup
@@ -216,7 +241,8 @@ def execute(self):
     stop = multiprocess.Event()
     processes = []
     for i in range(self._workers):
-      p = multiprocess.Process(target=worker_controller, args=(stop, workerIn, workerOut, i))
+      p = multiprocess.Process(target=worker_controller,
+                               args=(stop, workerIn, workerOut, i))
       processes.append(p)
       p.start()
 
@@ -243,7 +269,8 @@ def execute(self):
           for task in doneTasks:
             if task._id == tId:
               if exc:
-                raise Exception(f'Calibration pipeline failed during \'{task}\'') from exc
+                raise Exception(
+                    f'Calibration pipeline failed during \'{task}\'') from exc
               task.finish(ret, exc)
               remaining -= 1
     finally:
@@ -251,6 +278,7 @@ def execute(self):
       for p in processes:
         p.join()
 
+
 class ParallelFunctionTransformer(ast.NodeTransformer):
   """AST Transformer for parsing ParallelFunctions and replacing calls to other parallel functions with pw.run(fun)"""
 
@@ -261,17 +289,10 @@ def visit_Expr(self, node):
     """Finds all standalone expressions and converts them to __ParallelWorker_run__"""
     if isinstance(node.value, ast.Call):
       call_node = node.value
-      new_node = ast.Expr(
-        value=ast.Call(
+      new_node = ast.Expr(value=ast.Call(
           func=ast.Name(id='__ParallelWorker_run__', ctx=ast.Load()),
-          args=[
-            ast.Constant(0),
-            call_node.func,
-            *call_node.args
-          ],
-          keywords=call_node.keywords
-        )
-      )
+          args=[ast.Constant(0), call_node.func, *call_node.args],
+          keywords=call_node.keywords))
       ast.fix_missing_locations(new_node)
       return ast.copy_location(new_node, node)
     return node
@@ -284,7 +305,8 @@ def visit_Assign(self, node):
     target_names = []
     for target in targets:
       if isinstance(target, ast.Tuple):
-        target_names.extend([elt.id for elt in target.elts if isinstance(elt, ast.Name)])
+        target_names.extend(
+            [elt.id for elt in target.elts if isinstance(elt, ast.Name)])
       elif isinstance(target, ast.Name):
         target_names.append(target.id)
 
@@ -292,17 +314,14 @@ def visit_Assign(self, node):
     if isinstance(node.value, ast.Call):
       assign_call = node.value
       new_assign = ast.Assign(
-        targets=node.targets,
-        value=ast.Call(
-          func=ast.Name(id='__ParallelWorker_run__', ctx=ast.Load()),
-          args=[
-            ast.Constant(len(target_names)),
-            assign_call.func,
-            *assign_call.args
-          ],
-          keywords=assign_call.keywords
-        )
-      )
+          targets=node.targets,
+          value=ast.Call(func=ast.Name(id='__ParallelWorker_run__',
+                                       ctx=ast.Load()),
+                         args=[
+                             ast.Constant(len(target_names)), assign_call.func,
+                             *assign_call.args
+                         ],
+                         keywords=assign_call.keywords))
       ast.fix_missing_locations(new_assign)
       return new_assign
     return node
@@ -328,12 +347,13 @@ def visit_For(self, node: ast.For):
       match el:
         case ast.Expr():
           # Filter for only '.append' functions
-          if not isinstance(el.value, ast.Call) or el.value.func.attr != 'append':
+          if not isinstance(el.value,
+                            ast.Call) or el.value.func.attr != 'append':
             continue
           appending[el.value.args[0].id] = copy.deepcopy(el.value.func.value)
           appending[el.value.args[0].id].ctx = ast.Load()
         case ast.Assign():
-          if funName: # If there already was a function, then it can't be valid
+          if funName:  # If there already was a function, then it can't be valid
             funName = None
             break
           if not isinstance(el.value, ast.Call):
@@ -352,7 +372,9 @@ def visit_For(self, node: ast.For):
     argMap = {a: b for a, b in zip(itElNames, being_iterated)}
     assigns = [appending[a.id] for a in assigns if a.id in appending]
 
-    args =  [ast.Constant(len(assigns))] + [funName] + [argMap.get(a.id if isinstance(a, ast.Name) else 0, a) for a in args]
+    args = [ast.Constant(len(assigns))] + [funName] + [
+        argMap.get(a.id if isinstance(a, ast.Name) else 0, a) for a in args
+    ]
 
     for arg in args:
       arg.ctx = ast.Load()
@@ -364,39 +386,28 @@ def visit_For(self, node: ast.For):
 
     # Create the map call node and return it
     if len(assigns) != 1:
-      assigns = [ast.Tuple(
-        elts=assigns,
-        ctx=ast.Store()
-      )]
-
-    workerMap = ast.Assign(
-      targets=assigns,
-      value=ast.Call(
-        func=ast.Name(id="__ParallelWorker_map__", ctx=ast.Load()),
-        args=args,
-        keywords=[]
-      )
-    )
-
-    if_condition=ast.Call(
-      func=ast.Name(id="isinstance", ctx=ast.Load()),
-      args=[
-        funName,
-        ast.Name(id="ParallelFunction", ctx=ast.Load())
-      ],
-      keywords=[]
-    )
-    if_node = ast.If(
-      test=if_condition,
-      body=[workerMap],
-      orelse=[node]
-    )
+      assigns = [ast.Tuple(elts=assigns, ctx=ast.Store())]
+
+    workerMap = ast.Assign(targets=assigns,
+                           value=ast.Call(func=ast.Name(
+                               id="__ParallelWorker_map__", ctx=ast.Load()),
+                                          args=args,
+                                          keywords=[]))
+
+    if_condition = ast.Call(
+        func=ast.Name(id="isinstance", ctx=ast.Load()),
+        args=[funName,
+              ast.Name(id="ParallelFunction", ctx=ast.Load())],
+        keywords=[])
+    if_node = ast.If(test=if_condition, body=[workerMap], orelse=[node])
 
     ast.fix_missing_locations(if_node)
     return if_node
 
+
 class ParallelFunction:
   """Decorator for a parallel function"""
+
   def __init__(self, fun):
     self._fun = fun
 
@@ -421,7 +432,7 @@ def run_parallel(self, workers: int = 8, *args, **kwargs):
 
     # Execute all parallel functions
     pw.execute()
-    
+
     # Unwind and replace retvals
     def replace_retvals(el):
       if isinstance(el, Retvals):
@@ -433,12 +444,13 @@ def replace_retvals(el):
       elif isinstance(el, tuple):
         return tuple(map(replace_retvals, el))
       return el
-    
+
     return replace_retvals(ret)
 
   def _convert_to_pw(self, pw: ParallelWorker):
     source_lines = inspect.getsourcelines(self._fun)[0]
-    source = ''.join([line for line in source_lines if not line.startswith('@')])
+    source = ''.join(
+        [line for line in source_lines if not line.startswith('@')])
 
     tree = ast.parse(source)
 
@@ -469,14 +481,19 @@ def worker_map(nret, fun, *args):
           return retvals[:nret]
         return retvals
       return fun(*args)
-    
+
     # Inject __ParallelWorker... definitions
-    func_globals = {**func_globals, '__ParallelWorker_run__': worker_run, '__ParallelWorker_map__': worker_map}
+    func_globals = {
+        **func_globals, '__ParallelWorker_run__': worker_run,
+        '__ParallelWorker_map__': worker_map
+    }
     exec(compiled_code, func_globals)
 
     return func_globals
 
+
 T = TypeVar("T")
 
+
 def parallel_function(fun: T) -> T:
-  return ParallelFunction(fun)
\ No newline at end of file
+  return ParallelFunction(fun)

From e9d24dc7644207cc142b13948a69713f5992edde Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 20 Dec 2024 18:27:32 +0100
Subject: [PATCH 86/87] Add comments

---
 worker.py | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/worker.py b/worker.py
index cbce9ee..50d0a54 100644
--- a/worker.py
+++ b/worker.py
@@ -18,6 +18,7 @@ def allArgs(args, kwargs):
 
 
 class Retvals(Generic[T]):
+  """Representation of a value yet to be returned by a worker"""
 
   def __init__(self, taskOrGroup: 'ParallelTask', key: slice | tuple | int):
     self._taskOrGroup = taskOrGroup
@@ -40,12 +41,14 @@ def __repr__(self):
     return f'<Retvals of {self._taskOrGroup}>'
 
   def ret(self) -> T:
+    """Retrieve the value returned by the worker"""
     if isinstance(self._key, list | tuple):
       ret = self._taskOrGroup.ret()
       return [ret[i] for i in self._key]
     return self._taskOrGroup.ret()[self._key]
 
   def finished(self) -> bool:
+    """Check if the worker has finished"""
     return self._taskOrGroup.finished()
 
 
@@ -68,6 +71,7 @@ def __repr__(self):
     return f'<Task {self._fun} {self._id}>'
 
   def resolveArguments(self) -> None:
+    """Convert all Retvals arguments into their underlying values"""
 
     def _replace(args):
       for arg in args:
@@ -82,6 +86,8 @@ def _replace(args):
         self._kwargs[key] = value.ret()
 
   def isExecutable(self) -> bool:
+    """Check if all tasks on which this task dependes have finished"""
+
     for arg in allArgs(self._args, self._kwargs):
       if isinstance(arg, ParallelTask | ParallelTaskGroup
                     | Retvals) and not arg.finished():
@@ -89,6 +95,8 @@ def isExecutable(self) -> bool:
     return True
 
   def finished(self) -> bool:
+    """Check if this task has finished"""
+
     return self._finished
 
   def finish(self, ret, exc) -> None:
@@ -97,9 +105,11 @@ def finish(self, ret, exc) -> None:
     self._finished = True
 
   def exc(self) -> BaseException | None:
+    """Retrieve an exception, if it occured, otherwise None"""
     return self._exc
 
   def ret(self) -> T | None:
+    """Retrieve the return value"""
     return self._ret
 
 
@@ -118,6 +128,8 @@ def __repr__(self):
     return f'<TaskGroup {self._fun}>'
 
   def finished(self) -> bool:
+    """Check if all tasks in the group have finished"""
+
     if not self._tasks:
       return False
     for task in self._tasks:
@@ -126,9 +138,13 @@ def finished(self) -> bool:
     return True
 
   def exc(self) -> List[BaseException | None]:
+    """Retrieve any exceptions from the tasks of this group"""
+
     return list(map(lambda t: t.exc(), self._tasks))
 
   def tasks(self) -> List[ParallelTask]:
+    """Retrieve the tasks which make up this task group"""
+
     nTasks = 1
     for arg in allArgs(self._args, self._kwargs):
       if isinstance(arg, ParallelTask | Retvals):
@@ -168,6 +184,7 @@ def kwargsAtI(i: int):
     return self._tasks
 
   def ret(self) -> List[T | None]:
+    """Retrieve the return value of all tasks in the group as a single list"""
 
     def zip_retvals(tasks):
 
@@ -186,6 +203,7 @@ def _iRet(i):
     return list(zip_retvals(self._tasks))
 
   def isExecutable(self) -> bool:
+    """Check if all dependency tasks have finished"""
     for arg in allArgs(self._args, self._kwargs):
       if isinstance(arg, ParallelTask | ParallelTaskGroup
                     | Retvals) and not arg.finished():

From b631ee0fc8d805b281b2672836aac3eda1cf7cce Mon Sep 17 00:00:00 2001
From: Tommy Walker <tomprochazka0@gmail.com>
Date: Fri, 20 Dec 2024 18:28:10 +0100
Subject: [PATCH 87/87] Remove retval opening

---
 calibration_utils.py | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/calibration_utils.py b/calibration_utils.py
index 7261be5..a8dde97 100644
--- a/calibration_utils.py
+++ b/calibration_utils.py
@@ -1227,27 +1227,19 @@ def calibrate(self,
     for dataset in intrinsicCameras:
       for socket in board_config['cameras']:
         if board_config['cameras'][socket]['name'] == dataset.name:
-          #board_config['cameras'][socket] = camInfos[dataset.id].ret()
           board_config['cameras'][socket] = camInfos[dataset.id]
 
     for left, _ in extrinsicPairs:
       for socket in board_config['cameras']:
         if board_config['cameras'][socket]['name'] == left.name:
-          #board_config['cameras'][socket]['extrinsics'] = camInfos[left.id].ret()['extrinsics']
           board_config['cameras'][socket]['extrinsics'] = camInfos[
               left.id]['extrinsics']
 
     for stereoConfig in stereoConfigs:
-      #if stereoConfig.ret():
-      #  board_config['stereo_config'].update(stereoConfig.ret())
       if stereoConfig:
         board_config['stereo_config'].update(stereoConfig)
 
-    #for key in filteredCharucos.keys():
-    #filteredCharucos[key] = [e.ret() for e in filteredCharucos[key]]
-
     if debug:
-      #return [s.ret() for s in stereoConfigs], [e.ret() for e in allExtrinsics], board_config, {k: v.ret() for k, v in camInfos.items()}, filteredCharucos
       return stereoConfigs, allExtrinsics, board_config, camInfos, filteredCharucos
 
     return board_config, filteredCharucos