Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[camera_calibration] fix/feat: enable to select range policy for 16 bit images #841

Open
wants to merge 1 commit into
base: noetic
Choose a base branch
from
Open

[camera_calibration] fix/feat: enable to select range policy for 16 bit images #841

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 7 additions & 1 deletion camera_calibration/nodes/cameracalibrator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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__":
Expand Down
26 changes: 24 additions & 2 deletions camera_calibration/src/camera_calibration/calibrator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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):
"""
Expand All @@ -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
Expand Down
14 changes: 8 additions & 6 deletions camera_calibration/src/camera_calibration/camera_calibrator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"]:
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -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_speed, 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]
Expand Down