fix #503: (#545)

set_cammodel of StereoCalibrator need to override the method of parent class

fix related to opencv/opencv#11085:
unlike cv2.calibrate, the cv2.fisheye.calibrate method expects float64 points and in an array with an extra dimension. The same for cv2.stereoCalibrate vs cv2.fisheye.stereoCalibrate

Trimmed whitespace at line endings

camera_calibration/src/camera_calibration/calibrator.py

@@ -162,7 +162,7 @@ def _get_corners(img, board, refine = True, checkerboard_flags=0):
             corners = numpy.copy(numpy.flipud(corners))
     else:
         direction_corners=(corners[-1]-corners[0])>=numpy.array([[0.0,0.0]])
-    
+
         if not numpy.all(direction_corners):
             if not numpy.any(direction_corners):
                 corners = numpy.copy(numpy.flipud(corners))
@@ -232,7 +232,7 @@ class Calibrator():
     """
     Base class for calibration system
     """
-    def __init__(self, boards, flags=0, fisheye_flags = 0, pattern=Patterns.Chessboard, name='', 
+    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):
         # Ordering the dimensions for the different detectors is actually a minefield...
         if pattern == Patterns.Chessboard:
@@ -306,6 +306,7 @@ def get_parameters(self, corners, board, size):
         skew = _get_skew(corners, board)
         params = [p_x, p_y, p_size, skew]
         return params
+
     def set_cammodel(self, modeltype):
         self.camera_model = modeltype
 
@@ -576,7 +577,7 @@ def do_save(self):
 
 def image_from_archive(archive, name):
     """
-    Load image PGM file from tar archive. 
+    Load image PGM file from tar archive.
 
     Used for tarfile loading and unit test.
     """
@@ -652,10 +653,10 @@ def collect_corners(self, images):
 
     def cal_fromcorners(self, good):
         """
-        :param good: Good corner positions and boards 
+        :param good: Good corner positions and boards
         :type good: [(corners, ChessboardInfo)]
 
-        
+
         """
         boards = [ b for (_, b) in good ]
 
@@ -665,6 +666,7 @@ def cal_fromcorners(self, good):
         intrinsics_in = numpy.eye(3, dtype=numpy.float64)
 
         if self.camera_model == CAMERA_MODEL.PINHOLE:
+            print("mono pinhole calibration...")
             reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
                        opts, ipts,
                        self.size,
@@ -675,6 +677,12 @@ def cal_fromcorners(self, good):
             # The extra ones include e.g. thin prism coefficients, which we are not interested in.
             self.distortion = dist_coeffs.flat[:8].reshape(-1, 1)
         elif self.camera_model == CAMERA_MODEL.FISHEYE:
+            print("mono fisheye calibration...")
+            # WARNING: cv2.fisheye.calibrate wants float64 points
+            ipts64 = numpy.asarray(ipts, dtype=numpy.float64)
+            ipts = ipts64
+            opts64 = numpy.asarray(opts, dtype=numpy.float64)
+            opts = opts64
             reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate(
                 opts, ipts, self.size,
                 intrinsics_in, None, flags = self.fisheye_calib_flags)
@@ -842,7 +850,7 @@ def handle_msg(self, msg):
                     self.db.append((params, gray))
                     self.good_corners.append((corners, board))
                     print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params)))
-        
+
         self.last_frame_corners = corners
         rv = MonoDrawable()
         rv.scrib = scrib
@@ -917,6 +925,12 @@ def __init__(self, *args, **kwargs):
         # full X range in the left camera.
         self.param_ranges[0] = 0.4
 
+    #override
+    def set_cammodel(self, modeltype):
+        super(StereoCalibrator, self).set_cammodel(modeltype)
+        self.l.set_cammodel(modeltype)
+        self.r.set_cammodel(modeltype)
+
     def cal(self, limages, rimages):
         """
         :param limages: source left images containing chessboards
@@ -958,7 +972,7 @@ def cal_fromcorners(self, good):
         lipts = [ l for (l, _, _) in good ]
         ripts = [ r for (_, r, _) in good ]
         boards = [ b for (_, _, b) in good ]
-        
+
         opts = self.mk_object_points(boards, True)
 
         flags = cv2.CALIB_FIX_INTRINSIC
@@ -967,6 +981,7 @@ def cal_fromcorners(self, good):
         self.R = numpy.eye(3, dtype=numpy.float64)
 
         if self.camera_model == CAMERA_MODEL.PINHOLE:
+            print("stereo pinhole calibration...")
             if LooseVersion(cv2.__version__).version[0] == 2:
                 cv2.stereoCalibrate(opts, lipts, ripts, self.size,
                                    self.l.intrinsics, self.l.distortion,
@@ -985,10 +1000,19 @@ def cal_fromcorners(self, good):
                                    criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
                                    flags = flags)
         elif self.camera_model == CAMERA_MODEL.FISHEYE:
+            print("stereo fisheye calibration...")
             if LooseVersion(cv2.__version__).version[0] == 2:
                 print("ERROR: You need OpenCV >3 to use fisheye camera model")
                 sys.exit()
             else:
+                # WARNING: cv2.fisheye.stereoCalibrate wants float64 points
+                lipts64 = numpy.asarray(lipts, dtype=numpy.float64)
+                lipts = lipts64
+                ripts64 = numpy.asarray(ripts, dtype=numpy.float64)
+                ripts = ripts64
+                opts64 = numpy.asarray(opts, dtype=numpy.float64)
+                opts = opts64
+
                 cv2.fisheye.stereoCalibrate(opts, lipts, ripts,
                                    self.l.intrinsics, self.l.distortion,
                                    self.r.intrinsics, self.r.distortion,
@@ -1025,12 +1049,12 @@ def set_alpha(self, a):
 
         elif self.camera_model == CAMERA_MODEL.FISHEYE:
             self.Q = numpy.zeros((4,4), dtype=numpy.float64)
-            
+
             flags = cv2.CALIB_ZERO_DISPARITY   # Operation flags that may be zero or CALIB_ZERO_DISPARITY .
                             # If the flag is set, the function makes the principal points of each camera have the same pixel coordinates in the rectified views.
-                            # And if the flag is not set, the function may still shift the images in the horizontal or vertical direction 
+                            # And if the flag is not set, the function may still shift the images in the horizontal or vertical direction
                             # (depending on the orientation of epipolar lines) to maximize the useful image area.
-            
+
             cv2.fisheye.stereoRectify(self.l.intrinsics, self.l.distortion,
                              self.r.intrinsics, self.r.distortion,
                              self.size,
@@ -1205,7 +1229,7 @@ def handle_msg(self, msg):
                     self.db.append( (params, lgray, rgray) )
                     self.good_corners.append( (lcorners, rcorners, lboard) )
                     print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % tuple([len(self.db)] + params)))
-        
+
         self.last_frame_corners = lcorners
         rv = StereoDrawable()
         rv.lscrib = lscrib
@@ -1258,7 +1282,7 @@ def do_tarfile_calibration(self, filename):
 
         if not len(limages) == len(rimages):
             raise CalibrationException("Left, right images don't match. %d left images, %d right" % (len(limages), len(rimages)))
-        
+
         ##\todo Check that the filenames match and stuff
 
         self.cal(limages, rimages)

camera_calibration/src/camera_calibration/camera_calibrator.py

@@ -136,7 +136,7 @@ def __init__(self, name, boards, service_check = True, synchronizer = message_fi
         self.q_stereo = BufferQueue(queue_size)
 
         self.c = None
-        
+
         self._last_display = None
 
         mth = ConsumerThread(self.q_mono, self.handle_monocular)