diff --git a/camera_calibration/nodes/cameracalibrator.py b/camera_calibration/nodes/cameracalibrator.py index 379cf3ffe..932d80148 100755 --- a/camera_calibration/nodes/cameracalibrator.py +++ b/camera_calibration/nodes/cameracalibrator.py @@ -108,7 +108,8 @@ 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("--mono-before-stereo", action='store_true', default=False, + help="Independently perform monocular calibration before stereo calibration") parser.add_option_group(group) options, args = parser.parse_args() @@ -191,7 +192,7 @@ def main(): 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, mono_before_stereo=options.mono_before_stereo) rospy.spin() if __name__ == "__main__": diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 0fa3d508d..c42ceec84 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -915,6 +915,8 @@ class StereoCalibrator(Calibrator): def __init__(self, *args, **kwargs): if 'name' not in kwargs: kwargs['name'] = 'narrow_stereo' + self.lcal = kwargs.pop('lcal', None) + self.rcal = kwargs.pop('rcal', None) super(StereoCalibrator, self).__init__(*args, **kwargs) self.l = MonoCalibrator(*args, **kwargs) self.r = MonoCalibrator(*args, **kwargs) @@ -955,10 +957,26 @@ def collect_corners(self, limages, rimages): def cal_fromcorners(self, good): # Perform monocular calibrations - lcorners = [(l, b) for (l, r, b) in good] - rcorners = [(r, b) for (l, r, b) in good] - self.l.cal_fromcorners(lcorners) - self.r.cal_fromcorners(rcorners) + if self.lcal and self.rcal: + self.lcal.do_calibration() + self.rcal.do_calibration() + self.l.intrinsics = self.lcal.intrinsics + self.l.distortion = self.lcal.distortion + self.r.intrinsics = self.rcal.intrinsics + self.r.distortion = self.rcal.distortion + self.l.R = self.lcal.R + self.r.R = self.rcal.R + self.l.P = self.lcal.P + self.r.P = self.rcal.P + self.l.mapx = self.lcal.mapx + self.l.mapy = self.lcal.mapy + self.r.mapx = self.rcal.mapx + self.r.mapy = self.rcal.mapy + else: + lcorners = [(l, b) for (l, r, b) in good] + rcorners = [(r, b) for (l, r, b) in good] + self.l.cal_fromcorners(lcorners) + self.r.cal_fromcorners(rcorners) lipts = [ l for (l, _, _) in good ] ripts = [ r for (_, r, _) in good ] diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py old mode 100755 new mode 100644 index 5065cb831..302ba1b87 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -111,7 +111,7 @@ 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): + max_chessboard_speed = -1, queue_size = 1, mono_before_stereo=False): 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"]: @@ -133,6 +133,7 @@ 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._mono_before_stereo = mono_before_stereo lsub = message_filters.Subscriber('left', sensor_msgs.msg.Image) rsub = message_filters.Subscriber('right', sensor_msgs.msg.Image) ts = synchronizer([lsub, rsub], 4) @@ -154,6 +155,9 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters. self.c = None self._last_display = None + self.lcal = None + self.rcal = None + self.lock = threading.Lock() mth = ConsumerThread(self.q_mono, self.handle_monocular) mth.setDaemon(True) @@ -172,19 +176,43 @@ def queue_monocular(self, msg): self.q_mono.put(msg) def queue_stereo(self, lmsg, rmsg): - self.q_stereo.put((lmsg, rmsg)) + if self._mono_before_stereo: + if self.lcal == None: + self.q_mono.put(lmsg) + elif self.rcal == None: + self.q_mono.put(rmsg) + else: + self.q_stereo.put((lmsg, rmsg)) + else: + self.q_stereo.put((lmsg, rmsg)) + + def init_monocular(self): + if self._camera_name: + 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: + 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) + return c + + def init_stereo(self): + if self._camera_name: + 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, + lcal=self.lcal, rcal=self.rcal) + else: + 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, + lcal=self.lcal, rcal=self.rcal) + return c 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) - 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 = self.init_monocular() # This should just call the MonoCalibrator drawable = self.c.handle_msg(msg) self.displaywidth = drawable.scrib.shape[1] @@ -192,15 +220,7 @@ 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) - 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 = self.init_stereo() drawable = self.c.handle_msg(msg) self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1] self.redraw_stereo(drawable) @@ -265,10 +285,29 @@ def on_mouse(self, event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN and self.displaywidth < x: if self.c.goodenough: if 180 <= y < 280: - print("**** Calibrating ****") - self.c.do_calibration() - self.buttons(self._last_display) - self.queue_display.put(self._last_display) + if self._mono_before_stereo: + if self.lcal == None: + self.lcal = self.c + self.q_mono.clear() + self.q_stereo.clear() + self.c = self.init_monocular() + elif self.rcal == None: + self.lock.acquire() + self.rcal = self.c + self.q_mono.clear() + self.q_stereo.clear() + self.c = self.init_stereo() + self.lock.release() + else: + print("**** Calibrating ****") + self.c.do_calibration() + self.buttons(self._last_display) + self.queue_display.put(self._last_display) + else: + print("**** Calibrating ****") + self.c.do_calibration() + self.buttons(self._last_display) + self.queue_display.put(self._last_display) if self.c.calibrated: if 280 <= y < 380: self.c.do_save()