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 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
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() while not rospy.is_shutdown():
xyw_orig = np.copy(xyw) rm = rfid_model.RfidModel(rfid_model.yaml_fname) # pf = lib_pfilter.PFilter( lib_pfilter.NoMotion(), rm, xyw ) # pf.measurement( 80 ) # Antenna reading at origin (max near 4,0) # rv = np.copy( xyw ) rv = rm.weight_set(83, xyw) # Move antenna to 4,-4 and rotated 90 deg xy = xyw[:, 0:2] trans = tr.composeHomogeneousTransform(tr.rotZ(math.radians(-90)), np.matrix([5, 5, 0]).T) xy_new = np.array((trans * tr.xyToHomogenous(xy.T))[0:2].T) xyw_new = np.column_stack([xy_new, rv[:, 2]]) # take weights from last iteration rv_new = rm.weight_set(83, xyw_new) to_print = np.copy(xyw_orig) to_print[:, 2] = rv_new[:, 2] # Just keep weights, but use original points in "map" frame dw = DisplayWeights() while not rospy.is_shutdown(): print 'UPDATING' dw.update(to_print)