feat(drs): camera-lidar and lidar-lidar calibration

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml

@@ -0,0 +1,59 @@
+# yamllint disable-line rule:truthy
+board_type: chess_board
+board_parameters:
+  cols: 8
+  rows: 6
+  cell_size: 0.1
+
+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
+  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
+  plot_calibration_data_statistics: true # yamllint disable-line rule:truthy
+  plot_calibration_results_statistics: true # yamllint disable-line rule:truthy
+  viz_pixel_cells: 16
+  viz_tilt_resolution: 15.0
+  viz_max_tilt_deg: 45.0
+  viz_z_cells: 12
+  radial_distortion_coefficients: 3
+
+calibrator_type: opencv
+
+data_collector:
+  max_samples: 500
+  decorrelate_eval_samples: 5 # cspell:disable-line
+  max_allowed_tilt: 45.0
+  filter_by_speed: true # yamllint disable-line rule:truthy
+  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
+  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
+  min_3d_center_difference: 1.0
+  min_tilt_difference: 15.0
+  heatmap_cells: 16
+  rotation_heatmap_angle_res: 10
+  point_2d_hist_bins: 20
+  point_3d_hist_bins: 20
+
+chess_board_detector:
+  adaptive_thresh: false
+  normalize_image: false
+  fast_check: false
+  resized_detection: false
+  resized_max_resolution: 1000
+  sub_pixel_refinement: true

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml

@@ -0,0 +1,59 @@
+# yamllint disable-line rule:truthy
+board_type: chess_board
+board_parameters:
+  cols: 8
+  rows: 6
+  cell_size: 0.1
+
+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
+  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
+  plot_calibration_data_statistics: true # yamllint disable-line rule:truthy
+  plot_calibration_results_statistics: true # yamllint disable-line rule:truthy
+  viz_pixel_cells: 16
+  viz_tilt_resolution: 15.0
+  viz_max_tilt_deg: 45.0
+  viz_z_cells: 12
+  radial_distortion_coefficients: 5
+
+calibrator_type: opencv
+
+data_collector:
+  max_samples: 500
+  decorrelate_eval_samples: 5 # cspell:disable-line
+  max_allowed_tilt: 45.0
+  filter_by_speed: true # yamllint disable-line rule:truthy
+  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
+  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
+  min_3d_center_difference: 1.0
+  min_tilt_difference: 15.0
+  heatmap_cells: 16
+  rotation_heatmap_angle_res: 10
+  point_2d_hist_bins: 20
+  point_3d_hist_bins: 20
+
+chess_board_detector:
+  adaptive_thresh: false
+  normalize_image: false
+  fast_check: false
+  resized_detection: false
+  resized_max_resolution: 1000
+  sub_pixel_refinement: true

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml

@@ -1,5 +1,63 @@
-board_type: dot_board
+# yamllint disable-line rule:truthy
+board_type: chess_board
 board_parameters:
   cols: 8
   rows: 6
   cell_size: 0.1
+
+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.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.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
+  viz_tilt_resolution: 15.0
+  viz_max_tilt_deg: 45.0
+  viz_z_cells: 12
+  radial_distortion_coefficients: 3
+  rational_distortion_coefficients: 3
+  use_tangential_distortion: true
+  pre_calibration_num_samples: 40
+  regularization_weight: 0.2
+
+calibrator_type: ceres
+
+data_collector:
+  max_samples: 500
+  decorrelate_eval_samples: 5 # cspell:disable-line
+  max_allowed_tilt: 45.0
+  filter_by_speed: true # yamllint disable-line rule:truthy
+  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: 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: false # yamllint disable-line rule:truthy
+  min_3d_center_difference: 1.0
+  min_tilt_difference: 15.0
+  heatmap_cells: 16
+  rotation_heatmap_angle_res: 10
+  point_2d_hist_bins: 20
+  point_3d_hist_bins: 20
+
+chess_board_detector:
+  adaptive_thresh: false
+  normalize_image: false
+  fast_check: false
+  resized_detection: false
+  resized_max_resolution: 1000
+  sub_pixel_refinement: true

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_c2_120.yaml

@@ -0,0 +1,13 @@
+board_type: chess_board
+board_parameters:
+  cols: 8
+  rows: 6
+  cell_size: 0.1
+calibrator_type: opencv
+calibration_parameters:
+  radial_distortion_coefficients: 4
+  pre_rejection_max_rms_error: 5.0
+  post_rejection_max_rms_error: 3.0
+data_collector:
+  max_allowed_max_reprojection_error: 1.5
+  max_allowed_rms_reprojection_error: 1.0

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_c2_30.yaml

@@ -0,0 +1,11 @@
+board_type: chess_board
+board_parameters:
+  cols: 8
+  rows: 6
+  cell_size: 0.1
+calibrator_type: opencv
+calibration_parameters:
+  radial_distortion_coefficients: 2
+data_collector:
+  max_allowed_max_reprojection_error: 1.5
+  max_allowed_rms_reprojection_error: 1.0

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml

@@ -0,0 +1,63 @@
+# yamllint disable-line rule:truthy
+board_type: chess_board
+board_parameters:
+  cols: 8
+  rows: 6
+  cell_size: 0.1
+
+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.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.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
+  viz_tilt_resolution: 15.0
+  viz_max_tilt_deg: 45.0
+  viz_z_cells: 12
+  radial_distortion_coefficients: 3
+  rational_distortion_coefficients: 3
+  use_tangential_distortion: true
+  pre_calibration_num_samples: 40
+  regularization_weight: 0.2
+
+calibrator_type: ceres
+
+data_collector:
+  max_samples: 500
+  decorrelate_eval_samples: 5 # cspell:disable-line
+  max_allowed_tilt: 45.0
+  filter_by_speed: true # yamllint disable-line rule:truthy
+  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: 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: false # yamllint disable-line rule:truthy
+  min_3d_center_difference: 1.0
+  min_tilt_difference: 15.0
+  heatmap_cells: 16
+  rotation_heatmap_angle_res: 10
+  point_2d_hist_bins: 20
+  point_3d_hist_bins: 20
+
+chess_board_detector:
+  adaptive_thresh: false
+  normalize_image: false
+  fast_check: false
+  resized_detection: false
+  resized_max_resolution: 1000
+  sub_pixel_refinement: true

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py

@@ -62,23 +62,55 @@ def squared_error(p, p1, p2):
             p = p - p1
             p2 = p2 - p1
             p2 /= np.linalg.norm(p2)
-            return np.abs(np.power(np.linalg.norm(p), 2) - np.power(np.dot(p, p2), 2))
+            squared_distance = np.abs(np.power(np.linalg.norm(p), 2) - np.power(np.dot(p, p2), 2))
+            return squared_distance
 
-        if self._cached_linear_error_rms is not None:
-            return self._cached_linear_error_rms
+        if (
+            self._cached_linear_error_rows_rms is not None
+            and self._cached_linear_error_cols_rms is not None
+        ):
+            return self._cached_linear_error_rows_rms, self._cached_linear_error_cols_rms
 
-        error = 0
+        error_rows = 0
+        pct_err_rows = 0.0
 
         for j in range(self.rows):
             p1 = self.image_points[j, 0]
             p2 = self.image_points[j, -1]
+            points_dist = np.linalg.norm(p2 - p1)
 
             for i in range(1, self.cols - 1):
                 p = self.image_points[j, i]
-                error += squared_error(p, p1, p2)
+                sq_error = squared_error(p, p1, p2)
+                error_rows += sq_error
+                pct_err_rows += np.sqrt(sq_error) / points_dist
 
-        self._cached_linear_error_rms = np.sqrt(error / (self.rows * (self.cols - 2)))
-        return self._cached_linear_error_rms
+        self._cached_linear_error_rows_rms = np.sqrt(error_rows / (self.rows * (self.cols - 2)))
+        pct_err_rows = pct_err_rows / (self.rows * (self.cols - 2))
+
+        error_cols = 0
+        pct_err_cols = 0.0
+
+        for j in range(self.cols):
+            p1 = self.image_points[0, j]
+            p2 = self.image_points[-1, j]
+            points_dist = np.linalg.norm(p2 - p1)
+
+            for i in range(1, self.rows - 1):
+                p = self.image_points[i, j]
+                sq_error = squared_error(p, p1, p2)
+                error_cols += sq_error
+                pct_err_cols += np.sqrt(sq_error) / points_dist
+
+        self._cached_linear_error_cols_rms = np.sqrt(error_cols / (self.cols * (self.rows - 2)))
+        pct_err_cols = pct_err_cols / (self.cols * (self.rows - 2))
+
+        return (
+            self._cached_linear_error_rows_rms,
+            self._cached_linear_error_cols_rms,
+            pct_err_rows,
+            pct_err_cols,
+        )
 
     def get_flattened_cell_sizes(self):
         if self._cached_flattened_cell_sizes is not None:

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py

@@ -44,6 +44,8 @@ def __init__(
         self._cached_normalized_skew = None
         self._cached_normalized_size = None
         self._cached_linear_error_rms = None
+        self._cached_linear_error_rows_rms = None
+        self._cached_linear_error_cols_rms = None
         self._cached_flattened_cell_sizes = None
         self._cached_center_2d = None
 
@@ -99,6 +101,10 @@ def get_linear_error_rms(self) -> float:
         """Return RMS error product of the projection of the lines of each row of the detection into the line produced by the first and line point of each row."""
         raise NotImplementedError
 
+    def restart_linearity_heatmap(self):
+        """Restart linearity heatmap."""
+        raise NotImplementedError
+
     def get_center_2d(self) -> np.array:
         """Return the center of detection in the image."""
         if self._cached_center_2d is not None:
@@ -232,3 +238,27 @@ def get_speed(self, last: "BoardDetection") -> float:
         last_image_points = last.get_flattened_image_points()
 
         return np.linalg.norm(current_image_points - last_image_points, axis=-1).mean()
+
+    def get_aspect_ratio_pattern(self, model: CameraModel) -> float:
+        """Get aspect ratio using the calibration pattern, which should be squared."""
+        tilt, pan = self.get_rotation_angles(model)
+        acceptance_angle = 10
+
+        # dont update if we the detection has big angles, calculation will not be accurate
+        if np.abs(tilt) > acceptance_angle or np.abs(pan) > acceptance_angle:
+            return 0.0
+        # Calculate distances between adjacent corners
+        aspect_ratio = 0
+        count = 0
+        for j in range(self.rows - 1):
+            for i in range(self.cols - 1):
+                p = self.image_points[j, i]
+                point_col = self.image_points[j + 1, i]
+                point_row = self.image_points[j, i + 1]
+                horizontal_distance = np.linalg.norm(p - point_row)
+                vertical_distance = np.linalg.norm(p - point_col)
+                aspect_ratio = aspect_ratio + (horizontal_distance / vertical_distance)
+                count += 1
+        aspect_ratio = aspect_ratio / ((self.rows - 1) * (self.cols - 1))
+
+        return aspect_ratio