Add Fisheye calibration tool (#440)

* Add Fisheye calibration tool

* Add calibration for fisheye cameras
Fix #146

* Correct typo

* Restore camera_calib files permisions

* Upgrades to calibrator tool for multi model calibration

* Solve fisheye balance selection TODO:
For some reason estimateNewCameraMatrixForUndistortRectify is not producing the expected result, hence a workaround was implemented

* Add fisheye calibration flags as user arguments

* Add undistortion of points for fisheye

* cam_calib: rolling back flags
Rolling back changes to previous commit on camera calibrator flags to enable backwards compatibility

* cam_calib: Style formating

camera_calibration/src/camera_calibration/calibrator.py

@@ -45,6 +45,7 @@
 import time
 import sys
 from distutils.version import LooseVersion
+import sys
 from enum import Enum
 
 # Supported camera models
@@ -306,6 +307,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 set_cammodel(self, modeltype):
         self.camera_model = modeltype
@@ -469,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()
@@ -534,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)
@@ -552,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,

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
@@ -91,8 +91,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, max_chessboard_speed = -1,
-                 queue_size = 1):
+                 fisheye_flags = 0, pattern=Patterns.Chessboard, camera_name='', checkerboard_flags = 0,
+                 max_chessboard_speed = -1, queue_size = 1):
         super().__init__(name)
 
         self.set_camera_info_service = self.create_client(sensor_msgs.srv.SetCameraInfo,
@@ -287,6 +287,8 @@ def on_mouse(self, event, x, y, flags, param):
                     # Only shut down if we set camera info correctly, #3993
                     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_model_change(self, model_select_val):
         self.c.set_cammodel( CAMERA_MODEL.PINHOLE if model_select_val < 0.5 else CAMERA_MODEL.FISHEYE)

camera_calibration/src/camera_calibration/nodes/cameracalibrator.py

@@ -74,21 +74,40 @@ def main():
     group = OptionGroup(parser, "Calibration Optimizer Options")
     group.add_option("--fix-principal-point",
                      action="store_true", default=False,
-                     help="fix the principal point at the image center")
+                     help="for pinhole, fix the principal point at the image center")
     group.add_option("--fix-aspect-ratio",
                      action="store_true", default=False,
-                     help="enforce focal lengths (fx, fy) are equal")
+                     help="for pinhole, enforce focal lengths (fx, fy) are equal")
     group.add_option("--zero-tangent-dist",
                      action="store_true", default=False,
-                     help="set tangential distortion coefficients (p1, p2) to zero")
+                     help="for pinhole, set tangential distortion coefficients (p1, p2) to zero")
     group.add_option("-k", "--k-coefficients",
                      type="int", default=2, metavar="NUM_COEFFS",
-                     help="number of radial distortion coefficients to use (up to 6, default %default)")
+                     help="for pinhole, number of radial distortion coefficients to use (up to 6, default %default)")
+
+    group.add_option("--fisheye-recompute-extrinsicsts",
+                     action="store_true", default=False,
+                     help="for fisheye, extrinsic will be recomputed after each iteration of intrinsic optimization")
+    group.add_option("--fisheye-fix-skew",
+                     action="store_true", default=False,
+                     help="for fisheye, skew coefficient (alpha) is set to zero and stay zero")
+    group.add_option("--fisheye-fix-principal-point",
+                     action="store_true", default=False,
+                     help="for fisheye,fix the principal point at the image center")
+    group.add_option("--fisheye-k-coefficients",
+                     type="int", default=4, metavar="NUM_COEFFS",
+                     help="for fisheye, number of radial distortion coefficients to use fixing to zero the rest (up to 4, default %default)")
+
+    group.add_option("--fisheye-check-conditions",
+                     action="store_true", default=False,
+                     help="for fisheye, the functions will check validity of condition number")
+
     group.add_option("--disable_calib_cb_fast_check", action='store_true', default=False,
                      help="uses the CALIB_CB_FAST_CHECK flag for findChessboardCorners")
     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.")
+
     parser.add_option_group(group)
     options, _ = parser.parse_args(rclpy.utilities.remove_ros_args())
 
@@ -109,6 +128,7 @@ def main():
     else:
         sync = functools.partial(ApproximateTimeSynchronizer, slop=options.approximate)
 
+    # Pinhole opencv calibration options parsing
     num_ks = options.k_coefficients
 
     calib_flags = 0
@@ -133,6 +153,26 @@ def main():
     if (num_ks < 1):
         calib_flags |= cv2.CALIB_FIX_K1
 
+    # Opencv calibration flags parsing:
+    num_ks = options.fisheye_k_coefficients
+    fisheye_calib_flags = 0
+    if options.fisheye_fix_principal_point:
+        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_PRINCIPAL_POINT
+    if options.fisheye_fix_skew:
+        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_SKEW
+    if options.fisheye_recompute_extrinsicsts:
+        fisheye_calib_flags |= cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC
+    if options.fisheye_check_conditions:
+        fisheye_calib_flags |= cv2.fisheye.CALIB_CHECK_COND
+    if (num_ks < 4):
+        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K4
+    if (num_ks < 3):
+        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K3
+    if (num_ks < 2):
+        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K2
+    if (num_ks < 1):
+        fisheye_calib_flags |= cv2.fisheye.CALIB_FIX_K1
+
     pattern = Patterns.Chessboard
     if options.pattern == 'circles':
         pattern = Patterns.Circles
@@ -147,7 +187,8 @@ def main():
         checkerboard_flags = cv2.CALIB_CB_FAST_CHECK
 
     rclpy.init()
-    node = OpenCVCalibrationNode("cameracalibrator", boards, options.service_check, sync, calib_flags, pattern, options.camera_name,
+    node = OpenCVCalibrationNode("cameracalibrator", 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)
     node.spin()