adding --mono_before_stereo option to do combined monocular and stereo calibration for a camera pair

Author: phil0stine

camera_calibration/nodes/cameracalibrator.py

@@ -90,6 +90,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()
 
@@ -150,7 +152,7 @@ def main():
     rospy.init_node('cameracalibrator')
     node = OpenCVCalibrationNode(boards, options.service_check, sync, 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__":

camera_calibration/src/camera_calibration/calibrator.py

@@ -881,6 +881,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)
@@ -921,10 +923,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 ]

camera_calibration/src/camera_calibration/camera_calibrator.py

@@ -109,7 +109,7 @@ def run(self):
 class CalibrationNode:
     def __init__(self, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
                  pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0, max_chessboard_speed = -1,
-                 queue_size = 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"]:
@@ -130,6 +130,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)
@@ -149,8 +150,10 @@ def __init__(self, boards, service_check = True, synchronizer = message_filters.
         self.q_stereo = BufferQueue(queue_size)
 
         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)
@@ -169,40 +172,55 @@ 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._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._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._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._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._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._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._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._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)
 
-
     def check_set_camera_info(self, response):
         if response.success:
             return True
@@ -262,10 +280,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()