Skip to content

Commit

Permalink
Add Fisheye calibration tool (ros-perception#440)
Browse files Browse the repository at this point in the history 
* Add Fisheye calibration tool

* Add calibration for fisheye cameras
Fix ros-perception#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
  • Loading branch information
@DavidTorresOcana
DavidTorresOcana authored and wep21 committed Oct 17, 2021
1 parent f7ae9b8 commit 3756f07
Show file tree
Hide file tree
Showing 3 changed files with 200 additions and 80 deletions.
211 changes: 143 additions & 68 deletions camera_calibration/src/camera_calibration/calibrator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,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:
Expand Down Expand Up @@ -208,13 +214,25 @@ 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='',
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:
Expand All @@ -230,10 +248,11 @@ def __init__(self, boards, flags=0, pattern=Patterns.Chessboard, name='',
# 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.
Expand Down Expand Up @@ -287,6 +306,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):
"""
Expand Down Expand Up @@ -447,16 +468,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()
Expand Down Expand Up @@ -512,13 +530,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)
Expand All @@ -530,7 +550,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,
Expand Down Expand Up @@ -578,7 +598,6 @@ def __init__(self):
ImageDrawable.__init__(self)
self.scrib = None
self.linear_error = -1.0


class StereoDrawable(ImageDrawable):
def __init__(self):
Expand Down Expand Up @@ -642,18 +661,23 @@ 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:
reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.fisheye.calibrate(
opts, ipts, self.size,
intrinsics_in, None, flags = self.fisheye_calib_flags)

# R is identity matrix for monocular calibration
self.R = numpy.eye(3, dtype=numpy.float64)
Expand All @@ -669,14 +693,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: 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):
"""
Expand All @@ -694,12 +726,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, 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 """
Expand All @@ -719,7 +753,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):
"""
Expand Down Expand Up @@ -931,23 +965,38 @@ 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:
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)

self.set_alpha(0.0)

Expand All @@ -958,30 +1007,56 @@ 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):
"""
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. """
Expand Down Expand Up @@ -1009,7 +1084,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):
Expand Down
Loading

0 comments on commit 3756f07

Please sign in to comment.