diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py index 2a6cb5e49..379cf3ffe 100755 --- a/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/nodes/cameracalibrator.py @@ -75,22 +75,42 @@ 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, args = parser.parse_args() if len(options.size) != len(options.square): @@ -110,6 +130,7 @@ def main(): else: sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate) + # Pinhole opencv calibration options parsing num_ks = options.k_coefficients calib_flags = 0 @@ -134,6 +155,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 @@ -148,7 +189,7 @@ def main(): checkerboard_flags = cv2.CALIB_CB_FAST_CHECK rospy.init_node('cameracalibrator') - node = OpenCVCalibrationNode(boards, options.service_check, sync, calib_flags, pattern, options.camera_name, + node = OpenCVCalibrationNode(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) rospy.spin() diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 0bd58c873..0fa3d508d 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -49,7 +49,13 @@ import tarfile import time from distutils.version import LooseVersion +import sys +from enum import Enum +# Supported camera models +class CAMERA_MODEL(Enum): + PINHOLE = 0 + FISHEYE = 1 # Supported calibration patterns class Patterns: @@ -213,13 +219,25 @@ def _get_circles(img, board, pattern): return (ok, corners) +def _get_dist_model(dist_params, cam_model): + # Select dist model + if CAMERA_MODEL.PINHOLE == cam_model: + if dist_params.size > 5: + dist_model = "rational_polynomial" + else: + dist_model = "plumb_bob" + elif CAMERA_MODEL.FISHEYE == cam_model: + dist_model = "fisheye" + else: + dist_model = "unknown" + return dist_model # TODO self.size needs to come from CameraInfo, full resolution class Calibrator(object): """ Base class for calibration system """ - def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', + def __init__(self, boards, flags=0, fisheye_flags = 0, pattern=Patterns.Chessboard, name='', checkerboard_flags=cv2.CALIB_CB_FAST_CHECK, max_chessboard_speed = -1.0): # Ordering the dimensions for the different detectors is actually a minefield... if pattern == Patterns.Chessboard: @@ -235,10 +253,11 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', # Set to true after we perform calibration self.calibrated = False self.calib_flags = flags + self.fisheye_calib_flags = fisheye_flags self.checkerboard_flags = checkerboard_flags self.pattern = pattern self.br = cv_bridge.CvBridge() - + self.camera_model = CAMERA_MODEL.PINHOLE # self.db is list of (parameters, image) samples for use in calibration. parameters has form # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken # and ensure enough variety. @@ -292,6 +311,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 is_slow_moving(self, corners, last_frame_corners): """ @@ -452,16 +473,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() @@ -517,13 +535,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) @@ -535,7 +555,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, @@ -583,7 +603,6 @@ def __init__(self): ImageDrawable.__init__(self) self.scrib = None self.linear_error = -1.0 - class StereoDrawable(ImageDrawable): def __init__(self): @@ -647,18 +666,23 @@ def cal_fromcorners(self, good): ipts = [ points for (points, _) in good ] opts = self.mk_object_points(boards) - # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio intrinsics_in = numpy.eye(3, dtype=numpy.float64) - reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( - opts, ipts, - self.size, - intrinsics_in, - None, - flags = self.calib_flags) - # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set. - # The extra ones include e.g. thin prism coefficients, which we are not interested in. - self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) + + if self.camera_model == CAMERA_MODEL.PINHOLE: + reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera( + opts, ipts, + self.size, + intrinsics_in, + None, + flags = self.calib_flags) + # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set. + # The extra ones include e.g. thin prism coefficients, which we are not interested in. + self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) + elif self.camera_model == CAMERA_MODEL.FISHEYE: + reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate( + opts, ipts, self.size, + intrinsics_in, None, flags = self.fisheye_calib_flags) # R is identity matrix for monocular calibration self.R = numpy.eye(3, dtype=numpy.float64) @@ -674,14 +698,22 @@ def set_alpha(self, a): original image are in calibrated image). """ - # NOTE: Prior to Electric, this code was broken such that we never actually saved the new - # camera matrix. In effect, this enforced P = [K|0] for monocular cameras. - # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix) - ncm, _ = cv2.getOptimalNewCameraMatrix(self.intrinsics, self.distortion, self.size, a) - for j in range(3): - for i in range(3): - self.P[j,i] = ncm[j, i] - self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) + if self.camera_model == CAMERA_MODEL.PINHOLE: + # NOTE: Prior to Electric, this code was broken such that we never actually saved the new + # camera matrix. In effect, this enforced P = [K|0] for monocular cameras. + # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix) + ncm, _ = cv2.getOptimalNewCameraMatrix(self.intrinsics, self.distortion, self.size, a) + for j in range(3): + for i in range(3): + self.P[j,i] = ncm[j, i] + self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1) + elif self.camera_model == CAMERA_MODEL.FISHEYE: + # NOTE: estimateNewCameraMatrixForUndistortRectify not producing proper results, using a naive approach instead: + self.P[:3,:3] = self.intrinsics[:3,:3] + self.P[0,0] /= (1. + a) + self.P[1,1] /= (1. + a) + self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, self.P, self.size, cv2.CV_32FC1) + def remap(self, src): """ @@ -699,12 +731,14 @@ def undistort_points(self, src): Apply the post-calibration undistortion to the source points """ - - return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) + if self.camera_model == CAMERA_MODEL.PINHOLE: + return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) + elif self.camera_model == CAMERA_MODEL.FISHEYE: + return cv2.fisheye.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P) def as_message(self): """ Return the camera calibration as a CameraInfo message """ - return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size) + return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size, self.camera_model) def from_message(self, msg, alpha = 0.0): """ Initialize the camera calibration from a CameraInfo message """ @@ -724,7 +758,7 @@ def ost(self): return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size) def yaml(self): - return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size) + return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size, self.camera_model) def linear_error_from_image(self, image): """ @@ -936,23 +970,38 @@ def cal_fromcorners(self, good): self.T = numpy.zeros((3, 1), dtype=numpy.float64) self.R = numpy.eye(3, dtype=numpy.float64) - if LooseVersion(cv2.__version__).version[0] == 2: - cv2.stereoCalibrate(opts, lipts, ripts, self.size, - self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.R, # R - self.T, # T - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - flags = flags) - else: - cv2.stereoCalibrate(opts, lipts, ripts, - self.l.intrinsics, self.l.distortion, - self.r.intrinsics, self.r.distortion, - self.size, - self.R, # R - self.T, # T - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - flags = flags) + + if self.camera_model == CAMERA_MODEL.PINHOLE: + if LooseVersion(cv2.__version__).version[0] == 2: + cv2.stereoCalibrate(opts, lipts, ripts, self.size, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.R, # R + self.T, # T + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + flags = flags) + else: + cv2.stereoCalibrate(opts, lipts, ripts, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, # R + self.T, # T + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + flags = flags) + elif self.camera_model == CAMERA_MODEL.FISHEYE: + if LooseVersion(cv2.__version__).version[0] == 2: + print("ERROR: You need OpenCV >3 to use fisheye camera model") + sys.exit() + else: + cv2.fisheye.stereoCalibrate(opts, lipts, ripts, + self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, # R + self.T, # T + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), # 30, 1e-6 + flags = flags) self.set_alpha(0.0) @@ -963,21 +1012,47 @@ def set_alpha(self, a): in calibrated image are valid) to 1 (zoomed out, all pixels in original image are in calibrated image). """ - - cv2.stereoRectify(self.l.intrinsics, - self.l.distortion, - self.r.intrinsics, - self.r.distortion, - self.size, - self.R, - self.T, - self.l.R, self.r.R, self.l.P, self.r.P, - alpha = a) - - cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1, - self.l.mapx, self.l.mapy) - cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1, - self.r.mapx, self.r.mapy) + if self.camera_model == CAMERA_MODEL.PINHOLE: + cv2.stereoRectify(self.l.intrinsics, + self.l.distortion, + self.r.intrinsics, + self.r.distortion, + self.size, + self.R, + self.T, + self.l.R, self.r.R, self.l.P, self.r.P, + alpha = a) + + cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1, + self.l.mapx, self.l.mapy) + cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1, + self.r.mapx, self.r.mapy) + + elif self.camera_model == CAMERA_MODEL.FISHEYE: + self.Q = numpy.zeros((4,4), dtype=numpy.float64) + + flags = cv2.CALIB_ZERO_DISPARITY # Operation flags that may be zero or CALIB_ZERO_DISPARITY . + # If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views. + # And if the flag is not set, the function may still shift the images in the horizontal or vertical direction + # (depending on the orientation of epipolar lines) to maximize the useful image area. + + cv2.fisheye.stereoRectify(self.l.intrinsics, self.l.distortion, + self.r.intrinsics, self.r.distortion, + self.size, + self.R, self.T, + flags, + self.l.R, self.r.R, + self.l.P, self.r.P, + self.Q, + self.size, + a, + 1.0 ) + self.l.P[:3,:3] = numpy.dot(self.l.intrinsics,self.l.R) + self.r.P[:3,:3] = numpy.dot(self.r.intrinsics,self.r.R) + cv2.fisheye.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.intrinsics, self.size, cv2.CV_32FC1, + self.l.mapx, self.l.mapy) + cv2.fisheye.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.intrinsics, self.size, cv2.CV_32FC1, + self.r.mapx, self.r.mapy) def as_message(self): """ @@ -985,8 +1060,8 @@ def as_message(self): and right cameras respectively. """ - return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size), - self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) + return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size, self.l.camera_model), + self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size, self.r.camera_model)) def from_message(self, msgs, alpha = 0.0): """ Initialize the camera calibration from a pair of CameraInfo messages. """ @@ -1014,7 +1089,7 @@ def ost(self): self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size)) def yaml(self, suffix, info): - return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size) + return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size, self.camera_model) # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners def epipolar_error_from_images(self, limage, rimage): diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index 79f89b0f9..5065cb831 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 @@ -81,7 +81,9 @@ def __init__(self, queue, opencv_calibration_node): def run(self): cv2.namedWindow("display", cv2.WINDOW_NORMAL) cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse) + cv2.createTrackbar("Camera type: \n 0 : pinhole \n 1 : fisheye", "display", 0,1, self.opencv_calibration_node.on_model_change) cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale) + while True: if self.queue.qsize() > 0: self.image = self.queue.get() @@ -108,8 +110,8 @@ def run(self): class CalibrationNode: def __init__(self, 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): if service_check: # assume any non-default service names have been set. Wait for the service to become ready for svcname in ["camera", "left_camera", "right_camera"]: @@ -126,6 +128,7 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters. self._boards = boards self._calib_flags = flags + self._fisheye_calib_flags = fisheye_flags self._checkerboard_flags = checkerboard_flags self._pattern = pattern self._camera_name = camera_name @@ -174,11 +177,11 @@ def queue_stereo(self, lmsg, rmsg): def handle_monocular(self, msg): if self.c == None: if self._camera_name: - self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, + self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name, checkerboard_flags=self._checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) else: - self.c = MonoCalibrator(self._boards, self._calib_flags, self._pattern, + self.c = MonoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, checkerboard_flags=self.checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) @@ -190,11 +193,11 @@ def handle_monocular(self, msg): def handle_stereo(self, msg): if self.c == None: if self._camera_name: - self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, name=self._camera_name, + self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, name=self._camera_name, checkerboard_flags=self._checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) else: - self.c = StereoCalibrator(self._boards, self._calib_flags, self._pattern, + self.c = StereoCalibrator(self._boards, self._calib_flags, self._fisheye_calib_flags, self._pattern, checkerboard_flags=self._checkerboard_flags, max_chessboard_speed = self._max_chessboard_speed) @@ -273,6 +276,8 @@ def on_mouse(self, event, x, y, flags, param): # Only shut down if we set camera info correctly, #3993 if self.do_upload(): rospy.signal_shutdown('Quit') + 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_scale(self, scalevalue): if self.c.calibrated: