Add Fisheye calibration tool

camera_calibration/src/camera_calibration/calibrator.py

@@ -49,7 +49,13 @@
 import tarfile
 import time
 from distutils.version import LooseVersion
+import sys
+from enum import Enum
 
+# Supported camera models
+class CAMERA_MODEL(Enum):
+    PINHOLE = 0
+    FISHEYE = 1
 
 # Supported calibration patterns
 class Patterns:
@@ -213,6 +219,18 @@ def _get_circles(img, board, pattern):
 
     return (ok, corners)
 
+def _get_dist_model(dist_params, cam_model):
+    # Select dist model
+    if CAMERA_MODEL.PINHOLE == cam_model:
+        if dist_params.size > 5:
+            dist_model = "rational_polynomial"
+        else:
+            dist_model = "plumb_bob"
+    elif CAMERA_MODEL.FISHEYE == cam_model:
+        dist_model = "fisheye"
+    else:
+        dist_model = "unknown"
+    return dist_model
 
 # TODO self.size needs to come from CameraInfo, full resolution
 class Calibrator(object):
@@ -238,7 +256,7 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='',
         self.checkerboard_flags = checkerboard_flags
         self.pattern = pattern
         self.br = cv_bridge.CvBridge()
-
+        self.camera_model = CAMERA_MODEL.PINHOLE
         # self.db is list of (parameters, image) samples for use in calibration. parameters has form
         # (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken
         # and ensure enough variety.
@@ -292,6 +310,8 @@ 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
 
     def is_slow_moving(self, corners, last_frame_corners):
         """
@@ -452,16 +472,13 @@ def downsample_and_detect(self, img):
 
         return (scrib, corners, downsampled_corners, board, (x_scale, y_scale))
 
-
     @staticmethod
-    def lrmsg(d, k, r, p, size):
+    def lrmsg(d, k, r, p, size, camera_model):
         """ Used by :meth:`as_message`.  Return a CameraInfo message for the given calibration matrices """
         msg = sensor_msgs.msg.CameraInfo()
         msg.width, msg.height = size
-        if d.size > 5:
-            msg.distortion_model = "rational_polynomial"
-        else:
-            msg.distortion_model = "plumb_bob"
+        msg.distortion_model = _get_dist_model(d, camera_model)
+
         msg.D = numpy.ravel(d).copy().tolist()
         msg.K = numpy.ravel(k).copy().tolist()
         msg.R = numpy.ravel(r).copy().tolist()
@@ -517,13 +534,15 @@ def lrost(name, d, k, r, p, size):
         return calmessage
 
     @staticmethod
-    def lryaml(name, d, k, r, p, size):
+    def lryaml(name, d, k, r, p, size, cam_model):
         def format_mat(x, precision):
             return ("[%s]" % (
                 numpy.array2string(x, precision=precision, suppress_small=True, separator=", ")
                     .replace("[", "").replace("]", "").replace("\n", "\n        ")
             ))
 
+        dist_model = _get_dist_model(d, cam_model)
+
         assert k.shape == (3, 3)
         assert r.shape == (3, 3)
         assert p.shape == (3, 4)
@@ -535,7 +554,7 @@ def format_mat(x, precision):
             "  rows: 3",
             "  cols: 3",
             "  data: " + format_mat(k, 5),
-            "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob"),
+            "camera_model: " + dist_model,
             "distortion_coefficients:",
             "  rows: 1",
             "  cols: %d" % d.size,
@@ -583,7 +602,6 @@ def __init__(self):
         ImageDrawable.__init__(self)
         self.scrib = None
         self.linear_error = -1.0
-
 
 class StereoDrawable(ImageDrawable):
     def __init__(self):
@@ -647,19 +665,26 @@ def cal_fromcorners(self, good):
 
         ipts = [ points for (points, _) in good ]
         opts = self.mk_object_points(boards)
-
         # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio
         intrinsics_in = numpy.eye(3, dtype=numpy.float64)
-        reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
-                   opts, ipts,
-                   self.size,
-                   intrinsics_in,
-                   None,
-                   flags = self.calib_flags)
-        # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set.
-        # The extra ones include e.g. thin prism coefficients, which we are not interested in.
-        self.distortion = dist_coeffs.flat[:8].reshape(-1, 1)
 
+        if self.camera_model == CAMERA_MODEL.PINHOLE:
+            reproj_err, self.intrinsics, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
+                       opts, ipts,
+                       self.size,
+                       intrinsics_in,
+                       None,
+                       flags = self.calib_flags)
+            # OpenCV returns more than 8 coefficients (the additional ones all zeros) when CALIB_RATIONAL_MODEL is set.
+            # 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:
+            calibration_flags = cv2.fisheye.CALIB_FIX_SKEW + self.calib_flags # Add user flags
+            reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate(
+                opts, ipts, self.size,
+                intrinsics_in, None, flags = calibration_flags)
+        else:
+            print("Something went wrong when selecting a model")
         # R is identity matrix for monocular calibration
         self.R = numpy.eye(3, dtype=numpy.float64)
         self.P = numpy.zeros((3, 4), dtype=numpy.float64)
@@ -674,14 +699,26 @@ def set_alpha(self, a):
         original image are in calibrated image).
         """
 
-        # NOTE: Prior to Electric, this code was broken such that we never actually saved the new
-        # camera matrix. In effect, this enforced P = [K|0] for monocular cameras.
-        # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix)
-        ncm, _ = cv2.getOptimalNewCameraMatrix(self.intrinsics, self.distortion, self.size, a)
-        for j in range(3):
-            for i in range(3):
-                self.P[j,i] = ncm[j, i]
-        self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1)
+        if self.camera_model == CAMERA_MODEL.PINHOLE:
+            # NOTE: Prior to Electric, this code was broken such that we never actually saved the new
+            # camera matrix. In effect, this enforced P = [K|0] for monocular cameras.
+            # TODO: Verify that OpenCV #1199 gets applied (improved GetOptimalNewCameraMatrix)
+            ncm, _ = cv2.getOptimalNewCameraMatrix(self.intrinsics, self.distortion, self.size, a)
+            for j in range(3):
+                for i in range(3):
+                    self.P[j,i] = ncm[j, i]
+            self.mapx, self.mapy = cv2.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, ncm, self.size, cv2.CV_32FC1)
+        elif self.camera_model == CAMERA_MODEL.FISHEYE:
+            # ncm = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.intrinsics, self.distortion, self.size, self.R, balance=a)
+            # self.P[:3,:3] = ncm[:3,:3]
+            # NOTE: estimateNewCameraMatrixForUndistortRectify not producing proper results, using a naive approach instead:
+            self.P[:3,:3] = self.intrinsics[:3,:3]
+            self.P[0,0] /= (1. + a)
+            self.P[1,1] /= (1. + a)
+            self.mapx, self.mapy = cv2.fisheye.initUndistortRectifyMap(self.intrinsics, self.distortion, self.R, self.P, self.size, cv2.CV_32FC1)
+        else:
+            print("Something went wrong when selecting a model")
+
 
     def remap(self, src):
         """
@@ -704,7 +741,7 @@ def undistort_points(self, src):
 
     def as_message(self):
         """ Return the camera calibration as a CameraInfo message """
-        return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size)
+        return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P, self.size, self.camera_model)
 
     def from_message(self, msg, alpha = 0.0):
         """ Initialize the camera calibration from a CameraInfo message """
@@ -724,7 +761,7 @@ def ost(self):
         return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size)
 
     def yaml(self):
-        return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size)
+        return self.lryaml(self.name, self.distortion, self.intrinsics, self.R, self.P, self.size, self.camera_model)
 
     def linear_error_from_image(self, image):
         """
@@ -936,23 +973,40 @@ def cal_fromcorners(self, good):
 
         self.T = numpy.zeros((3, 1), dtype=numpy.float64)
         self.R = numpy.eye(3, dtype=numpy.float64)
-        if LooseVersion(cv2.__version__).version[0] == 2:
-            cv2.stereoCalibrate(opts, lipts, ripts, self.size,
-                               self.l.intrinsics, self.l.distortion,
-                               self.r.intrinsics, self.r.distortion,
-                               self.R,                            # R
-                               self.T,                            # T
-                               criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
-                               flags = flags)
+
+        if self.camera_model == CAMERA_MODEL.PINHOLE:
+            if LooseVersion(cv2.__version__).version[0] == 2:
+                cv2.stereoCalibrate(opts, lipts, ripts, self.size,
+                                   self.l.intrinsics, self.l.distortion,
+                                   self.r.intrinsics, self.r.distortion,
+                                   self.R,                            # R
+                                   self.T,                            # T
+                                   criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
+                                   flags = flags)
+            else:
+                cv2.stereoCalibrate(opts, lipts, ripts,
+                                   self.l.intrinsics, self.l.distortion,
+                                   self.r.intrinsics, self.r.distortion,
+                                   self.size,
+                                   self.R,                            # R
+                                   self.T,                            # T
+                                   criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
+                                   flags = flags)
+        elif self.camera_model == CAMERA_MODEL.FISHEYE:
+            if LooseVersion(cv2.__version__).version[0] == 2:
+                print("ERROR: You need OpenCV >3 to use fisheye camera model")
+                sys.exit()
+            else:
+                cv2.fisheye.stereoCalibrate(opts, lipts, ripts,
+                                   self.l.intrinsics, self.l.distortion,
+                                   self.r.intrinsics, self.r.distortion,
+                                   self.size,
+                                   self.R,                            # R
+                                   self.T,                            # T
+                                   criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), # 30, 1e-6
+                                   flags = flags)
         else:
-            cv2.stereoCalibrate(opts, lipts, ripts,
-                               self.l.intrinsics, self.l.distortion,
-                               self.r.intrinsics, self.r.distortion,
-                               self.size,
-                               self.R,                            # R
-                               self.T,                            # T
-                               criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5),
-                               flags = flags)
+            print("Something went wrong when selecting a model")
 
         self.set_alpha(0.0)
 
@@ -963,30 +1017,58 @@ def set_alpha(self, a):
         in calibrated image are valid) to 1 (zoomed out, all pixels in
         original image are in calibrated image).
         """
-
-        cv2.stereoRectify(self.l.intrinsics,
-                         self.l.distortion,
-                         self.r.intrinsics,
-                         self.r.distortion,
-                         self.size,
-                         self.R,
-                         self.T,
-                         self.l.R, self.r.R, self.l.P, self.r.P,
-                         alpha = a)
-
-        cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1,
-                                   self.l.mapx, self.l.mapy)
-        cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1,
-                                   self.r.mapx, self.r.mapy)
+        if self.camera_model == CAMERA_MODEL.PINHOLE:
+            cv2.stereoRectify(self.l.intrinsics,
+                             self.l.distortion,
+                             self.r.intrinsics,
+                             self.r.distortion,
+                             self.size,
+                             self.R,
+                             self.T,
+                             self.l.R, self.r.R, self.l.P, self.r.P,
+                             alpha = a)
+
+            cv2.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.P, self.size, cv2.CV_32FC1,
+                                       self.l.mapx, self.l.mapy)
+            cv2.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.P, self.size, cv2.CV_32FC1,
+                                       self.r.mapx, self.r.mapy)
+
+        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 
+                            # (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,
+                             self.R, self.T,
+                             flags,
+                             self.l.R, self.r.R,
+                             self.l.P, self.r.P,
+                             self.Q,
+                             self.size,
+                             a,
+                             1.0 )
+            self.l.P[:3,:3] = numpy.dot(self.l.intrinsics,self.l.R)
+            self.r.P[:3,:3] = numpy.dot(self.r.intrinsics,self.r.R)
+            cv2.fisheye.initUndistortRectifyMap(self.l.intrinsics, self.l.distortion, self.l.R, self.l.intrinsics, self.size, cv2.CV_32FC1,
+                                       self.l.mapx, self.l.mapy)
+            cv2.fisheye.initUndistortRectifyMap(self.r.intrinsics, self.r.distortion, self.r.R, self.r.intrinsics, self.size, cv2.CV_32FC1,
+                                       self.r.mapx, self.r.mapy)
+        else:
+            print("Something went wrong when selecting a model")
 
     def as_message(self):
         """
         Return the camera calibration as a pair of CameraInfo messages, for left
         and right cameras respectively.
         """
 
-        return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size),
-                self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size))
+        return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size, self.l.camera_model),
+                self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size, self.r.camera_model))
 
     def from_message(self, msgs, alpha = 0.0):
         """ Initialize the camera calibration from a pair of CameraInfo messages.  """
@@ -1014,7 +1096,7 @@ def ost(self):
           self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P, self.size))
 
     def yaml(self, suffix, info):
-        return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size)
+        return self.lryaml(self.name + suffix, info.distortion, info.intrinsics, info.R, info.P, self.size,self.camera_model)
 
     # TODO Get rid of "from_images" versions of these, instead have function to get undistorted corners
     def epipolar_error_from_images(self, limage, rimage):

camera_calibration/src/camera_calibration/camera_calibrator.py

@@ -47,7 +47,7 @@
     from queue import Queue
 except ImportError:
     from Queue import Queue
-
+from calibrator import CAMERA_MODEL
 
 class BufferQueue(Queue):
     """Slight modification of the standard Queue that discards the oldest item
@@ -81,7 +81,9 @@ def __init__(self, queue, opencv_calibration_node):
     def run(self):
         cv2.namedWindow("display", cv2.WINDOW_NORMAL)
         cv2.setMouseCallback("display", self.opencv_calibration_node.on_mouse)
+        cv2.createTrackbar("Camera type: \n 0 : pinhole \n 1 : fisheye", "display", 0,1, self.opencv_calibration_node.on_model_change)
         cv2.createTrackbar("scale", "display", 0, 100, self.opencv_calibration_node.on_scale)
+
         while True:
             if self.queue.qsize() > 0:
                 self.image = self.queue.get()
@@ -273,6 +275,8 @@ def on_mouse(self, event, x, y, flags, param):
                     # Only shut down if we set camera info correctly, #3993
                     if self.do_upload():
                         rospy.signal_shutdown('Quit')
+    def on_model_change(self, model_select_val):
+        self.c.set_cammodel( CAMERA_MODEL.PINHOLE if model_select_val < 0.5 else CAMERA_MODEL.FISHEYE)
 
     def on_scale(self, scalevalue):
         if self.c.calibrated: