$search
00001 #!/usr/bin/env python 00002 ################################################################# 00003 ##\file 00004 # 00005 # \note 00006 # Copyright (c) 2011-2012 \n 00007 # Fraunhofer Institute for Manufacturing Engineering 00008 # and Automation (IPA) \n\n 00009 # 00010 ################################################################# 00011 # 00012 # \note 00013 # Project name: care-o-bot 00014 # \note 00015 # ROS stack name: cob_calibration 00016 # \note 00017 # ROS package name: cob_camera_calibration 00018 # 00019 # \author 00020 # Author: Sebastian Haug, email:sebhaug@gmail.com 00021 # \author 00022 # Supervised by: Florian Weisshardt, email:florian.weisshardt@ipa.fhg.de 00023 # 00024 # \date Date of creation: November 2011 00025 # 00026 ################################################################# 00027 # 00028 # Redistribution and use in source and binary forms, with or without 00029 # modification, are permitted provided that the following conditions are met: 00030 # 00031 # - Redistributions of source code must retain the above copyright 00032 # notice, this list of conditions and the following disclaimer. \n 00033 # - Redistributions in binary form must reproduce the above copyright 00034 # notice, this list of conditions and the following disclaimer in the 00035 # documentation and/or other materials provided with the distribution. \n 00036 # - Neither the name of the Fraunhofer Institute for Manufacturing 00037 # Engineering and Automation (IPA) nor the names of its 00038 # contributors may be used to endorse or promote products derived from 00039 # this software without specific prior written permission. \n 00040 # 00041 # This program is free software: you can redistribute it and/or modify 00042 # it under the terms of the GNU Lesser General Public License LGPL as 00043 # published by the Free Software Foundation, either version 3 of the 00044 # License, or (at your option) any later version. 00045 # 00046 # This program is distributed in the hope that it will be useful, 00047 # but WITHOUT ANY WARRANTY; without even the implied warranty of 00048 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00049 # GNU Lesser General Public License LGPL for more details. 00050 # 00051 # You should have received a copy of the GNU Lesser General Public 00052 # License LGPL along with this program. 00053 # If not, see <http://www.gnu.org/licenses/>. 00054 # 00055 ################################################################# 00056 00057 import cv2 00058 import os 00059 import numpy as np 00060 00061 import cv2util 00062 00063 class Calibrator: 00064 ''' 00065 Base class for specific camera calibrators 00066 ''' 00067 def __init__(self): 00068 raise NotImplementedError("Instantiate either MonoCalibrator or StereoCalibrator!") 00069 00070 def _list_folder(self, folder): 00071 ''' 00072 Return list with all files/folders in 'folder' 00073 00074 @param folder: List files of this folder 00075 @type folder: string 00076 ''' 00077 files = sorted(os.listdir(folder)) 00078 return map(lambda x: folder+'/'+x, files) 00079 00080 def _load_images(self, folder, image_file_prefix, grayscale = False): 00081 ''' 00082 Load set of images (jpg or png) 00083 00084 Returns list of images stored as cv2 compatible numpy image. 00085 00086 @param folder: Folder from where the images are located 00087 @type folder: string 00088 00089 @param image_file_prefix: Prefix the image files which are loaded must have 00090 @type image_file_prefix: string 00091 00092 @param grayscale: If true load images as grayscale 00093 @type grayscale: bool 00094 ''' 00095 images = [] 00096 for all_files in self._list_folder(folder): 00097 filename = all_files.rsplit('/')[-1] 00098 00099 # check if current file is image (i.e. png or jpg suffix) 00100 if filename.startswith(image_file_prefix) and (filename.lower().endswith('png') or filename.lower().endswith('jpg')): 00101 if grayscale: 00102 # load grayscale image 00103 images.append(cv2.imread(all_files, cv2.CV_LOAD_IMAGE_GRAYSCALE)) 00104 else: 00105 # load color image 00106 images.append(cv2.imread(all_files, cv2.CV_LOAD_IMAGE_COLOR)) 00107 00108 if len(images) == 0: 00109 raise Exception("Did not find any pictures with prefix", image_file_prefix, "in folder", folder) 00110 return images 00111 00112 def _detect_points(self, images, is_grayscale=True): 00113 ''' 00114 Detect image and object points in set of images 00115 00116 Returns list of image points and list of object points 00117 00118 @param is_grayscale: If true images are assumend grayscale 00119 @type is_grayscale: bool 00120 00121 @param images: list of images stored as cv2 compatible numpy image 00122 @type images: bool 00123 ''' 00124 pattern_points = self.calibration_object.get_pattern_points() 00125 object_points = [] 00126 image_points = [] 00127 for image in images: 00128 corners = self.calibration_object_detector.detect_image_points(image, is_grayscale) 00129 if corners != None: 00130 image_points.append(corners.reshape(-1, 2)) 00131 object_points.append(pattern_points) 00132 00133 assert(len(image_points) == len(object_points)) 00134 if len(image_points) == 0: 00135 raise Exception("Calibration object could not be detected in any image") 00136 return (image_points, object_points) 00137 00138 00139 class MonoCalibrator(Calibrator): 00140 ''' 00141 Calibrates a monocular camera 00142 ''' 00143 def __init__(self, calibration_object, calibration_object_detector, folder, image_prefix): 00144 ''' 00145 Initialize object 00146 00147 @param calibration_object: Calibration object to work with. 00148 @type calibration_object: must implement CalibrationObject interface 00149 00150 @param calibration_object_detector: Calibration object detector to work with. Must be compatible to CalibrationObject 00151 @type calibration_object_detector: must implement CalibrationObjectDetector interface 00152 00153 @param folder: Folder from where the images are located 00154 @type folder: string 00155 00156 @param image_file_prefix: Prefix the image files which are loaded must have 00157 @type image_file_prefix: string 00158 ''' 00159 self.calibration_object = calibration_object 00160 self.calibration_object_detector = calibration_object_detector 00161 self.folder = folder 00162 self.image_prefix = image_prefix 00163 00164 def calibrate_monocular_camera(self, alpha=0.0): 00165 ''' 00166 Run monocular calibration 00167 00168 Returns calibration rms error (float), image height, width 00169 and camera_matrix, dist_coeffs, projection_matrix, rvecs, tvecs (as np.ndarray() type) 00170 00171 @param alpha: Set zoom, alpha = 0.0 results in cropped image with all pixels valid in undistorted image, alpha = 1.0 preserves all pixels from original image 00172 @type alpha: float (0.0 < alpha < 1.0) 00173 ''' 00174 # load images 00175 print "--> loading images from folder '%s' with prefix '%s'" % (self.folder, self.image_prefix) 00176 images = self._load_images(self.folder, self.image_prefix, grayscale=True) 00177 print "--> loaded %i images" % len(images) 00178 00179 # get width and height of images and check all 00180 (h, w) = images[0].shape[:2] 00181 for i in images: assert i.shape[:2] == (h, w) 00182 00183 # detect image and object points in all images 00184 print "--> detecting calibration object in all images..." 00185 (image_points, object_points) = self._detect_points(images, is_grayscale=True) 00186 00187 # prepare matrices 00188 camera_matrix = np.zeros((3,3), np.float32) 00189 dist_coeffs = np.zeros((1,5), np.float32) 00190 00191 # flags 00192 flags = 0 00193 #flags |= cv2.CALIB_FIX_K3 00194 00195 # calibrate camera 00196 print "--> calibrating..." 00197 rms, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(object_points, image_points, (w, h), camera_matrix, dist_coeffs, flags=flags) 00198 00199 # set zoom value alpha, if alpha = 0.0 no black border from undistortion 00200 # will be visible for alpha = 1.0 everything from original image is preserved 00201 (projection_matrix, _) = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coeffs, (w, h), alpha) 00202 projection_matrix = np.array(np.hstack((projection_matrix, np.matrix([0, 0, 0]).reshape(3,1)))) 00203 00204 # # DEBUG: check types -> should all be np.ndarray 00205 # print type(camera_matrix) 00206 # print type(projection_matrix) 00207 # print type(dist_coeffs) 00208 # print type(rvecs) 00209 # print type(tvecs) 00210 00211 return (rms, camera_matrix, projection_matrix, dist_coeffs, (h, w), rvecs, tvecs) 00212 00213 class StereoCalibrator(Calibrator): 00214 ''' 00215 Calibrates a stereo camera pair with parallel cameras 00216 ''' 00217 def __init__(self, calibration_object, calibration_object_detector, folder, image_prefix_left, image_prefix_right): 00218 ''' 00219 Initialize object 00220 00221 @param calibration_object: Calibration object to work with. 00222 @type calibration_object: must implement CalibrationObject interface 00223 00224 @param calibration_object_detector: Calibration object detector to work with. Must be compatible to CalibrationObject 00225 @type calibration_object_detector: must implement CalibrationObjectDetector interface 00226 00227 @param folder: Folder from where the images are located 00228 @type folder: string 00229 00230 @param image_prefix_left: Prefix the left camera's image files which are loaded must have 00231 @type image_prefix_left: string 00232 00233 @param image_prefix_right: Prefix the right camera's image files which are loaded must have 00234 @type image_prefix_right: string 00235 ''' 00236 self.calibration_object = calibration_object 00237 self.calibration_object_detector = calibration_object_detector 00238 self.folder = folder 00239 self.image_prefix_left = image_prefix_left 00240 self.image_prefix_right = image_prefix_right 00241 00242 def calibrate_stereo_camera(self, alpha=0.0): 00243 ''' 00244 Run stereo calibration 00245 00246 Returns calibration rms error (float), image height, width 00247 and camera_matrix, dist_coeffs, projection_matrix and rectification_matrix 00248 for right and left camera (as np.ndarray() type) as well as baseline R and T 00249 00250 @param alpha: Set zoom, alpha = 0.0 results in cropped image with all pixels valid in undistorted image, alpha = 1.0 preserves all pixels from original image 00251 @type alpha: float (0.0 < alpha < 1.0) 00252 ''' 00253 # load images 00254 print "--> loading left images from folder '%s' with prefix '%s'" % (self.folder, self.image_prefix_left) 00255 print "--> loading right images from folder '%s' with prefix '%s'" % (self.folder, self.image_prefix_right) 00256 images_l = self._load_images(self.folder, self.image_prefix_left, grayscale=True) 00257 images_r = self._load_images(self.folder, self.image_prefix_right, grayscale=True) 00258 print "--> loaded %i left images" % len(images_l) 00259 print "--> loaded %i right images" % len(images_r) 00260 00261 # get width and height of images and check all 00262 (h, w) = images_l[0].shape[:2] 00263 for l in images_l: assert l.shape[:2] == (h, w) 00264 for r in images_r: assert r.shape[:2] == (h, w) 00265 00266 # detect image and object points in all images 00267 print "--> detecting calibration object in all images..." 00268 (image_points_l, object_points_l) = self._detect_points(images_l, is_grayscale=True) 00269 (image_points_r, object_points_r) = self._detect_points(images_r, is_grayscale=True) 00270 00271 # sanity checks for image and object points 00272 print "image_points_l = " + str(len(image_points_l)) + ", image_points_r = " + str(len(image_points_r)) 00273 print "object_points_l = " + str(len(object_points_l)) + ", object_points_r = " + str(len(object_points_r)) 00274 assert (len(image_points_l) == len(image_points_r)) 00275 assert (len(object_points_l) == len(object_points_r)) 00276 object_points = object_points_l 00277 00278 # prepare matrices 00279 camera_matrix_l = np.zeros((3,3), np.float32) 00280 dist_coeffs_l = np.zeros((1,5), np.float32) 00281 camera_matrix_r = np.zeros((3,3), np.float32) 00282 dist_coeffs_r = np.zeros((1,5), np.float32) 00283 00284 # mono flags 00285 mono_flags = 0 00286 #mono_flags |= cv2.CALIB_FIX_K3 00287 00288 # run monocular calibration on each camera to get intrinsic parameters 00289 (rms_l, camera_matrix_l, dist_coeffs_l, _, _) = cv2.calibrateCamera(object_points, image_points_l, (w, h), camera_matrix_l, dist_coeffs_l, flags=mono_flags) 00290 (rms_r, camera_matrix_r, dist_coeffs_r, _, _) = cv2.calibrateCamera(object_points, image_points_r, (w, h), camera_matrix_r, dist_coeffs_r, flags=mono_flags) 00291 #(camera_matrix_l, _) = cv2.getOptimalNewCameraMatrix(camera_matrix_l, dist_coeffs_l, (w, h), alpha) 00292 #(camera_matrix_r, _) = cv2.getOptimalNewCameraMatrix(camera_matrix_r, dist_coeffs_r, (w, h), alpha) 00293 00294 # set stereo flags 00295 stereo_flags = 0 00296 stereo_flags |= cv2.CALIB_FIX_INTRINSIC 00297 00298 # # More availabe flags... 00299 # stereo_flags |= cv2.CALIB_USE_INTRINSIC_GUESS # Refine intrinsic parameters 00300 # stereo_flags |= cv2.CALIB_FIX_PRINCIPAL_POINT # Fix the principal points during the optimization. 00301 # stereo_flags |= cv2.CALIB_FIX_FOCAL_LENGTH # Fix focal length 00302 # stereo_flags |= cv2.CALIB_FIX_ASPECT_RATIO # fix aspect ratio 00303 # stereo_flags |= cv2.CALIB_SAME_FOCAL_LENGTH # Use same focal length 00304 # stereo_flags |= cv2.CALIB_ZERO_TANGENT_DIST # Set tangential distortion to zero 00305 # stereo_flags |= cv2.CALIB_RATIONAL_MODEL # Use 8 param rational distortion model instead of 5 param plumb bob model 00306 00307 # run stereo calibration 00308 res = cv2.stereoCalibrate(object_points, image_points_l, image_points_r, camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, (w, h), flags=stereo_flags) 00309 (rms_stereo, camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, R, T, E, F) = res 00310 00311 # run stereo rectification 00312 res = self._rectify(camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, (h, w), R, T, alpha) 00313 (rectification_matrix_l, rectification_matrix_r, projection_matrix_l, projection_matrix_r) = res 00314 00315 # # DEBUG: check types -> should all be np.ndarray 00316 # print type(camera_matrix_l) 00317 # print type(dist_coeffs_l) 00318 # print type(rectification_matrix_l) 00319 # print type(projection_matrix_l) 00320 # print type(camera_matrix_r) 00321 # print type(dist_coeffs_r) 00322 # print type(rectification_matrix_r) 00323 # print type(projection_matrix_r) 00324 # print type(R) 00325 # print type(T) 00326 00327 return ((rms_l, rms_r, rms_stereo), 00328 camera_matrix_l, dist_coeffs_l, rectification_matrix_l, projection_matrix_l, 00329 camera_matrix_r, dist_coeffs_r, rectification_matrix_r, projection_matrix_r, 00330 (h, w), R, T) 00331 00332 def _rectify(self, camera_matrix_l, dist_coeffs_l, camera_matrix_r, dist_coeffs_r, (h, w), R, T, alpha): 00333 ''' 00334 Helper function, wraps cv2.cv.StereoRectify with cv2 input/output types 00335 as this function is missing in new opencv python cv2 interface 00336 00337 see https://code.ros.org/trac/opencv/changeset/6843 00338 ''' 00339 # convert input to cvmat 00340 cameraMatrix1 = cv2util.np2cvmat(camera_matrix_l) 00341 cameraMatrix2 = cv2util.np2cvmat(camera_matrix_r) 00342 distCoeffs1 = cv2util.np2cvmat(dist_coeffs_l) 00343 distCoeffs2 = cv2util.np2cvmat(dist_coeffs_r) 00344 R = cv2util.np2cvmat(R) 00345 T = cv2util.np2cvmat(T.T) 00346 # initialize result cvmats 00347 R1 = cv2.cv.CreateMat(3, 3, cv2.CV_64FC1) 00348 R2 = cv2.cv.CreateMat(3, 3, cv2.CV_64FC1) 00349 P1 = cv2.cv.CreateMat(3, 4, cv2.CV_64FC1) 00350 P2 = cv2.cv.CreateMat(3, 4, cv2.CV_64FC1) 00351 00352 # do rectification 00353 cv2.cv.StereoRectify(cameraMatrix1, cameraMatrix2, distCoeffs1, distCoeffs2, (h, w), R, T, R1, R2, P1, P2, alpha=alpha) 00354 00355 # convert results back to np data types 00356 R1 = cv2util.cvmat2np(R1).reshape((3,3)) 00357 R2 = cv2util.cvmat2np(R2).reshape((3,3)) 00358 P1 = cv2util.cvmat2np(P1).reshape((3,4)) 00359 P2 = cv2util.cvmat2np(P2).reshape((3,4)) 00360 return (R1, R2, P1, P2)