From 1699ca9ccfb8adc40f82ee4fe22d528bd1b2155d Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 6 Nov 2024 17:20:54 +0900 Subject: [PATCH 01/38] feat(intrinsic_camera_calibrator): prototype for multiple camera models & ceres integration Signed-off-by: amadeuszsz --- .../board_detections/board_detection.py | 42 ++--- .../calibrators/calibrator.py | 12 +- .../calibrators/calibrator_factory.py | 7 +- .../calibrators/ceres_calibrator.py | 33 +++- .../calibrators/opencv_calibrator.py | 40 ++-- .../calibrators/utils.py | 34 ++-- .../camera_calibrator.py | 101 +++++++--- .../{ => camera_models}/camera_model.py | 130 +++++++------ .../camera_models/camera_model_factory.py | 45 +++++ .../camera_models/ceres_camera_model.py | 172 ++++++++++++++++++ .../camera_models/opencv_camera_model.py | 147 +++++++++++++++ .../data_collector.py | 6 +- .../intrinsic_camera_calibrator/utils.py | 2 +- .../views/data_collector_view.py | 2 +- .../views/initialization_view.py | 2 +- .../intrinsic_camera_calibrator/package.xml | 1 + 16 files changed, 609 insertions(+), 167 deletions(-) rename calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/{ => camera_models}/camera_model.py (70%) create mode 100644 calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py create mode 100644 calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py create mode 100644 calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py 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..075a98b4 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 @@ -20,7 +20,7 @@ from typing import Tuple import cv2 -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel import numpy as np @@ -54,9 +54,8 @@ def __init__( self._cached_pose = None self._cached_flattened_3d_points = None - def _precompute_single_shot_model(self): + def _precompute_single_shot_model(self, model: CameraModel): """Compute and caches a camera model calibrated with the current detection.""" - model = CameraModel() model.calibrate( height=self.height, width=self.width, @@ -65,10 +64,10 @@ def _precompute_single_shot_model(self): ) self._cached_camera_model = model - def _get_cached_model(self) -> CameraModel: + def _get_cached_model(self, model: CameraModel) -> CameraModel: """Return the single shot camera model and computes it is has not been pre-computed yet.""" if self._cached_camera_model is None: - self._precompute_single_shot_model() + self._precompute_single_shot_model(model) return self._cached_camera_model @@ -108,14 +107,13 @@ def get_center_2d(self) -> np.array: self._cached_center_2d = self.get_flattened_image_points().mean(axis=0) return self._cached_center_2d - def get_reprojection_errors(self, model: Optional[CameraModel] = None) -> np.array: + def get_reprojection_errors(self, model: CameraModel) -> np.array: """Return the error of projecting the object points into the image and comparing them with the detections.""" - if model is None: - model = self._get_cached_model() + if self._cached_camera_model is None: + self._precompute_single_shot_model(model) if ( - self._cached_camera_model is not None - and model == self._cached_camera_model + model == self._cached_camera_model and self._cached_reprojection_errors is not None ): return self._cached_reprojection_errors @@ -125,16 +123,13 @@ def get_reprojection_errors(self, model: Optional[CameraModel] = None) -> np.arr return self._cached_reprojection_errors - def get_tilt(self, model: Optional[CameraModel] = None) -> float: + def get_tilt(self, model: CameraModel) -> float: """Return the angle difference between the detection and the camera. Specifically, the pose of the detection points considers +z pointing towards the camera and the camera itself uses +z pointing towards the scene.""" - if model is None: - model = self._get_cached_model() - if model == self._cached_camera_model and self._cached_tilt is not None: return self._cached_tilt # cSpell:enableCompoundWords - rvec, _ = self.get_pose() + rvec, _ = self.get_pose(model) rotmat, _ = cv2.Rodrigues(rvec) rotmat[:2, :] *= -1 @@ -145,15 +140,12 @@ def get_tilt(self, model: Optional[CameraModel] = None) -> float: return self._cached_tilt - def get_rotation_angles(self, model: Optional[CameraModel] = None) -> Tuple[float, float]: + def get_rotation_angles(self, model: CameraModel) -> Tuple[float, float]: """Return the angle difference between the detection and the camera with respect to the x and y axes of the camera.""" - if model is None: - model = self._get_cached_model() - if model == self._cached_camera_model and self._cached_rotation_angles is not None: return self._cached_rotation_angles - rvec, _ = self.get_pose() + rvec, _ = self.get_pose(model) rotmat, _ = cv2.Rodrigues(rvec) rotmat[:2, :] *= -1 @@ -166,11 +158,8 @@ def get_rotation_angles(self, model: Optional[CameraModel] = None) -> Tuple[floa return self._cached_rotation_angles - def get_pose(self, model: Optional[CameraModel] = None) -> Tuple[np.array, np.array]: + def get_pose(self, model: CameraModel) -> Tuple[np.array, np.array]: """Return the pose of the detection in rodrigues tuple formulation. If a model is not provided, whe single-shot version is used, which produced only a rough estimation in most cases, and a complete incorrect one in some.""" - if model is None: - model = self._get_cached_model() - if model == self._cached_camera_model and self._cached_pose is not None: return self._cached_pose @@ -179,11 +168,8 @@ def get_pose(self, model: Optional[CameraModel] = None) -> Tuple[np.array, np.ar return self._cached_pose - def get_flattened_3d_points(self, model: Optional[CameraModel] = None) -> np.array: + def get_flattened_3d_points(self, model: CameraModel) -> np.array: """Get the image points reprojected into camera coordinates in the 3d space as a (M, 3) array.""" - if model is None: - model = self._get_cached_model() - if model == self._cached_camera_model and self._cached_flattened_3d_points is not None: return self._cached_pose diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py index 15cc6cde..4280afec 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py @@ -35,7 +35,10 @@ ) from intrinsic_camera_calibrator.calibrators.utils import add_detection from intrinsic_camera_calibrator.calibrators.utils import get_entropy -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model from intrinsic_camera_calibrator.data_collector import DataCollector from intrinsic_camera_calibrator.parameter import Parameter from intrinsic_camera_calibrator.parameter import ParameterizedClass @@ -78,9 +81,12 @@ class Calibrator(ParameterizedClass, QObject): partial_calibration_request = Signal(object, object) partial_calibration_results_signal = Signal(object) - def __init__(self, lock: threading.RLock, cfg: Optional[Dict] = {}): + def __init__(self, camera_model_type: CameraModelEnum, lock: threading.RLock, cfg: Optional[Dict] = {}): ParameterizedClass.__init__(self, lock) QObject.__init__(self, None) + self.camera_model_type = camera_model_type + self.model = make_opencv_camera_model( + camera_model_type) if camera_model_type.value["calibrator"] == "opencv" else make_ceres_camera_model(camera_model_type) self.use_ransac_pre_rejection = Parameter(bool, value=True, min_value=False, max_value=True) self.pre_rejection_iterations = Parameter(int, value=100, min_value=1, max_value=100) @@ -190,6 +196,8 @@ def _calibrate(self, data_collector: DataCollector): training_post_rejection_inliers = calibration_training_detections evaluation_post_rejection_inliers = raw_evaluation_detections + self.model = calibrated_model + num_training_post_rejection_inliers = len(training_post_rejection_inliers) num_evaluation_post_rejection_inliers = len(evaluation_post_rejection_inliers) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py index 7afc6d67..8f306c64 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py @@ -19,12 +19,15 @@ from intrinsic_camera_calibrator.calibrators.calibrator import CalibratorEnum from intrinsic_camera_calibrator.calibrators.ceres_calibrator import CeresCalibrator from intrinsic_camera_calibrator.calibrators.opencv_calibrator import OpenCVCalibrator +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum -def make_calibrator(calibrator_type: CalibratorEnum, **kwargs) -> Calibrator: +def make_calibrator(calibrator_type: CalibratorEnum, camera_model_type: CameraModelEnum, **kwargs) -> Calibrator: """Create a calibrator using a factory design pattern.""" classes_dic = { CalibratorEnum.OPENCV: OpenCVCalibrator, CalibratorEnum.CERES: CeresCalibrator, } - return classes_dic[calibrator_type](**kwargs) + return classes_dic[calibrator_type](camera_model_type, **kwargs) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index cd10278b..58967384 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -15,22 +15,41 @@ # limitations under the License. +import logging import threading from typing import Dict from typing import List +import cv2 +import numpy as np + +from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection from intrinsic_camera_calibrator.calibrators.calibrator import Calibrator -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModel +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model from intrinsic_camera_calibrator.parameter import Parameter class CeresCalibrator(Calibrator): - def __init__(self, lock: threading.RLock, cfg: Dict = {}): - super().__init__(lock, cfg) - - self.some_parameter_name = Parameter(int, value=2, min_value=0, max_value=6) + def __init__(self, camera_model_type: CeresCameraModelEnum, lock: threading.RLock, cfg: Dict = {}): + super().__init__(camera_model_type, lock, cfg) self.set_parameters(**cfg) - def _calibration_impl(self, detections: List[BoardDetection]) -> CameraModel: - raise NotImplementedError + def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraModel: + """Implement the calibrator interface.""" + height = detections[0].get_image_height() + width = detections[0].get_image_width() + + camera_model = make_ceres_camera_model(self.camera_model_type) + camera_model.calibrate( + height=height, + width=width, + object_points_list=[ + detection.get_flattened_object_points() for detection in detections + ], + image_points_list=[detection.get_flattened_image_points() for detection in detections] + ) + + return camera_model 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..a98799d4 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 @@ -22,51 +22,39 @@ import cv2 from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection from intrinsic_camera_calibrator.calibrators.calibrator import Calibrator -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model from intrinsic_camera_calibrator.parameter import Parameter class OpenCVCalibrator(Calibrator): """Wrapper of the opencv's camera calibration routine.""" - def __init__(self, lock: threading.RLock, cfg: Dict = {}): - super().__init__(lock, cfg) - - self.radial_distortion_coefficients = Parameter(int, value=2, min_value=0, max_value=6) - self.use_tangential_distortion = Parameter( - bool, value=True, min_value=False, max_value=True - ) - 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) + def __init__(self, camera_model_type: OpenCVCameraModelEnum, lock: threading.RLock, cfg: Dict = {}): + super().__init__(camera_model_type, lock, cfg) + # self.radial_distortion_coefficients = Parameter(int, value=2, min_value=0, max_value=6) + # self.use_tangential_distortion = Parameter( + # bool, value=True, min_value=False, max_value=True + # ) + # 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.set_parameters(**cfg) - def _calibration_impl(self, detections: List[BoardDetection]) -> CameraModel: + def _calibration_impl(self, detections: List[BoardDetection]) -> OpenCVCameraModel: """Implement the calibrator interface.""" - flags = 0 - flags |= cv2.CALIB_FIX_PRINCIPAL_POINT if self.fix_principal_point.value else 0 - flags |= cv2.CALIB_FIX_ASPECT_RATIO if self.fix_principal_point.value else 0 - flags |= cv2.CALIB_ZERO_TANGENT_DIST if not self.use_tangential_distortion.value else 0 - flags |= cv2.CALIB_RATIONAL_MODEL if self.radial_distortion_coefficients.value > 3 else 0 - flags |= cv2.CALIB_FIX_K6 if self.radial_distortion_coefficients.value < 6 else 0 - flags |= cv2.CALIB_FIX_K5 if self.radial_distortion_coefficients.value < 5 else 0 - flags |= cv2.CALIB_FIX_K4 if self.radial_distortion_coefficients.value < 4 else 0 - 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 - height = detections[0].get_image_height() width = detections[0].get_image_width() - camera_model = CameraModel() + camera_model = make_opencv_camera_model(self.camera_model_type) camera_model.calibrate( height=height, width=width, object_points_list=[ detection.get_flattened_object_points() for detection in detections ], - image_points_list=[detection.get_flattened_image_points() for detection in detections], - flags=flags, + image_points_list=[detection.get_flattened_image_points() for detection in detections] ) return camera_model diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py index 320dec3e..2eb33fe2 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py @@ -18,7 +18,7 @@ from typing import List from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel import matplotlib.pyplot as plt import numpy as np @@ -77,7 +77,7 @@ def add_detection_errors( tilt_i = int(np.clip((0.5 * tilt_cells) * (tilt_x + 1), 0, tilt_cells - 1)) tilt_j = int(np.clip((0.5 * tilt_cells) * (tilt_y + 1), 0, tilt_cells - 1)) - errors = detection.get_reprojection_errors() + errors = detection.get_reprojection_errors(camera_model) tilt_errors[tilt_j][tilt_i].append(np.sqrt(np.power(errors, 2).mean())) @@ -126,7 +126,7 @@ def process_detections(detections: List[BoardDetection]): return pixel_occupancy, tilt_occupancy, np.array(z_list) - def plot_detection_set(i: int, name: str, detections: List[BoardDetection]): + def plot_detection_set(i: int, name: str, detections: List[BoardDetection], camera_model: CameraModel): if len(detections) == 0: return @@ -171,11 +171,11 @@ def plot_detection_set(i: int, name: str, detections: List[BoardDetection]): linewidth=0.5, ) - plot_detection_set(0, "Training", training_detections) - plot_detection_set(1, "Pre rejection inliers", pre_rejection_inlier_detections) - plot_detection_set(2, "Subsampled", subsampled_detections) - plot_detection_set(3, "Post rejection inliers", post_rejection_inlier_detections) - plot_detection_set(4, "Evaluation", evaluation_detections) + plot_detection_set(0, "Training", training_detections, calibrated_model) + plot_detection_set(1, "Pre rejection inliers", pre_rejection_inlier_detections, calibrated_model) + plot_detection_set(2, "Subsampled", subsampled_detections, calibrated_model) + plot_detection_set(3, "Post rejection inliers", post_rejection_inlier_detections, calibrated_model) + plot_detection_set(4, "Evaluation", evaluation_detections, calibrated_model) plt.show() @@ -231,7 +231,7 @@ def process_detections(detections: List[BoardDetection]): return pixel_errors_mean, pixel_errors_std, tilt_errors_mean, tilt_errors_std - def plot_detection_set(j: int, name: str, detections: List[BoardDetection]): + def plot_detection_set(j: int, name: str, detections: List[BoardDetection], camera_model: CameraModel): if len(detections) == 0: return @@ -282,7 +282,7 @@ def plot_detection_set(j: int, name: str, detections: List[BoardDetection]): ) plt.colorbar(tilt_rms_std_ax, ax=axes1[j, 3]) - def plot_calibration_vs_single_shot_calibration(j, name, detections: List[BoardDetection]): + def plot_calibration_vs_single_shot_calibration(j, name, detections: List[BoardDetection], camera_model: CameraModel): label = np.array([str(i) for i in range(len(detections))]) calibrated_errors = np.array( @@ -290,7 +290,7 @@ def plot_calibration_vs_single_shot_calibration(j, name, detections: List[BoardD ) single_shot_errors = np.array( [ - np.sqrt(np.power(detection.get_reprojection_errors(), 2).mean()) + np.sqrt(np.power(detection.get_reprojection_errors(camera_model), 2).mean()) for detection in detections ] ) @@ -315,12 +315,12 @@ def plot_calibration_vs_single_shot_calibration(j, name, detections: List[BoardD axes2[j].set_xticks(x_axis, label, rotation="vertical") axes2[j].legend() - plot_detection_set(0, "Training", training_detections) - plot_detection_set(1, "Inliers", inlier_detections) - plot_detection_set(2, "Evaluation", evaluation_detections) + plot_detection_set(0, "Training", training_detections, calibrated_model) + plot_detection_set(1, "Inliers", inlier_detections, calibrated_model) + plot_detection_set(2, "Evaluation", evaluation_detections, calibrated_model) - plot_calibration_vs_single_shot_calibration(0, "Training", training_detections) - plot_calibration_vs_single_shot_calibration(1, "Inliers", inlier_detections) - plot_calibration_vs_single_shot_calibration(2, "Evaluation", evaluation_detections) + plot_calibration_vs_single_shot_calibration(0, "Training", training_detections, calibrated_model) + plot_calibration_vs_single_shot_calibration(1, "Inliers", inlier_detections, calibrated_model) + plot_calibration_vs_single_shot_calibration(2, "Evaluation", evaluation_detections, calibrated_model) plt.show() 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..6da64403 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 @@ -54,7 +54,12 @@ from intrinsic_camera_calibrator.calibrators.calibrator import Calibrator from intrinsic_camera_calibrator.calibrators.calibrator import CalibratorEnum from intrinsic_camera_calibrator.calibrators.calibrator_factory import make_calibrator -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model from intrinsic_camera_calibrator.data_collector import CollectionStatus from intrinsic_camera_calibrator.data_collector import DataCollector from intrinsic_camera_calibrator.data_sources.data_source import DataSource @@ -102,6 +107,9 @@ def __init__(self, cfg): self.estimated_fps = 0 self.last_processed_stamp = None + # Available camera models + self.camera_models = {"opencv": OpenCVCameraModelEnum, "ceres": CeresCameraModelEnum} + # Camera models to use normally self.current_camera_model: CameraModel = None self.pending_partial_calibration = False @@ -119,7 +127,8 @@ def __init__(self, cfg): self.board_parameters: ParameterizedClass = None self.detector: BoardDetector = None self.data_collector = DataCollector(self.cfg["data_collector"]) - self.calibrator_dict: Dict[CalibratorEnum, Calibrator] = {} + self.calibrator_dict: Dict[CalibratorEnum, + Dict[CameraModelEnum, Calibrator]] = defaultdict(dict) self.image_view_mode = ImageViewMode.SOURCE_UNRECTIFIED self.paused = False @@ -133,15 +142,17 @@ def __init__(self, cfg): ): calibrator_cfg = self.cfg["calibration_parameters"] - calibrator = make_calibrator(calibrator_type, lock=self.lock, cfg=calibrator_cfg) - self.calibrator_dict[calibrator_type] = calibrator + for camera_model_type in self.camera_models[calibrator_type.value["name"]]: + calibrator = make_calibrator( + calibrator_type, camera_model_type, lock=self.lock, cfg=calibrator_cfg) + self.calibrator_dict[calibrator_type][camera_model_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 - ) + calibrator.moveToThread(self.calibration_thread) # comment this to debug + 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) @@ -322,7 +333,10 @@ def make_calibration_group(self): self.calibration_group.setFlat(True) self.calibrator_type_combobox = QComboBox() - self.calibrator_type_combobox.setEnabled(False) # TODO: implement this later + self.calibrator_type_combobox.setEnabled(True) + + self.camera_model_type_combobox = QComboBox() + self.camera_model_type_combobox.setEnabled(True) self.calibration_parameters_button = QPushButton("Calibration parameters") self.calibration_button = QPushButton("Calibrate") @@ -344,24 +358,30 @@ def make_calibration_group(self): self.calibration_evaluation_inlier_rms_label = QLabel("\trms error (inlier):") def on_parameters_view_closed(): - # self.calibrator_type_combobox.setEnabled(True) TODO implement this later self.calibration_parameters_button.setEnabled(True) + self.calibrator_type_combobox.setEnabled(True) + self.camera_model_type_combobox.setEnabled(True) def on_parameters_button_clicked(): self.calibrator_type_combobox.setEnabled(False) + self.camera_model_type_combobox.setEnabled(False) self.calibration_parameters_button.setEnabled(False) calibrator_type = self.calibrator_type_combobox.currentData() + camera_model_type = self.camera_model_type_combobox.currentData() - data_collection_parameters_view = ParameterView(self.calibrator_dict[calibrator_type]) + data_collection_parameters_view = ParameterView( + self.calibrator_dict[calibrator_type][camera_model_type]) data_collection_parameters_view.closed.connect(on_parameters_view_closed) def on_calibration_clicked(): calibrator_type = self.calibrator_type_combobox.currentData() - self.calibrator_dict[calibrator_type].calibration_request.emit( + camera_model_type = self.camera_model_type_combobox.currentData() + self.calibrator_dict[calibrator_type][camera_model_type].calibration_request.emit( self.data_collector.clone_without_images() ) self.calibrator_type_combobox.setEnabled(False) + self.camera_model_type_combobox.setEnabled(False) self.calibration_parameters_button.setEnabled(False) self.calibration_button.setEnabled(False) self.evaluation_button.setEnabled(False) @@ -370,23 +390,49 @@ def on_calibration_clicked(): def on_evaluation_clicked(): calibrator_type = self.calibrator_type_combobox.currentData() + camera_model_type = self.camera_model_type_combobox.currentData() camera_model = ( self.current_camera_model if self.calibrated_camera_model is None else self.calibrated_camera_model ) - self.calibrator_dict[calibrator_type].evaluation_request.emit( + self.calibrator_dict[calibrator_type][camera_model_type].evaluation_request.emit( self.data_collector.clone_without_images(), camera_model ) self.calibrator_type_combobox.setEnabled(False) + self.camera_model_type_combobox.setEnabled(False) self.calibration_parameters_button.setEnabled(False) self.calibration_button.setEnabled(False) self.evaluation_button.setEnabled(False) self.calibration_status_label.setText("Calibration status: evaluating") + def on_calibrator_clicked(): + self.camera_model_type_combobox.clear() + calibrator_type = self.calibrator_type_combobox.currentData() + for camera_model_type in self.camera_models[calibrator_type.value["name"]]: + self.camera_model_type_combobox.addItem( + camera_model_type.value["display"], camera_model_type) + + if "camera_model_type" in self.cfg: + try: + self.camera_model_type_combobox.setCurrentIndex( + calibrator_type.value["name"].from_name( + self.cfg["camera_model_type"]).get_id() + ) + except Exception as e: + logging.error(f"Invalid camera_model_type: {e}") + else: + self.camera_model_type_combobox.setCurrentIndex(0) + + def on_camera_model_clicked(): + calibrator_type = self.calibrator_type_combobox.currentData() + camera_model_type = self.camera_model_type_combobox.currentData() + if calibrator_type is not None and camera_model_type is not None: + self.current_camera_model = self.calibrator_dict[calibrator_type][camera_model_type].model + self.calibration_parameters_button.clicked.connect(on_parameters_button_clicked) self.calibration_button.clicked.connect(on_calibration_clicked) @@ -398,6 +444,9 @@ def on_evaluation_clicked(): self.save_button.clicked.connect(self.on_save_clicked) self.save_button.setEnabled(False) + self.calibrator_type_combobox.currentIndexChanged.connect(on_calibrator_clicked) + self.camera_model_type_combobox.currentIndexChanged.connect(on_camera_model_clicked) + for calibrator_type in CalibratorEnum: self.calibrator_type_combobox.addItem(calibrator_type.value["display"], calibrator_type) @@ -414,6 +463,7 @@ def on_evaluation_clicked(): calibration_layout = QVBoxLayout() calibration_layout.setAlignment(Qt.AlignTop) calibration_layout.addWidget(self.calibrator_type_combobox) + calibration_layout.addWidget(self.camera_model_type_combobox) calibration_layout.addWidget(self.calibration_parameters_button) calibration_layout.addWidget(self.calibration_button) calibration_layout.addWidget(self.evaluation_button) @@ -648,7 +698,8 @@ def start( self.data_source = data_source self.board_type = board_type self.board_parameters = board_parameters - self.current_camera_model = initial_intrinsics + if initial_intrinsics is not None: + self.current_camera_model = initial_intrinsics self.setEnabled(True) self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()})") @@ -670,7 +721,7 @@ def start( if self.operation_mode == OperationMode.EVALUATION: self.calibration_button.setEnabled(False) - self.detector.moveToThread(self.detector_thread) + self.detector.moveToThread(self.detector_thread) # comment this to debug self.detector.detection_results_signal.connect(self.process_detection_results) self.request_image_detection.connect(self.detector.detect) @@ -726,7 +777,8 @@ def process_calibration_results( f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}" # noqa E231 ) - # self.calibrator_type_combobox.setEnabled(True) TODO implement this later + self.calibrator_type_combobox.setEnabled(True) + self.camera_model_type_combobox.setEnabled(True) self.calibration_parameters_button.setEnabled(True) self.calibration_button.setEnabled(True) self.evaluation_button.setEnabled(True) @@ -778,7 +830,8 @@ def process_evaluation_results( f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}" # noqa E231 ) - # self.calibrator_type_combobox.setEnabled(True) TODO implement this later + self.calibrator_type_combobox.setEnabled(True) + self.camera_model_type_combobox.setEnabled(True) self.calibration_parameters_button.setEnabled(True) self.calibration_button.setEnabled(self.operation_mode == OperationMode.CALIBRATION) self.evaluation_button.setEnabled(True) @@ -897,7 +950,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.image_view.set_detection_ordered_points(ordered_image_points) self.image_view.set_grid_size_pixels(detection.get_flattened_cell_sizes().mean()) - reprojection_errors = detection.get_reprojection_errors() + reprojection_errors = detection.get_reprojection_errors(camera_model) reprojection_errors_norm = np.linalg.norm(reprojection_errors, axis=-1) reprojection_error_max = reprojection_errors_norm.max() reprojection_error_mean = reprojection_errors_norm.mean() @@ -919,7 +972,7 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im f"Linear error rms: {detection.get_linear_error_rms():.2f} px" # noqa E231 ) self.rough_tilt_label.setText( - f"Rough tilt: {detection.get_tilt():.2f} degrees" # noqa E231 + f"Rough tilt: {detection.get_tilt(camera_model):.2f} degrees" # noqa E231 ) self.rough_angles_label.setText( f"Rough angles: x={rough_angles[0]:.2f} y={rough_angles[1]:.2f} degrees" # noqa E231 @@ -1022,7 +1075,8 @@ def update_current_camera_model(self): return calibrator_type = self.calibrator_type_combobox.currentData() - self.calibrator_dict[calibrator_type].partial_calibration_request.emit( + camera_model_type = self.camera_model_type_combobox.currentData() + self.calibrator_dict[calibrator_type][camera_model_type].partial_calibration_request.emit( self.data_collector.clone_without_images(), self.current_camera_model ) @@ -1107,7 +1161,8 @@ def data_source_external_callback(self, img: np.array, stamp: float): 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 + # Using a signal from another thread results in the slot being executed in the class Qt thread + self.produced_data_signal.emit() def on_parameter_changed(self): self.should_process_image.emit() 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_models/camera_model.py similarity index 70% rename from calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_model.py rename to calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model.py index 1c3bba0e..9e55c866 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_models/camera_model.py @@ -14,6 +14,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging + +from enum import Enum from typing import Dict from typing import List from typing import Optional @@ -23,8 +26,33 @@ import numpy as np +class CameraModelEnum(Enum): + @classmethod + def from_name(cls, name: str): + """ + Return the enum member that matches the name. + """ + for model in cls: + if model.value["name"] == name: + return model + raise ValueError(f"{name} not found in {cls.__name__}") + + @classmethod + def from_index(cls, i: int): + """ + Return the enum member by index. + """ + return list(cls)[i] + + def get_id(self) -> int: + """ + Return the index of the current enum member. + """ + return list(self.__class__).index(self) + + class CameraModel: - """Basic opencv's camera intrinsics. It approximates the distortion via a rational model.""" + """Base class of camera model.""" def __init__( self, @@ -33,8 +61,8 @@ def __init__( height: Optional[int] = None, width: Optional[int] = None, ): - self.k = k - self.d = d + self.k = np.zeros((3, 3)) if k is None else k + self.d = np.zeros((5,)) if d is None else d self.height = height self.width = width @@ -48,6 +76,7 @@ def __eq__(self, other: "CameraModel") -> bool: and self.width == other.width and (self.k == other.k).all() and (self.d == other.d).all() + and type(self) == type(other) ) def calibrate( @@ -55,29 +84,14 @@ def calibrate( height: int, width: int, object_points_list: List[np.array], - image_points_list: List[np.array], - flags: Optional[int] = 0, + image_points_list: List[np.array] ): - """Calibrate the model using a set objet-image pairs.""" + """Calibrate the model.""" assert len(object_points_list) == len(image_points_list) self.height = height self.width = width - - object_points_list = [ - object_points.astype(np.float32).reshape(-1, 3) for object_points in object_points_list - ] - image_points_list = [ - image_points.astype(np.float32).reshape(-1, 1, 2) for image_points in image_points_list - ] - - _, self.k, self.d, rvecs, tvecs = cv2.calibrateCamera( - object_points_list, - image_points_list, - (self.width, self.height), - cameraMatrix=None, - distCoeffs=None, - flags=flags, - ) + logging.warning(f"Camera model: {self.__class__.__name__}") # TODO(amadeuszsz): remove before opening PR + self._calibrate_impl(object_points_list, image_points_list) def get_pose( self, @@ -90,10 +104,7 @@ def get_pose( object_points = board_detection.get_flattened_object_points() image_points = board_detection.get_flattened_image_points() - d = np.zeros((5,)) if self.d is None else self.d - k = self.k - - _, rvec, tvec = cv2.solvePnP(object_points, image_points, k, d) + _, rvec, tvec = cv2.solvePnP(object_points, image_points, self.k, self.d) return rvec, tvec @@ -150,37 +161,8 @@ def get_reprojection_errors( projected_points = projected_points.reshape((num_points, 2)) return projected_points - image_points - def get_undistorted_camera_model(self, alpha: float): - """Compute the undistorted version of the camera model.""" - undistorted_k, _ = cv2.getOptimalNewCameraMatrix( - self.k, self.d, (self.width, self.height), alpha - ) - - return CameraModel( - k=undistorted_k, d=np.zeros_like(self.d), height=self.height, width=self.width - ) - - def rectify(self, img: np.array, alpha=0.0) -> np.array: - """Rectifies an image using the current camera model. Alpha is a value in the [0,1] range to regulate how the rectified image is cropped. 0 means that all the pixels in the rectified image are valid whereas 1 keeps all the original pixels from the unrectified image into the rectifies one, filling with zeroes the invalid pixels.""" - if np.abs(self.d).sum() == 0: - return img - - if self._cached_undistorted_model is None or alpha != self._cached_undistortion_alpha: - self._cached_undistortion_alpha = alpha - self._cached_undistorted_model = self.get_undistorted_camera_model(alpha=alpha) - ( - self._cached_undistortion_map_x, - self._cached_undistortion_map_y, - ) = cv2.initUndistortRectifyMap( - self.k, self.d, None, self._cached_undistorted_model.k, (self.width, self.height), 5 - ) - - return cv2.remap( - img, self._cached_undistortion_map_x, self._cached_undistortion_map_y, cv2.INTER_LINEAR - ) - def as_dict(self, alpha: float = 0.0) -> Dict: - undistorted = self.get_undistorted_camera_model(alpha) + undistorted = self._get_undistorted_camera_model_impl(alpha) p = np.zeros((3, 4)) p[0:3, 0:3] = undistorted.k @@ -222,6 +204,42 @@ def from_dict(self, d): d["distortion_model"]["rows"], d["distortion_model"]["cols"] ) + def _calibrate_impl(self, object_points_list: List[np.array], image_points_list: List[np.array]): + """Abstract method to calibrate the camera model.""" + raise NotImplementedError + + def _get_undistorted_camera_model_impl(self, alpha: float): + """Abstract method to compute the undistorted version of the camera model.""" + raise NotImplementedError + + def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: + """Abstract method to rectify the input image.""" + raise NotImplementedError + +#################################################################################################### + # def _calibrate_impl( + # self, + # object_points_list: List[np.array], + # image_points_list: List[np.array] + # ): + # """Calibrate OpenCV camera model.""" + + # object_points_list = [ + # object_points.astype(np.float32).reshape(-1, 3) for object_points in object_points_list + # ] + # image_points_list = [ + # image_points.astype(np.float32).reshape(-1, 1, 2) for image_points in image_points_list + # ] + + # _, self.k, self.d, rvecs, tvecs = cv2.calibrateCamera( + # object_points_list, + # image_points_list, + # (self.width, self.height), + # cameraMatrix=None, + # distCoeffs=None + # ) +#################################################################################################### + class CameraModelWithBoardDistortion(CameraModel): """An slightly improves model that also incorporates the distortion/bending of the calibration board..""" diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py new file mode 100644 index 00000000..b8ad27d9 --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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. + + +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PolynomialOpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import RationalOpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PrismOpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModel +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import PolynomialCeresCameraModel +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import RationalCeresCameraModel + + +def make_opencv_camera_model(camera_model_type: OpenCVCameraModelEnum, **kwargs) -> OpenCVCameraModel: + """Create a OpenCV camera model using a factory design pattern.""" + classes_dic = { + OpenCVCameraModelEnum.OPENCV_POLYNOMIAL: PolynomialOpenCVCameraModel, + OpenCVCameraModelEnum.OPENCV_RATIONAL: RationalOpenCVCameraModel, + OpenCVCameraModelEnum.OPENCV_PRISM: PrismOpenCVCameraModel + } + return classes_dic[camera_model_type](**kwargs) + + +def make_ceres_camera_model(camera_model_type: CeresCameraModelEnum, **kwargs) -> CeresCameraModel: + """Create a Ceres camera model using a factory design pattern.""" + classes_dic = { + CeresCameraModelEnum.CERES_POLYNOMIAL: PolynomialCeresCameraModel, + CeresCameraModelEnum.CERES_RATIONAL: RationalCeresCameraModel + } + return classes_dic[camera_model_type](**kwargs) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py new file mode 100644 index 00000000..7880f960 --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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. + +from typing import List +from typing import Optional + +import cv2 +import numpy as np + +from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PolynomialOpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import RationalOpenCVCameraModel +from intrinsic_camera_calibrator.parameter import Parameter + + +class CeresCameraModelEnum(CameraModelEnum): + CERES_POLYNOMIAL = {"name": "ceres_polynomial", + "display": "Ceres Polynomial", "calibrator": "ceres"} + CERES_RATIONAL = {"name": "ceres_rational", "display": "Ceres Rational", "calibrator": "ceres"} + + +class CeresCameraModel(CameraModel): + """Basic Ceres camera model class.""" + + def __init__( + self, + k: Optional[np.array] = None, + d: Optional[np.array] = None, + height: Optional[int] = None, + width: Optional[int] = None + ): + super().__init__(k, d, height, width) + + def _init_calibrate_impl(self, **kwargs): + """Abstract method to initial calibration of the Ceres camera model.""" + raise NotImplementedError + + def _calibrate_impl( + self, + object_points_list: List[np.array], + image_points_list: List[np.array] + ): + """Calibrate Ceres camera model.""" + camera_model = self._init_calibrate_impl(object_points_list, image_points_list) + + rms_error, camera_matrix, dist_coeffs, rvecs, tvecs = calibrate( + object_points_list=object_points_list, + image_points_list=image_points_list, + initial_camera_matrix=camera_model.k, + initial_dist_coeffs=camera_model.d, + num_radial_coeffs=self.radial_distortion_coefficients, + num_rational_coeffs=self.rational_distortion_coefficients, + use_tangential_distortion=self.use_tangential_distortion, + verbose=False + ) + + self.k = camera_matrix + self.d = dist_coeffs + + def _get_undistorted_camera_model_impl(self, alpha: float): + """Compute the undistorted version of the camera model.""" + undistorted_k, _ = cv2.getOptimalNewCameraMatrix( + self.k, self.d, (self.width, self.height), alpha + ) + + return CameraModel( + k=undistorted_k, d=np.zeros_like(self.d), height=self.height, width=self.width + ) + + def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: + """Rectifies an image using the current camera model. Alpha is a value in the [0,1] range to regulate how the rectified image is cropped. 0 means that all the pixels in the rectified image are valid whereas 1 keeps all the original pixels from the unrectified image into the rectifies one, filling with zeroes the invalid pixels.""" + if np.abs(self.d).sum() == 0: + return img + + if self._cached_undistorted_model is None or alpha != self._cached_undistortion_alpha: + self._cached_undistortion_alpha = alpha + self._cached_undistorted_model = self.get_undistorted_camera_model(alpha=alpha) + ( + self._cached_undistortion_map_x, + self._cached_undistortion_map_y, + ) = cv2.initUndistortRectifyMap( + self.k, self.d, None, self._cached_undistorted_model.k, (self.width, self.height), 5 + ) + + return cv2.remap( + img, self._cached_undistortion_map_x, self._cached_undistortion_map_y, cv2.INTER_LINEAR + ) + + +class PolynomialCeresCameraModel(CeresCameraModel): + """Polynomial Ceres camera model class.""" + + def __init__( + self, + k: Optional[np.array] = None, + d: Optional[np.array] = None, + height: Optional[int] = None, + width: Optional[int] = None + ): + super().__init__(k, d, height, width) + self.radial_distortion_coefficients = 3 + self.rational_distortion_coefficients = 0 + self.use_tangential_distortion = False + + def _init_calibrate_impl( + self, + object_points_list: List[np.array], + image_points_list: List[np.array] + ) -> PolynomialOpenCVCameraModel: + """Initialize the calibration of the camera model.""" + camera_model = PolynomialOpenCVCameraModel() + camera_model.calibrate( + height=self.height, + width=self.width, + object_points_list=object_points_list, + image_points_list=image_points_list + ) + + if (camera_model.d.size > 5): + camera_model.d = camera_model.d.reshape(-1, camera_model.d.size)[:, :5] + + return camera_model + + +class RationalCeresCameraModel(CeresCameraModel): + """Polynomial Ceres camera model class.""" + + def __init__( + self, + k: Optional[np.array] = None, + d: Optional[np.array] = None, + height: Optional[int] = None, + width: Optional[int] = None + ): + super().__init__(k, d, height, width) + self.radial_distortion_coefficients = 3 + self.rational_distortion_coefficients = 3 + self.use_tangential_distortion = True + + def _init_calibrate_impl( + self, + object_points_list: List[np.array], + image_points_list: List[np.array] + ) -> RationalOpenCVCameraModel: + """Initialize the calibration of the camera model.""" + camera_model = RationalOpenCVCameraModel() + camera_model.calibrate( + height=self.height, + width=self.width, + object_points_list=object_points_list, + image_points_list=image_points_list + ) + + if (camera_model.d.size > 8): + camera_model.d = camera_model.d.reshape(-1, camera_model.d.size)[:, :8] + + return camera_model diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py new file mode 100644 index 00000000..931a5979 --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 + +# Copyright 2024 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# 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. + +from typing import List +from typing import Optional + +import cv2 +import numpy as np + +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.parameter import Parameter + + +class OpenCVCameraModelEnum(CameraModelEnum): + OPENCV_POLYNOMIAL = {"name": "opencv_polynomial", "display": "OpenCV Polynomial", "calibrator": "opencv"} + OPENCV_RATIONAL = {"name": "opencv_rational", "display": "OpenCV Rational", "calibrator": "opencv"} + OPENCV_PRISM = {"name": "opencv_prism", "display": "OpenCV Prism", "calibrator": "opencv"} + + +class OpenCVCameraModel(CameraModel): + """Basic opencv's camera model class.""" + + def __init__( + self, + k: Optional[np.array] = None, + d: Optional[np.array] = None, + height: Optional[int] = None, + width: Optional[int] = None + ): + super().__init__(k, d, height, width) + self.flags = 0 + # self.flags |= cv2.CALIB_FIX_PRINCIPAL_POINT + # self.flags |= cv2.CALIB_FIX_ASPECT_RATIO + # self.flags |= cv2.CALIB_ZERO_TANGENT_DIST + + def _calibrate_impl( + self, + object_points_list: List[np.array], + image_points_list: List[np.array] + ): + """Calibrate OpenCV camera model.""" + object_points_list = [ + object_points.astype(np.float32).reshape(-1, 3) for object_points in object_points_list + ] + image_points_list = [ + image_points.astype(np.float32).reshape(-1, 1, 2) for image_points in image_points_list + ] + + _, self.k, self.d, rvecs, tvecs = cv2.calibrateCamera( + object_points_list, + image_points_list, + (self.width, self.height), + cameraMatrix=None, + distCoeffs=None, + flags=self.flags, + ) + pass + + def _get_undistorted_camera_model_impl(self, alpha: float): + """Compute the undistorted version of the camera model.""" + undistorted_k, _ = cv2.getOptimalNewCameraMatrix( + self.k, self.d, (self.width, self.height), alpha + ) + + return CameraModel( # TODO(amadeuszsz): handle top class properly + k=undistorted_k, d=np.zeros_like(self.d), height=self.height, width=self.width + ) + + def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: + """Rectifies an image using the current camera model. Alpha is a value in the [0,1] range to regulate how the rectified image is cropped. 0 means that all the pixels in the rectified image are valid whereas 1 keeps all the original pixels from the unrectified image into the rectifies one, filling with zeroes the invalid pixels.""" + if np.abs(self.d).sum() == 0: + return img + + if self._cached_undistorted_model is None or alpha != self._cached_undistortion_alpha: + self._cached_undistortion_alpha = alpha + self._cached_undistorted_model = self.get_undistorted_camera_model(alpha=alpha) + ( + self._cached_undistortion_map_x, + self._cached_undistortion_map_y, + ) = cv2.initUndistortRectifyMap( + self.k, self.d, None, self._cached_undistorted_model.k, (self.width, self.height), 5 + ) + + return cv2.remap( + img, self._cached_undistortion_map_x, self._cached_undistortion_map_y, cv2.INTER_LINEAR + ) + + +class PolynomialOpenCVCameraModel(OpenCVCameraModel): + """Polynomial OpenCV camera model class.""" + + def __init__( + self, + k: Optional[np.array] = None, + d: Optional[np.array] = None, + height: Optional[int] = None, + width: Optional[int] = None + ): + super().__init__(k, d, height, width) + self.flags |= cv2.CALIB_FIX_K1 + self.flags |= cv2.CALIB_FIX_K2 + self.flags |= cv2.CALIB_FIX_K3 + + +class RationalOpenCVCameraModel(PolynomialOpenCVCameraModel): + """Rational OpenCV camera model class.""" + + def __init__( + self, + k: Optional[np.array] = None, + d: Optional[np.array] = None, + height: Optional[int] = None, + width: Optional[int] = None + ): + super().__init__(k, d, height, width) + self.flags |= cv2.CALIB_RATIONAL_MODEL + self.flags |= cv2.CALIB_FIX_K4 + self.flags |= cv2.CALIB_FIX_K5 + self.flags |= cv2.CALIB_FIX_K6 + + +class PrismOpenCVCameraModel(RationalOpenCVCameraModel): + """Prism OpenCV camera model class.""" + + def __init__( + self, + k: Optional[np.array] = None, + d: Optional[np.array] = None, + height: Optional[int] = None, + width: Optional[int] = None + ): + super().__init__(k, d, height, width) + self.flags |= cv2.CALIB_THIN_PRISM_MODEL 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..75998a8d 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 @@ -21,7 +21,7 @@ from PySide2.QtCore import Signal from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.parameter import Parameter from intrinsic_camera_calibrator.parameter import ParameterizedClass from intrinsic_camera_calibrator.types import CollectionStatus @@ -457,7 +457,7 @@ def process_detection( self, image: np.array, detection: BoardDetection, - camera_model: Optional[CameraModel] = None, + camera_model: CameraModel, mode: OperationMode = OperationMode.CALIBRATION, ) -> CollectionStatus: """Evaluate if a detection should be added to either the training or evaluation dataset.""" @@ -470,7 +470,7 @@ def process_detection( accepted &= speed < self.max_allowed_speed if self.filter_by_reprojection_error: - reprojection_errors = detection.get_reprojection_errors() + reprojection_errors = detection.get_reprojection_errors(camera_model) reprojection_errors_norm = np.linalg.norm(reprojection_errors, axis=-1) reprojection_error_max = reprojection_errors_norm.max() diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py index f50a8443..ebb3d01a 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py @@ -15,7 +15,7 @@ # limitations under the License. import cv2 -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel import numpy as np import ruamel.yaml import yaml diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py index 71f0f61b..5ca8bbdf 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/data_collector_view.py @@ -19,7 +19,7 @@ from PySide2.QtCore import QObject from PySide2.QtCore import Signal -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.data_collector import CollectedData from intrinsic_camera_calibrator.data_collector import DataCollector import matplotlib.pyplot as plt 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..508b0b2e 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 @@ -27,7 +27,7 @@ make_board_parameters, ) from intrinsic_camera_calibrator.boards import BoardEnum -from intrinsic_camera_calibrator.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.data_sources.data_source import DataSource from intrinsic_camera_calibrator.data_sources.data_source import DataSourceEnum from intrinsic_camera_calibrator.data_sources.data_source_factory import make_data_source diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml index 21ac7686..8d53971c 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/package.xml @@ -7,6 +7,7 @@ Kenzo Lobos Tsunekawa TODO: License declaration + ceres_intrinsic_camera_calibrator python-dt-apriltags-pip python3-matplotlib python3-pyside2.qtquick From 453f4f1918d07f0a425092ac4998c24bf121187b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 6 Nov 2024 08:24:38 +0000 Subject: [PATCH 02/38] ci(pre-commit): autofix --- .../board_detections/board_detection.py | 5 +- .../calibrators/calibrator.py | 13 +++-- .../calibrators/calibrator_factory.py | 6 +- .../calibrators/ceres_calibrator.py | 13 +++-- .../calibrators/opencv_calibrator.py | 8 ++- .../calibrators/utils.py | 28 +++++++--- .../camera_calibrator.py | 29 ++++++---- .../camera_models/camera_model.py | 56 ++++++++++--------- .../camera_models/camera_model_factory.py | 22 +++++--- .../camera_models/ceres_camera_model.py | 44 +++++++-------- .../camera_models/opencv_camera_model.py | 27 +++++---- 11 files changed, 145 insertions(+), 106 deletions(-) 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 075a98b4..94f39434 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 @@ -112,10 +112,7 @@ def get_reprojection_errors(self, model: CameraModel) -> np.array: if self._cached_camera_model is None: self._precompute_single_shot_model(model) - if ( - model == self._cached_camera_model - and self._cached_reprojection_errors is not None - ): + if model == self._cached_camera_model and self._cached_reprojection_errors is not None: return self._cached_reprojection_errors self._cached_camera_model = model diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py index 4280afec..fc16ca5e 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py @@ -37,8 +37,8 @@ from intrinsic_camera_calibrator.calibrators.utils import get_entropy from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model from intrinsic_camera_calibrator.data_collector import DataCollector from intrinsic_camera_calibrator.parameter import Parameter from intrinsic_camera_calibrator.parameter import ParameterizedClass @@ -81,12 +81,17 @@ class Calibrator(ParameterizedClass, QObject): partial_calibration_request = Signal(object, object) partial_calibration_results_signal = Signal(object) - def __init__(self, camera_model_type: CameraModelEnum, lock: threading.RLock, cfg: Optional[Dict] = {}): + def __init__( + self, camera_model_type: CameraModelEnum, lock: threading.RLock, cfg: Optional[Dict] = {} + ): ParameterizedClass.__init__(self, lock) QObject.__init__(self, None) self.camera_model_type = camera_model_type - self.model = make_opencv_camera_model( - camera_model_type) if camera_model_type.value["calibrator"] == "opencv" else make_ceres_camera_model(camera_model_type) + self.model = ( + make_opencv_camera_model(camera_model_type) + if camera_model_type.value["calibrator"] == "opencv" + else make_ceres_camera_model(camera_model_type) + ) self.use_ransac_pre_rejection = Parameter(bool, value=True, min_value=False, max_value=True) self.pre_rejection_iterations = Parameter(int, value=100, min_value=1, max_value=100) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py index 8f306c64..1c16e8b7 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py @@ -20,11 +20,13 @@ from intrinsic_camera_calibrator.calibrators.ceres_calibrator import CeresCalibrator from intrinsic_camera_calibrator.calibrators.opencv_calibrator import OpenCVCalibrator from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum -def make_calibrator(calibrator_type: CalibratorEnum, camera_model_type: CameraModelEnum, **kwargs) -> Calibrator: +def make_calibrator( + calibrator_type: CalibratorEnum, camera_model_type: CameraModelEnum, **kwargs +) -> Calibrator: """Create a calibrator using a factory design pattern.""" classes_dic = { CalibratorEnum.OPENCV: OpenCVCalibrator, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 58967384..e37d1b87 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -20,20 +20,21 @@ from typing import Dict from typing import List -import cv2 -import numpy as np - from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate +import cv2 from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection from intrinsic_camera_calibrator.calibrators.calibrator import Calibrator +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModel from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model from intrinsic_camera_calibrator.parameter import Parameter +import numpy as np class CeresCalibrator(Calibrator): - def __init__(self, camera_model_type: CeresCameraModelEnum, lock: threading.RLock, cfg: Dict = {}): + def __init__( + self, camera_model_type: CeresCameraModelEnum, lock: threading.RLock, cfg: Dict = {} + ): super().__init__(camera_model_type, lock, cfg) self.set_parameters(**cfg) @@ -49,7 +50,7 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraMode object_points_list=[ detection.get_flattened_object_points() for detection in detections ], - image_points_list=[detection.get_flattened_image_points() for detection in detections] + image_points_list=[detection.get_flattened_image_points() for detection in detections], ) return camera_model 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 a98799d4..cb4c897d 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 @@ -22,16 +22,18 @@ import cv2 from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection from intrinsic_camera_calibrator.calibrators.calibrator import Calibrator +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model from intrinsic_camera_calibrator.parameter import Parameter class OpenCVCalibrator(Calibrator): """Wrapper of the opencv's camera calibration routine.""" - def __init__(self, camera_model_type: OpenCVCameraModelEnum, lock: threading.RLock, cfg: Dict = {}): + def __init__( + self, camera_model_type: OpenCVCameraModelEnum, lock: threading.RLock, cfg: Dict = {} + ): super().__init__(camera_model_type, lock, cfg) # self.radial_distortion_coefficients = Parameter(int, value=2, min_value=0, max_value=6) # self.use_tangential_distortion = Parameter( @@ -54,7 +56,7 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> OpenCVCameraMod object_points_list=[ detection.get_flattened_object_points() for detection in detections ], - image_points_list=[detection.get_flattened_image_points() for detection in detections] + image_points_list=[detection.get_flattened_image_points() for detection in detections], ) return camera_model diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py index 2eb33fe2..deb08c81 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py @@ -126,7 +126,9 @@ def process_detections(detections: List[BoardDetection]): return pixel_occupancy, tilt_occupancy, np.array(z_list) - def plot_detection_set(i: int, name: str, detections: List[BoardDetection], camera_model: CameraModel): + def plot_detection_set( + i: int, name: str, detections: List[BoardDetection], camera_model: CameraModel + ): if len(detections) == 0: return @@ -172,9 +174,13 @@ def plot_detection_set(i: int, name: str, detections: List[BoardDetection], came ) plot_detection_set(0, "Training", training_detections, calibrated_model) - plot_detection_set(1, "Pre rejection inliers", pre_rejection_inlier_detections, calibrated_model) + plot_detection_set( + 1, "Pre rejection inliers", pre_rejection_inlier_detections, calibrated_model + ) plot_detection_set(2, "Subsampled", subsampled_detections, calibrated_model) - plot_detection_set(3, "Post rejection inliers", post_rejection_inlier_detections, calibrated_model) + plot_detection_set( + 3, "Post rejection inliers", post_rejection_inlier_detections, calibrated_model + ) plot_detection_set(4, "Evaluation", evaluation_detections, calibrated_model) plt.show() @@ -231,7 +237,9 @@ def process_detections(detections: List[BoardDetection]): return pixel_errors_mean, pixel_errors_std, tilt_errors_mean, tilt_errors_std - def plot_detection_set(j: int, name: str, detections: List[BoardDetection], camera_model: CameraModel): + def plot_detection_set( + j: int, name: str, detections: List[BoardDetection], camera_model: CameraModel + ): if len(detections) == 0: return @@ -282,7 +290,9 @@ def plot_detection_set(j: int, name: str, detections: List[BoardDetection], came ) plt.colorbar(tilt_rms_std_ax, ax=axes1[j, 3]) - def plot_calibration_vs_single_shot_calibration(j, name, detections: List[BoardDetection], camera_model: CameraModel): + def plot_calibration_vs_single_shot_calibration( + j, name, detections: List[BoardDetection], camera_model: CameraModel + ): label = np.array([str(i) for i in range(len(detections))]) calibrated_errors = np.array( @@ -319,8 +329,12 @@ def plot_calibration_vs_single_shot_calibration(j, name, detections: List[BoardD plot_detection_set(1, "Inliers", inlier_detections, calibrated_model) plot_detection_set(2, "Evaluation", evaluation_detections, calibrated_model) - plot_calibration_vs_single_shot_calibration(0, "Training", training_detections, calibrated_model) + plot_calibration_vs_single_shot_calibration( + 0, "Training", training_detections, calibrated_model + ) plot_calibration_vs_single_shot_calibration(1, "Inliers", inlier_detections, calibrated_model) - plot_calibration_vs_single_shot_calibration(2, "Evaluation", evaluation_detections, calibrated_model) + plot_calibration_vs_single_shot_calibration( + 2, "Evaluation", evaluation_detections, calibrated_model + ) plt.show() 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 6da64403..189b8260 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 @@ -56,10 +56,10 @@ from intrinsic_camera_calibrator.calibrators.calibrator_factory import make_calibrator from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum from intrinsic_camera_calibrator.data_collector import CollectionStatus from intrinsic_camera_calibrator.data_collector import DataCollector from intrinsic_camera_calibrator.data_sources.data_source import DataSource @@ -127,8 +127,9 @@ def __init__(self, cfg): self.board_parameters: ParameterizedClass = None self.detector: BoardDetector = None self.data_collector = DataCollector(self.cfg["data_collector"]) - self.calibrator_dict: Dict[CalibratorEnum, - Dict[CameraModelEnum, Calibrator]] = defaultdict(dict) + self.calibrator_dict: Dict[CalibratorEnum, Dict[CameraModelEnum, Calibrator]] = defaultdict( + dict + ) self.image_view_mode = ImageViewMode.SOURCE_UNRECTIFIED self.paused = False @@ -144,7 +145,8 @@ def __init__(self, cfg): for camera_model_type in self.camera_models[calibrator_type.value["name"]]: calibrator = make_calibrator( - calibrator_type, camera_model_type, lock=self.lock, cfg=calibrator_cfg) + calibrator_type, camera_model_type, lock=self.lock, cfg=calibrator_cfg + ) self.calibrator_dict[calibrator_type][camera_model_type] = calibrator calibrator.moveToThread(self.calibration_thread) # comment this to debug @@ -370,7 +372,8 @@ def on_parameters_button_clicked(): camera_model_type = self.camera_model_type_combobox.currentData() data_collection_parameters_view = ParameterView( - self.calibrator_dict[calibrator_type][camera_model_type]) + self.calibrator_dict[calibrator_type][camera_model_type] + ) data_collection_parameters_view.closed.connect(on_parameters_view_closed) def on_calibration_clicked(): @@ -414,13 +417,15 @@ def on_calibrator_clicked(): calibrator_type = self.calibrator_type_combobox.currentData() for camera_model_type in self.camera_models[calibrator_type.value["name"]]: self.camera_model_type_combobox.addItem( - camera_model_type.value["display"], camera_model_type) + camera_model_type.value["display"], camera_model_type + ) if "camera_model_type" in self.cfg: try: self.camera_model_type_combobox.setCurrentIndex( - calibrator_type.value["name"].from_name( - self.cfg["camera_model_type"]).get_id() + calibrator_type.value["name"] + .from_name(self.cfg["camera_model_type"]) + .get_id() ) except Exception as e: logging.error(f"Invalid camera_model_type: {e}") @@ -431,7 +436,9 @@ def on_camera_model_clicked(): calibrator_type = self.calibrator_type_combobox.currentData() camera_model_type = self.camera_model_type_combobox.currentData() if calibrator_type is not None and camera_model_type is not None: - self.current_camera_model = self.calibrator_dict[calibrator_type][camera_model_type].model + self.current_camera_model = self.calibrator_dict[calibrator_type][ + camera_model_type + ].model self.calibration_parameters_button.clicked.connect(on_parameters_button_clicked) 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 9e55c866..f642ec44 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 @@ -14,9 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -import logging - from enum import Enum +import logging from typing import Dict from typing import List from typing import Optional @@ -84,13 +83,15 @@ def calibrate( height: int, width: int, object_points_list: List[np.array], - image_points_list: List[np.array] + image_points_list: List[np.array], ): """Calibrate the model.""" assert len(object_points_list) == len(image_points_list) self.height = height self.width = width - logging.warning(f"Camera model: {self.__class__.__name__}") # TODO(amadeuszsz): remove before opening PR + logging.warning( + f"Camera model: {self.__class__.__name__}" + ) # TODO(amadeuszsz): remove before opening PR self._calibrate_impl(object_points_list, image_points_list) def get_pose( @@ -204,7 +205,9 @@ def from_dict(self, d): d["distortion_model"]["rows"], d["distortion_model"]["cols"] ) - def _calibrate_impl(self, object_points_list: List[np.array], image_points_list: List[np.array]): + def _calibrate_impl( + self, object_points_list: List[np.array], image_points_list: List[np.array] + ): """Abstract method to calibrate the camera model.""" raise NotImplementedError @@ -216,28 +219,29 @@ def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: """Abstract method to rectify the input image.""" raise NotImplementedError + #################################################################################################### - # def _calibrate_impl( - # self, - # object_points_list: List[np.array], - # image_points_list: List[np.array] - # ): - # """Calibrate OpenCV camera model.""" - - # object_points_list = [ - # object_points.astype(np.float32).reshape(-1, 3) for object_points in object_points_list - # ] - # image_points_list = [ - # image_points.astype(np.float32).reshape(-1, 1, 2) for image_points in image_points_list - # ] - - # _, self.k, self.d, rvecs, tvecs = cv2.calibrateCamera( - # object_points_list, - # image_points_list, - # (self.width, self.height), - # cameraMatrix=None, - # distCoeffs=None - # ) +# def _calibrate_impl( +# self, +# object_points_list: List[np.array], +# image_points_list: List[np.array] +# ): +# """Calibrate OpenCV camera model.""" + +# object_points_list = [ +# object_points.astype(np.float32).reshape(-1, 3) for object_points in object_points_list +# ] +# image_points_list = [ +# image_points.astype(np.float32).reshape(-1, 1, 2) for image_points in image_points_list +# ] + +# _, self.k, self.d, rvecs, tvecs = cv2.calibrateCamera( +# object_points_list, +# image_points_list, +# (self.width, self.height), +# cameraMatrix=None, +# distCoeffs=None +# ) #################################################################################################### diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py index b8ad27d9..91c0b605 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py @@ -15,23 +15,27 @@ # limitations under the License. -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PolynomialOpenCVCameraModel -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import RationalOpenCVCameraModel -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PrismOpenCVCameraModel -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModel +from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum from intrinsic_camera_calibrator.camera_models.ceres_camera_model import PolynomialCeresCameraModel from intrinsic_camera_calibrator.camera_models.ceres_camera_model import RationalCeresCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import ( + PolynomialOpenCVCameraModel, +) +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PrismOpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import RationalOpenCVCameraModel -def make_opencv_camera_model(camera_model_type: OpenCVCameraModelEnum, **kwargs) -> OpenCVCameraModel: +def make_opencv_camera_model( + camera_model_type: OpenCVCameraModelEnum, **kwargs +) -> OpenCVCameraModel: """Create a OpenCV camera model using a factory design pattern.""" classes_dic = { OpenCVCameraModelEnum.OPENCV_POLYNOMIAL: PolynomialOpenCVCameraModel, OpenCVCameraModelEnum.OPENCV_RATIONAL: RationalOpenCVCameraModel, - OpenCVCameraModelEnum.OPENCV_PRISM: PrismOpenCVCameraModel + OpenCVCameraModelEnum.OPENCV_PRISM: PrismOpenCVCameraModel, } return classes_dic[camera_model_type](**kwargs) @@ -40,6 +44,6 @@ def make_ceres_camera_model(camera_model_type: CeresCameraModelEnum, **kwargs) - """Create a Ceres camera model using a factory design pattern.""" classes_dic = { CeresCameraModelEnum.CERES_POLYNOMIAL: PolynomialCeresCameraModel, - CeresCameraModelEnum.CERES_RATIONAL: RationalCeresCameraModel + CeresCameraModelEnum.CERES_RATIONAL: RationalCeresCameraModel, } return classes_dic[camera_model_type](**kwargs) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index 7880f960..b2ea023e 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -17,20 +17,24 @@ from typing import List from typing import Optional -import cv2 -import numpy as np - from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate +import cv2 from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PolynomialOpenCVCameraModel +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import ( + PolynomialOpenCVCameraModel, +) from intrinsic_camera_calibrator.camera_models.opencv_camera_model import RationalOpenCVCameraModel from intrinsic_camera_calibrator.parameter import Parameter +import numpy as np class CeresCameraModelEnum(CameraModelEnum): - CERES_POLYNOMIAL = {"name": "ceres_polynomial", - "display": "Ceres Polynomial", "calibrator": "ceres"} + CERES_POLYNOMIAL = { + "name": "ceres_polynomial", + "display": "Ceres Polynomial", + "calibrator": "ceres", + } CERES_RATIONAL = {"name": "ceres_rational", "display": "Ceres Rational", "calibrator": "ceres"} @@ -42,7 +46,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) @@ -51,9 +55,7 @@ def _init_calibrate_impl(self, **kwargs): raise NotImplementedError def _calibrate_impl( - self, - object_points_list: List[np.array], - image_points_list: List[np.array] + self, object_points_list: List[np.array], image_points_list: List[np.array] ): """Calibrate Ceres camera model.""" camera_model = self._init_calibrate_impl(object_points_list, image_points_list) @@ -66,7 +68,7 @@ def _calibrate_impl( num_radial_coeffs=self.radial_distortion_coefficients, num_rational_coeffs=self.rational_distortion_coefficients, use_tangential_distortion=self.use_tangential_distortion, - verbose=False + verbose=False, ) self.k = camera_matrix @@ -110,7 +112,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) self.radial_distortion_coefficients = 3 @@ -118,9 +120,7 @@ def __init__( self.use_tangential_distortion = False def _init_calibrate_impl( - self, - object_points_list: List[np.array], - image_points_list: List[np.array] + self, object_points_list: List[np.array], image_points_list: List[np.array] ) -> PolynomialOpenCVCameraModel: """Initialize the calibration of the camera model.""" camera_model = PolynomialOpenCVCameraModel() @@ -128,10 +128,10 @@ def _init_calibrate_impl( height=self.height, width=self.width, object_points_list=object_points_list, - image_points_list=image_points_list + image_points_list=image_points_list, ) - if (camera_model.d.size > 5): + if camera_model.d.size > 5: camera_model.d = camera_model.d.reshape(-1, camera_model.d.size)[:, :5] return camera_model @@ -145,7 +145,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) self.radial_distortion_coefficients = 3 @@ -153,9 +153,7 @@ def __init__( self.use_tangential_distortion = True def _init_calibrate_impl( - self, - object_points_list: List[np.array], - image_points_list: List[np.array] + self, object_points_list: List[np.array], image_points_list: List[np.array] ) -> RationalOpenCVCameraModel: """Initialize the calibration of the camera model.""" camera_model = RationalOpenCVCameraModel() @@ -163,10 +161,10 @@ def _init_calibrate_impl( height=self.height, width=self.width, object_points_list=object_points_list, - image_points_list=image_points_list + image_points_list=image_points_list, ) - if (camera_model.d.size > 8): + if camera_model.d.size > 8: camera_model.d = camera_model.d.reshape(-1, camera_model.d.size)[:, :8] return camera_model diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py index 931a5979..271b7ccd 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py @@ -18,16 +18,23 @@ from typing import Optional import cv2 -import numpy as np - from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum from intrinsic_camera_calibrator.parameter import Parameter +import numpy as np class OpenCVCameraModelEnum(CameraModelEnum): - OPENCV_POLYNOMIAL = {"name": "opencv_polynomial", "display": "OpenCV Polynomial", "calibrator": "opencv"} - OPENCV_RATIONAL = {"name": "opencv_rational", "display": "OpenCV Rational", "calibrator": "opencv"} + OPENCV_POLYNOMIAL = { + "name": "opencv_polynomial", + "display": "OpenCV Polynomial", + "calibrator": "opencv", + } + OPENCV_RATIONAL = { + "name": "opencv_rational", + "display": "OpenCV Rational", + "calibrator": "opencv", + } OPENCV_PRISM = {"name": "opencv_prism", "display": "OpenCV Prism", "calibrator": "opencv"} @@ -39,7 +46,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) self.flags = 0 @@ -48,9 +55,7 @@ def __init__( # self.flags |= cv2.CALIB_ZERO_TANGENT_DIST def _calibrate_impl( - self, - object_points_list: List[np.array], - image_points_list: List[np.array] + self, object_points_list: List[np.array], image_points_list: List[np.array] ): """Calibrate OpenCV camera model.""" object_points_list = [ @@ -108,7 +113,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) self.flags |= cv2.CALIB_FIX_K1 @@ -124,7 +129,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) self.flags |= cv2.CALIB_RATIONAL_MODEL @@ -141,7 +146,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) self.flags |= cv2.CALIB_THIN_PRISM_MODEL From e8594131871f1d3f10e9746e8e053ca2e74a639b Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Thu, 7 Nov 2024 00:15:16 +0900 Subject: [PATCH 03/38] fix(intrinsic_camera_calibrator): inverse flag logic Signed-off-by: amadeuszsz --- .../camera_models/opencv_camera_model.py | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py index 271b7ccd..80a2e0d8 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py @@ -116,9 +116,9 @@ def __init__( width: Optional[int] = None, ): super().__init__(k, d, height, width) - self.flags |= cv2.CALIB_FIX_K1 - self.flags |= cv2.CALIB_FIX_K2 - self.flags |= cv2.CALIB_FIX_K3 + self.flags |= cv2.CALIB_FIX_K4 + self.flags |= cv2.CALIB_FIX_K5 + self.flags |= cv2.CALIB_FIX_K6 class RationalOpenCVCameraModel(PolynomialOpenCVCameraModel): @@ -129,13 +129,10 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None, + width: Optional[int] = None ): super().__init__(k, d, height, width) self.flags |= cv2.CALIB_RATIONAL_MODEL - self.flags |= cv2.CALIB_FIX_K4 - self.flags |= cv2.CALIB_FIX_K5 - self.flags |= cv2.CALIB_FIX_K6 class PrismOpenCVCameraModel(RationalOpenCVCameraModel): From d62f37b3c2090aa3673d7423ae2df1eb72169153 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 6 Nov 2024 15:16:03 +0000 Subject: [PATCH 04/38] ci(pre-commit): autofix --- .../camera_models/opencv_camera_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py index 80a2e0d8..10578411 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py @@ -129,7 +129,7 @@ def __init__( k: Optional[np.array] = None, d: Optional[np.array] = None, height: Optional[int] = None, - width: Optional[int] = None + width: Optional[int] = None, ): super().__init__(k, d, height, width) self.flags |= cv2.CALIB_RATIONAL_MODEL From 69433123bf2e94af7c638b138c1a6359dab39779 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 11 Nov 2024 13:34:51 +0900 Subject: [PATCH 05/38] feat(intrinsic_camera_calibrator): handle camera model only in calibration settings Signed-off-by: amadeuszsz --- .../calibrators/calibrator.py | 16 +- .../calibrators/calibrator_factory.py | 9 +- .../calibrators/ceres_calibrator.py | 31 ++-- .../calibrators/opencv_calibrator.py | 39 +++-- .../camera_calibrator.py | 97 +++--------- .../camera_models/camera_model.py | 87 +++++----- .../camera_models/camera_model_factory.py | 31 +--- .../camera_models/ceres_camera_model.py | 148 +++++------------- .../camera_models/opencv_camera_model.py | 112 +++---------- .../intrinsic_camera_calibrator/parameter.py | 11 ++ .../intrinsic_camera_calibrator/utils.py | 9 ++ 11 files changed, 202 insertions(+), 388 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py index fc16ca5e..6e7eb7d6 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py @@ -37,8 +37,6 @@ from intrinsic_camera_calibrator.calibrators.utils import get_entropy from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model from intrinsic_camera_calibrator.data_collector import DataCollector from intrinsic_camera_calibrator.parameter import Parameter from intrinsic_camera_calibrator.parameter import ParameterizedClass @@ -81,17 +79,9 @@ class Calibrator(ParameterizedClass, QObject): partial_calibration_request = Signal(object, object) partial_calibration_results_signal = Signal(object) - def __init__( - self, camera_model_type: CameraModelEnum, lock: threading.RLock, cfg: Optional[Dict] = {} - ): + def __init__(self, lock: threading.RLock, cfg: Optional[Dict] = {}): ParameterizedClass.__init__(self, lock) QObject.__init__(self, None) - self.camera_model_type = camera_model_type - self.model = ( - make_opencv_camera_model(camera_model_type) - if camera_model_type.value["calibrator"] == "opencv" - else make_ceres_camera_model(camera_model_type) - ) self.use_ransac_pre_rejection = Parameter(bool, value=True, min_value=False, max_value=True) self.pre_rejection_iterations = Parameter(int, value=100, min_value=1, max_value=100) @@ -133,6 +123,10 @@ def __init__( self.evaluation_request.connect(self._evaluate) self.partial_calibration_request.connect(self._calibrate_fast) + def get_model_info(self) -> Tuple[Dict, CameraModelEnum]: + """Return the configuration of the camera model.""" + raise NotImplementedError + def _calibrate(self, data_collector: DataCollector): """ General calibration routine. diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py index 1c16e8b7..7afc6d67 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator_factory.py @@ -19,17 +19,12 @@ from intrinsic_camera_calibrator.calibrators.calibrator import CalibratorEnum from intrinsic_camera_calibrator.calibrators.ceres_calibrator import CeresCalibrator from intrinsic_camera_calibrator.calibrators.opencv_calibrator import OpenCVCalibrator -from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum -def make_calibrator( - calibrator_type: CalibratorEnum, camera_model_type: CameraModelEnum, **kwargs -) -> Calibrator: +def make_calibrator(calibrator_type: CalibratorEnum, **kwargs) -> Calibrator: """Create a calibrator using a factory design pattern.""" classes_dic = { CalibratorEnum.OPENCV: OpenCVCalibrator, CalibratorEnum.CERES: CeresCalibrator, } - return classes_dic[calibrator_type](camera_model_type, **kwargs) + return classes_dic[calibrator_type](**kwargs) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index e37d1b87..35bd2878 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -15,35 +15,44 @@ # limitations under the License. -import logging import threading from typing import Dict from typing import List +from typing import Tuple -from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate -import cv2 from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection from intrinsic_camera_calibrator.calibrators.calibrator import Calibrator -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_camera_model from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModel -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum from intrinsic_camera_calibrator.parameter import Parameter -import numpy as np class CeresCalibrator(Calibrator): - def __init__( - self, camera_model_type: CeresCameraModelEnum, lock: threading.RLock, cfg: Dict = {} - ): - super().__init__(camera_model_type, lock, cfg) + def __init__(self, lock: threading.RLock, cfg: Dict = {}): + super().__init__(lock, cfg) + self.radial_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) + self.rational_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) + self.use_tangential_distortion = Parameter( + bool, value=True, min_value=False, max_value=True + ) + self.set_parameters(**cfg) + def get_model_info(self) -> Tuple[Dict, CameraModelEnum]: + return self.get_parameters_values(), CameraModelEnum.CERES + def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraModel: """Implement the calibrator interface.""" height = detections[0].get_image_height() width = detections[0].get_image_width() - camera_model = make_ceres_camera_model(self.camera_model_type) + camera_model = make_camera_model(camera_model_type=CameraModelEnum.CERES) + camera_model.update_config( + radial_distortion_coefficients=self.radial_distortion_coefficients.value, + rational_distortion_coefficients=self.rational_distortion_coefficients.value, + use_tangential_distortion=self.use_tangential_distortion.value, + ) camera_model.calibrate( height=height, width=width, 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 cb4c897d..a074a12b 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 @@ -18,38 +18,49 @@ import threading from typing import Dict from typing import List +from typing import Tuple -import cv2 from intrinsic_camera_calibrator.board_detections.board_detection import BoardDetection from intrinsic_camera_calibrator.calibrators.calibrator import Calibrator -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_camera_model from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum from intrinsic_camera_calibrator.parameter import Parameter class OpenCVCalibrator(Calibrator): """Wrapper of the opencv's camera calibration routine.""" - def __init__( - self, camera_model_type: OpenCVCameraModelEnum, lock: threading.RLock, cfg: Dict = {} - ): - super().__init__(camera_model_type, lock, cfg) - # self.radial_distortion_coefficients = Parameter(int, value=2, min_value=0, max_value=6) - # self.use_tangential_distortion = Parameter( - # bool, value=True, min_value=False, max_value=True - # ) - # 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) + def __init__(self, lock: threading.RLock, cfg: Dict = {}): + super().__init__(lock, cfg) + self.radial_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) + self.rational_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) + self.use_tangential_distortion = Parameter( + bool, value=True, min_value=False, max_value=True + ) + self.enable_prism_model = Parameter(bool, value=True, min_value=False, max_value=True) + 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.set_parameters(**cfg) + def get_model_info(self) -> Tuple[Dict, CameraModelEnum]: + return self.get_parameters_values(), CameraModelEnum.OPENCV + def _calibration_impl(self, detections: List[BoardDetection]) -> OpenCVCameraModel: """Implement the calibrator interface.""" height = detections[0].get_image_height() width = detections[0].get_image_width() - camera_model = make_opencv_camera_model(self.camera_model_type) + camera_model = make_camera_model(camera_model_type=CameraModelEnum.OPENCV) + camera_model.update_config( + radial_distortion_coefficients=self.radial_distortion_coefficients.value, + rational_distortion_coefficients=self.rational_distortion_coefficients.value, + use_tangential_distortion=self.use_tangential_distortion.value, + enable_prism_model=self.enable_prism_model.value, + fix_principal_point=self.fix_principal_point.value, + fix_aspect_ratio=self.fix_aspect_ratio.value, + ) camera_model.calibrate( height=height, width=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 189b8260..9aef5a0d 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 @@ -55,11 +55,7 @@ from intrinsic_camera_calibrator.calibrators.calibrator import CalibratorEnum from intrinsic_camera_calibrator.calibrators.calibrator_factory import make_calibrator from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel -from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_ceres_camera_model -from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_opencv_camera_model -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_camera_model from intrinsic_camera_calibrator.data_collector import CollectionStatus from intrinsic_camera_calibrator.data_collector import DataCollector from intrinsic_camera_calibrator.data_sources.data_source import DataSource @@ -107,11 +103,9 @@ def __init__(self, cfg): self.estimated_fps = 0 self.last_processed_stamp = None - # Available camera models - self.camera_models = {"opencv": OpenCVCameraModelEnum, "ceres": CeresCameraModelEnum} - # Camera models to use normally self.current_camera_model: CameraModel = None + self.current_calibrator_type: CalibratorEnum = None self.pending_partial_calibration = False # Camera model produced via a full calibration @@ -127,9 +121,7 @@ def __init__(self, cfg): self.board_parameters: ParameterizedClass = None self.detector: BoardDetector = None self.data_collector = DataCollector(self.cfg["data_collector"]) - self.calibrator_dict: Dict[CalibratorEnum, Dict[CameraModelEnum, Calibrator]] = defaultdict( - dict - ) + self.calibrator_dict: Dict[CalibratorEnum, Calibrator] = {} self.image_view_mode = ImageViewMode.SOURCE_UNRECTIFIED self.paused = False @@ -143,18 +135,15 @@ def __init__(self, cfg): ): calibrator_cfg = self.cfg["calibration_parameters"] - for camera_model_type in self.camera_models[calibrator_type.value["name"]]: - calibrator = make_calibrator( - calibrator_type, camera_model_type, lock=self.lock, cfg=calibrator_cfg - ) - self.calibrator_dict[calibrator_type][camera_model_type] = calibrator + calibrator = make_calibrator(calibrator_type, lock=self.lock, cfg=calibrator_cfg) + self.calibrator_dict[calibrator_type] = calibrator - calibrator.moveToThread(self.calibration_thread) # comment this to debug - 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 - ) + 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) @@ -337,9 +326,6 @@ def make_calibration_group(self): self.calibrator_type_combobox = QComboBox() self.calibrator_type_combobox.setEnabled(True) - self.camera_model_type_combobox = QComboBox() - self.camera_model_type_combobox.setEnabled(True) - self.calibration_parameters_button = QPushButton("Calibration parameters") self.calibration_button = QPushButton("Calibrate") self.evaluation_button = QPushButton("Evaluate") @@ -362,29 +348,22 @@ def make_calibration_group(self): def on_parameters_view_closed(): self.calibration_parameters_button.setEnabled(True) self.calibrator_type_combobox.setEnabled(True) - self.camera_model_type_combobox.setEnabled(True) def on_parameters_button_clicked(): self.calibrator_type_combobox.setEnabled(False) - self.camera_model_type_combobox.setEnabled(False) self.calibration_parameters_button.setEnabled(False) calibrator_type = self.calibrator_type_combobox.currentData() - camera_model_type = self.camera_model_type_combobox.currentData() - data_collection_parameters_view = ParameterView( - self.calibrator_dict[calibrator_type][camera_model_type] - ) + data_collection_parameters_view = ParameterView(self.calibrator_dict[calibrator_type]) data_collection_parameters_view.closed.connect(on_parameters_view_closed) def on_calibration_clicked(): calibrator_type = self.calibrator_type_combobox.currentData() - camera_model_type = self.camera_model_type_combobox.currentData() - self.calibrator_dict[calibrator_type][camera_model_type].calibration_request.emit( + self.calibrator_dict[calibrator_type].calibration_request.emit( self.data_collector.clone_without_images() ) self.calibrator_type_combobox.setEnabled(False) - self.camera_model_type_combobox.setEnabled(False) self.calibration_parameters_button.setEnabled(False) self.calibration_button.setEnabled(False) self.evaluation_button.setEnabled(False) @@ -393,19 +372,17 @@ def on_calibration_clicked(): def on_evaluation_clicked(): calibrator_type = self.calibrator_type_combobox.currentData() - camera_model_type = self.camera_model_type_combobox.currentData() camera_model = ( self.current_camera_model if self.calibrated_camera_model is None else self.calibrated_camera_model ) - self.calibrator_dict[calibrator_type][camera_model_type].evaluation_request.emit( + self.calibrator_dict[calibrator_type].evaluation_request.emit( self.data_collector.clone_without_images(), camera_model ) self.calibrator_type_combobox.setEnabled(False) - self.camera_model_type_combobox.setEnabled(False) self.calibration_parameters_button.setEnabled(False) self.calibration_button.setEnabled(False) self.evaluation_button.setEnabled(False) @@ -413,32 +390,7 @@ def on_evaluation_clicked(): self.calibration_status_label.setText("Calibration status: evaluating") def on_calibrator_clicked(): - self.camera_model_type_combobox.clear() - calibrator_type = self.calibrator_type_combobox.currentData() - for camera_model_type in self.camera_models[calibrator_type.value["name"]]: - self.camera_model_type_combobox.addItem( - camera_model_type.value["display"], camera_model_type - ) - - if "camera_model_type" in self.cfg: - try: - self.camera_model_type_combobox.setCurrentIndex( - calibrator_type.value["name"] - .from_name(self.cfg["camera_model_type"]) - .get_id() - ) - except Exception as e: - logging.error(f"Invalid camera_model_type: {e}") - else: - self.camera_model_type_combobox.setCurrentIndex(0) - - def on_camera_model_clicked(): - calibrator_type = self.calibrator_type_combobox.currentData() - camera_model_type = self.camera_model_type_combobox.currentData() - if calibrator_type is not None and camera_model_type is not None: - self.current_camera_model = self.calibrator_dict[calibrator_type][ - camera_model_type - ].model + self.current_calibrator_type = self.calibrator_type_combobox.currentData() self.calibration_parameters_button.clicked.connect(on_parameters_button_clicked) @@ -452,7 +404,6 @@ def on_camera_model_clicked(): self.save_button.setEnabled(False) self.calibrator_type_combobox.currentIndexChanged.connect(on_calibrator_clicked) - self.camera_model_type_combobox.currentIndexChanged.connect(on_camera_model_clicked) for calibrator_type in CalibratorEnum: self.calibrator_type_combobox.addItem(calibrator_type.value["display"], calibrator_type) @@ -470,7 +421,6 @@ def on_camera_model_clicked(): calibration_layout = QVBoxLayout() calibration_layout.setAlignment(Qt.AlignTop) calibration_layout.addWidget(self.calibrator_type_combobox) - calibration_layout.addWidget(self.camera_model_type_combobox) calibration_layout.addWidget(self.calibration_parameters_button) calibration_layout.addWidget(self.calibration_button) calibration_layout.addWidget(self.evaluation_button) @@ -728,7 +678,7 @@ def start( if self.operation_mode == OperationMode.EVALUATION: self.calibration_button.setEnabled(False) - self.detector.moveToThread(self.detector_thread) # comment this to debug + self.detector.moveToThread(self.detector_thread) self.detector.detection_results_signal.connect(self.process_detection_results) self.request_image_detection.connect(self.detector.detect) @@ -785,7 +735,6 @@ def process_calibration_results( ) self.calibrator_type_combobox.setEnabled(True) - self.camera_model_type_combobox.setEnabled(True) self.calibration_parameters_button.setEnabled(True) self.calibration_button.setEnabled(True) self.evaluation_button.setEnabled(True) @@ -838,7 +787,6 @@ def process_evaluation_results( ) self.calibrator_type_combobox.setEnabled(True) - self.camera_model_type_combobox.setEnabled(True) self.calibration_parameters_button.setEnabled(True) self.calibration_button.setEnabled(self.operation_mode == OperationMode.CALIBRATION) self.evaluation_button.setEnabled(True) @@ -914,11 +862,11 @@ def process_detection_results(self, img: np.array, detection: BoardDetection, im self.single_shot_reprojection_error_rms_label.setText("Reprojection error (rms):") else: - camera_model = ( - self.current_camera_model - if self.calibrated_camera_model is None - else self.calibrated_camera_model - ) + camera_model_cfg, camera_model_type = self.calibrator_dict[ + self.current_calibrator_type + ].get_model_info() + camera_model = make_camera_model(camera_model_type) + camera_model.update_config(**camera_model_cfg) if self.image_view_type_combobox.currentData() == ImageViewMode.SOURCE_UNRECTIFIED: filter_result = self.data_collector.process_detection( @@ -1082,8 +1030,7 @@ def update_current_camera_model(self): return calibrator_type = self.calibrator_type_combobox.currentData() - camera_model_type = self.camera_model_type_combobox.currentData() - self.calibrator_dict[calibrator_type][camera_model_type].partial_calibration_request.emit( + self.calibrator_dict[calibrator_type].partial_calibration_request.emit( self.data_collector.clone_without_images(), self.current_camera_model ) 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 f642ec44..921019df 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 @@ -15,7 +15,6 @@ # limitations under the License. from enum import Enum -import logging from typing import Dict from typing import List from typing import Optional @@ -26,11 +25,12 @@ class CameraModelEnum(Enum): + OPENCV = {"name": "opencv", "display": "OpenCV"} + CERES = {"name": "ceres", "display": "Ceres"} + @classmethod def from_name(cls, name: str): - """ - Return the enum member that matches the name. - """ + """Return the enum member that matches the name.""" for model in cls: if model.value["name"] == name: return model @@ -38,15 +38,11 @@ def from_name(cls, name: str): @classmethod def from_index(cls, i: int): - """ - Return the enum member by index. - """ + """Return the enum member by index.""" return list(cls)[i] def get_id(self) -> int: - """ - Return the index of the current enum member. - """ + """Return the index of the current enum member.""" return list(self.__class__).index(self) @@ -75,7 +71,7 @@ def __eq__(self, other: "CameraModel") -> bool: and self.width == other.width and (self.k == other.k).all() and (self.d == other.d).all() - and type(self) == type(other) + and isinstance(self, type(other)) ) def calibrate( @@ -89,9 +85,6 @@ def calibrate( assert len(object_points_list) == len(image_points_list) self.height = height self.width = width - logging.warning( - f"Camera model: {self.__class__.__name__}" - ) # TODO(amadeuszsz): remove before opening PR self._calibrate_impl(object_points_list, image_points_list) def get_pose( @@ -205,46 +198,50 @@ def from_dict(self, d): d["distortion_model"]["rows"], d["distortion_model"]["cols"] ) + def update_config(self, **kwargs): + """Update the camera model configuration.""" + self._update_config_impl(**kwargs) + + def _get_undistorted_camera_model_impl(self, alpha: float): + """Compute the undistorted version of the camera model.""" + undistorted_k, _ = cv2.getOptimalNewCameraMatrix( + self.k, self.d, (self.width, self.height), alpha + ) + + return type(self)( + k=undistorted_k, d=np.zeros_like(self.d), height=self.height, width=self.width + ) + + def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: + """Rectifies an image using the current camera model. Alpha is a value in the [0,1] range to regulate how the rectified image is cropped. 0 means that all the pixels in the rectified image are valid whereas 1 keeps all the original pixels from the unrectified image into the rectifies one, filling with zeroes the invalid pixels.""" + if np.abs(self.d).sum() == 0: + return img + + if self._cached_undistorted_model is None or alpha != self._cached_undistortion_alpha: + self._cached_undistortion_alpha = alpha + self._cached_undistorted_model = self.get_undistorted_camera_model(alpha=alpha) + ( + self._cached_undistortion_map_x, + self._cached_undistortion_map_y, + ) = cv2.initUndistortRectifyMap( + self.k, self.d, None, self._cached_undistorted_model.k, (self.width, self.height), 5 + ) + + return cv2.remap( + img, self._cached_undistortion_map_x, self._cached_undistortion_map_y, cv2.INTER_LINEAR + ) + def _calibrate_impl( self, object_points_list: List[np.array], image_points_list: List[np.array] ): """Abstract method to calibrate the camera model.""" raise NotImplementedError - def _get_undistorted_camera_model_impl(self, alpha: float): - """Abstract method to compute the undistorted version of the camera model.""" - raise NotImplementedError - - def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: - """Abstract method to rectify the input image.""" + def _update_config_impl(self, **kwargs): + """Abstract method to update the camera model configuration.""" raise NotImplementedError -#################################################################################################### -# def _calibrate_impl( -# self, -# object_points_list: List[np.array], -# image_points_list: List[np.array] -# ): -# """Calibrate OpenCV camera model.""" - -# object_points_list = [ -# object_points.astype(np.float32).reshape(-1, 3) for object_points in object_points_list -# ] -# image_points_list = [ -# image_points.astype(np.float32).reshape(-1, 1, 2) for image_points in image_points_list -# ] - -# _, self.k, self.d, rvecs, tvecs = cv2.calibrateCamera( -# object_points_list, -# image_points_list, -# (self.width, self.height), -# cameraMatrix=None, -# distCoeffs=None -# ) -#################################################################################################### - - class CameraModelWithBoardDistortion(CameraModel): """An slightly improves model that also incorporates the distortion/bending of the calibration board..""" diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py index 91c0b605..1703952f 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/camera_model_factory.py @@ -15,35 +15,16 @@ # limitations under the License. +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel +from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModel -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import CeresCameraModelEnum -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import PolynomialCeresCameraModel -from intrinsic_camera_calibrator.camera_models.ceres_camera_model import RationalCeresCameraModel -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import ( - PolynomialOpenCVCameraModel, -) from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import PrismOpenCVCameraModel -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import RationalOpenCVCameraModel -def make_opencv_camera_model( - camera_model_type: OpenCVCameraModelEnum, **kwargs -) -> OpenCVCameraModel: - """Create a OpenCV camera model using a factory design pattern.""" +def make_camera_model(camera_model_type: CameraModelEnum, **kwargs) -> CameraModel: + """Create a camera model using a factory design pattern.""" classes_dic = { - OpenCVCameraModelEnum.OPENCV_POLYNOMIAL: PolynomialOpenCVCameraModel, - OpenCVCameraModelEnum.OPENCV_RATIONAL: RationalOpenCVCameraModel, - OpenCVCameraModelEnum.OPENCV_PRISM: PrismOpenCVCameraModel, - } - return classes_dic[camera_model_type](**kwargs) - - -def make_ceres_camera_model(camera_model_type: CeresCameraModelEnum, **kwargs) -> CeresCameraModel: - """Create a Ceres camera model using a factory design pattern.""" - classes_dic = { - CeresCameraModelEnum.CERES_POLYNOMIAL: PolynomialCeresCameraModel, - CeresCameraModelEnum.CERES_RATIONAL: RationalCeresCameraModel, + CameraModelEnum.OPENCV: OpenCVCameraModel, + CameraModelEnum.CERES: CeresCameraModel, } return classes_dic[camera_model_type](**kwargs) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index b2ea023e..b96d96be 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -18,26 +18,11 @@ from typing import Optional from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate -import cv2 from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel -from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import ( - PolynomialOpenCVCameraModel, -) -from intrinsic_camera_calibrator.camera_models.opencv_camera_model import RationalOpenCVCameraModel -from intrinsic_camera_calibrator.parameter import Parameter +from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel import numpy as np -class CeresCameraModelEnum(CameraModelEnum): - CERES_POLYNOMIAL = { - "name": "ceres_polynomial", - "display": "Ceres Polynomial", - "calibrator": "ceres", - } - CERES_RATIONAL = {"name": "ceres_rational", "display": "Ceres Rational", "calibrator": "ceres"} - - class CeresCameraModel(CameraModel): """Basic Ceres camera model class.""" @@ -49,16 +34,38 @@ def __init__( width: Optional[int] = None, ): super().__init__(k, d, height, width) + self.radial_distortion_coefficients = 3 + self.rational_distortion_coefficients = 3 + self.use_tangential_distortion = True + + def init_calibrate( + self, object_points_list: List[np.array], image_points_list: List[np.array] + ) -> OpenCVCameraModel: + """Init calibrate of Ceres camera model.""" + camera_model = OpenCVCameraModel() + camera_model.update_config( + radial_distortion_coefficients=self.radial_distortion_coefficients, + rational_distortion_coefficients=self.rational_distortion_coefficients, + use_tangential_distortion=self.use_tangential_distortion, + ) + camera_model.calibrate( + height=self.height, + width=self.width, + object_points_list=object_points_list, + image_points_list=image_points_list, + ) + + num_coeffs = 5 if self.rational_distortion_coefficients == 0 else 8 + if camera_model.d.size > num_coeffs: + camera_model.d = camera_model.d.reshape(-1, camera_model.d.size)[:, :num_coeffs] - def _init_calibrate_impl(self, **kwargs): - """Abstract method to initial calibration of the Ceres camera model.""" - raise NotImplementedError + return camera_model def _calibrate_impl( self, object_points_list: List[np.array], image_points_list: List[np.array] ): """Calibrate Ceres camera model.""" - camera_model = self._init_calibrate_impl(object_points_list, image_points_list) + camera_model = self.init_calibrate(object_points_list, image_points_list) rms_error, camera_matrix, dist_coeffs, rvecs, tvecs = calibrate( object_points_list=object_points_list, @@ -74,97 +81,14 @@ def _calibrate_impl( self.k = camera_matrix self.d = dist_coeffs - def _get_undistorted_camera_model_impl(self, alpha: float): - """Compute the undistorted version of the camera model.""" - undistorted_k, _ = cv2.getOptimalNewCameraMatrix( - self.k, self.d, (self.width, self.height), alpha - ) - - return CameraModel( - k=undistorted_k, d=np.zeros_like(self.d), height=self.height, width=self.width - ) - - def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: - """Rectifies an image using the current camera model. Alpha is a value in the [0,1] range to regulate how the rectified image is cropped. 0 means that all the pixels in the rectified image are valid whereas 1 keeps all the original pixels from the unrectified image into the rectifies one, filling with zeroes the invalid pixels.""" - if np.abs(self.d).sum() == 0: - return img - - if self._cached_undistorted_model is None or alpha != self._cached_undistortion_alpha: - self._cached_undistortion_alpha = alpha - self._cached_undistorted_model = self.get_undistorted_camera_model(alpha=alpha) - ( - self._cached_undistortion_map_x, - self._cached_undistortion_map_y, - ) = cv2.initUndistortRectifyMap( - self.k, self.d, None, self._cached_undistorted_model.k, (self.width, self.height), 5 - ) - - return cv2.remap( - img, self._cached_undistortion_map_x, self._cached_undistortion_map_y, cv2.INTER_LINEAR - ) - - -class PolynomialCeresCameraModel(CeresCameraModel): - """Polynomial Ceres camera model class.""" - - def __init__( - self, - k: Optional[np.array] = None, - d: Optional[np.array] = None, - height: Optional[int] = None, - width: Optional[int] = None, - ): - super().__init__(k, d, height, width) - self.radial_distortion_coefficients = 3 - self.rational_distortion_coefficients = 0 - self.use_tangential_distortion = False - - def _init_calibrate_impl( - self, object_points_list: List[np.array], image_points_list: List[np.array] - ) -> PolynomialOpenCVCameraModel: - """Initialize the calibration of the camera model.""" - camera_model = PolynomialOpenCVCameraModel() - camera_model.calibrate( - height=self.height, - width=self.width, - object_points_list=object_points_list, - image_points_list=image_points_list, - ) - - if camera_model.d.size > 5: - camera_model.d = camera_model.d.reshape(-1, camera_model.d.size)[:, :5] - - return camera_model - - -class RationalCeresCameraModel(CeresCameraModel): - """Polynomial Ceres camera model class.""" - - def __init__( + def _update_config_impl( self, - k: Optional[np.array] = None, - d: Optional[np.array] = None, - height: Optional[int] = None, - width: Optional[int] = None, + radial_distortion_coefficients: int, + rational_distortion_coefficients: int, + use_tangential_distortion: bool, + **kwargs ): - super().__init__(k, d, height, width) - self.radial_distortion_coefficients = 3 - self.rational_distortion_coefficients = 3 - self.use_tangential_distortion = True - - def _init_calibrate_impl( - self, object_points_list: List[np.array], image_points_list: List[np.array] - ) -> RationalOpenCVCameraModel: - """Initialize the calibration of the camera model.""" - camera_model = RationalOpenCVCameraModel() - camera_model.calibrate( - height=self.height, - width=self.width, - object_points_list=object_points_list, - image_points_list=image_points_list, - ) - - if camera_model.d.size > 8: - camera_model.d = camera_model.d.reshape(-1, camera_model.d.size)[:, :8] - - return camera_model + """Update parameters.""" + self.radial_distortion_coefficients = radial_distortion_coefficients + self.rational_distortion_coefficients = rational_distortion_coefficients + self.use_tangential_distortion = use_tangential_distortion diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py index 10578411..b1476118 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/opencv_camera_model.py @@ -19,25 +19,10 @@ import cv2 from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel -from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum -from intrinsic_camera_calibrator.parameter import Parameter +from intrinsic_camera_calibrator.utils import toggle_flag import numpy as np -class OpenCVCameraModelEnum(CameraModelEnum): - OPENCV_POLYNOMIAL = { - "name": "opencv_polynomial", - "display": "OpenCV Polynomial", - "calibrator": "opencv", - } - OPENCV_RATIONAL = { - "name": "opencv_rational", - "display": "OpenCV Rational", - "calibrator": "opencv", - } - OPENCV_PRISM = {"name": "opencv_prism", "display": "OpenCV Prism", "calibrator": "opencv"} - - class OpenCVCameraModel(CameraModel): """Basic opencv's camera model class.""" @@ -50,9 +35,6 @@ def __init__( ): super().__init__(k, d, height, width) self.flags = 0 - # self.flags |= cv2.CALIB_FIX_PRINCIPAL_POINT - # self.flags |= cv2.CALIB_FIX_ASPECT_RATIO - # self.flags |= cv2.CALIB_ZERO_TANGENT_DIST def _calibrate_impl( self, object_points_list: List[np.array], image_points_list: List[np.array] @@ -75,75 +57,29 @@ def _calibrate_impl( ) pass - def _get_undistorted_camera_model_impl(self, alpha: float): - """Compute the undistorted version of the camera model.""" - undistorted_k, _ = cv2.getOptimalNewCameraMatrix( - self.k, self.d, (self.width, self.height), alpha - ) - - return CameraModel( # TODO(amadeuszsz): handle top class properly - k=undistorted_k, d=np.zeros_like(self.d), height=self.height, width=self.width - ) - - def _rectify_impl(self, img: np.array, alpha=0.0) -> np.array: - """Rectifies an image using the current camera model. Alpha is a value in the [0,1] range to regulate how the rectified image is cropped. 0 means that all the pixels in the rectified image are valid whereas 1 keeps all the original pixels from the unrectified image into the rectifies one, filling with zeroes the invalid pixels.""" - if np.abs(self.d).sum() == 0: - return img - - if self._cached_undistorted_model is None or alpha != self._cached_undistortion_alpha: - self._cached_undistortion_alpha = alpha - self._cached_undistorted_model = self.get_undistorted_camera_model(alpha=alpha) - ( - self._cached_undistortion_map_x, - self._cached_undistortion_map_y, - ) = cv2.initUndistortRectifyMap( - self.k, self.d, None, self._cached_undistorted_model.k, (self.width, self.height), 5 - ) - - return cv2.remap( - img, self._cached_undistortion_map_x, self._cached_undistortion_map_y, cv2.INTER_LINEAR - ) - - -class PolynomialOpenCVCameraModel(OpenCVCameraModel): - """Polynomial OpenCV camera model class.""" - - def __init__( + def _update_config_impl( self, - k: Optional[np.array] = None, - d: Optional[np.array] = None, - height: Optional[int] = None, - width: Optional[int] = None, + radial_distortion_coefficients: int, + rational_distortion_coefficients: int, + use_tangential_distortion: bool, + enable_prism_model: bool = False, + fix_principal_point: bool = False, + fix_aspect_ratio: bool = False, + **kwargs ): - super().__init__(k, d, height, width) - self.flags |= cv2.CALIB_FIX_K4 - self.flags |= cv2.CALIB_FIX_K5 - self.flags |= cv2.CALIB_FIX_K6 - - -class RationalOpenCVCameraModel(PolynomialOpenCVCameraModel): - """Rational OpenCV camera model class.""" - - def __init__( - self, - k: Optional[np.array] = None, - d: Optional[np.array] = None, - height: Optional[int] = None, - width: Optional[int] = None, - ): - super().__init__(k, d, height, width) - self.flags |= cv2.CALIB_RATIONAL_MODEL - - -class PrismOpenCVCameraModel(RationalOpenCVCameraModel): - """Prism OpenCV camera model class.""" + """Update parameters.""" + for idx, k in enumerate([cv2.CALIB_FIX_K1, cv2.CALIB_FIX_K2, cv2.CALIB_FIX_K3]): + self.flags = toggle_flag(self.flags, k, not (idx < radial_distortion_coefficients)) + + for idx, k in enumerate([cv2.CALIB_FIX_K4, cv2.CALIB_FIX_K5, cv2.CALIB_FIX_K6]): + self.flags = toggle_flag(self.flags, k, not (idx < rational_distortion_coefficients)) + self.flags = toggle_flag( + self.flags, cv2.CALIB_RATIONAL_MODEL, rational_distortion_coefficients > 0 + ) - def __init__( - self, - k: Optional[np.array] = None, - d: Optional[np.array] = None, - height: Optional[int] = None, - width: Optional[int] = None, - ): - super().__init__(k, d, height, width) - self.flags |= cv2.CALIB_THIN_PRISM_MODEL + self.flags = toggle_flag(self.flags, cv2.CALIB_THIN_PRISM_MODEL, enable_prism_model) + self.flags = toggle_flag(self.flags, cv2.CALIB_FIX_PRINCIPAL_POINT, fix_principal_point) + self.flags = toggle_flag(self.flags, cv2.CALIB_FIX_ASPECT_RATIO, fix_aspect_ratio) + self.flags = toggle_flag( + self.flags, cv2.CALIB_ZERO_TANGENT_DIST, not use_tangential_distortion + ) 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..0c687af8 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 get_parameters_values(self) -> dict: + """Return the values of the Parameter objects 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/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py index ebb3d01a..2515184c 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py @@ -58,3 +58,12 @@ def load_intrinsics(file_path: str): camera_model.from_dict(data) return camera_model + + +def toggle_flag(flags: int, flag: int, state: bool) -> int: + """Toogle flag.""" + if state: + flags |= flag + else: + flags &= ~flag + return flags From b26671071f8a0bcc620172fdd35b0de075c9fbe4 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 11 Nov 2024 15:39:05 +0900 Subject: [PATCH 06/38] fix(intrinsic_camera_calibrator): use desired camera model Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/calibrators/calibrator.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py index 6e7eb7d6..30d686cf 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py @@ -37,6 +37,7 @@ from intrinsic_camera_calibrator.calibrators.utils import get_entropy from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.camera_model import CameraModelEnum +from intrinsic_camera_calibrator.camera_models.camera_model_factory import make_camera_model from intrinsic_camera_calibrator.data_collector import DataCollector from intrinsic_camera_calibrator.parameter import Parameter from intrinsic_camera_calibrator.parameter import ParameterizedClass @@ -463,7 +464,10 @@ def _pre_rejection_filter_impl( detection.get_flattened_image_points() for detection in sampled_detections ] - model = CameraModel() + model_cfg, model_type = self.get_model_info() + model = make_camera_model(model_type) + model.update_config(**model_cfg) + model.calibrate( height=height, width=width, From 26723a46b512e9694665e86e6ada5d69665b662f Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 11 Nov 2024 17:43:15 +0900 Subject: [PATCH 07/38] feat(intrinsic_camera_calibrator): use limited num of frames in init calibration for Ceres Signed-off-by: amadeuszsz --- .../calibrators/ceres_calibrator.py | 2 ++ .../camera_models/ceres_camera_model.py | 19 +++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 35bd2878..c9d892cd 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -36,6 +36,7 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): self.use_tangential_distortion = Parameter( bool, value=True, min_value=False, max_value=True ) + self.opencv_calibration_num_frames = Parameter(int, value=5, min_value=0, max_value=20) self.set_parameters(**cfg) @@ -52,6 +53,7 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraMode radial_distortion_coefficients=self.radial_distortion_coefficients.value, rational_distortion_coefficients=self.rational_distortion_coefficients.value, use_tangential_distortion=self.use_tangential_distortion.value, + opencv_calibration_num_frames=self.opencv_calibration_num_frames.value, ) camera_model.calibrate( height=height, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index b96d96be..6749d631 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -37,6 +37,7 @@ def __init__( self.radial_distortion_coefficients = 3 self.rational_distortion_coefficients = 3 self.use_tangential_distortion = True + self.opencv_calibration_num_frames = 5 def init_calibrate( self, object_points_list: List[np.array], image_points_list: List[np.array] @@ -48,11 +49,23 @@ def init_calibrate( rational_distortion_coefficients=self.rational_distortion_coefficients, use_tangential_distortion=self.use_tangential_distortion, ) + + # Consider only part of data (distributed uniformly) + indices = np.round( + np.linspace( + 0, + len(object_points_list) - 1, + np.min([self.opencv_calibration_num_frames, len(object_points_list)]), + ) + ).astype(int) + partial_object_points_list = [object_points_list[i] for i in indices] + partial_image_points_list = [image_points_list[i] for i in indices] + camera_model.calibrate( height=self.height, width=self.width, - object_points_list=object_points_list, - image_points_list=image_points_list, + object_points_list=partial_object_points_list, + image_points_list=partial_image_points_list, ) num_coeffs = 5 if self.rational_distortion_coefficients == 0 else 8 @@ -86,9 +99,11 @@ def _update_config_impl( radial_distortion_coefficients: int, rational_distortion_coefficients: int, use_tangential_distortion: bool, + opencv_calibration_num_frames: int, **kwargs ): """Update parameters.""" self.radial_distortion_coefficients = radial_distortion_coefficients self.rational_distortion_coefficients = rational_distortion_coefficients self.use_tangential_distortion = use_tangential_distortion + self.opencv_calibration_num_frames = opencv_calibration_num_frames From 9acfa23616db077e5b3660f2c148aa0fc08fb5c8 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 12 Nov 2024 12:46:23 +0900 Subject: [PATCH 08/38] fix(intrinsic_camera_calibrator): suppress Ceres factorization warnings Signed-off-by: amadeuszsz --- .../camera_models/ceres_camera_model.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index 6749d631..8c5c1881 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -14,14 +14,19 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os from typing import List from typing import Optional -from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel import numpy as np +os.environ["GLOG_minloglevel"] = "2" # supress ceres factorization warnings +from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import ( # noqa: E402 + calibrate, +) + class CeresCameraModel(CameraModel): """Basic Ceres camera model class.""" From 376a4bb52bc9bb0abab12fe46893231716f8dcb3 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 12 Nov 2024 13:16:28 +0900 Subject: [PATCH 09/38] feat(intrinsic_camera_calibrator): update parameter Signed-off-by: amadeuszsz --- .../calibrators/ceres_calibrator.py | 4 ++-- .../camera_models/ceres_camera_model.py | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index c9d892cd..4a28d121 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -36,7 +36,7 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): self.use_tangential_distortion = Parameter( bool, value=True, min_value=False, max_value=True ) - self.opencv_calibration_num_frames = Parameter(int, value=5, min_value=0, max_value=20) + self.pre_calibration_num_samples = Parameter(int, value=6, min_value=0, max_value=20) self.set_parameters(**cfg) @@ -53,7 +53,7 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraMode radial_distortion_coefficients=self.radial_distortion_coefficients.value, rational_distortion_coefficients=self.rational_distortion_coefficients.value, use_tangential_distortion=self.use_tangential_distortion.value, - opencv_calibration_num_frames=self.opencv_calibration_num_frames.value, + pre_calibration_num_samples=self.pre_calibration_num_samples.value, ) camera_model.calibrate( height=height, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index 8c5c1881..211730e2 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -42,7 +42,7 @@ def __init__( self.radial_distortion_coefficients = 3 self.rational_distortion_coefficients = 3 self.use_tangential_distortion = True - self.opencv_calibration_num_frames = 5 + self.pre_calibration_num_samples = 6 def init_calibrate( self, object_points_list: List[np.array], image_points_list: List[np.array] @@ -60,7 +60,7 @@ def init_calibrate( np.linspace( 0, len(object_points_list) - 1, - np.min([self.opencv_calibration_num_frames, len(object_points_list)]), + np.min([self.pre_calibration_num_samples, len(object_points_list)]), ) ).astype(int) partial_object_points_list = [object_points_list[i] for i in indices] @@ -104,11 +104,11 @@ def _update_config_impl( radial_distortion_coefficients: int, rational_distortion_coefficients: int, use_tangential_distortion: bool, - opencv_calibration_num_frames: int, + pre_calibration_num_samples: int, **kwargs ): """Update parameters.""" self.radial_distortion_coefficients = radial_distortion_coefficients self.rational_distortion_coefficients = rational_distortion_coefficients self.use_tangential_distortion = use_tangential_distortion - self.opencv_calibration_num_frames = opencv_calibration_num_frames + self.pre_calibration_num_samples = pre_calibration_num_samples From d38112ccdb9fee5daa1836aa819fbb2c71bf02d5 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 13 Nov 2024 14:53:19 +0900 Subject: [PATCH 10/38] fix(intrinsic_camera_calibrator): remove redundant parameter Signed-off-by: amadeuszsz --- .../calibrators/utils.py | 26 +++++++------------ 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py index deb08c81..b615c9da 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/utils.py @@ -237,9 +237,7 @@ def process_detections(detections: List[BoardDetection]): return pixel_errors_mean, pixel_errors_std, tilt_errors_mean, tilt_errors_std - def plot_detection_set( - j: int, name: str, detections: List[BoardDetection], camera_model: CameraModel - ): + def plot_detection_set(j: int, name: str, detections: List[BoardDetection]): if len(detections) == 0: return @@ -290,9 +288,7 @@ def plot_detection_set( ) plt.colorbar(tilt_rms_std_ax, ax=axes1[j, 3]) - def plot_calibration_vs_single_shot_calibration( - j, name, detections: List[BoardDetection], camera_model: CameraModel - ): + def plot_calibration_vs_single_shot_calibration(j, name, detections: List[BoardDetection]): label = np.array([str(i) for i in range(len(detections))]) calibrated_errors = np.array( @@ -300,7 +296,7 @@ def plot_calibration_vs_single_shot_calibration( ) single_shot_errors = np.array( [ - np.sqrt(np.power(detection.get_reprojection_errors(camera_model), 2).mean()) + np.sqrt(np.power(detection.get_reprojection_errors(calibrated_model), 2).mean()) for detection in detections ] ) @@ -325,16 +321,12 @@ def plot_calibration_vs_single_shot_calibration( axes2[j].set_xticks(x_axis, label, rotation="vertical") axes2[j].legend() - plot_detection_set(0, "Training", training_detections, calibrated_model) - plot_detection_set(1, "Inliers", inlier_detections, calibrated_model) - plot_detection_set(2, "Evaluation", evaluation_detections, calibrated_model) + plot_detection_set(0, "Training", training_detections) + plot_detection_set(1, "Inliers", inlier_detections) + plot_detection_set(2, "Evaluation", evaluation_detections) - plot_calibration_vs_single_shot_calibration( - 0, "Training", training_detections, calibrated_model - ) - plot_calibration_vs_single_shot_calibration(1, "Inliers", inlier_detections, calibrated_model) - plot_calibration_vs_single_shot_calibration( - 2, "Evaluation", evaluation_detections, calibrated_model - ) + plot_calibration_vs_single_shot_calibration(0, "Training", training_detections) + plot_calibration_vs_single_shot_calibration(1, "Inliers", inlier_detections) + plot_calibration_vs_single_shot_calibration(2, "Evaluation", evaluation_detections) plt.show() From a5def04b04fdc99bce2e939422b7b39bcf2814f2 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 13 Nov 2024 14:56:17 +0900 Subject: [PATCH 11/38] fix(intrinsic_camera_calibrator): remove unused variable Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/calibrators/calibrator.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py index 30d686cf..c8feb101 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/calibrator.py @@ -196,8 +196,6 @@ def _calibrate(self, data_collector: DataCollector): training_post_rejection_inliers = calibration_training_detections evaluation_post_rejection_inliers = raw_evaluation_detections - self.model = calibrated_model - num_training_post_rejection_inliers = len(training_post_rejection_inliers) num_evaluation_post_rejection_inliers = len(evaluation_post_rejection_inliers) From b87a47f24b562cff146442f178c4954f6baa9d7c Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 13 Nov 2024 15:03:11 +0900 Subject: [PATCH 12/38] fix(intrinsic_camera_calibrator): typos, cSpell Signed-off-by: amadeuszsz --- .../camera_models/ceres_camera_model.py | 2 +- .../intrinsic_camera_calibrator/utils.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index 211730e2..a432c60a 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -22,7 +22,7 @@ from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel import numpy as np -os.environ["GLOG_minloglevel"] = "2" # supress ceres factorization warnings +os.environ["GLOG_minloglevel"] = "2" # cSpell:ignore minloglevel from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import ( # noqa: E402 calibrate, ) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py index 2515184c..c4533b31 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py @@ -61,7 +61,6 @@ def load_intrinsics(file_path: str): def toggle_flag(flags: int, flag: int, state: bool) -> int: - """Toogle flag.""" if state: flags |= flag else: From 774dd907d7f2ce6aee26c2f28bb2c1491bb57cf4 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Fri, 15 Nov 2024 19:52:26 +0900 Subject: [PATCH 13/38] feat(intrinsic_camera_calibrator): add coefficients regularization in Ceres Signed-off-by: amadeuszsz --- .../ceres_camera_intrinsics_optimizer.hpp | 7 + .../coefficients_residual.hpp | 142 ++++++++++++++++++ .../src/ceres_camera_intrinsics_optimizer.cpp | 13 ++ .../ceres_intrinsic_camera_calibrator_py.cpp | 10 +- .../src/main.cpp | 12 +- .../calibrators/ceres_calibrator.py | 2 + .../camera_models/ceres_camera_model.py | 4 + 7 files changed, 182 insertions(+), 8 deletions(-) create mode 100644 calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp index 72e69ce0..099922b0 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp @@ -68,6 +68,12 @@ class CeresCameraIntrinsicsOptimizer */ void setRationalDistortionCoefficients(int rational_distortion_coefficients); + /*! + * Sets the regularization weight for the distortion coefficients + * @param[in] regularization_weight the regularization weight + */ + void setRegularizationWeight(double regularization_weight); + /*! * Sets the verbose mode * @param[in] verbose whether or not to use tangential distortion @@ -126,6 +132,7 @@ class CeresCameraIntrinsicsOptimizer int radial_distortion_coefficients_; bool use_tangential_distortion_; int rational_distortion_coefficients_; + double regularization_weight_; bool verbose_; cv::Mat_ camera_matrix_; cv::Mat_ distortion_coeffs_; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp new file mode 100644 index 00000000..6135d322 --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp @@ -0,0 +1,142 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// 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. + +#ifndef CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ +#define CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ + +#include +#include + +#include +#include + +struct CoefficientsResidual +{ + static constexpr int RESIDUAL_DIM = 8; + + CoefficientsResidual( + int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, + int num_samples_factor, double regularization_weight) + { + radial_distortion_coeffs_ = radial_distortion_coeffs; + use_tangential_distortion_ = use_tangential_distortion; + rational_distortion_coeffs_ = rational_distortion_coeffs; + num_samples_factor_ = num_samples_factor; + regularization_weight_ = regularization_weight; + } + + /*! + * The cost function representing the reprojection error + * @param[in] camera_intrinsics The camera intrinsics + * @param[in] residuals The residual error of projecting the tag into the camera + * @returns success status + */ + template + bool operator()(const T * const camera_intrinsics, T * residuals) const + { + const T null_value = T(0.0); + int distortion_index = 4; + const T & k1 = + radial_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value; + const T & k2 = + radial_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value; + const T & k3 = + radial_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; + const T & p1 = use_tangential_distortion_ ? camera_intrinsics[distortion_index++] : null_value; + const T & p2 = use_tangential_distortion_ ? camera_intrinsics[distortion_index++] : null_value; + const T & k4 = + rational_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value; + const T & k5 = + rational_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value; + const T & k6 = + rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; + + residuals[0] = num_samples_factor_ * regularization_weight_ * pow(k1, 2); + residuals[1] = num_samples_factor_ * regularization_weight_ * pow(k2, 2); + residuals[2] = num_samples_factor_ * regularization_weight_ * pow(k3, 2); + residuals[3] = num_samples_factor_ * regularization_weight_ * pow(p1, 2); + residuals[4] = num_samples_factor_ * regularization_weight_ * pow(p2, 2); + residuals[5] = num_samples_factor_ * regularization_weight_ * pow(k4, 2); + residuals[6] = num_samples_factor_ * regularization_weight_ * pow(k5, 2); + residuals[7] = num_samples_factor_ * regularization_weight_ * pow(k6, 2); + + return true; + } + + /*! + * Residual factory method + * @param[in] object_point The object point + * @param[in] image_point The image point + * @param[in] radial_distortion_coeffs The number of radial distortion coefficients + * @param[in] use_tangential_distortion Whether to use or not tangential distortion + * @returns the ceres residual + */ + static ceres::CostFunction * createResidual( + int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, + int num_samples_factor, double regularization_weight) + { + auto f = new CoefficientsResidual( + radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs, + num_samples_factor, regularization_weight); + + int distortion_coefficients = radial_distortion_coeffs + + 2 * static_cast(use_tangential_distortion) + + rational_distortion_coeffs; + ceres::CostFunction * cost_function = nullptr; + + switch (distortion_coefficients) { + case 0: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 1: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 2: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 3: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 4: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 5: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 6: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 7: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 8: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + default: + throw std::runtime_error("Invalid number of distortion coefficients"); + } + + return cost_function; + } + + Eigen::Vector3d object_point_; + Eigen::Vector2d image_point_; + int radial_distortion_coeffs_; + bool use_tangential_distortion_; + int rational_distortion_coeffs_; + int num_samples_factor_; + double regularization_weight_; +}; + +#endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp index 673211a2..fbfcad4b 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -51,6 +52,11 @@ void CeresCameraIntrinsicsOptimizer::setRationalDistortionCoefficients( rational_distortion_coefficients_ = rational_distortion_coefficients; } +void CeresCameraIntrinsicsOptimizer::setRegularizationWeight(double regularization_weight) +{ + regularization_weight_ = regularization_weight; +} + void CeresCameraIntrinsicsOptimizer::setVerbose(bool verbose) { verbose_ = verbose; } void CeresCameraIntrinsicsOptimizer::setData( @@ -355,6 +361,13 @@ void CeresCameraIntrinsicsOptimizer::solve() } } + problem.AddResidualBlock( + CoefficientsResidual::createResidual( + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_, object_points_.size(), regularization_weight_), + nullptr, // L2 + intrinsics_placeholder_.data()); + double initial_cost = 0.0; std::vector residuals; ceres::Problem::EvaluateOptions eval_opt; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index 2a1a16d4..0cd57feb 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -45,7 +45,7 @@ calibrate( const std::vector & image_points_eigen_list, const Eigen::MatrixXd & initial_camera_matrix_eigen, const Eigen::MatrixXd & initial_dist_coeffs_eigen, int num_radial_coeffs, int num_rational_coeffs, - bool use_tangential_distortion, bool verbose) + bool use_tangential_distortion, double regularization_weight, bool verbose) { Eigen::Index num_dist_coeffs = initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols(); @@ -54,7 +54,7 @@ calibrate( initial_camera_matrix_eigen.cols() != 3 || initial_camera_matrix_eigen.rows() != 3 || object_points_eigen_list.size() != image_points_eigen_list.size() || num_radial_coeffs < 0 || num_radial_coeffs > 3 || num_rational_coeffs < 0 || num_rational_coeffs > 3 || - num_dist_coeffs != expected_dist_coeffs) { + num_dist_coeffs != expected_dist_coeffs || regularization_weight < 0.0) { std::cout << "Invalid parameters" << std::endl; std::cout << "\t object_points_list.size(): " << object_points_eigen_list.size() << std::endl; std::cout << "\t image_points_list.size(): " << image_points_eigen_list.size() << std::endl; @@ -63,6 +63,7 @@ calibrate( std::cout << "\t num_radial_coeffs: " << num_radial_coeffs << std::endl; std::cout << "\t num_rational_coeffs: " << num_rational_coeffs << std::endl; std::cout << "\t use_tangential_distortion: " << use_tangential_distortion << std::endl; + std::cout << "\t regularization_weight: " << regularization_weight << std::endl; return std::tuple< double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector, std::vector>(); @@ -121,6 +122,7 @@ calibrate( optimizer.setRadialDistortionCoefficients(num_radial_coeffs); optimizer.setTangentialDistortion(use_tangential_distortion); optimizer.setRationalDistortionCoefficients(num_rational_coeffs); + optimizer.setRegularizationWeight(regularization_weight); optimizer.setVerbose(verbose); optimizer.setData( initial_camera_matrix_cv, initial_dist_coeffs_cv, object_points_list_cv, image_points_list_cv, @@ -182,6 +184,7 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) num_radial_coeffs (int): The number of radial distortion coefficients used during calibration num_rational_coeffs (int): The number of rational distortion coefficients used during calibration use_tangential_distortion (bool): Whether we should use tangential distortion during calibration + regularization_weight (float): The regularization weight for distortion coefficients verbose (bool): Whether we should print debug information Returns: @@ -189,7 +192,8 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) )pbdoc", py::arg("object_points_list"), py::arg("image_points_list"), py::arg("initial_camera_matrix"), py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"), py::arg("num_rational_coeffs"), - py::arg("use_tangential_distortion"), py::arg("verbose") = false); + py::arg("use_tangential_distortion"), py::arg("regularization_weight"), + py::arg("verbose") = false); #ifdef VERSION_INFO m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index 3e9379ef..77700deb 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -30,11 +30,11 @@ int main(int argc, char ** argv) { - if (argc != 5) { - std::cout - << "Usage: " << argv[0] - << " " - << std::endl; + if (argc != 6) { + std::cout << "Usage: " << argv[0] + << " " + " " + << std::endl; return 1; } @@ -46,6 +46,7 @@ int main(int argc, char ** argv) int num_radial_distortion_coeffs = atoi(argv[2]); bool use_tangent_distortion = atoi(argv[3]); int num_rational_distortion_coeffs = atoi(argv[4]); + double regularization_weight = atof(argv[5]); // Placeholders std::vector> all_object_points; @@ -248,6 +249,7 @@ int main(int argc, char ** argv) optimizer.setRadialDistortionCoefficients(num_radial_distortion_coeffs); optimizer.setTangentialDistortion(use_tangent_distortion); optimizer.setRationalDistortionCoefficients(num_rational_distortion_coeffs); + optimizer.setRegularizationWeight(regularization_weight); optimizer.setVerbose(true); optimizer.setData( mini_opencv_camera_matrix, mini_opencv_dist_coeffs, calibration_object_points, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 4a28d121..1d186d52 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -37,6 +37,7 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): bool, value=True, min_value=False, max_value=True ) self.pre_calibration_num_samples = Parameter(int, value=6, min_value=0, max_value=20) + self.regularization_weight = Parameter(float, value=0.001, min_value=0.0, max_value=1.0) self.set_parameters(**cfg) @@ -54,6 +55,7 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraMode rational_distortion_coefficients=self.rational_distortion_coefficients.value, use_tangential_distortion=self.use_tangential_distortion.value, pre_calibration_num_samples=self.pre_calibration_num_samples.value, + regularization_weight=self.regularization_weight.value, ) camera_model.calibrate( height=height, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index a432c60a..91ecec4f 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -43,6 +43,7 @@ def __init__( self.rational_distortion_coefficients = 3 self.use_tangential_distortion = True self.pre_calibration_num_samples = 6 + self.regularization_weight = 0.001 def init_calibrate( self, object_points_list: List[np.array], image_points_list: List[np.array] @@ -93,6 +94,7 @@ def _calibrate_impl( num_radial_coeffs=self.radial_distortion_coefficients, num_rational_coeffs=self.rational_distortion_coefficients, use_tangential_distortion=self.use_tangential_distortion, + regularization_weight=self.regularization_weight, verbose=False, ) @@ -105,6 +107,7 @@ def _update_config_impl( rational_distortion_coefficients: int, use_tangential_distortion: bool, pre_calibration_num_samples: int, + regularization_weight: float, **kwargs ): """Update parameters.""" @@ -112,3 +115,4 @@ def _update_config_impl( self.rational_distortion_coefficients = rational_distortion_coefficients self.use_tangential_distortion = use_tangential_distortion self.pre_calibration_num_samples = pre_calibration_num_samples + self.regularization_weight = regularization_weight From abf6bfdb65f4c7c6750ce64a2600696883bff694 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 18 Nov 2024 10:09:37 +0900 Subject: [PATCH 14/38] fix(ceres_intrinsic_camera_calibrator): remove unused variables Signed-off-by: amadeuszsz --- .../ceres_intrinsic_camera_calibrator/coefficients_residual.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp index 6135d322..21abd09d 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp @@ -130,8 +130,6 @@ struct CoefficientsResidual return cost_function; } - Eigen::Vector3d object_point_; - Eigen::Vector2d image_point_; int radial_distortion_coeffs_; bool use_tangential_distortion_; int rational_distortion_coeffs_; From 57e34987aaa31017efaf1e8cfd78af1ae878e166 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 18 Nov 2024 10:18:18 +0900 Subject: [PATCH 15/38] docs(ceres_intrinsic_camera_calibrator): add missing docs Signed-off-by: amadeuszsz --- .../coefficients_residual.hpp | 5 +++-- .../reprojection_residual.hpp | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp index 21abd09d..7112d8c0 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp @@ -76,10 +76,11 @@ struct CoefficientsResidual /*! * Residual factory method - * @param[in] object_point The object point - * @param[in] image_point The image point * @param[in] radial_distortion_coeffs The number of radial distortion coefficients * @param[in] use_tangential_distortion Whether to use or not tangential distortion + * @param[in] rational_distortion_coeffs The number of rational distortion coefficients + * @param[in] num_samples_factor The number of samples used for during optimization + * @param[in] regularization_weight The regularization weight for distortion coefficients * @returns the ceres residual */ static ceres::CostFunction * createResidual( diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp index 82049fa1..c57b2431 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp @@ -122,6 +122,7 @@ struct ReprojectionResidual * @param[in] image_point The image point * @param[in] radial_distortion_coeffs The number of radial distortion coefficients * @param[in] use_tangential_distortion Whether to use or not tangential distortion + * @param[in] rational_distortion_coeffs The number of rational distortion coefficients * @returns the ceres residual */ static ceres::CostFunction * createResidual( From 055aa6c5f43e13789aff6017c0ad8c444c66de7c Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 09:01:50 +0900 Subject: [PATCH 16/38] fix(ceres_intrinsic_camera_calibrator): remove unused headers Signed-off-by: amadeuszsz --- .../coefficients_residual.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp index 7112d8c0..9c3b4ccf 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp @@ -15,9 +15,6 @@ #ifndef CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ #define CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ -#include -#include - #include #include From ea9978619505086ffb1610d81360bb294318134e Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 09:06:41 +0900 Subject: [PATCH 17/38] refactor(ceres_intrinsic_camera_calibrator): change struct name Signed-off-by: amadeuszsz --- ...p => distortion_coefficients_residual.hpp} | 24 +++++++++---------- .../src/ceres_camera_intrinsics_optimizer.cpp | 4 ++-- 2 files changed, 14 insertions(+), 14 deletions(-) rename calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/{coefficients_residual.hpp => distortion_coefficients_residual.hpp} (81%) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp similarity index 81% rename from calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp index 9c3b4ccf..21523aa4 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/coefficients_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp @@ -18,11 +18,11 @@ #include #include -struct CoefficientsResidual +struct DistortionCoefficientsResidual { static constexpr int RESIDUAL_DIM = 8; - CoefficientsResidual( + DistortionCoefficientsResidual( int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, int num_samples_factor, double regularization_weight) { @@ -84,7 +84,7 @@ struct CoefficientsResidual int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, int num_samples_factor, double regularization_weight) { - auto f = new CoefficientsResidual( + auto f = new DistortionCoefficientsResidual( radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs, num_samples_factor, regularization_weight); @@ -95,31 +95,31 @@ struct CoefficientsResidual switch (distortion_coefficients) { case 0: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 1: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 2: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 3: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 4: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 5: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 6: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 7: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; case 8: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = new ceres::AutoDiffCostFunction(f); break; default: throw std::runtime_error("Invalid number of distortion coefficients"); diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp index fbfcad4b..f7451557 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include #include @@ -362,7 +362,7 @@ void CeresCameraIntrinsicsOptimizer::solve() } problem.AddResidualBlock( - CoefficientsResidual::createResidual( + DistortionCoefficientsResidual::createResidual( radial_distortion_coefficients_, use_tangential_distortion_, rational_distortion_coefficients_, object_points_.size(), regularization_weight_), nullptr, // L2 From a560b57ae70eeac237a309597685e1a24f7274ce Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 09:22:44 +0900 Subject: [PATCH 18/38] refactor(ceres_intrinsic_camera_calibrator): remove squaring Signed-off-by: amadeuszsz --- .../distortion_coefficients_residual.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp index 21523aa4..695d92f4 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp @@ -59,14 +59,14 @@ struct DistortionCoefficientsResidual const T & k6 = rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; - residuals[0] = num_samples_factor_ * regularization_weight_ * pow(k1, 2); - residuals[1] = num_samples_factor_ * regularization_weight_ * pow(k2, 2); - residuals[2] = num_samples_factor_ * regularization_weight_ * pow(k3, 2); - residuals[3] = num_samples_factor_ * regularization_weight_ * pow(p1, 2); - residuals[4] = num_samples_factor_ * regularization_weight_ * pow(p2, 2); - residuals[5] = num_samples_factor_ * regularization_weight_ * pow(k4, 2); - residuals[6] = num_samples_factor_ * regularization_weight_ * pow(k5, 2); - residuals[7] = num_samples_factor_ * regularization_weight_ * pow(k6, 2); + residuals[0] = num_samples_factor_ * regularization_weight_ * k1; + residuals[1] = num_samples_factor_ * regularization_weight_ * k2; + residuals[2] = num_samples_factor_ * regularization_weight_ * k3; + residuals[3] = num_samples_factor_ * regularization_weight_ * p1; + residuals[4] = num_samples_factor_ * regularization_weight_ * p2; + residuals[5] = num_samples_factor_ * regularization_weight_ * k4; + residuals[6] = num_samples_factor_ * regularization_weight_ * k5; + residuals[7] = num_samples_factor_ * regularization_weight_ * k6; return true; } From ca7b41c45b9f5dc21ce09fc17fa98bb0643154b6 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 09:24:16 +0900 Subject: [PATCH 19/38] docs(ceres_intrinsic_camera_calibrator): typo Signed-off-by: amadeuszsz --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index 0cd57feb..e3774584 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -184,7 +184,7 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) num_radial_coeffs (int): The number of radial distortion coefficients used during calibration num_rational_coeffs (int): The number of rational distortion coefficients used during calibration use_tangential_distortion (bool): Whether we should use tangential distortion during calibration - regularization_weight (float): The regularization weight for distortion coefficients + regularization_weight (double): The regularization weight for distortion coefficients verbose (bool): Whether we should print debug information Returns: From 184701ce4a34037ce28b397f4e611692f11f35d2 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 09:47:40 +0900 Subject: [PATCH 20/38] refactor(intrinsic_camera_calibrator): use of comprehension Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/parameter.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) 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 0c687af8..411ad382 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 @@ -93,10 +93,4 @@ def parameters(self) -> dict: def get_parameters_values(self) -> dict: """Return the values of the Parameter objects 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 + return {k: v.value for k, v in self.parameters().items()} \ No newline at end of file From 1ad49f1b1f6a38b01b3cdf7e96508469006fafee Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 09:49:58 +0900 Subject: [PATCH 21/38] feat(intrinsic_camera_calibrator): update parameter max value Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/calibrators/ceres_calibrator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 1d186d52..ec72b460 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -36,7 +36,7 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): self.use_tangential_distortion = Parameter( bool, value=True, min_value=False, max_value=True ) - self.pre_calibration_num_samples = Parameter(int, value=6, min_value=0, max_value=20) + self.pre_calibration_num_samples = Parameter(int, value=6, min_value=0, max_value=100) self.regularization_weight = Parameter(float, value=0.001, min_value=0.0, max_value=1.0) self.set_parameters(**cfg) From 2e26b158b248757abd99ce41ca6f9a013c77a6fa Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 17:08:03 +0900 Subject: [PATCH 22/38] refactor(ceres_intrinsic_camera_calibrator): apply regularizaion weight for distortion coefficients via loss function Signed-off-by: amadeuszsz --- .../distortion_coefficients_residual.hpp | 31 +++++++------------ .../src/ceres_camera_intrinsics_optimizer.cpp | 5 +-- 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp index 695d92f4..b4bb5006 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp @@ -23,14 +23,11 @@ struct DistortionCoefficientsResidual static constexpr int RESIDUAL_DIM = 8; DistortionCoefficientsResidual( - int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, - int num_samples_factor, double regularization_weight) + int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs) { radial_distortion_coeffs_ = radial_distortion_coeffs; use_tangential_distortion_ = use_tangential_distortion; rational_distortion_coeffs_ = rational_distortion_coeffs; - num_samples_factor_ = num_samples_factor; - regularization_weight_ = regularization_weight; } /*! @@ -59,14 +56,14 @@ struct DistortionCoefficientsResidual const T & k6 = rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; - residuals[0] = num_samples_factor_ * regularization_weight_ * k1; - residuals[1] = num_samples_factor_ * regularization_weight_ * k2; - residuals[2] = num_samples_factor_ * regularization_weight_ * k3; - residuals[3] = num_samples_factor_ * regularization_weight_ * p1; - residuals[4] = num_samples_factor_ * regularization_weight_ * p2; - residuals[5] = num_samples_factor_ * regularization_weight_ * k4; - residuals[6] = num_samples_factor_ * regularization_weight_ * k5; - residuals[7] = num_samples_factor_ * regularization_weight_ * k6; + residuals[0] = k1; + residuals[1] = k2; + residuals[2] = k3; + residuals[3] = p1; + residuals[4] = p2; + residuals[5] = k4; + residuals[6] = k5; + residuals[7] = k6; return true; } @@ -76,17 +73,13 @@ struct DistortionCoefficientsResidual * @param[in] radial_distortion_coeffs The number of radial distortion coefficients * @param[in] use_tangential_distortion Whether to use or not tangential distortion * @param[in] rational_distortion_coeffs The number of rational distortion coefficients - * @param[in] num_samples_factor The number of samples used for during optimization - * @param[in] regularization_weight The regularization weight for distortion coefficients * @returns the ceres residual */ static ceres::CostFunction * createResidual( - int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, - int num_samples_factor, double regularization_weight) + int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs) { auto f = new DistortionCoefficientsResidual( - radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs, - num_samples_factor, regularization_weight); + radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs); int distortion_coefficients = radial_distortion_coeffs + 2 * static_cast(use_tangential_distortion) + @@ -131,8 +124,6 @@ struct DistortionCoefficientsResidual int radial_distortion_coeffs_; bool use_tangential_distortion_; int rational_distortion_coeffs_; - int num_samples_factor_; - double regularization_weight_; }; #endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp index f7451557..e89296d4 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp @@ -364,8 +364,9 @@ void CeresCameraIntrinsicsOptimizer::solve() problem.AddResidualBlock( DistortionCoefficientsResidual::createResidual( radial_distortion_coefficients_, use_tangential_distortion_, - rational_distortion_coefficients_, object_points_.size(), regularization_weight_), - nullptr, // L2 + rational_distortion_coefficients_), + new ceres::ScaledLoss( + nullptr, regularization_weight_ * object_points_.size(), ceres::TAKE_OWNERSHIP), // L2 intrinsics_placeholder_.data()); double initial_cost = 0.0; From 47d0c16ee070724e64fe386f43b47aa45eb3a750 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 17:28:15 +0900 Subject: [PATCH 23/38] fix(intrinsic_camera_calibrator): assign default values only once Signed-off-by: amadeuszsz --- .../calibrators/ceres_calibrator.py | 2 +- .../camera_models/ceres_camera_model.py | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index ec72b460..7c366c14 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -36,7 +36,7 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): self.use_tangential_distortion = Parameter( bool, value=True, min_value=False, max_value=True ) - self.pre_calibration_num_samples = Parameter(int, value=6, min_value=0, max_value=100) + self.pre_calibration_num_samples = Parameter(int, value=6, min_value=1, max_value=100) self.regularization_weight = Parameter(float, value=0.001, min_value=0.0, max_value=1.0) self.set_parameters(**cfg) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index 91ecec4f..47ee2058 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -39,11 +39,11 @@ def __init__( width: Optional[int] = None, ): super().__init__(k, d, height, width) - self.radial_distortion_coefficients = 3 - self.rational_distortion_coefficients = 3 - self.use_tangential_distortion = True - self.pre_calibration_num_samples = 6 - self.regularization_weight = 0.001 + self.radial_distortion_coefficients: int = None + self.rational_distortion_coefficients: int = None + self.use_tangential_distortion: bool = None + self.pre_calibration_num_samples: int = None + self.regularization_weight: float = None def init_calibrate( self, object_points_list: List[np.array], image_points_list: List[np.array] From 3d9b31a191d63e87163b9f28bc2d269d0484511c Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 19 Nov 2024 17:29:35 +0900 Subject: [PATCH 24/38] style(intrinsic_camera_calibrator): pre-commit Signed-off-by: amadeuszsz --- .../distortion_coefficients_residual.hpp | 33 ++++++++++++------- .../intrinsic_camera_calibrator/parameter.py | 2 +- 2 files changed, 22 insertions(+), 13 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp index b4bb5006..78636653 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ -#define CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ +#ifndef CERES_INTRINSIC_CAMERA_CALIBRATOR__DISTORTION_COEFFICIENTS_RESIDUAL_HPP_ +#define CERES_INTRINSIC_CAMERA_CALIBRATOR__DISTORTION_COEFFICIENTS_RESIDUAL_HPP_ #include #include @@ -88,31 +88,40 @@ struct DistortionCoefficientsResidual switch (distortion_coefficients) { case 0: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 1: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 2: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 3: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 4: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 5: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 6: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 7: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; case 8: - cost_function = new ceres::AutoDiffCostFunction(f); + cost_function = + new ceres::AutoDiffCostFunction(f); break; default: throw std::runtime_error("Invalid number of distortion coefficients"); @@ -126,4 +135,4 @@ struct DistortionCoefficientsResidual int rational_distortion_coeffs_; }; -#endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_ +#endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__DISTORTION_COEFFICIENTS_RESIDUAL_HPP_ 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 411ad382..d61f0d1f 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 @@ -93,4 +93,4 @@ def parameters(self) -> dict: def get_parameters_values(self) -> dict: """Return the values of the Parameter objects from this class as a dictionary.""" with self.lock: - return {k: v.value for k, v in self.parameters().items()} \ No newline at end of file + return {k: v.value for k, v in self.parameters().items()} From 34679cb30708feb3e26624c318b5db30ba8f0809 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 09:43:24 +0900 Subject: [PATCH 25/38] feat(intrinsic_camera_calibrator): logging severity Signed-off-by: amadeuszsz --- .../camera_models/ceres_camera_model.py | 9 +++------ .../launch/calibrator.launch.xml | 6 +++++- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index 47ee2058..98c0eee3 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -18,15 +18,11 @@ from typing import List from typing import Optional +from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import calibrate from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel from intrinsic_camera_calibrator.camera_models.opencv_camera_model import OpenCVCameraModel import numpy as np -os.environ["GLOG_minloglevel"] = "2" # cSpell:ignore minloglevel -from ceres_intrinsic_camera_calibrator.ceres_intrinsic_camera_calibrator_py import ( # noqa: E402 - calibrate, -) - class CeresCameraModel(CameraModel): """Basic Ceres camera model class.""" @@ -44,6 +40,7 @@ def __init__( self.use_tangential_distortion: bool = None self.pre_calibration_num_samples: int = None self.regularization_weight: float = None + self.verbose = True if os.getenv("GLOG_minloglevel") == "0" else False def init_calibrate( self, object_points_list: List[np.array], image_points_list: List[np.array] @@ -95,7 +92,7 @@ def _calibrate_impl( num_rational_coeffs=self.rational_distortion_coefficients, use_tangential_distortion=self.use_tangential_distortion, regularization_weight=self.regularization_weight, - verbose=False, + verbose=self.verbose, ) self.k = camera_matrix 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 5814511c..d86b169a 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,6 +1,10 @@ + + - + + + From 693807a7b0b408f5910f048be78a165054879ede Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 09:49:48 +0900 Subject: [PATCH 26/38] fix(intrinsic_camera_calibrator): cspell Signed-off-by: amadeuszsz --- .../camera_models/ceres_camera_model.py | 4 +++- .../intrinsic_camera_calibrator/launch/calibrator.launch.xml | 3 ++- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index 98c0eee3..c19e53cb 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -40,7 +40,9 @@ def __init__( self.use_tangential_distortion: bool = None self.pre_calibration_num_samples: int = None self.regularization_weight: float = None - self.verbose = True if os.getenv("GLOG_minloglevel") == "0" else False + self.verbose = ( + True if os.getenv("GLOG_minloglevel") == "0" else False + ) # cSpell:ignore minloglevel def init_calibrate( self, object_points_list: List[np.array], image_points_list: List[np.array] 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 d86b169a..056b83f0 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,10 +1,11 @@ - + + From 237330de94fb6dd773c9511270f002aac733b6ff Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 09:57:17 +0900 Subject: [PATCH 27/38] fix(intrinsic_camera_calibrator): unecessary lock Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/parameter.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 d61f0d1f..b36ac231 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 @@ -92,5 +92,4 @@ def parameters(self) -> dict: def get_parameters_values(self) -> dict: """Return the values of the Parameter objects from this class as a dictionary.""" - with self.lock: - return {k: v.value for k, v in self.parameters().items()} + return {k: v.value for k, v in self.parameters().items()} From e5a1de6b5896d904da39babd9618356680951e8e Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 10:32:01 +0900 Subject: [PATCH 28/38] feat(intrinsic_camera_calibrator): update default parameter value Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/calibrators/ceres_calibrator.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 7c366c14..ade8b942 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -36,7 +36,7 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): self.use_tangential_distortion = Parameter( bool, value=True, min_value=False, max_value=True ) - self.pre_calibration_num_samples = Parameter(int, value=6, min_value=1, max_value=100) + self.pre_calibration_num_samples = Parameter(int, value=40, min_value=1, max_value=100) self.regularization_weight = Parameter(float, value=0.001, min_value=0.0, max_value=1.0) self.set_parameters(**cfg) From b6168317810cfd8abf813aa2d7598b9940111530 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 10:37:19 +0900 Subject: [PATCH 29/38] style(intrinsic_camera_calibrator): var type Signed-off-by: amadeuszsz --- .../camera_models/ceres_camera_model.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index c19e53cb..c14d65fe 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -35,11 +35,11 @@ def __init__( width: Optional[int] = None, ): super().__init__(k, d, height, width) - self.radial_distortion_coefficients: int = None - self.rational_distortion_coefficients: int = None - self.use_tangential_distortion: bool = None - self.pre_calibration_num_samples: int = None - self.regularization_weight: float = None + self.radial_distortion_coefficients: Optional[int] = None + self.rational_distortion_coefficients: Optional[int] = None + self.use_tangential_distortion: Optional[bool] = None + self.pre_calibration_num_samples: Optional[int] = None + self.regularization_weight: Optional[float] = None self.verbose = ( True if os.getenv("GLOG_minloglevel") == "0" else False ) # cSpell:ignore minloglevel From d966e0376241ef319b1b72fc814da0f36be0eeba Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 10:44:10 +0900 Subject: [PATCH 30/38] fix(intrinsic_camera_calibrator): return correct value Signed-off-by: amadeuszsz --- .../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 94f39434..40b531c3 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 @@ -168,7 +168,7 @@ def get_pose(self, model: CameraModel) -> Tuple[np.array, np.array]: def get_flattened_3d_points(self, model: CameraModel) -> np.array: """Get the image points reprojected into camera coordinates in the 3d space as a (M, 3) array.""" if model == self._cached_camera_model and self._cached_flattened_3d_points is not None: - return self._cached_pose + return self._cached_flattened_3d_points self._cached_camera_model = model From 50d013f5ee80717c808cdc6ddbe7e95256eebbfc Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 12:16:49 +0900 Subject: [PATCH 31/38] fix(intrinsic_camera_calibrator): revert unnecessary change Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/camera_calibrator.py | 3 +-- 1 file changed, 1 insertion(+), 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 9aef5a0d..f3a8bb1b 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 @@ -655,8 +655,7 @@ def start( self.data_source = data_source self.board_type = board_type self.board_parameters = board_parameters - if initial_intrinsics is not None: - self.current_camera_model = initial_intrinsics + self.current_camera_model = initial_intrinsics self.setEnabled(True) self.setWindowTitle(f"Camera intrinsics calibrator ({self.data_source.get_camera_name()})") From 76f361388c16eb5663527f167a48557b47c1685e Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 12:32:25 +0900 Subject: [PATCH 32/38] fix(intrinsic_camera_calibrator): mark unused variables Signed-off-by: amadeuszsz --- .../camera_models/ceres_camera_model.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index c14d65fe..a36e3b94 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -85,7 +85,7 @@ def _calibrate_impl( """Calibrate Ceres camera model.""" camera_model = self.init_calibrate(object_points_list, image_points_list) - rms_error, camera_matrix, dist_coeffs, rvecs, tvecs = calibrate( + _, camera_matrix, dist_coeffs, _, _ = calibrate( object_points_list=object_points_list, image_points_list=image_points_list, initial_camera_matrix=camera_model.k, From 494fcb46736c3c6c5d7062ac420678a76e18eb77 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 20 Nov 2024 13:09:32 +0900 Subject: [PATCH 33/38] feat(intrinsic_camera_calibrator): update default value Signed-off-by: amadeuszsz --- .../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 75998a8d..745f4745 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 @@ -242,7 +242,7 @@ def __init__(self, cfg: dict = dict(), **kwargs): # noqa C408 # Other criteria for new samples is using 3d statistics # They have the advantage of considering the different out-of-plane rotations instead of a single scalar (e.g., differentiation of left and right rotations) - self.filter_by_3d_redundancy = Parameter(bool, value=True, min_value=False, max_value=True) + self.filter_by_3d_redundancy = Parameter(bool, value=False, min_value=False, max_value=True) self.min_3d_center_difference = Parameter(float, value=1.0, min_value=0.1, max_value=100.0) self.min_tilt_difference = Parameter(float, value=15.0, min_value=0.0, max_value=90) From 73eb77bd6fbfb12e67a5707eeddc1082e8fe91d1 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 25 Nov 2024 15:38:53 +0900 Subject: [PATCH 34/38] fix(intrinsic_camera_calibrator): lock thread while read parameters Signed-off-by: amadeuszsz --- .../calibrators/ceres_calibrator.py | 18 +++++++++-------- .../calibrators/opencv_calibrator.py | 20 ++++++++++--------- 2 files changed, 21 insertions(+), 17 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index ade8b942..50470e35 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -42,7 +42,8 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): self.set_parameters(**cfg) def get_model_info(self) -> Tuple[Dict, CameraModelEnum]: - return self.get_parameters_values(), CameraModelEnum.CERES + with self.lock: + return self.get_parameters_values(), CameraModelEnum.CERES def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraModel: """Implement the calibrator interface.""" @@ -50,13 +51,14 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraMode width = detections[0].get_image_width() camera_model = make_camera_model(camera_model_type=CameraModelEnum.CERES) - camera_model.update_config( - radial_distortion_coefficients=self.radial_distortion_coefficients.value, - rational_distortion_coefficients=self.rational_distortion_coefficients.value, - use_tangential_distortion=self.use_tangential_distortion.value, - pre_calibration_num_samples=self.pre_calibration_num_samples.value, - regularization_weight=self.regularization_weight.value, - ) + with self.lock: + camera_model.update_config( + radial_distortion_coefficients=self.radial_distortion_coefficients.value, + rational_distortion_coefficients=self.rational_distortion_coefficients.value, + use_tangential_distortion=self.use_tangential_distortion.value, + pre_calibration_num_samples=self.pre_calibration_num_samples.value, + regularization_weight=self.regularization_weight.value, + ) camera_model.calibrate( height=height, width=width, 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 a074a12b..1b002387 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 @@ -45,7 +45,8 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}): self.set_parameters(**cfg) def get_model_info(self) -> Tuple[Dict, CameraModelEnum]: - return self.get_parameters_values(), CameraModelEnum.OPENCV + with self.lock: + return self.get_parameters_values(), CameraModelEnum.OPENCV def _calibration_impl(self, detections: List[BoardDetection]) -> OpenCVCameraModel: """Implement the calibrator interface.""" @@ -53,14 +54,15 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> OpenCVCameraMod width = detections[0].get_image_width() camera_model = make_camera_model(camera_model_type=CameraModelEnum.OPENCV) - camera_model.update_config( - radial_distortion_coefficients=self.radial_distortion_coefficients.value, - rational_distortion_coefficients=self.rational_distortion_coefficients.value, - use_tangential_distortion=self.use_tangential_distortion.value, - enable_prism_model=self.enable_prism_model.value, - fix_principal_point=self.fix_principal_point.value, - fix_aspect_ratio=self.fix_aspect_ratio.value, - ) + with self.lock: + camera_model.update_config( + radial_distortion_coefficients=self.radial_distortion_coefficients.value, + rational_distortion_coefficients=self.rational_distortion_coefficients.value, + use_tangential_distortion=self.use_tangential_distortion.value, + enable_prism_model=self.enable_prism_model.value, + fix_principal_point=self.fix_principal_point.value, + fix_aspect_ratio=self.fix_aspect_ratio.value, + ) camera_model.calibrate( height=height, width=width, From 8d990f011f2b20de69de4e41d1e76b24c67bf7f2 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 25 Nov 2024 16:26:17 +0900 Subject: [PATCH 35/38] feat(intrinsic_camera_calibrator): update default parameters Signed-off-by: amadeuszsz --- .../calibrators/ceres_calibrator.py | 4 ++-- .../calibrators/opencv_calibrator.py | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 50470e35..124eb5a9 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -31,8 +31,8 @@ class CeresCalibrator(Calibrator): def __init__(self, lock: threading.RLock, cfg: Dict = {}): super().__init__(lock, cfg) - self.radial_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) - self.rational_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) + self.radial_distortion_coefficients = Parameter(int, value=2, min_value=0, max_value=3) + self.rational_distortion_coefficients = Parameter(int, value=0, min_value=0, max_value=3) self.use_tangential_distortion = Parameter( bool, value=True, min_value=False, max_value=True ) 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 1b002387..b1c06bdd 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 @@ -33,12 +33,12 @@ class OpenCVCalibrator(Calibrator): def __init__(self, lock: threading.RLock, cfg: Dict = {}): super().__init__(lock, cfg) - self.radial_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) - self.rational_distortion_coefficients = Parameter(int, value=3, min_value=0, max_value=3) + self.radial_distortion_coefficients = Parameter(int, value=2, min_value=0, max_value=3) + self.rational_distortion_coefficients = Parameter(int, value=0, min_value=0, max_value=3) self.use_tangential_distortion = Parameter( bool, value=True, min_value=False, max_value=True ) - self.enable_prism_model = Parameter(bool, value=True, min_value=False, max_value=True) + self.enable_prism_model = Parameter(bool, value=False, min_value=False, max_value=True) 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) From 4e3a48d88b47b017c617c1942c46ae8cf2745fd3 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 25 Nov 2024 16:27:19 +0900 Subject: [PATCH 36/38] feat(intrinsic_camera_calibrator): apply logging severity for Python logging Signed-off-by: amadeuszsz --- .../camera_calibrator.py | 2 ++ .../intrinsic_camera_calibrator/utils.py | 16 ++++++++++++++++ 2 files changed, 18 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 f3a8bb1b..d6615029 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 @@ -63,6 +63,7 @@ from intrinsic_camera_calibrator.types import ImageViewMode from intrinsic_camera_calibrator.types import OperationMode from intrinsic_camera_calibrator.utils import save_intrinsics +from intrinsic_camera_calibrator.utils import set_logger_severity from intrinsic_camera_calibrator.views.data_collector_view import DataCollectorView from intrinsic_camera_calibrator.views.image_view import CustomQGraphicsView from intrinsic_camera_calibrator.views.image_view import ImageView @@ -1122,6 +1123,7 @@ def on_parameter_changed(self): def main(args=None): + set_logger_severity() parser = OptionParser() parser.add_option("-c", "--config-file", type="string", help="calibration file path") diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py index c4533b31..dddd4156 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py @@ -14,6 +14,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +import logging +import os + import cv2 from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel import numpy as np @@ -66,3 +69,16 @@ def toggle_flag(flags: int, flag: int, state: bool) -> int: else: flags &= ~flag return flags + + +def set_logger_severity(): + severity = os.getenv("GLOG_minloglevel") + if severity is None: + return + mapping = { + "0": logging.INFO, + "1": logging.WARNING, + "2": logging.ERROR, + "3": logging.CRITICAL, + } + logging.basicConfig(level=mapping.get(severity, logging.INFO)) From ef56973de8a4ba26221ed45ec697963445ab4532 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 25 Nov 2024 16:39:16 +0900 Subject: [PATCH 37/38] feat(intrinsic_camera_calibrator): override default number of calibration samples for Ceres calibrator Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/calibrators/ceres_calibrator.py | 1 + 1 file changed, 1 insertion(+) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 124eb5a9..ad505277 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -31,6 +31,7 @@ class CeresCalibrator(Calibrator): def __init__(self, lock: threading.RLock, cfg: Dict = {}): super().__init__(lock, cfg) + self.max_calibration_samples.value = 500 self.radial_distortion_coefficients = Parameter(int, value=2, min_value=0, max_value=3) self.rational_distortion_coefficients = Parameter(int, value=0, min_value=0, max_value=3) self.use_tangential_distortion = Parameter( From 236fd57216ccf551762bf8d8a8583dd7cee7f105 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 25 Nov 2024 16:45:31 +0900 Subject: [PATCH 38/38] style(intrinsic_camera_calibrator): cspell Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py index dddd4156..450308d0 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/utils.py @@ -72,7 +72,7 @@ def toggle_flag(flags: int, flag: int, state: bool) -> int: def set_logger_severity(): - severity = os.getenv("GLOG_minloglevel") + severity = os.getenv("GLOG_minloglevel") # cSpell:ignore minloglevel if severity is None: return mapping = {