cluster_bounding_box_finder.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # author: Kaijen Hsiao
00035 
00036 ## @package cluster_bounding_box_finder
00037 #Use PCA to find the principal directions and bounding box for a cluster
00038 
00039 from __future__ import division
00040 import roslib
00041 roslib.load_manifest('object_manipulator')
00042 import rospy
00043 import scipy
00044 import pdb
00045 import random
00046 import math
00047 import tf
00048 import scipy.linalg
00049 from geometry_msgs.msg import PoseStamped, Point, Pose, Vector3
00050 from object_manipulation_msgs.srv import FindClusterBoundingBox, FindClusterBoundingBoxResponse
00051 from convert_functions import *
00052 
00053 ## class for using PCA to find the principal directions and bounding
00054 # box for a point cluster
00055 class ClusterBoundingBoxFinder:
00056 
00057     def __init__(self, tf_listener = None, tf_broadcaster = None): 
00058 
00059         #init a TF transform listener
00060         if tf_listener == None:
00061             self.tf_listener = tf.TransformListener()
00062         else:
00063             self.tf_listener = tf_listener
00064 
00065         #init a TF transform broadcaster for the object frame
00066         if tf_broadcaster == None:
00067             self.tf_broadcaster = tf.TransformBroadcaster()
00068         else:
00069             self.tf_broadcaster = tf_broadcaster
00070 
00071 
00072     ##run eigenvector PCA for a 2xn scipy matrix, return the directions 
00073     #(list of 2x1 scipy arrays)
00074     def pca(self, points):
00075         cov_mat = points[0:3,:] * points[0:3,:].T/scipy.shape(points)[1]
00076         (values, vectors) = scipy.linalg.eig(cov_mat)
00077         if values[1] >  values[0]:
00078             directions = [vectors[:,1], vectors[:,0]] 
00079         else:
00080             directions = [vectors[:,0], vectors[:,1]]
00081 
00082         return directions 
00083 
00084 
00085     ##shift the points to be around the mean for x and y (matrix is 4xn)
00086     def mean_shift_xy(self, points):
00087         mean = [0]*2
00088         shifted_points = points.copy()
00089         for i in range(2):
00090             mean[i] = scipy.mean(points[i,:])
00091             shifted_points[i,:] -= mean[i]    
00092         return (shifted_points, mean)
00093 
00094 
00095     ##remove outliers from the cluster (4xn scipy matrix)
00096     def remove_outliers(self, points):
00097 
00098         points_filtered = points[:]
00099         empty_space_width = .02
00100         check_percent = .02
00101         edge_width = .005
00102 
00103         #remove outliers in each dimension (x,y,z)
00104         for dim in range(3):
00105 
00106             #sort the points by their values in that dimension
00107             num_points = scipy.shape(points_filtered)[1]
00108             points_sorted = points_filtered[:, points_filtered[dim, :].argsort().tolist()[0]]
00109 
00110             #chop off the top points if they are more than empty_space_width away from the bounding box edge
00111             ind = int(math.floor(num_points*(1-check_percent)))
00112             if points_sorted[dim, -1] - points_sorted[dim, ind] > empty_space_width:
00113                 
00114                 #find the first point that isn't within edge_width of the point at ind, and lop off all points after
00115                 searcharr = scipy.array(points_sorted[dim, :]).flatten()
00116                 searchval = points_sorted[dim, ind] + edge_width
00117                 thres_ind = scipy.searchsorted(searcharr, searchval)
00118                 if thres_ind != 0 and thres_ind != len(searcharr):
00119                     points_filtered = points_sorted[:, 0:thres_ind] 
00120                     rospy.loginfo("chopped points off of dim %d, highest val = %5.3f, searchval = %5.3f"%(dim, points_sorted[dim, -1], searchval))
00121             else:
00122                 points_filtered = points_sorted
00123 
00124             #do both sides for x and y
00125             if dim != 2:
00126                 ind = int(math.floor(num_points*check_percent))
00127                 if points_filtered[dim, ind] - points_filtered[dim, 0] > empty_space_width:
00128                     
00129                     #find the first point that isn't within edge_width of the point at ind, and lop off all points before
00130                     searcharr = scipy.array(points_sorted[dim, :]).flatten()
00131                     searchval = points_sorted[dim, ind] - edge_width
00132                     thres_ind = scipy.searchsorted(searcharr, searchval)
00133                     if thres_ind != 0 and thres_ind != len(searcharr):
00134                         points_filtered = points_filtered[:, thres_ind:-1]
00135                         rospy.loginfo("chopped points off of dim -%d, lowest val = %5.3f, searchval = %5.3f"%(dim, points_sorted[dim, 0], searchval))
00136         return points_filtered
00137 
00138 
00139     ##find the object frame and bounding box for a PointCloud or PointCloud2
00140     #use the local rosparam z_up_frame to specify the desired frame to use where the z-axis is special (box z will be frame z)
00141     #if not specified, the point cloud's frame is assumed to be the desired z_up_frame
00142     def find_object_frame_and_bounding_box(self, point_cloud):
00143         
00144         #get the name of the frame to use with z-axis being "up" or "normal to surface" 
00145         #(the cluster will be transformed to this frame, and the resulting box z will be this frame's z)
00146         #if param is not set, assumes the point cloud's frame is such
00147         self.base_frame = rospy.get_param("~z_up_frame", point_cloud.header.frame_id)
00148 
00149         #convert from PointCloud to 4xn scipy matrix in the base_frame
00150         cluster_frame = point_cloud.header.frame_id
00151 
00152         (points, cluster_to_base_frame) = transform_point_cloud(self.tf_listener, point_cloud, self.base_frame)
00153         if points == None:
00154             return (None, None, None)
00155         #print "cluster_to_base_frame:\n", ppmat(cluster_to_base_frame)
00156 
00157         #find the lowest point in the cluster to use as the 'table height'
00158         table_height = points[2,:].min()
00159 
00160         #run PCA on the x-y dimensions to find the tabletop orientation of the cluster
00161         (shifted_points, xy_mean) = self.mean_shift_xy(points)
00162         directions = self.pca(shifted_points[0:2, :])
00163 
00164         #convert the points to object frame:
00165         #rotate all the points about z so that the shortest direction is parallel to the y-axis (long side of object is parallel to x-axis) 
00166         #and translate them so that the table height is z=0 (x and y are already centered around the object mean)
00167         y_axis = scipy.mat([directions[1][0], directions[1][1], 0.])
00168         z_axis = scipy.mat([0.,0.,1.])
00169         x_axis = scipy.cross(y_axis, z_axis)
00170         rotmat = scipy.matrix(scipy.identity(4))
00171         rotmat[0:3, 0] = x_axis.T
00172         rotmat[0:3, 1] = y_axis.T
00173         rotmat[0:3, 2] = z_axis.T
00174         rotmat[2, 3] = table_height
00175         object_points = rotmat**-1 * shifted_points
00176 
00177         #remove outliers from the cluster
00178         object_points = self.remove_outliers(object_points)
00179 
00180         #find the object bounding box in the new object frame as [[xmin, ymin, zmin], [xmax, ymax, zmax]] (coordinates of opposite corners)
00181         object_bounding_box = [[0]*3 for i in range(2)]
00182         object_bounding_box_dims = [0]*3
00183         for dim in range(3):
00184             object_bounding_box[0][dim] = object_points[dim,:].min()
00185             object_bounding_box[1][dim] = object_points[dim,:].max()
00186             object_bounding_box_dims[dim] = object_bounding_box[1][dim] - object_bounding_box[0][dim]
00187 
00188         #now shift the object frame and bounding box so that the z-axis is centered at the middle of the bounding box
00189         x_offset = object_bounding_box[1][0] - object_bounding_box_dims[0]/2.
00190         y_offset = object_bounding_box[1][1] - object_bounding_box_dims[1]/2.
00191         for i in range(2):
00192             object_bounding_box[i][0] -= x_offset
00193             object_bounding_box[i][1] -= y_offset
00194         object_points[0, :] -= x_offset
00195         object_points[1, :] -= y_offset
00196         offset_mat = scipy.mat(scipy.identity(4))
00197         offset_mat[0,3] = x_offset
00198         offset_mat[1,3] = y_offset
00199         rotmat = rotmat * offset_mat
00200         #pdb.set_trace()
00201 
00202         #record the transforms from object frame to base frame and to the original cluster frame,
00203         #broadcast the object frame to tf, and draw the object frame in rviz
00204         unshift_mean = scipy.identity(4)
00205         unshift_mean[0,3] = xy_mean[0]
00206         unshift_mean[1,3] = xy_mean[1]
00207         object_to_base_frame = unshift_mean*rotmat
00208         object_to_cluster_frame = cluster_to_base_frame**-1 * object_to_base_frame
00209 
00210         #broadcast the object frame to tf
00211         (object_frame_pos, object_frame_quat) = mat_to_pos_and_quat(object_to_cluster_frame)
00212         self.tf_broadcaster.sendTransform(object_frame_pos, object_frame_quat, rospy.Time.now(), "object_frame", cluster_frame) 
00213 
00214         return (object_points, object_bounding_box_dims, object_bounding_box, object_to_base_frame, object_to_cluster_frame)
00215 
00216 
00217     


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04