diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index ec8dd4138..ae86828ae 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -45,6 +45,7 @@ import time import sys from distutils.version import LooseVersion +import sys from enum import Enum # Supported camera models @@ -306,6 +307,8 @@ def get_parameters(self, corners, board, size): skew = _get_skew(corners, board) params = [p_x, p_y, p_size, skew] return params + def set_cammodel(self, modeltype): + self.camera_model = modeltype def set_cammodel(self, modeltype): self.camera_model = modeltype @@ -469,16 +472,13 @@ def downsample_and_detect(self, img): return (scrib, corners, downsampled_corners, board, (x_scale, y_scale)) - @staticmethod - def lrmsg(d, k, r, p, size): + def lrmsg(d, k, r, p, size, camera_model): """ Used by :meth:`as_message`. Return a CameraInfo message for the given calibration matrices """ msg = sensor_msgs.msg.CameraInfo() msg.width, msg.height = size - if d.size > 5: - msg.distortion_model = "rational_polynomial" - else: - msg.distortion_model = "plumb_bob" + msg.distortion_model = _get_dist_model(d, camera_model) + msg.d = numpy.ravel(d).copy().tolist() msg.k = numpy.ravel(k).copy().tolist() msg.r = numpy.ravel(r).copy().tolist() @@ -534,13 +534,15 @@ def lrost(name, d, k, r, p, size): return calmessage @staticmethod - def lryaml(name, d, k, r, p, size): + def lryaml(name, d, k, r, p, size, cam_model): def format_mat(x, precision): return ("[%s]" % ( numpy.array2string(x, precision=precision, suppress_small=True, separator=", ") .replace("[", "").replace("]", "").replace("\n", "\n ") )) + dist_model = _get_dist_model(d, cam_model) + assert k.shape == (3, 3) assert r.shape == (3, 3) assert p.shape == (3, 4) @@ -552,7 +554,7 @@ def format_mat(x, precision): " rows: 3", " cols: 3", " data: " + format_mat(k, 5), - "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob"), + "camera_model: " + dist_model, "distortion_coefficients:", " rows: 1", " cols: %d" % d.size, diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index 631ac42bd..96eadb1a8 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -47,7 +47,7 @@ from queue import Queue except ImportError: from Queue import Queue - +from calibrator import CAMERA_MODEL class BufferQueue(Queue): """Slight modification of the standard Queue that discards the oldest item @@ -91,8 +91,8 @@ def run(self): class CalibrationNode(Node): def __init__(self, name, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, - pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0, max_chessboard_speed = -1, - queue_size = 1): + fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0, + max_chessboard_speed = -1, queue_size = 1): super().__init__(name) self.set_camera_info_service = self.create_client(sensor_msgs.srv.SetCameraInfo, @@ -287,6 +287,8 @@ def on_mouse(self, event, x, y, flags, param): # Only shut down if we set camera info correctly, #3993 if self.do_upload(): rclpy.shutdown() + def on_model_change(self, model_select_val): + self.c.set_cammodel( CAMERA_MODEL.PINHOLE if model_select_val < 0.5 else CAMERA_MODEL.FISHEYE) def on_model_change(self, model_select_val): self.c.set_cammodel( CAMERA_MODEL.PINHOLE if model_select_val < 0.5 else CAMERA_MODEL.FISHEYE) diff --git a/camera_calibration/src/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/src/camera_calibration/nodes/cameracalibrator.py index 51536cdc9..511029f32 100755 --- a/camera_calibration/src/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/src/camera_calibration/nodes/cameracalibrator.py @@ -74,21 +74,40 @@ def main(): group = OptionGroup(parser, "Calibration Optimizer Options") group.add_option("--fix-principal-point", action="store_true", default=False, - help="fix the principal point at the image center") + help="for pinhole, fix the principal point at the image center") group.add_option("--fix-aspect-ratio", action="store_true", default=False, - help="enforce focal lengths (fx, fy) are equal") + help="for pinhole, enforce focal lengths (fx, fy) are equal") group.add_option("--zero-tangent-dist", action="store_true", default=False, - help="set tangential distortion coefficients (p1, p2) to zero") + help="for pinhole, set tangential distortion coefficients (p1, p2) to zero") group.add_option("-k", "--k-coefficients", type="int", default=2, metavar="NUM_COEFFS", - help="number of radial distortion coefficients to use (up to 6, default %default)") + help="for pinhole, number of radial distortion coefficients to use (up to 6, default %default)") + + group.add_option("--fisheye-recompute-extrinsicsts", + action="store_true", default=False, + help="for fisheye, extrinsic will be recomputed after each iteration of intrinsic optimization") + group.add_option("--fisheye-fix-skew", + action="store_true", default=False, + help="for fisheye, skew coefficient (alpha) is set to zero and stay zero") + group.add_option("--fisheye-fix-principal-point", + action="store_true", default=False, + help="for fisheye,fix the principal point at the image center") + group.add_option("--fisheye-k-coefficients", + type="int", default=4, metavar="NUM_COEFFS", + help="for fisheye, number of radial distortion coefficients to use fixing to zero the rest (up to 4, default %default)") + + group.add_option("--fisheye-check-conditions", + action="store_true", default=False, + help="for fisheye, the functions will check validity of condition number") + group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False, help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners") group.add_option("--max-chessboard-speed", type="float", default=-1.0, help="Do not use samples where the calibration pattern is moving faster \ than this speed in px/frame. Set to eg. 0.5 for rolling shutter cameras.") + parser.add_option_group(group) options, _ = parser.parse_args(rclpy.utilities.remove_ros_args()) @@ -109,6 +128,7 @@ def main(): else: sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate) + # Pinhole opencv calibration options parsing num_ks = options.k_coefficients calib_flags = 0 @@ -133,6 +153,26 @@ def main(): if (num_ks < 1): calib_flags |= cv2.CALIB_FIX_K1 + # Opencv calibration flags parsing: + num_ks = options.fisheye_k_coefficients + fisheye_calib_flags = 0 + if options.fisheye_fix_principal_point: + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_PRINCIPAL_POINT + if options.fisheye_fix_skew: + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_SKEW + if options.fisheye_recompute_extrinsicsts: + fisheye_calib_flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + if options.fisheye_check_conditions: + fisheye_calib_flags |= cv2.fisheye.CALIB_CHECK_COND + if (num_ks < 4): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K4 + if (num_ks < 3): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K3 + if (num_ks < 2): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K2 + if (num_ks < 1): + fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K1 + pattern = Patterns.Chessboard if options.pattern == 'circles': pattern = Patterns.Circles @@ -147,7 +187,8 @@ def main(): checkerboard_flags = cv2.CALIB_CB_FAST_CHECK rclpy.init() - node = OpenCVCalibrationNode("cameracalibrator", boards, options.service_check, sync, calib_flags, pattern, options.camera_name, + node = OpenCVCalibrationNode("cameracalibrator", boards, options.service_check, sync, + calib_flags, fisheye_calib_flags, pattern, options.camera_name, checkerboard_flags=checkerboard_flags, max_chessboard_speed=options.max_chessboard_speed, queue_size=options.queue_size) node.spin()