def update(self,i=0): # Regenerate GMs merged_gauss_2d = GaussianMixture(weights, means, covariances, max_num_mixands=self.num_mixands) matlab_merged_gauss_2d = self.parent.matlab_gm(weights, means, covariances, max_num_mixands=self.num_mixands) # Replot GMs title = 'Python Merged GM ({} mixands)'.format(merged_gauss_2d.weights.size) if hasattr(self,'old_contour'): merged_gauss_2d.contourf = self.old_contour merged_gauss_2d.plot_remove() self.old_contour = merged_gauss_2d.plot(ax=self.axes[1], levels=self.levels, title=title) title = 'Matlab Merged GM ({} mixands)'.format(matlab_merged_gauss_2d.weights.size) if hasattr(self,'old_matlab_contour'): matlab_merged_gauss_2d.contourf = self.old_matlab_contour matlab_merged_gauss_2d.plot_remove() self.old_matlab_contour = matlab_merged_gauss_2d.plot(ax=self.axes[2], levels=self.levels, title=title) # Decrement mixands (with wrapping) if self.num_mixands == self.max_num_mixands: self.num_mixands = 1 elif np.int(self.num_mixands * self.mixand_rate) < self.max_num_mixands: self.num_mixands = np.int(self.num_mixands * self.mixand_rate) else: self.num_mixands = self.max_num_mixands
def clear_probability_from_objects(self, target, obj): logging.info('Clearing probability from {}'.format(obj.name)) fusion_engine = self.robot.fusion_engine vb = VariationalBayes() if not hasattr(obj, 'relations'): obj.define_relations() if target is not None: prior = fusion_engine.filters[target].probability likelihood = obj.relations.binary_models['Inside'] mu, sigma, beta = vb.update( measurement='Not Inside', likelihood=likelihood, prior=prior, use_LWIS=False, ) gm = GaussianMixture(beta, mu, sigma) fusion_engine.filters[target].probability = gm else: # Update all but combined filters for name, filter_ in fusion_engine.filters.iteritems(): if name == 'combined': pass else: prior = filter_.probability likelihood = obj.relations.binary_models['Inside'] mu, sigma, beta = vb.update( measurement='Not Inside', likelihood=likelihood, prior=prior, use_LWIS=False, ) gm = GaussianMixture(beta, mu, sigma) filter_.probability = gm
def _detect_probability(self, prior): mu, sigma, beta = self.vb.update(measurement='No Detection', likelihood=self.detection_model, prior=prior, use_LWIS=True, poly=self.detection_model.poly) gm = GaussianMixture(beta, mu, sigma) gm.camera_viewcone = self.detection_model.poly # for plotting return gm
def lwis_update(self, prior): """ clustering: pairwise greedy merging - compare means, weights & variances salmond's method and runnals' method (better) """ prior_mean = np.asarray(prior.means[0]) prior_var = np.asarray(prior.covariances[0]) # Importance distribution q = GaussianMixture(1, prior_mean, prior_var) # Importance sampling correction w = np.zeros(num_samples) # Importance weights x = q.rvs(size=num_samples) # Sampled points x = np.asarray(x) if hasattr(likelihood, 'subclasses'): measurement_class = likelihood.subclasses[measurement] else: measurement_class = likelihood.classes[measurement] for i in range(num_samples): w[i] = prior.pdf(x[i]) \ * measurement_class.probability(state=x[i])\ / q.pdf(x[i]) w /= np.sum(w) # Normalize weights mu_hat = np.zeros_like(np.asarray(mu_VB)) for i in range(num_samples): x_i = np.asarray(x[i]) mu_hat = mu_hat + x_i.dot(w[i]) var_hat = np.zeros_like(np.asarray(var_VB)) for i in range(num_samples): x_i = np.asarray(x[i]) var_hat = var_hat + w[i] * np.outer(x_i, x_i) var_hat -= np.outer(mu_hat, mu_hat) if mu_hat.size == 1 and mu_hat.ndim > 0: mu_lwis = mu_hat[0] else: mu_lwis = mu_hat if var_hat.size == 1: var_lwis = var_hat[0][0] else: var_lwis = var_hat logging.debug( 'LWIS update found mean of {} and variance of {}.'.format( mu_lwis, var_lwis)) return mu_lwis, var_lwis, log_c_hat
def _detect_probability(self, prior): mu, sigma, beta = self.vb.update(measurement='No Detection', likelihood=self.detection_model, prior=prior, use_LWIS=True, poly=self.detection_model.poly ) gm = GaussianMixture(beta, mu, sigma) gm.camera_viewcone = self.detection_model.poly # for plotting return gm
def detect_probability(self, prior): if not hasattr(self.grounding, 'relations') \ or self.grounding.name.lower() == 'deckard': logging.info( "Defining relations because {} didn't have any.".format( self.grounding.name)) self.grounding.define_relations() if self.grounding.name.lower() == 'deckard': if self.relation == 'Right': self.relation = 'Left' elif self.relation == 'Left': self.relation = 'Right' # Position update label = self.relation if self.target_name == 'nothing' and self.positivity == 'is not': label = 'a robot' elif self.target_name == 'nothing' or self.positivity == 'is not': label = 'Not ' + label likelihood = self.grounding.relations.binary_models[self.relation] mu, sigma, beta = self.vb.update( measurement=label, likelihood=likelihood, prior=prior, use_LWIS=False, ) gm = GaussianMixture(beta, mu, sigma) alpha = self.false_alarm_prob / 2 posterior = prior.combine_gms(gm, alpha) # Weight based on human possibly being wrong return posterior
def compare_to_matlab(measurement='Near'): prior = GaussianMixture( weights=[1, 1, 1, 1, 1], means=[ [-2, -4], # GM1 mean [-1, -2], # GM2 mean [0, 0], # GM3 mean [1, -2], # GM4 mean [2, -4], # GM5 mean ], covariances=[ [ [0.1, 0], # GM1 mean [0, 0.1] ], [ [0.2, 0], # GM2 mean [0, 0.2] ], [ [0.3, 0], # GM3 mean [0, 0.3] ], [ [0.2, 0], # GM4 mean [0, 0.2] ], [ [0.1, 0], # GM5 mean [0, 0.1] ], ]) # prior = GaussianMixture(weights=[1], # means=[[-2, -4], # GM1 mean # ], # covariances=[[[0.1, 0], # GM1 mean # [0, 0.1] # ], # ]) # Define sensor likelihood brm = range_model() file_ = open('/Users/nick/Downloads/VBIS GM Fusion/nick_output.csv', 'w') for i in range(30): # Do a VBIS update logging.info('Starting VB update...') vb = VariationalBayes() mu_hat, var_hat, beta_hat = vb.update(measurement, brm, prior) # Flatten values flat = np.hstack((beta_hat, mu_hat.flatten(), var_hat.flatten())) # Save Flattened values np.savetxt(file_, np.atleast_2d(flat), delimiter=',') file_.close()
def update(self, i=0): self.camera_pose = next(self.trajectory) logging.info('Moving to pose {}.'.format(self.camera_pose)) self.detection_model.move(self.camera_pose) # Do a VBIS update mu, sigma, beta = self.vb.update(measurement='No Detection', likelihood=detection_model, prior=self.gm, use_LWIS=True, poly=detection_model.poly, num_std=self.num_std) self.gm = GaussianMixture(weights=beta, means=mu, covariances=sigma) # Log what's going on logging.info(self.gm) logging.info('Weight sum: {}'.format(beta.sum())) self.remove() self.plot()
def test_voi(): from cops_and_robots.robo_tools.robber import Robber from cops_and_robots.map_tools.map import Map from cops_and_robots.fusion.human import Human from cops_and_robots.fusion.gaussian_mixture import GaussianMixture import matplotlib.pyplot as plt from matplotlib.colors import cnames m = Map() pris = Robber('Pris') pris.map_obj.color = cnames['cornflowerblue'] m.add_robber(pris.map_obj) zhora = Robber('Zhora') zhora.map_obj.color = cnames['cornflowerblue'] m.add_robber(zhora.map_obj) roy = Robber('Roy') m.add_robber(roy.map_obj) h = Human(map_=m) m.add_human_sensor(h) prior = GaussianMixture( [0.1, 0.7, 0.2], [[3, -2], [-6, -2], [0, 0]], [[[1.5, 1.0], [1.0, 1.5]], [[2.5, -0.3], [-0.3, 2.5]], [[0.5, -0.3], [-0.3, 0.5]]]) prior._discretize(bounds=m.bounds, grid_spacing=0.1) q = Questioner(human_sensor=h, target_order=['Pris', 'Roy'], target_weights=[11., 10.]) m.setup_plot(show_human_interface=False) m.update() ax = m.axes['combined'] # prior.plot(bounds=m.bounds, alpha=0.5, ax=ax) # m.update() # plt.show() q.weigh_questions({'Roy': prior}) for qu in q.weighted_questions: print qu
def test_voi(): from cops_and_robots.robo_tools.robber import Robber from cops_and_robots.map_tools.map import Map from cops_and_robots.fusion.human import Human from cops_and_robots.fusion.gaussian_mixture import GaussianMixture import matplotlib.pyplot as plt from matplotlib.colors import cnames m = Map() pris = Robber("Pris") pris.map_obj.color = cnames["cornflowerblue"] m.add_robber(pris.map_obj) zhora = Robber("Zhora") zhora.map_obj.color = cnames["cornflowerblue"] m.add_robber(zhora.map_obj) roy = Robber("Roy") m.add_robber(roy.map_obj) h = Human(map_=m) m.add_human_sensor(h) prior = GaussianMixture( [0.1, 0.7, 0.2], [[3, -2], [-6, -2], [0, 0]], [[[1.5, 1.0], [1.0, 1.5]], [[2.5, -0.3], [-0.3, 2.5]], [[0.5, -0.3], [-0.3, 0.5]]], ) prior._discretize(bounds=m.bounds, grid_spacing=0.1) q = Questioner(human_sensor=h, target_order=["Pris", "Roy"], target_weights=[11.0, 10.0]) m.setup_plot(show_human_interface=False) m.update() ax = m.axes["combined"] # prior.plot(bounds=m.bounds, alpha=0.5, ax=ax) # m.update() # plt.show() q.weigh_questions({"Roy": prior}) for qu in q.weighted_questions: print qu
def matlab_gm(self, weights, means, covariances, max_num_mixands): mdict = { 'weights': weights, 'means': means, 'covariances': covariances, 'max_num_mixands': max_num_mixands, } savemat('matlab/gaussian_mixture/data/from_python.mat', mdict) raw_input("Hit enter when Matlab has created some output to use...") mdict = loadmat('matlab/gaussian_mixture/data/from_matlab.mat') # Get weights, means, covariances from MATLAB weights = mdict['weights'][0] means = mdict['means'] covariances = mdict['covariances'].flatten('F').reshape(-1, 2, 2) gm = GaussianMixture(weights, means, covariances) return gm
def robber_detected(self, robber_pose): """Update the particle filter for a detected robber. """ # <>TODO: Figure out better strategy when robber detected # Find closest particle to target # robber_pt = robber_pose[0:2] # dist = [math.sqrt((pt[0] - robber_pt[0]) ** 2 # + (pt[1] - robber_pt[1]) ** 2) # for pt in self.particles[:, 1:3]] # index = dist.index(min(dist)) # Set all other particles to 0 probability # self.particles[:, 2] = 0 # self.particles[index] = 1 self.probability = GaussianMixture(1, robber_pose[0:2], 0.01 * np.eye(2)) self.finished = True self.recieved_human_update = False
def update(self, i=0): # Regenerate GMs merged_gauss_2d = GaussianMixture( weights, means, covariances, max_num_mixands=self.num_mixands) matlab_merged_gauss_2d = self.parent.matlab_gm( weights, means, covariances, max_num_mixands=self.num_mixands) # Replot GMs title = 'Python Merged GM ({} mixands)'.format( merged_gauss_2d.weights.size) if hasattr(self, 'old_contour'): merged_gauss_2d.contourf = self.old_contour merged_gauss_2d.plot_remove() self.old_contour = merged_gauss_2d.plot(ax=self.axes[1], levels=self.levels, title=title) title = 'Matlab Merged GM ({} mixands)'.format( matlab_merged_gauss_2d.weights.size) if hasattr(self, 'old_matlab_contour'): matlab_merged_gauss_2d.contourf = self.old_matlab_contour matlab_merged_gauss_2d.plot_remove() self.old_matlab_contour = matlab_merged_gauss_2d.plot( ax=self.axes[2], levels=self.levels, title=title) # Decrement mixands (with wrapping) if self.num_mixands == self.max_num_mixands: self.num_mixands = 1 elif np.int(self.num_mixands * self.mixand_rate) < self.max_num_mixands: self.num_mixands = np.int(self.num_mixands * self.mixand_rate) else: self.num_mixands = self.max_num_mixands
def camera_test(num_std=1, time_interval=1): # prior = fleming_prior() # prior = uniform_prior() # prior = GaussianMixture(1, np.zeros(2), np.eye(2)) prior = GaussianMixture([1, 1, 1], np.array([ [-7, 0], [-3, 0], [1, 0], ]), np.eye(2)[None, :].repeat(3, axis=0)) bounds = [-12.5, -3.5, 2.5, 3.5] min_view_dist = 0.3 # [m] max_view_dist = 1.0 # [m] detection_model = camera_model_2D(min_view_dist, max_view_dist) trajectory = np.zeros((20, 2)) ls = np.linspace(-10, 3, 20) trajectory = np.hstack((ls[:, None], trajectory)) class camera_tester(object): """docstring for merged_gm""" def __init__(self, prior, detection_model, trajectory, num_std=1, bounds=None): self.fig = plt.figure(figsize=(16, 8)) self.gm = prior self.detection_model = detection_model self.trajectory = itertools.cycle(trajectory) self.vb = VariationalBayes() self.num_std = num_std if bounds is None: self.bounds = [-5, -5, 5, 5] else: self.bounds = bounds def update(self, i=0): self.camera_pose = next(self.trajectory) logging.info('Moving to pose {}.'.format(self.camera_pose)) self.detection_model.move(self.camera_pose) # Do a VBIS update mu, sigma, beta = self.vb.update(measurement='No Detection', likelihood=detection_model, prior=self.gm, use_LWIS=True, poly=detection_model.poly, num_std=self.num_std) self.gm = GaussianMixture(weights=beta, means=mu, covariances=sigma) # Log what's going on logging.info(self.gm) logging.info('Weight sum: {}'.format(beta.sum())) self.remove() self.plot() def plot(self): levels_res = 50 self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)), levels_res) self.contourf = self.ax.contourf(self.xx, self.yy, self.gm.pdf(self.pos), levels=self.levels, cmap=plt.get_cmap('jet')) # Plot camera self.cam_patch = PolygonPatch(self.detection_model.poly, facecolor='none', linewidth=2, edgecolor='white') self.ax.add_patch(self.cam_patch) # Plot ellipses self.ellipse_patches = self.gm.plot_ellipses( poly=self.detection_model.poly) def plot_setup(self): # Define gridded space for graphing min_x, max_x = self.bounds[0], self.bounds[2] min_y, max_y = self.bounds[1], self.bounds[3] res = 30 self.xx, self.yy = np.mgrid[min_x:max_x:1 / res, min_y:max_y:1 / res] pos = np.empty(self.xx.shape + (2, )) pos[:, :, 0] = self.xx pos[:, :, 1] = self.yy self.pos = pos # Plot setup self.ax = self.fig.add_subplot(111) self.ax.set_title('VBIS with camera detection test') plt.axis('scaled') self.ax.set_xlim([min_x, max_x]) self.ax.set_ylim([min_y, max_y]) levels_res = 50 self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)), levels_res) cax = self.contourf = self.ax.contourf(self.xx, self.yy, self.gm.pdf(self.pos), levels=self.levels, cmap=plt.get_cmap('jet')) self.fig.colorbar(cax) def remove(self): if hasattr(self, 'cam_patch'): self.cam_patch.remove() del self.cam_patch if hasattr(self, 'ellipse_patches'): for patch in self.ellipse_patches: patch.remove() del self.ellipse_patches if hasattr(self, 'contourf'): for collection in self.contourf.collections: collection.remove() del self.contourf gm = camera_tester(prior, detection_model, trajectory, num_std, bounds) logging.info('Initial GM:') logging.info(prior) ani = animation.FuncAnimation(gm.fig, gm.update, interval=time_interval, repeat=True, blit=False, init_func=gm.plot_setup) plt.show()
def gmm_sm_test(measurement='Outside'): # Define prior # prior = GaussianMixture(weights=[1, 4, 5], # means=[[0.5, 1.3], # GM1 mean # [-0.7, -0.6], # GM2 mean # [0.2, -3], # GM3 mean # ], # covariances=[[[0.4, 0.3], # GM1 mean # [0.3, 0.4] # ], # [[0.3, 0.1], # GM2 mean # [0.1, 0.3] # ], # [[0.5, 0.4], # GM3 mean # [0.4, 0.5]], # ]) prior = GaussianMixture( weights=[1, 1, 1, 1, 1], means=[ [-2, -4], # GM1 mean [-1, -2], # GM2 mean [0, 0], # GM3 mean [1, -2], # GM4 mean [2, -4], # GM5 mean ], covariances=[ [ [0.1, 0], # GM1 mean [0, 0.1] ], [ [0.2, 0], # GM2 mean [0, 0.2] ], [ [0.3, 0], # GM3 mean [0, 0.3] ], [ [0.2, 0], # GM4 mean [0, 0.2] ], [ [0.1, 0], # GM5 mean [0, 0.1] ], ]) # prior = GaussianMixture(weights=[1], # means=[[-2, -4], # GM1 mean # ], # covariances=[[[0.1, 0], # GM1 mean # [0, 0.1] # ], # ]) # Define sensor likelihood brm = range_model() # Do a VBIS update logging.info('Starting VB update...') vb = VariationalBayes() mu_hat, var_hat, beta_hat = vb.update(measurement, brm, prior, use_LWIS=True) vbis_posterior = GaussianMixture(weights=beta_hat, means=mu_hat, covariances=var_hat) # Define gridded space for graphing min_x, max_x = -5, 5 min_y, max_y = -5, 5 res = 100 x_space, y_space = np.mgrid[min_x:max_x:1 / res, min_y:max_y:1 / res] pos = np.empty(x_space.shape + (2, )) pos[:, :, 0] = x_space pos[:, :, 1] = y_space levels_res = 50 max_prior = np.max(prior.pdf(pos)) prior_levels = np.linspace(0, max_prior, levels_res) brm.probability() max_lh = np.max(brm.probs) lh_levels = np.linspace(0, max_lh, levels_res) max_post = np.max(vbis_posterior.pdf(pos)) post_levels = np.linspace(0, max_post, levels_res) # Plot results fig = plt.figure() likelihood_label = 'Likelihood of \'{}\''.format(measurement) prior_ax = plt.subplot2grid((2, 32), (0, 0), colspan=14) prior_cax = plt.subplot2grid((2, 32), (0, 14), colspan=1) prior_c = prior_ax.contourf(x_space, y_space, prior.pdf(pos), levels=prior_levels) cbar = plt.colorbar(prior_c, cax=prior_cax) prior_ax.set_xlabel('x1') prior_ax.set_ylabel('x2') prior_ax.set_title('Prior Distribution') lh_ax = plt.subplot2grid((2, 32), (0, 17), colspan=14) lh_cax = plt.subplot2grid((2, 32), (0, 31), colspan=1) brm.classes[measurement].plot(ax=lh_ax, label=likelihood_label, ls='--', levels=lh_levels, show_plot=False, plot_3D=False) # plt.colorbar(sm.probs, cax=lh_cax) lh_ax.set_title(likelihood_label) posterior_ax = plt.subplot2grid((2, 32), (1, 0), colspan=31) posterior_cax = plt.subplot2grid((2, 32), (1, 31), colspan=1) posterior_c = posterior_ax.contourf(x_space, y_space, vbis_posterior.pdf(pos), levels=post_levels) plt.colorbar(posterior_c, cax=posterior_cax) posterior_ax.set_xlabel('x1') posterior_ax.set_ylabel('x2') posterior_ax.set_title('VBIS Posterior Distribution') logging.info( 'Prior Weights: \n {} \n Means: \n {} \n Variances: \n {} \n'.format( prior.weights, prior.means, prior.covariances)) logging.info( 'Posterior Weights: \n {} \n Means: \n {} \n Variances: \n {} \n'. format(vbis_posterior.weights, vbis_posterior.means, vbis_posterior.covariances)) plt.show()
def comparison_2d(): # Define prior prior_mean = np.array([2.3, 1.2]) prior_var = np.array([[2, 0.6], [0.6, 2]]) prior = GaussianMixture(1, prior_mean, prior_var) # Define sensor likelihood sm = intrinsic_space_model() measurement = 'Front' measurement_i = sm.classes[measurement].id # Do a VB update init_mean = np.zeros((1, 2)) init_var = np.eye(2) init_alpha = 0.5 init_xi = np.ones(5) vb = VariationalBayes() vb_mean, vb_var, _ = vb.vb_update(measurement, sm, prior, init_mean, init_var, init_alpha, init_xi) nisar_vb_mean = np.array([1.795546121012238, 2.512627005425541]) nisar_vb_var = np.array([[0.755723395661314, 0.091742424424428], [0.091742424424428, 0.747611340151417]]) diff_vb_mean = vb_mean - nisar_vb_mean diff_vb_var = vb_var - nisar_vb_var logging.info( 'Nisar\'s VB update had mean difference: \n {}\n and var difference: \n {}\n' .format(diff_vb_mean, diff_vb_var)) vb_mean, vb_var, _ = vb.vbis_update(measurement, sm, prior, init_mean, init_var, init_alpha, init_xi) vb_posterior = GaussianMixture(1, vb_mean, vb_var) # Define gridded space for graphing min_x, max_x = -5, 5 min_y, max_y = -5, 5 res = 200 x_space, y_space = np.mgrid[min_x:max_x:1 / res, min_y:max_y:1 / res] pos = np.empty(x_space.shape + (2, )) pos[:, :, 0] = x_space pos[:, :, 1] = y_space levels_res = 30 max_prior = np.max(prior.pdf(pos)) prior_levels = np.linspace(0, max_prior, levels_res) sm.probability() max_lh = np.max(sm.probs) lh_levels = np.linspace(0, max_lh, levels_res) max_post = np.max(vb_posterior.pdf(pos)) post_levels = np.linspace(0, max_post, levels_res) # Plot results fig = plt.figure() likelihood_label = 'Likelihood of \'{}\''.format(measurement) prior_ax = plt.subplot2grid((2, 32), (0, 0), colspan=14) prior_cax = plt.subplot2grid((2, 32), (0, 14), colspan=1) prior_c = prior_ax.contourf(x_space, y_space, prior.pdf(pos), levels=prior_levels) cbar = plt.colorbar(prior_c, cax=prior_cax) prior_ax.set_xlabel('x1') prior_ax.set_ylabel('x2') prior_ax.set_title('Prior Distribution') lh_ax = plt.subplot2grid((2, 32), (0, 17), colspan=14) lh_cax = plt.subplot2grid((2, 32), (0, 31), colspan=1) sm.classes[measurement].plot(ax=lh_ax, label=likelihood_label, plot_3D=False, levels=lh_levels) # plt.colorbar(sm.probs, cax=lh_cax) lh_ax.set_title(likelihood_label) posterior_ax = plt.subplot2grid((2, 32), (1, 0), colspan=31) posterior_cax = plt.subplot2grid((2, 32), (1, 31), colspan=1) posterior_c = posterior_ax.contourf(x_space, y_space, vb_posterior.pdf(pos), levels=post_levels) plt.colorbar(posterior_c, cax=posterior_cax) posterior_ax.set_xlabel('x1') posterior_ax.set_ylabel('x2') posterior_ax.set_title('VB Posterior Distribution') plt.show()
def comparison_1d(): # Define prior prior_mean, prior_var = 0.3, 0.01 min_x, max_x = -5, 5 res = 10000 prior = GaussianMixture(1, prior_mean, prior_var) x_space = np.linspace(min_x, max_x, res) # Define sensor likelihood sm = speed_model() measurement = 'Slow' measurement_i = sm.class_labels.index(measurement) # Do a VB update init_mean, init_var = 0, 1 init_alpha, init_xi = 0.5, np.ones(4) vb = VariationalBayes() vb_mean, vb_var, _ = vb.vb_update(measurement, sm, prior, init_mean, init_var, init_alpha, init_xi) vb_posterior = GaussianMixture(1, vb_mean, vb_var) nisar_vb_mean = 0.131005297841171 nisar_vb_var = 6.43335516254277e-05 diff_vb_mean = vb_mean - nisar_vb_mean diff_vb_var = vb_var - nisar_vb_var logging.info( 'Nisar\'s VB update had mean difference {} and var difference {}\n'. format(diff_vb_mean, diff_vb_var)) # Do a VBIS update vbis_mean, vbis_var, _ = vb.vbis_update(measurement, sm, prior, init_mean, init_var, init_alpha, init_xi) vbis_posterior = GaussianMixture(1, vbis_mean, vbis_var) nisar_vbis_mean = 0.154223416817080 nisar_vbis_var = 0.00346064073274943 diff_vbis_mean = vbis_mean - nisar_vbis_mean diff_vbis_var = vbis_var - nisar_vbis_var logging.info( 'Nisar\'s VBIS update had mean difference {} and var difference {}\n'. format(diff_vbis_mean, diff_vbis_var)) # Plot results likelihood_label = 'Likelihood of \'{}\''.format(measurement) fig = plt.figure() ax = fig.add_subplot(111) sm.classes[measurement].plot(ax=ax, fill_between=False, label=likelihood_label, ls='--') ax.plot(x_space, prior.pdf(x_space), lw=1, label='prior pdf', c='grey', ls='--') ax.plot(x_space, vb_posterior.pdf(x_space), lw=2, label='VB posterior', c='r') ax.fill_between(x_space, 0, vb_posterior.pdf(x_space), alpha=0.2, facecolor='r') ax.plot(x_space, vbis_posterior.pdf(x_space), lw=2, label='VBIS Posterior', c='g') ax.fill_between(x_space, 0, vbis_posterior.pdf(x_space), alpha=0.2, facecolor='g') ax.set_title('VBIS Update') ax.legend() ax.set_xlim([0, 0.4]) ax.set_ylim([0, 7]) plt.show()
def update(self, measurement, likelihood, prior, use_LWIS=False, poly=None, num_std=1): """VB update using Gaussian mixtures and multimodal softmax. This uses Variational Bayes with Importance Sampling (VBIS) for each mixand-softmax pair available. """ # If we have a polygon, update only the mixands intersecting with it if poly is None: update_intersections_only = False else: update_intersections_only = True h = 0 relevant_subclasses = likelihood.classes[measurement].subclasses num_relevant_subclasses = len(relevant_subclasses) # Use intersecting priors only if update_intersections_only: other_priors = prior.copy() weights = [] means = [] covariances = [] mixand_ids = [] ellipses = prior.std_ellipses(num_std) any_intersection = False for i, ellipse in enumerate(ellipses): try: has_intersection = poly.intersects(ellipse) except ValueError: logging.warn('Null geometry error! Defaulting to true.') has_intersection = True if has_intersection: # Get parameters for intersecting priors mixand_ids.append(i) weights.append(prior.weights[i]) means.append(prior.means[i]) covariances.append(prior.covariances[i]) any_intersection = True if not any_intersection: logging.debug('No intersection with any ellipse.') mu_hat = other_priors.means var_hat = other_priors.covariances beta_hat = other_priors.weights return mu_hat, var_hat, beta_hat # Remove these from the other priors other_priors.weights = \ np.delete(other_priors.weights, mixand_ids, axis=0) other_priors.means = \ np.delete(other_priors.means, mixand_ids, axis=0) other_priors.covariances = \ np.delete(other_priors.covariances, mixand_ids, axis=0) # Retain total weight of intersection weights for renormalization max_intersecion_weight = sum(weights) # Create new prior prior = GaussianMixture(weights, means, covariances) logging.debug( 'Using only mixands {} for VBIS fusion. Total weight {}'. format(mixand_ids, max_intersecion_weight)) # Parameters for all new mixands K = num_relevant_subclasses * prior.weights.size mu_hat = np.zeros((K, prior.means.shape[1])) var_hat = np.zeros( (K, prior.covariances.shape[1], prior.covariances.shape[2])) log_beta_hat = np.zeros(K) # Weight estimates for u, mixand_weight in enumerate(prior.weights): mix_sm_corr = 0 # Check to see if the mixand is completely contained within # the softmax class (i.e. doesn't need an update) mixand = GaussianMixture(1, prior.means[u], prior.covariances[u]) mixand_samples = mixand.rvs(self.num_mixand_samples) p_hat_ru_samples = likelihood.classes[measurement].probability( state=mixand_samples) mix_sm_corr = np.sum(p_hat_ru_samples) / self.num_mixand_samples if mix_sm_corr > self.mix_sm_corr_thresh: logging.debug( 'Mixand {}\'s correspondence with {} was {},' 'above the threshold of {}, so VBIS was skipped.'.format( u, measurement, mix_sm_corr, self.mix_sm_corr_thresh)) # Append the prior's parameters to the mixand parameter lists mu_hat[h, :] = prior.means[u] var_hat[h, :] = prior.covariances[u] log_beta_hat[h] = np.log(mixand_weight) h += 1 continue # Otherwise complete the full VBIS update ordered_subclasses = iter(sorted(relevant_subclasses.iteritems())) for label, subclass in ordered_subclasses: # Compute \hat{P}_s(r|u) mixand_samples = mixand.rvs(self.num_mixand_samples) p_hat_ru_samples = subclass.probability(state=mixand_samples) p_hat_ru_sampled = np.sum( p_hat_ru_samples) / self.num_mixand_samples mu_vbis, var_vbis, log_c_hat = \ self.vbis_update(label, subclass.softmax_collection, mixand, use_LWIS=use_LWIS) # Compute log odds of r given u if np.isnan(log_c_hat): # from LWIS update log_p_hat_ru = np.log(p_hat_ru_sampled) else: log_p_hat_ru = np.max( (log_c_hat, np.log(p_hat_ru_sampled))) # Find log of P(u,r|D_k) \approxequal \hat{B}_{ur} log_beta_vbis = np.log(mixand_weight) + log_p_hat_ru # Symmetrize var_vbis var_vbis = 0.5 * (var_vbis.T + var_vbis) # Update estimate values log_beta_hat[h] = log_beta_vbis mu_hat[h, :] = mu_vbis var_hat[h, :] = var_vbis h += 1 # Renormalize and truncate (based on weight threshold) log_beta_hat = log_beta_hat - np.max(log_beta_hat) unnormalized_beta_hats = np.exp(log_beta_hat) beta_hat = np.exp(log_beta_hat) / np.sum(np.exp(log_beta_hat)) # Reattach untouched prior values if update_intersections_only: beta_hat = unnormalized_beta_hats * max_intersecion_weight beta_hat = np.hstack((other_priors.weights, beta_hat)) mu_hat = np.vstack((other_priors.means, mu_hat)) var_hat = np.concatenate((other_priors.covariances, var_hat)) # Shrink mu, var and beta if necessary h += other_priors.weights.size beta_hat = beta_hat[:h] mu_hat = mu_hat[:h] var_hat = var_hat[:h] beta_hat /= beta_hat.sum() else: # Shrink mu, var and beta if necessary beta_hat = beta_hat[:h] mu_hat = mu_hat[:h] var_hat = var_hat[:h] # Threshold based on weights mu_hat = mu_hat[beta_hat > self.weight_threshold, :] var_hat = var_hat[beta_hat > self.weight_threshold, :] beta_hat = beta_hat[beta_hat > self.weight_threshold] # Check if covariances are positive semidefinite for i, var in enumerate(var_hat): try: assert np.all(np.linalg.det(var) > 0) except AssertionError, e: logging.warn('Following variance is not positive ' 'semidefinite: \n{}'.format(var)) var_hat[i] = np.eye(var.shape[0]) * 10**-3
def test_merging(self, num_mixands=100, max_num_mixands=10, spread=4, speak=False): if max_num_mixands is None: animate = True max_num_mixands = num_mixands else: animate = False # Generate the unmerged and merged gaussians weights, means, covariances = generate_random_params(num_mixands, ndims=2, spread=spread) unmerged_gauss_2d = GaussianMixture(weights.copy(), means.copy(), covariances.copy(), max_num_mixands=len(weights)) merged_gauss_2d = GaussianMixture(weights.copy(), means.copy(), covariances.copy(), max_num_mixands=max_num_mixands) matlab_merged_gauss_2d = self.matlab_gm(weights.copy(), means.copy(), covariances.copy(), max_num_mixands) mixtures = {'unmerged': unmerged_gauss_2d, 'merged': merged_gauss_2d, 'matlab merged': matlab_merged_gauss_2d } # Setup figure and levels fig = plt.figure(figsize=(18,6)) axes = [] _, max_1 = unmerged_gauss_2d.max_point_by_grid() _, max_2 = merged_gauss_2d.max_point_by_grid() _, max_3 = matlab_merged_gauss_2d.max_point_by_grid() max_prob = np.max((max_1, max_2, max_3)) levels = np.linspace(0, max_prob * 1.2, 50) # Plot all three ax = fig.add_subplot(131) axes.append(ax) title = 'Original GM ({} mixands)'\ .format(unmerged_gauss_2d.weights.size) unmerged_gauss_2d.plot(ax=ax, levels=levels, title=title) ax = fig.add_subplot(132) axes.append(ax) title = 'Python Merged GM ({} mixands)'\ .format(merged_gauss_2d.weights.size) merged_gauss_2d.plot(ax=ax, levels=levels, title=title) ax = fig.add_subplot(133) axes.append(ax) title = 'Matlab Merged GM ({} mixands)'\ .format(matlab_merged_gauss_2d.weights.size) matlab_merged_gauss_2d.plot(ax=ax, levels=levels, title=title) # Add a colorbar fig.subplots_adjust(right=0.85) cbar_ax = fig.add_axes([0.875, 0.1, 0.025, 0.8]) fig.colorbar(unmerged_gauss_2d.contourf, cax=cbar_ax) class GMAnimation(object): """docstring for merged_gm""" def __init__(self, parent, mixand_rate=2, levels=None, axes=None): self.max_num_mixands = mixtures['unmerged'].weights.size self.num_mixands = 1 self.mixand_rate = mixand_rate self.levels = levels self.axes = axes self.parent = parent def update(self,i=0): # Regenerate GMs merged_gauss_2d = GaussianMixture(weights, means, covariances, max_num_mixands=self.num_mixands) matlab_merged_gauss_2d = self.parent.matlab_gm(weights, means, covariances, max_num_mixands=self.num_mixands) # Replot GMs title = 'Python Merged GM ({} mixands)'.format(merged_gauss_2d.weights.size) if hasattr(self,'old_contour'): merged_gauss_2d.contourf = self.old_contour merged_gauss_2d.plot_remove() self.old_contour = merged_gauss_2d.plot(ax=self.axes[1], levels=self.levels, title=title) title = 'Matlab Merged GM ({} mixands)'.format(matlab_merged_gauss_2d.weights.size) if hasattr(self,'old_matlab_contour'): matlab_merged_gauss_2d.contourf = self.old_matlab_contour matlab_merged_gauss_2d.plot_remove() self.old_matlab_contour = matlab_merged_gauss_2d.plot(ax=self.axes[2], levels=self.levels, title=title) # Decrement mixands (with wrapping) if self.num_mixands == self.max_num_mixands: self.num_mixands = 1 elif np.int(self.num_mixands * self.mixand_rate) < self.max_num_mixands: self.num_mixands = np.int(self.num_mixands * self.mixand_rate) else: self.num_mixands = self.max_num_mixands def compare_results(): pass if animate: gm_ani = GMAnimation(self, mixand_rate=2, levels=levels, axes=axes) ani = animation.FuncAnimation(fig, gm_ani.update, interval=100, repeat=True, blit=False, ) else: self.diff(merged_gauss_2d, matlab_merged_gauss_2d) plt.show() self.check_diff()
def test_merging(self, num_mixands=100, max_num_mixands=10, spread=4, speak=False): if max_num_mixands is None: animate = True max_num_mixands = num_mixands else: animate = False # Generate the unmerged and merged gaussians weights, means, covariances = generate_random_params(num_mixands, ndims=2, spread=spread) unmerged_gauss_2d = GaussianMixture(weights.copy(), means.copy(), covariances.copy(), max_num_mixands=len(weights)) merged_gauss_2d = GaussianMixture(weights.copy(), means.copy(), covariances.copy(), max_num_mixands=max_num_mixands) matlab_merged_gauss_2d = self.matlab_gm(weights.copy(), means.copy(), covariances.copy(), max_num_mixands) mixtures = { 'unmerged': unmerged_gauss_2d, 'merged': merged_gauss_2d, 'matlab merged': matlab_merged_gauss_2d } # Setup figure and levels fig = plt.figure(figsize=(18, 6)) axes = [] _, max_1 = unmerged_gauss_2d.max_point_by_grid() _, max_2 = merged_gauss_2d.max_point_by_grid() _, max_3 = matlab_merged_gauss_2d.max_point_by_grid() max_prob = np.max((max_1, max_2, max_3)) levels = np.linspace(0, max_prob * 1.2, 50) # Plot all three ax = fig.add_subplot(131) axes.append(ax) title = 'Original GM ({} mixands)'\ .format(unmerged_gauss_2d.weights.size) unmerged_gauss_2d.plot(ax=ax, levels=levels, title=title) ax = fig.add_subplot(132) axes.append(ax) title = 'Python Merged GM ({} mixands)'\ .format(merged_gauss_2d.weights.size) merged_gauss_2d.plot(ax=ax, levels=levels, title=title) ax = fig.add_subplot(133) axes.append(ax) title = 'Matlab Merged GM ({} mixands)'\ .format(matlab_merged_gauss_2d.weights.size) matlab_merged_gauss_2d.plot(ax=ax, levels=levels, title=title) # Add a colorbar fig.subplots_adjust(right=0.85) cbar_ax = fig.add_axes([0.875, 0.1, 0.025, 0.8]) fig.colorbar(unmerged_gauss_2d.contourf, cax=cbar_ax) class GMAnimation(object): """docstring for merged_gm""" def __init__(self, parent, mixand_rate=2, levels=None, axes=None): self.max_num_mixands = mixtures['unmerged'].weights.size self.num_mixands = 1 self.mixand_rate = mixand_rate self.levels = levels self.axes = axes self.parent = parent def update(self, i=0): # Regenerate GMs merged_gauss_2d = GaussianMixture( weights, means, covariances, max_num_mixands=self.num_mixands) matlab_merged_gauss_2d = self.parent.matlab_gm( weights, means, covariances, max_num_mixands=self.num_mixands) # Replot GMs title = 'Python Merged GM ({} mixands)'.format( merged_gauss_2d.weights.size) if hasattr(self, 'old_contour'): merged_gauss_2d.contourf = self.old_contour merged_gauss_2d.plot_remove() self.old_contour = merged_gauss_2d.plot(ax=self.axes[1], levels=self.levels, title=title) title = 'Matlab Merged GM ({} mixands)'.format( matlab_merged_gauss_2d.weights.size) if hasattr(self, 'old_matlab_contour'): matlab_merged_gauss_2d.contourf = self.old_matlab_contour matlab_merged_gauss_2d.plot_remove() self.old_matlab_contour = matlab_merged_gauss_2d.plot( ax=self.axes[2], levels=self.levels, title=title) # Decrement mixands (with wrapping) if self.num_mixands == self.max_num_mixands: self.num_mixands = 1 elif np.int(self.num_mixands * self.mixand_rate) < self.max_num_mixands: self.num_mixands = np.int(self.num_mixands * self.mixand_rate) else: self.num_mixands = self.max_num_mixands def compare_results(): pass if animate: gm_ani = GMAnimation(self, mixand_rate=2, levels=levels, axes=axes) ani = animation.FuncAnimation( fig, gm_ani.update, interval=100, repeat=True, blit=False, ) else: self.diff(merged_gauss_2d, matlab_merged_gauss_2d) plt.show() self.check_diff()
def _update_combined(self, sensors, robbers): """Update the `combined` filter. Parameters ---------- sensors : dict A collection of all sensors to be updated. """ if self.filter_type == 'particle': # Remove all particles from combined filter # <>TODO: correct number of particles based on state self.filters['combined'].particles = np.zeros((1, 5)) # Add all particles from missing robots to combined filter for robber in robbers.values(): self.filters['combined'].n_particles += \ self.filters[robber.name].n_particles self.filters['combined'].particles = \ np.append(self.filters['combined'].particles, self.filters[robber.name].particles, axis=0) self.filters['combined'].n_particles = \ len(self.filters['combined'].particles) # Reset the human sensor sensors['human'].utterance = '' sensors['human'].target = '' else: # Pre-allocate parameter arrays num_mixands = 0 all_done = True for label, filter_ in self.filters.iteritems(): if label == 'combined' or filter_.finished: continue all_done = False num_mixands += filter_.probability.num_mixands # <>TODO: Don't find ndims each time ndims = filter_.probability.ndims if all_done: # <>TODO: Fix hardcoded dimensions, clean it up gm = GaussianMixture(1, [0, 0], 0.01 * np.eye(2)) self.filters['combined'].probability = gm return weights = np.empty((num_mixands)) means = np.empty((num_mixands, ndims)) covariances = np.empty((num_mixands, ndims, ndims)) # Load parameter arrays i = 0 for label, filter_ in self.filters.iteritems(): if label == 'combined' or filter_.finished: continue size = filter_.probability.num_mixands weights[i:i + size] = filter_.probability.weights means[i:i + size] = filter_.probability.means covariances[i:i + size] = filter_.probability.covariances i += size combined_gm = GaussianMixture(weights, means, covariances, max_num_mixands=num_mixands) self.filters['combined'].probability = combined_gm
class camera_tester(object): """docstring for merged_gm""" def __init__(self, prior, detection_model, trajectory, num_std=1, bounds=None): self.fig = plt.figure(figsize=(16, 8)) self.gm = prior self.detection_model = detection_model self.trajectory = itertools.cycle(trajectory) self.vb = VariationalBayes() self.num_std = num_std if bounds is None: self.bounds = [-5, -5, 5, 5] else: self.bounds = bounds def update(self, i=0): self.camera_pose = next(self.trajectory) logging.info('Moving to pose {}.'.format(self.camera_pose)) self.detection_model.move(self.camera_pose) # Do a VBIS update mu, sigma, beta = self.vb.update(measurement='No Detection', likelihood=detection_model, prior=self.gm, use_LWIS=True, poly=detection_model.poly, num_std=self.num_std) self.gm = GaussianMixture(weights=beta, means=mu, covariances=sigma) # Log what's going on logging.info(self.gm) logging.info('Weight sum: {}'.format(beta.sum())) self.remove() self.plot() def plot(self): levels_res = 50 self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)), levels_res) self.contourf = self.ax.contourf(self.xx, self.yy, self.gm.pdf(self.pos), levels=self.levels, cmap=plt.get_cmap('jet')) # Plot camera self.cam_patch = PolygonPatch(self.detection_model.poly, facecolor='none', linewidth=2, edgecolor='white') self.ax.add_patch(self.cam_patch) # Plot ellipses self.ellipse_patches = self.gm.plot_ellipses( poly=self.detection_model.poly) def plot_setup(self): # Define gridded space for graphing min_x, max_x = self.bounds[0], self.bounds[2] min_y, max_y = self.bounds[1], self.bounds[3] res = 30 self.xx, self.yy = np.mgrid[min_x:max_x:1 / res, min_y:max_y:1 / res] pos = np.empty(self.xx.shape + (2, )) pos[:, :, 0] = self.xx pos[:, :, 1] = self.yy self.pos = pos # Plot setup self.ax = self.fig.add_subplot(111) self.ax.set_title('VBIS with camera detection test') plt.axis('scaled') self.ax.set_xlim([min_x, max_x]) self.ax.set_ylim([min_y, max_y]) levels_res = 50 self.levels = np.linspace(0, np.max(self.gm.pdf(self.pos)), levels_res) cax = self.contourf = self.ax.contourf(self.xx, self.yy, self.gm.pdf(self.pos), levels=self.levels, cmap=plt.get_cmap('jet')) self.fig.colorbar(cax) def remove(self): if hasattr(self, 'cam_patch'): self.cam_patch.remove() del self.cam_patch if hasattr(self, 'ellipse_patches'): for patch in self.ellipse_patches: patch.remove() del self.ellipse_patches if hasattr(self, 'contourf'): for collection in self.contourf.collections: collection.remove() del self.contourf
def vbis_update(self, measurement, likelihood, prior, init_mean=0, init_var=1, init_alpha=0.5, init_xi=1, num_samples=None, use_LWIS=False): """VB update with importance sampling for Gaussian and Softmax. """ if num_samples is None: num_samples = self.num_importance_samples if use_LWIS: q_mu = np.asarray(prior.means[0]) log_c_hat = np.nan else: # Use VB update q_mu, var_VB, log_c_hat = self.vb_update(measurement, likelihood, prior, init_mean, init_var, init_alpha, init_xi) q_var = np.asarray(prior.covariances[0]) # Importance distribution q = GaussianMixture(1, q_mu, q_var) # Importance sampling correction w = np.zeros(num_samples) # Importance weights x = q.rvs(size=num_samples) # Sampled points x = np.asarray(x) if hasattr(likelihood, 'subclasses'): measurement_class = likelihood.subclasses[measurement] else: measurement_class = likelihood.classes[measurement] # Compute parameters using samples w = prior.pdf(x) * measurement_class.probability(state=x) / q.pdf(x) w /= np.sum(w) # Normalize weights mu_hat = np.sum(x.T * w, axis=-1) # <>TODO: optimize this var_hat = np.zeros_like(np.asarray(q_var)) for i in range(num_samples): x_i = np.asarray(x[i]) var_hat = var_hat + w[i] * np.outer(x_i, x_i) var_hat -= np.outer(mu_hat, mu_hat) # Ensure properly formatted output if mu_hat.size == 1 and mu_hat.ndim > 0: mu_post_vbis = mu_hat[0] else: mu_post_vbis = mu_hat if var_hat.size == 1: var_post_vbis = var_hat[0][0] else: var_post_vbis = var_hat logging.debug( 'VBIS update found mean of {} and variance of {}.'.format( mu_post_vbis, var_post_vbis)) return mu_post_vbis, var_post_vbis, log_c_hat
return self.contourf def remove(self): if hasattr(self, 'contourf'): for collection in self.contourf.collections: collection.remove() del self.contourf if hasattr(self, 'ellipse_patches'): for patch in self.ellipse_patches: patch.remove() del self.ellipse_patches if __name__ == '__main__': d = GaussianMixture(1, [0, 0], [[1, 0], [0, 1]]) filter_ = type('test', (object, ), {'probability': d})() pl = ProbabilityLayer(d, z_levels=50, alpha=1) test_probability = [] test_probability.append(GaussianMixture(1, [2, 0], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [1, 1], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [0, 2], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [-1, 1], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [-2, 0], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [-1, -1], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [0, -2], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [1, -1], [[1, 0], [0, 1]])) test_probability.append(GaussianMixture(1, [2, 0], [[1, 0], [0, 1]])) pl.test_probability = itertools.cycle(test_probability)