def process_read( pf, p_start, m ):
# Note: p_start is numpy array (Nx3) and it's weights ([:,2]) will be modified in-place!
# m is form: RFIDread, PoseStamped (ant in map), PoseStamped (robot base in map)
# We assume it is a postive reading for the desired tagID!
xy_map = np.copy( p_start[:,0:2].T ) # 2xN
# Antenna is at position given by m.ps_ant_map.
# IMPORTANT NOTE: I'm assuming that the antenna XY plane is parallel to the map XY plane!
o = m.ps_ant_map.pose.orientation
d = m.ps_ant_map.pose.position
roll, pitch, yaw = tft.euler_from_quaternion([ o.x, o.y, o.z, o.w ])
dx, dy = d.x, d.y
# Transform xy_map points into antenna frame
t_new = tr.composeHomogeneousTransform( tr.rotZ( yaw ),
np.matrix([dx,dy,0.0]).T )
trans = tr.invertHomogeneousTransform( t_new )
xy_new = (trans * tr.xyToHomogenous( xy_map ))[0:2] # This is a matrix! (2xN)
xy_new = np.array( xy_new ) # Convert it back to array!
# Build new particles using old weights (Nx3)
p_new = np.column_stack([ xy_new.T, p_start[:,2] ]) # take weights from last iteration
rv = pf.step( 0.0, m.read.rssi, p_new, should_resample_func = pfilter.retFalse )
# Put back the new weights:
p_start[:,2] = rv[:,2]
return
def mecanumTglobal(p,floating_vector=False):
''' 3x1 vector from global to mecanum.
'''
p_hom = tr.xyzToHomogenous(p, floating_vector)
p_gl = tr.invertHomogeneousTransform(_globalT['mecanum']) * p_hom
if floating_vector == False:
return p_gl[0:3]/p_gl[3]
else:
return p_gl[0:3]
def globalTutmcam0(p,ang,floating_vector=False):
t = utmcam0Tglobal_mat(ang)
t = tr.invertHomogeneousTransform(t)
p_hom = tr.xyzToHomogenous(p, floating_vector)
p_c = t * p_hom
if floating_vector == False:
pt = p_c[0:3]/p_c[3]
else:
pt = p_c[0:3]
return pt
def utmcam0Tglobal_mat(ang):
thok0Tglobal_mat = tr.invertHomogeneousTransform(_globalT['thok0'])
# servo angle.
disp = np.matrix([0.,0.,0.]).T
tmat = tr.composeHomogeneousTransform(tr.Ry(ang),disp)*thok0Tglobal_mat
# cameraTlaser from thok_cam_calib.py
x = 0.012
y = -0.056
z = 0.035
r1 = 0.
r2 = 0.
r3 = -0.7
disp = np.matrix([-x,-y,-z]).T
r = tr.Rz(math.radians(-90))*tr.Ry(math.radians(90.))
disp = r*disp
r = r*tr.Rx(math.radians(r1))
r = r*tr.Ry(math.radians(r2))
r = r*tr.Rz(math.radians(r3))
t = tr.composeHomogeneousTransform(r, disp)
tmat = t*tmat
return tmat
def process_read(pf, p_start, m):
# Note: p_start is numpy array (Nx3) and it's weights ([:,2]) will be modified in-place!
# m is form: RFIDread, PoseStamped (ant in map), PoseStamped (robot base in map)
# We assume it is a postive reading for the desired tagID!
xy_map = np.copy(p_start[:, 0:2].T) # 2xN
# Antenna is at position given by m.ps_ant_map.
# IMPORTANT NOTE: I'm assuming that the antenna XY plane is parallel to the map XY plane!
o = m.ps_ant_map.pose.orientation
d = m.ps_ant_map.pose.position
roll, pitch, yaw = tft.euler_from_quaternion([o.x, o.y, o.z, o.w])
dx, dy = d.x, d.y
# Transform xy_map points into antenna frame
t_new = tr.composeHomogeneousTransform(tr.rotZ(yaw),
np.matrix([dx, dy, 0.0]).T)
trans = tr.invertHomogeneousTransform(t_new)
xy_new = (trans *
tr.xyToHomogenous(xy_map))[0:2] # This is a matrix! (2xN)
xy_new = np.array(xy_new) # Convert it back to array!
# Build new particles using old weights (Nx3)
p_new = np.column_stack([xy_new.T,
p_start[:,
2]]) # take weights from last iteration
rv = pf.step(0.0,
m.read.rssi,
p_new,
should_resample_func=pfilter.retFalse)
# Put back the new weights:
p_start[:, 2] = rv[:, 2]
return
def tool_pointerTlaser(rollAng, panAng, tiltAng, residuals = np.zeros([4,6])):
return tr.invertHomogeneousTransform( laserTtool_pointer(rollAng, panAng, tiltAng, residuals) )
xyw = np.row_stack([ X.flatten(), # Build Nx3
Y.flatten(),
np.ones( X.shape ).flatten() ]).T # weights (multiplicative)
p_set = np.copy( xyw )
pf = pfilter.PFilter( rfid_model.NoMotion(), rfid_model.RfidModel( rfid_model.yaml_fname ))
t0 = time.time()
rv = pf.step( 0.0, 83, p_set, should_resample_func = pfilter.retFalse ) # rv are the new weights
# New antenna is at position (5,-5) and rotated 90-deg. relative to MAP frame
t_new = tr.composeHomogeneousTransform( tr.rotZ( math.radians(90) ), np.matrix([5,-5,0]).T )
# This transform takes points from map frame into t_new:
trans = tr.invertHomogeneousTransform( t_new )
xy_new = np.array((trans * tr.xyToHomogenous( xyw[:,0:2].T) )[0:2].T)
xyw_new = np.column_stack([ xy_new, rv[:,2] ]) # take weights from last iteration
rv = pf.step( 0.0, 83, xyw_new, should_resample_func = pfilter.retFalse ) # rv are the new weights
t1 = time.time()
print 'Time: %3.2f ' % (t1 - t0)
to_print = np.copy( xyw )
to_print[:,2] = rv[:,2] # Just keep weights, but use original points in "map" frame
dw = DisplayWeights()
p_set = np.copy(xyw)
pf = pfilter.PFilter(rfid_model.NoMotion(),
rfid_model.RfidModel(rfid_model.yaml_fname))
t0 = time.time()
rv = pf.step(0.0, 83, p_set,
should_resample_func=pfilter.retFalse) # rv are the new weights
# New antenna is at position (5,-5) and rotated 90-deg. relative to MAP frame
t_new = tr.composeHomogeneousTransform(tr.rotZ(math.radians(90)),
np.matrix([5, -5, 0]).T)
# This transform takes points from map frame into t_new:
trans = tr.invertHomogeneousTransform(t_new)
xy_new = np.array((trans * tr.xyToHomogenous(xyw[:, 0:2].T))[0:2].T)
xyw_new = np.column_stack([xy_new, rv[:,
2]]) # take weights from last iteration
rv = pf.step(0.0, 83, xyw_new,
should_resample_func=pfilter.retFalse) # rv are the new weights
t1 = time.time()
print 'Time: %3.2f ' % (t1 - t0)
to_print = np.copy(xyw)
to_print[:,
2] = rv[:,
2] # Just keep weights, but use original points in "map" frame
