From 4928023f1cbb9eb9baaa33039e47b7b212c27b00 Mon Sep 17 00:00:00 2001 From: Myron Rodrigues <41271144+MRo47@users.noreply.github.com> Date: Tue, 20 Aug 2024 09:55:07 +0100 Subject: [PATCH] Refactoring calibration code (#1000) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- camera_calibration/doc/api.rst | 4 +- .../src/camera_calibration/calibrator.py | 820 +----------------- .../camera_calibration/camera_calibrator.py | 18 +- .../src/camera_calibration/camera_checker.py | 20 +- .../src/camera_calibration/mono_calibrator.py | 414 +++++++++ .../nodes/tarfile_calibration.py | 8 +- .../camera_calibration/stereo_calibrator.py | 501 +++++++++++ camera_calibration/test/test_directed.py | 6 +- .../test/test_multiple_boards.py | 5 +- 9 files changed, 949 insertions(+), 847 deletions(-) create mode 100644 camera_calibration/src/camera_calibration/mono_calibrator.py create mode 100644 camera_calibration/src/camera_calibration/stereo_calibrator.py diff --git a/camera_calibration/doc/api.rst b/camera_calibration/doc/api.rst index b898197c5..d998cc7e6 100644 --- a/camera_calibration/doc/api.rst +++ b/camera_calibration/doc/api.rst @@ -1,8 +1,8 @@ API Documentation ================= -.. autoclass:: camera_calibration.calibrator.MonoCalibrator +.. autoclass:: camera_calibration.mono_calibrator.MonoCalibrator :members: -.. autoclass:: camera_calibration.calibrator.StereoCalibrator +.. autoclass:: camera_calibration.stereo_calibrator.StereoCalibrator :members: diff --git a/camera_calibration/src/camera_calibration/calibrator.py b/camera_calibration/src/camera_calibration/calibrator.py index 5a21c3c02..6ad10c738 100644 --- a/camera_calibration/src/camera_calibration/calibrator.py +++ b/camera_calibration/src/camera_calibration/calibrator.py @@ -32,19 +32,12 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -from io import BytesIO import cv2 import cv_bridge -import image_geometry import math import numpy.linalg -import pickle -import random import sensor_msgs.msg -import sys import tarfile -import time -from semver import VersionInfo from enum import Enum # Supported camera models @@ -385,7 +378,7 @@ def __init__(self, boards, flags=0, fisheye_flags=0, pattern=Patterns.Chessboard # We end up having to check both ways anyway self._boards = boards else: - raise CalibratorException( + raise CalibrationException( 'pattern must be one of: Chessboard, Circles, ACircles, or ChArUco') # Set to true after we perform calibration @@ -778,814 +771,3 @@ class ImageDrawable(): def __init__(self): self.params = None - - -class MonoDrawable(ImageDrawable): - def __init__(self): - ImageDrawable.__init__(self) - self.scrib = None - self.linear_error = -1.0 - - -class StereoDrawable(ImageDrawable): - def __init__(self): - ImageDrawable.__init__(self) - self.lscrib = None - self.rscrib = None - self.epierror = -1 - self.dim = -1 - - -class MonoCalibrator(Calibrator): - """ - Calibration class for monocular cameras:: - - images = [cv2.imread("mono%d.png") for i in range(8)] - mc = MonoCalibrator() - mc.cal(images) - print mc.as_message() - """ - - is_mono = True # TODO Could get rid of is_mono - - def __init__(self, *args, **kwargs): - if 'name' not in kwargs: - kwargs['name'] = 'narrow_stereo/left' - super(MonoCalibrator, self).__init__(*args, **kwargs) - - def cal(self, images): - """ - Calibrate camera from given images - """ - goodcorners = self.collect_corners(images) - self.cal_fromcorners(goodcorners) - self.calibrated = True - - def collect_corners(self, images): - """ - :param images: source images containing chessboards - :type images: list of :class:`cvMat` - - Find chessboards in all images. - - Return [ (corners, ids, ChessboardInfo) ] - """ - self.size = (images[0].shape[1], images[0].shape[0]) - corners = [self.get_corners(i) for i in images] - - goodcorners = [(co, ids, b) for (ok, co, ids, b) in corners if ok] - if not goodcorners: - raise CalibrationException("No corners found in images!") - return goodcorners - - def cal_fromcorners(self, good): - """ - :param good: Good corner positions and boards - :type good: [(corners, ChessboardInfo)] - """ - - (ipts, ids, boards) = zip(*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) - - if self.pattern == Patterns.ChArUco: - if self.camera_model == CAMERA_MODEL.FISHEYE: - raise NotImplemented( - "Can't perform fisheye calibration with ChArUco board") - - reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.aruco.calibrateCameraCharuco( - ipts, ids, boards[0].charuco_board, self.size, intrinsics_in, None) - - elif 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: - # rational polynomial - self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) - else: - # plumb bob - self.distortion = dist_coeffs.flat[:5].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) - - # R is identity matrix for monocular calibration - self.R = numpy.eye(3, dtype=numpy.float64) - self.P = numpy.zeros((3, 4), dtype=numpy.float64) - - self.set_alpha(0.0) - - def set_alpha(self, a): - """ - Set the alpha value for the calibrated camera solution. The alpha - value is a zoom, and ranges from 0 (zoomed in, all pixels in - calibrated image are valid) to 1 (zoomed out, all pixels in - original image are in calibrated image). - """ - - 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): - """ - :param src: source image - :type src: :class:`cvMat` - - Apply the post-calibration undistortion to the source image - """ - return cv2.remap(src, self.mapx, self.mapy, cv2.INTER_LINEAR) - - def undistort_points(self, src): - """ - :param src: N source pixel points (u,v) as an Nx2 matrix - :type src: :class:`cvMat` - - Apply the post-calibration undistortion to the source points - """ - 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, self.camera_model) - - def from_message(self, msg, alpha=0.0): - """ Initialize the camera calibration from a CameraInfo message """ - - self.size = (msg.width, msg.height) - self.intrinsics = numpy.array( - msg.k, dtype=numpy.float64, copy=True).reshape((3, 3)) - self.distortion = numpy.array( - msg.d, dtype=numpy.float64, copy=True).reshape((len(msg.d), 1)) - self.R = numpy.array(msg.r, dtype=numpy.float64, - copy=True).reshape((3, 3)) - self.P = numpy.array(msg.p, dtype=numpy.float64, - copy=True).reshape((3, 4)) - - self.set_alpha(0.0) - - 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, self.size) - - def yaml(self): - 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): - """ - Detect the checkerboard and compute the linear error. - Mainly for use in tests. - """ - _, corners, _, ids, board, _ = self.downsample_and_detect(image) - if corners is None: - return None - - undistorted = self.undistort_points(corners) - return self.linear_error(undistorted, ids, board) - - @staticmethod - def linear_error(corners, ids, b): - """ - Returns the linear error for a set of corners detected in the unrectified image. - """ - - if corners is None: - return None - - corners = numpy.squeeze(corners) - - def pt2line(x0, y0, x1, y1, x2, y2): - """ point is (x0, y0), line is (x1, y1, x2, y2) """ - return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + - (y2 - y1) ** 2) - - n_cols = b.n_cols - n_rows = b.n_rows - if b.pattern == 'charuco': - n_cols -= 1 - n_rows -= 1 - n_pts = n_cols * n_rows - - if ids is None: - ids = numpy.arange(n_pts).reshape((n_pts, 1)) - - ids_to_idx = dict((ids[i, 0], i) for i in range(len(ids))) - - errors = [] - for row in range(n_rows): - row_min = row * n_cols - row_max = (row+1) * n_cols - pts_in_row = [x for x in ids if row_min <= x < row_max] - - # not enough points to calculate error - if len(pts_in_row) <= 2: - continue - - left_pt = min(pts_in_row)[0] - right_pt = max(pts_in_row)[0] - x_left = corners[ids_to_idx[left_pt], 0] - y_left = corners[ids_to_idx[left_pt], 1] - x_right = corners[ids_to_idx[right_pt], 0] - y_right = corners[ids_to_idx[right_pt], 1] - - for pt in pts_in_row: - if pt[0] in (left_pt, right_pt): - continue - x = corners[ids_to_idx[pt[0]], 0] - y = corners[ids_to_idx[pt[0]], 1] - errors.append(pt2line(x, y, x_left, y_left, x_right, y_right)) - - if errors: - return math.sqrt(sum([e**2 for e in errors]) / len(errors)) - else: - return None - - def handle_msg(self, msg): - """ - Detects the calibration target and, if found and provides enough new information, - adds it to the sample database. - - Returns a MonoDrawable message with the display image and progress info. - """ - gray = self.mkgray(msg) - linear_error = -1 - - # Get display-image-to-be (scrib) and detection of the calibration target - scrib_mono, corners, downsampled_corners, ids, board, ( - x_scale, y_scale) = self.downsample_and_detect(gray) - - if self.calibrated: - # Show rectified image - # TODO Pull out downsampling code into function - gray_remap = self.remap(gray) - gray_rect = gray_remap - if x_scale != 1.0 or y_scale != 1.0: - gray_rect = cv2.resize( - gray_remap, (scrib_mono.shape[1], scrib_mono.shape[0])) - - scrib = cv2.cvtColor(gray_rect, cv2.COLOR_GRAY2BGR) - - if corners is not None: - # Report linear error - undistorted = self.undistort_points(corners) - linear_error = self.linear_error(undistorted, ids, board) - - # Draw rectified corners - scrib_src = undistorted.copy() - scrib_src[:, :, 0] /= x_scale - scrib_src[:, :, 1] /= y_scale - cv2.drawChessboardCorners( - scrib, (board.n_cols, board.n_rows), scrib_src, True) - - else: - scrib = cv2.cvtColor(scrib_mono, cv2.COLOR_GRAY2BGR) - if corners is not None: - # Draw (potentially downsampled) corners onto display image - if board.pattern == "charuco": - cv2.aruco.drawDetectedCornersCharuco( - scrib, downsampled_corners, ids) - else: - cv2.drawChessboardCorners( - scrib, (board.n_cols, board.n_rows), downsampled_corners, True) - - # Add sample to database only if it's sufficiently different from any previous sample. - params = self.get_parameters( - corners, ids, board, (gray.shape[1], gray.shape[0])) - if self.is_good_sample( - params, corners, ids, self.last_frame_corners, self.last_frame_ids): - self.db.append((params, gray)) - if self.pattern == Patterns.ChArUco: - self.good_corners.append((corners, ids, board)) - else: - self.good_corners.append((corners, None, 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 - self.last_frame_ids = ids - rv = MonoDrawable() - rv.scrib = scrib - rv.params = self.compute_goodenough() - rv.linear_error = linear_error - return rv - - def do_calibration(self, dump=False): - if not self.good_corners: - print("**** Collecting corners for all images! ****") # DEBUG - images = [i for (p, i) in self.db] - self.good_corners = self.collect_corners(images) - # TODO Needs to be set externally - self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) - # 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.cal_fromcorners(self.good_corners) - self.calibrated = True - # DEBUG - print((self.report())) - print((self.ost())) - - def do_tarfile_save(self, tf): - """ Write images and calibration solution to a tarfile object """ - - def taradd(name, buf): - if isinstance(buf, str): - s = BytesIO(buf.encode('utf-8')) - else: - s = BytesIO(buf) - ti = tarfile.TarInfo(name) - ti.size = len(s.getvalue()) - ti.uname = 'calibrator' - ti.mtime = int(time.time()) - tf.addfile(tarinfo=ti, fileobj=s) - - ims = [("left-%04d.png" % i, im) for i, (_, im) in enumerate(self.db)] - for (name, im) in ims: - taradd(name, cv2.imencode(".png", im)[1].tostring()) - taradd('ost.yaml', self.yaml()) - taradd('ost.txt', self.ost()) - - def do_tarfile_calibration(self, filename): - archive = tarfile.open(filename, 'r') - - limages = [image_from_archive(archive, f) for f in archive.getnames() if ( - f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')))] - - self.cal(limages) - -# TODO Replicate MonoCalibrator improvements in stereo - - -class StereoCalibrator(Calibrator): - """ - Calibration class for stereo cameras:: - - limages = [cv2.imread("left%d.png") for i in range(8)] - rimages = [cv2.imread("right%d.png") for i in range(8)] - sc = StereoCalibrator() - sc.cal(limages, rimages) - print sc.as_message() - """ - - is_mono = False - - def __init__(self, *args, **kwargs): - if 'name' not in kwargs: - kwargs['name'] = 'narrow_stereo' - super(StereoCalibrator, self).__init__(*args, **kwargs) - self.l = MonoCalibrator(*args, **kwargs) - self.r = MonoCalibrator(*args, **kwargs) - # Collecting from two cameras in a horizontal stereo rig, can't get - # 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 - :type limages: list of :class:`cvMat` - :param rimages: source right images containing chessboards - :type rimages: list of :class:`cvMat` - - Find chessboards in images, and runs the OpenCV calibration solver. - """ - goodcorners = self.collect_corners(limages, rimages) - self.size = (limages[0].shape[1], limages[0].shape[0]) - self.l.size = self.size - self.r.size = self.size - self.cal_fromcorners(goodcorners) - self.calibrated = True - - def collect_corners(self, limages, rimages): - """ - For a sequence of left and right images, find pairs of images where both - left and right have a chessboard, and return their corners as a list of pairs. - """ - # Pick out (corners, ids, board) tuples - lcorners = [] - rcorners = [] - for img in limages: - (_, corners, _, ids, board, _) = self.downsample_and_detect(img) - lcorners.append((corners, ids, board)) - for img in rimages: - (_, corners, _, ids, board, _) = self.downsample_and_detect(img) - rcorners.append((corners, ids, board)) - - good = [(lco, rco, lid, rid, b) for ((lco, lid, b), (rco, rid, br)) - in zip(lcorners, rcorners) if (lco is not None and rco is not None)] - - if len(good) == 0: - raise CalibrationException("No corners found in images!") - return good - - def cal_fromcorners(self, good): - # Perform monocular calibrations - lcorners = [(lco, lid, b) for (lco, rco, lid, rid, b) in good] - rcorners = [(rco, rid, b) for (lco, rco, lid, rid, b) in good] - self.l.cal_fromcorners(lcorners) - self.r.cal_fromcorners(rcorners) - - (lipts, ripts, _, _, boards) = zip(*good) - - opts = self.mk_object_points(boards, True) - - flags = cv2.CALIB_FIX_INTRINSIC - - self.T = numpy.zeros((3, 1), dtype=numpy.float64) - self.R = numpy.eye(3, dtype=numpy.float64) - - if self.camera_model == CAMERA_MODEL.PINHOLE: - print("stereo pinhole calibration...") - if VersionInfo.parse(cv2.__version__).major < 3: - ret_values = 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: - ret_values = 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(f"Stereo RMS re-projection error: {ret_values[0]}") - elif self.camera_model == CAMERA_MODEL.FISHEYE: - print("stereo fisheye calibration...") - if VersionInfo.parse(cv2.__version__).major < 3: - 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 - # 30, 1e-6 - criteria=( - cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), - flags=flags) - - self.set_alpha(0.0) - - def set_alpha(self, a): - """ - Set the alpha value for the calibrated camera solution. The - alpha value is a zoom, and ranges from 0 (zoomed in, all pixels - in calibrated image are valid) to 1 (zoomed out, all pixels in - original image are in calibrated image). - """ - 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) - - # Operation flags that may be zero or CALIB_ZERO_DISPARITY . - flags = cv2.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.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. """ - self.size = (msgs[0].width, msgs[0].height) - - self.T = numpy.zeros((3, 1), dtype=numpy.float64) - self.R = numpy.eye(3, dtype=numpy.float64) - - self.l.from_message(msgs[0]) - self.r.from_message(msgs[1]) - # Need to compute self.T and self.R here, using the monocular parameters above - if False: - self.set_alpha(0.0) - - def report(self): - print("\nLeft:") - 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()) - - def ost(self): - 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, - 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): - """ - Detect the checkerboard in both images and compute the epipolar error. - Mainly for use in tests. - """ - lcorners = self.downsample_and_detect(limage)[1] - rcorners = self.downsample_and_detect(rimage)[1] - if lcorners is None or rcorners is None: - return None - - lundistorted = self.l.undistort_points(lcorners) - rundistorted = self.r.undistort_points(rcorners) - - return self.epipolar_error(lundistorted, rundistorted) - - def epipolar_error(self, lcorners, rcorners): - """ - Compute the epipolar error from two sets of matching undistorted points - """ - d = lcorners[:, :, 1] - rcorners[:, :, 1] - return numpy.sqrt(numpy.square(d).sum() / d.size) - - def chessboard_size_from_images(self, limage, rimage): - _, lcorners, _, _, board, _ = self.downsample_and_detect(limage) - _, rcorners, _, _, board, _ = self.downsample_and_detect(rimage) - if lcorners is None or rcorners is None: - return None - - lundistorted = self.l.undistort_points(lcorners) - rundistorted = self.r.undistort_points(rcorners) - - return self.chessboard_size(lundistorted, rundistorted, board) - - def chessboard_size(self, lcorners, rcorners, board, msg=None): - """ - Compute the square edge length from two sets of matching undistorted points - given the current calibration. - :param msg: a tuple of (left_msg, right_msg) - """ - # Project the points to 3d - cam = image_geometry.StereoCameraModel() - if msg == None: - msg = self.as_message() - cam.from_camera_info(*msg) - disparities = lcorners[:, :, 0] - rcorners[:, :, 0] - pt3d = [cam.project_pixel_to_3d( - (lcorners[i, 0, 0], - lcorners[i, 0, 1]), - disparities[i, 0]) for i in range(lcorners.shape[0])] - - def l2(p0, p1): - return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)])) - - # Compute the length from each horizontal and vertical line, and return the mean - cc = board.n_cols - cr = board.n_rows - lengths = ( - [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] + - [l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)]) - return sum(lengths) / len(lengths) - - def update_db(self, lgray, rgray, lcorners, rcorners, lids, rids, lboard): - """ - update database with images and good corners if good samples are detected - """ - params = self.get_parameters( - lcorners, lids, lboard, (lgray.shape[1], lgray.shape[0])) - if self.is_good_sample( - params, lcorners, lids, self.last_frame_corners, self.last_frame_ids): - self.db.append((params, lgray, rgray)) - self.good_corners.append( - (lcorners, rcorners, lids, rids, lboard)) - print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % - tuple([len(self.db)] + params))) - - def handle_msg(self, msg): - # TODO Various asserts that images have same dimension, same board detected... - (lmsg, rmsg) = msg - lgray = self.mkgray(lmsg) - rgray = self.mkgray(rmsg) - epierror = -1 - - # Get display-images-to-be and detections of the calibration target - lscrib_mono, lcorners, ldownsampled_corners, lids, lboard, ( - x_scale, y_scale) = self.downsample_and_detect(lgray) - rscrib_mono, rcorners, rdownsampled_corners, rids, rboard, _ = self.downsample_and_detect( - rgray) - - if self.calibrated: - # Show rectified images - lremap = self.l.remap(lgray) - rremap = self.r.remap(rgray) - lrect = lremap - rrect = rremap - if x_scale != 1.0 or y_scale != 1.0: - lrect = cv2.resize( - lremap, (lscrib_mono.shape[1], lscrib_mono.shape[0])) - rrect = cv2.resize( - rremap, (rscrib_mono.shape[1], rscrib_mono.shape[0])) - - lscrib = cv2.cvtColor(lrect, cv2.COLOR_GRAY2BGR) - rscrib = cv2.cvtColor(rrect, cv2.COLOR_GRAY2BGR) - - # Draw rectified corners - if lcorners is not None: - lundistorted = self.l.undistort_points(lcorners) - scrib_src = lundistorted.copy() - scrib_src[:, :, 0] /= x_scale - scrib_src[:, :, 1] /= y_scale - cv2.drawChessboardCorners( - lscrib, (lboard.n_cols, lboard.n_rows), scrib_src, True) - - if rcorners is not None: - rundistorted = self.r.undistort_points(rcorners) - scrib_src = rundistorted.copy() - scrib_src[:, :, 0] /= x_scale - scrib_src[:, :, 1] /= y_scale - cv2.drawChessboardCorners( - rscrib, (rboard.n_cols, rboard.n_rows), scrib_src, True) - - # Report epipolar error - if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): - epierror = self.epipolar_error(lundistorted, rundistorted) - - else: - lscrib = cv2.cvtColor(lscrib_mono, cv2.COLOR_GRAY2BGR) - rscrib = cv2.cvtColor(rscrib_mono, cv2.COLOR_GRAY2BGR) - # Draw any detected chessboards onto display (downsampled) images - if lcorners is not None: - cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows), - ldownsampled_corners, True) - if rcorners is not None: - cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows), - rdownsampled_corners, True) - - # 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): - # Add samples only with entire board in view if charuco - if self.pattern == Patterns.ChArUco: - if len(lcorners) == lboard.charuco_board.chessboardCorners.shape[0]: - self.update_db(lgray, rgray, lcorners, - rcorners, lids, rids, lboard) - else: - self.update_db(lgray, rgray, lcorners, - rcorners, lids, rids, lboard) - - self.last_frame_corners = lcorners - self.last_frame_ids = lids - rv = StereoDrawable() - rv.lscrib = lscrib - rv.rscrib = rscrib - rv.params = self.compute_goodenough() - rv.epierror = epierror - return rv - - def do_calibration(self, dump=False): - # TODO MonoCalibrator collects corners if needed here - # TODO Needs to be set externally - self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) - # 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.l.size = self.size - self.r.size = self.size - self.cal_fromcorners(self.good_corners) - self.calibrated = True - # DEBUG - print((self.report())) - print((self.ost())) - - def do_tarfile_save(self, tf): - """ Write images and calibration solution to a tarfile object """ - ims = ([("left-%04d.png" % i, im) for i, (_, im, _) in enumerate(self.db)] + - [("right-%04d.png" % i, im) for i, (_, _, im) in enumerate(self.db)]) - - def taradd(name, buf): - if isinstance(buf, str): - s = BytesIO(buf.encode('utf-8')) - else: - s = BytesIO(buf) - ti = tarfile.TarInfo(name) - ti.size = len(s.getvalue()) - ti.uname = 'calibrator' - ti.mtime = int(time.time()) - tf.addfile(tarinfo=ti, fileobj=s) - - for (name, im) in ims: - taradd(name, cv2.imencode(".png", im)[1].tostring()) - taradd('left.yaml', self.yaml("/left", self.l)) - taradd('right.yaml', self.yaml("/right", self.r)) - taradd('ost.txt', self.ost()) - - def do_tarfile_calibration(self, filename): - archive = tarfile.open(filename, 'r') - limages = [image_from_archive(archive, f) for f in archive.getnames() if ( - f.startswith('left') and (f.endswith('pgm') or f.endswith('png')))] - rimages = [image_from_archive(archive, f) for f in archive.getnames() if ( - f.startswith('right') and (f.endswith('pgm') or f.endswith('png')))] - - 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 29e060742..90357bb2d 100755 --- a/camera_calibration/src/camera_calibration/camera_calibrator.py +++ b/camera_calibration/src/camera_calibration/camera_calibrator.py @@ -31,23 +31,27 @@ # POSSIBILITY OF SUCH DAMAGE. import cv2 -import message_filters import numpy import os import rclpy -from rclpy.node import Node -import sensor_msgs.msg -import sensor_msgs.srv -import threading import time -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, Patterns +import threading +import message_filters + try: from queue import Queue except ImportError: from Queue import Queue -from camera_calibration.calibrator import CAMERA_MODEL + +from rclpy.node import Node from rclpy.qos import qos_profile_system_default from rclpy.qos import QoSProfile +import sensor_msgs.msg +import sensor_msgs.srv + +from camera_calibration.mono_calibrator import MonoCalibrator +from camera_calibration.stereo_calibrator import StereoCalibrator +from camera_calibration.calibrator import Patterns, CAMERA_MODEL class BufferQueue(Queue): diff --git a/camera_calibration/src/camera_calibration/camera_checker.py b/camera_calibration/src/camera_calibration/camera_checker.py index c4551a9a2..10b0d86eb 100755 --- a/camera_calibration/src/camera_calibration/camera_checker.py +++ b/camera_calibration/src/camera_calibration/camera_checker.py @@ -35,22 +35,24 @@ import cv2 import cv_bridge import functools -import message_filters import numpy -import rclpy -from rclpy.node import Node -import sensor_msgs.msg -import sensor_msgs.srv import threading - -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, ChessboardInfo -from message_filters import ApproximateTimeSynchronizer - try: from queue import Queue except ImportError: from Queue import Queue +import message_filters +from message_filters import ApproximateTimeSynchronizer +import rclpy +from rclpy.node import Node +import sensor_msgs.msg +import sensor_msgs.srv + +from camera_calibration.mono_calibrator import MonoCalibrator +from camera_calibration.stereo_calibrator import StereoCalibrator +from camera_calibration.calibrator import ChessboardInfo + def mean(seq): return sum(seq) / len(seq) diff --git a/camera_calibration/src/camera_calibration/mono_calibrator.py b/camera_calibration/src/camera_calibration/mono_calibrator.py new file mode 100644 index 000000000..326462a20 --- /dev/null +++ b/camera_calibration/src/camera_calibration/mono_calibrator.py @@ -0,0 +1,414 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from io import BytesIO +import cv2 +import math +import numpy.linalg +import pickle +import random +import tarfile +import time +from camera_calibration.calibrator import ( + Calibrator, + CalibrationException, + CAMERA_MODEL, + Patterns, + ImageDrawable, + image_from_archive +) + + +class MonoDrawable(ImageDrawable): + def __init__(self): + ImageDrawable.__init__(self) + self.scrib = None + self.linear_error = -1.0 + + +class MonoCalibrator(Calibrator): + """ + Calibration class for monocular cameras:: + + images = [cv2.imread("mono%d.png") for i in range(8)] + mc = MonoCalibrator() + mc.cal(images) + print mc.as_message() + """ + + is_mono = True # TODO Could get rid of is_mono + + def __init__(self, *args, **kwargs): + if 'name' not in kwargs: + kwargs['name'] = 'narrow_stereo/left' + super(MonoCalibrator, self).__init__(*args, **kwargs) + + def cal(self, images): + """ + Calibrate camera from given images + """ + goodcorners = self.collect_corners(images) + self.cal_fromcorners(goodcorners) + self.calibrated = True + + def collect_corners(self, images): + """ + :param images: source images containing chessboards + :type images: list of :class:`cvMat` + + Find chessboards in all images. + + Return [ (corners, ids, ChessboardInfo) ] + """ + self.size = (images[0].shape[1], images[0].shape[0]) + corners = [self.get_corners(i) for i in images] + + goodcorners = [(co, ids, b) for (ok, co, ids, b) in corners if ok] + if not goodcorners: + raise CalibrationException("No corners found in images!") + return goodcorners + + def cal_fromcorners(self, good): + """ + :param good: Good corner positions and boards + :type good: [(corners, ChessboardInfo)] + """ + + (ipts, ids, boards) = zip(*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) + + if self.pattern == Patterns.ChArUco: + if self.camera_model == CAMERA_MODEL.FISHEYE: + raise NotImplemented( + "Can't perform fisheye calibration with ChArUco board") + + reproj_err, self.intrinsics, self.distortion, rvecs, tvecs = cv2.aruco.calibrateCameraCharuco( + ipts, ids, boards[0].charuco_board, self.size, intrinsics_in, None) + + elif 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: + # rational polynomial + self.distortion = dist_coeffs.flat[:8].reshape(-1, 1) + else: + # plumb bob + self.distortion = dist_coeffs.flat[:5].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) + + # R is identity matrix for monocular calibration + self.R = numpy.eye(3, dtype=numpy.float64) + self.P = numpy.zeros((3, 4), dtype=numpy.float64) + + self.set_alpha(0.0) + + def set_alpha(self, a): + """ + Set the alpha value for the calibrated camera solution. The alpha + value is a zoom, and ranges from 0 (zoomed in, all pixels in + calibrated image are valid) to 1 (zoomed out, all pixels in + original image are in calibrated image). + """ + + 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): + """ + :param src: source image + :type src: :class:`cvMat` + + Apply the post-calibration undistortion to the source image + """ + return cv2.remap(src, self.mapx, self.mapy, cv2.INTER_LINEAR) + + def undistort_points(self, src): + """ + :param src: N source pixel points (u,v) as an Nx2 matrix + :type src: :class:`cvMat` + + Apply the post-calibration undistortion to the source points + """ + 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, self.camera_model) + + def from_message(self, msg, alpha=0.0): + """ Initialize the camera calibration from a CameraInfo message """ + + self.size = (msg.width, msg.height) + self.intrinsics = numpy.array( + msg.k, dtype=numpy.float64, copy=True).reshape((3, 3)) + self.distortion = numpy.array( + msg.d, dtype=numpy.float64, copy=True).reshape((len(msg.d), 1)) + self.R = numpy.array(msg.r, dtype=numpy.float64, + copy=True).reshape((3, 3)) + self.P = numpy.array(msg.p, dtype=numpy.float64, + copy=True).reshape((3, 4)) + + self.set_alpha(0.0) + + 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, self.size) + + def yaml(self): + 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): + """ + Detect the checkerboard and compute the linear error. + Mainly for use in tests. + """ + _, corners, _, ids, board, _ = self.downsample_and_detect(image) + if corners is None: + return None + + undistorted = self.undistort_points(corners) + return self.linear_error(undistorted, ids, board) + + @staticmethod + def linear_error(corners, ids, b): + """ + Returns the linear error for a set of corners detected in the unrectified image. + """ + + if corners is None: + return None + + corners = numpy.squeeze(corners) + + def pt2line(x0, y0, x1, y1, x2, y2): + """ point is (x0, y0), line is (x1, y1, x2, y2) """ + return abs((x2 - x1) * (y1 - y0) - (x1 - x0) * (y2 - y1)) / math.sqrt((x2 - x1) ** 2 + + (y2 - y1) ** 2) + + n_cols = b.n_cols + n_rows = b.n_rows + if b.pattern == 'charuco': + n_cols -= 1 + n_rows -= 1 + n_pts = n_cols * n_rows + + if ids is None: + ids = numpy.arange(n_pts).reshape((n_pts, 1)) + + ids_to_idx = dict((ids[i, 0], i) for i in range(len(ids))) + + errors = [] + for row in range(n_rows): + row_min = row * n_cols + row_max = (row+1) * n_cols + pts_in_row = [x for x in ids if row_min <= x < row_max] + + # not enough points to calculate error + if len(pts_in_row) <= 2: + continue + + left_pt = min(pts_in_row)[0] + right_pt = max(pts_in_row)[0] + x_left = corners[ids_to_idx[left_pt], 0] + y_left = corners[ids_to_idx[left_pt], 1] + x_right = corners[ids_to_idx[right_pt], 0] + y_right = corners[ids_to_idx[right_pt], 1] + + for pt in pts_in_row: + if pt[0] in (left_pt, right_pt): + continue + x = corners[ids_to_idx[pt[0]], 0] + y = corners[ids_to_idx[pt[0]], 1] + errors.append(pt2line(x, y, x_left, y_left, x_right, y_right)) + + if errors: + return math.sqrt(sum([e**2 for e in errors]) / len(errors)) + else: + return None + + def handle_msg(self, msg): + """ + Detects the calibration target and, if found and provides enough new information, + adds it to the sample database. + + Returns a MonoDrawable message with the display image and progress info. + """ + gray = self.mkgray(msg) + linear_error = -1 + + # Get display-image-to-be (scrib) and detection of the calibration target + scrib_mono, corners, downsampled_corners, ids, board, ( + x_scale, y_scale) = self.downsample_and_detect(gray) + + if self.calibrated: + # Show rectified image + # TODO Pull out downsampling code into function + gray_remap = self.remap(gray) + gray_rect = gray_remap + if x_scale != 1.0 or y_scale != 1.0: + gray_rect = cv2.resize( + gray_remap, (scrib_mono.shape[1], scrib_mono.shape[0])) + + scrib = cv2.cvtColor(gray_rect, cv2.COLOR_GRAY2BGR) + + if corners is not None: + # Report linear error + undistorted = self.undistort_points(corners) + linear_error = self.linear_error(undistorted, ids, board) + + # Draw rectified corners + scrib_src = undistorted.copy() + scrib_src[:, :, 0] /= x_scale + scrib_src[:, :, 1] /= y_scale + cv2.drawChessboardCorners( + scrib, (board.n_cols, board.n_rows), scrib_src, True) + + else: + scrib = cv2.cvtColor(scrib_mono, cv2.COLOR_GRAY2BGR) + if corners is not None: + # Draw (potentially downsampled) corners onto display image + if board.pattern == "charuco": + cv2.aruco.drawDetectedCornersCharuco( + scrib, downsampled_corners, ids) + else: + cv2.drawChessboardCorners( + scrib, (board.n_cols, board.n_rows), downsampled_corners, True) + + # Add sample to database only if it's sufficiently different from any previous sample. + params = self.get_parameters( + corners, ids, board, (gray.shape[1], gray.shape[0])) + if self.is_good_sample( + params, corners, ids, self.last_frame_corners, self.last_frame_ids): + self.db.append((params, gray)) + if self.pattern == Patterns.ChArUco: + self.good_corners.append((corners, ids, board)) + else: + self.good_corners.append((corners, None, 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 + self.last_frame_ids = ids + rv = MonoDrawable() + rv.scrib = scrib + rv.params = self.compute_goodenough() + rv.linear_error = linear_error + return rv + + def do_calibration(self, dump=False): + if not self.good_corners: + print("**** Collecting corners for all images! ****") # DEBUG + images = [i for (p, i) in self.db] + self.good_corners = self.collect_corners(images) + # TODO Needs to be set externally + self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) + # 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.cal_fromcorners(self.good_corners) + self.calibrated = True + # DEBUG + print((self.report())) + print((self.ost())) + + def do_tarfile_save(self, tf): + """ Write images and calibration solution to a tarfile object """ + + def taradd(name, buf): + if isinstance(buf, str): + s = BytesIO(buf.encode('utf-8')) + else: + s = BytesIO(buf) + ti = tarfile.TarInfo(name) + ti.size = len(s.getvalue()) + ti.uname = 'calibrator' + ti.mtime = int(time.time()) + tf.addfile(tarinfo=ti, fileobj=s) + + ims = [("left-%04d.png" % i, im) for i, (_, im) in enumerate(self.db)] + for (name, im) in ims: + taradd(name, cv2.imencode(".png", im)[1].tostring()) + taradd('ost.yaml', self.yaml()) + taradd('ost.txt', self.ost()) + + def do_tarfile_calibration(self, filename): + archive = tarfile.open(filename, 'r') + + limages = [image_from_archive(archive, f) for f in archive.getnames() if ( + f.startswith('left') and (f.endswith('.pgm') or f.endswith('png')))] + + self.cal(limages) diff --git a/camera_calibration/src/camera_calibration/nodes/tarfile_calibration.py b/camera_calibration/src/camera_calibration/nodes/tarfile_calibration.py index b891eb92a..92f134a9c 100755 --- a/camera_calibration/src/camera_calibration/nodes/tarfile_calibration.py +++ b/camera_calibration/src/camera_calibration/nodes/tarfile_calibration.py @@ -34,17 +34,17 @@ import os import numpy - import cv2 import cv_bridge import tarfile -from camera_calibration.calibrator import ( - MonoCalibrator, StereoCalibrator, CalibrationException, ChessboardInfo) - import rclpy import sensor_msgs.srv +from camera_calibration.mono_calibrator import MonoCalibrator +from camera_calibration.stereo_calibrator import StereoCalibrator +from camera_calibration.calibrator import ChessboardInfo + def display(win_name, img): cv2.namedWindow(win_name, cv2.WINDOW_NORMAL) diff --git a/camera_calibration/src/camera_calibration/stereo_calibrator.py b/camera_calibration/src/camera_calibration/stereo_calibrator.py new file mode 100644 index 000000000..4173531d7 --- /dev/null +++ b/camera_calibration/src/camera_calibration/stereo_calibrator.py @@ -0,0 +1,501 @@ +#!/usr/bin/env python +# +# Software License Agreement (BSD License) +# +# Copyright (c) 2009, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of the Willow Garage nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from io import BytesIO +import cv2 +import image_geometry +import math +import numpy.linalg +import pickle +import random +import sys +import tarfile +import time +from semver import VersionInfo +from camera_calibration.mono_calibrator import MonoCalibrator +from camera_calibration.calibrator import ( + Calibrator, + CalibrationException, + CAMERA_MODEL, + Patterns, + ImageDrawable, + image_from_archive +) + + +class StereoDrawable(ImageDrawable): + def __init__(self): + ImageDrawable.__init__(self) + self.lscrib = None + self.rscrib = None + self.epierror = -1 + self.dim = -1 + +# TODO Replicate MonoCalibrator improvements in stereo + + +class StereoCalibrator(Calibrator): + """ + Calibration class for stereo cameras:: + + limages = [cv2.imread("left%d.png") for i in range(8)] + rimages = [cv2.imread("right%d.png") for i in range(8)] + sc = StereoCalibrator() + sc.cal(limages, rimages) + print sc.as_message() + """ + + is_mono = False + + def __init__(self, *args, **kwargs): + if 'name' not in kwargs: + kwargs['name'] = 'narrow_stereo' + super(StereoCalibrator, self).__init__(*args, **kwargs) + self.l = MonoCalibrator(*args, **kwargs) + self.r = MonoCalibrator(*args, **kwargs) + # Collecting from two cameras in a horizontal stereo rig, can't get + # 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 + :type limages: list of :class:`cvMat` + :param rimages: source right images containing chessboards + :type rimages: list of :class:`cvMat` + + Find chessboards in images, and runs the OpenCV calibration solver. + """ + goodcorners = self.collect_corners(limages, rimages) + self.size = (limages[0].shape[1], limages[0].shape[0]) + self.l.size = self.size + self.r.size = self.size + self.cal_fromcorners(goodcorners) + self.calibrated = True + + def collect_corners(self, limages, rimages): + """ + For a sequence of left and right images, find pairs of images where both + left and right have a chessboard, and return their corners as a list of pairs. + """ + # Pick out (corners, ids, board) tuples + lcorners = [] + rcorners = [] + for img in limages: + (_, corners, _, ids, board, _) = self.downsample_and_detect(img) + lcorners.append((corners, ids, board)) + for img in rimages: + (_, corners, _, ids, board, _) = self.downsample_and_detect(img) + rcorners.append((corners, ids, board)) + + good = [(lco, rco, lid, rid, b) for ((lco, lid, b), (rco, rid, br)) + in zip(lcorners, rcorners) if (lco is not None and rco is not None)] + + if len(good) == 0: + raise CalibrationException("No corners found in images!") + return good + + def cal_fromcorners(self, good): + # Perform monocular calibrations + lcorners = [(lco, lid, b) for (lco, rco, lid, rid, b) in good] + rcorners = [(rco, rid, b) for (lco, rco, lid, rid, b) in good] + self.l.cal_fromcorners(lcorners) + self.r.cal_fromcorners(rcorners) + + (lipts, ripts, _, _, boards) = zip(*good) + + opts = self.mk_object_points(boards, True) + + flags = cv2.CALIB_FIX_INTRINSIC + + self.T = numpy.zeros((3, 1), dtype=numpy.float64) + self.R = numpy.eye(3, dtype=numpy.float64) + + if self.camera_model == CAMERA_MODEL.PINHOLE: + print("stereo pinhole calibration...") + if VersionInfo.parse(cv2.__version__).major < 3: + ret_values = 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: + ret_values = 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(f"Stereo RMS re-projection error: {ret_values[0]}") + elif self.camera_model == CAMERA_MODEL.FISHEYE: + print("stereo fisheye calibration...") + if VersionInfo.parse(cv2.__version__).major < 3: + 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 + # 30, 1e-6 + criteria=( + cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 1, 1e-5), + flags=flags) + + self.set_alpha(0.0) + + def set_alpha(self, a): + """ + Set the alpha value for the calibrated camera solution. The + alpha value is a zoom, and ranges from 0 (zoomed in, all pixels + in calibrated image are valid) to 1 (zoomed out, all pixels in + original image are in calibrated image). + """ + 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) + + # Operation flags that may be zero or CALIB_ZERO_DISPARITY . + flags = cv2.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.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. """ + self.size = (msgs[0].width, msgs[0].height) + + self.T = numpy.zeros((3, 1), dtype=numpy.float64) + self.R = numpy.eye(3, dtype=numpy.float64) + + self.l.from_message(msgs[0]) + self.r.from_message(msgs[1]) + # Need to compute self.T and self.R here, using the monocular parameters above + if False: + self.set_alpha(0.0) + + def report(self): + print("\nLeft:") + 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()) + + def ost(self): + 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, + 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): + """ + Detect the checkerboard in both images and compute the epipolar error. + Mainly for use in tests. + """ + lcorners = self.downsample_and_detect(limage)[1] + rcorners = self.downsample_and_detect(rimage)[1] + if lcorners is None or rcorners is None: + return None + + lundistorted = self.l.undistort_points(lcorners) + rundistorted = self.r.undistort_points(rcorners) + + return self.epipolar_error(lundistorted, rundistorted) + + def epipolar_error(self, lcorners, rcorners): + """ + Compute the epipolar error from two sets of matching undistorted points + """ + d = lcorners[:, :, 1] - rcorners[:, :, 1] + return numpy.sqrt(numpy.square(d).sum() / d.size) + + def chessboard_size_from_images(self, limage, rimage): + _, lcorners, _, _, board, _ = self.downsample_and_detect(limage) + _, rcorners, _, _, board, _ = self.downsample_and_detect(rimage) + if lcorners is None or rcorners is None: + return None + + lundistorted = self.l.undistort_points(lcorners) + rundistorted = self.r.undistort_points(rcorners) + + return self.chessboard_size(lundistorted, rundistorted, board) + + def chessboard_size(self, lcorners, rcorners, board, msg=None): + """ + Compute the square edge length from two sets of matching undistorted points + given the current calibration. + :param msg: a tuple of (left_msg, right_msg) + """ + # Project the points to 3d + cam = image_geometry.StereoCameraModel() + if msg == None: + msg = self.as_message() + cam.from_camera_info(*msg) + disparities = lcorners[:, :, 0] - rcorners[:, :, 0] + pt3d = [cam.project_pixel_to_3d( + (lcorners[i, 0, 0], + lcorners[i, 0, 1]), + disparities[i, 0]) for i in range(lcorners.shape[0])] + + def l2(p0, p1): + return math.sqrt(sum([(c0 - c1) ** 2 for (c0, c1) in zip(p0, p1)])) + + # Compute the length from each horizontal and vertical line, and return the mean + cc = board.n_cols + cr = board.n_rows + lengths = ( + [l2(pt3d[cc * r + 0], pt3d[cc * r + (cc - 1)]) / (cc - 1) for r in range(cr)] + + [l2(pt3d[c + 0], pt3d[c + (cc * (cr - 1))]) / (cr - 1) for c in range(cc)]) + return sum(lengths) / len(lengths) + + def update_db(self, lgray, rgray, lcorners, rcorners, lids, rids, lboard): + """ + update database with images and good corners if good samples are detected + """ + params = self.get_parameters( + lcorners, lids, lboard, (lgray.shape[1], lgray.shape[0])) + if self.is_good_sample( + params, lcorners, lids, self.last_frame_corners, self.last_frame_ids): + self.db.append((params, lgray, rgray)) + self.good_corners.append( + (lcorners, rcorners, lids, rids, lboard)) + print(("*** Added sample %d, p_x = %.3f, p_y = %.3f, p_size = %.3f, skew = %.3f" % + tuple([len(self.db)] + params))) + + def handle_msg(self, msg): + # TODO Various asserts that images have same dimension, same board detected... + (lmsg, rmsg) = msg + lgray = self.mkgray(lmsg) + rgray = self.mkgray(rmsg) + epierror = -1 + + # Get display-images-to-be and detections of the calibration target + lscrib_mono, lcorners, ldownsampled_corners, lids, lboard, ( + x_scale, y_scale) = self.downsample_and_detect(lgray) + rscrib_mono, rcorners, rdownsampled_corners, rids, rboard, _ = self.downsample_and_detect( + rgray) + + if self.calibrated: + # Show rectified images + lremap = self.l.remap(lgray) + rremap = self.r.remap(rgray) + lrect = lremap + rrect = rremap + if x_scale != 1.0 or y_scale != 1.0: + lrect = cv2.resize( + lremap, (lscrib_mono.shape[1], lscrib_mono.shape[0])) + rrect = cv2.resize( + rremap, (rscrib_mono.shape[1], rscrib_mono.shape[0])) + + lscrib = cv2.cvtColor(lrect, cv2.COLOR_GRAY2BGR) + rscrib = cv2.cvtColor(rrect, cv2.COLOR_GRAY2BGR) + + # Draw rectified corners + if lcorners is not None: + lundistorted = self.l.undistort_points(lcorners) + scrib_src = lundistorted.copy() + scrib_src[:, :, 0] /= x_scale + scrib_src[:, :, 1] /= y_scale + cv2.drawChessboardCorners( + lscrib, (lboard.n_cols, lboard.n_rows), scrib_src, True) + + if rcorners is not None: + rundistorted = self.r.undistort_points(rcorners) + scrib_src = rundistorted.copy() + scrib_src[:, :, 0] /= x_scale + scrib_src[:, :, 1] /= y_scale + cv2.drawChessboardCorners( + rscrib, (rboard.n_cols, rboard.n_rows), scrib_src, True) + + # Report epipolar error + if lcorners is not None and rcorners is not None and len(lcorners) == len(rcorners): + epierror = self.epipolar_error(lundistorted, rundistorted) + + else: + lscrib = cv2.cvtColor(lscrib_mono, cv2.COLOR_GRAY2BGR) + rscrib = cv2.cvtColor(rscrib_mono, cv2.COLOR_GRAY2BGR) + # Draw any detected chessboards onto display (downsampled) images + if lcorners is not None: + cv2.drawChessboardCorners(lscrib, (lboard.n_cols, lboard.n_rows), + ldownsampled_corners, True) + if rcorners is not None: + cv2.drawChessboardCorners(rscrib, (rboard.n_cols, rboard.n_rows), + rdownsampled_corners, True) + + # 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): + # Add samples only with entire board in view if charuco + if self.pattern == Patterns.ChArUco: + if len(lcorners) == lboard.charuco_board.chessboardCorners.shape[0]: + self.update_db(lgray, rgray, lcorners, + rcorners, lids, rids, lboard) + else: + self.update_db(lgray, rgray, lcorners, + rcorners, lids, rids, lboard) + + self.last_frame_corners = lcorners + self.last_frame_ids = lids + rv = StereoDrawable() + rv.lscrib = lscrib + rv.rscrib = rscrib + rv.params = self.compute_goodenough() + rv.epierror = epierror + return rv + + def do_calibration(self, dump=False): + # TODO MonoCalibrator collects corners if needed here + # TODO Needs to be set externally + self.size = (self.db[0][1].shape[1], self.db[0][1].shape[0]) + # 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.l.size = self.size + self.r.size = self.size + self.cal_fromcorners(self.good_corners) + self.calibrated = True + # DEBUG + print((self.report())) + print((self.ost())) + + def do_tarfile_save(self, tf): + """ Write images and calibration solution to a tarfile object """ + ims = ([("left-%04d.png" % i, im) for i, (_, im, _) in enumerate(self.db)] + + [("right-%04d.png" % i, im) for i, (_, _, im) in enumerate(self.db)]) + + def taradd(name, buf): + if isinstance(buf, str): + s = BytesIO(buf.encode('utf-8')) + else: + s = BytesIO(buf) + ti = tarfile.TarInfo(name) + ti.size = len(s.getvalue()) + ti.uname = 'calibrator' + ti.mtime = int(time.time()) + tf.addfile(tarinfo=ti, fileobj=s) + + for (name, im) in ims: + taradd(name, cv2.imencode(".png", im)[1].tostring()) + taradd('left.yaml', self.yaml("/left", self.l)) + taradd('right.yaml', self.yaml("/right", self.r)) + taradd('ost.txt', self.ost()) + + def do_tarfile_calibration(self, filename): + archive = tarfile.open(filename, 'r') + limages = [image_from_archive(archive, f) for f in archive.getnames() if ( + f.startswith('left') and (f.endswith('pgm') or f.endswith('png')))] + rimages = [image_from_archive(archive, f) for f in archive.getnames() if ( + f.startswith('right') and (f.endswith('pgm') or f.endswith('png')))] + + 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/test/test_directed.py b/camera_calibration/test/test_directed.py index 0abeca273..3481b491f 100644 --- a/camera_calibration/test/test_directed.py +++ b/camera_calibration/test/test_directed.py @@ -33,7 +33,6 @@ # POSSIBILITY OF SUCH DAMAGE. import cv2 - import collections import copy import numpy @@ -42,8 +41,9 @@ import tarfile import unittest -from camera_calibration.calibrator import MonoCalibrator, StereoCalibrator, \ - Patterns, CalibrationException, ChessboardInfo, image_from_archive +from camera_calibration.mono_calibrator import MonoCalibrator +from camera_calibration.stereo_calibrator import StereoCalibrator +from camera_calibration.calibrator import Patterns, CalibrationException, ChessboardInfo, image_from_archive board = ChessboardInfo() board.n_cols = 8 diff --git a/camera_calibration/test/test_multiple_boards.py b/camera_calibration/test/test_multiple_boards.py index 472fae0e4..a1c17a35a 100644 --- a/camera_calibration/test/test_multiple_boards.py +++ b/camera_calibration/test/test_multiple_boards.py @@ -32,14 +32,13 @@ # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. -import rclpy -import ament_index_python import requests import unittest import tarfile import os -from camera_calibration.calibrator import StereoCalibrator, ChessboardInfo, image_from_archive +from camera_calibration.stereo_calibrator import StereoCalibrator +from camera_calibration.calibrator import ChessboardInfo, image_from_archive # Large board used for PR2 calibration board = ChessboardInfo()