diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py index e5341bad3..96af07515 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.") + parser.add_option("-r", "--range-policy", + type="string", default="upper", + help="For 16 bit images, the policy to use for determining the range of pixel values to use for thresholding. Choose from 'upper', 'dynamic', or 'legacy'.") parser.add_option_group(group) @@ -219,10 +222,13 @@ def main(): else: checkerboard_flags = cv2.CALIB_CB_FAST_CHECK + range_policy = options.range_policy + assert range_policy in ['upper', 'dynamic', 'legacy'], "Invalid range policy: %s" % range_policy + 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) + queue_size=options.queue_size, range_policy=range_policy) rospy.spin() if __name__ == "__main__": diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 51b721e6a..030b40cd0 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -318,7 +318,8 @@ 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): + checkerboard_flags=cv2.CALIB_CB_FAST_CHECK, max_chessboard_speed = -1.0, + range_policy="upper"): # 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 @@ -353,6 +354,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.range_policy = range_policy def mkgray(self, msg): """ @@ -362,7 +364,27 @@ def mkgray(self, msg): # TODO: get a Python API in cv_bridge to check for the image depth. if self.br.encoding_to_dtype_with_channels(msg.encoding)[0] in ['uint16', 'int16']: mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') - mono8 = numpy.array(mono16 / 256, dtype=numpy.uint8) + if self.range_policy == "upper": + # this policy works when the image is too bright + mono16 = self.br.imgmsg_to_cv2(msg, '16UC1') + mono8 = numpy.array(mono16 / 256, dtype=numpy.uint8) + elif self.range_policy == "dynamic": + min_val, max_val, _, _ = cv2.minMaxLoc(mono16) + value_range = max_val - min_val + if value_range > 0: + mono8_float = (mono16 - min_val) * (255.0 / value_range) + else: + mono8_float = mono16 + mono8 = mono8_float.astype(numpy.uint8) + elif self.range_policy == "legacy": + # revival of old behavior removed in + # https://github.com/ros-perception/image_pipeline/pull/334 + # this policy works when the image is too dark which might be the case + # with old IR cameras + mono8 = numpy.array(numpy.clip(mono16, 0, 255), dtype=numpy.uint8) + else: + assert False, "Unknown range policy '%s'" % self.range_policy + return mono8 elif 'FC1' in msg.encoding: # floating point image handling diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py index 09a231aaa..651baeb9c 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -109,8 +109,8 @@ 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): + fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name = '', checkerboard_flags = 0, + max_chessboard_speed = -1, queue_size = 1, range_policy = "upper"): 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"]: @@ -132,6 +132,8 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters. self._pattern = pattern self._camera_name = camera_name self._max_chessboard_speed = max_chessboard_speed + self._range_policy = range_policy + lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) ts = synchronizer([lsub, rsub], 4) @@ -178,11 +180,11 @@ def handle_monocular(self, msg): 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) + max_chessboard_speed=self._max_chessboard_speed, range_policy=self._range_policy) 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) + max_chessboard_speed=self._max_chessboard_speed, range_policy=self._range_policy) # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) @@ -194,11 +196,11 @@ def handle_stereo(self, msg): 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) + max_chessboard_speed=self._max_chessboard_speeds, range_policy=self._range_policy) 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) + max_chessboard_speed=self._max_chessboard_speed, range_policy=self._range_policy) drawable = self.c.handle_msg(msg) self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]