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_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 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