def test_jacobian_pose_pose():
eps = 1e-5;
x1 = scipy.array([1.1, 0.9, 1])
x2 = scipy.array([2.2, 1.85, 1.2])
z = scipy.array([0.9, 1.1, 1.05])
# get the analytic Jacobian
[e, A, B] = main.linearize_pose_pose_constraint(x1, x2, z)
# check the error vector
e_true = scipy.array([-1.06617 -1.18076 -0.85000])
if norm(e - e_true) > eps:
print('Your error function seems to return a wrong value')
print('Result of your function', e)
print('True value', e_true)
else:
print('The computation of the error vector appears to be correct')
# compute it numerically
delta = 1e-6
scalar = 1. / (2*delta)
# test for x1
ANumeric = scipy.zeros((3,3))
for d in xrange(3):# = 1:3
curX = x1
curX[d] += delta
err = main.linearize_pose_pose_constraint(curX, x2, z)
curX = x1
curX[d] -= delta
err -= main.linearize_pose_pose_constraint(curX, x2, z)
ANumeric[:, d] = scalar * err
diff = ANumeric - A
if scipy.max(abs(diff))) > eps:
print('Error in the Jacobian for x1')
print('Your analytic Jacobian', A)
print('Numerically computed Jacobian', ANumeric)
print('Difference', diff)
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
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)
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
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)
def test_jacobian_pose_landmark():
eps = 1e-5;
x1 = scipy.array([1.1, 0.9, 1.])
x2 = scipy.array([2.2 1.9])
z = scipy.array([1.3 -0.4])
# get the analytic Jacobian
[e, A, B] = main.linearize_pose_landmark_constraint(x1, x2, z)
# check the error vector
e_true = scipy.array([0.135804, 0.014684])
if norm(e - e_true) > eps:
print('Your error function seems to return a wrong value')
print('Result of your function', e)
print('True value', e_true)
else:
print('The computation of the error vector appears to be correct')
# compute it numerically
delta = 1e-6
scalar = 1. / (2*delta)
# test for x1
ANumeric = scipy.zeros((2,3))
for d in xrange(3):#= 1:3
curX = x1
curX[d] += delta
err = main.linearize_pose_landmark_constraint(curX, x2, z)
curX = x1
curX[d] -= delta
err -= main.linearize_pose_landmark_constraint(curX, x2, z)
ANumeric[:, d] = scalar * err
diff = ANumeric - A
if scipy.max(abs(diff)) > eps:
print('Error in the Jacobian for x1')
print('Your analytic Jacobian', A)
print('Numerically computed Jacobian', ANumeric)
print('Difference', diff)
else:
print('Jacobian for x1 appears to be correct')
# test for x2
BNumeric = scipy.zeros((2,2))
for d in xrange(2):# 1:2
curX = x2
curX[d] += delta
err = main.linearize_pose_landmark_constraint(x1, curX, z)
curX = x2
curX[d] -= delta
err -= main.linearize_pose_landmark_constraint(x1, curX, z)
BNumeric[:, d] = scalar * err
diff = BNumeric - B
if scipy.max(abs(diff))) > eps:
print('Error in the Jacobian for x2')
print('Your analytic Jacobian', B)
print('Numerically computed Jacobian', BNumeric)
print('Difference', diff)
#test for x2
BNumeric = scipy.zeros((3,3))
for d in xrange(3):# = 1:3
curX = x2
curX[d] += delta
err = main.linearize_pose_pose_constraint(x1, curX, z)
curX = x2
curX[d] -= delta
err -= main.linearize_pose_pose_constraint(x1, curX, z)
BNumeric[:, d] = scalar * err
diff = BNumeric - B;
if scipy.max(abs(diff))) > eps:
print('Error in the Jacobian for x2')
print('Your analytic Jacobian', B)
print('Numerically computed Jacobian', BNumeric)
print('Difference', diff)
else:
print('Jacobian for x2 appears to be correct')
def test_jacobian_pose_landmark():
eps = 1e-5;
x1 = scipy.array([1.1, 0.9, 1.])
x2 = scipy.array([2.2 1.9])
z = scipy.array([1.3 -0.4])
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')