Ejemplo n.º 1
0
def robotlaser_as_cartesian(rl, maxRange=15, subsample=False):

    numBeams = len(rl.ranges)
    maxRange=scipy.min(maxRange, rl.maximum_range);
    # apply the max range
    idx = scipy.logical_and(rl.ranges < maxRange, rl.ranges>0)
    
    if subsample:
        idx[1:2:] = 0.

    angles = scipy.linspace(rl.start_angle, rl.start_angle + numBeams*rl.angular_resolution, numBeams)[idx]
    points = scipy.array([rl.ranges(idx)*scipy.cos(angles),
                          rl.ranges(idx)*scipy.sin(angles),
                          scipy.ones(1, len(angles))])
    transf = v2t(rl.laser_offset)

    # apply the laser offset
    points = transf * points

    return points
Ejemplo n.º 2
0
def calc_load_xyz(datafile, invert=False, filter=None, scale=None,
                  clip=None):
    """A function to take a file of data and create an XYV data object.
    
    datafile  a file containing (lat, lon, val) values
    invert    used if 'filter' not supplied and an internal filter is used.
              if 'invert' is True, switch the order of lon/lat in data lines.
    filter    a function to extract (lon, lat, val) from one datafile line
              if not supplied an internal function is used
    scale     amount to scale the data (ie, divide)
    clip      if defined is a dictionary defining clip limits. Recognised
              keys in the dictionary are:
                 'high'     sets high limit above which values are clipped
                 'low'      sets low limit below which values are clipped
              At least one of the above keys must exist.
              The clipping is done before any scaling.

    Returns a list of tuples (lon, lat, val).

    """

    # if user didn't supply a data filter, use our own
    if filter is None:
        # define a default filter and use it
        if invert:
            def default_filter(line):
                return (float(line[1]), float(line[0]), float(line[2]))
        else:
            def default_filter(line):
                return (float(line[0]), float(line[1]), float(line[2]))

        filter = default_filter

    # get data into memory
    data = []
    for line in file(datafile):
        # ignore blank or comment lines
        line = line.strip()
        if line == '' or line[0] in '%#':
            continue

        # get lon+lat+value from line
        data.append(filter(SplitPattern.split(line)))
    data = scipy.array(data)

    # do clipping, if required
    if clip:
        low_clip = clip.get('low', None)
        high_clip = clip.get('high', None)

        if low_clip is None:
            low_clip = scipy.min(data[:,2])
        if high_clip is None:
            high_clip = scipy.max(data[:,2])

        data[:,2] = scipy.clip(data[:,2], low_clip, high_clip)

    # handle any scaling
    if scale:
        scale = int(scale)
        data[:,2] = data[:,2] / scale

    return data
Ejemplo n.º 3
0
def calc_mean_xyz(datafile,
                  scale=None,
                  bins=100,
                  filter=None,
                  invert=False,
                  clip=None):
    """Convert a file of (lon, lat, val) data to gridded XYZ data object.
    
    Take values at random x,y points and bin into summed values.

    datafile  a file containing (lon, lat, val) values
    scale     amount to scale the data (ie, divide)
    bins      number of bins in X and Y direction
                  if an int, # bins in X and Y direction
                  if [int, int] the X and Y bin counts may be different
    filter    a function to extract (lon, lat, val) from one datafile line
              if not supplied an internal function is used
    invert    if 'filter' not supplied, an internal filter is used.  if
              'invert' is True, switch the order of lat/lon in data lines.
    clip      if defined is a dictionary defining clip limits. Recognised
              keys in the dictionary are:
                 'high'     sets high limit above which values are clipped
                 'low'      sets low limit below which values are clipped
              At least one of the above keys must exist.
              The clipping is done before any scaling.

    Returns a tuple (bins, xyz) where bins is the number of bins in the X
    direction and xyz is a numpy array of XYZ data [[lon, lat, val], ...].

    Values that are 0 replaced with Nan.

    """

    # handle optional parameters
    try:
        bin_len = len(bins)
    except TypeError:
        bins_x = bins_y = bins
    else:
        try:
            (bins_x, bins_y) = bins
        except ValueError:
            raise RuntimeError("Bad 'bins' value, expected int or [int, int]")

    # if user didn't supply a data filter, use our own
    if filter is None:
        # define a default filter and use it
        if invert:

            def default_filter(line):
                return (float(line[1]), float(line[0]), float(line[2]))
        else:

            def default_filter(line):
                return (float(line[0]), float(line[1]), float(line[2]))

        filter = default_filter

    # get data into memory
    data = []
    for line in file(datafile):
        # ignore blank or comment lines
        line = line.strip()
        if line == '' or line[0] in '%#':
            continue

        # get lon+lat+value from line
        data.append(filter(SplitPattern.split(line)))
    data = scipy.array(data)

    # do clipping, if required
    if clip:
        low_clip = clip.get('low', None)
        high_clip = clip.get('high', None)

        if low_clip is None:
            low_clip = scipy.min(data[:, 2])
        if high_clip is None:
            high_clip = scipy.max(data[:, 2])

        data[:, 2] = scipy.clip(data[:, 2], low_clip, high_clip)

    # handle any scaling
    if scale:
        scale = int(scale)
        data[:, 2] = data[:, 2] / scale

#    # get extent of data (tight first, then with margin)
#    (tll_lat, tll_lon, tur_lat, tur_lon) = ge.get_extent(data, margin=0)
#    tr_opt = '-R%f/%f/%f/%f' % (tll_lon, tur_lon, tll_lat, tur_lat)
#
#    (ll_lat, ll_lon, ur_lat, ur_lon) = ge.get_extent(data)
#    r_opt = '-R%f/%f/%f/%f' % (ll_lon, ur_lon, ll_lat, ur_lat)

# now generate a binned dataset
    (sum_data, xedges, yedges) = num.histogram2d(data[:, 0],
                                                 data[:, 1],
                                                 bins=bins,
                                                 normed=False,
                                                 weights=data[:, 2])

    (count_data, _, _) = num.histogram2d(data[:, 0],
                                         data[:, 1],
                                         bins=bins,
                                         normed=False)

    calc_mean = sum_data / count_data

    # create XYZ object
    # make sure X+Y is *centre* of each bin
    xyz = []
    xedges = scipy.array(xedges)
    xedges = xedges[:-1] + (xedges[1] - xedges[0]) / 2
    yedges = scipy.array(yedges)
    yedges = yedges[:-1] + (yedges[1] - yedges[0]) / 2
    for (xi, x) in enumerate(xedges):
        for (yi, y) in enumerate(yedges):
            xyz.append([x, y, calc_mean[xi, yi]])
    xyz = scipy.array(xyz)

    return (bins_x, xyz)
Ejemplo n.º 4
0
def calc_load_xyvv(datafile, invert=False, filter=None, scale=None,
                   clip=None):
    """A function to take a file of data and create an XYVV data object.
    
    datafile  a file containing (lat, lon, val) values
    invert    used if 'filter' not supplied and an internal filter is used.
              if 'invert' is True, switch the order of lon/lat in data lines.
    filter    a function to extract (lon, lat, val) from one datafile line
              if not supplied an internal function is used
    scale     amount to scale the data (ie, divide)
    clip      if defined is a dictionary defining clip limits. Recognised
              keys in the dictionary are:
                 'high'     sets high limit above which values are clipped
                 'low'      sets low limit below which values are clipped
              At least one of the above keys must exist.
              The clipping is done before any scaling.

    Returns a list of tuples (lon, lat, val1, val2).

    """

    # if user didn't supply a data filter, use our own
    if filter is None:
        # define a default filter and use it
        if invert:
            def default_filter(line):
                return (float(line[1]), float(line[0]), float(line[2]),
                        float(line[3]))
        else:
            def default_filter(line):
                return (float(line[0]), float(line[1]), float(line[2]),
                        float(line[3]))

        filter = default_filter

    # get data into memory
    data = []
    for line in file(datafile):
        # ignore blank or comment lines
        line = line.strip()
        if line == '' or line[0] in '%#':
            continue

        # get lon+lat+value from line
        data.append(filter(SplitPattern.split(line)))
    data = scipy.array(data)

    # do clipping, if required
    if clip:
        low_clip = clip.get('low', None)
        high_clip = clip.get('high', None)

        if low_clip is None:
            low_clip = scipy.min(data[:,2])
        if high_clip is None:
            high_clip = scipy.max(data[:,2])

        data[:,2] = scipy.clip(data[:,2], low_clip, high_clip)

    # handle any scaling
    if scale:
        scale = int(scale)
        data[:,2] = data[:,2] / scale

    return data
Ejemplo n.º 5
0
def new_activation(x):
    return sp.min(Activation.relu(x))
Ejemplo n.º 6
0
def calc_sum_xyz(datafile, scale=None, bins=100, filter=None, invert=False,
                clip=None):
    """A function to take a file of data and create an XYZ data object.
    
    Take values at random x,y points and bin into summed values.

    datafile  a file containing (lat, lon, val) values
    scale     amount to scale the data (ie, divide)
    bins      number of bins in X and Y direction
                  if an int, # bins in X and Y direction
                  if [int, int] the X and Y bin counts may be different
    filter    a function to extract (lon, lat, val) from one datafile line
              if not supplied an internal function is used
    invert    if 'filter' not supplied, an internal filter is used.  if
              'invert' is True, switch the order of lat/lon in data lines.
    clip      if defined is a dictionary defining clip limits. Recognised
              keys in the dictionary are:
                 'high'     sets high limit above which values are clipped
                 'low'      sets low limit below which values are clipped
              At least one of the above keys must exist.
              The clipping is done before any scaling.

    Returns a numpy array of XYZ data [[lon, lat, val], ...].

    Values that are 0 replaced with Nan.

    """

    # handle optional parameters
    try:
        bin_len = len(bins)
    except TypeError:
        bins_x = bins_y = bins
    else:
        try:
            (bins_x, bins_y) = bins
        except ValueError:
            raise RuntimeError("Bad 'bins' value, expected int or [int, int]")

    # if user didn't supply a data filter, use our own
    if filter is None:
        # define a default filter and use it
        if invert:
            def default_filter(line):
                return (float(line[1]), float(line[0]), float(line[2]))
        else:
            def default_filter(line):
                return (float(line[0]), float(line[1]), float(line[2]))

        filter = default_filter

    # get data into memory
    data = []
    for line in file(datafile):
        # ignore blank or comment lines
        line = line.strip()
        if line == '' or line[0] in '%#':
            continue

        # get lon+lat+value from line
        data.append(filter(SplitPattern.split(line)))
    data = scipy.array(data)

    # do clipping, if required
    if clip:
        low_clip = clip.get('low', None)
        high_clip = clip.get('high', None)

        if low_clip is None:
            low_clip = scipy.min(data[:,2])
        if high_clip is None:
            high_clip = scipy.max(data[:,2])

        data[:,2] = scipy.clip(data[:,2], low_clip, high_clip)

    # handle any scaling
    if scale:
        scale = int(scale)
        data[:,2] = data[:,2] / scale

    # get extent of data (tight first, then with margin)
    (tll_lat, tll_lon, tur_lat, tur_lon) = ge.get_extent(data, margin=0)
    tr_opt = '-R%f/%f/%f/%f' % (tll_lon, tur_lon, tll_lat, tur_lat)

    (ll_lat, ll_lon, ur_lat, ur_lon) = ge.get_extent(data)
    r_opt = '-R%f/%f/%f/%f' % (ll_lon, ur_lon, ll_lat, ur_lat)

    # now generate a binned dataset
    (binned_data, xedges, yedges) = scipy.histogram2d(data[:,0], data[:,1],
                                                      bins=bins, normed=False,
                                                      weights=data[:,2])

    # create XYZ object
    # make sure X+Y is *centre* of each bin
    xyz = []
    xedges = scipy.array(xedges)
    xedges = xedges[:-1] + (xedges[1] - xedges[0])/2
    yedges = scipy.array(yedges)
    yedges = yedges[:-1] + (yedges[1] - yedges[0])/2
    for (xi, x) in enumerate(xedges):
        for (yi, y) in enumerate(yedges):
            xyz.append([x, y, binned_data[xi,yi]])

    return scipy.array(xyz)
Ejemplo n.º 7
0
draw_probe_ellipse(mu, sigma, 0.9, 'r')
plt.plot(sigma_points[0], sigma_points[1], 'kx', markersize=10., linewidth=3.)

# Transform sigma points
sigma_points_trans = transform(sigma_points)

# Recover mu and sigma of the transformed distribution
mu_trans, sigma_trans = recover_gaussian(sigma_points_trans, w_m, w_m)

# Plot transformed sigma points with corresponding mu and sigma
plt.plot(mu_trans[0], mu_trans[1], 'bo', markersize=12., linewidth=3.)
plt.legend('transformed distribution')
draw_probe_ellipse(mu_trans, sigma_trans, 0.9, color='b')
plt.plot(sigma_points_trans[0],
         sigma_points_trans[1],
         'kx',
         markersize=10.,
         linewidth=3.)

# Figure axes setup
plt.title('Unscented Transform', 'fontsize', 20)
x_min = scipy.min(mu[0], mu_trans[0])
x_max = scipy.max(mu[0], mu_trans[0])
y_min = scipy.min(mu[1], mu_trans[1])
y_max = scipy.max(mu[1], mu_trans[1])
plt.axis([x_min - 3, x_max + 3, y_min - 3, y_max + 3])
plt.axis('equal')
plt.show()
# Print and save plot
#plt.savefig('unscented.png')