Example #1
0
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
Example #5
0
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
Example #6
0
def tool_pointerTlaser(rollAng, panAng, tiltAng, residuals = np.zeros([4,6])):
    return tr.invertHomogeneousTransform( laserTtool_pointer(rollAng, panAng, tiltAng, residuals) )
Example #7
0
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()
Example #8
0
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