feat: subsampled + roi approach to accelerate intrinsic calibration (#…

…152)

* feat: adding some stamp info in the calibrator to ease debugging and acceleration improvement

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* feat: implemented a subsampled detection followed by a roi detection scheme to accelerate computations in big images

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* chore: removed debug print

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

* ci(pre-commit): autofix

* chore: fixed pre commit

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>

---------

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py

@@ -85,10 +85,10 @@ def __init__(self, **kwargs):
         self.current_decode_sharpening = None
         self.current_debug = None
 
-    def detect(self, img):
+    def detect(self, img, stamp):
         """Slot to detect boards from an image. Results are sent through the detection_results signals."""
         if img is None:
-            self.detection_results_signal.emit(None, None)
+            self.detection_results_signal.emit(None, None, None)
             return
 
         with self.lock:
@@ -148,7 +148,7 @@ def detect(self, img):
         tags.sort(key=lambda tag: tag.tag_id)
 
         if len(tags) < rows * cols * min_detection_ratio:
-            self.detection_results_signal.emit(img, None)
+            self.detection_results_signal.emit(img, None, stamp)
             return
 
         detection = ApriltagGridDetection(
@@ -162,4 +162,4 @@ def detect(self, img):
             tags=tags,
         )
 
-        self.detection_results_signal.emit(img, detection)
+        self.detection_results_signal.emit(img, detection, stamp)

sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py

@@ -24,12 +24,13 @@
 from PySide2.QtCore import Signal
 from intrinsic_camera_calibrator.board_parameters.board_parameters import BoardParameters
 from intrinsic_camera_calibrator.parameter import ParameterizedClass
+import numpy as np
 
 
 class BoardDetector(ParameterizedClass, QObject):
     """Base class of board detectors."""
 
-    detection_results_signal = Signal(object, object)
+    detection_results_signal = Signal(object, object, float)
 
     def __init__(
         self, lock: threading.RLock, board_parameters: BoardParameters, cfg: Optional[Dict] = {}
@@ -40,7 +41,7 @@ def __init__(
 
         self.set_parameters(**cfg)
 
-    def detect(self, img):
+    def detect(self, img: np.array, stamp):
         """Slot to detect boards from an image. Subclasses must implement this method."""
         raise NotImplementedError
 

sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py

@@ -29,34 +29,85 @@ def __init__(self, **kwargs):
         self.adaptive_thresh = Parameter(bool, value=True, min_value=False, max_value=True)
         self.normalize_image = Parameter(bool, value=True, min_value=False, max_value=True)
         self.fast_check = Parameter(bool, value=True, min_value=False, max_value=True)
-        self.refine = Parameter(bool, value=True, min_value=False, max_value=True)
 
-    def detect(self, img):
+        self.resized_detection = Parameter(bool, value=True, min_value=False, max_value=True)
+        self.resized_max_resolution = Parameter(int, value=1000, min_value=500, max_value=3000)
+        self.sub_pixel_refinement = Parameter(bool, value=True, min_value=False, max_value=True)
+
+    def detect(self, img: np.array, stamp: float):
         """Slot to detect boards from an image. Results are sent through the detection_results signals."""
         if img is None:
             self.detection_results_signal.emit(None, None)
             return
 
         with self.lock:
-            h, w = img.shape[0:2]
             (cols, rows) = (self.board_parameters.cols.value, self.board_parameters.rows.value)
             cell_size = self.board_parameters.cell_size.value
 
             flags = 0
             flags |= cv2.CALIB_CB_ADAPTIVE_THRESH if self.adaptive_thresh.value else 0
             flags |= cv2.CALIB_CB_NORMALIZE_IMAGE if self.normalize_image.value else 0
             flags |= cv2.CALIB_CB_FAST_CHECK if self.fast_check.value else 0
-            refine = self.refine.value
+            sub_pixel_refinement = self.sub_pixel_refinement.value
+
+            resized_detection = self.resized_detection.value
+            resized_max_resolution = self.resized_max_resolution.value
 
+        h, w = img.shape[0:2]
         grayscale = to_grayscale(img)
 
-        (ok, corners) = cv2.findChessboardCorners(grayscale, (cols, rows), flags=flags)
+        if not resized_detection or max(h, w) <= resized_max_resolution:
+            (ok, corners) = cv2.findChessboardCorners(grayscale, (cols, rows), flags=flags)
 
-        if not ok:
-            self.detection_results_signal.emit(img, None)
-            return
+            if not ok:
+                self.detection_results_signal.emit(img, None, stamp)
+                return
+        else:
+            # Find the resized dimensions
+            ratio = float(w) / float(h)
+
+            if w > h:
+                resized_w = int(resized_max_resolution)
+                resized_h = int(resized_max_resolution / ratio)
+            else:
+                resized_w = int(resized_max_resolution * ratio)
+                resized_h = int(resized_max_resolution)
+
+            # Resize
+            resized = cv2.resize(img, (resized_w, resized_h), interpolation=cv2.INTER_NEAREST)
+
+            # Run the detector on the resized image
+            (ok, resized_corners) = cv2.findChessboardCorners(resized, (cols, rows), flags=flags)
+
+            if not ok:
+                self.detection_results_signal.emit(img, None, stamp)
+                return
+
+            # Re escalate the corners
+            corners = resized_corners * np.array(
+                [float(w) / resized_w, float(h) / resized_h], dtype=np.float32
+            )
+
+            # Estimate the ROI in the original image
+            roi_min_j = int(max(0, corners[:, 0, 1].min() - 10))
+            roi_min_i = int(max(0, corners[:, 0, 0].min() - 10))
+            roi_max_j = int(min(w, corners[:, 0, 1].max() + 10))
+            roi_max_i = int(min(w, corners[:, 0, 0].max() + 10))
+
+            # Extract the ROI of the original image
+            roi = grayscale[roi_min_j:roi_max_j, roi_min_i:roi_max_i]
+
+            # Run the detector again
+            (ok, roi_corners) = cv2.findChessboardCorners(roi, (cols, rows), flags=flags)
+
+            if not ok:
+                self.detection_results_signal.emit(img, None, stamp)
+                return
+
+            # Re escalate the coordinates
+            corners = roi_corners + np.array([roi_min_i, roi_min_j], dtype=np.float32)
 
-        if ok and refine:
+        if sub_pixel_refinement:
             dist_matrix = np.linalg.norm(
                 corners.reshape(-1, 1, 2) - corners.reshape(1, -1, 2), axis=-1
             )
@@ -81,4 +132,4 @@ def detect(self, img):
             image_points=image_points,
         )
 
-        self.detection_results_signal.emit(img, detection)
+        self.detection_results_signal.emit(img, detection, stamp)

sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py

@@ -39,7 +39,10 @@ def __init__(self, **kwargs):
             float, value=1.0, min_value=0.1, max_value=10.0
         )
 
-    def detect(self, img: np.array):
+        self.resized_detection = Parameter(bool, value=True, min_value=False, max_value=True)
+        self.resized_max_resolution = Parameter(int, value=2000, min_value=500, max_value=5000)
+
+    def detect(self, img: np.array, stamp: float):
         """Slot to detect boards from an image. Results are sent through the detection_results signals."""
         if img is None:
             self.detection_results_signal.emit(None, None)
@@ -50,16 +53,10 @@ def detect(self, img: np.array):
             (cols, rows) = (self.board_parameters.cols.value, self.board_parameters.rows.value)
             cell_size = self.board_parameters.cell_size.value
 
-            # Setting blob detector
-            params = cv2.SimpleBlobDetector_Params()
-            params.filterByArea = self.filter_by_area.value
-            params.minArea = self.min_area_percentage.value * h * w / 100.0
-            params.maxArea = self.max_area_percentage.value * h * w / 100.0
-            params.minDistBetweenBlobs = (
-                self.min_dist_between_blobs_percentage.value * max(h, w) / 100.0
-            )
-
-            detector = cv2.SimpleBlobDetector_create(params)
+            filter_by_area = self.filter_by_area.value
+            min_area_percentage = self.min_area_percentage.value
+            max_area_percentage = self.max_area_percentage.value
+            min_dist_between_blobs_percentage = self.min_dist_between_blobs_percentage.value
 
             flags = 0
             flags |= cv2.CALIB_CB_CLUSTERING if self.clustering.value else 0
@@ -69,26 +66,99 @@ def detect(self, img: np.array):
                 else cv2.CALIB_CB_ASYMMETRIC_GRID
             )
 
-        grayscale = to_grayscale(img)
-
-        (ok, corners) = cv2.findCirclesGrid(
-            grayscale, (cols, rows), flags=flags, blobDetector=detector
+            resized_detection = self.resized_detection.value
+            resized_max_resolution = self.resized_max_resolution.value
+
+        # Find the resized dimensions
+        ratio = float(w) / float(h)
+
+        if w > h:
+            resized_w = int(resized_max_resolution)
+            resized_h = int(resized_max_resolution / ratio)
+        else:
+            resized_w = int(resized_max_resolution * ratio)
+            resized_h = int(resized_max_resolution)
+
+        # Setting blob detector
+        full_res_params = cv2.SimpleBlobDetector_Params()
+        full_res_params.filterByArea = filter_by_area
+        full_res_params.minArea = min_area_percentage * h * w / 100.0
+        full_res_params.maxArea = max_area_percentage * h * w / 100.0
+        full_res_params.minDistBetweenBlobs = min_dist_between_blobs_percentage * max(h, w) / 100.0
+
+        resized_params = cv2.SimpleBlobDetector_Params()
+        resized_params.filterByArea = filter_by_area
+        resized_params.minArea = min_area_percentage * resized_h * resized_w / 100.0
+        resized_params.maxArea = max_area_percentage * resized_h * resized_w / 100.0
+        resized_params.minDistBetweenBlobs = (
+            min_dist_between_blobs_percentage * max(resized_h, resized_w) / 100.0
         )
 
-        if not ok:
+        full_res_detector = cv2.SimpleBlobDetector_create(full_res_params)
+        resized_detector = cv2.SimpleBlobDetector_create(resized_params)
+
+        grayscale = to_grayscale(img)
+
+        def detect(detection_image, detector):
             (ok, corners) = cv2.findCirclesGrid(
-                grayscale, (cols, rows), flags=flags, blobDetector=detector
+                detection_image, (cols, rows), flags=flags, blobDetector=detector
             )
 
-            # we need to swap the axes of the detections back to make it consistent
-            if ok:
-                corners_2d_array = corners.reshape((cols, rows, 2))
-                corners_transposed = np.transpose(corners_2d_array, (1, 0, 2))
-                corners = corners_transposed.reshape(-1, 1, 2)
+            if not ok:
+                (ok, corners) = cv2.findCirclesGrid(
+                    detection_image, (cols, rows), flags=flags, blobDetector=detector
+                )
 
-        if not ok:
-            self.detection_results_signal.emit(img, None)
-            return
+                # we need to swap the axes of the detections back to make it consistent
+                if ok:
+                    corners_2d_array = corners.reshape((cols, rows, 2))
+                    corners_transposed = np.transpose(corners_2d_array, (1, 0, 2))
+                    corners = corners_transposed.reshape(-1, 1, 2)
+
+            return (ok, corners)
+
+        if not resized_detection or max(h, w) <= resized_max_resolution:
+            (ok, corners) = detect(grayscale, full_res_detector)
+
+        else:
+            # Resize
+            resized = cv2.resize(img, (resized_w, resized_h), interpolation=cv2.INTER_NEAREST)
+
+            # Run the detector on the resized image
+            (ok, resized_corners) = detect(resized, resized_detector)
+
+            if not ok:
+                self.detection_results_signal.emit(img, None, stamp)
+                return
+
+            # Re escalate the corners
+            corners = resized_corners * np.array(
+                [float(w) / resized_w, float(h) / resized_h], dtype=np.float32
+            )
+
+            # Estimate the ROI in the original image
+            border = max(
+                corners[:, 0, 0].max() - corners[:, 0, 0].min(),
+                corners[:, 0, 1].max() - corners[:, 0, 1].min(),
+            ) / min(cols, rows)
+
+            roi_min_j = int(max(0, corners[:, 0, 1].min() - border))
+            roi_min_i = int(max(0, corners[:, 0, 0].min() - border))
+            roi_max_j = int(min(w, corners[:, 0, 1].max() + border))
+            roi_max_i = int(min(w, corners[:, 0, 0].max() + border))
+
+            # Extract the ROI of the original image
+            roi = grayscale[roi_min_j:roi_max_j, roi_min_i:roi_max_i]
+
+            # Run the detector again
+            (ok, roi_corners) = detect(roi, full_res_detector)
+
+            if not ok:
+                self.detection_results_signal.emit(img, None, stamp)
+                return
+
+            # Re escalate the coordinates
+            corners = roi_corners + np.array([roi_min_i, roi_min_j], dtype=np.float32)
 
         # reverse the corners if needed
         if np.linalg.norm(corners[0]) > np.linalg.norm(corners[1]):
@@ -108,4 +178,4 @@ def detect(self, img: np.array):
             image_points=image_points,
         )
 
-        self.detection_results_signal.emit(img, detection)
+        self.detection_results_signal.emit(img, detection, stamp)