pf_stats.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import rfid_model
00004 import lib_pfilter
00005 
00006 import roslib
00007 roslib.load_manifest('rfid_behaviors')
00008 roslib.load_manifest('rfid_datacapture')
00009 roslib.load_manifest('hrl_lib')
00010 roslib.load_manifest('visualization_msgs')
00011 roslib.load_manifest('hrl_lib')
00012 roslib.load_manifest('pfilter')
00013 import rospy
00014 
00015 import hrl_lib.transforms as tr
00016 import tf.transformations as tft
00017 import display_particles
00018 from display_particles import DisplayParticles
00019 from geometry_msgs.msg import Quaternion, Point
00020 
00021 import rfid_datacapture.math_util as mu
00022 import visualization_msgs.msg as vm
00023 import hrl_lib.transforms as tr
00024 import hrl_lib.viz as viz
00025 vsm = viz.single_marker
00026 
00027 
00028 import random as rd
00029 import cPickle as pkl
00030 import numpy as np, math
00031 import pylab as pl
00032 import time
00033 import glob
00034 
00035 # trial_num = 4
00036 # obj_num = 3
00037 # servo_yn = False
00038 
00039 
00040 def pprint(r):
00041     print '\nLocation %d, Object %d, Trial %d' % (r['loc'], r['obj_num'], r['trial_num'])
00042 
00043     if r['servo_yn']:
00044         print '\tSEARCH plus SERVO'
00045     else:
00046         print '\tSEARCH ONLY'
00047         
00048     print '\tPos Reads:      %d' % (r['pos_readings'])
00049     print '\tTot Reads:      %d' % (r['tot_readings'])
00050     print '\tPercent Reads:  %2.1f' % (r['pos_readings']*100.0/r['tot_readings'])
00051 
00052     if r['best_pos'].__class__ == ''.__class__:
00053         print '\tEstimate Loc:   --'
00054     else:
00055         print '\tEstimate Loc:   <%2.3f, %2.3f>' % ( r['best_pos'][0], r['best_pos'][1] )
00056 
00057     if r['orient_est'].__class__ == ''.__class__:
00058         print '\tEstimate Theta: --'
00059     else:
00060         print '\tEstimate Theta: %2.1f (deg)' % ( math.degrees( r['orient_est'] ))
00061 
00062     if r['dxy'].__class__ == ''.__class__:
00063         print '\tDist Err (m):   --'
00064     else:
00065         print '\tDist Err (m):   %2.3f' % ( r['dxy'] )
00066 
00067     if r['dtheta'].__class__ == ''.__class__:
00068         print '\tAng Err (deg):  --'
00069     else:
00070         print '\tAng Err (deg):  %2.1f' % ( math.degrees( r['dtheta'] ))
00071 
00072     # if r.has_key('other'):
00073     #     print '\tOther params: ', r['other']
00074 
00075     print '\n\n\n'        
00076 
00077 
00078 
00079 tdb = { 0: ['OrangeMedBot',[]],
00080         1: ['TravisTVremo',[]],
00081         2: ['RedBottle   ',[]],
00082         3: ['OnMetalKeys ',[]],
00083         4: ['WhiteMedsBot',[]],
00084         5: ['BlueMedsBox ',[]],
00085         6: ['TeddyBearToy',[]],
00086         7: ['CordlessPhon',[]],
00087         8: ['BlueHairBrus',[]]}
00088 
00089 pts = { 0: ['BehindTree',[3.757, 6.017, 0.036]],
00090         1: ['FireplaceMantle',[5.090, 4.238, 1.514]],
00091         2: ['CircleEndTable',[5.399, 2.857, 0.607]],
00092         3: ['Couch',[3.944, 1.425, 0.527]],
00093         4: ['RectEndTable',[3.302, 0.932, 0.534]],
00094         5: ['BehindKitchenTable',[-0.339, -2.393, 0.793]],
00095         6: ['NearDishwaser',[-1.926, -0.835, 0.946]],
00096         7: ['InCupboard',[-3.257, 1.294, 1.397]],
00097         8: ['OnFilingCabinet',[-0.083, 2.332, 0.670]]}
00098 
00099 def process_trialobj( trial_num, obj_num, servo_yn ):
00100     obj_name = tdb[obj_num][0]
00101     tname = obj_name.replace( ' ', '' )
00102 
00103     loc = (trial_num + obj_num) % 9
00104     loc_name = pts[loc][0]
00105     loc_pos  = np.array(pts[loc][1])  # Tag ground-truth location
00106 
00107 
00108     fname_prefix = '/home/travis/svn/robot1/src/projects/rfid_datacapture/src/rfid_datacapture/search_cap/'
00109 
00110     servo_fname = fname_prefix
00111     servo_fname += 'search_aware_home/'
00112     servo_fname += 'woot_150_'+str(trial_num)+'_tag_'+obj_name.replace(' ','')+'_servo.pkl'
00113 
00114     pf_search = servo_fname.replace('_servo.pkl', '_pf_search.pkl')
00115     pf_servo = servo_fname.replace('_servo.pkl', '_pf_servo.pkl')
00116 
00117 
00118 
00119     # Search only
00120     search_reads_fname = fname_prefix
00121     search_reads_fname += 'search_aware_home/woot_150_'+str(trial_num)+'_reads.pkl'
00122     f = open( search_reads_fname, 'r' )
00123     summary_search = pkl.load( f )
00124     f.close()
00125 
00126     pos_readings_search = sum([ True for p in summary_search if p.read.rssi != -1 and p.read.tagID == obj_name ])
00127     tot_readings_search = len( summary_search )
00128 
00129     if pos_readings_search == 0:  # No results!
00130         print '\t No results for this instance.'
00131 
00132         res = { 'loc': loc,
00133                 'obj_num': obj_num,
00134                 'trial_num': trial_num,
00135                 'pos_readings': pos_readings_search,
00136                 'tot_readings':tot_readings_search,
00137                 'best_pos': '--',
00138                 'orient_est': '--',
00139                 'dxy': '--',
00140                 'dtheta': '--',
00141                 'servo_yn': servo_yn,
00142                 'other': { 'w_mass': 0.0,  # specific to pf
00143                            'orient_fit': 0.0 }}
00144         return False, res, None
00145 
00146     f = open( pf_search )  # load the particle set from Search data.  Will overwrite if using search plus servo
00147     p_set_loaded = pkl.load( f )
00148     f.close()
00149 
00150     pos_readings = pos_readings_search
00151     tot_readings = tot_readings_search
00152 
00153     # If this is Search PLUS servo...
00154     if servo_yn:
00155 
00156         # update the stats for pos reads
00157         f = open( servo_fname, 'r' )
00158         summary_servo = pkl.load( f )
00159         f.close()
00160 
00161         pos_readings_servo = sum([ True for p in summary_servo if p.read.rssi != -1 and p.read.tagID == obj_name ])
00162         tot_readings_servo = len( summary_servo )
00163 
00164         pos_readings = pos_readings_search + pos_readings_servo
00165         tot_readings = tot_readings_search + tot_readings_servo
00166 
00167 
00168         # use the particle set from the SERVO data
00169         f = open( pf_servo )
00170         p_set_loaded = pkl.load( f )
00171         f.close()
00172 
00173 
00174 
00175     # print '\t Positive Reads: %d of %d (%2.1f)' % ( pos_readings,
00176     #                                                 tot_readings,
00177     #                                                 100.0 * pos_readings / tot_readings )
00178 
00179     # maxw = np.max( p_set_loaded[:,2] )
00180     # minw = np.min( p_set_loaded[:,2] )
00181     # w_normed = 1.0 * ( p_set_loaded[:,2] - minw ) / ( maxw - minw )
00182 
00183     # # Only keep particles in the top 98% of likelihoods (250 / 255).  This
00184     # # makes the selection only consider locations where the probability
00185     # # mass is actually located. (These are the ones displayed in the
00186     # # screen captures!)
00187 
00188     # p_set = p_set_loaded[ np.where( w_normed > 0.02 )[0] ]
00189     # print 'Shape: ', p_set.shape
00190 
00191     # # print 'p_set size (pre-cap): ', p_set.shape
00192     # # p_set_precap = np.copy( p_set )
00193     # # p_set = p_set[np.argsort(p_set[:,2])][:1000] # Cap the total number: keep at most the top 1000
00194     # # print 'p_set size (pre-cap): ', p_set.shape
00195 
00196     p_set = np.copy( p_set_loaded )
00197     p_set = p_set[ np.argsort( p_set[:,2] )[::-1] ] # sort by decreasing weight
00198     w_norm = p_set[:,2] / np.sum( p_set[:,2] )
00199     w_cum = np.cumsum( w_norm )
00200 
00201 
00202     # ONLY KEEP top 8000 particles (computation)
00203     w_mass =  w_cum[:8000][-1] * 100.0 # Ratio of mass in p_set to total:
00204     p_set = p_set[:8000] # only keep top 8000 for computational reasons!
00205 
00206     # # ONLY KEEP particles that are in top 98% of normalized values. (these are the ones displayed)
00207     # maxw = np.max( p_set[:,2] )
00208     # minw = np.min( p_set[:,2] )
00209     # w_scaled = 1.0 * ( p_set[:,2] - minw ) / ( maxw - minw )
00210     # p_set = p_set[ np.where( w_scaled > 0.02 )[0] ]
00211     # p_set = p_set[:8000] # Only keep top 8000 max
00212     # w_mass = w_cum[ p_set.shape[0] ]
00213     # print 'p_set size (pre-cap): ', p_set.shape
00214     # print w_mass
00215     
00216 
00217     # print '\tShape: ', p_set.shape
00218 
00219     pf_costmap = '/home/travis/svn/robot1/src/projects/rfid_pf/src/rfid_pf/pf_costmap.pkl'
00220 
00221     f = open( pf_costmap )
00222     costmap = pkl.load( f )
00223     f.close()
00224 
00225     cm = costmap[ np.where( costmap[:,2] )[0] ]  # Locations where the robot can be located
00226 
00227     # Determine the score for each possible xy robot location
00228 
00229     def score_loc( xy ):
00230         # The xy location under consideration for the robot
00231         mag = np.sqrt( np.sum( np.power( p_set[:,0:2] - xy, 2.0 ), axis = 1))  # || dist ||
00232         score = mag * p_set[:,2] #  || dist || * w's
00233         return np.sum( score )
00234 
00235 
00236     # Compute all the scores for possible robot locations
00237     t0 = time.time()
00238     pos_scores = [ score_loc( i[0:2] ) for i in cm ]
00239     dt = time.time() - t0
00240 
00241     # print '\tScore computations per second: ', len( pos_scores ) * 1.0 / dt
00242 
00243     best_ind = np.argmin( pos_scores )
00244     best_pos = cm[ best_ind ][0:2]
00245 
00246 
00247     # # Calculate the angle that is the mean
00248     # # Now that we have best_pos, we need to find the best orientation.
00249 
00250     # def score_orient( xyw, best_pos ): # returns 1x2
00251     #     # xyw is 1x3
00252     #     dxy = xyw[0:2] - best_pos  # move into best_pose frame (1x2)
00253 
00254     #     dxy_unit = dxy / np.linalg.norm( dxy ) # normalize to unit circle
00255     #     return dxy_unit * xyw[2] # 1x2;  [x_circ => x / |dxy| * w, y_circ...]
00256 
00257     # so = np.array([ score_orient( i, best_pos ) for i in p_set ])
00258     # so[ np.where(np.isnan( so )) ] = 0.0  # for positions where the particle is one and the same, the norm is 0.0 so score is nan.
00259 
00260     # x_so = np.sum( so[:,0] )
00261     # y_so = np.sum( so[:,1] )
00262     # orient_est = np.arctan2( y_so, x_so )
00263 
00264     # orient_fit = np.sqrt( x_so**2.0 + y_so**2.0 ) / ( np.sum( p_set[:,2] ))
00265 
00266     # Brute force calculate the angle that yields the minimum |dtheta|
00267     theta_hats = np.linspace( -1.0 * np.pi, np.pi, 360, endpoint = False )
00268     theta_hats = np.array([ mu.standard_rad( i ) for i in theta_hats ])
00269 
00270     dxy = p_set[:,0:2] - best_pos # put the p_set into the best_pos frame!
00271     pset_thetas = np.arctan2( dxy[:,1], dxy[:,0] )
00272     pset_thetas = np.array([ mu.standard_rad( i ) for i in pset_thetas ])
00273 
00274     pset_w_normed = p_set[:,2] / np.sum( p_set[:,2] )
00275 
00276     def exp_err( th ):
00277         errs = np.abs([ mu.standard_rad( i ) for i in th - pset_thetas ])
00278         weighted_errs = pset_w_normed * errs
00279         mean_we = np.mean( weighted_errs )
00280         return mean_we
00281         
00282     theta_hats_res = np.array([ exp_err( i ) for i in theta_hats ])
00283     # rrr = theta_hats
00284     # res = theta_hats_res
00285 
00286     orient_est_ind = np.argmin( theta_hats_res )
00287     orient_est = theta_hats[ orient_est_ind ]
00288 
00289     # Compute errors:
00290     dxy = np.linalg.norm( best_pos - loc_pos[0:2] )
00291     true_theta = np.arctan2( loc_pos[1] - best_pos[1],  # y / x
00292                              loc_pos[0] - best_pos[0] )
00293     dtheta = mu.standard_rad( orient_est - true_theta )
00294 
00295     res = { 'loc': loc,
00296             'obj_num': obj_num,
00297             'trial_num': trial_num,
00298             'pos_readings': pos_readings,
00299             'tot_readings':tot_readings,
00300             'best_pos': best_pos,
00301             'orient_est': orient_est,
00302             'dxy': dxy,
00303             'dtheta': dtheta,
00304             'servo_yn': servo_yn,
00305             'other': { 'w_mass': w_mass,  # specific to pf
00306                        'theta_hats': theta_hats,
00307                        'theta_hats_res': theta_hats_res,
00308                        'p_set': p_set }}
00309     
00310     return True, res, np.copy( p_set )  # Was_reads?, results dict, particles (for display)
00311 
00312 
00313 
00314 def MAIN_PROCESS( trial_num, obj_num, servo_yn, screen_cap = False ):
00315     print 'In MP: ', trial_num, obj_num, servo_yn, screen_cap
00316     # best_pos, orient_est, p_set = process_trialobj( 4, 3, True )
00317     was_reads, res, p_set = process_trialobj( trial_num, obj_num, servo_yn )
00318 
00319     print 'RESULTS'
00320     pprint( res )
00321 
00322     # save results:
00323 
00324     if screen_cap:
00325         f = open( 'Obj%d_Trial%d_Servo%d_pf_results.pkl' % (obj_num, trial_num, int( servo_yn )), 'w')
00326         pkl.dump( res, f )
00327         f.close()
00328 
00329     # Make screen capture.
00330 
00331     if not was_reads:  # Skip step if no reads.
00332         return
00333 
00334     best_pos = res[ 'best_pos' ]
00335     orient_est = res[ 'orient_est' ]
00336 
00337     pub_mark = rospy.Publisher( '/tag_poses', vm.Marker )
00338 
00339     if servo_yn:
00340         c_tm = [0./255, 205./255, 255./255, 1.0] # rgba
00341     else:
00342         c_tm = [255./255, 123./255, 1./255, 1.0] # rgba
00343 
00344     tm = vsm( np.matrix([ best_pos[0],best_pos[1], 0.0 ]).T,
00345               np.matrix(tft.quaternion_from_euler( 0.0, 0.0, orient_est )).T,
00346               'arrow', '/map',
00347               scale = [0.5, 1.0, 1.0],
00348               color = c_tm, 
00349               duration = 50.0,
00350               m_id = 2*p_set.shape[0] + 1 )
00351 
00352     def pub_tm( ):
00353         tm.header.stamp = rospy.Time.now()
00354         for i in xrange( 10 ):
00355             pub_mark.publish( tm )
00356             rospy.sleep( 0.3 )
00357 
00358     print 'Click on RVIZ!'
00359     time.sleep( 3 )
00360 
00361     pub_tm()
00362     pub_tm()
00363     display_particles.display_trialobj( trial_num, obj_num, servo_yn, screen_cap = screen_cap )
00364     print 'Done.\n\n\n'
00365 
00366     return
00367 
00368 
00369 
00370 
00371 
00372 
00373 if __name__ == '__main__':
00374 
00375     import optparse
00376     p = optparse.OptionParser()
00377     p.add_option('--trial', action='store', type='int', dest='trial',
00378                  help='trial number (0-8)')
00379     p.add_option('--obj', action='store', type='int', dest='obj',
00380                  help='object number (0-8)')
00381     p.add_option('--servo', action='store_true', dest='servo',
00382                  help='Use combined search and servo?', default = False)
00383     opt, args = p.parse_args()
00384 
00385     obj_num = opt.obj
00386     trial_num = opt.trial
00387     servo_yn = opt.servo
00388     
00389     rospy.init_node( 'goober' )
00390 
00391 
00392     if trial_num < 9:
00393         while not rospy.is_shutdown():
00394             print 'Publishing.'
00395             MAIN_PROCESS( trial_num, obj_num, servo_yn )
00396     else:
00397         # for i in range( 0, 1 ): # trial
00398         #     for j in range( 4, 5 ): # obj
00399         for i in range( 0, 9 ): # trial
00400             for j in range( 0, 9 ): # obj
00401                 for s in [False, True]: # servo_yn
00402                     MAIN_PROCESS( i, j, s, screen_cap = True )


rfid_pf
Author(s): Travis Deyle
autogenerated on Wed Nov 27 2013 12:03:34