Skip to content

Commit

Permalink
adding --mono_before_stereo option to do combined monocular and stere…
Browse files Browse the repository at this point in the history 
…o calibration for a camera pair
  • Loading branch information
@phil0stine
phil0stine committed Dec 5, 2019
1 parent 00e7c02 commit fbf954d
Show file tree
Hide file tree
Showing 3 changed files with 88 additions and 30 deletions.
5 changes: 3 additions & 2 deletions camera_calibration/nodes/cameracalibrator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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__":
Expand Down
26 changes: 22 additions & 4 deletions camera_calibration/src/camera_calibration/calibrator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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 ]
Expand Down
87 changes: 63 additions & 24 deletions camera_calibration/src/camera_calibration/camera_calibrator.py
100755 → 100644
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"]:
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -172,35 +176,51 @@ 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]
self.redraw_monocular(drawable)

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)
Expand Down Expand Up @@ -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()
Expand Down

0 comments on commit fbf954d

Please sign in to comment.