00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 import roslib; roslib.load_manifest('hrl_tilting_hokuyo')
00031
00032 from enthought.mayavi import mlab
00033
00034 import sys,os,time
00035 import optparse
00036 import hrl_lib.util as ut
00037 import numpy as np, math
00038
00039
00040 color_list = [(1.,1.,1.),(1.,0.,0.),(0.,1.,0.),(0.,0.,1.),(1.,1.,0.),(1.,0.,1.),\
00041 (0.,1.,1.),(0.5,1.,0.5),(1.,0.5,0.5)]
00042
00043
00044
00045
00046
00047 def white_bg():
00048 mlab.figure(fgcolor = (0,0,0), bgcolor = (1,1,1))
00049
00050
00051
00052
00053
00054 def savefig(name, size):
00055 mlab.savefig(name, size=size)
00056
00057
00058
00059
00060
00061
00062
00063
00064 def plot(pts,color=(1.,1.,1.), scalar_list=None):
00065 if scalar_list != None:
00066 mlab.plot3d(pts[0,:].A1,pts[1,:].A1,pts[2,:].A1,scalar_list,
00067 representation = 'wireframe', tube_radius = None)
00068 mlab.colorbar()
00069 else:
00070 mlab.plot3d(pts[0,:].A1,pts[1,:].A1,pts[2,:].A1,color=color,
00071 representation = 'wireframe', tube_radius = None)
00072
00073
00074
00075
00076
00077
00078
00079
00080 def plot_points(pts,color=(1.,1.,1.),mode='point',scale_factor=0.01,scalar_list=None):
00081 if scalar_list != None:
00082 mlab.points3d(pts[0,:].A1,pts[1,:].A1,pts[2,:].A1,scalar_list,mode=mode,scale_factor=scale_factor)
00083 mlab.colorbar()
00084 else:
00085 mlab.points3d(pts[0,:].A1,pts[1,:].A1,pts[2,:].A1,mode=mode,color=color,scale_factor=scale_factor)
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100 def plot_normals(pts, normals, curvature=None, mask_points=1,
00101 color=(0.,1.,0.), scale_factor = 0.1):
00102 x = pts[0,:].A1
00103 y = pts[1,:].A1
00104 z = pts[2,:].A1
00105
00106 u = normals[0,:].A1
00107 v = normals[1,:].A1
00108 w = normals[2,:].A1
00109
00110 if curvature != None:
00111 curvature = np.array(curvature)
00112
00113
00114 mlab.points3d(x,y,z,curvature,mode='sphere',scale_factor=0.1,mask_points=1, color=color)
00115
00116 mlab.colorbar()
00117 else:
00118 mlab.points3d(x,y,z,mode='point')
00119 mlab.quiver3d(x, y, z, u, v, w, mask_points=mask_points,
00120 scale_factor=scale_factor, color=color)
00121
00122
00123
00124
00125
00126 def plot_cuboid(corner_tups):
00127 for tup in corner_tups:
00128 p1 = tup[0]
00129 p2 = tup[1]
00130 mlab.plot3d([p1[0,0],p2[0,0]],[p1[1,0],p2[1,0]],
00131 [p1[2,0],p2[2,0]],color=(1.,1.,0.),
00132 representation='wireframe',tube_radius=None)
00133
00134
00135
00136 def show():
00137 mlab.show()
00138
00139
00140
00141 if __name__ == '__main__':
00142 p = optparse.OptionParser()
00143
00144 p.add_option('-c', action='store', type='string', dest='pts_pkl',
00145 help='pkl file with 3D points')
00146 p.add_option('-f', action='store', type='string', dest='dict_pkl',
00147 help='pkl file with 3D dict')
00148 p.add_option('--save_cloud', action='store_true', dest='save_cloud',
00149 help='pickle the point cloud (3xN matrix)')
00150 p.add_option('--pan_angle', action='store', type='float',
00151 dest='max_pan_angle', default=60.0,
00152 help='angle in DEGREES. points b/w (-pan_angle and +pan_angle) are displayed. [default=60.]')
00153 p.add_option('--max_dist', action='store', type='float',
00154 dest='max_dist', default=3.0,
00155 help='maximum distance (meters). Points further than this are discarded. [default=3.]')
00156
00157 opt, args = p.parse_args()
00158
00159 pts_pkl_fname = opt.pts_pkl
00160 dict_pkl_fname = opt.dict_pkl
00161 save_cloud_flag = opt.save_cloud
00162 max_pan_angle = opt.max_pan_angle
00163 max_dist = opt.max_dist
00164
00165 if pts_pkl_fname != None:
00166 pts = ut.load_pickle(pts_pkl_fname)
00167 elif dict_pkl_fname != None:
00168 import tilting_hokuyo.processing_3d as p3d
00169 dict = ut.load_pickle(dict_pkl_fname)
00170 pts = p3d.generate_pointcloud(dict['pos_list'],dict['scan_list'],
00171 math.radians(-max_pan_angle),
00172 math.radians(max_pan_angle),
00173 dict['l1'],dict['l2'],
00174 min_tilt=math.radians(-90),max_tilt=math.radians(90))
00175 else:
00176 print 'Specify either a pts pkl or a dict pkl (-c or -f)'
00177 print 'Exiting...'
00178 sys.exit()
00179
00180 dist_mat = ut.norm(pts)
00181 idxs = np.where(dist_mat<max_dist)[1]
00182 print 'pts.shape', pts.shape
00183 pts = pts[:,idxs.A1]
00184 print 'pts.shape', pts.shape
00185
00186 if save_cloud_flag:
00187 ut.save_pickle(pts,'numpy_pc_'+ut.formatted_time()+'.pkl')
00188
00189 plot_points(pts)
00190 mlab.show()
00191
00192