From e4eda5fe91575b65eb3615b716fe41466592c49c Mon Sep 17 00:00:00 2001
From: Gabor Soros <gabor.soros@nokia-bell-labs.com>
Date: Fri, 12 Jun 2020 11:51:30 +0200
Subject: [PATCH 1/7] trimmed whitespace at line endings

---
 .../src/camera_calibration/calibrator.py         | 16 ++++++++--------
 .../src/camera_calibration/camera_calibrator.py  |  2 +-
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py
index 6ee8814e8..ca8747142 100644
--- a/camera_calibration/src/camera_calibration/calibrator.py
+++ b/camera_calibration/src/camera_calibration/calibrator.py
@@ -156,7 +156,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))
@@ -513,7 +513,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.
     """
@@ -535,7 +535,7 @@ def __init__(self):
         ImageDrawable.__init__(self)
         self.scrib = None
         self.linear_error = -1.0
-                
+
 
 class StereoDrawable(ImageDrawable):
     def __init__(self):
@@ -590,10 +590,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 ]
 
@@ -882,7 +882,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
@@ -926,7 +926,7 @@ def set_alpha(self, a):
                          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,
@@ -1140,7 +1140,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)
diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py
index 98d0c729e..947054795 100755
--- a/camera_calibration/src/camera_calibration/camera_calibrator.py
+++ b/camera_calibration/src/camera_calibration/camera_calibrator.py
@@ -59,7 +59,7 @@ def __init__(self, node):
 
     def run(self):
         rclpy.spin(self.node)
-        
+
 
 class ConsumerThread(threading.Thread):
     def __init__(self, queue, function):

From 8f70c2e73ebb062967786f349cdf7861c6edbeed Mon Sep 17 00:00:00 2001
From: Gabor Soros <gabor.soros@nokia-bell-labs.com>
Date: Sat, 13 Jun 2020 09:29:27 +0200
Subject: [PATCH 2/7] implemented fisheye mono and stereo calibration based on
 the melodic branch

---
 .../src/camera_calibration/calibrator.py      | 318 +++++++++++++-----
 .../camera_calibration/camera_calibrator.py   |  32 +-
 2 files changed, 249 insertions(+), 101 deletions(-)

diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py
index ca8747142..fa707d861 100644
--- a/camera_calibration/src/camera_calibration/calibrator.py
+++ b/camera_calibration/src/camera_calibration/calibrator.py
@@ -45,6 +45,10 @@
 import time
 from distutils.version import LooseVersion
 
+# Supported camera models
+class CAMERA_MODEL(Enum):
+    PINHOLE = 0
+    FISHEYE = 1
 
 # Supported calibration patterns
 class Patterns:
@@ -208,13 +212,26 @@ 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():
     """
     Base class for calibration system
     """
-    def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checkerboard_flags=cv2.CALIB_CB_FAST_CHECK):
+    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:
             # Make sure n_cols > n_rows to agree with OpenCV CB detector output
@@ -229,10 +246,11 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checke
         # Set to true after we perform calibration
         self.calibrated = False
         self.calib_flags = flags
+        self.fisheye_calib_flags = fisheye_flags
         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.
@@ -243,6 +261,8 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='', checke
         self.goodenough = False
         self.param_ranges = [0.7, 0.7, 0.4, 0.5]
         self.name = name
+        self.last_frame_corners = None
+        self.max_chessboard_speed = max_chessboard_speed
 
     def mkgray(self, msg):
         """
@@ -285,7 +305,24 @@ def get_parameters(self, corners, board, size):
         params = [p_x, p_y, p_size, skew]
         return params
 
-    def is_good_sample(self, params):
+    def set_cammodel(self, modeltype):
+        self.camera_model = modeltype
+
+    def is_slow_moving(self, corners, last_frame_corners):
+        """
+        Returns true if the motion of the checkerboard is sufficiently low between
+        this and the previous frame.
+        """
+        # If we don't have previous frame corners, we can't accept the sample
+        if last_frame_corners is None:
+            return False
+        num_corners = len(corners)
+        corner_deltas = (corners - last_frame_corners).reshape(num_corners, 2)
+        # Average distance travelled overall for all corners
+        average_motion = numpy.average(numpy.linalg.norm(corner_deltas, axis = 1))
+        return average_motion <= self.max_chessboard_speed
+
+    def is_good_sample(self, params, corners, last_frame_corners):
         """
         Returns true if the checkerboard detection described by params should be added to the database.
         """
@@ -299,7 +336,15 @@ def param_distance(p1, p2):
         d = min([param_distance(params, p) for p in db_params])
         #print "d = %.3f" % d #DEBUG
         # TODO What's a good threshold here? Should it be configurable?
-        return d > 0.2
+        if d <= 0.2:
+            return False
+
+        if self.max_chessboard_speed > 0:
+            if not self.is_slow_moving(corners, last_frame_corners):
+                return False
+
+        # All tests passed, image should be good for calibration
+        return True
 
     _param_names = ["X", "Y", "Size", "Skew"]
 
@@ -422,28 +467,31 @@ def downsample_and_detect(self, img):
 
         return (scrib, corners, downsampled_corners, board, (x_scale, y_scale))
 
-
-    def lrmsg(self, d, k, r, p):
+    @staticmethod
+    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) = self.size
-        if d.size > 5:
-            msg.distortion_model = "rational_polynomial"
-        else:
-            msg.distortion_model = "plumb_bob"
+        (msg.width, msg.height) = size
+        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()
         msg.p = numpy.ravel(p).copy().tolist()
         return msg
 
-    def lrreport(self, d, k, r, p):
+    @staticmethod
+    def lrreport(d, k, r, p):
         print("D = ", numpy.ravel(d).tolist())
         print("K = ", numpy.ravel(k).tolist())
         print("R = ", numpy.ravel(r).tolist())
         print("P = ", numpy.ravel(p).tolist())
 
-    def lrost(self, name, d, k, r, p):
+    @staticmethod
+    def lrost(name, d, k, r, p, size):
+        assert k.shape == (3, 3)
+        assert r.shape == (3, 3)
+        assert p.shape == (3, 4)
         calmessage = (
         "# oST version 5.0 parameters\n"
         + "\n"
@@ -451,10 +499,10 @@ def lrost(self, name, d, k, r, p):
         + "[image]\n"
         + "\n"
         + "width\n"
-        + str(self.size[0]) + "\n"
+        + str(size[0]) + "\n"
         + "\n"
         + "height\n"
-        + str(self.size[1]) + "\n"
+        + str(size[1]) + "\n"
         + "\n"
         + "[%s]" % name + "\n"
         + "\n"
@@ -479,16 +527,22 @@ def lrost(self, name, d, k, r, p):
         assert len(calmessage) < 525, "Calibration info must be less than 525 bytes"
         return calmessage
 
-    def lryaml(self, name, d, k, r, p):
+    @staticmethod
+    def lryaml(name, d, k, r, p, size, cam_model):
+        dist_model = _get_dist_model(d, cam_model)
+
+        assert k.shape == (3, 3)
+        assert r.shape == (3, 3)
+        assert p.shape == (3, 4)
         calmessage = (""
-        + "image_width: " + str(self.size[0]) + "\n"
-        + "image_height: " + str(self.size[1]) + "\n"
+        + "image_width: " + str(size[0]) + "\n"
+        + "image_height: " + str(size[1]) + "\n"
         + "camera_name: " + name + "\n"
         + "camera_matrix:\n"
         + "  rows: 3\n"
         + "  cols: 3\n"
         + "  data: [" + ", ".join(["%8f" % i for i in k.reshape(1,9)[0]]) + "]\n"
-        + "distortion_model: " + ("rational_polynomial" if d.size > 5 else "plumb_bob") + "\n"
+        + "distortion_model: " + dist_model + "\n"
         + "distortion_coefficients:\n"
         + "  rows: 1\n"
         + "  cols: 5\n"
@@ -536,7 +590,6 @@ def __init__(self):
         self.scrib = None
         self.linear_error = -1.0
 
-
 class StereoDrawable(ImageDrawable):
     def __init__(self):
         ImageDrawable.__init__(self)
@@ -592,27 +645,41 @@ def cal_fromcorners(self, good):
         """
         :param good: Good corner positions and boards
         :type good: [(corners, ChessboardInfo)]
-
-
         """
         boards = [ b for (_, b) in good ]
 
         ipts = [ points for (points, _) in good ]
         opts = self.mk_object_points(boards)
-
-        self.intrinsics = numpy.zeros((3, 3), numpy.float64)
-        if self.calib_flags & cv2.CALIB_RATIONAL_MODEL:
-            self.distortion = numpy.zeros((8, 1), numpy.float64) # rational polynomial
-        else:
-            self.distortion = numpy.zeros((5, 1), numpy.float64) # plumb bob
         # If FIX_ASPECT_RATIO flag set, enforce focal lengths have 1/1 ratio
-        self.intrinsics[0,0] = 1.0
-        self.intrinsics[1,1] = 1.0
-        cv2.calibrateCamera(
-                   opts, ipts,
-                   self.size, self.intrinsics,
-                   self.distortion,
-                   flags = self.calib_flags)
+        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,
+                    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.
+            if self.calib_flags & cv2.CALIB_RATIONAL_MODEL:
+                self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) # rational polynomial
+            else:
+                self.distortion = dist_coeffs.flat[:5].reshape(-1, 1) # plumb bob
+        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, dist_coeffs, rvecs, tvecs = cv2.fisheye.calibrate(
+                    opts, ipts, self.size,
+                    intrinsics_in,
+                    None,
+                    flags = self.fisheye_calib_flags)
+            self.distortion = dist_coeffs.flat[:4].reshape(-1, 1) # Kannala-Brandt
 
         # R is identity matrix for monocular calibration
         self.R = numpy.eye(3, dtype=numpy.float64)
@@ -628,14 +695,22 @@ 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:
+            # NOTE: cv2.fisheye.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)
+
 
     def remap(self, src):
         """
@@ -653,12 +728,14 @@ def undistort_points(self, src):
 
         Apply the post-calibration undistortion to the source points
         """
-
-        return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P)
+        if self.camera_model == CAMERA_MODEL.PINHOLE:
+            return cv2.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P)
+        elif self.camera_model == CAMERA_MODEL.FISHEYE:
+            return cv2.fisheye.undistortPoints(src, self.intrinsics, self.distortion, R = self.R, P = self.P)
 
     def as_message(self):
         """ Return the camera calibration as a CameraInfo message """
-        return self.lrmsg(self.distortion, self.intrinsics, self.R, self.P)
+        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 """
@@ -675,10 +752,10 @@ def report(self):
         self.lrreport(self.distortion, self.intrinsics, self.R, self.P)
 
     def ost(self):
-        return self.lrost(self.name, self.distortion, self.intrinsics, self.R, self.P)
+        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)
+        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):
         """
@@ -763,11 +840,12 @@ def handle_msg(self, msg):
 
                 # Add sample to database only if it's sufficiently different from any previous sample.
                 params = self.get_parameters(corners, board, (gray.shape[1], gray.shape[0]))
-                if self.is_good_sample(params):
+                if self.is_good_sample(params, corners, self.last_frame_corners):
                     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
         rv.params = self.compute_goodenough()
@@ -779,11 +857,11 @@ def do_calibration(self, dump = False):
             print("**** Collecting corners for all images! ****") #DEBUG
             images = [i for (p, i) in self.db]
             self.good_corners = self.collect_corners(images)
+        self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally
         # Dump should only occur if user wants it
         if dump:
             pickle.dump((self.is_mono, self.size, self.good_corners),
                         open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w"))
-        self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally
         self.cal_fromcorners(self.good_corners)
         self.calibrated = True
         # DEBUG
@@ -841,6 +919,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
@@ -889,23 +973,48 @@ 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)
-        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)
+
+        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,
+                                   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:
+            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,
+                                   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)
 
         self.set_alpha(0.0)
 
@@ -916,21 +1025,47 @@ 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)
 
     def as_message(self):
         """
@@ -938,8 +1073,8 @@ def as_message(self):
         and right cameras respectively.
         """
 
-        return (self.lrmsg(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P),
-                self.lrmsg(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P))
+        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.  """
@@ -959,15 +1094,15 @@ def report(self):
         self.lrreport(self.l.distortion, self.l.intrinsics, self.l.R, self.l.P)
         print("\nRight:")
         self.lrreport(self.r.distortion, self.r.intrinsics, self.r.R, self.r.P)
-        print("self.T ", numpy.ravel(self.T).tolist())
-        print("self.R ", numpy.ravel(self.R).tolist())
+        print("self.T =", numpy.ravel(self.T).tolist())
+        print("self.R =", numpy.ravel(self.R).tolist())
 
     def ost(self):
-        return (self.lrost(self.name + "/left", self.l.distortion, self.l.intrinsics, self.l.R, self.l.P) +
-          self.lrost(self.name + "/right", self.r.distortion, self.r.intrinsics, self.r.R, self.r.P))
+        return (self.lrost(self.name + "/left", self.l.distortion, self.l.intrinsics, self.l.R, self.l.P, self.size) +
+          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)
+        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):
@@ -1084,11 +1219,12 @@ def handle_msg(self, msg):
             # Add sample to database only if it's sufficiently different from any previous sample
             if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners):
                 params = self.get_parameters(lcorners, lboard, (lgray.shape[1], lgray.shape[0]))
-                if self.is_good_sample(params):
+                if self.is_good_sample(params, lcorners, self.last_frame_corners):
                     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
         rv.rscrib = rscrib
@@ -1098,11 +1234,11 @@ def handle_msg(self, msg):
 
     def do_calibration(self, dump = False):
         # TODO MonoCalibrator collects corners if needed here
+        self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally
         # Dump should only occur if user wants it
         if dump:
             pickle.dump((self.is_mono, self.size, self.good_corners),
                         open("/tmp/camera_calibration_%08x.pickle" % random.getrandbits(32), "w"))
-        self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) # TODO Needs to be set externally
         self.l.size = self.size
         self.r.size = self.size
         self.cal_fromcorners(self.good_corners)
diff --git a/camera_calibration/src/camera_calibration/camera_calibrator.py b/camera_calibration/src/camera_calibration/camera_calibrator.py
index 947054795..94d9dd728 100755
--- a/camera_calibration/src/camera_calibration/camera_calibrator.py
+++ b/camera_calibration/src/camera_calibration/camera_calibrator.py
@@ -77,7 +77,8 @@ def run(self):
 
 class CalibrationNode(Node):
     def __init__(self, name, boards, service_check = True, synchronizer = message_filters.TimeSynchronizer, flags = 0,
-                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0):
+                 pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0,
+                 max_chessboard_speed = -1):
         super().__init__(name)
 
         self.set_camera_info_service = self.create_client(sensor_msgs.srv.SetCameraInfo,
@@ -104,9 +105,11 @@ def __init__(self, name, boards, service_check = True, synchronizer = message_fi
 
         self._boards = boards
         self._calib_flags = flags
+        self._fisheye_calib_flags = fisheye_flags
         self._checkerboard_flags = checkerboard_flags
         self._pattern = pattern
         self._camera_name = camera_name
+        self._max_chessboard_speed = max_chessboard_speed
         lsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'left')
         rsub = message_filters.Subscriber(self, sensor_msgs.msg.Image, 'right')
         ts = synchronizer([lsub, rsub], 4)
@@ -142,11 +145,13 @@ def queue_stereo(self, lmsg, rmsg):
     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)
+                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._pattern,
-                                        checkerboard_flags=self.checkerboard_flags)
+                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)
 
         # This should just call the MonoCalibrator
         drawable = self.c.handle_msg(msg)
@@ -156,11 +161,13 @@ def handle_monocular(self, msg):
     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)
+                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._pattern,
-                                          checkerboard_flags=self._checkerboard_flags)
+                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)
 
         drawable = self.c.handle_msg(msg)
         self.displaywidth = drawable.lscrib.shape[1] + drawable.rscrib.shape[1]
@@ -238,6 +245,7 @@ def spin(self):
     def initWindow(self):
         cv2.namedWindow("display", cv2.WINDOW_NORMAL)
         cv2.setMouseCallback("display", self.on_mouse)
+        cv2.createTrackbar("Camera type: \n 0 : pinhole \n 1 : fisheye", "display", 0,1, self.on_model_change)
         cv2.createTrackbar("scale", "display", 0, 100, self.on_scale)
 
     @classmethod
@@ -252,6 +260,7 @@ 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()
             if self.c.calibrated:
                 if 280 <= y < 380:
@@ -261,6 +270,9 @@ def on_mouse(self, event, x, y, flags, param):
                     if self.do_upload():
                         rclpy.shutdown()
 
+    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:
             self.c.set_alpha(scalevalue / 100.0)
@@ -291,6 +303,7 @@ def screendump(self, im):
         while os.access("/tmp/dump%d.png" % i, os.R_OK):
             i += 1
         cv2.imwrite("/tmp/dump%d.png" % i, im)
+        print("Saved screen dump to /tmp/dump%d.png" % i)
 
     def redraw_monocular(self, drawable):
         height = drawable.scrib.shape[0]
@@ -300,7 +313,6 @@ def redraw_monocular(self, drawable):
         display[0:height, 0:width,:] = drawable.scrib
         display[0:height, width:width+100,:].fill(255)
 
-
         self.buttons(display)
         if not self.c.calibrated:
             if drawable.params:

From f48b7aec663c2710b06c18475bdfc8e77fe66e3e Mon Sep 17 00:00:00 2001
From: Patrick Musau <pmusau13ster@gmail.com>
Date: Tue, 29 Jun 2021 10:07:23 -0500
Subject: [PATCH 3/7] rebase change

---
 camera_calibration/pytest.ini | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/camera_calibration/pytest.ini b/camera_calibration/pytest.ini
index fe55d2ed6..0452f1275 100644
--- a/camera_calibration/pytest.ini
+++ b/camera_calibration/pytest.ini
@@ -1,2 +1 @@
-[pytest]
-junit_family=xunit2
+junit_family=xunit1

From fd85251fdd885dfc23ec931cf779e252ce45e2b5 Mon Sep 17 00:00:00 2001
From: Patrick Musau <pmusau13ster@gmail.com>
Date: Tue, 29 Jun 2021 10:44:38 -0500
Subject: [PATCH 4/7] fixes to pass tests

---
 camera_calibration/pytest.ini                           | 3 ++-
 camera_calibration/src/camera_calibration/calibrator.py | 1 +
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/camera_calibration/pytest.ini b/camera_calibration/pytest.ini
index 0452f1275..fe55d2ed6 100644
--- a/camera_calibration/pytest.ini
+++ b/camera_calibration/pytest.ini
@@ -1 +1,2 @@
-junit_family=xunit1
+[pytest]
+junit_family=xunit2
diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py
index fa707d861..9d5951cd4 100644
--- a/camera_calibration/src/camera_calibration/calibrator.py
+++ b/camera_calibration/src/camera_calibration/calibrator.py
@@ -44,6 +44,7 @@
 import tarfile
 import time
 from distutils.version import LooseVersion
+from enum import Enum
 
 # Supported camera models
 class CAMERA_MODEL(Enum):

From c9a5bf30d6fb85553d720b6ad610a82ea8321238 Mon Sep 17 00:00:00 2001
From: Gabor Soros <gabor.soros@nokia-bell-labs.com>
Date: Wed, 15 Jul 2020 16:50:56 +0200
Subject: [PATCH 5/7] update pytest.ini

---
 camera_calibration/pytest.ini | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/camera_calibration/pytest.ini b/camera_calibration/pytest.ini
index fe55d2ed6..170c18d95 100644
--- a/camera_calibration/pytest.ini
+++ b/camera_calibration/pytest.ini
@@ -1,2 +1,8 @@
+<<<<<<< HEAD
 [pytest]
 junit_family=xunit2
+=======
+# pytest.ini
+[pytest]
+junit_family=xunit1
+>>>>>>> update pytest.ini

From d72ff5129719c6132a79c59d46be04bd37e549ff Mon Sep 17 00:00:00 2001
From: Gabor Soros <gabor.soros@nokia-bell-labs.com>
Date: Wed, 19 Aug 2020 09:16:57 +0200
Subject: [PATCH 6/7] added missing imports

---
 camera_calibration/src/camera_calibration/calibrator.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py
index 9d5951cd4..f60b93a4b 100644
--- a/camera_calibration/src/camera_calibration/calibrator.py
+++ b/camera_calibration/src/camera_calibration/calibrator.py
@@ -43,6 +43,7 @@
 import sensor_msgs.msg
 import tarfile
 import time
+import sys
 from distutils.version import LooseVersion
 from enum import Enum
 

From d5ecb86d4102525b3ee9edf2d322aad7fb5fd280 Mon Sep 17 00:00:00 2001
From: Patrick Musau <pmusau13ster@gmail.com>
Date: Tue, 29 Jun 2021 10:59:45 -0500
Subject: [PATCH 7/7] revert back

---
 camera_calibration/pytest.ini | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/camera_calibration/pytest.ini b/camera_calibration/pytest.ini
index 170c18d95..fe55d2ed6 100644
--- a/camera_calibration/pytest.ini
+++ b/camera_calibration/pytest.ini
@@ -1,8 +1,2 @@
-<<<<<<< HEAD
 [pytest]
 junit_family=xunit2
-=======
-# pytest.ini
-[pytest]
-junit_family=xunit1
->>>>>>> update pytest.ini