image_region_functions.py
Go to the documentation of this file.
00001 # Software License Agreement (BSD License)
00002 #
00003 # Copyright (c) 2010, 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 the Willow Garage 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 # author: Kaijen Hsiao
00034 
00035 ## @package object_manipulator
00036 # Helper functions for using image regions of PointCloud2s 
00037 
00038 from __future__ import division
00039 import roslib
00040 roslib.load_manifest('object_manipulator')
00041 import rospy
00042 import scipy
00043 import math
00044 from sensor_msgs.msg import RegionOfInterest
00045 import pdb
00046 
00047 
00048 #compute an image mask for the points in the point cluster (PointCloud1 in the camera frame)
00049 def compute_cluster_mask(camera_info, cluster, dist = 1):
00050     indices = []
00051     
00052     #put the corresponding image pixels for each cluster point into the set of indices
00053     #along with the other pixels up to dist away from it
00054     for point in cluster.points:
00055         (x,y) = compute_image_point(camera_info, point)
00056         for i in range(-dist,dist+1):
00057             if x+i < 0 or x+i > camera_info.width:
00058                 continue
00059             for j in range(-dist,dist+1):
00060                 if y+j < 0 or y+j > camera_info.height:
00061                     continue
00062                 index = image_points_to_indices(camera_info.width, [x+i], [y+j])[0]
00063                 indices.append(index)
00064 
00065     #kill duplicates
00066     unique_indices = set(indices)
00067     return list(unique_indices)
00068 
00069 
00070 #turn a list of x,y coords in the image to indices (index 0 = (0,0), 1 = (1,0), width+1 = (0,1))
00071 def image_points_to_indices(width, xvals, yvals):
00072     return [y*width + x for (x,y) in zip(xvals, yvals)]
00073 
00074 
00075 #turn a set of indices into image (x,y) coords
00076 def indices_to_image_points(width, indices):
00077     xvals = [ind - width*(math.floor(ind / width)) for ind in indices]
00078     yvals = [math.floor(ind / width) for ind in indices]
00079     return (xvals, yvals)
00080 
00081 
00082 #compute the relevant point in the image for the given Point (x=0, y=0 is top left corner)
00083 def compute_image_point(camera_info, point):
00084 
00085     trans_mat = scipy.matrix(scipy.identity(4))
00086     for i in range(3):
00087         for j in range(4):
00088             trans_mat[i,j] = camera_info.P[i*4+j]
00089     point4 = scipy.matrix(scipy.ones([4,1]))
00090     point4[0] = point.x
00091     point4[1] = point.y
00092     point4[2] = point.z
00093 
00094     image_pt = trans_mat * point4
00095     x = int(math.floor(image_pt[0,0] / image_pt[2,0]))
00096     y = int(math.floor(image_pt[1,0] / image_pt[2,0]))
00097 
00098     return (x,y)
00099 
00100     
00101 #generate indices for a rectangular mask
00102 def generate_rect_mask(x_offset, y_offset, height, width, row_step):
00103         
00104     point_indices = []
00105     for y in range(height+1):
00106         row = range(x_offset+y*row_step, x_offset+y*row_step+width+1) 
00107         point_indices.extend(row)
00108     return point_indices
00109 
00110 
00111 #compute an image rectangle from a set of indices for a PointCloud2
00112 def compute_roi_from_indices(indices, width, height, padding = 30):
00113         
00114     xvals = [ind - width*(math.floor(ind / width)) for ind in indices]
00115     yvals = [math.floor(ind / width) for ind in indices]
00116     roi = RegionOfInterest()
00117     roi.x_offset = max(0, min(xvals) - padding)
00118     roi.y_offset = max(0, min(yvals) - padding)
00119     
00120     roi.height = max(yvals) - roi.y_offset + padding
00121     if roi.height + roi.y_offset > height:
00122         roi.height = height - roi.y_offset
00123     roi.width = max(xvals) - roi.x_offset + padding
00124     if roi.width + roi.x_offset > width:
00125         roi.width = width - roi.x_offset
00126     roi.do_rectify = 0
00127 
00128     return roi


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