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 _evaluate(self, x, out, *args, **kwargs):
# X = [x, y] vectores de posicion
# f1 = gaussian_ei(X, self.gps["s2"], np.min(self.train_targets["s2"]))
# f = {"s1": x[:, 0] ** 2 + x[:, 1] ** 2, "s2": (x[:, 0] - 1000) ** 2 + (x[:, 1]) ** 2}
# out["F"] = np.column_stack([f[key] for key in self.sensors])
out["F"] = np.column_stack([
-gaussian_ei(x, self.gps[key], np.min(self.train_targets[key]))
for key in self.sensors
])
constraints = np.ones(np.shape(x)[0])
for iid, point in enumerate(x):
point = np.round(point).astype(np.int)
if self.map_data[point[1], point[0]] == 0 and reg_poly.contains(
Point(point)):
constraints[iid] = -1
out["G"] = np.column_stack([constraints])
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