def make_data_twoclass(N=50): # generates some toy data mu = sp.array([[0,2],[0,-2]]).T C = sp.array([[5.,4.],[4.,5.]]) X = sp.hstack((mvn(mu[:,0],C,N/2).T, mvn(mu[:,1],C,N/2).T)) Y = sp.hstack((sp.ones((1,N/2.)),-sp.ones((1,N/2.)))) return X,Y
def main():
# create centers
means = [[1.0, 1.0], [2.5, 2.5]]
data = []
rect_start_x = 2.0
rect_start_y = 2.0
rect_width = 1.0
rec_height = 1.0
for i, mean in enumerate(means):
class_centers = mvn(mean, np.eye(2), 10)
class_data = [np.hstack((mvn(class_center, np.eye(2), 10), np.zeros((10, 1)) + i))
for class_center in class_centers]
data.append(class_data)
data = np.array(data).reshape(200, 3)
rectangle = np.array([[rect_start_x, rect_start_y],
[rect_start_x + rect_width, rect_start_y],
[rect_start_x + rect_width, rect_start_y + rec_height],
[rect_start_x, rect_start_y + rec_height],
[rect_start_x, rect_start_y]]).reshape(5, 2)
pts = get_points_in_rectangle(rectangle, data)
posterior = calc_posterior(means, pts)
print posterior
for class_data in data:
plt.plot(class_data[:, 0], class_data[:, 1], 'o', markersize=8)
plt.plot(rectangle[:, 0], rectangle[:, 1])
plt.show()
def main():
# create centers
means = [[1.0, 1.0], [2.5, 2.5]]
data = []
rect_start_x = 2.0
rect_start_y = 2.0
rect_width = 1.0
rec_height = 1.0
for i, mean in enumerate(means):
class_centers = mvn(mean, np.eye(2), 10)
class_data = [
np.hstack((mvn(class_center, np.eye(2), 10), np.zeros(
(10, 1)) + i)) for class_center in class_centers
]
data.append(class_data)
data = np.array(data).reshape(200, 3)
rectangle = np.array(
[[rect_start_x,
rect_start_y], [rect_start_x + rect_width, rect_start_y],
[rect_start_x + rect_width, rect_start_y + rec_height],
[rect_start_x, rect_start_y + rec_height],
[rect_start_x, rect_start_y]]).reshape(5, 2)
pts = get_points_in_rectangle(rectangle, data)
posterior = calc_posterior(means, pts)
print posterior
for class_data in data:
plt.plot(class_data[:, 0], class_data[:, 1], 'o', markersize=8)
plt.plot(rectangle[:, 0], rectangle[:, 1])
plt.show()
def make_data_twoclass(N=50):
# generates some toy data
mu = sp.array([[0,2],[0,-2]]).T
C = sp.array([[5.,4.],[4.,5.]])
X = sp.hstack((mvn(mu[:,0],C,N/2).T, mvn(mu[:,1],C,N/2).T))
Y = sp.hstack((sp.ones((1,N/2.)),-sp.ones((1,N/2.))))
return X,Y
def make_data_xor(N=80,noise=.25):
# generates some toy data
mu = sp.array([[-1,1],[1,1]]).T
C = sp.eye(2)*noise
X = sp.hstack((mvn(mu[:,0],C,N/4).T,mvn(-mu[:,0],C,N/4).T, mvn(mu[:,1],C,N/4).T,mvn(-mu[:,1],C,N/4).T))
Y = sp.hstack((sp.ones((1,N/2.)),-sp.ones((1,N/2.))))
randidx = sp.random.permutation(N)
Y = Y[0,randidx]
X = X[:,randidx]
return X,Y
def make_data_xor(N=80,noise=.25):
# generates some toy data
mu = sp.array([[-1,1],[1,1]]).T
C = sp.eye(2)*noise
X = sp.hstack((mvn(mu[:,0],C,N/4).T,mvn(-mu[:,0],C,N/4).T, mvn(mu[:,1],C,N/4).T,mvn(-mu[:,1],C,N/4).T))
Y = sp.hstack((sp.ones((1,N/2.)),-sp.ones((1,N/2.))))
randidx = sp.random.permutation(N)
Y = Y[0,randidx]
X = X[:,randidx]
return X,Y
def main():
# create centers
means = [[0, 0], [2.5, 2.5]]
data = []
for class_num, mean in enumerate(means):
class_centers = mvn(mean, np.eye(2), 10)
class_data = [np.hstack((mvn(class_center, np.eye(2), 10), np.zeros((10, 1)) + class_num))
for class_center in class_centers]
data.extend(class_data)
data = np.array(data).reshape(200, 3)
bias_variance(data, 2)
def main():
# create centers
means = [[0, 0], [2.5, 2.5]]
data = []
for class_num, mean in enumerate(means):
class_centers = mvn(mean, np.eye(2), 10)
class_data = [
np.hstack((mvn(class_center, np.eye(2), 10), np.zeros(
(10, 1)) + class_num)) for class_center in class_centers
]
data.extend(class_data)
data = np.array(data).reshape(200, 3)
bias_variance(data, 2)
def _get_data_rng(self, data_cov_matrix):
from numpy.random import multivariate_normal as mvn
mu = np.zeros(self.p)
cov = np.array(data_cov_matrix)
assert len(mu) == cov.shape[0] == cov.shape[1]
rng = lambda x: mvn(mu, cov, size=x)
return rng
def bivrandom (x0, y0, sx, sy, cxy, size=None):
"""Compute random values distributed according to the specified bivariate
distribution.
Inputs:
* x0: the center of the x distribution (i.e. its intended mean)
* y0: the center of the y distribution
* sx: standard deviation (not variance) of x var
* sy: standard deviation (not variance) of y var
* cxy: covariance (not correlation coefficient) of x and y
* size (optional): the number of values to compute
Returns: array of shape (size, 2); or just (2, ), if size was not
specified.
The bivariate parameters of the generated data are approximately
recoverable by calling 'databiv(retval)'.
"""
from numpy.random import multivariate_normal as mvn
p0 = np.asarray ([x0, y0])
cov = np.asarray ([[sx**2, cxy],
[cxy, sy**2]])
return mvn (p0, cov, size)
def __call__(self, i, y, u, deterministic=True):
if i > 0:
self.filter(y, u)
mu = self.mu
covar = self.covar
self.mus.append(mu)
self.covars.append(covar)
self.optimize(self.n_iter, mu, covar)
self.xu_history.append(self.i2c.get_marginal_state_action())
self.z_history.append(self.i2c.get_marginal_observed_trajectory()[0])
mu = np.copy(self.i2c.cells[0].mu_u0_m)
sig = np.copy(self.i2c.cells[0].sig_u0_m)
if deterministic:
ctrl = mu.reshape((self.dim_u, 1))
else:
ctrl = mvn(mu.squeeze(), sig, 1).reshape((self.dim_u, 1))
self.i2c.cells.pop(0)
self.i2c.cells.append(deepcopy(self.cell_init))
self.i2c.cells[-1].sys = self.i2c.sys
if self.z_traj is not None:
if (i + self.i2c.H) < self.z_traj.shape[0]:
self.i2c.cells[-1].z = self.z_traj[i + self.i2c.H, :, None]
else:
self.i2c.cells[-1].z = self.i2c.cells[-2].z
return ctrl
def draw(self, n=1):
'''
draw a random sample of size n form this cluster
'''
# cast n to a int incase it's a numpy.int
n = int(n)
return mvn(self.mu, self.sigma, n)
def sample_m(mean, cov, shift, amount):
"""
Create samples from input density.
:param mean: Mean with which to sample
:param cov: Covariance with which to sample
:param shift: Boolean to determine whether to shift the orientation
:param amount: Number of samples to be drawn
:return: The sample for amount==1 and an array of samples for amount > 1
"""
# shift mean by 90 degree and switch length and width if demanded
s_mean = mean.copy()
if shift:
save_w = s_mean[W]
s_mean[W] = s_mean[L]
s_mean[L] = save_w
s_mean[AL] += 0.5 * np.pi
# draw sample
samp = mvn(s_mean, cov, amount)
# enforce restrictions
samp[:, L] = np.maximum(0.1, samp[:, L])
samp[:, W] = np.maximum(0.1, samp[:, W])
samp[:, AL] %= 2 * np.pi
# if only one sample, do not store it in a 1d array
if amount == 1:
samp = samp[0]
return samp
def draw(self, n=1):
"""
draw a random sample of size n form this cluster
"""
# cast n to a int incase it's a numpy.int
n = int(n)
return mvn(self.mu, self.sigma, n)
def batch_forward(self, xu):
x = self.dynamics(xu)
if self.deterministic:
return x
else:
noise = mvn(np.zeros((self.dim_x)), self.sig_eta, x.shape[0])
return x + noise
def backward_sampling(ms, Cs, W):
T = len(ms) - 1
theta = mvn(mean=np.atleast_1d(ms[T]), cov=np.atleast_2d(Cs[T]))
thetas = [theta]
for t in reversed(range(1, T)):
theta = FFBS.backward_sampling_step(theta, ms[t], Cs[t], W)
thetas.append(theta)
return list(reversed(thetas))
def forward(self, u):
assert u.shape[0] == 1
xu = np.hstack((self.x, u))
self.x = self.dynamics(xu)
if not self.deterministic:
disturbance = mvn(np.zeros((self.dim_x, )), self.sig_eta, 1)
self.x += disturbance
return self.x
def simulate_measurement(self, meas, geom):
"""Simulate a measurement by the given sensor, for a true radiance
sampled to instrument wavelengths. This basically just means
drawing a sample from the noise distribution."""
Sy = self.Sy(meas, geom)
mu = s.zeros(meas.shape)
rdn_sim = meas + mvn(mu, Sy)
return rdn_sim
def SampleCanonicalPosterior(mu_posterior, C_posterior, NumPosteriorSamples,
h_c, undotransform = True):
"""
:param mu_posterior:
:param C_posterior:
:param NumPosteriorSamples:
:param h_c:
:return: h_c_post
"""
from numpy.random import multivariate_normal as mvn
from statsmodels.distributions.empirical_distribution import ECDF
from scipy.stats import norm
PosteriorSamples = mvn(mu_posterior, C_posterior, NumPosteriorSamples)
if undotransform:
PosteriorSamplesTransformed = np.zeros_like(PosteriorSamples)
# back transform Normal Score Transformation:
for ii in range(np.shape(h_c)[1]):
OriginalScores = h_c[:, ii]
TransformedSamples = np.copy(PosteriorSamples[:,ii])
BackTransformedValue = np.zeros_like(TransformedSamples)
edf = ECDF(OriginalScores)
F,x = edf.y[1:], edf.x[1:]
FStar = norm.cdf(TransformedSamples, np.mean(OriginalScores),
scale=np.std(OriginalScores))
# for each FStar, find closest F
for jj in range(len(FStar)):
index = np.argmin(np.abs(F - FStar[jj]))
if index==1:
BackTransformedValue[jj] = x[index]
elif index==len(x):
BackTransformedValue[jj] = x[-1]
elif F[index] < FStar[jj]:
BackTransformedValue[jj] = 0.5 * (x[index] + x[index - 1])
elif index+1 == len(F):
BackTransformedValue[jj] = x[index]
else:
BackTransformedValue[jj] = 0.5 * (x[index] + x[index + 1])
if BackTransformedValue[jj]==float('inf') or BackTransformedValue[jj]==-float('inf'):
raise Exception('Bad interpolation')
PosteriorSamplesTransformed[:, ii] = np.copy(BackTransformedValue)
h_c_post = np.copy(PosteriorSamplesTransformed)
else:
h_c_post = np.copy(PosteriorSamples)
return h_c_post
def __init__(self, sizes, bandwidth):
self.sizes = sizes
self.input_size = self.sizes[0]
self.hidden_size = self.sizes[1]
self.freq = mvn(mean=np.zeros((self.input_size, )),
cov=np.diag(1.0 / bandwidth),
size=self.hidden_size)
self.shift = npr.uniform(-np.pi, np.pi, size=self.hidden_size)
def redistribute_attribute(graph, gt_community, std, random_state=None):
"""
Redistribute the attribute of the input graph.
:param graph: Networkx Graph: the initial graph read from DANCer.
:param gt_community: Dictionary: the ground truth community.
:param std: Float: the standard deviation of attributes for each community.
:param random_state: Integer: an integer value specifying the random state for mvn.
:return: new_graph: Networkx Graph: a new object of graph with attributes.
"""
num_community = len(set(gt_community.values())) # number of communities
if random_state:
seed(random_state)
centers = mvn([0, 0], 5 * np.identity(2), num_community)
for node in graph.nodes():
community_id = gt_community[node]
attr = mvn(centers[community_id], std * np.identity(2))
graph.node[node]['attr'] = attr
new_graph = graph.copy()
return new_graph
def _load_mu_at_fit(self):
(n, d) = self.prior_mu.shape
if d != self.d:
raise ValueError('Dimension mismatch between Mus and Data')
elif n < self.nclusts:
self._prior_mu = zeros((self.nclusts, self.d))
self._prior_mu[0:n, :] = (self.prior_mu.copy() - self.m) / self.s
self._prior_mu[n:, :] = mvn(zeros((self.d,)), eye(self.d), self.nclusts - n)
else:
self._prior_mu = (self.prior_mu.copy() - self.m) / self.s
def single_particle_approx_gaussian(m_a, l_a, w_a, al_a, cov_a, n_particles,
use_pos):
l_pa = np.zeros(n_particles)
w_pa = np.zeros(n_particles)
al_pa = np.zeros(n_particles)
vec_A = np.zeros((n_particles, 5))
ma_final = 0
Pa_final = np.zeros((2, 2))
for i in range(n_particles):
# if i % 100 == 0:
# print(i)
if use_pos:
m_pa = mvn(m_a, cov_a[:2, :2])
else:
m_pa = 1.0 * m_a
l_pa[i] = np.maximum(normal(l_a, cov_a[3, 3]), 0.1)
w_pa[i] = np.maximum(normal(w_a, cov_a[4, 4]), 0.1)
al_pa[i] = normal(al_a, cov_a[2, 2])
al_pa[i] %= (2 * np.pi)
rot_p = np.array([[np.cos(al_pa[i]), -np.sin(al_pa[i])],
[np.sin(al_pa[i]),
np.cos(al_pa[i])]])
Pa = np.dot(np.dot(rot_p, np.diag([l_pa[i], w_pa[i]])**2), rot_p.T)
Pa_sr = sqrtm(Pa)
ma_final += m_pa
Pa_final += Pa_sr
vec_A[i] = np.array(
[m_pa[0], m_pa[1], Pa_sr[0, 0], Pa_sr[0, 1], Pa_sr[1, 1]])
ma_final /= n_particles
Pa_final /= n_particles
if use_pos:
mean_A = np.sum(vec_A, axis=0) / n_particles
var_A = np.sum(np.einsum('xa, xb -> xab', vec_A - mean_A,
vec_A - mean_A),
axis=0) / n_particles
else:
mean_A = np.sum(vec_A[:, 2:], axis=0) / n_particles
var_A = np.sum(np.einsum('xa, xb -> xab', vec_A[:, 2:] - mean_A,
vec_A[:, 2:] - mean_A),
axis=0) / n_particles
var_A += var_A.T
var_A *= 0.5
return ma_final, mean_A, var_A
def _load_mu_at_fit(self):
(n, d) = self.prior_mu.shape
if d != self.d:
raise ValueError('Dimension mismatch between Mus and Data')
elif n < self.nclusts:
self._prior_mu = zeros((self.nclusts, self.d))
self._prior_mu[0:n, :] = (self.prior_mu.copy() - self.m) / self.s
self._prior_mu[n:, :] = mvn(zeros((self.d, )), eye(self.d),
self.nclusts - n)
else:
self._prior_mu = (self.prior_mu.copy() - self.m) / self.s
def prediction_sample(self, size=1):
"""
Sample function values from the GP prediction.
:param int size: sample size to draw
:return np.array: a (s, n) array, with s the sample size and n the
length of the test input array.
"""
if np.array_equal(self.predcov, self.covariance):
raise RuntimeWarning('Posterior covariance is identical to prior '
'covariance. Try using the prediction method '
'first.')
return mvn(mean=self.predmean, cov=self.predcov, size=size)
def __call__(self, i, x, deterministic=True):
assert i < self.H
if i % self.control_step == 0:
kx = self.K[i, :, :] @ x
mu = kx + self.k[i, :, None]
sig = self.sig_k[i, :, :]
if deterministic:
self.u = mu
else:
self.u = mvn(mu[:, 0], sig, 1)
return self.u
def prediction_sample(self, size=1):
"""
Sample function values from the GP prediction.
:param int size: sample size to draw
:return np.array: a (s, n) array, with s the sample size and n the
length of the test input array.
"""
if np.array_equal(self.predcov, self.covariance):
raise RuntimeWarning('Posterior covariance is identical to prior '
'covariance. Try using the prediction method '
'first.')
return mvn(mean=self.predmean, cov=self.predcov, size=size)
def rollout(dt, A, tf, x0, Q=None):
N = A.shape[0]
if Q is None:
Q = 0.001 * dt * np.identity(N)
t = np.linspace(start=0, stop=tf, num=math.ceil(tf/dt)+1)
xs = [x0]
Qs = [Q]
for ti in t[:-1]:
noise = mvn(np.zeros(N), Q)
xi = np.dot(A, xs[-1]) + noise
xs.append(xi)
Qs.append(Q)
return {'x' : np.array(xs), 't' : np.array(t), 'Q' : np.array(Qs)}
def gaussian_mutate(a, cov):
'''Perform Gaussian mutation on a vector gene.
'''
N = len(a)
if np.iterable(cov):
cov = np.asarray(cov)
elif type(cov) is float or type(cov) is int:
cov = cov * np.identity(N)
else:
raise ValueError('cov must be numeric')
if len(cov.shape) < 2:
cov = np.diag(cov)
return a + mvn(mean=np.zeros(N), cov=cov)
def __call__(self, i, x, deterministic=True):
assert i < self.H
dist = x - self.mu[i, :, None]
exp = 0.5 * dist.T @ self.lam[i, :, :] @ dist
if self.soft:
p = np.exp(-exp).item()
else: # hard
p = float(abs(exp) < self.hard_exp_threshold)
kx = p * self.K[i, :, :] @ dist
mu = self.k[i, :, None] + kx
if deterministic:
return mu.reshape((self.dim_u, 1))
else:
sig = self.sig_k[i, :, :].reshape((self.dim_u, self.dim_u))
return mu + mvn(np.zeros((self.dim_u,)), sig, 1).reshape((self.dim_u, 1))
def find_pick_region_labeler(self, results, c_img, d_img, count):
"""Called during bed-making data collection, only if we use the labeler.
It relies on the raw depth image! DO NOT PASS A PROCESSED DEPTH IMAGE!!!
Also, shows the image to the user via `plot_on_true`.
NOTE: main difference between this and the other method for the net is
that the `results` argument encodes the pose implicitly and we have to
compute it. Otherwise, though, the depth computation is the same, and
cropping is the same, so the methods do similar stuff.
Args:
results: Dict from QueryLabeler class (human supervisor).
"""
poses = []
p_list = []
for result in results['objects']:
print result
x_min = float(result['box'][0])
y_min = float(result['box'][1])
x_max = float(result['box'][2])
y_max = float(result['box'][3])
x = (x_max - x_min) / 2.0 + x_min
y = (y_max - y_min) / 2.0 + y_min
# Will need to test; I assume this requires human intervention.
if cfg.USE_DART:
pose = np.array([x, y])
action_rand = mvn(pose, cfg.DART_MAT)
print "ACTION RAND ", action_rand
print "POSE ", pose
x = action_rand[0]
y = action_rand[1]
self.plot_on_true([x, y], c_img)
#Crop D+img
d_img_c = d_img[int(y - cfg.BOX):int(y + cfg.BOX),
int(x - cfg.BOX):int(cfg.BOX + x)]
depth = self.gp.find_mean_depth(d_img_c)
# Note that `result['class']` is an integer (a class index).
# 0=success, anything else indicates a grasping failure.
poses.append([result['class'], [x, y, depth]])
self.broadcast_poses(poses, count)
def sample_mult(x, cov, w, n_samples):
"""
Sample from a multimodal Gaussian density.
:param x: the components' means
:param cov: the components' covariances
:param w: the components' weights
:param n_samples: the number of samples to be drawn
:return: the samples
"""
# sample the components with repetition
chosen = np.random.choice(len(x), n_samples, True, p=w)
# sample from the respective components
samples = np.zeros((n_samples, len(x[0])))
for i in range(len(x)):
if np.sum(chosen == i) > 0:
samples[chosen == i] = mvn(x[i], cov[i], np.sum(chosen == i))
return samples
def simulate_data(init, meas_cov):
gt = init.copy()
for i in range(TIME_STEPS):
# move along predetermined path
if i > 0:
vel = VELS[i - 1] * np.array(
[np.cos(ORS[i - 1]), np.sin(ORS[i - 1])])
gt[:2] = gt[:2] + vel * TD
gt[2:4] = vel
gt[4] = ORS[i - 1]
gt[5:7] = gt[5:7]
# generate measurements
n_m = np.max([np.random.poisson(GT_POIS, 1)[0],
3]) # at least three measurement
meas_sources = get_meas_sources(gt[[0, 1, 4, 5, 6]], n_m)
meas = np.asarray([mvn(meas_sources[j], meas_cov) for j in range(n_m)])
yield gt, meas
def find_pick_region_labeler(self, results, c_img, d_img, count):
'''
Evaluates the current policy and then executes the motion
specified in the the common class
'''
poses = []
#IPython.embed()
p_list = []
for result in results['objects']:
print result
x_min = float(result['box'][0])
y_min = float(result['box'][1])
x_max = float(result['box'][2])
y_max = float(result['box'][3])
x = (x_max - x_min) / 2.0 + x_min
y = (y_max - y_min) / 2.0 + y_min
if cfg.USE_DART:
pose = np.array([x, y])
action_rand = mvn(pose, cfg.DART_MAT)
print "ACTION RAND ", action_rand
print "POSE ", pose
x = action_rand[0]
y = action_rand[1]
self.plot_on_true([x, y], c_img)
#Crop D+img
d_img_c = d_img[y - cfg.BOX:y + cfg.BOX, x - cfg.BOX:cfg.BOX + x]
depth = self.gp.find_mean_depth(d_img_c)
poses.append([result['class'], [x, y, depth]])
self.broadcast_poses(poses, count)
def wishart(sig, n):
"""
Sample from a Wishart distribution (naive implementation).
Parameters
----------
sig : (d,d) ndarray
The scale matrix for the Wishart distribution, must be pos. definite.
n : int
Must be >= d.
Returns
-------
w : (d,d) ndarray
The sample from the distribution.
"""
d = sig.shape[0]
s = mvn(zeros(d), sig, n)
w = empty((d,d))
for i in xrange(n):
w += outer(s[i], s[i])
return w
def bivrandom(x0, y0, sx, sy, cxy, size=None):
"""Compute random values distributed according to the specified bivariate
distribution.
Inputs:
* x0: the center of the x distribution (i.e. its intended mean)
* y0: the center of the y distribution
* sx: standard deviation (not variance) of x var
* sy: standard deviation (not variance) of y var
* cxy: covariance (not correlation coefficient) of x and y
* size (optional): the number of values to compute
Returns: array of shape (size, 2); or just (2, ), if size
was not specified.
The bivariate parameters of the generated data are approximately recoverable
by calling 'databiv(retval)'.
"""
from numpy.random import multivariate_normal as mvn
p0 = np.asarray([x0, y0])
cov = np.asarray([[sx**2, cxy], [cxy, sy**2]])
return mvn(p0, cov, size)
def __call__(self, i, x, deterministic=True):
self.optimize(self.n_iter, x)
self.i2c.compute_update_alpha(True,
calc_evar=False,
calc_propagate=False)
self.xu_history.append(self.i2c.get_marginal_state_action())
self.z_history.append(self.i2c.get_marginal_observed_trajectory()[0])
mu = np.copy(self.i2c.cells[0].mu_u0_m)
sig = np.copy(self.i2c.cells[0].sig_u0_m)
if deterministic:
u = mu.reshape((self.dim_u, 1))
else:
u = mvn(mu.squeeze(), sig, 1).reshape((self.dim_u, 1))
self.i2c.cells.pop(0)
self.i2c.cells.append(deepcopy(self.cell_init))
self.i2c.cells[-1].sys = self.i2c.sys
if self.z_traj is not None:
if (i + self.i2c.H) < self.z_traj.shape[0]:
self.i2c.cells[-1].z = self.z_traj[i + self.i2c.H, :, None]
else:
self.i2c.cells[-1].z = self.i2c.cells[-2].z
return u
ax.set_title('Prediction using posterior mean and covariance', fontsize=12)
ax.set_ylabel('function value', fontsize=12)
ax.set_xlabel('spatial coordinate', fontsize=12)
ax.grid()
ax.legend(loc=2, fontsize=12)
plt.tight_layout()
plt.savefig('regression_estimate.png')
plt.close()
# As the estimate itself is defined by a multivariate normal distribution,
# we can draw samples from that distribution.
# to do this, we need to build the full covariance matrix and mean for the
# desired set of points using the 'build_posterior' method:
mu, sigma = GP.build_posterior(q)
# now draw samples
samples = mvn(mu, sigma, 100)
# and plot all the samples
fig = plt.figure(figsize=(9, 6))
ax = fig.add_subplot(111)
for i in range(100):
ax.plot(q, samples[i, :], lw=0.5)
ax.set_title('100 samples drawn from the posterior distribution', fontsize=12)
ax.set_ylabel('function value', fontsize=12)
ax.set_xlabel('spatial coordinate', fontsize=12)
ax.set_xlim([-4, 10])
plt.grid()
plt.tight_layout()
plt.savefig('posterior_samples.png')
plt.close()
# The gradient of the Gaussian process estimate also has a multivariate normal distribution.
s.add_robot(car)
# Number of timesteps
T = 20 #arg
# Dynamics and measurement noise
num_states = car.NX
num_ctrls = car.NU
num_measure = len(beacons) #arg/make part of robot observe
Q = np.mat(np.diag([0.0005] * num_states)) #arg
Q[2, 2] = 0 # Gets out of hand if noise in theta or phi
Q[3, 3] = 0
R = np.mat(np.diag([0.00005] * num_measure)) * 0.001 #arg
# Sample noise
dynamics_noise = mvn([0] * num_states, Q, T - 1).T
measurement_noise = mvn([0] * num_measure, R, T - 1).T
# Setup for EKF
mus = np.mat(np.zeros((num_states, T)))
mus[:, 0] = np.mat(x0).T
Sigmas = np.zeros((Q.shape[0], Q.shape[1], T))
Sigmas[:, :, 0] = np.mat(np.diag([0.001] * num_states)) #arg
# Generate trajectory and obtain observations along trajectory
X_bar = np.mat(np.zeros((car.NX, T))) #arg
X_bar[:, 0] = np.mat(x0).T
U_bar = np.ones((car.NU, T - 1)) * 1.7 #arg
for t in xrange(1, T):
U_bar[1, t - 1] = 0
parser = argparse.ArgumentParser(description="Measure statistics across multiple chains.")
parser.add_argument("--opt_jump", default="opt_jump.npy", help="The numpy save file holding the estimated covariance.")
parser.add_argument("--N", default=10000, help="Number of samples to draw from the covariance matrix.")
args = parser.parse_args()
N = args.N
# Use the opt_jump.npy, sample a bunch, and then plot it later.
import numpy as np
from numpy.random import multivariate_normal as mvn
Sigma = np.load(args.opt_jump)
ndim = Sigma.shape[0]
samples = mvn(np.zeros(ndim), Sigma, N)
np.save("samples.npy", samples)
import triangle
# Fixed distance
labels = [r"$M_\ast\quad [M_\odot]$", r"$r_c$ [AU]", r"$T_{10}$ [K]",
r"$q$", r"$\log M_\textrm{gas} \quad \log [M_\odot]$", r"$\xi$ [km/s]",
# r"$d$ [pc]",
r"$i_d \quad [{}^\circ]$", r"PA $[{}^\circ]$", r"$v_r$ [km/s]",
r"$\mu_\alpha$ ['']", r"$\mu_\delta$ ['']"]
# Vertical temperature gradient
# labels = [r"$M_\ast\quad [M_\odot]$", r"$r_c$ [AU]", r"$T_{10,m}$ [K]",
def segment(image, n_segments=2, burn_in=1000, samples=1000, lag=5):
"""
Return image segment samples.
Parameters
----------
image : (N,M,3) ndarray
Pixel array with 3-dimensional values (e.g. RGB)
Returns
-------
labels : (samples,N,M) ndarray
The image segment label array
emission_mus: (samples,n_segments,3) ndarray
The Gaussian emission distribution (mean).
emission_precs : (samples,n_segments,3,3) ndarray
The Gaussian emission distribution (precision).
log_prob_p : (samples,) ndarray
Log probability portion contributed by prior
log_prob_e : (samples,) ndarray
Log probability portion contributed by emissions
"""
RETURN_MINUS_ONE = False
N,M = image.shape[:2]
# subtract out the mean
image = normalize(image)
# allocate arrays
res_labels = zeros((samples+1,N,M))
res_emission_mu = zeros((samples,n_segments,3))
res_emission_prec = zeros((samples,n_segments,3,3))
res_log_prob_p = zeros((samples,))
res_log_prob_e = zeros((samples,))
# initialize hyperparmeters
nu = 4 # df hyperparameter for wishart (uninformative?)
V = eye(3) # scale matrix hyperparameter for wishart (uninformative?)
Vinv = inv3(V)
mu = zeros(3) # mean hyperparameter for MVN
S = eye(3)*0.0001 # precision hyperparameter for MVN (uninformative?)
# initialize labels/params
padded_labels = ones((N+2,M+2), dtype=int)*-1
padded_labels[1:-1,1:-1] = randint(n_segments,size=(N,M))
labels = padded_labels[1:-1, 1:-1]
for n in xrange(N):
for m in xrange(M):
labels[n,m] = (n*M + m)%n_segments
res_labels[-1] = labels
emission_mu = mvn(mu,S,size=n_segments) # draw from prior
#emission_prec = array([wishart(V,nu) for i in xrange(n_segments)]) # draw from prior
emission_prec = array([eye(3)*0.0001 for i in xrange(n_segments)])
log_prob_p = None
log_prob_e = None
conditional = zeros((n_segments,))
try:
# gibbs
for i in xrange(burn_in + samples*lag - (lag - 1)):
for n in xrange(N):
for m in xrange(M):
# resample label
for k in xrange(n_segments):
labels[n,m] = k
conditional[k] = 0.
conditional[k] += phi_blanket(
memoryview(padded_labels), n, m,
memoryview(FS))
conditional[k] += logNormPDF(image[n,m,:],
emission_mu[k], emission_prec[k])
labels[n,m] = sample_categorical(conditional)
for k in xrange(n_segments):
mask = (labels == k)
n_k = sum(mask)
# resample label mean
if n_k:
P = inv3(S+n_k*emission_prec[k])
xbar = mean(image[mask],axis=0)
emission_mu[k,:] = mvn(dot(P,n_k*dot(emission_prec[k],xbar)),P)
else:
emission_mu = mvn(mu,S,size=n_segments) # draw from prior
# resample label precision
if n_k:
D = outer(image[mask][0,:]-emission_mu[k,:],image[mask][0,:]-emission_mu[k,:])
for ii in xrange(1,n_k):
D += outer(image[mask][ii,:]-emission_mu[k,:],image[mask][ii,:]-emission_mu[k,:])
emission_prec[k,:,:] = wishart(inv3(Vinv+D),nu+n_k)
else:
emission_prec[k,:,:] = wishart(V,nu) # resample using the prior
log_prob = 0.
for c in xrange(n_segments):
log_prob_e += logNormPDF(emission_mu[c],mu,S)
log_prob_e += logWisPDF(emission_prec[c],nu,Vinv)
for n in xrange(N):
for m in xrange(M):
label = labels[n,m]
log_prob_e += logNormPDF(image[n,m,:],emission_mu[label],emission_prec[label])
log_prob_p = phi_all(memoryview(padded_labels), memoryview(FS))
sys.stdout.write('\riter {} log_prob_prior {} '
'log_prob_emission {} k0 {} k1 {} '.format(i,
log_prob_p, log_prob_e, sum(labels==0), sum(labels==1)))
sys.stdout.flush()
if not sum(labels==0) or not sum(labels==1):
RETURN_MINUS_ONE = True
raise ValueError("converged to 0...")
if i < burn_in:
pass
elif not (i - burn_in)%lag:
res_i = i/lag
res_emission_mu[res_i] = emission_mu[:,:]
res_emission_prec[res_i] = emission_prec[:,:,:]
res_labels[res_i] = labels
res_log_prob_p[i] = log_prob_p
res_log_prob_e[i] = log_prob_e
sys.stdout.write('\n')
except Exception as e:
print e
finally:
if RETURN_MINUS_ONE:
return -1
else:
return (res_labels, res_emission_mu, res_emission_prec,
res_log_prob_p, res_log_prob_e)
def test_convergence(steps, runs, prior, cov_prior, cov_meas, random_param,
save_path):
"""
Test convergence of error for different fusion methods. Creates plot of root mean square error convergence and
errors at first and last measurement step.
:param steps: Number of measurements
:param runs: Number of MC runs
:param prior: Prior prediction mean (ground truth will be drawn from it each run)
:param cov_prior: Prior prediction covariance (ground truth will be drawn from it each run)
:param cov_meas: Noise of sensor
:param random_param: use random parameter switch to simulate ambiguous parameterization
:param save_path: Path for saving figures
"""
error = np.zeros(steps * 2)
# setup state for various ellipse fusion methods
mmgw_mc = np.zeros(1, dtype=state_dtype)
mmgw_mc[0]['error'] = error.copy()
mmgw_mc[0]['name'] = 'MC-MMGW'
mmgw_mc[0]['color'] = 'cyan'
regular = np.zeros(1, dtype=state_dtype)
regular[0]['error'] = error.copy()
regular[0]['name'] = 'Regular'
regular[0]['color'] = 'red'
regular_mmgw = np.zeros(1, dtype=state_dtype)
regular_mmgw[0]['error'] = error.copy()
regular_mmgw[0]['name'] = 'Regular-MMGW'
regular_mmgw[0]['color'] = 'orange'
red_mmgw = np.zeros(1, dtype=state_dtype)
red_mmgw[0]['error'] = error.copy()
red_mmgw[0]['name'] = 'RED-MMGW'
red_mmgw[0]['color'] = 'green'
# red_mmgw_r = np.zeros(1, dtype=state_dtype)
# red_mmgw_r[0]['error'] = error.copy()
# red_mmgw_r[0]['name'] = 'RED-MMGW-r'
# red_mmgw_r[0]['color'] = 'lightgreen'
# red_mmgw_s = np.zeros(1, dtype=state_dtype)
# red_mmgw_s[0]['error'] = error.copy()
# red_mmgw_s[0]['name'] = 'RED-MMGW-s'
# red_mmgw_s[0]['color'] = 'turquoise'
shape_mean = np.zeros(1, dtype=state_dtype)
shape_mean[0]['error'] = error.copy()
shape_mean[0]['name'] = 'Shape-Mean'
shape_mean[0]['color'] = 'magenta'
# rt_red = 0.0
# rt_red_r = 0.0
# rt_red_s = 0.0
for r in range(runs):
print('Run %i of %i' % (r + 1, runs))
# initialize ===================================================================================================
# create gt from prior
gt = sample_m(prior, cov_prior, False, 1)
# ellipse orientation should be velocity orientation
vel = np.linalg.norm(gt[V])
gt[V] = np.array(np.cos(gt[AL]), np.sin(gt[AL])) * vel
# get prior in square root space
mmgw_mc[0]['x'], mmgw_mc[0][
'cov'], particles_mc = single_particle_approx_gaussian(
prior, cov_prior, N_PARTICLES_MMGW)
mmgw_mc[0]['est'] = mmgw_mc[0]['x'].copy()
mmgw_mc[0]['est'][SR] = get_ellipse_params_from_sr(mmgw_mc[0]['x'][SR])
mmgw_mc[0]['figure'], mmgw_mc[0]['axes'] = plt.subplots(1, 1)
# get prior for regular state
regular[0]['x'] = prior.copy()
regular[0]['cov'] = cov_prior.copy()
regular[0]['est'] = prior.copy()
regular[0]['figure'], regular[0]['axes'] = plt.subplots(1, 1)
regular_mmgw[0]['x'] = prior.copy()
regular_mmgw[0]['cov'] = cov_prior.copy()
particles = sample_m(regular_mmgw[0]['x'], regular_mmgw[0]['cov'],
False, N_PARTICLES_MMGW)
regular_mmgw[0]['est'] = mmgw_estimate_from_particles(particles)
regular_mmgw[0]['figure'], regular_mmgw[0]['axes'] = plt.subplots(1, 1)
# get prior for red
red_mmgw[0]['x'], red_mmgw[0]['cov'], red_mmgw[0][
'comp_weights'] = turn_mult(prior.copy(), cov_prior.copy())
particles = sample_mult(red_mmgw[0]['x'], red_mmgw[0]['cov'],
red_mmgw[0]['comp_weights'], N_PARTICLES_MMGW)
red_mmgw[0]['est'] = mmgw_estimate_from_particles(particles)
red_mmgw[0]['figure'], red_mmgw[0]['axes'] = plt.subplots(1, 1)
# red_mmgw_r[0]['x'], red_mmgw_r[0]['cov'], red_mmgw_r[0]['comp_weights'] = turn_mult(prior.copy(), cov_prior.copy())
# particles = sample_mult(red_mmgw_r[0]['x'], red_mmgw_r[0]['cov'], red_mmgw_r[0]['comp_weights'], N_PARTICLES_MMGW)
# red_mmgw_r[0]['est'] = mmgw_estimate_from_particles(particles)
# red_mmgw_r[0]['figure'], red_mmgw_r[0]['axes'] = plt.subplots(1, 1)
#
# red_mmgw_s[0]['x'], red_mmgw_s[0]['cov'], red_mmgw_s[0]['comp_weights'] = turn_mult(prior.copy(),
# cov_prior.copy())
# particles = sample_mult(red_mmgw_s[0]['x'], red_mmgw_s[0]['cov'], red_mmgw_s[0]['comp_weights'],
# N_PARTICLES_MMGW)
# red_mmgw_s[0]['est'] = mmgw_estimate_from_particles(particles)
# red_mmgw_s[0]['figure'], red_mmgw_s[0]['axes'] = plt.subplots(1, 1)
# get prior for RM mean
shape_mean[0]['x'] = prior[KIN]
shape_mean[0]['shape'] = to_matrix(prior[AL], prior[L], prior[W],
False)
shape_mean[0]['cov'] = cov_prior[KIN][:, KIN]
shape_mean[0]['gamma'] = 6.0
shape_mean[0]['figure'], shape_mean[0]['axes'] = plt.subplots(1, 1)
# test different methods
for i in range(steps):
if i % 10 == 0:
print('Step %i of %i' % (i + 1, steps))
plot_cond = (r + 1 == runs) & ((i % 2) == 1) # & (i + 1 == steps)
# move ground truth
gt = np.dot(F, gt)
error_mat = np.array([
[0.5 * T**2, 0.0],
[0.0, 0.5 * T**2],
[T, 0.0],
[0.0, T],
])
kin_cov = np.dot(
np.dot(error_mat,
np.diag([SIGMA_V1, SIGMA_V2])**2), error_mat.T)
gt[KIN] += mvn(np.zeros(len(KIN)), kin_cov)
gt[AL] = np.arctan2(gt[V2], gt[V1])
# create measurement from gt (using alternating sensors) ===================================================
k = np.random.randint(0, 4) if random_param else 0
gt_mean = gt.copy()
if k % 2 == 1:
l_save = gt_mean[L]
gt_mean[L] = gt_mean[W]
gt_mean[W] = l_save
gt_mean[AL] = (gt_mean[AL] + 0.5 * np.pi * k +
np.pi) % (2 * np.pi) - np.pi
meas = sample_m(np.dot(H, gt_mean), cov_meas, False, 1)
# fusion methods ===========================================================================================
mmgw_mc_update(mmgw_mc[0], meas.copy(), cov_meas.copy(),
N_PARTICLES_MMGW, gt, i, steps, plot_cond,
save_path)
regular_update(regular[0], meas.copy(), cov_meas.copy(), gt, i,
steps, plot_cond, save_path, False)
regular_update(regular_mmgw[0], meas.copy(), cov_meas.copy(), gt,
i, steps, plot_cond, save_path, True)
# tic = time.time()
red_update(red_mmgw[0],
meas.copy(),
cov_meas.copy(),
gt,
i,
steps,
plot_cond,
save_path,
True,
mixture_reduction='salmond')
# toc = time.time()
# rt_red += toc - tic
# tic = time.time()
# red_update(red_mmgw_r[0], meas.copy(), cov_meas.copy(), gt, i, steps, plot_cond, save_path, True,
# mixture_reduction='salmond', pruning=False)
# toc = time.time()
# rt_red_r += toc - tic
# tic = time.time()
# red_update(red_mmgw_s[0], meas.copy(), cov_meas.copy(), gt, i, steps, plot_cond, save_path, True,
# mixture_reduction='salmond')
# toc = time.time()
# rt_red_s += toc - tic
shape_mean_update(
shape_mean[0], meas.copy(), cov_meas.copy(), gt, i, steps,
plot_cond, save_path, 0.2 if cov_meas[AL, AL] < 0.1 * np.pi
else 5.0 if cov_meas[AL, AL] < 0.4 * np.pi else 10.0)
plt.close(mmgw_mc[0]['figure'])
plt.close(regular[0]['figure'])
plt.close(regular_mmgw[0]['figure'])
plt.close(red_mmgw[0]['figure'])
# plt.close(red_mmgw_r[0]['figure'])
# plt.close(red_mmgw_s[0]['figure'])
plt.close(shape_mean[0]['figure'])
mmgw_mc[0]['error'] = np.sqrt(mmgw_mc[0]['error'] / runs)
regular[0]['error'] = np.sqrt(regular[0]['error'] / runs)
regular_mmgw[0]['error'] = np.sqrt(regular_mmgw[0]['error'] / runs)
red_mmgw[0]['error'] = np.sqrt(red_mmgw[0]['error'] / runs)
# red_mmgw_r[0]['error'] = np.sqrt(red_mmgw_r[0]['error'] / runs)
# red_mmgw_s[0]['error'] = np.sqrt(red_mmgw_s[0]['error'] / runs)
shape_mean[0]['error'] = np.sqrt(shape_mean[0]['error'] / runs)
# print('Runtime RED:')
# print(rt_red / (runs*steps))
# print('Runtime RED no pruning:')
# print(rt_red_r / (runs * steps))
# print('Runtime RED-S:')
# print(rt_red_s / (runs * steps))
# error plotting ===================================================================================================
plot_error_bars(
np.block([regular, regular_mmgw, shape_mean, mmgw_mc, red_mmgw]),
steps)
plot_convergence(
np.block([regular, regular_mmgw, shape_mean, mmgw_mc, red_mmgw]),
steps, save_path)
def create_data(means, var=np.eye(2), data=100):
data_list = [
np.hstack((mvn(mean, var, data), np.zeros((data, 1)) + class_num)) for class_num, mean in enumerate(means)
]
return data_list
# Number of timesteps T = 30 #arg # Dynamics and measurement noise num_states = car.NX num_ctrls = car.NU num_measure = len(beacons)+2 #arg/make part of robot observe Q = np.mat(np.diag([0.000001]*num_states)) #arg Q[2,2] = 0 # Gets out of hand if noise in theta or phi Q[3,3] = 0 # Can also add theta/phi to measurement like Sameep #TODO? R = np.mat(np.diag([0.0005]*num_measure)) #arg R[2,2] = 0.000005 R[3,3] = 0.000005 # Sample noise dynamics_noise = mvn([0]*num_states, Q, T-1).T*0 #FIXME measurement_noise = mvn([0]*num_measure, R, T-1).T*0 #FIXME # Setup for EKF mus = np.mat(np.zeros((num_states,T))) mus[:,0] = np.mat(x0).T Sigmas = np.zeros((Q.shape[0], Q.shape[1],T)) Sigmas[:,:,0] = np.mat(np.diag([0.0001]*num_states)) #arg Sigmas[2,2,0] = 0.0000001 Sigmas[3,3,0] = 0.0000001 # Generate nominal belief trajectory ''' X_bar = np.mat(np.zeros((car.NX, T))) #arg X_bar[:,0] = np.mat(x0).T
