perf: skip frames, roi, fixed aspect ratio

calibrators/interactive_camera_lidar_calibrator/interactive_camera_lidar_calibrator/image_view.py

@@ -540,8 +540,6 @@ def draw_pointcloud(self, painter):
             painter.drawEllipse if self.data_renderer.marker_type == "circles" else painter.drawRect
         )
 
-        # print(f"Drawing pointcloud size: {scale_px.shape[0]}")
-
         for point, radius, color_channel in zip(pointcloud_wcs, scale_px, color_scalars):
             if self.data_renderer.color_channel == "intensity":
                 color = intensity_to_rainbow_qcolor(

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp

@@ -382,11 +382,11 @@ void CeresCameraIntrinsicsOptimizer::solve()
   ceres::Solver::Options options;
   options.linear_solver_type = ceres::DENSE_SCHUR;  // cSpell:ignore SCHUR
   options.minimizer_progress_to_stdout = verbose_;
-  options.max_num_iterations = 500;
+  options.max_num_iterations = 100;
   options.function_tolerance = 1e-10;
   options.gradient_tolerance = 1e-14;
   options.num_threads = 8;
-  options.max_num_consecutive_invalid_steps = 1000;
+  options.max_num_consecutive_invalid_steps = 100;
   options.use_inner_iterations = true;
   ceres::Solver::Summary summary;
   ceres::Solve(options, &problem, &summary);

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml

@@ -9,15 +9,15 @@ calibration_parameters:
   use_ransac_pre_rejection: true # yamllint disable-line rule:truthy
   pre_rejection_iterations: 100
   pre_rejection_min_hypotheses: 6
-  pre_rejection_max_rms_error: 0.35
+  pre_rejection_max_rms_error: 0.5
   max_calibration_samples: 80
   max_fast_calibration_samples: 20
   use_entropy_maximization_subsampling: true # yamllint disable-line rule:truthy
   subsampling_pixel_cells: 16
   subsampling_tilt_resolution: 15.0
   subsampling_max_tilt_deg: 45.0
   use_post_rejection: true # yamllint disable-line rule:truthy
-  post_rejection_max_rms_error: 0.35
+  post_rejection_max_rms_error: 0.5
   plot_calibration_data_statistics: true # yamllint disable-line rule:truthy
   plot_calibration_results_statistics: true # yamllint disable-line rule:truthy
   viz_pixel_cells: 16
@@ -43,13 +43,13 @@ data_collector:
   max_allowed_pixel_speed: 10.0
   max_allowed_speed: 0.1
   filter_by_reprojection_error: true # yamllint disable-line rule:truthy
-  max_allowed_max_reprojection_error: 0.5
-  max_allowed_rms_reprojection_error: 0.3
+  max_allowed_max_reprojection_error: 2.0
+  max_allowed_rms_reprojection_error: 0.5
   filter_by_2d_redundancy: true # yamllint disable-line rule:truthy
   min_normalized_2d_center_difference: 0.05
   min_normalized_skew_difference: 0.05
   min_normalized_2d_size_difference: 0.05
-  filter_by_3d_redundancy: true # yamllint disable-line rule:truthy
+  filter_by_3d_redundancy: false # yamllint disable-line rule:truthy
   min_3d_center_difference: 1.0
   min_tilt_difference: 15.0
   heatmap_cells: 16

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/ceres_intrinsics_calibrator.yaml

@@ -9,15 +9,15 @@ calibration_parameters:
   use_ransac_pre_rejection: true # yamllint disable-line rule:truthy
   pre_rejection_iterations: 100
   pre_rejection_min_hypotheses: 6
-  pre_rejection_max_rms_error: 0.35
+  pre_rejection_max_rms_error: 0.5
   max_calibration_samples: 80
   max_fast_calibration_samples: 20
   use_entropy_maximization_subsampling: true # yamllint disable-line rule:truthy
   subsampling_pixel_cells: 16
   subsampling_tilt_resolution: 15.0
   subsampling_max_tilt_deg: 45.0
   use_post_rejection: true # yamllint disable-line rule:truthy
-  post_rejection_max_rms_error: 0.35
+  post_rejection_max_rms_error: 0.5
   plot_calibration_data_statistics: true # yamllint disable-line rule:truthy
   plot_calibration_results_statistics: true # yamllint disable-line rule:truthy
   viz_pixel_cells: 16
@@ -40,8 +40,8 @@ data_collector:
   max_allowed_pixel_speed: 10.0
   max_allowed_speed: 0.1
   filter_by_reprojection_error: true # yamllint disable-line rule:truthy
-  max_allowed_max_reprojection_error: 0.5
-  max_allowed_rms_reprojection_error: 0.3
+  max_allowed_max_reprojection_error: 2.0
+  max_allowed_rms_reprojection_error: 0.5
   filter_by_2d_redundancy: true # yamllint disable-line rule:truthy
   min_normalized_2d_center_difference: 0.05
   min_normalized_skew_difference: 0.05

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/general_intrinsics_calibrator.yaml

@@ -9,15 +9,15 @@ calibration_parameters:
   use_ransac_pre_rejection: true # yamllint disable-line rule:truthy
   pre_rejection_iterations: 100
   pre_rejection_min_hypotheses: 6
-  pre_rejection_max_rms_error: 0.35
+  pre_rejection_max_rms_error: 0.5
   max_calibration_samples: 80
   max_fast_calibration_samples: 20
   use_entropy_maximization_subsampling: true # yamllint disable-line rule:truthy
   subsampling_pixel_cells: 16
   subsampling_tilt_resolution: 15.0
   subsampling_max_tilt_deg: 45.0
   use_post_rejection: true # yamllint disable-line rule:truthy
-  post_rejection_max_rms_error: 0.35
+  post_rejection_max_rms_error: 0.5
   plot_calibration_data_statistics: true # yamllint disable-line rule:truthy
   plot_calibration_results_statistics: true # yamllint disable-line rule:truthy
   viz_pixel_cells: 16
@@ -43,13 +43,13 @@ data_collector:
   max_allowed_pixel_speed: 10.0
   max_allowed_speed: 0.1
   filter_by_reprojection_error: true # yamllint disable-line rule:truthy
-  max_allowed_max_reprojection_error: 0.5
-  max_allowed_rms_reprojection_error: 0.3
+  max_allowed_max_reprojection_error: 2.0
+  max_allowed_rms_reprojection_error: 0.5
   filter_by_2d_redundancy: true # yamllint disable-line rule:truthy
   min_normalized_2d_center_difference: 0.05
   min_normalized_skew_difference: 0.05
   min_normalized_2d_size_difference: 0.05
-  filter_by_3d_redundancy: true # yamllint disable-line rule:truthy
+  filter_by_3d_redundancy: false # yamllint disable-line rule:truthy
   min_3d_center_difference: 1.0
   min_tilt_difference: 15.0
   heatmap_cells: 16

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py

@@ -47,3 +47,6 @@ def detect(self, img: np.array, stamp):
 
     def single_shot_calibration_error(self, object_points, image_points) -> Tuple[float, float]:
         raise NotImplementedError
+
+    def restart_lost_frames_counter(self):
+        raise NotImplementedError

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py

@@ -14,6 +14,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+import logging
+
 import cv2
 from intrinsic_camera_calibrator.board_detections.chess_board_detection import ChessBoardDetection
 from intrinsic_camera_calibrator.board_detectors.board_detector import BoardDetector
@@ -34,6 +36,12 @@ def __init__(self, **kwargs):
         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)
         self.set_parameters(**kwargs["cfg"])
+        self.roi = None
+        self.lost_frames = 0
+        self.max_lost_frames = 3
+
+    def restart_lost_frames_counter(self):
+        self.lost_frames = self.max_lost_frames
 
     def detect(self, img: np.array, stamp: float):
         """Slot to detect boards from an image. Results are sent through the detection_results signals."""
@@ -54,15 +62,42 @@ def detect(self, img: np.array, stamp: float):
             resized_detection = self.resized_detection.value
             resized_max_resolution = self.resized_max_resolution.value
 
+        def get_roi(corners, frame_shape, padding=120):
+            # Region to keep track of the chessboard in the next frame
+            x_min, y_min = np.min(corners, axis=0).ravel().astype(int) - padding
+            x_max, y_max = np.max(corners, axis=0).ravel().astype(int) + padding
+            x_min, y_min = max(0, x_min), max(0, y_min)
+            x_max, y_max = min(frame_shape[1], x_max), min(frame_shape[0], y_max)
+            return (x_min, y_min, x_max, y_max)
+
         h, w = img.shape[0:2]
         grayscale = to_grayscale(img)
-
         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, stamp)
-                return
+            if self.roi is None or self.lost_frames >= self.max_lost_frames:
+                (detected, corners) = cv2.findChessboardCorners(
+                    grayscale, (cols, rows), flags=flags
+                )
+                # if chessboard was found, keep track of the region to try to detect easily in the next frame
+                if detected:
+                    self.roi = get_roi(corners, img.shape[:2])
+                    self.lost_frames = 0
+                else:
+                    self.lost_frames += 1
+                    self.detection_results_signal.emit(img, None, stamp)
+                    return
+            else:
+                roi_frame = grayscale[self.roi[1] : self.roi[3], self.roi[0] : self.roi[2]]
+                (detected, corners) = cv2.findChessboardCorners(
+                    roi_frame, (cols, rows), flags=flags
+                )
+                if detected:
+                    corners += (self.roi[0], self.roi[1])
+                    self.roi = get_roi(corners, img.shape[:2])
+                    self.lost_frames = 0
+                else:
+                    self.lost_frames += 1
+                    self.detection_results_signal.emit(img, None, stamp)
+                    return
         else:
             # Find the resized dimensions
             ratio = float(w) / float(h)
@@ -114,10 +149,16 @@ def detect(self, img: np.array, stamp: float):
             )
             np.fill_diagonal(dist_matrix, np.inf)
             min_distance = dist_matrix.min()
-            radius = int(np.ceil(min_distance * 0.5))
+            radius = max(
+                1, int(np.ceil(min_distance * 0.5))
+            )  # ensuring radius has a value of at least 1
 
             criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
-            corners = cv2.cornerSubPix(grayscale, corners, (radius, radius), (-1, -1), criteria)
+
+            try:
+                corners = cv2.cornerSubPix(grayscale, corners, (radius, radius), (-1, -1), criteria)
+            except Exception as e:
+                logging.error(e)
 
         image_points = corners.reshape((rows, cols, 2))
         x_array = cell_size * (np.array(range(cols)) - 0.5 * cols)
@@ -132,5 +173,4 @@ def detect(self, img: np.array, stamp: float):
             object_points=object_points,
             image_points=image_points,
         )
-
         self.detection_results_signal.emit(img, detection, stamp)