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

Jump to bottom

adding option to independently calibrate mono cameras before a stereo cal #446

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

adding option to independently calibrate mono cameras before a stereo cal #446

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
5 changes: 3 additions & 2 deletions camera_calibration/nodes/cameracalibrator.py
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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