[camera_calibration] Enable to select range policy for 16 bit images

Author: HiroIshida

camera_calibration/nodes/cameracalibrator.py

@@ -132,6 +132,10 @@ 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 +223,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__":

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

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]