From e1d3baab3398210b05891f8596fab6899b0a2027 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 5 Nov 2024 13:08:36 +0900 Subject: [PATCH 01/36] [fix] speed filter option wrong type, added params to config camera calibration file --- .../config/intrinsics_calibrator.yaml | 47 ++++++++++++++++++- .../data_collector.py | 2 +- 2 files changed, 47 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index 740178bd..393f880d 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -1,5 +1,50 @@ -board_type: dot_board +board_type: chess_board board_parameters: cols: 8 rows: 6 cell_size: 0.1 + +calibration_parameters: + use_ransac_pre_rejection: True + 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 + subsampling_pixel_cells: 16 + subsampling_tilt_resolution: 15.0 + subsampling_max_tilt_deg: 45.0 + use_post_rejection: True + post_rejection_max_rms_error: 0.35 + plot_calibration_data_statistics: True + plot_calibration_results_statistics: True + 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 + max_allowed_tilt: 45.0 + filter_by_speed: True + max_allowed_pixel_speed: 10.0 + max_allowed_speed: 0.1 + filter_by_reprojection_error: True + max_allowed_max_reprojection_error: 0.5 + max_allowed_rms_reprojection_error: 0.3 + filter_by_2d_redundancy: True + 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 + 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 4c3b1dce..b1420dd4 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -467,7 +467,7 @@ def process_detection( speed = 0 if self.last_detection is None else detection.get_speed(self.last_detection) self.last_detection = detection - accepted &= speed < self.max_allowed_speed + accepted &= speed < self.max_allowed_pixel_speed.value if self.filter_by_reprojection_error: reprojection_errors = detection.get_reprojection_errors() From 75ac1582e26c31de03b41ae338a0026dfaa3ccaf Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 5 Nov 2024 13:32:19 +0900 Subject: [PATCH 02/36] [wip] functions to get training coverage percentage for skew, board size --- .../data_collector.py | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index b1420dd4..396ff742 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -530,3 +530,47 @@ def process_detection( return CollectionStatus.ACCEPTED return CollectionStatus.REDUNDANT + + def get_skew_coverage(self): + # Define the number of intervals + num_intervals = 50 + interval_size = 1 / num_intervals + # Create a unique set to store covered intervals + covered_intervals = set() + # range in radians ToDo: define the range + skew_range = np.array([0,1.04]) + + for detection in self.training_data.get_detections(): + if skew_range[0] <= detection.get_normalized_skew() < skew_range[1]: + interval_index = int(detection.get_normalized_skew() / interval_size) + covered_intervals.add(interval_index) + + # Calculate the percentage of covered intervals + percentage_coverage = (len(covered_intervals) / num_intervals) # * 100 + + return percentage_coverage + + def get_skew_percentage(self): + return self.get_skew_coverage() + + def get_size_coverage(self): + # Define the number of intervals + num_intervals = 20 + interval_size = 1 / num_intervals + # Create a set to store covered intervals + covered_intervals = set() + # range for board size ToDo: define the range + size_range = np.array([0.08,0.21]) + + for detection in self.training_data.get_detections(): + if size_range[0] <= detection.get_normalized_size() < size_range[1]: + interval_index = int(detection.get_normalized_size() / interval_size) + covered_intervals.add(interval_index) + + # Calculate the percentage of covered intervals + percentage_coverage = (len(covered_intervals) / num_intervals) # * 100 + + return percentage_coverage + + def get_size_percentage(self): + return self.get_size_coverage() From 85426552e09254b597eef37597b81d0c08dfb498 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 5 Nov 2024 14:29:53 +0900 Subject: [PATCH 03/36] [New] added UI indicators for speed, skew and size coverage of the board --- .../camera_calibrator.py | 9 +++ .../views/image_view.py | 75 +++++++++++++++++++ 2 files changed, 84 insertions(+) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index f77df4d9..e62b7b3b 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -123,6 +123,7 @@ def __init__(self, cfg): self.image_view_mode = ImageViewMode.SOURCE_UNRECTIFIED self.paused = False + self.last_detection = None for calibrator_type in CalibratorEnum: calibrator_cfg = defaultdict() @@ -949,6 +950,14 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Evaluation occupancy: {100.0*self.data_collector.get_evaluation_occupancy_rate():.2f}" # noqa E231 ) + board_speed = 0 if self.last_detection is None else detection.get_speed(self.last_detection) + self.last_detection = detection + self.image_view.set_draw_indicators(board_speed, + self.data_collector.max_allowed_pixel_speed.value, + self.data_collector.get_skew_percentage(), + self.data_collector.get_size_percentage(), + True) + # Draw training / evaluation points self.image_view.set_draw_training_points(self.draw_training_points_checkbox.isChecked()) self.image_view.set_draw_evaluation_points(self.draw_evaluation_points_checkbox.isChecked()) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 73d64641..7e695f6f 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -27,6 +27,7 @@ from PySide2.QtGui import QPainter from PySide2.QtGui import QPen from PySide2.QtGui import QPixmap +from PySide2.QtGui import QFont from PySide2.QtWidgets import QGraphicsItem from PySide2.QtWidgets import QGraphicsView import cv2 @@ -202,6 +203,77 @@ def set_rendering_alpha(self, value: float): """Set the alpha channel of the drawings.""" self.rendering_alpha = value + def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float, + skew_percentage: float, board_size_percentage: float, + value: bool): + """Set values for the indicators.""" + self.current_board_speed = board_speed + self.max_board_allowed_speed = max_allowed_board_speed + self.skew_percentage = skew_percentage + self.board_size_percentage = board_size_percentage + self.is_draw_speed_indicator = value + + def draw_indicators(self, painter: QPainter, display_size): + """Draw indicators for speed, skew and size of the detected board.""" + color_green = QColor(0.0, 255, 0.0, int(255 * self.rendering_alpha)) + color_red = QColor(255, 0.0, 0.0, int(255 * self.rendering_alpha)) + if (self.current_board_speed < self.max_board_allowed_speed): + pen = QPen(color_green) + brush = QBrush(color_green) + else: + pen = QPen(color_red) + brush = QBrush(color_red) + painter.setPen(pen) + painter.setBrush(brush) + speed_indicator = QRectF(QPointF(0, 0), QSize(display_size.width(), int(display_size.height()*0.04))) + # Draw the rectangle + painter.drawRect(speed_indicator) + + # Set the font according to the window size + font_size = max(10, display_size.height() / 40) + font = QFont("Arial", font_size) + painter.setFont(font) + position_text_speed = QPointF(int(display_size.width() * .01), int(display_size.height() * .10)) + painter.drawText(position_text_speed, "Speed") + + thresold_to_be_green = 0.3 + if self.skew_percentage < thresold_to_be_green: + pen_skew = QPen(color_red) + brush_skew = QBrush(color_red) + else: + pen_skew = QPen(color_green) + brush_skew = QBrush(color_green) + painter.setPen(pen_skew) + painter.setBrush(brush_skew) + + # Draw Skew text + position_text_skew = QPointF(int(display_size.width() * .01), int(display_size.height() * .88)) + painter.drawText(position_text_skew, "Skew " + str(int(self.skew_percentage * 100)) + "%") + + skew_indicator = QRectF(QPointF(int(display_size.width() * .12), int(display_size.height() * .85)), + QSize(display_size.width() * 0.08 * self.skew_percentage, int(display_size.height()*0.03))) + # Draw the skew progrees bar + painter.drawRect(skew_indicator) + + thresold_to_be_green = 0.2 + if self.board_size_percentage < thresold_to_be_green: + pen_size_board = QPen(color_red) + brush_size_board = QBrush(color_red) + else: + pen_size_board = QPen(color_green) + brush_size_board = QBrush(color_green) + + painter.setPen(pen_size_board) + painter.setBrush(brush_size_board) + + # Draw board size text + position_text_size = QPointF(int(display_size.width() * .01), int(display_size.height() * .93)) + painter.drawText(position_text_size, "Size " + str(int(self.board_size_percentage * 100)) + "%") + board_size_indicator = QRectF(QPointF(int(display_size.width() * .12), int(display_size.height() * .90)), + QSize(display_size.width() * 0.08 * self.board_size_percentage, int(display_size.height() * 0.03))) + # Draw the board size progrees bar + painter.drawRect(board_size_indicator) + def pixmap(self) -> QPixmap: """Return the rendering QPixmap.""" return self.raw_pixmap @@ -440,6 +512,9 @@ def paint(self, painter: QPainter, option, widget): if self.is_draw_evaluation_heatmap and self.evaluation_heatmap is not None: self.draw_heatmap(painter, self.evaluation_heatmap, display_size) + + if self.is_draw_speed_indicator and self.current_board_speed is not None: + self.draw_indicators(painter, display_size) def boundingRect(self): """Return the size of the Widget to let other widgets adjust correctly.""" From ae33f40750d4cca0ba87ebd8981a665209aeabc5 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Thu, 7 Nov 2024 15:07:56 +0900 Subject: [PATCH 04/36] [wip] Added Linear error rows/cols indicator --- .../board_detections/array_board_detection.py | 21 +++- .../board_detections/board_detection.py | 2 + .../camera_calibrator.py | 43 +++++++- .../views/image_view.py | 102 +++++++++++++++++- 4 files changed, 157 insertions(+), 11 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index dfbd8672..8433c536 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -64,8 +64,8 @@ def squared_error(p, p1, p2): p2 /= np.linalg.norm(p2) return np.abs(np.power(np.linalg.norm(p), 2) - np.power(np.dot(p, p2), 2)) - 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 @@ -77,8 +77,21 @@ def squared_error(p, p1, p2): p = self.image_points[j, i] error += squared_error(p, p1, p2) - 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 / (self.rows * (self.cols - 2))) + + error_cols = 0 + + for j in range(self.cols): + p1 = self.image_points[0, j] + p2 = self.image_points[-1, j] + + for i in range(1, self.rows - 1): + p = self.image_points[i, j] + error_cols += squared_error(p, p1, p2) + + self._cached_linear_error_cols_rms = np.sqrt(error_cols / (self.cols * (self.rows - 2))) + + return self._cached_linear_error_rows_rms, self._cached_linear_error_cols_rms def get_flattened_cell_sizes(self): if self._cached_flattened_cell_sizes is not None: diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index bb02b831..e79b25ff 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index e62b7b3b..b7e4d27c 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -463,6 +463,8 @@ def make_detection_group(self): self.raw_detection_label = QLabel("Detected:") self.raw_linear_error_rms_label = QLabel("Linear error (rms):") + self.raw_linear_error_rows_rms_label = QLabel("Linear error rows (rms):") + self.raw_linear_error_cols_rms_label = QLabel("Linear error cols (rms):") self.rough_tilt_label = QLabel("Rough tilt:") self.rough_angles_label = QLabel("Rough angles:") self.rough_position_label = QLabel("Rough position:") @@ -486,6 +488,8 @@ def make_detection_group(self): raw_detection_results_layout.addWidget(self.skew_label) raw_detection_results_layout.addWidget(self.relative_area_label) raw_detection_results_layout.addWidget(self.raw_linear_error_rms_label) + raw_detection_results_layout.addWidget(self.raw_linear_error_rows_rms_label) + raw_detection_results_layout.addWidget(self.raw_linear_error_cols_rms_label) single_shot_detection_results_layout.addWidget( self.single_shot_reprojection_error_max_label @@ -603,6 +607,9 @@ def draw_evaluation_heatmap_callback(value): self.draw_evaluation_heatmap_checkbox.setChecked(False) self.draw_evaluation_heatmap_checkbox.stateChanged.connect(draw_evaluation_heatmap_callback) + self.draw_indicators_checkbox = QCheckBox("Draw indicators") + self.draw_indicators_checkbox.setChecked(False) + rendering_alpha_label = QLabel("Drawings alpha:") self.rendering_alpha_spinbox = QDoubleSpinBox() @@ -624,6 +631,14 @@ def draw_evaluation_heatmap_callback(value): ) self.undistortion_alpha_spinbox.setEnabled(False) + indicators_alpha_label = QLabel("Indicators alpha:") + + self.indicators_alpha_spinbox = QDoubleSpinBox() + self.indicators_alpha_spinbox.setDecimals(2) + self.indicators_alpha_spinbox.setRange(0.0, 1.0) + self.indicators_alpha_spinbox.setSingleStep(0.05) + self.indicators_alpha_spinbox.setValue(1.0) + visualization_options_layout = QVBoxLayout() visualization_options_layout.setAlignment(Qt.AlignTop) visualization_options_layout.addWidget(draw_detection_checkbox) @@ -631,10 +646,13 @@ def draw_evaluation_heatmap_callback(value): visualization_options_layout.addWidget(self.draw_evaluation_points_checkbox) visualization_options_layout.addWidget(self.draw_training_heatmap_checkbox) visualization_options_layout.addWidget(self.draw_evaluation_heatmap_checkbox) + visualization_options_layout.addWidget(self.draw_indicators_checkbox) visualization_options_layout.addWidget(rendering_alpha_label) visualization_options_layout.addWidget(self.rendering_alpha_spinbox) visualization_options_layout.addWidget(undistortion_alpha_label) visualization_options_layout.addWidget(self.undistortion_alpha_spinbox) + visualization_options_layout.addWidget(indicators_alpha_label) + visualization_options_layout.addWidget(self.indicators_alpha_spinbox) self.visualization_options_group.setLayout(visualization_options_layout) def start( @@ -844,6 +862,8 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.raw_detection_label.setText("Detected: False") self.raw_linear_error_rms_label.setText("Linear error rms:") + self.raw_linear_error_rows_rms_label.setText("Linear error rows rms:") + self.raw_linear_error_cols_rms_label.setText("Linear error cols rms:") self.rough_tilt_label.setText("Rough tilt:") self.rough_angles_label.setText("Rough angles:") self.rough_position_label.setText("Rough position:") @@ -853,6 +873,14 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.single_shot_reprojection_error_max_label.setText("Reprojection error (max):") self.single_shot_reprojection_error_avg_label.setText("Reprojection error (avg):") self.single_shot_reprojection_error_rms_label.setText("Reprojection error (rms):") + board_speed = None + self.image_view.set_draw_indicators(board_speed, + self.data_collector.max_allowed_pixel_speed.value, + self.data_collector.get_skew_percentage(), + self.data_collector.get_size_percentage(), + 0, 0, + self.indicators_alpha_spinbox.value(), + False) else: camera_model = ( @@ -916,8 +944,15 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im rough_angles = detection.get_rotation_angles(camera_model) self.raw_detection_label.setText("Detected: True") - self.raw_linear_error_rms_label.setText( - f"Linear error rms: {detection.get_linear_error_rms():.2f} px" # noqa E231 + # self.raw_linear_error_rms_label.setText( + # f"Linear rows error rms: {detection.get_linear_error_rms():.2f} px" # noqa E231 + # ) + err_rms_rows, err_rms_cols = detection.get_linear_error_rms() + self.raw_linear_error_rows_rms_label.setText( + f"Linear error rows rms: {err_rms_rows:.2f} px" # noqa E231 + ) + self.raw_linear_error_cols_rms_label.setText( + f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt():.2f} degrees" # noqa E231 @@ -956,7 +991,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.data_collector.max_allowed_pixel_speed.value, self.data_collector.get_skew_percentage(), self.data_collector.get_size_percentage(), - True) + err_rms_rows, err_rms_cols, + self.indicators_alpha_spinbox.value(), + self.draw_indicators_checkbox.isChecked()) # Draw training / evaluation points self.image_view.set_draw_training_points(self.draw_training_points_checkbox.isChecked()) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 7e695f6f..bac3641b 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -205,18 +205,23 @@ def set_rendering_alpha(self, value: float): def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float, skew_percentage: float, board_size_percentage: float, + rows_linear_error: float, cols_linear_error: float, + alpha_indicators: float, value: bool): """Set values for the indicators.""" self.current_board_speed = board_speed self.max_board_allowed_speed = max_allowed_board_speed self.skew_percentage = skew_percentage self.board_size_percentage = board_size_percentage - self.is_draw_speed_indicator = value + self.rows_linear_error = rows_linear_error + self.cols_linear_error = cols_linear_error + self.alpha_indicators = alpha_indicators + self.is_draw_indicators = value def draw_indicators(self, painter: QPainter, display_size): """Draw indicators for speed, skew and size of the detected board.""" - color_green = QColor(0.0, 255, 0.0, int(255 * self.rendering_alpha)) - color_red = QColor(255, 0.0, 0.0, int(255 * self.rendering_alpha)) + color_green = QColor(0.0, 255, 0.0, int(255 * self.alpha_indicators)) + color_red = QColor(255, 0.0, 0.0, int(255 * self.alpha_indicators)) if (self.current_board_speed < self.max_board_allowed_speed): pen = QPen(color_green) brush = QBrush(color_green) @@ -274,6 +279,13 @@ def draw_indicators(self, painter: QPainter, display_size): # Draw the board size progrees bar painter.drawRect(board_size_indicator) + # Draw Linear errors text + position_text_err_rows = QPointF(int(display_size.width() * .85), int(display_size.height() * .88)) + painter.drawText(position_text_err_rows, "Err Rows " + str(round(self.rows_linear_error, 1)) + "px") + + position_text_err_cols = QPointF(int(display_size.width() * .85), int(display_size.height() * .93)) + painter.drawText(position_text_err_cols, "Err Cols " + str(round(self.cols_linear_error, 1)) + "px") + def pixmap(self) -> QPixmap: """Return the rendering QPixmap.""" return self.raw_pixmap @@ -359,6 +371,87 @@ def draw_detection_points(self, painter: QPainter): painter.setPen(pen) painter.drawLine(QPointF(p1[0], p1[1]), QPointF(p2[0], p2[1])) + def draw_linear_errors(self, painter: QPainter): + #### ToDo here investigating how elements were drawn specially ines + # determine the linearity error + if self.points_list is None: + return + def compute_line_equation(p1, p2): + A = p2[1] - p1[1] + B = p1[0] - p2[0] + C = p2[0] * p1[1] - p1[0] * p2[1] + return A, B, C + # Function to compute the perpendicular distance from point (x0, y0) to the line Ax + By + C = 0 + def perpendicular_distance(x0, y0, A, B, C): + return abs(A * x0 + B * y0 + C) / np.sqrt(A ** 2 + B ** 2) + """Draw new lines.""" + alpha = 255 + pen = QPen(QColor(0.0, 255.0, 0.0, alpha)) + painter.setPen(pen) + for i in range(len(self.points_list)): + p1 = self.image_to_view_factor * self.points_list[i][0] # first element + p2 = self.image_to_view_factor * self.points_list[i][-1] # last element + line_pixel_distance = np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2) + panel_width_mm = 700 + conv_factor_chessboard_mm = panel_width_mm / line_pixel_distance + # Set the font according to the window size + font_size = 12 + font = QFont("Arial", font_size) + painter.setFont(font) + # position_text_speed = QPointF(p1[0],p1[1]) + # painter.drawText(position_text_speed, "S") + # position_text_speed = QPointF(p2[0],p2[1]) + # painter.drawText(position_text_speed, "E") + # painter.drawLine(QPointF(p1[0], p1[1]), QPointF(p2[0],p2[1])) + + A, B, C = compute_line_equation(p1, p2) + count = 0 + mse = 0 + for point in self.points_list[i]: + x0, y0 = self.image_to_view_factor * point + distance_mm = conv_factor_chessboard_mm * perpendicular_distance(x0, y0, A, B, C) + mse = mse + distance_mm ** 2 + count += 1 + mse = mse / count + # position_text_speed = QPointF(p1[0],p1[1]) + # painter.drawText(position_text_speed, "E:" + str(round(mse,1))) + + ## we got for rows now for columns + + alpha = 255 + pen = QPen(QColor(255.0, 0.0, 0.0, alpha)) + painter.setPen(pen) + for i in range(len(self.points_list[0])): + p1 = self.image_to_view_factor * self.points_list[0][i] # first element + p2 = self.image_to_view_factor * self.points_list[-1][i] # last element + line_pixel_distance = np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2) + panel_width_mm = 700 + conv_factor_chessboard_mm = panel_width_mm / line_pixel_distance + # Set the font according to the window size + # font_size = 12 + # font = QFont("Arial", font_size) + # painter.setFont(font) + # position_text_speed = QPointF(p1[0],p1[1]) + # painter.drawText(position_text_speed, "nS") + # position_text_speed = QPointF(p2[0],p2[1]) + # painter.drawText(position_text_speed, "nE") + # painter.drawLine(QPointF(p1[0], p1[1]), QPointF(p2[0],p2[1])) + + A, B, C = compute_line_equation(p1, p2) + count = 0 + mse = 0 + #worst_dst_meters = 0 + for j in range (1,len(self.points_list)-1): + x0, y0 = self.image_to_view_factor * self.points_list[j][i] + distance_mm = conv_factor_chessboard_mm * perpendicular_distance(x0, y0, A, B, C) + mse = mse + distance_mm ** 2 + # position_text_p = QPointF(x0,y0) + # painter.drawText(position_text_p, str(j)) + count += 1 + mse = mse / count + # position_text_speed = QPointF(p1[0],p1[1]) + # painter.drawText(position_text_speed, "E:" + str(round(mse,1))) + def draw_points(self, painter: QPainter, points: np.array): """Draw a set of points as rectangles.""" pen = QPen(QColor(255, 0.0, 255, int(255 * self.rendering_alpha))) @@ -513,8 +606,9 @@ def paint(self, painter: QPainter, option, widget): if self.is_draw_evaluation_heatmap and self.evaluation_heatmap is not None: self.draw_heatmap(painter, self.evaluation_heatmap, display_size) - if self.is_draw_speed_indicator and self.current_board_speed is not None: + if self.is_draw_indicators and self.current_board_speed is not None: self.draw_indicators(painter, display_size) + self.draw_linear_errors(painter) def boundingRect(self): """Return the size of the Widget to let other widgets adjust correctly.""" From 81aaf2f648717b266f53a7c3dd02acc8214dae03 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Fri, 8 Nov 2024 15:12:05 +0900 Subject: [PATCH 05/36] [wip] manually fix problem loading intrinsics file, enabling disabling elements for eval mode --- .../intrinsic_camera_calibrator/camera_calibrator.py | 6 ++++++ .../intrinsic_camera_calibrator/camera_model.py | 4 ++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index b7e4d27c..c68e9f8b 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -688,6 +688,12 @@ def start( if self.operation_mode == OperationMode.EVALUATION: self.calibration_button.setEnabled(False) + # ToDo Verify if this changes everything correctly + self.calibrated_camera_model = self.current_camera_model + self.image_view_type_combobox.setEnabled(True) + self.image_view_type_combobox.setCurrentIndex(1) # rectified + self.undistortion_alpha_spinbox.setEnabled(True) + self.calibration_parameters_button.setEnabled(False) self.detector.moveToThread(self.detector_thread) self.detector.detection_results_signal.connect(self.process_detection_results) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py index 1c3bba0e..8bf37bd6 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py @@ -218,8 +218,8 @@ def from_dict(self, d): self.k = np.array(d["camera_matrix"]["data"]).reshape( d["camera_matrix"]["rows"], d["camera_matrix"]["cols"] ) - self.d = np.array(d["distortion_model"]["data"]).reshape( - d["distortion_model"]["rows"], d["distortion_model"]["cols"] + self.d = np.array(d["distortion_coefficients"]["data"]).reshape( + d["distortion_coefficients"]["rows"], d["distortion_coefficients"]["cols"] ) From bd3225279a73bf403d4b8012844c504b20823ced Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Mon, 11 Nov 2024 11:47:02 +0900 Subject: [PATCH 06/36] [New] Added percentage linear error indicator rows cols --- .../board_detections/array_board_detection.py | 31 +++++++++++++------ .../camera_calibrator.py | 4 ++- .../views/image_view.py | 13 +++++--- 3 files changed, 34 insertions(+), 14 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index 8433c536..7fa66904 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -62,36 +62,49 @@ 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_rows_rms is not None and self._cached_linear_error_cols_rms is not None: + 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_rows_rms = np.sqrt(error_rows / (self.rows * (self.cols - 2))) + pct_err_rows = pct_err_rows / (self.rows * (self.cols - 2)) - self._cached_linear_error_rows_rms = np.sqrt(error / (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] - error_cols += squared_error(p, p1, p2) + 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))) - - return self._cached_linear_error_rows_rms, self._cached_linear_error_cols_rms + 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: diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index b7e4d27c..044b3284 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -879,6 +879,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.data_collector.get_skew_percentage(), self.data_collector.get_size_percentage(), 0, 0, + 0, 0, self.indicators_alpha_spinbox.value(), False) @@ -947,7 +948,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im # self.raw_linear_error_rms_label.setText( # f"Linear rows error rms: {detection.get_linear_error_rms():.2f} px" # noqa E231 # ) - err_rms_rows, err_rms_cols = detection.get_linear_error_rms() + err_rms_rows, err_rms_cols, pct_err_rows, pct_err_cols = detection.get_linear_error_rms() self.raw_linear_error_rows_rms_label.setText( f"Linear error rows rms: {err_rms_rows:.2f} px" # noqa E231 ) @@ -992,6 +993,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.data_collector.get_skew_percentage(), self.data_collector.get_size_percentage(), err_rms_rows, err_rms_cols, + pct_err_rows, pct_err_cols, self.indicators_alpha_spinbox.value(), self.draw_indicators_checkbox.isChecked()) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index bac3641b..ce5b1376 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -206,6 +206,7 @@ def set_rendering_alpha(self, value: float): def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float, skew_percentage: float, board_size_percentage: float, rows_linear_error: float, cols_linear_error: float, + pct_err_rows: float, pct_err_cols: float, alpha_indicators: float, value: bool): """Set values for the indicators.""" @@ -215,6 +216,8 @@ def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float self.board_size_percentage = board_size_percentage self.rows_linear_error = rows_linear_error self.cols_linear_error = cols_linear_error + self.pct_err_rows = pct_err_rows + self.pct_err_cols = pct_err_cols self.alpha_indicators = alpha_indicators self.is_draw_indicators = value @@ -280,11 +283,13 @@ def draw_indicators(self, painter: QPainter, display_size): painter.drawRect(board_size_indicator) # Draw Linear errors text - position_text_err_rows = QPointF(int(display_size.width() * .85), int(display_size.height() * .88)) - painter.drawText(position_text_err_rows, "Err Rows " + str(round(self.rows_linear_error, 1)) + "px") + position_text_err_rows = QPointF(int(display_size.width() * .80), int(display_size.height() * .88)) + painter.drawText(position_text_err_rows, "ErrRows " + str(round(self.rows_linear_error, 1)) + "px " + \ + str(round(self.pct_err_rows * 100, 1)) + "%") - position_text_err_cols = QPointF(int(display_size.width() * .85), int(display_size.height() * .93)) - painter.drawText(position_text_err_cols, "Err Cols " + str(round(self.cols_linear_error, 1)) + "px") + position_text_err_cols = QPointF(int(display_size.width() * .80), int(display_size.height() * .93)) + painter.drawText(position_text_err_cols, "ErrCols " + str(round(self.cols_linear_error, 1)) + "px " + \ + str(round(self.pct_err_cols *100, 1)) + "%") def pixmap(self) -> QPixmap: """Return the rendering QPixmap.""" From d6261a259b0e9417ce4bfc4038b82f399067dcf0 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 12 Nov 2024 14:02:42 +0900 Subject: [PATCH 07/36] [New] Aspect ratio measurement and heatmap for linearity added --- .../board_detections/array_board_detection.py | 27 +++++++++++++ .../board_detections/board_detection.py | 4 ++ .../camera_calibrator.py | 31 ++++++++++++--- .../data_collector.py | 39 ++++++++++++++++++- .../views/image_view.py | 11 ++++++ 5 files changed, 105 insertions(+), 7 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index 7fa66904..c489cfca 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -122,3 +122,30 @@ def get_flattened_cell_sizes(self): self._cached_flattened_cell_sizes = cell_sizes.flatten() return self._cached_flattened_cell_sizes + + def get_aspect_ratio_pattern_squares(self) -> float: + # Calculate distances between adjacent corners + horizontal_distances = [] + vertical_distances = [] + + for j in range(self.rows): + for i in range(self.cols - 1): + p1 = self.image_points[j, i] + p2 = self.image_points[j, i + 1] + points_distance = np.linalg.norm(p2 - p1) + horizontal_distances.append(points_distance) + + for j in range(self.cols): + for i in range(self.rows - 1): + p1 = self.image_points[i, j] + p2 = self.image_points[i + 1, j] + points_distance = np.linalg.norm(p2 - p1) + vertical_distances.append(points_distance) + + avg_horizontal = np.mean(horizontal_distances) + avg_vertical = np.mean(vertical_distances) + aspect_ratio = avg_horizontal / avg_vertical + #print("aspect_ratio: ", aspect_ratio) + + return aspect_ratio + diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index e79b25ff..499a21ab 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -101,6 +101,10 @@ def get_flattened_image_points(self) -> np.array: 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 get_aspect_ratio_pattern_squares(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 get_center_2d(self) -> np.array: """Return the center of detection in the image.""" diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index becf9625..5055cd5c 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -462,9 +462,9 @@ def make_detection_group(self): self.single_shot_detection_results_group.setFlat(True) self.raw_detection_label = QLabel("Detected:") - self.raw_linear_error_rms_label = QLabel("Linear error (rms):") self.raw_linear_error_rows_rms_label = QLabel("Linear error rows (rms):") self.raw_linear_error_cols_rms_label = QLabel("Linear error cols (rms):") + self.aspect_ratio_label = QLabel("Aspect Ratio:") self.rough_tilt_label = QLabel("Rough tilt:") self.rough_angles_label = QLabel("Rough angles:") self.rough_position_label = QLabel("Rough position:") @@ -487,9 +487,9 @@ def make_detection_group(self): raw_detection_results_layout.addWidget(self.rough_position_label) raw_detection_results_layout.addWidget(self.skew_label) raw_detection_results_layout.addWidget(self.relative_area_label) - raw_detection_results_layout.addWidget(self.raw_linear_error_rms_label) raw_detection_results_layout.addWidget(self.raw_linear_error_rows_rms_label) raw_detection_results_layout.addWidget(self.raw_linear_error_cols_rms_label) + raw_detection_results_layout.addWidget(self.aspect_ratio_label) single_shot_detection_results_layout.addWidget( self.single_shot_reprojection_error_max_label @@ -602,11 +602,20 @@ def draw_training_heatmap_callback(value): def draw_evaluation_heatmap_callback(value): self.image_view.set_draw_evaluation_heatmap(value == Qt.Checked) self.should_process_image.emit() + + def draw_linearity_heatmap_callback(value): + self.image_view.set_draw_linearity_heatmap(value == Qt.Checked) + self.should_process_image.emit() + self.draw_evaluation_heatmap_checkbox = QCheckBox("Draw evaluation occupancy") self.draw_evaluation_heatmap_checkbox.setChecked(False) self.draw_evaluation_heatmap_checkbox.stateChanged.connect(draw_evaluation_heatmap_callback) + self.draw_linearity_heatmap_checkbox = QCheckBox("Draw lineariy error") + self.draw_linearity_heatmap_checkbox.setChecked(False) + self.draw_linearity_heatmap_checkbox.stateChanged.connect(draw_linearity_heatmap_callback) + self.draw_indicators_checkbox = QCheckBox("Draw indicators") self.draw_indicators_checkbox.setChecked(False) @@ -646,6 +655,7 @@ def draw_evaluation_heatmap_callback(value): visualization_options_layout.addWidget(self.draw_evaluation_points_checkbox) visualization_options_layout.addWidget(self.draw_training_heatmap_checkbox) visualization_options_layout.addWidget(self.draw_evaluation_heatmap_checkbox) + visualization_options_layout.addWidget(self.draw_linearity_heatmap_checkbox) visualization_options_layout.addWidget(self.draw_indicators_checkbox) visualization_options_layout.addWidget(rendering_alpha_label) visualization_options_layout.addWidget(self.rendering_alpha_spinbox) @@ -867,9 +877,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.image_view.set_detection_ordered_points(None) self.raw_detection_label.setText("Detected: False") - self.raw_linear_error_rms_label.setText("Linear error rms:") self.raw_linear_error_rows_rms_label.setText("Linear error rows rms:") self.raw_linear_error_cols_rms_label.setText("Linear error cols rms:") + self.aspect_ratio_label.setText("Aspect Ratio:") self.rough_tilt_label.setText("Rough tilt:") self.rough_angles_label.setText("Rough angles:") self.rough_position_label.setText("Rough position:") @@ -951,9 +961,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im rough_angles = detection.get_rotation_angles(camera_model) self.raw_detection_label.setText("Detected: True") - # self.raw_linear_error_rms_label.setText( - # f"Linear rows error rms: {detection.get_linear_error_rms():.2f} px" # noqa E231 - # ) + detection.get_aspect_ratio_pattern_squares() err_rms_rows, err_rms_cols, pct_err_rows, pct_err_cols = detection.get_linear_error_rms() self.raw_linear_error_rows_rms_label.setText( f"Linear error rows rms: {err_rms_rows:.2f} px" # noqa E231 @@ -961,6 +969,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.raw_linear_error_cols_rms_label.setText( f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) + self.aspect_ratio_label.setText( + f"Aspect Reatio: {detection.get_aspect_ratio_pattern_squares():.2f} px" # noqa E231 + ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt():.2f} degrees" # noqa E231 ) @@ -1010,6 +1021,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.image_view.set_draw_evaluation_heatmap( self.draw_evaluation_heatmap_checkbox.isChecked() ) + self.image_view.set_draw_linearity_heatmap( + self.draw_linearity_heatmap_checkbox.isChecked() + ) if self.draw_training_points_checkbox.isChecked(): self.image_view.set_training_points( @@ -1031,6 +1045,11 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.data_collector.get_evaluation_occupancy_heatmap() ) + if self.draw_linearity_heatmap_checkbox.isChecked(): + self.image_view.set_linearity_heatmap( + self.data_collector.get_linearity_heatmap() + ) + if ( self.data_collector.get_num_training_samples() > 0 and not self.calibration_button.isEnabled() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 396ff742..21a49e91 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -263,6 +263,7 @@ def __init__(self, cfg: dict = dict(), **kwargs): # noqa C408 self.training_heatmap = np.zeros((self.heatmap_cells.value, self.heatmap_cells.value)) self.evaluation_heatmap = np.zeros((self.heatmap_cells.value, self.heatmap_cells.value)) + self.linearity_heatmap = np.zeros((self.heatmap_cells.value, self.heatmap_cells.value)) self.training_occupancy_rate = 0.0 self.evaluation_occupancy_rate = 0.0 @@ -361,6 +362,10 @@ def get_evaluation_occupancy_rate(self) -> float: def get_training_occupancy_heatmap(self) -> np.array: """Return the training heatmap, which defines the parts of the pixel space that have image points in it.""" return self.training_heatmap + + def get_linearity_heatmap(self) -> np.array: + """Return the linearity map to evaluate the recitification error.""" + return self.linearity_heatmap def get_evaluation_occupancy_heatmap(self) -> np.array: """Return the evaluation heatmap, which defines the parts of the pixel space that have image points in it.""" @@ -400,6 +405,38 @@ def update_collection_heatmap(self, heatmap: np.array, detection: BoardDetection occupied = float(np.count_nonzero(heatmap > 0)) / np.prod(heatmap.shape) return occupied + def update_linearity_heatmap(self, heatmap: np.array, detection: BoardDetection) -> float: + """Update a heatmap with a single detection's image points.""" + def squared_error(p, p1, p2): + p = p - p1 + p2 = p2 - p1 + p2 /= np.linalg.norm(p2) + squared_distance = np.abs(np.power(np.linalg.norm(p), 2) - np.power(np.dot(p, p2), 2)) + return squared_distance + image_points = detection.get_ordered_image_points() + + for j in range(detection.rows): + p1 = image_points[j][0] + p2 = image_points[j][-1] + for i in range(1, detection.cols - 1): + p = image_points[j][i] + sq_error = np.sqrt(squared_error(p, p1, p2)) + x = int(heatmap.shape[1] * p[0] / detection.width) + y = int(heatmap.shape[0] * p[1] / detection.height) + if (heatmap[y, x] < sq_error): + heatmap[y, x] = 1 * sq_error + + for j in range(detection.cols): + p1 = image_points[0][j] + p2 = image_points[-1][j] + for i in range(1, detection.rows - 1): + p = image_points[i][j] + sq_error = np.sqrt(squared_error(p, p1, p2)) + x = int(heatmap.shape[1] * p[0] / detection.width) + y = int(heatmap.shape[0] * p[1] / detection.height) + if (heatmap[y, x] < sq_error): + heatmap[y, x] = 1 * sq_error + def evaluate_redundancy( self, normalized_2d_center_x_distance: float, @@ -462,7 +499,7 @@ def process_detection( ) -> CollectionStatus: """Evaluate if a detection should be added to either the training or evaluation dataset.""" accepted = True - + self.update_linearity_heatmap(self.linearity_heatmap, detection) if self.filter_by_speed.value: speed = 0 if self.last_detection is None else detection.get_speed(self.last_detection) self.last_detection = detection diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index ce5b1376..468acaeb 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -174,6 +174,10 @@ def set_draw_training_heatmap(self, value: bool): def set_draw_evaluation_heatmap(self, value: bool): """Set the flag of wether or not to draw the occupancy heatmap of the evaluation dataset.""" self.is_draw_evaluation_heatmap = value + + def set_draw_linearity_heatmap(self, value: bool): + """Set the flag of wether or not to draw the linearity heatmap.""" + self.is_draw_linearity_heatmap = value def set_detection_ordered_points(self, points_list: List[np.array]): """Set the detection points to draw. The points are expected to be in a list of rows, where each row is rendered in a different color.""" @@ -195,6 +199,10 @@ def set_evaluation_heatmap(self, value: np.array): """Set the occupancy heatmap to draw from the evaluation dataset.""" self.evaluation_heatmap = value + def set_linearity_heatmap(self, value: np.array): + """Set the linearity heatmap to draw.""" + self.linearity_heatmap = value + def set_grid_size_pixels(self, cell_size_pixels): """Set the size in which to draw the detection's corners.""" self.cell_size_pixels = cell_size_pixels @@ -610,6 +618,9 @@ def paint(self, painter: QPainter, option, widget): if self.is_draw_evaluation_heatmap and self.evaluation_heatmap is not None: self.draw_heatmap(painter, self.evaluation_heatmap, display_size) + + if self.is_draw_linearity_heatmap and self.linearity_heatmap is not None: + self.draw_heatmap(painter, self.linearity_heatmap, display_size) if self.is_draw_indicators and self.current_board_speed is not None: self.draw_indicators(painter, display_size) From 2539a4bc3c8c6e26b97b177a1bb05723d13f95c4 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 19 Nov 2024 10:42:28 +0900 Subject: [PATCH 08/36] [New]Clear linearity hetmap, aspect ratio, board angles, save points and calibration parameters into a file, change ui elements in eval mode. --- .../board_detections/array_board_detection.py | 40 ++-- .../board_detections/board_detection.py | 10 +- .../calibrators/opencv_calibrator.py | 4 + .../camera_calibrator.py | 203 ++++++++++++------ .../data_collector.py | 51 ++++- .../intrinsic_camera_calibrator/parameter.py | 11 + .../views/image_view.py | 111 ++-------- .../views/initialization_view.py | 3 + 8 files changed, 244 insertions(+), 189 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index c489cfca..cef074f9 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -123,29 +123,27 @@ def get_flattened_cell_sizes(self): self._cached_flattened_cell_sizes = cell_sizes.flatten() return self._cached_flattened_cell_sizes - def get_aspect_ratio_pattern_squares(self) -> float: + def get_aspect_ratio_pattern(self) -> float: + """Get aspect ratio using the calibration pattern, wich should be squared.""" + tilt, pan = self.get_rotation_angles() + 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 - horizontal_distances = [] - vertical_distances = [] - - for j in range(self.rows): + aspect_ratio = 0 + count = 0 + for j in range(self.rows - 1): for i in range(self.cols - 1): - p1 = self.image_points[j, i] - p2 = self.image_points[j, i + 1] - points_distance = np.linalg.norm(p2 - p1) - horizontal_distances.append(points_distance) - - for j in range(self.cols): - for i in range(self.rows - 1): - p1 = self.image_points[i, j] - p2 = self.image_points[i + 1, j] - points_distance = np.linalg.norm(p2 - p1) - vertical_distances.append(points_distance) - - avg_horizontal = np.mean(horizontal_distances) - avg_vertical = np.mean(vertical_distances) - aspect_ratio = avg_horizontal / avg_vertical - #print("aspect_ratio: ", aspect_ratio) + p = self.image_points[j, i] + pcol = self.image_points[j + 1, i] + prow = self.image_points[j, i + 1] + horizontal_distance = np.linalg.norm(p - prow) + vertical_distance = np.linalg.norm(p - pcol) + aspect_ratio = aspect_ratio + (horizontal_distance / vertical_distance) + count += 1 + aspect_ratio = aspect_ratio / ((self.rows - 1) * (self.cols - 1)) return aspect_ratio diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index 499a21ab..9e993441 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -101,9 +101,13 @@ def get_flattened_image_points(self) -> np.array: 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 get_aspect_ratio_pattern_squares(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.""" + + def get_aspect_ratio_pattern(self) -> float: + """Return aspect ratio obtained from the mean of the horizontal and vertical distances of the calibration pattern.""" + raise NotImplementedError + + def restart_linearity_heatmap(self): + """Restart linearity heatmap.""" raise NotImplementedError def get_center_2d(self) -> np.array: diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py index 86ce559b..6d58cb23 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/opencv_calibrator.py @@ -38,6 +38,8 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): ) self.fix_principal_point = Parameter(bool, value=False, min_value=False, max_value=True) self.fix_aspect_ratio = Parameter(bool, value=False, min_value=False, max_value=True) + self.use_lu_decomposition = Parameter(bool, value=False, min_value=False, max_value=True) + self.use_qr_decomposition = Parameter(bool, value=False, min_value=False, max_value=True) self.set_parameters(**cfg) @@ -54,6 +56,8 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> CameraModel: flags |= cv2.CALIB_FIX_K3 if self.radial_distortion_coefficients.value < 3 else 0 flags |= cv2.CALIB_FIX_K2 if self.radial_distortion_coefficients.value < 2 else 0 flags |= cv2.CALIB_FIX_K1 if self.radial_distortion_coefficients.value < 1 else 0 + flags |= cv2.CALIB_USE_LU if self.use_lu_decomposition.value else 0 + flags |= cv2.CALIB_USE_QR if self.use_qr_decomposition.value else 0 height = detections[0].get_image_height() width = detections[0].get_image_width() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 5055cd5c..63e1e2ec 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -166,41 +166,6 @@ def __init__(self, cfg): self.right_menu_widget.setFixedWidth(300) self.right_menu_layout = QVBoxLayout(self.right_menu_widget) self.right_menu_layout.setAlignment(Qt.AlignTop) - - # Mode group - self.make_mode_group() - - # Calibration group - self.make_calibration_group() - - # Detector group - self.make_detector_group() - - # Detections group - self.make_detection_group() - - # Data collection group - self.make_data_collection_group() - - # Visualization group - self.make_visualization_group() - - # self.menu_layout.addWidget(label) - self.left_menu_layout.addWidget(self.calibration_group) - self.left_menu_layout.addWidget(self.detector_options_group) - self.left_menu_layout.addWidget(self.raw_detection_results_group) - self.left_menu_layout.addWidget(self.single_shot_detection_results_group) - - self.right_menu_layout.addWidget(self.mode_options_group) - self.right_menu_layout.addWidget(self.data_collection_group) - self.right_menu_layout.addWidget(self.visualization_options_group) - - self.layout.addWidget(self.graphics_view) - - self.layout.addWidget(self.left_menu_widget) - self.layout.addWidget(self.right_menu_widget) - - self.show() self.setEnabled(False) self.initialization_view = InitializationView(self, cfg) @@ -457,7 +422,7 @@ def make_detection_group(self): self.raw_detection_results_group.setFlat(True) self.single_shot_detection_results_group = QGroupBox( - "Single-shot calibration detection results" + "Single-shot detection results" ) self.single_shot_detection_results_group.setFlat(True) @@ -606,7 +571,13 @@ def draw_evaluation_heatmap_callback(value): def draw_linearity_heatmap_callback(value): self.image_view.set_draw_linearity_heatmap(value == Qt.Checked) self.should_process_image.emit() - + + def on_restart_linearity_heatmap_clicked(): + print("restart_lin_heatmap", flush=True) + self.data_collector.restart_linearity_heatmap() + + self.restart_linearity_heatmap_button = QPushButton("Clear heatmap linearity") + self.restart_linearity_heatmap_button.clicked.connect(on_restart_linearity_heatmap_clicked) self.draw_evaluation_heatmap_checkbox = QCheckBox("Draw evaluation occupancy") self.draw_evaluation_heatmap_checkbox.setChecked(False) @@ -638,6 +609,9 @@ def draw_linearity_heatmap_callback(value): self.undistortion_alpha_spinbox.valueChanged.connect( lambda: self.should_process_image.emit() ) + self.undistortion_alpha_spinbox.valueChanged.connect( + on_restart_linearity_heatmap_clicked + ) self.undistortion_alpha_spinbox.setEnabled(False) indicators_alpha_label = QLabel("Indicators alpha:") @@ -663,6 +637,7 @@ def draw_linearity_heatmap_callback(value): visualization_options_layout.addWidget(self.undistortion_alpha_spinbox) visualization_options_layout.addWidget(indicators_alpha_label) visualization_options_layout.addWidget(self.indicators_alpha_spinbox) + visualization_options_layout.addWidget(self.restart_linearity_heatmap_button) self.visualization_options_group.setLayout(visualization_options_layout) def start( @@ -678,9 +653,52 @@ def start( self.board_type = board_type self.board_parameters = board_parameters self.current_camera_model = initial_intrinsics + + # Creating the UI elements after selecting CALIBRATION or EVALUATION + # Mode group + self.make_mode_group() + + # Calibration group + if self.operation_mode == OperationMode.CALIBRATION: + self.make_calibration_group() + + # Detector group + if self.operation_mode == OperationMode.CALIBRATION: + self.make_detector_group() + + # Detections group + self.make_detection_group() + + # Data collection group + if self.operation_mode == OperationMode.CALIBRATION: + self.make_data_collection_group() + + # Visualization group + self.make_visualization_group() + + # self.menu_layout.addWidget(label) + if self.operation_mode == OperationMode.CALIBRATION: + self.left_menu_layout.addWidget(self.calibration_group) + self.left_menu_layout.addWidget(self.detector_options_group) + self.left_menu_layout.addWidget(self.raw_detection_results_group) + self.left_menu_layout.addWidget(self.single_shot_detection_results_group) + + self.right_menu_layout.addWidget(self.mode_options_group) + if self.operation_mode == OperationMode.CALIBRATION: + self.right_menu_layout.addWidget(self.data_collection_group) + self.right_menu_layout.addWidget(self.visualization_options_group) + + self.layout.addWidget(self.graphics_view) + + self.layout.addWidget(self.left_menu_widget) + self.layout.addWidget(self.right_menu_widget) + self.show() self.setEnabled(True) - self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()})") + if self.operation_mode == OperationMode.CALIBRATION: + self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()})") + if self.operation_mode == OperationMode.EVALUATION: + self.setWindowTitle(f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()})") logging.info("Init") logging.info(f"\tmode : {mode}") @@ -697,13 +715,23 @@ def start( ) if self.operation_mode == OperationMode.EVALUATION: - self.calibration_button.setEnabled(False) - # ToDo Verify if this changes everything correctly + # Initial state of the elements on evaluation mode self.calibrated_camera_model = self.current_camera_model self.image_view_type_combobox.setEnabled(True) - self.image_view_type_combobox.setCurrentIndex(1) # rectified self.undistortion_alpha_spinbox.setEnabled(True) - self.calibration_parameters_button.setEnabled(False) + self.draw_evaluation_heatmap_checkbox.setEnabled(False) + self.draw_evaluation_points_checkbox.setEnabled(False) + self.draw_training_points_checkbox.setEnabled(False) + self.draw_training_heatmap_checkbox.setEnabled(False) + self.training_sample_slider.setEnabled(False) + self.evaluation_sample_slider.setEnabled(False) + self.image_view_type_combobox.clear() + # Order of of how items are added to the combobox matters, + # default index is 0, so rectified image is added first to be default view + self.image_view_type_combobox.addItem(ImageViewMode.SOURCE_RECTIFIED.value, \ + ImageViewMode.SOURCE_RECTIFIED) + self.image_view_type_combobox.addItem(ImageViewMode.SOURCE_UNRECTIFIED.value, \ + ImageViewMode.SOURCE_UNRECTIFIED) self.detector.moveToThread(self.detector_thread) self.detector.detection_results_signal.connect(self.process_detection_results) @@ -822,6 +850,20 @@ def process_evaluation_results( def on_consumed(self): self.data_source.consumed() + def save_parameters(self,filename): + data_coll_params = self.data_collector.parameters_value() + board_params = self.board_parameters.parameters_value() + detector_params = self.detector.parameters_value() + calibrator_type = self.calibrator_type_combobox.currentData() + print(calibrator_type.value["name"], flush=True) + calib_params = self.calibrator_dict[calibrator_type].parameters_value() + with open(filename, 'w') as file: + yaml.dump({"board_parameters" : board_params}, file, default_flow_style=False) + yaml.dump({"calibrator_type" : calibrator_type.value["name"]}, file, default_flow_style=False) + yaml.dump({"calibration_parameters": calib_params}, file, default_flow_style=False) + yaml.dump({"data_collector" : data_coll_params}, file, default_flow_style=False) + yaml.dump({"detector_params" : detector_params}, file, default_flow_style=False) + def on_save_clicked(self): output_folder = QFileDialog.getExistingDirectory( None, @@ -842,6 +884,8 @@ def on_save_clicked(self): os.path.join(output_folder, f"{self.data_source.get_camera_name()}_info.yaml"), ) + self.save_parameters(os.path.join(output_folder, "parameters.yaml")) + training_folder = os.path.join(output_folder, "training_images") evaluation_folder = os.path.join(output_folder, "evaluation_images") @@ -852,9 +896,17 @@ def on_save_clicked(self): for index, image in enumerate(self.data_collector.get_training_images()): cv2.imwrite(os.path.join(training_folder, f"{index:04d}.jpg"), image) # noqa E231 + np.savetxt(os.path.join(training_folder,f"{index:04d}_training_img_points.txt"), \ + self.data_collector.get_training_detection(index).get_flattened_image_points()) + np.savetxt(os.path.join(training_folder,f"{index:04d}_training_obj_points.txt"), \ + self.data_collector.get_training_detection(index).get_flattened_object_points()) for index, image in enumerate(self.data_collector.get_evaluation_images()): cv2.imwrite(os.path.join(evaluation_folder, f"{index:04d}.jpg"), image) # noqa E231 + np.savetxt(os.path.join(evaluation_folder,f"{index:04d}_eval_img_points.txt"), \ + self.data_collector.get_evaluation_detection(index).get_flattened_image_points()) + np.savetxt(os.path.join(evaluation_folder,f"{index:04d}_eval_obj_points.txt"), \ + self.data_collector.get_evaluation_detection(index).get_flattened_object_points()) def process_detection_results(self, img: np.array, detection: BoardDetection, img_stamp: float): """Process the results from an object detection.""" @@ -894,7 +946,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.data_collector.max_allowed_pixel_speed.value, self.data_collector.get_skew_percentage(), self.data_collector.get_size_percentage(), - 0, 0, + 0, 0, # rows cols linear error + 0, 0, # rows cols percentage linear error + 0.0, # aspect ratio 0, 0, self.indicators_alpha_spinbox.value(), False) @@ -915,6 +969,15 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im ) else: filter_result = CollectionStatus.NOT_EVALUATED + + if self.operation_mode == OperationMode.EVALUATION and \ + self.image_view_type_combobox.currentData() == ImageViewMode.SOURCE_RECTIFIED: + self.data_collector.process_detection_eval_mode( + image=img, + detection=detection, + camera_model=camera_model, + mode=self.operation_mode, + ) # For each new sample that is accepted we try to update the current (partial) calibration if ( @@ -931,12 +994,13 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im } self.image_view.set_draw_detection_color(filter_result_color_dict[filter_result]) - self.data_collection_training_label.setText( - f"Training samples: {self.data_collector.get_num_training_samples()}" - ) - self.data_collection_evaluation_label.setText( - f"Evaluation samples: {self.data_collector.get_num_evaluation_samples()}" - ) + if self.operation_mode == OperationMode.CALIBRATION: + self.data_collection_training_label.setText( + f"Training samples: {self.data_collector.get_num_training_samples()}" + ) + self.data_collection_evaluation_label.setText( + f"Evaluation samples: {self.data_collector.get_num_evaluation_samples()}" + ) # object_points = detection.get_object_points() ordered_image_points = detection.get_ordered_image_points() @@ -961,7 +1025,6 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im rough_angles = detection.get_rotation_angles(camera_model) self.raw_detection_label.setText("Detected: True") - detection.get_aspect_ratio_pattern_squares() err_rms_rows, err_rms_cols, pct_err_rows, pct_err_cols = detection.get_linear_error_rms() self.raw_linear_error_rows_rms_label.setText( f"Linear error rows rms: {err_rms_rows:.2f} px" # noqa E231 @@ -970,7 +1033,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) self.aspect_ratio_label.setText( - f"Aspect Reatio: {detection.get_aspect_ratio_pattern_squares():.2f} px" # noqa E231 + f"Aspect Reatio: {detection.get_aspect_ratio_pattern():.2f} px" # noqa E231 ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt():.2f} degrees" # noqa E231 @@ -996,21 +1059,25 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Reprojection error (rms): {reprojection_error_rms:.3f} px ({100.0 * reprojection_error_rms_relative:.2f}%)" # noqa E231 ) - self.training_occupancy_rate_label.setText( - f"Training occupancy: {100.0*self.data_collector.get_training_occupancy_rate():.2f}" # noqa E231 - ) - self.evaluation_occupancy_rate_label.setText( - f"Evaluation occupancy: {100.0*self.data_collector.get_evaluation_occupancy_rate():.2f}" # noqa E231 - ) + if self.operation_mode == OperationMode.CALIBRATION: + self.training_occupancy_rate_label.setText( + f"Training occupancy: {100.0*self.data_collector.get_training_occupancy_rate():.2f}" # noqa E231 + ) + self.evaluation_occupancy_rate_label.setText( + f"Evaluation occupancy: {100.0*self.data_collector.get_evaluation_occupancy_rate():.2f}" # noqa E231 + ) board_speed = 0 if self.last_detection is None else detection.get_speed(self.last_detection) self.last_detection = detection + pan, tilt = detection.get_rotation_angles() self.image_view.set_draw_indicators(board_speed, self.data_collector.max_allowed_pixel_speed.value, self.data_collector.get_skew_percentage(), self.data_collector.get_size_percentage(), err_rms_rows, err_rms_cols, pct_err_rows, pct_err_cols, + detection.get_aspect_ratio_pattern(), + pan, tilt, self.indicators_alpha_spinbox.value(), self.draw_indicators_checkbox.isChecked()) @@ -1021,6 +1088,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.image_view.set_draw_evaluation_heatmap( self.draw_evaluation_heatmap_checkbox.isChecked() ) + self.image_view.set_draw_linearity_heatmap( self.draw_linearity_heatmap_checkbox.isChecked() ) @@ -1050,17 +1118,18 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.data_collector.get_linearity_heatmap() ) - if ( - self.data_collector.get_num_training_samples() > 0 - and not self.calibration_button.isEnabled() - ): - self.calibration_button.setEnabled(True) + if self.operation_mode == OperationMode.CALIBRATION: + if ( + self.data_collector.get_num_training_samples() > 0 + and not self.calibration_button.isEnabled() + ): + self.calibration_button.setEnabled(True) - if ( - self.data_collector.get_num_evaluation_samples() > 0 - and not self.evaluation_button.isEnabled() - ): - self.evaluation_button.setEnabled(True) + if ( + self.data_collector.get_num_evaluation_samples() > 0 + and not self.evaluation_button.isEnabled() + ): + self.evaluation_button.setEnabled(True) # Set drawing image self.image_view.set_image(img) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 21a49e91..ddb0f27d 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -362,7 +362,7 @@ def get_evaluation_occupancy_rate(self) -> float: def get_training_occupancy_heatmap(self) -> np.array: """Return the training heatmap, which defines the parts of the pixel space that have image points in it.""" return self.training_heatmap - + def get_linearity_heatmap(self) -> np.array: """Return the linearity map to evaluate the recitification error.""" return self.linearity_heatmap @@ -414,28 +414,41 @@ def squared_error(p, p1, p2): squared_distance = np.abs(np.power(np.linalg.norm(p), 2) - np.power(np.dot(p, p2), 2)) return squared_distance image_points = detection.get_ordered_image_points() + max_pct_error_tolerance = 0.04 for j in range(detection.rows): p1 = image_points[j][0] p2 = image_points[j][-1] + points_dist = np.linalg.norm(p2 -p1) for i in range(1, detection.cols - 1): p = image_points[j][i] - sq_error = np.sqrt(squared_error(p, p1, p2)) + dist_error = np.sqrt(squared_error(p, p1, p2)) + if (dist_error/points_dist > max_pct_error_tolerance): + # if distance is too big most likely is a miss detection + dist_error = 0 x = int(heatmap.shape[1] * p[0] / detection.width) y = int(heatmap.shape[0] * p[1] / detection.height) - if (heatmap[y, x] < sq_error): - heatmap[y, x] = 1 * sq_error + if (heatmap[y, x] < dist_error): + heatmap[y, x] = 1 * dist_error for j in range(detection.cols): p1 = image_points[0][j] p2 = image_points[-1][j] + points_dist = np.linalg.norm(p2 -p1) for i in range(1, detection.rows - 1): p = image_points[i][j] - sq_error = np.sqrt(squared_error(p, p1, p2)) + dist_error = np.sqrt(squared_error(p, p1, p2)) + if (dist_error/points_dist > max_pct_error_tolerance): + # if distance is too big most likely is a miss detection + dist_error = 0 x = int(heatmap.shape[1] * p[0] / detection.width) y = int(heatmap.shape[0] * p[1] / detection.height) - if (heatmap[y, x] < sq_error): - heatmap[y, x] = 1 * sq_error + if (heatmap[y, x] < dist_error): + heatmap[y, x] = 1 * dist_error + + def restart_linearity_heatmap(self): + """Restart heatmap created by aspect ratio.""" + self.linearity_heatmap = np.zeros((self.heatmap_cells.value, self.heatmap_cells.value)) def evaluate_redundancy( self, @@ -499,7 +512,10 @@ def process_detection( ) -> CollectionStatus: """Evaluate if a detection should be added to either the training or evaluation dataset.""" accepted = True + + # process detections without filtering, only to get linearity heatmap self.update_linearity_heatmap(self.linearity_heatmap, detection) + if self.filter_by_speed.value: speed = 0 if self.last_detection is None else detection.get_speed(self.last_detection) self.last_detection = detection @@ -567,8 +583,20 @@ def process_detection( return CollectionStatus.ACCEPTED return CollectionStatus.REDUNDANT - + + def process_detection_eval_mode( + self, + image: np.array, + detection: BoardDetection, + camera_model: Optional[CameraModel] = None, + mode: OperationMode = OperationMode.CALIBRATION, + ) -> CollectionStatus: + """Evaluate detections mad ein evaluation mode.""" + # process wihtout filtering detections + self.update_linearity_heatmap(self.linearity_heatmap, detection) + def get_skew_coverage(self): + """Get skew percentage covered from a defined range for the indicators.""" # Define the number of intervals num_intervals = 50 interval_size = 1 / num_intervals @@ -586,11 +614,13 @@ def get_skew_coverage(self): percentage_coverage = (len(covered_intervals) / num_intervals) # * 100 return percentage_coverage - + def get_skew_percentage(self): + """Get skew precentage with more useful name ToDo Refactor.""" return self.get_skew_coverage() def get_size_coverage(self): + """Get board size percentage covered from a defined range for the indicators.""" # Define the number of intervals num_intervals = 20 interval_size = 1 / num_intervals @@ -608,6 +638,7 @@ def get_size_coverage(self): percentage_coverage = (len(covered_intervals) / num_intervals) # * 100 return percentage_coverage - + def get_size_percentage(self): + """Get size precentage with more useful name ToDo Refactor.""" return self.get_size_coverage() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py index df27a7f3..1399d2f3 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/parameter.py @@ -89,3 +89,14 @@ def parameters(self) -> dict: p_dict[k] = v return p_dict + + def parameters_value(self) -> dict: + """Return the Parameter objects' names and values from this class as a dictionary.""" + with self.lock: + p_dict = {} + + for k, v in vars(self).items(): + if isinstance(v, Parameter): + p_dict[k] = v.value + + return p_dict diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 468acaeb..8b0deded 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -215,9 +215,11 @@ def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float skew_percentage: float, board_size_percentage: float, rows_linear_error: float, cols_linear_error: float, pct_err_rows: float, pct_err_cols: float, + aspect_ratio: float, + pan: float, tilt: float, alpha_indicators: float, value: bool): - """Set values for the indicators.""" + """Set values for indicators.""" self.current_board_speed = board_speed self.max_board_allowed_speed = max_allowed_board_speed self.skew_percentage = skew_percentage @@ -226,13 +228,17 @@ def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float self.cols_linear_error = cols_linear_error self.pct_err_rows = pct_err_rows self.pct_err_cols = pct_err_cols + self.aspect_ratio = aspect_ratio + self.pan = pan + self.tilt = tilt self.alpha_indicators = alpha_indicators self.is_draw_indicators = value - + def draw_indicators(self, painter: QPainter, display_size): - """Draw indicators for speed, skew and size of the detected board.""" + """Draw indicators for speed, skew, size aspect ratio, angles of the detected board.""" color_green = QColor(0.0, 255, 0.0, int(255 * self.alpha_indicators)) color_red = QColor(255, 0.0, 0.0, int(255 * self.alpha_indicators)) + # Change color according tot he speed if (self.current_board_speed < self.max_board_allowed_speed): pen = QPen(color_green) brush = QBrush(color_green) @@ -242,7 +248,7 @@ def draw_indicators(self, painter: QPainter, display_size): painter.setPen(pen) painter.setBrush(brush) speed_indicator = QRectF(QPointF(0, 0), QSize(display_size.width(), int(display_size.height()*0.04))) - # Draw the rectangle + # Draw the rectangle for speed indication painter.drawRect(speed_indicator) # Set the font according to the window size @@ -252,6 +258,7 @@ def draw_indicators(self, painter: QPainter, display_size): position_text_speed = QPointF(int(display_size.width() * .01), int(display_size.height() * .10)) painter.drawText(position_text_speed, "Speed") + # ToDo: define percentage to change skew and pct size to change to green thresold_to_be_green = 0.3 if self.skew_percentage < thresold_to_be_green: pen_skew = QPen(color_red) @@ -270,7 +277,8 @@ def draw_indicators(self, painter: QPainter, display_size): QSize(display_size.width() * 0.08 * self.skew_percentage, int(display_size.height()*0.03))) # Draw the skew progrees bar painter.drawRect(skew_indicator) - + + # ToDo: define percentage to change skew and pct size to change to green thresold_to_be_green = 0.2 if self.board_size_percentage < thresold_to_be_green: pen_size_board = QPen(color_red) @@ -290,6 +298,11 @@ def draw_indicators(self, painter: QPainter, display_size): # Draw the board size progrees bar painter.drawRect(board_size_indicator) + # Draw board pan tilt angle text + deg_sign = u'\N{DEGREE SIGN}' + position_text_pan = QPointF(int(display_size.width() * .01), int(display_size.height() * .98)) + painter.drawText(position_text_pan, "Pan " + str(int(self.pan)) + deg_sign + " Tilt " + str(int(self.tilt)) + deg_sign) + # Draw Linear errors text position_text_err_rows = QPointF(int(display_size.width() * .80), int(display_size.height() * .88)) painter.drawText(position_text_err_rows, "ErrRows " + str(round(self.rows_linear_error, 1)) + "px " + \ @@ -299,6 +312,10 @@ def draw_indicators(self, painter: QPainter, display_size): painter.drawText(position_text_err_cols, "ErrCols " + str(round(self.cols_linear_error, 1)) + "px " + \ str(round(self.pct_err_cols *100, 1)) + "%") + # Draw the aspect ratio text indicator + position_text_aspect_ratio = QPointF(int(display_size.width() * .80), int(display_size.height() * .98)) + painter.drawText(position_text_aspect_ratio, "AspectR " + str(round(self.aspect_ratio, 2))) + def pixmap(self) -> QPixmap: """Return the rendering QPixmap.""" return self.raw_pixmap @@ -384,87 +401,6 @@ def draw_detection_points(self, painter: QPainter): painter.setPen(pen) painter.drawLine(QPointF(p1[0], p1[1]), QPointF(p2[0], p2[1])) - def draw_linear_errors(self, painter: QPainter): - #### ToDo here investigating how elements were drawn specially ines - # determine the linearity error - if self.points_list is None: - return - def compute_line_equation(p1, p2): - A = p2[1] - p1[1] - B = p1[0] - p2[0] - C = p2[0] * p1[1] - p1[0] * p2[1] - return A, B, C - # Function to compute the perpendicular distance from point (x0, y0) to the line Ax + By + C = 0 - def perpendicular_distance(x0, y0, A, B, C): - return abs(A * x0 + B * y0 + C) / np.sqrt(A ** 2 + B ** 2) - """Draw new lines.""" - alpha = 255 - pen = QPen(QColor(0.0, 255.0, 0.0, alpha)) - painter.setPen(pen) - for i in range(len(self.points_list)): - p1 = self.image_to_view_factor * self.points_list[i][0] # first element - p2 = self.image_to_view_factor * self.points_list[i][-1] # last element - line_pixel_distance = np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2) - panel_width_mm = 700 - conv_factor_chessboard_mm = panel_width_mm / line_pixel_distance - # Set the font according to the window size - font_size = 12 - font = QFont("Arial", font_size) - painter.setFont(font) - # position_text_speed = QPointF(p1[0],p1[1]) - # painter.drawText(position_text_speed, "S") - # position_text_speed = QPointF(p2[0],p2[1]) - # painter.drawText(position_text_speed, "E") - # painter.drawLine(QPointF(p1[0], p1[1]), QPointF(p2[0],p2[1])) - - A, B, C = compute_line_equation(p1, p2) - count = 0 - mse = 0 - for point in self.points_list[i]: - x0, y0 = self.image_to_view_factor * point - distance_mm = conv_factor_chessboard_mm * perpendicular_distance(x0, y0, A, B, C) - mse = mse + distance_mm ** 2 - count += 1 - mse = mse / count - # position_text_speed = QPointF(p1[0],p1[1]) - # painter.drawText(position_text_speed, "E:" + str(round(mse,1))) - - ## we got for rows now for columns - - alpha = 255 - pen = QPen(QColor(255.0, 0.0, 0.0, alpha)) - painter.setPen(pen) - for i in range(len(self.points_list[0])): - p1 = self.image_to_view_factor * self.points_list[0][i] # first element - p2 = self.image_to_view_factor * self.points_list[-1][i] # last element - line_pixel_distance = np.sqrt((p2[0] - p1[0])**2 + (p2[1] - p1[1])**2) - panel_width_mm = 700 - conv_factor_chessboard_mm = panel_width_mm / line_pixel_distance - # Set the font according to the window size - # font_size = 12 - # font = QFont("Arial", font_size) - # painter.setFont(font) - # position_text_speed = QPointF(p1[0],p1[1]) - # painter.drawText(position_text_speed, "nS") - # position_text_speed = QPointF(p2[0],p2[1]) - # painter.drawText(position_text_speed, "nE") - # painter.drawLine(QPointF(p1[0], p1[1]), QPointF(p2[0],p2[1])) - - A, B, C = compute_line_equation(p1, p2) - count = 0 - mse = 0 - #worst_dst_meters = 0 - for j in range (1,len(self.points_list)-1): - x0, y0 = self.image_to_view_factor * self.points_list[j][i] - distance_mm = conv_factor_chessboard_mm * perpendicular_distance(x0, y0, A, B, C) - mse = mse + distance_mm ** 2 - # position_text_p = QPointF(x0,y0) - # painter.drawText(position_text_p, str(j)) - count += 1 - mse = mse / count - # position_text_speed = QPointF(p1[0],p1[1]) - # painter.drawText(position_text_speed, "E:" + str(round(mse,1))) - def draw_points(self, painter: QPainter, points: np.array): """Draw a set of points as rectangles.""" pen = QPen(QColor(255, 0.0, 255, int(255 * self.rendering_alpha))) @@ -621,10 +557,9 @@ def paint(self, painter: QPainter, option, widget): if self.is_draw_linearity_heatmap and self.linearity_heatmap is not None: self.draw_heatmap(painter, self.linearity_heatmap, display_size) - + if self.is_draw_indicators and self.current_board_speed is not None: self.draw_indicators(painter, display_size) - self.draw_linear_errors(painter) def boundingRect(self): """Return the size of the Widget to let other widgets adjust correctly.""" diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index 450e80d0..92116079 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -136,6 +136,9 @@ def load_intrinsics_button_callback(): ) self.initial_intrinsics = load_intrinsics(intrinsics_path) self.evaluation_radio_button.setEnabled(True) + # self.training_radio_button.setChecked(False) + self.training_radio_button.setEnabled(False) + self.evaluation_radio_button.setChecked(True) self.load_intrinsics_button.clicked.connect(load_intrinsics_button_callback) From cf3e90fe9a1166781c34e8d3e2efe6aa792358b6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 19 Nov 2024 05:01:33 +0000 Subject: [PATCH 09/36] ci(pre-commit): autofix --- .../board_detections/array_board_detection.py | 21 ++- .../camera_calibrator.py | 142 ++++++++++-------- .../data_collector.py | 24 +-- .../views/image_view.py | 115 ++++++++++---- 4 files changed, 192 insertions(+), 110 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index cef074f9..12f15819 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -65,8 +65,10 @@ def squared_error(p, p1, p2): 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_rows_rms is not None and \ - self._cached_linear_error_cols_rms is not None: + 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_rows = 0 @@ -103,8 +105,12 @@ def squared_error(p, p1, p2): 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 + 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: @@ -122,13 +128,13 @@ def get_flattened_cell_sizes(self): self._cached_flattened_cell_sizes = cell_sizes.flatten() return self._cached_flattened_cell_sizes - + def get_aspect_ratio_pattern(self) -> float: """Get aspect ratio using the calibration pattern, wich should be squared.""" tilt, pan = self.get_rotation_angles() acceptance_angle = 10 - # dont update if we the detection has big angles, calculation will not be accurate + # 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 @@ -143,7 +149,6 @@ def get_aspect_ratio_pattern(self) -> float: vertical_distance = np.linalg.norm(p - pcol) aspect_ratio = aspect_ratio + (horizontal_distance / vertical_distance) count += 1 - aspect_ratio = aspect_ratio / ((self.rows - 1) * (self.cols - 1)) + aspect_ratio = aspect_ratio / ((self.rows - 1) * (self.cols - 1)) return aspect_ratio - diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 63e1e2ec..e663119d 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -421,9 +421,7 @@ def make_detection_group(self): self.raw_detection_results_group = QGroupBox("Detection results") self.raw_detection_results_group.setFlat(True) - self.single_shot_detection_results_group = QGroupBox( - "Single-shot detection results" - ) + self.single_shot_detection_results_group = QGroupBox("Single-shot detection results") self.single_shot_detection_results_group.setFlat(True) self.raw_detection_label = QLabel("Detected:") @@ -567,7 +565,7 @@ def draw_training_heatmap_callback(value): def draw_evaluation_heatmap_callback(value): self.image_view.set_draw_evaluation_heatmap(value == Qt.Checked) self.should_process_image.emit() - + def draw_linearity_heatmap_callback(value): self.image_view.set_draw_linearity_heatmap(value == Qt.Checked) self.should_process_image.emit() @@ -609,9 +607,7 @@ def on_restart_linearity_heatmap_clicked(): self.undistortion_alpha_spinbox.valueChanged.connect( lambda: self.should_process_image.emit() ) - self.undistortion_alpha_spinbox.valueChanged.connect( - on_restart_linearity_heatmap_clicked - ) + self.undistortion_alpha_spinbox.valueChanged.connect(on_restart_linearity_heatmap_clicked) self.undistortion_alpha_spinbox.setEnabled(False) indicators_alpha_label = QLabel("Indicators alpha:") @@ -696,9 +692,13 @@ def start( self.setEnabled(True) if self.operation_mode == OperationMode.CALIBRATION: - self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()})") + self.setWindowTitle( + f"Camera intrinsics calibrator ({self.data_source.get_camera_name()})" + ) if self.operation_mode == OperationMode.EVALUATION: - self.setWindowTitle(f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()})") + self.setWindowTitle( + f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()})" + ) logging.info("Init") logging.info(f"\tmode : {mode}") @@ -727,11 +727,13 @@ def start( self.evaluation_sample_slider.setEnabled(False) self.image_view_type_combobox.clear() # Order of of how items are added to the combobox matters, - # default index is 0, so rectified image is added first to be default view - self.image_view_type_combobox.addItem(ImageViewMode.SOURCE_RECTIFIED.value, \ - ImageViewMode.SOURCE_RECTIFIED) - self.image_view_type_combobox.addItem(ImageViewMode.SOURCE_UNRECTIFIED.value, \ - ImageViewMode.SOURCE_UNRECTIFIED) + # default index is 0, so rectified image is added first to be default view + self.image_view_type_combobox.addItem( + ImageViewMode.SOURCE_RECTIFIED.value, ImageViewMode.SOURCE_RECTIFIED + ) + self.image_view_type_combobox.addItem( + ImageViewMode.SOURCE_UNRECTIFIED.value, ImageViewMode.SOURCE_UNRECTIFIED + ) self.detector.moveToThread(self.detector_thread) self.detector.detection_results_signal.connect(self.process_detection_results) @@ -850,19 +852,21 @@ def process_evaluation_results( def on_consumed(self): self.data_source.consumed() - def save_parameters(self,filename): + def save_parameters(self, filename): data_coll_params = self.data_collector.parameters_value() board_params = self.board_parameters.parameters_value() detector_params = self.detector.parameters_value() calibrator_type = self.calibrator_type_combobox.currentData() print(calibrator_type.value["name"], flush=True) calib_params = self.calibrator_dict[calibrator_type].parameters_value() - with open(filename, 'w') as file: - yaml.dump({"board_parameters" : board_params}, file, default_flow_style=False) - yaml.dump({"calibrator_type" : calibrator_type.value["name"]}, file, default_flow_style=False) + with open(filename, "w") as file: + yaml.dump({"board_parameters": board_params}, file, default_flow_style=False) + yaml.dump( + {"calibrator_type": calibrator_type.value["name"]}, file, default_flow_style=False + ) yaml.dump({"calibration_parameters": calib_params}, file, default_flow_style=False) - yaml.dump({"data_collector" : data_coll_params}, file, default_flow_style=False) - yaml.dump({"detector_params" : detector_params}, file, default_flow_style=False) + yaml.dump({"data_collector": data_coll_params}, file, default_flow_style=False) + yaml.dump({"detector_params": detector_params}, file, default_flow_style=False) def on_save_clicked(self): output_folder = QFileDialog.getExistingDirectory( @@ -896,17 +900,25 @@ def on_save_clicked(self): for index, image in enumerate(self.data_collector.get_training_images()): cv2.imwrite(os.path.join(training_folder, f"{index:04d}.jpg"), image) # noqa E231 - np.savetxt(os.path.join(training_folder,f"{index:04d}_training_img_points.txt"), \ - self.data_collector.get_training_detection(index).get_flattened_image_points()) - np.savetxt(os.path.join(training_folder,f"{index:04d}_training_obj_points.txt"), \ - self.data_collector.get_training_detection(index).get_flattened_object_points()) + np.savetxt( + os.path.join(training_folder, f"{index:04d}_training_img_points.txt"), + self.data_collector.get_training_detection(index).get_flattened_image_points(), + ) + np.savetxt( + os.path.join(training_folder, f"{index:04d}_training_obj_points.txt"), + self.data_collector.get_training_detection(index).get_flattened_object_points(), + ) for index, image in enumerate(self.data_collector.get_evaluation_images()): cv2.imwrite(os.path.join(evaluation_folder, f"{index:04d}.jpg"), image) # noqa E231 - np.savetxt(os.path.join(evaluation_folder,f"{index:04d}_eval_img_points.txt"), \ - self.data_collector.get_evaluation_detection(index).get_flattened_image_points()) - np.savetxt(os.path.join(evaluation_folder,f"{index:04d}_eval_obj_points.txt"), \ - self.data_collector.get_evaluation_detection(index).get_flattened_object_points()) + np.savetxt( + os.path.join(evaluation_folder, f"{index:04d}_eval_img_points.txt"), + self.data_collector.get_evaluation_detection(index).get_flattened_image_points(), + ) + np.savetxt( + os.path.join(evaluation_folder, f"{index:04d}_eval_obj_points.txt"), + self.data_collector.get_evaluation_detection(index).get_flattened_object_points(), + ) def process_detection_results(self, img: np.array, detection: BoardDetection, img_stamp: float): """Process the results from an object detection.""" @@ -942,16 +954,21 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.single_shot_reprojection_error_avg_label.setText("Reprojection error (avg):") self.single_shot_reprojection_error_rms_label.setText("Reprojection error (rms):") board_speed = None - self.image_view.set_draw_indicators(board_speed, - self.data_collector.max_allowed_pixel_speed.value, - self.data_collector.get_skew_percentage(), - self.data_collector.get_size_percentage(), - 0, 0, # rows cols linear error - 0, 0, # rows cols percentage linear error - 0.0, # aspect ratio - 0, 0, - self.indicators_alpha_spinbox.value(), - False) + self.image_view.set_draw_indicators( + board_speed, + self.data_collector.max_allowed_pixel_speed.value, + self.data_collector.get_skew_percentage(), + self.data_collector.get_size_percentage(), + 0, + 0, # rows cols linear error + 0, + 0, # rows cols percentage linear error + 0.0, # aspect ratio + 0, + 0, + self.indicators_alpha_spinbox.value(), + False, + ) else: camera_model = ( @@ -969,9 +986,11 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im ) else: filter_result = CollectionStatus.NOT_EVALUATED - - if self.operation_mode == OperationMode.EVALUATION and \ - self.image_view_type_combobox.currentData() == ImageViewMode.SOURCE_RECTIFIED: + + if ( + self.operation_mode == OperationMode.EVALUATION + and self.image_view_type_combobox.currentData() == ImageViewMode.SOURCE_RECTIFIED + ): self.data_collector.process_detection_eval_mode( image=img, detection=detection, @@ -1025,7 +1044,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im rough_angles = detection.get_rotation_angles(camera_model) self.raw_detection_label.setText("Detected: True") - err_rms_rows, err_rms_cols, pct_err_rows, pct_err_cols = detection.get_linear_error_rms() + err_rms_rows, err_rms_cols, pct_err_rows, pct_err_cols = ( + detection.get_linear_error_rms() + ) self.raw_linear_error_rows_rms_label.setText( f"Linear error rows rms: {err_rms_rows:.2f} px" # noqa E231 ) @@ -1067,19 +1088,26 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Evaluation occupancy: {100.0*self.data_collector.get_evaluation_occupancy_rate():.2f}" # noqa E231 ) - board_speed = 0 if self.last_detection is None else detection.get_speed(self.last_detection) + board_speed = ( + 0 if self.last_detection is None else detection.get_speed(self.last_detection) + ) self.last_detection = detection pan, tilt = detection.get_rotation_angles() - self.image_view.set_draw_indicators(board_speed, - self.data_collector.max_allowed_pixel_speed.value, - self.data_collector.get_skew_percentage(), - self.data_collector.get_size_percentage(), - err_rms_rows, err_rms_cols, - pct_err_rows, pct_err_cols, - detection.get_aspect_ratio_pattern(), - pan, tilt, - self.indicators_alpha_spinbox.value(), - self.draw_indicators_checkbox.isChecked()) + self.image_view.set_draw_indicators( + board_speed, + self.data_collector.max_allowed_pixel_speed.value, + self.data_collector.get_skew_percentage(), + self.data_collector.get_size_percentage(), + err_rms_rows, + err_rms_cols, + pct_err_rows, + pct_err_cols, + detection.get_aspect_ratio_pattern(), + pan, + tilt, + self.indicators_alpha_spinbox.value(), + self.draw_indicators_checkbox.isChecked(), + ) # Draw training / evaluation points self.image_view.set_draw_training_points(self.draw_training_points_checkbox.isChecked()) @@ -1089,9 +1117,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.draw_evaluation_heatmap_checkbox.isChecked() ) - self.image_view.set_draw_linearity_heatmap( - self.draw_linearity_heatmap_checkbox.isChecked() - ) + self.image_view.set_draw_linearity_heatmap(self.draw_linearity_heatmap_checkbox.isChecked()) if self.draw_training_points_checkbox.isChecked(): self.image_view.set_training_points( @@ -1114,9 +1140,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im ) if self.draw_linearity_heatmap_checkbox.isChecked(): - self.image_view.set_linearity_heatmap( - self.data_collector.get_linearity_heatmap() - ) + self.image_view.set_linearity_heatmap(self.data_collector.get_linearity_heatmap()) if self.operation_mode == OperationMode.CALIBRATION: if ( diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index ddb0f27d..813379d6 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -407,43 +407,45 @@ def update_collection_heatmap(self, heatmap: np.array, detection: BoardDetection def update_linearity_heatmap(self, heatmap: np.array, detection: BoardDetection) -> float: """Update a heatmap with a single detection's image points.""" + def squared_error(p, p1, p2): p = p - p1 p2 = p2 - p1 p2 /= np.linalg.norm(p2) squared_distance = np.abs(np.power(np.linalg.norm(p), 2) - np.power(np.dot(p, p2), 2)) return squared_distance + image_points = detection.get_ordered_image_points() max_pct_error_tolerance = 0.04 for j in range(detection.rows): p1 = image_points[j][0] p2 = image_points[j][-1] - points_dist = np.linalg.norm(p2 -p1) + points_dist = np.linalg.norm(p2 - p1) for i in range(1, detection.cols - 1): p = image_points[j][i] dist_error = np.sqrt(squared_error(p, p1, p2)) - if (dist_error/points_dist > max_pct_error_tolerance): + if dist_error / points_dist > max_pct_error_tolerance: # if distance is too big most likely is a miss detection dist_error = 0 x = int(heatmap.shape[1] * p[0] / detection.width) y = int(heatmap.shape[0] * p[1] / detection.height) - if (heatmap[y, x] < dist_error): + if heatmap[y, x] < dist_error: heatmap[y, x] = 1 * dist_error for j in range(detection.cols): p1 = image_points[0][j] p2 = image_points[-1][j] - points_dist = np.linalg.norm(p2 -p1) + points_dist = np.linalg.norm(p2 - p1) for i in range(1, detection.rows - 1): p = image_points[i][j] dist_error = np.sqrt(squared_error(p, p1, p2)) - if (dist_error/points_dist > max_pct_error_tolerance): + if dist_error / points_dist > max_pct_error_tolerance: # if distance is too big most likely is a miss detection dist_error = 0 x = int(heatmap.shape[1] * p[0] / detection.width) y = int(heatmap.shape[0] * p[1] / detection.height) - if (heatmap[y, x] < dist_error): + if heatmap[y, x] < dist_error: heatmap[y, x] = 1 * dist_error def restart_linearity_heatmap(self): @@ -592,7 +594,7 @@ def process_detection_eval_mode( mode: OperationMode = OperationMode.CALIBRATION, ) -> CollectionStatus: """Evaluate detections mad ein evaluation mode.""" - # process wihtout filtering detections + # process wihtout filtering detections self.update_linearity_heatmap(self.linearity_heatmap, detection) def get_skew_coverage(self): @@ -603,7 +605,7 @@ def get_skew_coverage(self): # Create a unique set to store covered intervals covered_intervals = set() # range in radians ToDo: define the range - skew_range = np.array([0,1.04]) + skew_range = np.array([0, 1.04]) for detection in self.training_data.get_detections(): if skew_range[0] <= detection.get_normalized_skew() < skew_range[1]: @@ -611,7 +613,7 @@ def get_skew_coverage(self): covered_intervals.add(interval_index) # Calculate the percentage of covered intervals - percentage_coverage = (len(covered_intervals) / num_intervals) # * 100 + percentage_coverage = len(covered_intervals) / num_intervals # * 100 return percentage_coverage @@ -627,7 +629,7 @@ def get_size_coverage(self): # Create a set to store covered intervals covered_intervals = set() # range for board size ToDo: define the range - size_range = np.array([0.08,0.21]) + size_range = np.array([0.08, 0.21]) for detection in self.training_data.get_detections(): if size_range[0] <= detection.get_normalized_size() < size_range[1]: @@ -635,7 +637,7 @@ def get_size_coverage(self): covered_intervals.add(interval_index) # Calculate the percentage of covered intervals - percentage_coverage = (len(covered_intervals) / num_intervals) # * 100 + percentage_coverage = len(covered_intervals) / num_intervals # * 100 return percentage_coverage diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 8b0deded..05500bff 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -23,11 +23,11 @@ from PySide2.QtCore import Qt from PySide2.QtGui import QBrush from PySide2.QtGui import QColor +from PySide2.QtGui import QFont from PySide2.QtGui import QImage from PySide2.QtGui import QPainter from PySide2.QtGui import QPen from PySide2.QtGui import QPixmap -from PySide2.QtGui import QFont from PySide2.QtWidgets import QGraphicsItem from PySide2.QtWidgets import QGraphicsView import cv2 @@ -174,7 +174,7 @@ def set_draw_training_heatmap(self, value: bool): def set_draw_evaluation_heatmap(self, value: bool): """Set the flag of wether or not to draw the occupancy heatmap of the evaluation dataset.""" self.is_draw_evaluation_heatmap = value - + def set_draw_linearity_heatmap(self, value: bool): """Set the flag of wether or not to draw the linearity heatmap.""" self.is_draw_linearity_heatmap = value @@ -211,14 +211,22 @@ def set_rendering_alpha(self, value: float): """Set the alpha channel of the drawings.""" self.rendering_alpha = value - def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float, - skew_percentage: float, board_size_percentage: float, - rows_linear_error: float, cols_linear_error: float, - pct_err_rows: float, pct_err_cols: float, - aspect_ratio: float, - pan: float, tilt: float, - alpha_indicators: float, - value: bool): + def set_draw_indicators( + self, + board_speed: float, + max_allowed_board_speed: float, + skew_percentage: float, + board_size_percentage: float, + rows_linear_error: float, + cols_linear_error: float, + pct_err_rows: float, + pct_err_cols: float, + aspect_ratio: float, + pan: float, + tilt: float, + alpha_indicators: float, + value: bool, + ): """Set values for indicators.""" self.current_board_speed = board_speed self.max_board_allowed_speed = max_allowed_board_speed @@ -231,7 +239,7 @@ def set_draw_indicators(self, board_speed: float, max_allowed_board_speed: float self.aspect_ratio = aspect_ratio self.pan = pan self.tilt = tilt - self.alpha_indicators = alpha_indicators + self.alpha_indicators = alpha_indicators self.is_draw_indicators = value def draw_indicators(self, painter: QPainter, display_size): @@ -239,7 +247,7 @@ def draw_indicators(self, painter: QPainter, display_size): color_green = QColor(0.0, 255, 0.0, int(255 * self.alpha_indicators)) color_red = QColor(255, 0.0, 0.0, int(255 * self.alpha_indicators)) # Change color according tot he speed - if (self.current_board_speed < self.max_board_allowed_speed): + if self.current_board_speed < self.max_board_allowed_speed: pen = QPen(color_green) brush = QBrush(color_green) else: @@ -247,7 +255,9 @@ def draw_indicators(self, painter: QPainter, display_size): brush = QBrush(color_red) painter.setPen(pen) painter.setBrush(brush) - speed_indicator = QRectF(QPointF(0, 0), QSize(display_size.width(), int(display_size.height()*0.04))) + speed_indicator = QRectF( + QPointF(0, 0), QSize(display_size.width(), int(display_size.height() * 0.04)) + ) # Draw the rectangle for speed indication painter.drawRect(speed_indicator) @@ -255,7 +265,9 @@ def draw_indicators(self, painter: QPainter, display_size): font_size = max(10, display_size.height() / 40) font = QFont("Arial", font_size) painter.setFont(font) - position_text_speed = QPointF(int(display_size.width() * .01), int(display_size.height() * .10)) + position_text_speed = QPointF( + int(display_size.width() * 0.01), int(display_size.height() * 0.10) + ) painter.drawText(position_text_speed, "Speed") # ToDo: define percentage to change skew and pct size to change to green @@ -270,15 +282,22 @@ def draw_indicators(self, painter: QPainter, display_size): painter.setBrush(brush_skew) # Draw Skew text - position_text_skew = QPointF(int(display_size.width() * .01), int(display_size.height() * .88)) + position_text_skew = QPointF( + int(display_size.width() * 0.01), int(display_size.height() * 0.88) + ) painter.drawText(position_text_skew, "Skew " + str(int(self.skew_percentage * 100)) + "%") - skew_indicator = QRectF(QPointF(int(display_size.width() * .12), int(display_size.height() * .85)), - QSize(display_size.width() * 0.08 * self.skew_percentage, int(display_size.height()*0.03))) + skew_indicator = QRectF( + QPointF(int(display_size.width() * 0.12), int(display_size.height() * 0.85)), + QSize( + display_size.width() * 0.08 * self.skew_percentage, + int(display_size.height() * 0.03), + ), + ) # Draw the skew progrees bar painter.drawRect(skew_indicator) - # ToDo: define percentage to change skew and pct size to change to green + # ToDo: define percentage to change skew and pct size to change to green thresold_to_be_green = 0.2 if self.board_size_percentage < thresold_to_be_green: pen_size_board = QPen(color_red) @@ -291,29 +310,61 @@ def draw_indicators(self, painter: QPainter, display_size): painter.setBrush(brush_size_board) # Draw board size text - position_text_size = QPointF(int(display_size.width() * .01), int(display_size.height() * .93)) - painter.drawText(position_text_size, "Size " + str(int(self.board_size_percentage * 100)) + "%") - board_size_indicator = QRectF(QPointF(int(display_size.width() * .12), int(display_size.height() * .90)), - QSize(display_size.width() * 0.08 * self.board_size_percentage, int(display_size.height() * 0.03))) + position_text_size = QPointF( + int(display_size.width() * 0.01), int(display_size.height() * 0.93) + ) + painter.drawText( + position_text_size, "Size " + str(int(self.board_size_percentage * 100)) + "%" + ) + board_size_indicator = QRectF( + QPointF(int(display_size.width() * 0.12), int(display_size.height() * 0.90)), + QSize( + display_size.width() * 0.08 * self.board_size_percentage, + int(display_size.height() * 0.03), + ), + ) # Draw the board size progrees bar painter.drawRect(board_size_indicator) # Draw board pan tilt angle text - deg_sign = u'\N{DEGREE SIGN}' - position_text_pan = QPointF(int(display_size.width() * .01), int(display_size.height() * .98)) - painter.drawText(position_text_pan, "Pan " + str(int(self.pan)) + deg_sign + " Tilt " + str(int(self.tilt)) + deg_sign) + deg_sign = "\N{DEGREE SIGN}" + position_text_pan = QPointF( + int(display_size.width() * 0.01), int(display_size.height() * 0.98) + ) + painter.drawText( + position_text_pan, + "Pan " + str(int(self.pan)) + deg_sign + " Tilt " + str(int(self.tilt)) + deg_sign, + ) # Draw Linear errors text - position_text_err_rows = QPointF(int(display_size.width() * .80), int(display_size.height() * .88)) - painter.drawText(position_text_err_rows, "ErrRows " + str(round(self.rows_linear_error, 1)) + "px " + \ - str(round(self.pct_err_rows * 100, 1)) + "%") + position_text_err_rows = QPointF( + int(display_size.width() * 0.80), int(display_size.height() * 0.88) + ) + painter.drawText( + position_text_err_rows, + "ErrRows " + + str(round(self.rows_linear_error, 1)) + + "px " + + str(round(self.pct_err_rows * 100, 1)) + + "%", + ) - position_text_err_cols = QPointF(int(display_size.width() * .80), int(display_size.height() * .93)) - painter.drawText(position_text_err_cols, "ErrCols " + str(round(self.cols_linear_error, 1)) + "px " + \ - str(round(self.pct_err_cols *100, 1)) + "%") + position_text_err_cols = QPointF( + int(display_size.width() * 0.80), int(display_size.height() * 0.93) + ) + painter.drawText( + position_text_err_cols, + "ErrCols " + + str(round(self.cols_linear_error, 1)) + + "px " + + str(round(self.pct_err_cols * 100, 1)) + + "%", + ) # Draw the aspect ratio text indicator - position_text_aspect_ratio = QPointF(int(display_size.width() * .80), int(display_size.height() * .98)) + position_text_aspect_ratio = QPointF( + int(display_size.width() * 0.80), int(display_size.height() * 0.98) + ) painter.drawText(position_text_aspect_ratio, "AspectR " + str(round(self.aspect_ratio, 2))) def pixmap(self) -> QPixmap: From ef16e47355ab4f8812e3f6489f680e854ff4cb86 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 20 Nov 2024 09:33:41 +0900 Subject: [PATCH 10/36] [wip] Adding menu to load profile parameters --- .../camera_calibrator.py | 158 ++++++++++++------ .../views/initialization_view.py | 38 ++++- 2 files changed, 147 insertions(+), 49 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 63e1e2ec..96fd21f2 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -13,7 +13,9 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - +import debugpy +debugpy.listen(5678) +debugpy.wait_for_client() # blocks execution until client is attached from collections import defaultdict import copy @@ -118,55 +120,55 @@ def __init__(self, cfg): self.board_type = BoardEnum.CHESS_BOARD self.board_parameters: ParameterizedClass = None self.detector: BoardDetector = None - self.data_collector = DataCollector(self.cfg["data_collector"]) - self.calibrator_dict: Dict[CalibratorEnum, Calibrator] = {} + # self.data_collector = DataCollector(self.cfg["data_collector"]) + # self.calibrator_dict: Dict[CalibratorEnum, Calibrator] = {} self.image_view_mode = ImageViewMode.SOURCE_UNRECTIFIED self.paused = False self.last_detection = None - for calibrator_type in CalibratorEnum: - calibrator_cfg = defaultdict() - - if ( - "calibrator_type" in self.cfg - and calibrator_type.value["name"] == self.cfg["calibrator_type"] - ): - calibrator_cfg = self.cfg["calibration_parameters"] - - calibrator = make_calibrator(calibrator_type, lock=self.lock, cfg=calibrator_cfg) - self.calibrator_dict[calibrator_type] = calibrator - - calibrator.moveToThread(self.calibration_thread) - calibrator.calibration_results_signal.connect(self.process_calibration_results) - calibrator.evaluation_results_signal.connect(self.process_evaluation_results) - calibrator.partial_calibration_results_signal.connect( - self.process_partial_calibration_result - ) - - # Qt logic - self.should_process_image.connect(self.process_data) - self.produced_data_signal.connect(self.process_new_data) - self.consumed_data_signal.connect(self.on_consumed) - - self.central_widget = QWidget(self) - self.setCentralWidget(self.central_widget) - self.layout = QHBoxLayout(self.central_widget) - - # Image View - self.make_image_view() - - # Menu Widgets - self.left_menu_widget = QWidget(self.central_widget) - self.left_menu_widget.setFixedWidth(300) - self.left_menu_layout = QVBoxLayout(self.left_menu_widget) - self.left_menu_layout.setAlignment(Qt.AlignTop) - - self.right_menu_widget = QWidget(self.central_widget) - self.right_menu_widget.setFixedWidth(300) - self.right_menu_layout = QVBoxLayout(self.right_menu_widget) - self.right_menu_layout.setAlignment(Qt.AlignTop) - self.setEnabled(False) + # for calibrator_type in CalibratorEnum: + # calibrator_cfg = defaultdict() + + # if ( + # "calibrator_type" in self.cfg + # and calibrator_type.value["name"] == self.cfg["calibrator_type"] + # ): + # calibrator_cfg = self.cfg["calibration_parameters"] + + # calibrator = make_calibrator(calibrator_type, lock=self.lock, cfg=calibrator_cfg) + # self.calibrator_dict[calibrator_type] = calibrator + + # calibrator.moveToThread(self.calibration_thread) + # calibrator.calibration_results_signal.connect(self.process_calibration_results) + # calibrator.evaluation_results_signal.connect(self.process_evaluation_results) + # calibrator.partial_calibration_results_signal.connect( + # self.process_partial_calibration_result + # ) + + # # Qt logic + # self.should_process_image.connect(self.process_data) + # self.produced_data_signal.connect(self.process_new_data) + # self.consumed_data_signal.connect(self.on_consumed) + + # self.central_widget = QWidget(self) + # self.setCentralWidget(self.central_widget) + # self.layout = QHBoxLayout(self.central_widget) + + # # Image View + # self.make_image_view() + + # # Menu Widgets + # self.left_menu_widget = QWidget(self.central_widget) + # self.left_menu_widget.setFixedWidth(300) + # self.left_menu_layout = QVBoxLayout(self.left_menu_widget) + # self.left_menu_layout.setAlignment(Qt.AlignTop) + + # self.right_menu_widget = QWidget(self.central_widget) + # self.right_menu_widget.setFixedWidth(300) + # self.right_menu_layout = QVBoxLayout(self.right_menu_widget) + # self.right_menu_layout.setAlignment(Qt.AlignTop) + # self.setEnabled(False) self.initialization_view = InitializationView(self, cfg) @@ -647,6 +649,7 @@ def start( board_type: BoardEnum, board_parameters: ParameterizedClass, initial_intrinsics: CameraModel, + cfg ): self.operation_mode = mode self.data_source = data_source @@ -654,6 +657,59 @@ def start( self.board_parameters = board_parameters self.current_camera_model = initial_intrinsics + # if a new parameter file was selected neleemnts are created with those parameters + self.cfg = cfg + + self.data_collector = DataCollector(self.cfg["data_collector"]) + self.calibrator_dict: Dict[CalibratorEnum, Calibrator] = {} + + # data_coll_params = self.data_collector.parameters_value() + # board_params = self.board_parameters.parameters_value() + # detector_params = self.detector.parameters_value() + # calibrator_type = self.calibrator_type_combobox.currentData() + for calibrator_type in CalibratorEnum: + calibrator_cfg = defaultdict() + + if ( + "calibrator_type" in self.cfg + and calibrator_type.value["name"] == self.cfg["calibrator_type"] + ): + calibrator_cfg = self.cfg["calibration_parameters"] + + calibrator = make_calibrator(calibrator_type, lock=self.lock, cfg=calibrator_cfg) + self.calibrator_dict[calibrator_type] = calibrator + + calibrator.moveToThread(self.calibration_thread) + calibrator.calibration_results_signal.connect(self.process_calibration_results) + calibrator.evaluation_results_signal.connect(self.process_evaluation_results) + calibrator.partial_calibration_results_signal.connect( + self.process_partial_calibration_result + ) + + # Qt logic + self.should_process_image.connect(self.process_data) + self.produced_data_signal.connect(self.process_new_data) + self.consumed_data_signal.connect(self.on_consumed) + + self.central_widget = QWidget(self) + self.setCentralWidget(self.central_widget) + self.layout = QHBoxLayout(self.central_widget) + + # Image View + self.make_image_view() + + # Menu Widgets + self.left_menu_widget = QWidget(self.central_widget) + self.left_menu_widget.setFixedWidth(300) + self.left_menu_layout = QVBoxLayout(self.left_menu_widget) + self.left_menu_layout.setAlignment(Qt.AlignTop) + + self.right_menu_widget = QWidget(self.central_widget) + self.right_menu_widget.setFixedWidth(300) + self.right_menu_layout = QVBoxLayout(self.right_menu_widget) + self.right_menu_layout.setAlignment(Qt.AlignTop) + self.setEnabled(False) + # Creating the UI elements after selecting CALIBRATION or EVALUATION # Mode group self.make_mode_group() @@ -1145,9 +1201,15 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.estimated_fps = 0.9 * self.estimated_fps + 0.1 * current_fps self.last_processed_stamp = current_time detection_time = current_time - self.detection_request_time - self.setWindowTitle( - f"Camera intrinsics calibrator ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" - ) + # self.setWindowTitle( + # f"Camera intrinsics calibrator ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" + # ) + if self.operation_mode == OperationMode.CALIBRATION: + self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" + ) + if self.operation_mode == OperationMode.EVALUATION: + self.setWindowTitle(f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" + ) self.image_view.update() self.graphics_view.update() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index 92116079..80bd2cfd 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -14,7 +14,9 @@ # See the License for the specific language governing permissions and # limitations under the License. - +import logging +import yaml +from collections import defaultdict from PySide2.QtCore import Signal from PySide2.QtWidgets import QComboBox from PySide2.QtWidgets import QFileDialog @@ -75,6 +77,38 @@ def __init__(self, calibrator: "CameraIntrinsicsCalibratorUI", cfg): # noqa F82 source_layout.addWidget(self.data_source_combobox) self.source_group.setLayout(source_layout) + self.params_combobox = QComboBox() + self.params_combobox.addItem("General", 0) + self.params_combobox.addItem("C1", 1) + self.params_combobox.addItem("C2", 2) + self.params_combobox.addItem("Load File", 3) + + def on_params_combo_box_changed(index): + if self.params_combobox.currentText() == "Load File": + file_name, _ = QFileDialog.getOpenFileName( + self, + "Open File", + "", + "All Files (*.*);;Text Files (*.yaml)" + ) + if file_name: + print(f"Selected file: {file_name}") + cfg = {} + try: + with open(file_name, "r") as stream: + cfg = yaml.safe_load(stream) + self.cfg = defaultdict(dict, cfg) + except Exception as e: + logging.error(f"Could not load the parameters from the YAML file ({e})") + + self.params_combobox.currentIndexChanged.connect(on_params_combo_box_changed) + + self.params_group = QGroupBox("Parameters Profile") + self.params_group.setFlat(True) + params_layout = QVBoxLayout() + params_layout.addWidget(self.params_combobox) + self.params_group.setLayout(params_layout) + # Board self.board_group = QGroupBox("Board options") self.board_group.setFlat(True) @@ -152,6 +186,7 @@ def load_intrinsics_button_callback(): self.layout.addWidget(self.source_group) self.layout.addWidget(self.board_group) self.layout.addWidget(self.mode_group) + self.layout.addWidget(self.params_group) self.layout.addWidget(self.initial_intrinsics_group) self.layout.addWidget(self.start_button) @@ -174,6 +209,7 @@ def on_success(): board_type, self.board_parameters_dict[board_type], self.initial_intrinsics, + self.cfg ) self.close() From c5b1ea8f228fc6a982db78498a69dc7962986363 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 20 Nov 2024 10:55:11 +0900 Subject: [PATCH 11/36] Fixing Typo errors an formatting --- .../board_detections/array_board_detection.py | 2 +- .../intrinsic_camera_calibrator/camera_calibrator.py | 4 ++-- .../intrinsic_camera_calibrator/data_collector.py | 4 ++-- .../intrinsic_camera_calibrator/views/image_view.py | 8 ++++---- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index 12f15819..02d1eef4 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -130,7 +130,7 @@ def get_flattened_cell_sizes(self): return self._cached_flattened_cell_sizes def get_aspect_ratio_pattern(self) -> float: - """Get aspect ratio using the calibration pattern, wich should be squared.""" + """Get aspect ratio using the calibration pattern, which should be squared.""" tilt, pan = self.get_rotation_angles() acceptance_angle = 10 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index e663119d..3814c6ee 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -581,7 +581,7 @@ def on_restart_linearity_heatmap_clicked(): self.draw_evaluation_heatmap_checkbox.setChecked(False) self.draw_evaluation_heatmap_checkbox.stateChanged.connect(draw_evaluation_heatmap_callback) - self.draw_linearity_heatmap_checkbox = QCheckBox("Draw lineariy error") + self.draw_linearity_heatmap_checkbox = QCheckBox("Draw linearity error") self.draw_linearity_heatmap_checkbox.setChecked(False) self.draw_linearity_heatmap_checkbox.stateChanged.connect(draw_linearity_heatmap_callback) @@ -1054,7 +1054,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) self.aspect_ratio_label.setText( - f"Aspect Reatio: {detection.get_aspect_ratio_pattern():.2f} px" # noqa E231 + f"Aspect Ratio: {detection.get_aspect_ratio_pattern():.2f} px" # noqa E231 ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt():.2f} degrees" # noqa E231 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 813379d6..af2dd16e 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -364,7 +364,7 @@ def get_training_occupancy_heatmap(self) -> np.array: return self.training_heatmap def get_linearity_heatmap(self) -> np.array: - """Return the linearity map to evaluate the recitification error.""" + """Return the linearity map to evaluate the image rectification.""" return self.linearity_heatmap def get_evaluation_occupancy_heatmap(self) -> np.array: @@ -594,7 +594,7 @@ def process_detection_eval_mode( mode: OperationMode = OperationMode.CALIBRATION, ) -> CollectionStatus: """Evaluate detections mad ein evaluation mode.""" - # process wihtout filtering detections + # process witout filtering detections self.update_linearity_heatmap(self.linearity_heatmap, detection) def get_skew_coverage(self): diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 05500bff..24f192d7 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -271,8 +271,8 @@ def draw_indicators(self, painter: QPainter, display_size): painter.drawText(position_text_speed, "Speed") # ToDo: define percentage to change skew and pct size to change to green - thresold_to_be_green = 0.3 - if self.skew_percentage < thresold_to_be_green: + threshold_to_be_green = 0.3 + if self.skew_percentage < threshold_to_be_green: pen_skew = QPen(color_red) brush_skew = QBrush(color_red) else: @@ -294,7 +294,7 @@ def draw_indicators(self, painter: QPainter, display_size): int(display_size.height() * 0.03), ), ) - # Draw the skew progrees bar + # Draw the skew progress bar painter.drawRect(skew_indicator) # ToDo: define percentage to change skew and pct size to change to green @@ -323,7 +323,7 @@ def draw_indicators(self, painter: QPainter, display_size): int(display_size.height() * 0.03), ), ) - # Draw the board size progrees bar + # Draw the board size progress bar painter.drawRect(board_size_indicator) # Draw board pan tilt angle text From 51b91ce33ce5197f65324d65626cbdfc84b864cd Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 20 Nov 2024 12:15:49 +0900 Subject: [PATCH 12/36] [Fix] Fixing more typo errors and spelling --- .../board_detections/array_board_detection.py | 8 ++++---- .../intrinsic_camera_calibrator/views/image_view.py | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index 02d1eef4..d201a9cc 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -143,10 +143,10 @@ def get_aspect_ratio_pattern(self) -> float: for j in range(self.rows - 1): for i in range(self.cols - 1): p = self.image_points[j, i] - pcol = self.image_points[j + 1, i] - prow = self.image_points[j, i + 1] - horizontal_distance = np.linalg.norm(p - prow) - vertical_distance = np.linalg.norm(p - pcol) + 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)) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 24f192d7..d1bdcdb1 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -298,8 +298,8 @@ def draw_indicators(self, painter: QPainter, display_size): painter.drawRect(skew_indicator) # ToDo: define percentage to change skew and pct size to change to green - thresold_to_be_green = 0.2 - if self.board_size_percentage < thresold_to_be_green: + threshold_to_be_green = 0.2 + if self.board_size_percentage < threshold_to_be_green: pen_size_board = QPen(color_red) brush_size_board = QBrush(color_red) else: From ac6dea50e3d278613c7076855c20cf301e4e4af2 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 20 Nov 2024 13:56:18 +0900 Subject: [PATCH 13/36] [Fix] disabling cspell for one line to pass check --- .../config/intrinsics_calibrator.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index 393f880d..c6ebad5b 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -29,7 +29,7 @@ calibrator_type: opencv data_collector: max_samples: 500 - decorrelate_eval_samples: 5 + decorrelate_eval_samples: 5 # cspell:disable-line max_allowed_tilt: 45.0 filter_by_speed: True max_allowed_pixel_speed: 10.0 From c22e13ef1fa48a0ed0610d4c9859d82459b77115 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 20 Nov 2024 14:17:27 +0900 Subject: [PATCH 14/36] [Fix] Removing capital from boolean yaml values because yamllint failure --- .../config/intrinsics_calibrator.yaml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index c6ebad5b..49605f27 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -5,20 +5,20 @@ board_parameters: cell_size: 0.1 calibration_parameters: - use_ransac_pre_rejection: True + use_ransac_pre_rejection: true 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 + use_entropy_maximization_subsampling: true subsampling_pixel_cells: 16 subsampling_tilt_resolution: 15.0 subsampling_max_tilt_deg: 45.0 - use_post_rejection: True + use_post_rejection: true post_rejection_max_rms_error: 0.35 - plot_calibration_data_statistics: True - plot_calibration_results_statistics: True + plot_calibration_data_statistics: true + plot_calibration_results_statistics: true viz_pixel_cells: 16 viz_tilt_resolution: 15.0 viz_max_tilt_deg: 45.0 @@ -31,17 +31,17 @@ data_collector: max_samples: 500 decorrelate_eval_samples: 5 # cspell:disable-line max_allowed_tilt: 45.0 - filter_by_speed: True + filter_by_speed: true max_allowed_pixel_speed: 10.0 max_allowed_speed: 0.1 - filter_by_reprojection_error: True + filter_by_reprojection_error: true max_allowed_max_reprojection_error: 0.5 max_allowed_rms_reprojection_error: 0.3 - filter_by_2d_redundancy: True + filter_by_2d_redundancy: true 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 + filter_by_3d_redundancy: true min_3d_center_difference: 1.0 min_tilt_difference: 15.0 heatmap_cells: 16 From 39ff37d0c169b3256e6ad39b32e66067b078ef90 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 20 Nov 2024 14:38:25 +0900 Subject: [PATCH 15/36] Disabling rule:truthy for yamllint check --- .../config/intrinsics_calibrator.yaml | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index 49605f27..51a73517 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -1,3 +1,4 @@ +# yamllint disable-line rule:truthy board_type: chess_board board_parameters: cols: 8 @@ -5,20 +6,20 @@ board_parameters: cell_size: 0.1 calibration_parameters: - use_ransac_pre_rejection: true + 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 + 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 + use_post_rejection: true # yamllint disable-line rule:truthy post_rejection_max_rms_error: 0.35 - plot_calibration_data_statistics: true - plot_calibration_results_statistics: true + 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 @@ -31,17 +32,17 @@ data_collector: max_samples: 500 decorrelate_eval_samples: 5 # cspell:disable-line max_allowed_tilt: 45.0 - filter_by_speed: true + 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 + 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 + 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 + filter_by_3d_redundancy: true # yamllint disable-line rule:truthy min_3d_center_difference: 1.0 min_tilt_difference: 15.0 heatmap_cells: 16 From 498cc458b2a446a58be9356e80142fd9cc3a77e5 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 27 Nov 2024 18:21:26 +0900 Subject: [PATCH 16/36] fix: solved runtime errors but disabled some features Signed-off-by: Kenzo Lobos-Tsunekawa --- .../config/intrinsics_calibrator.yaml | 2 +- .../intrinsic_camera_calibrator/camera_calibrator.py | 11 +++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index 51a73517..ae0ceadd 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -24,7 +24,7 @@ calibration_parameters: viz_tilt_resolution: 15.0 viz_max_tilt_deg: 45.0 viz_z_cells: 12 - radial_distortion_coefficients: 5 + radial_distortion_coefficients: 3 calibrator_type: opencv diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index fe6a5808..42f1cbea 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -14,7 +14,6 @@ # See the License for the specific language governing permissions and # limitations under the License. - from collections import defaultdict import copy import logging @@ -1061,9 +1060,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.raw_linear_error_cols_rms_label.setText( f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) - self.aspect_ratio_label.setText( - f"Aspect Ratio: {detection.get_aspect_ratio_pattern():.2f} px" # noqa E231 - ) + # self.aspect_ratio_label.setText( + # f"Aspect Ratio: {detection.get_aspect_ratio_pattern():.2f} px" # noqa E231 + # ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt(camera_model):.2f} degrees" # noqa E231 ) @@ -1100,7 +1099,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im 0 if self.last_detection is None else detection.get_speed(self.last_detection) ) self.last_detection = detection - pan, tilt = detection.get_rotation_angles() + pan, tilt = rough_angles self.image_view.set_draw_indicators( board_speed, self.data_collector.max_allowed_pixel_speed.value, @@ -1110,7 +1109,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im err_rms_cols, pct_err_rows, pct_err_cols, - detection.get_aspect_ratio_pattern(), + 0.0, # detection.get_aspect_ratio_pattern(), pan, tilt, self.indicators_alpha_spinbox.value(), From fc83b36b6cdec5b30fbefe554107e67c9bdd2bfc Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Thu, 28 Nov 2024 12:16:30 +0900 Subject: [PATCH 17/36] [wip] menu to load 3 camera parameters profile --- .../config/c1_intrinsics_calibrator.yaml | 51 ++++++++++++++++ .../config/c2_intrinsics_calibrator.yaml | 51 ++++++++++++++++ .../config/intrinsics_calibrator.yaml | 2 +- .../camera_calibrator.py | 60 ++----------------- .../views/initialization_view.py | 40 ++++++++++--- 5 files changed, 141 insertions(+), 63 deletions(-) create mode 100644 calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml create mode 100644 calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml new file mode 100644 index 00000000..ae0ceadd --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml @@ -0,0 +1,51 @@ +# 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml new file mode 100644 index 00000000..51a73517 --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml @@ -0,0 +1,51 @@ +# 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index 51a73517..01f9eda3 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -24,7 +24,7 @@ calibration_parameters: viz_tilt_resolution: 15.0 viz_max_tilt_deg: 45.0 viz_z_cells: 12 - radial_distortion_coefficients: 5 + radial_distortion_coefficients: 4 calibrator_type: opencv diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index dfba03e9..9da0d017 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -120,56 +120,11 @@ def __init__(self, cfg): self.board_type = BoardEnum.CHESS_BOARD self.board_parameters: ParameterizedClass = None self.detector: BoardDetector = None - # self.data_collector = DataCollector(self.cfg["data_collector"]) - # self.calibrator_dict: Dict[CalibratorEnum, Calibrator] = {} self.image_view_mode = ImageViewMode.SOURCE_UNRECTIFIED self.paused = False self.last_detection = None - # for calibrator_type in CalibratorEnum: - # calibrator_cfg = defaultdict() - - # if ( - # "calibrator_type" in self.cfg - # and calibrator_type.value["name"] == self.cfg["calibrator_type"] - # ): - # calibrator_cfg = self.cfg["calibration_parameters"] - - # calibrator = make_calibrator(calibrator_type, lock=self.lock, cfg=calibrator_cfg) - # self.calibrator_dict[calibrator_type] = calibrator - - # calibrator.moveToThread(self.calibration_thread) - # calibrator.calibration_results_signal.connect(self.process_calibration_results) - # calibrator.evaluation_results_signal.connect(self.process_evaluation_results) - # calibrator.partial_calibration_results_signal.connect( - # self.process_partial_calibration_result - # ) - - # # Qt logic - # self.should_process_image.connect(self.process_data) - # self.produced_data_signal.connect(self.process_new_data) - # self.consumed_data_signal.connect(self.on_consumed) - - # self.central_widget = QWidget(self) - # self.setCentralWidget(self.central_widget) - # self.layout = QHBoxLayout(self.central_widget) - - # # Image View - # self.make_image_view() - - # # Menu Widgets - # self.left_menu_widget = QWidget(self.central_widget) - # self.left_menu_widget.setFixedWidth(300) - # self.left_menu_layout = QVBoxLayout(self.left_menu_widget) - # self.left_menu_layout.setAlignment(Qt.AlignTop) - - # self.right_menu_widget = QWidget(self.central_widget) - # self.right_menu_widget.setFixedWidth(300) - # self.right_menu_layout = QVBoxLayout(self.right_menu_widget) - # self.right_menu_layout.setAlignment(Qt.AlignTop) - # self.setEnabled(False) - self.initialization_view = InitializationView(self, cfg) def make_image_view(self): @@ -645,7 +600,7 @@ def start( board_type: BoardEnum, board_parameters: ParameterizedClass, initial_intrinsics: CameraModel, - cfg + cfg : dict, ): self.operation_mode = mode self.data_source = data_source @@ -653,16 +608,12 @@ def start( self.board_parameters = board_parameters self.current_camera_model = initial_intrinsics - # if a new parameter file was selected neleemnts are created with those parameters - self.cfg = cfg + # if a new parameter file was selected new dictionary is created from those parameters + self.cfg = defaultdict(dict, cfg) self.data_collector = DataCollector(self.cfg["data_collector"]) self.calibrator_dict: Dict[CalibratorEnum, Calibrator] = {} - # data_coll_params = self.data_collector.parameters_value() - # board_params = self.board_parameters.parameters_value() - # detector_params = self.detector.parameters_value() - # calibrator_type = self.calibrator_type_combobox.currentData() for calibrator_type in CalibratorEnum: calibrator_cfg = defaultdict() @@ -917,6 +868,7 @@ def save_parameters(self, filename): calib_params = self.calibrator_dict[calibrator_type].parameters_value() with open(filename, "w") as file: yaml.dump({"board_parameters": board_params}, file, default_flow_style=False) + yaml.dump({"board_type" : self.board_type.value["name"]}, file, default_flow_style=False) yaml.dump( {"calibrator_type": calibrator_type.value["name"]}, file, default_flow_style=False ) @@ -1225,9 +1177,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.estimated_fps = 0.9 * self.estimated_fps + 0.1 * current_fps self.last_processed_stamp = current_time detection_time = current_time - self.detection_request_time - # self.setWindowTitle( - # f"Camera intrinsics calibrator ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" - # ) + if self.operation_mode == OperationMode.CALIBRATION: self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" ) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index 80bd2cfd..7b0fb387 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -39,6 +39,8 @@ from intrinsic_camera_calibrator.views.parameter_view import ParameterView from intrinsic_camera_calibrator.views.ros_bag_view import RosBagView from intrinsic_camera_calibrator.views.ros_topic_view import RosTopicView +from ament_index_python.packages import get_package_share_directory +import os class InitializationView(QWidget): @@ -62,6 +64,11 @@ def __init__(self, calibrator: "CameraIntrinsicsCalibratorUI", cfg): # noqa F82 for board_type in BoardEnum } + # Get the package share directory + package_share_dir = get_package_share_directory('intrinsic_camera_calibrator') + # Get the path to the config directory + config_dir = os.path.join(package_share_dir, 'config') + self.layout = QVBoxLayout(self) # Source @@ -93,11 +100,22 @@ def on_params_combo_box_changed(index): ) if file_name: print(f"Selected file: {file_name}") + config_file_path = file_name + elif self.params_combobox.currentText() == "C1": + config_file_path = os.path.join(config_dir, 'c1_intrinsics_calibrator.yaml') + elif self.params_combobox.currentText() == "C2": + config_file_path = os.path.join(config_dir, 'c2_intrinsics_calibrator.yaml') + elif self.params_combobox.currentText() == "General": + config_file_path = os.path.join(config_dir, 'c2_intrinsics_calibrator.yaml') + + if config_file_path: cfg = {} try: - with open(file_name, "r") as stream: + with open(config_file_path, "r") as stream: cfg = yaml.safe_load(stream) self.cfg = defaultdict(dict, cfg) + self.update_board_type() + print("successfully loaded") except Exception as e: logging.error(f"Could not load the parameters from the YAML file ({e})") @@ -119,12 +137,7 @@ def on_params_combo_box_changed(index): for board_type in BoardEnum: self.board_type_combobox.addItem(board_type.value["display"], board_type) - if self.cfg["board_type"] != "": - self.board_type_combobox.setCurrentIndex( - BoardEnum.from_name(self.cfg["board_type"]).get_id() - ) - else: - self.board_type_combobox.setCurrentIndex(0) + self.update_board_type() def board_parameters_on_closed(): self.setEnabled(True) @@ -192,6 +205,19 @@ def load_intrinsics_button_callback(): self.show() + def update_board_type(self): + if self.cfg["board_type"] != "": + self.board_type_combobox.setCurrentIndex( + BoardEnum.from_name(self.cfg["board_type"]).get_id() + ) + else: + self.board_type_combobox.setCurrentIndex(0) + + self.board_parameters_dict = { + board_type: make_board_parameters(board_type, cfg=self.cfg["board_parameters"]) + for board_type in BoardEnum + } + def on_start(self): """Start the calibration process after receiving the user settings.""" From ef13c4c3558f36d20438a107405c8641fccb1cfb Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 3 Dec 2024 14:24:00 +0900 Subject: [PATCH 18/36] [fix] eval mode working with new ceres solver, aspect ratio working --- .../config/intrinsics_calibrator_ceres.yaml | 55 +++++++++++++++ .../board_detections/array_board_detection.py | 23 ------- .../board_detections/board_detection.py | 27 +++++++- .../camera_calibrator.py | 69 +++++++++++-------- .../camera_models/camera_model.py | 4 +- .../views/initialization_view.py | 7 +- 6 files changed, 126 insertions(+), 59 deletions(-) create mode 100644 calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml new file mode 100644 index 00000000..5f8cdfcf --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml @@ -0,0 +1,55 @@ +# 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 + 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: 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: 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index d201a9cc..549e46ec 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -129,26 +129,3 @@ def get_flattened_cell_sizes(self): self._cached_flattened_cell_sizes = cell_sizes.flatten() return self._cached_flattened_cell_sizes - def get_aspect_ratio_pattern(self) -> float: - """Get aspect ratio using the calibration pattern, which should be squared.""" - tilt, pan = self.get_rotation_angles() - 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index 70a7c38c..2b9f6494 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -101,9 +101,6 @@ 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 get_aspect_ratio_pattern(self) -> float: - """Return aspect ratio obtained from the mean of the horizontal and vertical distances of the calibration pattern.""" - raise NotImplementedError def restart_linearity_heatmap(self): """Restart linearity heatmap.""" @@ -242,3 +239,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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 65f69a45..6a098d5c 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -240,13 +240,40 @@ def on_evaluation_sample_changed(index): self.mode_options_group.setLayout(mode_options_layout) + def make_solver_group(self): + self.solver_group = QGroupBox("Solver selection") + self.solver_group.setFlat(True) + self.calibrator_type_combobox = QComboBox() + self.calibrator_type_combobox.setEnabled(True) + + def on_calibrator_clicked(): + self.current_calibrator_type = self.calibrator_type_combobox.currentData() + + for calibrator_type in CalibratorEnum: + self.calibrator_type_combobox.addItem(calibrator_type.value["display"], calibrator_type) + + if "calibrator_type" in self.cfg: + try: + self.calibrator_type_combobox.setCurrentIndex( + CalibratorEnum.from_name(self.cfg["calibrator_type"]).get_id() + ) + except Exception as e: + logging.error(f"Invalid calibration_type: {e}") + else: + self.calibrator_type_combobox.setCurrentIndex(0) + + self.calibrator_type_combobox.currentIndexChanged.connect(on_calibrator_clicked) + self.current_calibrator_type = self.calibrator_type_combobox.currentData() + + solver_layout = QVBoxLayout() + solver_layout.setAlignment(Qt.AlignTop) + solver_layout.addWidget(self.calibrator_type_combobox) + self.solver_group.setLayout(solver_layout) + def make_calibration_group(self): self.calibration_group = QGroupBox("Calibration control") self.calibration_group.setFlat(True) - self.calibrator_type_combobox = QComboBox() - self.calibrator_type_combobox.setEnabled(True) - self.calibration_parameters_button = QPushButton("Calibration parameters") self.calibration_button = QPushButton("Calibrate") self.evaluation_button = QPushButton("Evaluate") @@ -310,9 +337,6 @@ def on_evaluation_clicked(): self.calibration_status_label.setText("Calibration status: evaluating") - def on_calibrator_clicked(): - self.current_calibrator_type = self.calibrator_type_combobox.currentData() - self.calibration_parameters_button.clicked.connect(on_parameters_button_clicked) self.calibration_button.clicked.connect(on_calibration_clicked) @@ -324,24 +348,8 @@ def on_calibrator_clicked(): self.save_button.clicked.connect(self.on_save_clicked) self.save_button.setEnabled(False) - self.calibrator_type_combobox.currentIndexChanged.connect(on_calibrator_clicked) - - for calibrator_type in CalibratorEnum: - self.calibrator_type_combobox.addItem(calibrator_type.value["display"], calibrator_type) - - if "calibrator_type" in self.cfg: - try: - self.calibrator_type_combobox.setCurrentIndex( - CalibratorEnum.from_name(self.cfg["calibrator_type"]).get_id() - ) - except Exception as e: - logging.error(f"Invalid calibration_type: {e}") - else: - self.calibrator_type_combobox.setCurrentIndex(0) - calibration_layout = QVBoxLayout() calibration_layout.setAlignment(Qt.AlignTop) - calibration_layout.addWidget(self.calibrator_type_combobox) calibration_layout.addWidget(self.calibration_parameters_button) calibration_layout.addWidget(self.calibration_button) calibration_layout.addWidget(self.evaluation_button) @@ -666,13 +674,15 @@ def start( # Mode group self.make_mode_group() + # Create solver selector + self.make_solver_group() + # Calibration group if self.operation_mode == OperationMode.CALIBRATION: self.make_calibration_group() # Detector group - if self.operation_mode == OperationMode.CALIBRATION: - self.make_detector_group() + self.make_detector_group() # Detections group self.make_detection_group() @@ -685,9 +695,10 @@ def start( self.make_visualization_group() # self.menu_layout.addWidget(label) + self.left_menu_layout.addWidget(self.solver_group) if self.operation_mode == OperationMode.CALIBRATION: self.left_menu_layout.addWidget(self.calibration_group) - self.left_menu_layout.addWidget(self.detector_options_group) + self.left_menu_layout.addWidget(self.detector_options_group) self.left_menu_layout.addWidget(self.raw_detection_results_group) self.left_menu_layout.addWidget(self.single_shot_detection_results_group) @@ -1066,9 +1077,9 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.raw_linear_error_cols_rms_label.setText( f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) - # self.aspect_ratio_label.setText( - # f"Aspect Ratio: {detection.get_aspect_ratio_pattern():.2f} px" # noqa E231 - # ) + self.aspect_ratio_label.setText( + f"Aspect Ratio: {detection.get_aspect_ratio_pattern(camera_model):.2f} px" # noqa E231 + ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt(camera_model):.2f} degrees" # noqa E231 ) @@ -1115,7 +1126,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im err_rms_cols, pct_err_rows, pct_err_cols, - 0.0, # detection.get_aspect_ratio_pattern(), + detection.get_aspect_ratio_pattern(camera_model), pan, tilt, self.indicators_alpha_spinbox.value(), diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model.py index 73f08766..cb60c931 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model.py @@ -197,8 +197,8 @@ def from_dict(self, d): .astype(np.float64) ) self.d = ( - np.array(d["distortion_model"]["data"]) - .reshape(d["distortion_model"]["rows"], d["distortion_model"]["cols"]) + np.array(d["distortion_coefficients"]["data"]) + .reshape(d["distortion_coefficients"]["rows"], d["distortion_coefficients"]["cols"]) .astype(np.float64) ) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index bc398bd4..b916d25b 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -88,7 +88,8 @@ def __init__(self, calibrator: "CameraIntrinsicsCalibratorUI", cfg): # noqa F82 self.params_combobox.addItem("General", 0) self.params_combobox.addItem("C1", 1) self.params_combobox.addItem("C2", 2) - self.params_combobox.addItem("Load File", 3) + self.params_combobox.addItem("Ceres Calib", 3) + self.params_combobox.addItem("Load File", 4) def on_params_combo_box_changed(index): if self.params_combobox.currentText() == "Load File": @@ -105,8 +106,10 @@ def on_params_combo_box_changed(index): config_file_path = os.path.join(config_dir, 'c1_intrinsics_calibrator.yaml') elif self.params_combobox.currentText() == "C2": config_file_path = os.path.join(config_dir, 'c2_intrinsics_calibrator.yaml') + elif self.params_combobox.currentText() == "Ceres Calib": + config_file_path = os.path.join(config_dir, 'intrinsics_calibrator_ceres.yaml') elif self.params_combobox.currentText() == "General": - config_file_path = os.path.join(config_dir, 'c2_intrinsics_calibrator.yaml') + config_file_path = os.path.join(config_dir, 'intrinsics_calibrator.yaml') if config_file_path: cfg = {} From 12e91d1946e92ed610384b61c39492949f7f81b5 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 3 Dec 2024 15:15:45 +0900 Subject: [PATCH 19/36] [New] Added set_parameters fcn to chessboard detection, params added to yaml files --- .../config/c1_intrinsics_calibrator.yaml | 8 ++++++++ .../config/c2_intrinsics_calibrator.yaml | 8 ++++++++ .../config/intrinsics_calibrator.yaml | 8 ++++++++ .../config/intrinsics_calibrator_ceres.yaml | 8 ++++++++ .../intrinsic_camera_calibrator/camera_calibrator.py | 4 +++- 5 files changed, 35 insertions(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml index ae0ceadd..6bb0d071 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml @@ -49,3 +49,11 @@ data_collector: 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml index 51a73517..b15d3904 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml @@ -49,3 +49,11 @@ data_collector: 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index ae0ceadd..e6d5c1a9 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -49,3 +49,11 @@ data_collector: 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 \ No newline at end of file diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml index 5f8cdfcf..c97f3db6 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml @@ -53,3 +53,11 @@ data_collector: 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 \ No newline at end of file diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 6a098d5c..afda162f 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -13,7 +13,9 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. - +import debugpy +debugpy.listen(5678) +debugpy.wait_for_client() # blocks execution until client is attached from collections import defaultdict import copy import logging From a5d196526fc2a89a3bdaf9bf718c1cfe40b7f4ad Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Tue, 3 Dec 2024 15:42:01 +0900 Subject: [PATCH 20/36] [fix] fixed mistake on the previous commit when adding files --- .../board_detectors/chessboard_detector.py | 1 + .../intrinsic_camera_calibrator/camera_calibrator.py | 3 --- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py index eae003f3..20e4beb5 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py @@ -33,6 +33,7 @@ def __init__(self, **kwargs): 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) + super().set_parameters(**kwargs["cfg"]) def detect(self, img: np.array, stamp: float): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index afda162f..794e1a3f 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -13,9 +13,6 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -import debugpy -debugpy.listen(5678) -debugpy.wait_for_client() # blocks execution until client is attached from collections import defaultdict import copy import logging From f2ad5cce4cffc72284d0b575020c40851475872e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 3 Dec 2024 06:52:10 +0000 Subject: [PATCH 21/36] ci(pre-commit): autofix --- .../config/c1_intrinsics_calibrator.yaml | 2 +- .../config/c2_intrinsics_calibrator.yaml | 2 +- .../config/intrinsics_calibrator.yaml | 4 +-- .../config/intrinsics_calibrator_ceres.yaml | 4 +-- .../board_detections/array_board_detection.py | 1 - .../board_detections/board_detection.py | 3 +- .../camera_calibrator.py | 18 ++++++------ .../views/initialization_view.py | 28 +++++++++---------- 8 files changed, 30 insertions(+), 32 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml index 6bb0d071..46ce0086 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml @@ -54,6 +54,6 @@ chess_board_detector: adaptive_thresh: false normalize_image: false fast_check: false - resized_detection: false + resized_detection: false resized_max_resolution: 1000 sub_pixel_refinement: true diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml index b15d3904..23edc255 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml @@ -54,6 +54,6 @@ chess_board_detector: adaptive_thresh: false normalize_image: false fast_check: false - resized_detection: false + resized_detection: false resized_max_resolution: 1000 sub_pixel_refinement: true diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml index e6d5c1a9..46ce0086 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml @@ -54,6 +54,6 @@ chess_board_detector: adaptive_thresh: false normalize_image: false fast_check: false - resized_detection: false + resized_detection: false resized_max_resolution: 1000 - sub_pixel_refinement: true \ No newline at end of file + sub_pixel_refinement: true diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml index c97f3db6..0188281d 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml @@ -58,6 +58,6 @@ chess_board_detector: adaptive_thresh: false normalize_image: false fast_check: false - resized_detection: false + resized_detection: false resized_max_resolution: 1000 - sub_pixel_refinement: true \ No newline at end of file + sub_pixel_refinement: true diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py index 549e46ec..0c6911c1 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/array_board_detection.py @@ -128,4 +128,3 @@ def get_flattened_cell_sizes(self): self._cached_flattened_cell_sizes = cell_sizes.flatten() return self._cached_flattened_cell_sizes - diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index 2b9f6494..33d0a169 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -101,7 +101,6 @@ 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 @@ -239,7 +238,7 @@ 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) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 794e1a3f..a55df25d 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -247,7 +247,7 @@ def make_solver_group(self): def on_calibrator_clicked(): self.current_calibrator_type = self.calibrator_type_combobox.currentData() - + for calibrator_type in CalibratorEnum: self.calibrator_type_combobox.addItem(calibrator_type.value["display"], calibrator_type) @@ -263,7 +263,7 @@ def on_calibrator_clicked(): self.calibrator_type_combobox.currentIndexChanged.connect(on_calibrator_clicked) self.current_calibrator_type = self.calibrator_type_combobox.currentData() - + solver_layout = QVBoxLayout() solver_layout.setAlignment(Qt.AlignTop) solver_layout.addWidget(self.calibrator_type_combobox) @@ -612,7 +612,7 @@ def start( board_type: BoardEnum, board_parameters: ParameterizedClass, initial_intrinsics: CameraModel, - cfg : dict, + cfg: dict, ): self.operation_mode = mode self.data_source = data_source @@ -644,7 +644,7 @@ def start( calibrator.partial_calibration_results_signal.connect( self.process_partial_calibration_result ) - + # Qt logic self.should_process_image.connect(self.process_data) self.produced_data_signal.connect(self.process_new_data) @@ -883,7 +883,7 @@ def save_parameters(self, filename): calib_params = self.calibrator_dict[calibrator_type].get_parameters_values() with open(filename, "w") as file: yaml.dump({"board_parameters": board_params}, file, default_flow_style=False) - yaml.dump({"board_type" : self.board_type.value["name"]}, file, default_flow_style=False) + yaml.dump({"board_type": self.board_type.value["name"]}, file, default_flow_style=False) yaml.dump( {"calibrator_type": calibrator_type.value["name"]}, file, default_flow_style=False ) @@ -1077,7 +1077,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) self.aspect_ratio_label.setText( - f"Aspect Ratio: {detection.get_aspect_ratio_pattern(camera_model):.2f} px" # noqa E231 + f"Aspect Ratio: {detection.get_aspect_ratio_pattern(camera_model):.2f} px" # noqa E231 ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt(camera_model):.2f} degrees" # noqa E231 @@ -1194,10 +1194,12 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im detection_time = current_time - self.detection_request_time if self.operation_mode == OperationMode.CALIBRATION: - self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" + self.setWindowTitle( + f"Camera intrinsics calibrator ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" ) if self.operation_mode == OperationMode.EVALUATION: - self.setWindowTitle(f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" + self.setWindowTitle( + f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" ) self.image_view.update() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index b916d25b..0d6e76cf 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -14,9 +14,10 @@ # See the License for the specific language governing permissions and # limitations under the License. -import logging -import yaml from collections import defaultdict +import logging +import os + from PySide2.QtCore import Signal from PySide2.QtWidgets import QComboBox from PySide2.QtWidgets import QFileDialog @@ -25,6 +26,7 @@ from PySide2.QtWidgets import QRadioButton from PySide2.QtWidgets import QVBoxLayout from PySide2.QtWidgets import QWidget +from ament_index_python.packages import get_package_share_directory from intrinsic_camera_calibrator.board_parameters.board_parameters_factory import ( make_board_parameters, ) @@ -39,8 +41,7 @@ from intrinsic_camera_calibrator.views.parameter_view import ParameterView from intrinsic_camera_calibrator.views.ros_bag_view import RosBagView from intrinsic_camera_calibrator.views.ros_topic_view import RosTopicView -from ament_index_python.packages import get_package_share_directory -import os +import yaml class InitializationView(QWidget): @@ -65,9 +66,9 @@ def __init__(self, calibrator: "CameraIntrinsicsCalibratorUI", cfg): # noqa F82 } # Get the package share directory - package_share_dir = get_package_share_directory('intrinsic_camera_calibrator') + package_share_dir = get_package_share_directory("intrinsic_camera_calibrator") # Get the path to the config directory - config_dir = os.path.join(package_share_dir, 'config') + config_dir = os.path.join(package_share_dir, "config") self.layout = QVBoxLayout(self) @@ -94,22 +95,19 @@ def __init__(self, calibrator: "CameraIntrinsicsCalibratorUI", cfg): # noqa F82 def on_params_combo_box_changed(index): if self.params_combobox.currentText() == "Load File": file_name, _ = QFileDialog.getOpenFileName( - self, - "Open File", - "", - "All Files (*.*);;Text Files (*.yaml)" + self, "Open File", "", "All Files (*.*);;Text Files (*.yaml)" ) if file_name: print(f"Selected file: {file_name}") config_file_path = file_name elif self.params_combobox.currentText() == "C1": - config_file_path = os.path.join(config_dir, 'c1_intrinsics_calibrator.yaml') + config_file_path = os.path.join(config_dir, "c1_intrinsics_calibrator.yaml") elif self.params_combobox.currentText() == "C2": - config_file_path = os.path.join(config_dir, 'c2_intrinsics_calibrator.yaml') + config_file_path = os.path.join(config_dir, "c2_intrinsics_calibrator.yaml") elif self.params_combobox.currentText() == "Ceres Calib": - config_file_path = os.path.join(config_dir, 'intrinsics_calibrator_ceres.yaml') + config_file_path = os.path.join(config_dir, "intrinsics_calibrator_ceres.yaml") elif self.params_combobox.currentText() == "General": - config_file_path = os.path.join(config_dir, 'intrinsics_calibrator.yaml') + config_file_path = os.path.join(config_dir, "intrinsics_calibrator.yaml") if config_file_path: cfg = {} @@ -238,7 +236,7 @@ def on_success(): board_type, self.board_parameters_dict[board_type], self.initial_intrinsics, - self.cfg + self.cfg, ) self.close() From bce18ef8f513dd258dee5cc1ff2f0b6de4b80585 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 11 Dec 2024 17:52:53 +0900 Subject: [PATCH 22/36] refactor: first changes according to the PR comments --- .../config/c1_intrinsics_calibrator.yaml | 7 +++ .../config/c2_intrinsics_calibrator.yaml | 9 ++- ....yaml => ceres_intrinsics_calibrator.yaml} | 0 ...aml => general_intrinsics_calibrator.yaml} | 9 ++- .../board_detections/board_detection.py | 2 +- .../camera_calibrator.py | 58 +++++++++---------- .../data_collector.py | 40 +++++-------- .../views/initialization_view.py | 34 +++++------ .../launch/calibrator.launch.xml | 2 +- 9 files changed, 86 insertions(+), 75 deletions(-) rename calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/{intrinsics_calibrator_ceres.yaml => ceres_intrinsics_calibrator.yaml} (100%) rename calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/{intrinsics_calibrator.yaml => general_intrinsics_calibrator.yaml} (88%) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml index 46ce0086..2cb04e4d 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c1_intrinsics_calibrator.yaml @@ -25,6 +25,13 @@ calibration_parameters: viz_max_tilt_deg: 45.0 viz_z_cells: 12 radial_distortion_coefficients: 3 + rational_distortion_coefficients: 0 + use_tangential_distortion: true + enable_prism_model: false + fix_principal_point: false + fix_aspect_ratio: false + use_lu_decomposition: false + use_qr_decomposition: false calibrator_type: opencv diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml index 23edc255..3d227c50 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml @@ -24,7 +24,14 @@ calibration_parameters: viz_tilt_resolution: 15.0 viz_max_tilt_deg: 45.0 viz_z_cells: 12 - radial_distortion_coefficients: 5 + radial_distortion_coefficients: 3 + rational_distortion_coefficients: 3 + use_tangential_distortion: true + enable_prism_model: false + fix_principal_point: false + fix_aspect_ratio: false + use_lu_decomposition: false + use_qr_decomposition: false calibrator_type: opencv diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/ceres_intrinsics_calibrator.yaml similarity index 100% rename from calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator_ceres.yaml rename to calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/ceres_intrinsics_calibrator.yaml diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/general_intrinsics_calibrator.yaml similarity index 88% rename from calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml rename to calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/general_intrinsics_calibrator.yaml index 46ce0086..c78474cd 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/general_intrinsics_calibrator.yaml @@ -24,7 +24,14 @@ calibration_parameters: viz_tilt_resolution: 15.0 viz_max_tilt_deg: 45.0 viz_z_cells: 12 - radial_distortion_coefficients: 3 + radial_distortion_coefficients: 2 + rational_distortion_coefficients: 0 + use_tangential_distortion: true + enable_prism_model: false + fix_principal_point: false + fix_aspect_ratio: false + use_lu_decomposition: false + use_qr_decomposition: false calibrator_type: opencv diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index 33d0a169..3aac6605 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -244,7 +244,7 @@ def get_aspect_ratio_pattern(self, model: CameraModel) -> float: tilt, pan = self.get_rotation_angles(model) acceptance_angle = 10 - # dont update if we the detection has big angles, calculation will not be accurate + # Dont update if the board 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 diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index a55df25d..04f9faad 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -540,7 +540,6 @@ def draw_linearity_heatmap_callback(value): self.should_process_image.emit() def on_restart_linearity_heatmap_clicked(): - print("restart_lin_heatmap", flush=True) self.data_collector.restart_linearity_heatmap() self.restart_linearity_heatmap_button = QPushButton("Clear heatmap linearity") @@ -748,7 +747,7 @@ def start( self.training_sample_slider.setEnabled(False) self.evaluation_sample_slider.setEnabled(False) self.image_view_type_combobox.clear() - # Order of of how items are added to the combobox matters, + # Order when adding items to the combobox, matters, # default index is 0, so rectified image is added first to be default view self.image_view_type_combobox.addItem( ImageViewMode.SOURCE_RECTIFIED.value, ImageViewMode.SOURCE_RECTIFIED @@ -879,7 +878,6 @@ def save_parameters(self, filename): board_params = self.board_parameters.get_parameters_values() detector_params = self.detector.get_parameters_values() calibrator_type = self.calibrator_type_combobox.currentData() - print(calibrator_type.value["name"], flush=True) calib_params = self.calibrator_dict[calibrator_type].get_parameters_values() with open(filename, "w") as file: yaml.dump({"board_parameters": board_params}, file, default_flow_style=False) @@ -978,19 +976,19 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.single_shot_reprojection_error_rms_label.setText("Reprojection error (rms):") board_speed = None self.image_view.set_draw_indicators( - board_speed, - self.data_collector.max_allowed_pixel_speed.value, - self.data_collector.get_skew_percentage(), - self.data_collector.get_size_percentage(), - 0, - 0, # rows cols linear error - 0, - 0, # rows cols percentage linear error - 0.0, # aspect ratio - 0, - 0, - self.indicators_alpha_spinbox.value(), - False, + board_speed = board_speed, + max_allowed_board_speed = self.data_collector.max_allowed_pixel_speed.value, + skew_percentage = self.data_collector.get_skew_percentage(), + board_size_percentage = self.data_collector.get_size_percentage(), + rows_linear_error = 0.0, + cols_linear_error = 0.0, # rows cols linear error + pct_err_rows = 0.0, + pct_err_cols = 0.0, # rows cols percentage linear error + aspect_ratio = 0.0, # aspect ratio + pan = 0.0, + tilt = 0.0, + alpha_indicators = self.indicators_alpha_spinbox.value(), + value = False, ) else: @@ -1112,24 +1110,24 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im ) board_speed = ( - 0 if self.last_detection is None else detection.get_speed(self.last_detection) + 100 if self.last_detection is None else detection.get_speed(self.last_detection) ) self.last_detection = detection pan, tilt = rough_angles self.image_view.set_draw_indicators( - board_speed, - self.data_collector.max_allowed_pixel_speed.value, - self.data_collector.get_skew_percentage(), - self.data_collector.get_size_percentage(), - err_rms_rows, - err_rms_cols, - pct_err_rows, - pct_err_cols, - detection.get_aspect_ratio_pattern(camera_model), - pan, - tilt, - self.indicators_alpha_spinbox.value(), - self.draw_indicators_checkbox.isChecked(), + board_speed = board_speed, + max_allowed_board_speed = self.data_collector.max_allowed_pixel_speed.value, + skew_percentage = self.data_collector.get_skew_percentage(), + board_size_percentage = self.data_collector.get_size_percentage(), + rows_linear_error = err_rms_rows, + cols_linear_error = err_rms_cols, + pct_err_rows = pct_err_rows, + pct_err_cols = pct_err_cols, + aspect_ratio = detection.get_aspect_ratio_pattern(camera_model), + pan = pan, + tilt = tilt, + alpha_indicators = self.indicators_alpha_spinbox.value(), + value = self.draw_indicators_checkbox.isChecked(), ) # Draw training / evaluation points diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 0d8faf1b..a77a97c8 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -197,6 +197,11 @@ class DataCollector(ParameterizedClass): new_evaluation_sample = Signal() new_sample = Signal() + num_intervals_board_size = 20 + num_intervals_skew_angle = 50 + size_range = np.array([0.08, 0.21]) + skew_range = np.array([0, 1.04]) + def __init__(self, cfg: dict = dict(), **kwargs): # noqa C408 super().__init__(cfg=cfg, **kwargs) @@ -593,54 +598,41 @@ def process_detection_eval_mode( camera_model: Optional[CameraModel] = None, mode: OperationMode = OperationMode.CALIBRATION, ) -> CollectionStatus: - """Evaluate detections mad ein evaluation mode.""" + """Evaluate detections made in evaluation mode.""" # process witout filtering detections self.update_linearity_heatmap(self.linearity_heatmap, detection) - def get_skew_coverage(self): + def get_skew_percentage(self): """Get skew percentage covered from a defined range for the indicators.""" - # Define the number of intervals - num_intervals = 50 - interval_size = 1 / num_intervals + # Define intervals + interval_size = 1 / DataCollector.num_intervals_skew_angle # Create a unique set to store covered intervals covered_intervals = set() - # range in radians ToDo: define the range - skew_range = np.array([0, 1.04]) for detection in self.training_data.get_detections(): - if skew_range[0] <= detection.get_normalized_skew() < skew_range[1]: + if DataCollector.skew_range[0] <= detection.get_normalized_skew() < DataCollector.skew_range[1]: interval_index = int(detection.get_normalized_skew() / interval_size) covered_intervals.add(interval_index) # Calculate the percentage of covered intervals - percentage_coverage = len(covered_intervals) / num_intervals # * 100 + percentage_coverage = len(covered_intervals) / DataCollector.num_intervals_skew_angle # * 100 return percentage_coverage - def get_skew_percentage(self): - """Get skew precentage with more useful name ToDo Refactor.""" - return self.get_skew_coverage() - - def get_size_coverage(self): + def get_size_percentage(self): """Get board size percentage covered from a defined range for the indicators.""" - # Define the number of intervals - num_intervals = 20 - interval_size = 1 / num_intervals + # Define intervals + interval_size = 1 / DataCollector.num_intervals_board_size # Create a set to store covered intervals covered_intervals = set() - # range for board size ToDo: define the range - size_range = np.array([0.08, 0.21]) for detection in self.training_data.get_detections(): - if size_range[0] <= detection.get_normalized_size() < size_range[1]: + if DataCollector.size_range[0] <= detection.get_normalized_size() < DataCollector.size_range[1]: interval_index = int(detection.get_normalized_size() / interval_size) covered_intervals.add(interval_index) # Calculate the percentage of covered intervals - percentage_coverage = len(covered_intervals) / num_intervals # * 100 + percentage_coverage = len(covered_intervals) / DataCollector.num_intervals_board_size # * 100 return percentage_coverage - def get_size_percentage(self): - """Get size precentage with more useful name ToDo Refactor.""" - return self.get_size_coverage() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index 0d6e76cf..0a5d5ba8 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -85,29 +85,30 @@ def __init__(self, calibrator: "CameraIntrinsicsCalibratorUI", cfg): # noqa F82 source_layout.addWidget(self.data_source_combobox) self.source_group.setLayout(source_layout) + # Calibration parameters self.params_combobox = QComboBox() - self.params_combobox.addItem("General", 0) - self.params_combobox.addItem("C1", 1) - self.params_combobox.addItem("C2", 2) - self.params_combobox.addItem("Ceres Calib", 3) - self.params_combobox.addItem("Load File", 4) + + # Add all YAML files from the config directory + for file in os.listdir(config_dir): + if file.endswith('.yaml'):# and file != 'intrinsics_calibrator.yaml': + file_path = os.path.join(config_dir, file) + self.params_combobox.addItem(file.split('.')[0].replace('_', ' ').title(), file_path) + # Add "Load File" option at the end + self.params_combobox.addItem("Load File", None) def on_params_combo_box_changed(index): - if self.params_combobox.currentText() == "Load File": + selected_params_file = self.params_combobox.itemData(index) + if selected_params_file is None: file_name, _ = QFileDialog.getOpenFileName( self, "Open File", "", "All Files (*.*);;Text Files (*.yaml)" ) if file_name: - print(f"Selected file: {file_name}") + logging.info(f"Selected file: {file_name}") config_file_path = file_name - elif self.params_combobox.currentText() == "C1": - config_file_path = os.path.join(config_dir, "c1_intrinsics_calibrator.yaml") - elif self.params_combobox.currentText() == "C2": - config_file_path = os.path.join(config_dir, "c2_intrinsics_calibrator.yaml") - elif self.params_combobox.currentText() == "Ceres Calib": - config_file_path = os.path.join(config_dir, "intrinsics_calibrator_ceres.yaml") - elif self.params_combobox.currentText() == "General": - config_file_path = os.path.join(config_dir, "intrinsics_calibrator.yaml") + else: + config_file_path = selected_params_file + + logging.info(f"Selected config file={config_file_path}") if config_file_path: cfg = {} @@ -116,7 +117,7 @@ def on_params_combo_box_changed(index): cfg = yaml.safe_load(stream) self.cfg = defaultdict(dict, cfg) self.update_board_type() - print("successfully loaded") + logging.info("Successfully opened parameters file") except Exception as e: logging.error(f"Could not load the parameters from the YAML file ({e})") @@ -184,7 +185,6 @@ def load_intrinsics_button_callback(): ) self.initial_intrinsics = load_intrinsics(intrinsics_path) self.evaluation_radio_button.setEnabled(True) - # self.training_radio_button.setChecked(False) self.training_radio_button.setEnabled(False) self.evaluation_radio_button.setChecked(True) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/launch/calibrator.launch.xml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/launch/calibrator.launch.xml index 9844fcc5..0c04fc11 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/launch/calibrator.launch.xml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/launch/calibrator.launch.xml @@ -1,5 +1,5 @@ - + From 185f319511879ad76442fda284a7e7efcc4b526a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 11 Dec 2024 09:03:36 +0000 Subject: [PATCH 23/36] ci(pre-commit): autofix --- .../camera_calibrator.py | 52 +++++++++---------- .../data_collector.py | 21 ++++++-- .../views/initialization_view.py | 6 ++- 3 files changed, 46 insertions(+), 33 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 04f9faad..05f1c4c8 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -976,19 +976,19 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.single_shot_reprojection_error_rms_label.setText("Reprojection error (rms):") board_speed = None self.image_view.set_draw_indicators( - board_speed = board_speed, - max_allowed_board_speed = self.data_collector.max_allowed_pixel_speed.value, - skew_percentage = self.data_collector.get_skew_percentage(), - board_size_percentage = self.data_collector.get_size_percentage(), - rows_linear_error = 0.0, - cols_linear_error = 0.0, # rows cols linear error - pct_err_rows = 0.0, - pct_err_cols = 0.0, # rows cols percentage linear error - aspect_ratio = 0.0, # aspect ratio - pan = 0.0, - tilt = 0.0, - alpha_indicators = self.indicators_alpha_spinbox.value(), - value = False, + board_speed=board_speed, + max_allowed_board_speed=self.data_collector.max_allowed_pixel_speed.value, + skew_percentage=self.data_collector.get_skew_percentage(), + board_size_percentage=self.data_collector.get_size_percentage(), + rows_linear_error=0.0, + cols_linear_error=0.0, # rows cols linear error + pct_err_rows=0.0, + pct_err_cols=0.0, # rows cols percentage linear error + aspect_ratio=0.0, # aspect ratio + pan=0.0, + tilt=0.0, + alpha_indicators=self.indicators_alpha_spinbox.value(), + value=False, ) else: @@ -1115,19 +1115,19 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.last_detection = detection pan, tilt = rough_angles self.image_view.set_draw_indicators( - board_speed = board_speed, - max_allowed_board_speed = self.data_collector.max_allowed_pixel_speed.value, - skew_percentage = self.data_collector.get_skew_percentage(), - board_size_percentage = self.data_collector.get_size_percentage(), - rows_linear_error = err_rms_rows, - cols_linear_error = err_rms_cols, - pct_err_rows = pct_err_rows, - pct_err_cols = pct_err_cols, - aspect_ratio = detection.get_aspect_ratio_pattern(camera_model), - pan = pan, - tilt = tilt, - alpha_indicators = self.indicators_alpha_spinbox.value(), - value = self.draw_indicators_checkbox.isChecked(), + board_speed=board_speed, + max_allowed_board_speed=self.data_collector.max_allowed_pixel_speed.value, + skew_percentage=self.data_collector.get_skew_percentage(), + board_size_percentage=self.data_collector.get_size_percentage(), + rows_linear_error=err_rms_rows, + cols_linear_error=err_rms_cols, + pct_err_rows=pct_err_rows, + pct_err_cols=pct_err_cols, + aspect_ratio=detection.get_aspect_ratio_pattern(camera_model), + pan=pan, + tilt=tilt, + alpha_indicators=self.indicators_alpha_spinbox.value(), + value=self.draw_indicators_checkbox.isChecked(), ) # Draw training / evaluation points diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index a77a97c8..b0e7ff40 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -610,12 +610,18 @@ def get_skew_percentage(self): covered_intervals = set() for detection in self.training_data.get_detections(): - if DataCollector.skew_range[0] <= detection.get_normalized_skew() < DataCollector.skew_range[1]: + if ( + DataCollector.skew_range[0] + <= detection.get_normalized_skew() + < DataCollector.skew_range[1] + ): interval_index = int(detection.get_normalized_skew() / interval_size) covered_intervals.add(interval_index) # Calculate the percentage of covered intervals - percentage_coverage = len(covered_intervals) / DataCollector.num_intervals_skew_angle # * 100 + percentage_coverage = ( + len(covered_intervals) / DataCollector.num_intervals_skew_angle + ) # * 100 return percentage_coverage @@ -627,12 +633,17 @@ def get_size_percentage(self): covered_intervals = set() for detection in self.training_data.get_detections(): - if DataCollector.size_range[0] <= detection.get_normalized_size() < DataCollector.size_range[1]: + if ( + DataCollector.size_range[0] + <= detection.get_normalized_size() + < DataCollector.size_range[1] + ): interval_index = int(detection.get_normalized_size() / interval_size) covered_intervals.add(interval_index) # Calculate the percentage of covered intervals - percentage_coverage = len(covered_intervals) / DataCollector.num_intervals_board_size # * 100 + percentage_coverage = ( + len(covered_intervals) / DataCollector.num_intervals_board_size + ) # * 100 return percentage_coverage - diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index 0a5d5ba8..03919aec 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -90,9 +90,11 @@ def __init__(self, calibrator: "CameraIntrinsicsCalibratorUI", cfg): # noqa F82 # Add all YAML files from the config directory for file in os.listdir(config_dir): - if file.endswith('.yaml'):# and file != 'intrinsics_calibrator.yaml': + if file.endswith(".yaml"): # and file != 'intrinsics_calibrator.yaml': file_path = os.path.join(config_dir, file) - self.params_combobox.addItem(file.split('.')[0].replace('_', ' ').title(), file_path) + self.params_combobox.addItem( + file.split(".")[0].replace("_", " ").title(), file_path + ) # Add "Load File" option at the end self.params_combobox.addItem("Load File", None) From d8ab908d92694139b2afdb289ca495b0485e6682 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Fri, 13 Dec 2024 13:48:41 +0900 Subject: [PATCH 24/36] refactor: all comments form the discussion were addressed here --- .../camera_calibrator.py | 28 ++++++++++--------- .../views/image_view.py | 4 +-- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 05f1c4c8..35a4adcd 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -396,7 +396,7 @@ def make_detection_group(self): self.raw_detection_label = QLabel("Detected:") self.raw_linear_error_rows_rms_label = QLabel("Linear error rows (rms):") self.raw_linear_error_cols_rms_label = QLabel("Linear error cols (rms):") - self.aspect_ratio_label = QLabel("Aspect Ratio:") + self.aspect_ratio_label = QLabel("Aspect ratio:") self.rough_tilt_label = QLabel("Rough tilt:") self.rough_angles_label = QLabel("Rough angles:") self.rough_position_label = QLabel("Rough position:") @@ -718,7 +718,7 @@ def start( ) if self.operation_mode == OperationMode.EVALUATION: self.setWindowTitle( - f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()})" + f"Camera intrinsics evaluation mode ({self.data_source.get_camera_name()})" ) logging.info("Init") @@ -880,14 +880,16 @@ def save_parameters(self, filename): calibrator_type = self.calibrator_type_combobox.currentData() calib_params = self.calibrator_dict[calibrator_type].get_parameters_values() with open(filename, "w") as file: - yaml.dump({"board_parameters": board_params}, file, default_flow_style=False) - yaml.dump({"board_type": self.board_type.value["name"]}, file, default_flow_style=False) - yaml.dump( - {"calibrator_type": calibrator_type.value["name"]}, file, default_flow_style=False - ) - yaml.dump({"calibration_parameters": calib_params}, file, default_flow_style=False) - yaml.dump({"data_collector": data_coll_params}, file, default_flow_style=False) - yaml.dump({"detector_params": detector_params}, file, default_flow_style=False) + all_params = { + "board_parameters": board_params, + "board_type" : self.board_type.value["name"], + "calibrator_type": calibrator_type.value["name"], + "calibration_parameters": calib_params, + "data_collector": data_coll_params, + "detector_params": detector_params + } + + yaml.dump(all_params, file, default_flow_style=False) def on_save_clicked(self): output_folder = QFileDialog.getExistingDirectory( @@ -964,7 +966,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.raw_detection_label.setText("Detected: False") self.raw_linear_error_rows_rms_label.setText("Linear error rows rms:") self.raw_linear_error_cols_rms_label.setText("Linear error cols rms:") - self.aspect_ratio_label.setText("Aspect Ratio:") + self.aspect_ratio_label.setText("Aspect ratio:") self.rough_tilt_label.setText("Rough tilt:") self.rough_angles_label.setText("Rough angles:") self.rough_position_label.setText("Rough position:") @@ -1075,7 +1077,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Linear error cols rms: {err_rms_cols:.2f} px" # noqa E231 ) self.aspect_ratio_label.setText( - f"Aspect Ratio: {detection.get_aspect_ratio_pattern(camera_model):.2f} px" # noqa E231 + f"Aspect ratio: {detection.get_aspect_ratio_pattern(camera_model):.2f} px" # noqa E231 ) self.rough_tilt_label.setText( f"Rough tilt: {detection.get_tilt(camera_model):.2f} degrees" # noqa E231 @@ -1197,7 +1199,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im ) if self.operation_mode == OperationMode.EVALUATION: self.setWindowTitle( - f"Camera intrinsics Evaluation Mode ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" + f"Camera intrinsics evaluation mode ({self.data_source.get_camera_name()}). Data delay={detection_delay: .2f} Detection time={detection_time: .2f} fps={self.estimated_fps: .2f} Data time={img_stamp: .2f}" ) self.image_view.update() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index d1bdcdb1..869a2fb1 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -270,7 +270,7 @@ def draw_indicators(self, painter: QPainter, display_size): ) painter.drawText(position_text_speed, "Speed") - # ToDo: define percentage to change skew and pct size to change to green + # TODO(Sergio): define percentage to change skew and pct size to change to green threshold_to_be_green = 0.3 if self.skew_percentage < threshold_to_be_green: pen_skew = QPen(color_red) @@ -297,7 +297,7 @@ def draw_indicators(self, painter: QPainter, display_size): # Draw the skew progress bar painter.drawRect(skew_indicator) - # ToDo: define percentage to change skew and pct size to change to green + # TODO(Sergio): define percentage to change skew and pct size to change to green threshold_to_be_green = 0.2 if self.board_size_percentage < threshold_to_be_green: pen_size_board = QPen(color_red) From d1347cbae541007d14ef39f9ebcde4c03ca6d41f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 13 Dec 2024 04:51:52 +0000 Subject: [PATCH 25/36] ci(pre-commit): autofix --- .../intrinsic_camera_calibrator/camera_calibrator.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index 35a4adcd..b9c70b63 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -882,11 +882,11 @@ def save_parameters(self, filename): with open(filename, "w") as file: all_params = { "board_parameters": board_params, - "board_type" : self.board_type.value["name"], + "board_type": self.board_type.value["name"], "calibrator_type": calibrator_type.value["name"], "calibration_parameters": calib_params, "data_collector": data_coll_params, - "detector_params": detector_params + "detector_params": detector_params, } yaml.dump(all_params, file, default_flow_style=False) From ff5e7c0e82688ed919592647726590f299c8a955 Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:04:01 +0900 Subject: [PATCH 26/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py accepting the suggestion Co-authored-by: Kenzo Lobos Tsunekawa --- .../board_detections/board_detection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index 3aac6605..9e2ec365 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -102,7 +102,7 @@ def get_linear_error_rms(self) -> float: raise NotImplementedError def restart_linearity_heatmap(self): - """Restart linearity heatmap.""" + """Restart the linearity heatmap.""" raise NotImplementedError def get_center_2d(self) -> np.array: From f1eadd7e2133db37a9bcf62c366d58535db79493 Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:04:35 +0900 Subject: [PATCH 27/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py accepting the suggestion Co-authored-by: Kenzo Lobos Tsunekawa --- .../board_detections/board_detection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index 9e2ec365..2a9ecd43 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -240,7 +240,7 @@ def get_speed(self, last: "BoardDetection") -> float: 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.""" + """Get the aspect ratio using the calibration pattern, which should be squared.""" tilt, pan = self.get_rotation_angles(model) acceptance_angle = 10 From f1d39360873ec340e54f7cd618ce6bb264383a32 Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:05:20 +0900 Subject: [PATCH 28/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py accepting the suggestion of changing the grammar Co-authored-by: Kenzo Lobos Tsunekawa --- .../board_detections/board_detection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py index 2a9ecd43..e18856b6 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detections/board_detection.py @@ -244,7 +244,7 @@ def get_aspect_ratio_pattern(self, model: CameraModel) -> float: tilt, pan = self.get_rotation_angles(model) acceptance_angle = 10 - # Dont update if the board has big angles, calculation will not be accurate + # Do not update if the board has large out-of-plane rotations since calculations will not be accurate if np.abs(tilt) > acceptance_angle or np.abs(pan) > acceptance_angle: return 0.0 # Calculate distances between adjacent corners From 066acd1928e82828e2aa8f024bace379565658c8 Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:05:33 +0900 Subject: [PATCH 29/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py accepting the suggestion of changing the grammar Co-authored-by: Kenzo Lobos Tsunekawa --- .../intrinsic_camera_calibrator/data_collector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index b0e7ff40..2d73a2e7 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -446,7 +446,7 @@ def squared_error(p, p1, p2): p = image_points[i][j] dist_error = np.sqrt(squared_error(p, p1, p2)) if dist_error / points_dist > max_pct_error_tolerance: - # if distance is too big most likely is a miss detection + # if the distance is too large, it is most likely a miss detection dist_error = 0 x = int(heatmap.shape[1] * p[0] / detection.width) y = int(heatmap.shape[0] * p[1] / detection.height) From 9d33e688367c7959bf8583a74aeeb3d074204895 Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:05:47 +0900 Subject: [PATCH 30/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py accepting the suggestion of changing the grammar Co-authored-by: Kenzo Lobos Tsunekawa --- .../intrinsic_camera_calibrator/data_collector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 2d73a2e7..46ca8bad 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -454,7 +454,7 @@ def squared_error(p, p1, p2): heatmap[y, x] = 1 * dist_error def restart_linearity_heatmap(self): - """Restart heatmap created by aspect ratio.""" + """Restart the heatmap created by aspect ratio.""" self.linearity_heatmap = np.zeros((self.heatmap_cells.value, self.heatmap_cells.value)) def evaluate_redundancy( From ba0e889f7fb6a2d24d413ac81b2e92ecea7921b5 Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:06:18 +0900 Subject: [PATCH 31/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py accepting the suggestion of changing the grammar Co-authored-by: Kenzo Lobos Tsunekawa --- .../intrinsic_camera_calibrator/views/image_view.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 869a2fb1..7485c583 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -258,7 +258,7 @@ def draw_indicators(self, painter: QPainter, display_size): speed_indicator = QRectF( QPointF(0, 0), QSize(display_size.width(), int(display_size.height() * 0.04)) ) - # Draw the rectangle for speed indication + # Draw the rectangle for the speed indicator painter.drawRect(speed_indicator) # Set the font according to the window size From 2bda2c692321f486f8d17d3726b78d6b6211adbe Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:06:49 +0900 Subject: [PATCH 32/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py accepting the suggestion Co-authored-by: Kenzo Lobos Tsunekawa --- .../intrinsic_camera_calibrator/views/image_view.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 7485c583..4e7cd78d 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -281,7 +281,7 @@ def draw_indicators(self, painter: QPainter, display_size): painter.setPen(pen_skew) painter.setBrush(brush_skew) - # Draw Skew text + # Draw skew text position_text_skew = QPointF( int(display_size.width() * 0.01), int(display_size.height() * 0.88) ) From 06c98295fb384cd03f99d7729e183d79e72b4b56 Mon Sep 17 00:00:00 2001 From: SergioReyesSan Date: Wed, 25 Dec 2024 09:27:27 +0900 Subject: [PATCH 33/36] Update calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py accepting suggestion Co-authored-by: Kenzo Lobos Tsunekawa --- .../intrinsic_camera_calibrator/data_collector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 46ca8bad..856e9641 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -431,7 +431,7 @@ def squared_error(p, p1, p2): p = image_points[j][i] dist_error = np.sqrt(squared_error(p, p1, p2)) if dist_error / points_dist > max_pct_error_tolerance: - # if distance is too big most likely is a miss detection + # if the distance is too large it is most likely a miss detection dist_error = 0 x = int(heatmap.shape[1] * p[0] / detection.width) y = int(heatmap.shape[0] * p[1] / detection.height) From f89b3ff4cb6f41ae9f344962700594a3b0d2fee4 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Wed, 25 Dec 2024 09:50:20 +0900 Subject: [PATCH 34/36] fix: correcting misspelled --- .../intrinsic_camera_calibrator/data_collector.py | 2 +- .../intrinsic_camera_calibrator/views/image_view.py | 2 +- .../intrinsic_camera_calibrator/views/initialization_view.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py index 856e9641..8f4f41ad 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_collector.py @@ -599,7 +599,7 @@ def process_detection_eval_mode( mode: OperationMode = OperationMode.CALIBRATION, ) -> CollectionStatus: """Evaluate detections made in evaluation mode.""" - # process witout filtering detections + # process without filtering detections self.update_linearity_heatmap(self.linearity_heatmap, detection) def get_skew_percentage(self): diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 4e7cd78d..3d721b16 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -270,7 +270,7 @@ def draw_indicators(self, painter: QPainter, display_size): ) painter.drawText(position_text_speed, "Speed") - # TODO(Sergio): define percentage to change skew and pct size to change to green + # TODO(SergioReyesSan): define percentage to change skew and pct size to change to green threshold_to_be_green = 0.3 if self.skew_percentage < threshold_to_be_green: pen_skew = QPen(color_red) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py index 03919aec..d4ce47a6 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/initialization_view.py @@ -125,7 +125,7 @@ def on_params_combo_box_changed(index): self.params_combobox.currentIndexChanged.connect(on_params_combo_box_changed) - self.params_group = QGroupBox("Parameters Profile") + self.params_group = QGroupBox("Parameters profile") self.params_group.setFlat(True) params_layout = QVBoxLayout() params_layout.addWidget(self.params_combobox) From ec063e916576efd4d43021693ba83ffc22ae1910 Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Thu, 26 Dec 2024 18:29:42 +0900 Subject: [PATCH 35/36] refactor: fixing some spelling errors --- .../board_detectors/chessboard_detector.py | 2 +- .../intrinsic_camera_calibrator/camera_calibrator.py | 2 +- .../intrinsic_camera_calibrator/views/image_view.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py index 20e4beb5..cb10f632 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py @@ -33,7 +33,7 @@ def __init__(self, **kwargs): 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) - super().set_parameters(**kwargs["cfg"]) + self.set_parameters(**kwargs["cfg"]) def detect(self, img: np.array, stamp: float): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index b9c70b63..e19693f9 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -1112,7 +1112,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im ) board_speed = ( - 100 if self.last_detection is None else detection.get_speed(self.last_detection) + 100.0 if self.last_detection is None else detection.get_speed(self.last_detection) ) self.last_detection = detection pan, tilt = rough_angles diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 3d721b16..e9f7a6b4 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -246,7 +246,7 @@ def draw_indicators(self, painter: QPainter, display_size): """Draw indicators for speed, skew, size aspect ratio, angles of the detected board.""" color_green = QColor(0.0, 255, 0.0, int(255 * self.alpha_indicators)) color_red = QColor(255, 0.0, 0.0, int(255 * self.alpha_indicators)) - # Change color according tot he speed + # Change color according to the board speed if self.current_board_speed < self.max_board_allowed_speed: pen = QPen(color_green) brush = QBrush(color_green) From 4b83dbd2e83b21cdadb7118847c5b41c0edf98cf Mon Sep 17 00:00:00 2001 From: Sergio Reyes Sanchez Date: Fri, 27 Dec 2024 11:10:14 +0900 Subject: [PATCH 36/36] refactor: change parameters file for c2 camera --- .../config/c2_intrinsics_calibrator.yaml | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml index 3d227c50..e8bb8fb6 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/c2_intrinsics_calibrator.yaml @@ -9,7 +9,7 @@ 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 @@ -17,7 +17,7 @@ calibration_parameters: 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 @@ -27,13 +27,15 @@ calibration_parameters: radial_distortion_coefficients: 3 rational_distortion_coefficients: 3 use_tangential_distortion: true + pre_calibration_num_samples: 40 + regularization_weight: 0.2 enable_prism_model: false fix_principal_point: false fix_aspect_ratio: false use_lu_decomposition: false use_qr_decomposition: false -calibrator_type: opencv +calibrator_type: ceres data_collector: max_samples: 500 @@ -43,13 +45,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