00001
00002
00003
00004
00005 import rfid_model
00006 import lib_pfilter
00007
00008 import random as rd
00009 import cPickle as pkl
00010 import numpy as np, math
00011 import pylab as pl
00012 import time
00013 import glob
00014
00015
00016
00017 tdb = { 0: ['OrangeMedBot',[]],
00018 1: ['TravisTVremo',[]],
00019 2: ['RedBottle ',[]],
00020 3: ['OnMetalKeys ',[]],
00021 4: ['WhiteMedsBot',[]],
00022 5: ['BlueMedsBox ',[]],
00023 6: ['TeddyBearToy',[]],
00024 7: ['CordlessPhon',[]],
00025 8: ['BlueHairBrus',[]]}
00026
00027 pts = { 0: ['BehindTree',[3.757, 6.017, 0.036]],
00028 1: ['FireplaceMantle',[5.090, 4.238, 1.514]],
00029 2: ['CircleEndTable',[5.399, 2.857, 0.607]],
00030 3: ['Couch',[3.944, 1.425, 0.527]],
00031 4: ['RectEndTable',[3.302, 0.932, 0.534]],
00032 5: ['BehindKitchenTable',[-0.339, -2.393, 0.793]],
00033 6: ['NearDishwaser',[-1.926, -0.835, 0.946]],
00034 7: ['InCupboard',[-3.257, 1.294, 1.397]],
00035 8: ['OnFilingCabinet',[-0.083, 2.332, 0.670]]}
00036
00037
00038 pf_costmap = '/home/travis/svn/robot1/src/projects/rfid_pf/src/rfid_pf/pf_costmap.pkl'
00039
00040 f = open( pf_costmap )
00041 costmap = pkl.load( f )
00042 f.close()
00043
00044
00045
00046 def best_location( loc_num ):
00047 tag_loc_xyz = pts[loc_num][1]
00048
00049 location_xy = np.array( tag_loc_xyz[0:2] )
00050
00051 cm = costmap[ np.where( costmap[:,2] )[0] ]
00052 dxycm = cm[ :, 0:2 ] - location_xy
00053 dist = np.sqrt( np.power( dxycm[:,0], 2.0 ) +
00054 np.power( dxycm[:,1], 2.0 ))
00055
00056 sind = np.argsort( dist )
00057
00058 best_pos = cm[sind[0],0:2]
00059 dxy = dist[sind[0]]
00060 next_nearest = dist[ sind[1] ]
00061
00062 print 'BEST for location %d: %s' % ( loc_num, pts[loc_num][0] )
00063 print '\tBest Position ', best_pos
00064 print '\tErr (meters) ', dxy
00065 print
00066
00067 return best_pos, dxy
00068
00069
00070
00071 def uniform_predict( loc_num ):
00072 tag_loc_xyz = pts[loc_num][1]
00073
00074 location_xy = np.array( tag_loc_xyz[0:2] )
00075 cm = costmap[ np.where( costmap[:,2] )[0] ]
00076
00077 dxycm = cm[ :, 0:2 ] - location_xy
00078 dist = np.sqrt( np.power( dxycm[:,0], 2.0 ) +
00079 np.power( dxycm[:,1], 2.0 ))
00080
00081 sind = np.argsort( dist )
00082
00083
00084 dxy_mean = np.mean( dist )
00085 dxy_std = np.std( dist )
00086
00087
00088
00089
00090
00091
00092
00093 dtheta_mean = 0.5 * ( np.pi + 0.0 )
00094 dtheta_std = np.sqrt( 1.0 / 12.0 ) * ( np.pi - 0 )
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109 print 'UNIFORM for location %d: %s' % ( loc_num, pts[loc_num][0] )
00110 print '\tPosition Err (meters): %2.2f (%2.2f)' % (dxy_mean, dxy_std)
00111 print '\tAngular Err (deg): %3.2f (%3.2f)' % (math.degrees( dtheta_mean ),
00112 math.degrees( dtheta_std ))
00113 print
00114
00115 return dxy_mean, dxy_std, dtheta_mean, dtheta_std
00116
00117
00118
00119 if __name__ == '__main__':
00120
00121 for nn in xrange( 9 ):
00122 best_location( nn )
00123 uniform_predict( nn )
00124
00125
00126