diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py index e5341bad3..1c069b97d 100755 --- a/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/nodes/cameracalibrator.py @@ -132,6 +132,9 @@ def main(): 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.") + group.add_option("--scale", type="float", default=None, + help="Scaling option for the input image relative to 640x480. \ + Default scaled to 640x480, 1.0 result with no up/down scaling") parser.add_option_group(group) @@ -220,9 +223,11 @@ def main(): checkerboard_flags = cv2.CALIB_CB_FAST_CHECK rospy.init_node('cameracalibrator') - 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) + 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, img_downscale=options.scale) rospy.spin() if __name__ == "__main__": diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 2f9dad464..a939471a4 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -305,8 +305,11 @@ class Calibrator(): """ Base class for calibration system """ - 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): + 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, + scale = None): # Ordering the dimensions for the different detectors is actually a minefield... if pattern == Patterns.Chessboard: # Make sure n_cols > n_rows to agree with OpenCV CB detector output @@ -341,6 +344,7 @@ def __init__(self, boards, flags=0, fisheye_flags = 0, pattern=Patterns.Chessboa self.last_frame_corners = None self.last_frame_ids = None self.max_chessboard_speed = max_chessboard_speed + self.scale = scale def mkgray(self, msg): """ @@ -520,7 +524,9 @@ def downsample_and_detect(self, img): # Scale the input image down to ~VGA size height = img.shape[0] width = img.shape[1] - scale = math.sqrt( (width*height) / (640.*480.) ) + + scale = float(self.scale) if self.scale else math.sqrt( (width*height) / (640.*480.) ) + if scale > 1.0: scrib = cv2.resize(img, (int(width / scale), int(height / scale))) else: diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index bd1111c73..68cfee919 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -108,9 +108,12 @@ def run(self): class CalibrationNode: - def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0, - fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0, - max_chessboard_speed = -1, queue_size = 1): + def __init__(self, boards, service_check = True, + synchronizer = message_filters.TimeSynchronizer, flags = 0, + fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', + checkerboard_flags = 0, max_chessboard_speed = -1, + queue_size = 1, img_downscale = None): + 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"]: @@ -131,6 +134,7 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters. self._checkerboard_flags = checkerboard_flags self._pattern = pattern self._camera_name = camera_name + self._img_scale = img_downscale self._max_chessboard_speed = max_chessboard_speed lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) @@ -176,13 +180,18 @@ 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._fisheye_calib_flags, self._pattern, name=self._camera_name, - checkerboard_flags=self._checkerboard_flags, - max_chessboard_speed = self._max_chessboard_speed) + 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, + scale = self._img_scale) else: - 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) + 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, + scale = self._img_scale) # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) @@ -192,13 +201,18 @@ 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._fisheye_calib_flags, self._pattern, name=self._camera_name, - checkerboard_flags=self._checkerboard_flags, - max_chessboard_speed = self._max_chessboard_speed) + 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, + scale=self._img_scale) else: - 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) + 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, + scale=self._img_scale) drawable = self.c.handle_msg(msg) self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]