diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py index 030ed9e9..935c5e2b 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/apriltag_grid_detector.py @@ -85,10 +85,10 @@ def __init__(self, **kwargs): self.current_decode_sharpening = None self.current_debug = None - def detect(self, img): + def detect(self, img, stamp): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" if img is None: - self.detection_results_signal.emit(None, None) + self.detection_results_signal.emit(None, None, None) return with self.lock: @@ -148,7 +148,7 @@ def detect(self, img): tags.sort(key=lambda tag: tag.tag_id) if len(tags) < rows * cols * min_detection_ratio: - self.detection_results_signal.emit(img, None) + self.detection_results_signal.emit(img, None, stamp) return detection = ApriltagGridDetection( @@ -162,4 +162,4 @@ def detect(self, img): tags=tags, ) - self.detection_results_signal.emit(img, detection) + self.detection_results_signal.emit(img, detection, stamp) diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py index e2a6dd6f..357d1085 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/board_detector.py @@ -24,12 +24,13 @@ from PySide2.QtCore import Signal from intrinsic_camera_calibrator.board_parameters.board_parameters import BoardParameters from intrinsic_camera_calibrator.parameter import ParameterizedClass +import numpy as np class BoardDetector(ParameterizedClass, QObject): """Base class of board detectors.""" - detection_results_signal = Signal(object, object) + detection_results_signal = Signal(object, object, float) def __init__( self, lock: threading.RLock, board_parameters: BoardParameters, cfg: Optional[Dict] = {} @@ -40,7 +41,7 @@ def __init__( self.set_parameters(**cfg) - def detect(self, img): + def detect(self, img: np.array, stamp): """Slot to detect boards from an image. Subclasses must implement this method.""" raise NotImplementedError diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py index 0e8457c9..efccfe90 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/chessboard_detector.py @@ -29,16 +29,18 @@ def __init__(self, **kwargs): self.adaptive_thresh = Parameter(bool, value=True, min_value=False, max_value=True) self.normalize_image = Parameter(bool, value=True, min_value=False, max_value=True) self.fast_check = Parameter(bool, value=True, min_value=False, max_value=True) - self.refine = Parameter(bool, value=True, min_value=False, max_value=True) - def detect(self, img): + self.resized_detection = Parameter(bool, value=True, min_value=False, max_value=True) + self.resized_max_resolution = Parameter(int, value=1000, min_value=500, max_value=3000) + self.sub_pixel_refinement = Parameter(bool, value=True, min_value=False, max_value=True) + + def detect(self, img: np.array, stamp: float): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" if img is None: self.detection_results_signal.emit(None, None) return with self.lock: - h, w = img.shape[0:2] (cols, rows) = (self.board_parameters.cols.value, self.board_parameters.rows.value) cell_size = self.board_parameters.cell_size.value @@ -46,17 +48,66 @@ def detect(self, img): flags |= cv2.CALIB_CB_ADAPTIVE_THRESH if self.adaptive_thresh.value else 0 flags |= cv2.CALIB_CB_NORMALIZE_IMAGE if self.normalize_image.value else 0 flags |= cv2.CALIB_CB_FAST_CHECK if self.fast_check.value else 0 - refine = self.refine.value + sub_pixel_refinement = self.sub_pixel_refinement.value + + resized_detection = self.resized_detection.value + resized_max_resolution = self.resized_max_resolution.value + h, w = img.shape[0:2] grayscale = to_grayscale(img) - (ok, corners) = cv2.findChessboardCorners(grayscale, (cols, rows), flags=flags) + if not resized_detection or max(h, w) <= resized_max_resolution: + (ok, corners) = cv2.findChessboardCorners(grayscale, (cols, rows), flags=flags) - if not ok: - self.detection_results_signal.emit(img, None) - return + if not ok: + self.detection_results_signal.emit(img, None, stamp) + return + else: + # Find the resized dimensions + ratio = float(w) / float(h) + + if w > h: + resized_w = int(resized_max_resolution) + resized_h = int(resized_max_resolution / ratio) + else: + resized_w = int(resized_max_resolution * ratio) + resized_h = int(resized_max_resolution) + + # Resize + resized = cv2.resize(img, (resized_w, resized_h), interpolation=cv2.INTER_NEAREST) + + # Run the detector on the resized image + (ok, resized_corners) = cv2.findChessboardCorners(resized, (cols, rows), flags=flags) + + if not ok: + self.detection_results_signal.emit(img, None, stamp) + return + + # Re escalate the corners + corners = resized_corners * np.array( + [float(w) / resized_w, float(h) / resized_h], dtype=np.float32 + ) + + # Estimate the ROI in the original image + roi_min_j = int(max(0, corners[:, 0, 1].min() - 10)) + roi_min_i = int(max(0, corners[:, 0, 0].min() - 10)) + roi_max_j = int(min(w, corners[:, 0, 1].max() + 10)) + roi_max_i = int(min(w, corners[:, 0, 0].max() + 10)) + + # Extract the ROI of the original image + roi = grayscale[roi_min_j:roi_max_j, roi_min_i:roi_max_i] + + # Run the detector again + (ok, roi_corners) = cv2.findChessboardCorners(roi, (cols, rows), flags=flags) + + if not ok: + self.detection_results_signal.emit(img, None, stamp) + return + + # Re escalate the coordinates + corners = roi_corners + np.array([roi_min_i, roi_min_j], dtype=np.float32) - if ok and refine: + if sub_pixel_refinement: dist_matrix = np.linalg.norm( corners.reshape(-1, 1, 2) - corners.reshape(1, -1, 2), axis=-1 ) @@ -81,4 +132,4 @@ def detect(self, img): image_points=image_points, ) - self.detection_results_signal.emit(img, detection) + self.detection_results_signal.emit(img, detection, stamp) diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py index 3f6145b3..1acbcc85 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/board_detectors/dotboard_detector.py @@ -39,7 +39,10 @@ def __init__(self, **kwargs): float, value=1.0, min_value=0.1, max_value=10.0 ) - def detect(self, img: np.array): + self.resized_detection = Parameter(bool, value=True, min_value=False, max_value=True) + self.resized_max_resolution = Parameter(int, value=2000, min_value=500, max_value=5000) + + def detect(self, img: np.array, stamp: float): """Slot to detect boards from an image. Results are sent through the detection_results signals.""" if img is None: self.detection_results_signal.emit(None, None) @@ -50,16 +53,10 @@ def detect(self, img: np.array): (cols, rows) = (self.board_parameters.cols.value, self.board_parameters.rows.value) cell_size = self.board_parameters.cell_size.value - # Setting blob detector - params = cv2.SimpleBlobDetector_Params() - params.filterByArea = self.filter_by_area.value - params.minArea = self.min_area_percentage.value * h * w / 100.0 - params.maxArea = self.max_area_percentage.value * h * w / 100.0 - params.minDistBetweenBlobs = ( - self.min_dist_between_blobs_percentage.value * max(h, w) / 100.0 - ) - - detector = cv2.SimpleBlobDetector_create(params) + filter_by_area = self.filter_by_area.value + min_area_percentage = self.min_area_percentage.value + max_area_percentage = self.max_area_percentage.value + min_dist_between_blobs_percentage = self.min_dist_between_blobs_percentage.value flags = 0 flags |= cv2.CALIB_CB_CLUSTERING if self.clustering.value else 0 @@ -69,26 +66,99 @@ def detect(self, img: np.array): else cv2.CALIB_CB_ASYMMETRIC_GRID ) - grayscale = to_grayscale(img) - - (ok, corners) = cv2.findCirclesGrid( - grayscale, (cols, rows), flags=flags, blobDetector=detector + resized_detection = self.resized_detection.value + resized_max_resolution = self.resized_max_resolution.value + + # Find the resized dimensions + ratio = float(w) / float(h) + + if w > h: + resized_w = int(resized_max_resolution) + resized_h = int(resized_max_resolution / ratio) + else: + resized_w = int(resized_max_resolution * ratio) + resized_h = int(resized_max_resolution) + + # Setting blob detector + full_res_params = cv2.SimpleBlobDetector_Params() + full_res_params.filterByArea = filter_by_area + full_res_params.minArea = min_area_percentage * h * w / 100.0 + full_res_params.maxArea = max_area_percentage * h * w / 100.0 + full_res_params.minDistBetweenBlobs = min_dist_between_blobs_percentage * max(h, w) / 100.0 + + resized_params = cv2.SimpleBlobDetector_Params() + resized_params.filterByArea = filter_by_area + resized_params.minArea = min_area_percentage * resized_h * resized_w / 100.0 + resized_params.maxArea = max_area_percentage * resized_h * resized_w / 100.0 + resized_params.minDistBetweenBlobs = ( + min_dist_between_blobs_percentage * max(resized_h, resized_w) / 100.0 ) - if not ok: + full_res_detector = cv2.SimpleBlobDetector_create(full_res_params) + resized_detector = cv2.SimpleBlobDetector_create(resized_params) + + grayscale = to_grayscale(img) + + def detect(detection_image, detector): (ok, corners) = cv2.findCirclesGrid( - grayscale, (cols, rows), flags=flags, blobDetector=detector + detection_image, (cols, rows), flags=flags, blobDetector=detector ) - # we need to swap the axes of the detections back to make it consistent - if ok: - corners_2d_array = corners.reshape((cols, rows, 2)) - corners_transposed = np.transpose(corners_2d_array, (1, 0, 2)) - corners = corners_transposed.reshape(-1, 1, 2) + if not ok: + (ok, corners) = cv2.findCirclesGrid( + detection_image, (cols, rows), flags=flags, blobDetector=detector + ) - if not ok: - self.detection_results_signal.emit(img, None) - return + # we need to swap the axes of the detections back to make it consistent + if ok: + corners_2d_array = corners.reshape((cols, rows, 2)) + corners_transposed = np.transpose(corners_2d_array, (1, 0, 2)) + corners = corners_transposed.reshape(-1, 1, 2) + + return (ok, corners) + + if not resized_detection or max(h, w) <= resized_max_resolution: + (ok, corners) = detect(grayscale, full_res_detector) + + else: + # Resize + resized = cv2.resize(img, (resized_w, resized_h), interpolation=cv2.INTER_NEAREST) + + # Run the detector on the resized image + (ok, resized_corners) = detect(resized, resized_detector) + + if not ok: + self.detection_results_signal.emit(img, None, stamp) + return + + # Re escalate the corners + corners = resized_corners * np.array( + [float(w) / resized_w, float(h) / resized_h], dtype=np.float32 + ) + + # Estimate the ROI in the original image + border = max( + corners[:, 0, 0].max() - corners[:, 0, 0].min(), + corners[:, 0, 1].max() - corners[:, 0, 1].min(), + ) / min(cols, rows) + + roi_min_j = int(max(0, corners[:, 0, 1].min() - border)) + roi_min_i = int(max(0, corners[:, 0, 0].min() - border)) + roi_max_j = int(min(w, corners[:, 0, 1].max() + border)) + roi_max_i = int(min(w, corners[:, 0, 0].max() + border)) + + # Extract the ROI of the original image + roi = grayscale[roi_min_j:roi_max_j, roi_min_i:roi_max_i] + + # Run the detector again + (ok, roi_corners) = detect(roi, full_res_detector) + + if not ok: + self.detection_results_signal.emit(img, None, stamp) + return + + # Re escalate the coordinates + corners = roi_corners + np.array([roi_min_i, roi_min_j], dtype=np.float32) # reverse the corners if needed if np.linalg.norm(corners[0]) > np.linalg.norm(corners[1]): @@ -108,4 +178,4 @@ def detect(self, img: np.array): image_points=image_points, ) - self.detection_results_signal.emit(img, detection) + self.detection_results_signal.emit(img, detection, stamp) diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index baca8b04..d24bf0a7 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py @@ -23,6 +23,7 @@ import signal import sys import threading +import time from typing import Dict from PySide2.QtCore import QThread @@ -75,7 +76,7 @@ class CameraIntrinsicsCalibratorUI(QMainWindow): produced_data_signal = Signal() consumed_data_signal = Signal() should_process_image = Signal() - request_image_detection = Signal(object) + request_image_detection = Signal(object, float) def __init__(self, cfg): super().__init__() @@ -85,6 +86,8 @@ def __init__(self, cfg): # Threading variables self.lock = threading.RLock() + self.produced_image = None + self.produced_stamp = None self.unprocessed_image = None self.pending_detection_request = False self.pending_detection_result = False @@ -96,6 +99,8 @@ def __init__(self, cfg): self.calibration_thread.start() # Calibration results + self.estimated_fps = 0 + self.last_processed_stamp = None # Camera models to use normally self.current_camera_model: CameraModel = None @@ -816,7 +821,7 @@ def on_save_clicked(self): 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 - def process_detection_results(self, img: np.array, detection: BoardDetection): + def process_detection_results(self, img: np.array, detection: BoardDetection, img_stamp: float): """Process the results from an object detection.""" # Signal that the detector is free self.consumed_data_signal.emit() @@ -988,6 +993,20 @@ def process_detection_results(self, img: np.array, detection: BoardDetection): self.image_view.set_image(img) # Request a redrawing + current_time = time.time() + detection_delay = time.time() - img_stamp + current_fps = ( + 0 + if self.last_processed_stamp is None + else 1.0 / max(1e-5, (current_time - self.last_processed_stamp)) + ) + 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.image_view.update() self.graphics_view.update() @@ -1015,6 +1034,8 @@ def process_partial_calibration_result(self, camera_model): def process_data(self): """Request the detector to process the image (the detector itself runs in another thread). Depending on the ImageViewMode selected, the image is also rectified.""" + stamp = self.unprocessed_stamp + if self.image_view_type_combobox.currentData() in { ImageViewMode.SOURCE_UNRECTIFIED, ImageViewMode.TRAINING_DB_UNRECTIFIED, @@ -1031,8 +1052,9 @@ def process_data(self): self.pending_detection_request = False self.pending_detection_result = True + self.detection_request_time = time.time() - self.request_image_detection.emit(img) + self.request_image_detection.emit(img, stamp) def process_db_data(self, img): assert self.image_view_type_combobox.currentData() in set( @@ -1060,26 +1082,31 @@ def process_new_data(self): with self.lock: if self.produced_image is not None: self.unprocessed_image = self.produced_image + self.unprocessed_stamp = self.produced_stamp else: self.unprocessed_image = self.produced_image + self.unprocessed_stamp = self.produced_stamp self.produced_image = None + self.produced_stamp = None if self.pending_detection_result: self.pending_detection_request = True else: self.should_process_image.emit() - def data_source_external_callback(self, img: np.array): + def data_source_external_callback(self, img: np.array, stamp: float): """ Consumer side of the producer/consumer pattern. The producer generally has its own thread so synchronization it is needed Args: img (np.array): the produced image coming from any data source. + stamp (float): the produced image's timestamp as a float. """ # We decouple the the data coming from the source with the processing slot to avoid dropping frames in case we can not process them all with self.lock: self.produced_image = img + self.produced_stamp = stamp self.produced_data_signal.emit() # Using a signal from another thread results in the slot being executed in the class Qt thread def on_parameter_changed(self): diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py index a72b3a14..75cce3e5 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/image_files_data_source.py @@ -15,6 +15,7 @@ # limitations under the License. import logging +import time from PySide2.QtCore import QObject from PySide2.QtCore import QThread @@ -75,4 +76,5 @@ def on_consumed(self): def send_data(self, image_path): """Send a image message to the consumer prior transformation to a numpy array.""" image = cv2.imread(image_path) - self.data_callback(image) + stamp = time.time() + self.data_callback(image, stamp) diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py index 42e58b75..74adfe6b 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_bag_data_source.py @@ -137,9 +137,11 @@ def send_data(self, topic, data): # cSpell:ignore imgmsg if isinstance(msg, Image): image_data = self.bridge.imgmsg_to_cv2(msg) - self.data_callback(image_data) + stamp = msg.header.stamp.sec + 1e-9 * msg.header.stamp.nanosec + self.data_callback(image_data, stamp) elif isinstance(msg, CompressedImage): image_data = np.frombuffer(msg.data, np.uint8) image_data = cv2.imdecode(image_data, cv2.IMREAD_COLOR) - self.data_callback(image_data) + stamp = msg.header.stamp.sec + 1e-9 * msg.header.stamp.nanosec + self.data_callback(image_data, stamp) diff --git a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py index 784cbeda..8c600aa3 100644 --- a/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py +++ b/sensor/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/data_sources/ros_topic_data_source.py @@ -116,7 +116,8 @@ def compressed_image_callback(self, msg: CompressedImage): with self.lock: image_data = np.frombuffer(msg.data, np.uint8) image_data = cv2.imdecode(image_data, cv2.IMREAD_COLOR) - self.data_callback(image_data) + stamp = msg.header.stamp.sec + 1e-9 * msg.header.stamp.nanosec + self.data_callback(image_data, stamp) def image_callback(self, msg: Image): """Process a raw image.""" @@ -125,7 +126,8 @@ def image_callback(self, msg: Image): # cSpell:ignore imgmsg with self.lock: image_data = self.bridge.imgmsg_to_cv2(msg, "bgr8") - self.data_callback(image_data) + stamp = msg.header.stamp.sec + 1e-9 * msg.header.stamp.nanosec + self.data_callback(image_data, stamp) def spin(self): """Start a new thread for ROS to spin in."""