00001 #!/usr/bin/env python 00002 # 00003 # Copyright (c) 2010, Georgia Tech Research Corporation 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 are met: 00008 # * Redistributions of source code must retain the above copyright 00009 # notice, this list of conditions and the following disclaimer. 00010 # * Redistributions in binary form must reproduce the above copyright 00011 # notice, this list of conditions and the following disclaimer in the 00012 # documentation and/or other materials provided with the distribution. 00013 # * Neither the name of the Georgia Tech Research Corporation nor the 00014 # names of its contributors may be used to endorse or promote products 00015 # derived from this software without specific prior written permission. 00016 # 00017 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND 00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT, 00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF 00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00027 # 00028 00029 # \author Martin Schuster (Healthcare Robotics Lab, Georgia Tech.) 00030 00031 00032 00033 ##import roslib; roslib.load_manifest('laser_camera_segmentation') 00034 ##import rospy 00035 00036 ##from std_msgs.msg import String 00037 00038 00039 import hrl_tilting_hokuyo.processing_3d as p3d 00040 from enthought.mayavi import mlab 00041 00042 00043 import hrl_lib.util as ut 00044 import numpy as np, math #import numpy.core as np?? 00045 import scipy 00046 from scipy import stats 00047 00048 ##import scanr as scanr 00049 ##import nearesNeighbourGather.NearestLaserPoint as NearestLaserPoint 00050 00051 if __name__ == '__main__': 00052 00053 ## print 'test' 00054 ## scanr = scanr.scanr() 00055 ## scanr.verify_laser_cam_callib() 00056 00057 00058 dict = ut.load_pickle('../../data/2009Aug31_172113_dict.pkl') 00059 pts = p3d.generate_pointcloud(dict['pos_list'],dict['scan_list'], math.radians(-60),math.radians(60),dict['l1'],dict['l2'], min_tilt=math.radians(-20),max_tilt=math.radians(20)) 00060 00061 hist = scipy.stats.histogram(pts[2],30) 00062 hist_max_index = hist[0].argmax() 00063 z_min = hist[1] + hist_max_index * hist[2] 00064 z_max = z_min + hist[2] 00065 scalar_list = list() 00066 for x,y,z in np.asarray(pts.T): 00067 #scalar_list.append(x) 00068 if z_min < z < z_max: 00069 scalar_list.append(29) 00070 else: 00071 scalar_list.append(x) 00072 00073 mlab.points3d(pts[0,:].A1,pts[1,:].A1,pts[2,:].A1,scalar_list,mode='point',scale_factor=0.01)#,colormap='winter' 00074 mlab.colorbar() 00075 00076 #scipy.stats.histogram(pts[2],30)[0].argmax() 00077 00078 ##mlab.triangular_mesh([[0,0,0]], [[0,1,0]], [[0,1,1]], [(0,1,2)]) 00079 mlab.show()