calibrate.py
Go to the documentation of this file.
00001 #!/usr/bin/env python 
00002 # Copyright (c) 2013, Oregon State University
00003 # All rights reserved.
00004 
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Oregon State University nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 
00016 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY
00020 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 
00027 # Author Dan Lazewatsky/lazewatd@engr.orst.edu
00028 
00029 import roslib; roslib.load_manifest('projector_calibration')
00030 from projector_calibration import CalibrationGrid
00031 from projector_calibration.msg import Homography
00032 import rospy
00033 from rospy.numpy_msg import numpy_msg
00034 from sensor_msgs.msg import Image
00035 from cv_bridge import CvBridge, CvBridgeError
00036 import PySide
00037 import PySide.QtCore
00038 import sys
00039 from functools import partial
00040 import numpy as np
00041 bridge = CvBridge()
00042 
00043 import cv, cv2
00044 
00045 calib_im = None
00046 
00047 class Calibrator(object):
00048         image = None
00049         fail_count = 0
00050         grid_corners = np.zeros((0,2))
00051         img_corners  = np.zeros((0,2))
00052         
00053         def detect(self):
00054                 try:
00055                         im1 = bridge.imgmsg_to_cv(self.image)
00056                 except AttributeError, e:
00057                         rospy.logwarn("Tried to convert a None image")
00058                         return False
00059                 corners1 = cv.FindChessboardCorners(im1, (self.grid.nCols-1, self.grid.nRows-1))
00060                 corners2 = np.array(self.grid.corners, dtype=np.float32)
00061 
00062                 if np.asarray(corners1[1]).shape == corners2.shape:
00063                         self.img_corners  = np.vstack((self.img_corners,  np.float32(corners1[1])))
00064                         self.grid_corners = np.vstack((self.grid_corners, corners2))
00065                         return True
00066                 return False
00067         
00068         def find_homography(self):
00069                 if len(self.img_corners) == len(self.grid_corners):
00070                         from pprint import pprint
00071                         H, mask = cv2.findHomography(self.img_corners, self.grid_corners, method=cv2.RANSAC)
00072                         pprint(H)
00073                         rospy.set_param('/homography', H.flatten().tolist())
00074                 # self.homography_pub.publish(H.flatten())
00075 
00076                 # c1 = np.array([corners1[1]])
00077                 # c2 = np.array([corners2])
00078                 # err = cv2.perspectiveTransform(c1,H) - c2
00079                         sys.exit(0)
00080                         return True
00081                 return False
00082                 
00083         
00084         def publish_image(self, grid_pub):
00085                 im = self.grid.getPatternAsImage(im_type='OPENCV')
00086 
00087                 msg = bridge.cv_to_imgmsg(self.removeAlpha(im), 'rgb8')
00088                 msg.header.stamp = rospy.Time.now()
00089                 grid_pub.publish(msg)
00090 
00091         def removeAlpha(self, im):
00092                 r = cv.CreateMat(im.height, im.width, cv.CV_8UC1)
00093                 g = cv.CreateMat(im.height, im.width, cv.CV_8UC1)
00094                 b = cv.CreateMat(im.height, im.width, cv.CV_8UC1)
00095                 cv.Split(im,r,g,b,None)
00096                 imrgb = cv.CreateMat(im.height, im.width, cv.CV_8UC3)
00097                 cv.Merge(r,g,b,None,imrgb)
00098                 return imrgb
00099 
00100         def calibrate(self, keep_going=False, retry=True):
00101                 print 'waiting...'
00102                 while not self.image and not rospy.is_shutdown():
00103                         rospy.sleep(0.1)
00104                 print 'ready'
00105                 if self.detect():
00106                         if not keep_going:
00107                                 sys.exit(self.find_homography())
00108                 if retry or keep_going:
00109                         if rospy.is_shutdown(): return
00110                         rospy.logwarn('Wrong number of corners. Is there something obstructing the calibration grid?')
00111                         self.fail_count += 1
00112                         if self.fail_count > 3:
00113                                 self.grid.scale = 0.5
00114                                 
00115                                 if len(self.origins) > 0:
00116                                         if self.fail_count % 4:
00117                                                 self.grid.origin = self.origins.pop()
00118                                                 self.grid.repaint()
00119                                                 self.grid.repaint()
00120                                                 self.grid.repaint()
00121                                                 self.grid.repaint()
00122                                                 rospy.sleep(2)
00123                                         self.calibrate(keep_going=(len(self.origins)>0), retry=False)
00124                                 else:
00125                                         self.grid.origin = None
00126                                         self.grid.scale = 1.0
00127                                         self.grid.repaint()
00128                         self.calibrate()
00129                 # sys.exit(self.find_homography())
00130                 self.find_homography()
00131                 
00132         def image_cb(self, msg):
00133                 self.image = msg
00134 
00135         def maybe_shutdown(self):
00136                 if rospy.is_shutdown():
00137                         sys.exit(0)
00138         
00139         def __init__(self):
00140                 rospy.init_node('grid')
00141                 rows, cols = [int(n) for n in rospy.get_param('~grid_size', default='5x7').split('x')]
00142                 print rows, cols
00143                 rospy.Subscriber('image', Image, self.image_cb)
00144                 grid_pub = rospy.Publisher('grid', Image)
00145                 self.homography_pub = rospy.Publisher('homography', numpy_msg(Homography), latch=True)
00146                 app = PySide.QtGui.QApplication(sys.argv)
00147                 self.grid = CalibrationGrid(nCols=cols, nRows=rows)
00148                 self.grid.addKeyHandler(67, self.calibrate)
00149                 self.grid.showFullScreen()
00150                 qtimer = PySide.QtCore.QTimer(self.grid)
00151                 qtimer.timeout.connect(self.maybe_shutdown)
00152                 qtimer.start()
00153                 self.origins = [
00154                         (self.grid.width(),self.grid.padding),
00155                         (self.grid.width(),self.grid.height()-self.grid.padding),
00156                         (self.grid.padding,self.grid.height()-self.grid.padding),
00157                         (self.grid.padding,self.grid.padding)
00158                 ]
00159                 
00160                 PySide.QtCore.QTimer.singleShot(1000, self.calibrate)
00161                 timer = PySide.QtCore.QTimer(self.grid)
00162                 timer.timeout.connect(self.grid.update)
00163                 timer.start(100)
00164                 sys.exit(app.exec_())
00165 
00166 if __name__ == '__main__':
00167         Calibrator()


projector_calibration
Author(s): Daniel Lazewatsky
autogenerated on Mon Oct 6 2014 10:10:01