$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 00059 DEBUG_OUTPUT = True 00060 00061 class CalibrationObjectDetector: 00062 ''' 00063 Base Class for calibration object detector 00064 00065 A specific calibration object detector implementation can detect the specific 00066 calibration object in images and calculate its 3D pose. 00067 ''' 00068 00069 def __init__(self, calibration_object): 00070 ''' 00071 @param calibration_object: Calibration object to work with. Must be compatible to CalibrationObjectDetector 00072 @type calibration_object: CalibrationObject 00073 ''' 00074 self.calibration_object = calibration_object 00075 00076 def detect_image_points(self, image, is_grayscale): 00077 raise NotImplementedError() 00078 00079 def calculate_object_pose(self, image_raw, camera_matrix, dist_coeffs, is_grayscale): 00080 ''' 00081 Calculate 3D pose of calibration object in image given the camera's 00082 camera matrix and distortion coefficients. 00083 00084 Returns rotation matrix and translation vector. 00085 00086 @param image_raw: input image, not distortion corrected 00087 @type image_raw: cv2 compatible numpy image 00088 00089 @param camera_matrix: camera matrix of camera 00090 @type camera_matrix: numpy matrix 00091 00092 @param dist_coeffs: distortion coefficients of camera 00093 @type dist_coeffs: numpy matrix 00094 00095 @param is_grayscale: set to true if image is grayscale 00096 @type is_grayscale: bool 00097 ''' 00098 # get image and object points 00099 image_points = self.detect_image_points(image_raw, is_grayscale) 00100 object_points = self.calibration_object.get_pattern_points() 00101 00102 # get object pose in raw image (not yet distortion corrected) 00103 (retval, rvec, tvec) = cv2.solvePnP(object_points, image_points, camera_matrix, dist_coeffs) 00104 00105 # convert rvec to rotation matrix 00106 rmat = cv2.Rodrigues(rvec)[0] 00107 return (rmat, tvec) 00108 00109 class CheckerboardDetector(CalibrationObjectDetector): 00110 ''' 00111 Detects a checkerboard calibration object 00112 ''' 00113 def detect_image_points(self, image, is_grayscale): 00114 ''' 00115 Detect the pixels at which the checkerboard's corners are. Returns 00116 list of (x, y) coordinates. 00117 00118 @param image: input image with checkerboard 00119 @type image: cv2 compatible numpy image 00120 00121 @param is_grayscale: set to true if image is grayscale 00122 @type is_grayscale: bool 00123 ''' 00124 # detect checkerboard 00125 found, corners = cv2.findChessboardCorners(image, self.calibration_object.pattern_size) 00126 if found: 00127 # create gray image if image is not grayscale 00128 if not is_grayscale: 00129 gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) 00130 else: 00131 gray_image = image 00132 00133 # refine checkerboard corners to subpixel accuracy 00134 term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1) 00135 cv2.cornerSubPix(gray_image, corners, (5, 5), (-1, -1), term) 00136 else: 00137 # could not find checkerboard 00138 if DEBUG_OUTPUT: 00139 print 'checkerboard not found' 00140 return None 00141 return corners