def get_acq(self, key, pose=np.zeros((1, 2)), acq_func="gaussian_ei", acq_mod="normal"): if acq_func == "gaussian_sei": return gaussian_sei(self.all_vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[0][:2], masked=acq_mod == "masked") elif acq_func == "maxvalue_entropy_search": return maxvalue_entropy_search(self.all_vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[0][:2], masked=acq_mod == "masked") elif acq_func == "gaussian_pi": return gaussian_pi(self.all_vector_pos, self.gps[key], np.min(self.train_targets[key]), masked=acq_mod == "masked") elif acq_func == "gaussian_ei": return gaussian_ei(self.all_vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[:2], masked=False, xi=1.0) elif acq_func == "max_std": return max_std(self.all_vector_pos, self.gps[key], np.min(self.train_targets[key]), masked=acq_mod == "masked")
def generate_new_goal(self, pose=np.zeros((1, 3)), other_poses=np.zeros((1, 3))): # nans = np.load(open('E:/ETSI/Proyecto/data/Databases/numpy_files/nans.npy', 'rb')) # smapz = np.zeros((1500, 1000)) # max_mapz = None # c_max = 0.0 if self.acq_mod == "split_path": if len(self.splitted_goals) > 0: new_pos = self.splitted_goals[0, :] self.splitted_goals = self.splitted_goals[1:, :] return np.append(new_pos, 0) xi = 1.0 _, reg = calc_voronoi(pose, other_poses, self.map_data) all_acq = [] c_max = 0.0 new_pos = None sum_all_acq = None if self.acquisition == "predictive_entropy_search": gps = self.surrogate(self.vector_pos, return_std=True) sum_sigmas = None for _, sigma in gps: sum_sigmas = sigma if sum_sigmas is None else sigma + sum_sigmas x_star = self.vector_pos[np.where( sum_sigmas == np.max(sum_sigmas))[0][0]] for i in range(len(self.sensors)): mu, sigma = gps[i] all_acq = predictive_entropy_search(self.vector_pos, mu, sigma, model=self.gps[list( self.sensors)[i]], x_star=x_star) if self.acq_fusion == "decoupled": arr1inds = all_acq.argsort() sorted_arr1 = self.vector_pos[arr1inds[::-1]] best_pos, idx = find_vect_pos4region(sorted_arr1, reg, return_idx=True) if all_acq[arr1inds[::-1][idx]] > c_max: new_pos = best_pos c_max = all_acq[arr1inds[::-1][idx]] elif self.acq_fusion == "coupled": sum_all_acq = sum_all_acq + all_acq if sum_all_acq is not None else all_acq else: for key in self.sensors: if self.acquisition == "gaussian_sei": all_acq = gaussian_sei(self.vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "maxvalue_entropy_search": all_acq = maxvalue_entropy_search( self.vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "gaussian_pi": all_acq = gaussian_pi(self.vector_pos, self.gps[key], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "gaussian_ei": all_acq = gaussian_ei( self.vector_pos, # self.gps[key], self.surrogate(keys=[key], return_std=True)[0], np.min(self.train_targets[key]), c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") elif self.acquisition == "max_std": all_acq = max_std(self.vector_pos, self.gps[key], np.min(self.train_targets[key]), masked=self.acq_mod == "masked") if self.acq_fusion == "decoupled": arr1inds = all_acq.argsort() sorted_arr1 = self.vector_pos[arr1inds[::-1]] best_pos, idx = find_vect_pos4region(sorted_arr1, reg, return_idx=True) if all_acq[arr1inds[::-1][idx]] > c_max: new_pos = best_pos c_max = all_acq[arr1inds[::-1][idx]] elif self.acq_fusion == "coupled": sum_all_acq = sum_all_acq + all_acq if sum_all_acq is not None else all_acq # mapz = gaussian_ei(self.all_vector_pos, self.gps[key], np.min(self.train_targets[key]), # c_point=pose[:2], # xi=xi, # masked=self.acq_mod == "masked").reshape((1000, 1500)).T # smapz += mapz # for nnan in nans: # mapz[nnan[0], nnan[1]] = -1 # mapz = np.ma.array(mapz, mask=(mapz == -1)) # if key == "s1": # plt.subplot(231) # elif key == "s2": # plt.subplot(232) # else: # plt.subplot(233) # plt.imshow(mapz, origin='lower', cmap=cmo.cm.matter_r) # plt.title("$AF_{}(x)$".format(str("{" + key + "}"))) # plt.plot(new_pos[0], new_pos[1], 'r.') # for pm in self.train_inputs: # plt.plot(pm[0], pm[1], 'y^') # plt.plot(pose[0], pose[1], 'b^') # plt.colorbar() # if max_mapz is None or all_acq[arr1inds[::-1][idx]] > c_max: # max_mapz = mapz # c_max = all_acq[arr1inds[::-1][idx]] # plt.subplot(235) # for nnan in nans: # smapz[nnan[0], nnan[1]] = -1 # smapz = np.ma.array(smapz, mask=(smapz == -1)) # plt.imshow(smapz, origin='lower', cmap=cmo.cm.matter_r) # plt.title("$\sum AF_i(x)$") # maxx = np.where(smapz == np.max(smapz)) # plt.plot(maxx[1][0], maxx[0][0], 'r.') # plt.plot(pose[0], pose[1], 'b^', zorder=9) # for pm in self.train_inputs: # plt.plot(pm[0], pm[1], 'y^') # plt.colorbar() # plt.legend(["best_next", "c_pose", "prev. measurements"], bbox_to_anchor=(3.5, 1.0), fancybox=True, # shadow=True) # plt.subplot(234) # plt.imshow(max_mapz, origin='lower', cmap=cmo.cm.matter_r) # for pm in self.train_inputs: # plt.plot(pm[0], pm[1], 'y^') # plt.plot(pose[0], pose[1], 'b^') # plt.title("$max(AF_i(x))$") # maxx = np.where(max_mapz == np.max(max_mapz)) # plt.plot(maxx[1][0], maxx[0][0], 'r.') # plt.colorbar() # plt.show(block=True) # if self.acq_fusion == "maxcoupled": # for best_pos in new_poses: # suma = 0 # for key in self.sensors: # this_acq = gaussian_ei(best_pos[0], # self.surrogate(best_pos[0].reshape(1, -1), return_std=True, keys=[key])[0], # np.min(self.train_targets[key]), # c_point=pose[:2], xi=xi, masked=self.acq_mod == "masked") # suma += this_acq # if suma > c_max: # new_pos = best_pos[0] # c_max = suma if self.acq_fusion == "coupled": arr1inds = sum_all_acq.argsort() sorted_arr1 = self.vector_pos[arr1inds[::-1]] best_pos = find_vect_pos4region(sorted_arr1, reg, return_idx=False) new_pos = best_pos if self.acq_mod == "split_path" or self.acq_mod == "truncated": beacons_splitted = [] vect_dist = np.subtract(new_pos, pose[:2]) ang = np.arctan2(vect_dist[1], vect_dist[0]) # d = 50 d = np.exp( np.min([ self.gps[key].kernel_.theta[0] for key in list(self.sensors) ])) * self.proportion for di in np.arange(0, np.linalg.norm(vect_dist), d)[1:]: mini_goal = np.array( [di * np.cos(ang) + pose[0], di * np.sin(ang) + pose[1]]).astype(np.int) if self.map_data[mini_goal[1], mini_goal[0]] == 0: beacons_splitted.append(mini_goal) beacons_splitted.append(np.array(new_pos)) self.splitted_goals = np.array(beacons_splitted) new_pos = self.splitted_goals[0, :] self.splitted_goals = self.splitted_goals[1:, :] new_pos = np.append(new_pos, 0) return new_pos