checkerboard.py
Go to the documentation of this file.00001 # Software License Agreement (BSD License) 00002 # 00003 # Copyright (c) 2008, Willow Garage, Inc. 00004 # All rights reserved. 00005 # 00006 # Redistribution and use in source and binary forms, with or without 00007 # modification, are permitted provided that the following conditions 00008 # are met: 00009 # 00010 # * Redistributions of source code must retain the above copyright 00011 # notice, this list of conditions and the following disclaimer. 00012 # * Redistributions in binary form must reproduce the above 00013 # copyright notice, this list of conditions and the following 00014 # disclaimer in the documentation and/or other materials provided 00015 # with the distribution. 00016 # * Neither the name of Willow Garage, Inc. nor the names of its 00017 # contributors may be used to endorse or promote products derived 00018 # from this software without specific prior written permission. 00019 # 00020 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00021 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00022 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00023 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00024 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00025 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00026 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00027 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00028 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00029 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00030 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 # POSSIBILITY OF SUCH DAMAGE. 00032 00033 import numpy 00034 from numpy import matrix, vsplit, sin, cos, reshape 00035 import rospy 00036 00037 # Primitive used to model PR2's tilting laser platform. Consists of 2 fixed transforms. 00038 # One before the joint, and one after. The joint axis is the x-axis after the first transform 00039 00040 class Checkerboard: 00041 00042 # Dictionary with four elems 00043 # num_x: number of internal corners along x axis (Not configurable) 00044 # num_y: number of internal corners along y axis (Not configurable) 00045 # width_x: spacing between corners along x direction 00046 # width_y: spacing between corners along y direction 00047 def __init__(self, config = {"corners_x": 2, 00048 "corners_y": 2, 00049 "spacing_x": .10, 00050 "spacing_y": .10} ): 00051 rospy.logdebug("Initializing Checkerboard") 00052 self._corners_x = config["corners_x"] 00053 self._corners_y = config["corners_y"] 00054 00055 param_vec = reshape( matrix([ config["spacing_x"], config["spacing_y"] ], float), (-1,1)) 00056 assert(param_vec.size == 2) 00057 self.inflate(param_vec) 00058 00059 def calc_free(self, free_config): 00060 assert("spacing_x" in free_config) 00061 assert("spacing_y" in free_config) 00062 return [ free_config["spacing_x"] ==1, free_config["spacing_y"] ==1] 00063 00064 def params_to_config(self, param_vec): 00065 assert(param_vec.shape == (2,1)) 00066 return { "corners_x" : self._corners_x, 00067 "corners_y" : self._corners_y, 00068 "spacing_x" : float(param_vec[0,0]), 00069 "spacing_y" : float(param_vec[1,0]) } 00070 00071 # Convert column vector of params into config 00072 def inflate(self, param_vec): 00073 self._spacing_x = param_vec[0,0] 00074 self._spacing_y = param_vec[1,0] 00075 00076 # Return column vector of config 00077 def deflate(self): 00078 param_vec = reshape(matrix( [self._spacing_x, self._spacing_y], float ), (-1,1)) 00079 return param_vec 00080 00081 # Returns # of params needed for inflation & deflation 00082 def get_length(self): 00083 return 2 00084 00085 # Generate the 3D points associated with all the corners of the checkerboard 00086 # returns - 4xN numpy matrix with all the points (in homogenous coords) 00087 def generate_points(self): 00088 N = self._corners_x * self._corners_y 00089 pts = matrix(numpy.zeros((4,N), float)) 00090 for y in range(0,self._corners_y): 00091 for x in range(0,self._corners_x): 00092 n = y*self._corners_x + x 00093 pts[:,n] = numpy.matrix([ [x * self._spacing_x], 00094 [y * self._spacing_y], 00095 [0], 00096 [1] ]) 00097 return pts