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
def generate_sensor_image(self):
self.db.updating_sensors = True
raw_data = self.db.sensors_df.loc[self.db.sensors_df['type'] ==
's1'].to_numpy()
last_index = len(raw_data)
raw_data = raw_data[self.db.sensors_c_index:, 1:6]
# if self.db.sensors_c_index == 0:
# new_data = [[row[:2], row[2]] for row in raw_data]
# self.coordinator.initialize_data_gpr(new_data)
# if self.ui.actionPredicci_n_GP.isChecked():
# self.axes[0].plot(new_data[0][0][0], new_data[0][0][1], 'Dy', markersize=12,
# label="Initial Information", zorder=6)
# self.axes[0].plot(new_data[1][0][0], new_data[1][0][1], 'Dy', markersize=12, zorder=6)
# self.axes[0].plot(new_data[2][0][0], new_data[2][0][1], '^y', markersize=12, zorder=6,
# label="Previous Positions")
# if self.ui.actionIncertidumbre_GP.isChecked():
# self.axes[1].plot(new_data[0][0][0], new_data[0][0][1], 'Dy', markersize=12,
# label="Initial Information", zorder=6)
# self.axes[1].plot(new_data[1][0][0], new_data[1][0][1], 'Dy', markersize=12, zorder=6)
# self.axes[1].plot(new_data[2][0][0], new_data[2][0][1], '^y', markersize=12,
# label="Previous Positions", zorder=6)
# if self.ui.actionFuncion_de_Adquisici_n.isChecked():
# self.axes[2].plot(new_data[0][0][0], new_data[0][0][1], 'Dy', markersize=12, zorder=6)
# self.axes[2].plot(new_data[1][0][0], new_data[1][0][1], 'Dy', markersize=12, zorder=6)
# self.axes[2].plot(new_data[2][0][0], new_data[2][0][1], '^y', markersize=12, zorder=6)
# else:
for data in raw_data:
if self.db.sensors_c_index > 0:
poses = self.db.drones_df.to_numpy()
_, reg = calc_voronoi(data[:2], [
np.array(read[:2]) for read in poses if read[2] != data[4]
], self.data[0])
else:
_, reg = calc_voronoi(data[:2], [
np.array(read[:2])
for read in raw_data if read[4] != data[4]
], self.data[0])
reg = np.vstack([reg, reg[0, :]])
if self.colors[int(data[4])] in self.voronoi_ax.keys():
for line in self.voronoi_ax[self.colors[int(data[4])]]:
line.pop(0).remove()
self.voronoi_ax[self.colors[int(data[4])]] = []
if self.ui.actionMapa.isChecked():
# self.axes[0].plot(data[0], data[1], '^{}'.format(drone_color), markersize=12,
# zorder=6, alpha=0.7, label="Drone: {}".format(int(data[4])))
self.axes[0].plot(data[0],
data[1],
'^',
color=self.colors[int(data[4])],
markersize=12,
zorder=6,
alpha=0.7,
label="Drone: {}".format(int(data[4])))
self.voronoi_ax[self.colors[int(data[4])]].append(
self.axes[0].plot(reg[:, 0],
reg[:, 1],
'-',
color=self.colors[int(data[4])],
zorder=9,
lw=3))
# self.axes[0].legend(loc="lower left", prop={"size": 20})
if self.ui.actionPredicci_n_GP.isChecked():
self.axes[1].plot(data[0],
data[1],
'^',
color=self.colors[int(data[4])],
markersize=12,
zorder=6,
alpha=0.7)
self.voronoi_ax[self.colors[int(data[4])]].append(
self.axes[1].plot(reg[:, 0],
reg[:, 1],
'-',
color=self.colors[int(data[4])],
zorder=9))
if self.ui.actionIncertidumbre_GP.isChecked():
self.axes[2].plot(data[0],
data[1],
'^',
color=self.colors[int(data[4])],
markersize=12,
zorder=6,
alpha=0.7)
self.voronoi_ax[self.colors[int(data[4])]].append(
self.axes[2].plot(reg[:, 0],
reg[:, 1],
'-',
color=self.colors[int(data[4])],
zorder=9))
# if self.ui.actionFuncion_de_Adquisici_n.isChecked():
# self.axes[3].plot(data[0], data[1], '^', color=self.colors[int(data[4])], markersize=12,
# zorder=6, alpha=0.7)
# self.voronoi_ax[self.colors[int(data[4])]].append(
# self.axes[3].plot(reg[:, 0], reg[:, 1], '-', color=self.colors[int(data[4])], zorder=9))
# print(data)
self.coordinator.add_data({"pos": data[:2], "s1": data[2]})
print(raw_data, data[:2], data[2])
self.coordinator.fit_data()
self.db.sensors_c_index = last_index
# print(raw_data)
self.current_drone_pos = raw_data[-1][0:3]
observe_maps = dict()
sensor_name = "s1"
if self.ui.actionPredicci_n_GP.isChecked(
) or self.ui.actionIncertidumbre_GP.isChecked():
if self.ui.actionIncertidumbre_GP.isChecked():
result = self.coordinator.surrogate(return_std=True,
keys=["s1"])
mu, std = result[0][0], result[0][1]
else:
mu = self.coordinator.surrogate(return_std=False)
if self.ui.actionPredicci_n_GP.isChecked():
observe_maps["{} gp".format(sensor_name)] = mu
# observe_maps["{} gp".format(sensor_name)] = (mu - self.real_map) ** 2
if self.ui.actionIncertidumbre_GP.isChecked():
observe_maps["{} gp un".format(sensor_name)] = std
# if self.ui.actionFuncion_de_Adquisici_n.isChecked():
# acq, sensor_name = self.coordinator.get_acq(self.current_drone_pos, self.acq_func)
# observe_maps["{} acq".format(sensor_name)] = acq
self.observe_maps(observe_maps)
self.db.updating_sensors = False
if self.auto_BO:
self.send_request()
def generate_new_goal(self, pose=np.zeros((1, 3)), idx=-1, other_poses=np.zeros((1, 3))):
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:, :]
# print('new pos is', new_pos)
return np.append(new_pos, 0)
xi = 1.0
_, reg = calc_voronoi(pose, other_poses, self.map_data)
if self.acquisition == "gaussian_sei":
all_acq = gaussian_sei(self.vector_pos, self.gp, np.min(self.data[1]), 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.gp, np.min(self.data[1]), c_point=pose[:2], xi=xi,
masked=self.acq_mod == "masked")
elif self.acquisition == "gaussian_pi":
all_acq = gaussian_pi(self.vector_pos, self.gp, np.min(self.data[1]), c_point=pose[:2], xi=xi,
masked=self.acq_mod == "masked")
elif self.acquisition == "gaussian_ei":
all_acq = gaussian_ei(self.vector_pos, self.gp, np.min(self.data[1]), c_point=pose[:2], xi=xi,
masked=self.acq_mod == "masked")
elif self.acquisition == "max_std":
all_acq = max_std(self.vector_pos, self.gp, np.min(self.data[1]),
masked=self.acq_mod == "masked")
# all_acq = gaussian_ei(self.vector_pos, self.gp, np.min(self.data[1]), xi=xi, return_grad=False)
# print('no, cvp isnt ', self.vector_pos)
# curr_vect_pos = find_vect_pos4region(self.point_v_pos, reg)
# print('yes, cvp is ', curr_vect_pos)
# print('no, cvp isnt ', self.vector_pos)
optimize_method = 'maximum'
if optimize_method == "maximum":
arr1inds = all_acq.argsort()
sorted_arr1 = self.vector_pos[arr1inds[::-1]]
new_pos = find_vect_pos4region(sorted_arr1, reg)
elif optimize_method == "cvt":
from time import time
t0 = time()
new_pos = find_cvt_pos4region(all_acq, self.vector_pos, reg)
# print("ne: ", new_pos)
# print("t0: ", time() - t0)
# t0 = time()
# arr1inds = all_acq.argsort()
# sorted_arr1 = self.vector_pos[arr1inds[::-1]]
# new_pos = find_vect_pos4region(sorted_arr1, reg)
# print("ne: ", new_pos)
# print("t1: ", time() - t0)
# print('---')
# print(self.vector_pos[np.where(all_acq == np.nanmax(all_acq))][0])
# print(new_pos)
# print('---')
if self.acq_mod == "split_path" or self.acq_mod == "truncated":
beacons_splitted = []
# print('new pos', new_pos)
# print('old pos', pose)
vect_dist = np.subtract(new_pos, pose[:2])
# print('vd', vect_dist)
ang = np.arctan2(vect_dist[1], vect_dist[0])
# print('ang', ang)
d = np.exp(self.gp.kernel_.theta)[0] / 2
# d = 220
# print(d)
# print('x', d * np.cos(ang), vect_dist[0])
# print('y', d * np.sin(ang), vect_dist[1])
# if d * np.cos(ang) < vect_dist[0]:
# lx = np.round(np.arange(pose[0], new_pos[0], d * np.cos(ang))[1:]).astype(np.int)
# else:
# lx = []
# if d * np.sin(ang) < vect_dist[1]:
# ly = np.round(np.arange(pose[1], new_pos[1], d * np.sin(ang))[1:]).astype(np.int)
# else:
# ly = []
# for dx, dy in zip(lx, ly):
# if self.map_data[dy, dx] == 0:
# beacons_splitted.append(np.array([dx, dy]))
# print(beacons_splitted)
# beacons_splitted = []
# print(np.linalg.norm(vect_dist))
# print('di', np.arange(0, np.linalg.norm(vect_dist), d)[1:])
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)
# print(beacons_splitted)
beacons_splitted.append(np.array(new_pos))
self.splitted_goals = np.array(beacons_splitted)
new_pos = self.splitted_goals[0, :]
# if idx == 5:
# plt.plot(np.append(pose[0], self.splitted_goals[:, 0]), np.append(pose[1], self.splitted_goals[:, 1]),
# '-*b')
# plt.plot([pose[0], new_pos[0]], [pose[1], new_pos[1]], '-*b')
self.splitted_goals = self.splitted_goals[1:, :]
# if idx == 0:
# all_acq, all_grad = gaussian_ei(self.all_vector_pos, self.gp, np.min(self.data[1]), xi=xi,
# return_grad=True)
# all_acq = gaussian_sei(self.vector_pos, self.gp, np.max(self.data[1]), c_point=pose[:2],
# masked=self.acq_mod == "masked")
# best_acqs = self.vector_pos[np.where(all_acq == np.nanmax(all_acq))]
# new_pos = best_acqs[np.random.choice(np.arange(0, len(best_acqs)))]
#
# self.acq_mod = "masked"
#
# all_acq = gaussian_sei(self.vector_pos, self.gp, np.max(self.data[1]), c_point=pose[:2],
# masked=self.acq_mod == "masked")
# best_acqs = self.vector_pos[np.where(all_acq == np.nanmax(all_acq))]
# aux_pos = best_acqs[np.random.choice(np.arange(0, len(best_acqs)))]
# plt.plot(pose[0], pose[1], '^r', markersize=12, label="Current Position")
#
# # beacons_splitted = []
# # vect_dist = np.subtract(new_pos, pose[:2])
# # ang = np.arctan2(vect_dist[1], vect_dist[0])
# # lx = np.round(np.arange(pose[0], new_pos[0], 150 * np.cos(ang))[1:]).astype(np.int)
# # ly = np.round(np.arange(pose[1], new_pos[1], 150 * np.sin(ang))[1:]).astype(np.int)
# # for dx, dy in zip(lx, ly):
# # if self.map_data[dy, dx] == 0:
# # beacons_splitted.append(np.array([dx, dy]))
# # beacons_splitted.append(np.array(new_pos))
# # asdf = np.array(beacons_splitted)
# # plt.plot(asdf[:, 0], asdf[:, 1], 'Xb', markersize=12, label="Next positions")
# # plt.plot(aux_pos[0], aux_pos[1], 'Xb', markersize=12, label="Next Position")
# # plt.plot(new_pos[0], new_pos[1], 'Xk', markersize=12, alpha=0.2, label="Optimal Position")
#
# extent = [0, 999, 0, 1499]
# img, _ = self.get_acq([pose], "gaussian_ei")
# img = img.reshape((1000, 1000)).T
#
# plt.imshow(all_grad.reshape((1000, 1500)), origin='lower', cmap='YlGn_r', extent=extent)
# plt.show(block=True)
# from scipy.spatial.distance import cdist
# # cmap = plt.cm.gray
# # my_cmap = cmap(np.arange(cmap.N))
# # my_cmap[:, -1] = np.linspace(0.8, 0, cmap.N)
# # my_cmap = ListedColormap(my_cmap)
# img = np.exp(-cdist([pose[:2]], self.all_vector_pos) / 250).reshape((1000, 1000)).T
# # for nnan in nans:
# # img[nnan[0], nnan[1]] = -1
# plt.imshow(img, origin='lower', cmap="gray")
#
# plt.legend(loc="upper right", prop={"size": 20})
# cbar = plt.colorbar(orientation='vertical')
# cbar.ax.tick_params(labelsize=20)
# plt.show(block=True)
# plt.plot(new_pos[0], new_pos[1], '^y', markersize=12)
new_pos = np.append(new_pos, 0)
# print('new pos is', new_pos)
return new_pos
def generate_new_goal(self,
pose=np.zeros((1, 3)),
other_poses=np.zeros((1, 3))):
global reg_poly
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)
_, reg = calc_voronoi(pose, other_poses, self.map_data)
reg_poly = Polygon(reg)
res = minimize(self.problem,
self.algorithm, ("n_gen", 150),
verbose=False)
prev_min_dist = 10000
new_pos = pose[:2]
for point in res.X:
curr_dist = np.linalg.norm(np.subtract(point, pose[:2]))
if curr_dist < prev_min_dist:
new_pos = np.round(point).astype(np.int)
prev_min_dist = curr_dist
if True and len(self.train_inputs) > 1:
# from pymoo.visualization.scatter import Scatter
# plot = Scatter()
# plot.add(res.F, color="red")
# plot.show()
plt.style.use("seaborn")
# results = res.F
# img_gen = [
# [0, 1],
# [0, 2]
# ]
# for s in img_gen:
# f1 = results[:, s[0]]
# f4 = results[:, s[1]]
# plt.figure()
# plt.title("Objective Space", fontsize=20)
# plt.plot(f1, f4, 'ob', label=f"Pareto Set: $\\alpha_{s[0] + 1}(x) \\;,\\; \\alpha_{s[1] + 1}(x)$")
# plt.xlabel(f"$\\alpha_{s[0] + 1}(x)$", fontsize=20)
# plt.ylabel(f"$\\alpha_{s[1] + 1}(x)$", fontsize=20)
# plt.xticks(fontsize=20)
# plt.yticks(fontsize=20)
# plt.legend(loc='lower left', prop={'size': 17}, fancybox=True, shadow=True, frameon=True)
xticks = np.arange(0, 1000, 200)
yticks = np.arange(0, 1500, 200)
xnticks = [str(format(num * 10, ',')) for num in xticks]
ynticks = [str(format(num * 10, ',')) for num in yticks]
surr = self.surrogate(keys=["s1"])[0]
# mapz = np.full_like(self.map_data, np.nan)
# for pos in enumerate(self.vector_pos):
# mapz[pos[1][1], pos[1][0]] = surr[pos[0]]
# mapz = np.power(np.subtract(mapz, lol), 2)
plt.imshow(self.map_data, cmap="BuGn_r", origin='lower', zorder=8)
# CS = plt.contour(mapz, colors='k',
# alpha=0.6, linewidths=1.0, zorder=9)
plt.grid(True, zorder=0, color="white")
plt.gca().set_facecolor('#eaeaf2')
# plt.clabel(CS, inline=1, fontsize=10)
plt.xlabel("x (m)", fontsize=20)
plt.ylabel("y (m)", fontsize=20)
plt.xticks(xticks, labels=xnticks, fontsize=20)
plt.yticks(yticks, labels=ynticks, fontsize=20)
k = True
# for point in self.train_inputs:
# if k:
# plt.plot(point[0], point[1], 'yo', zorder=10, label='Previous Locations')
# k = False
# else:
# plt.plot(point[0], point[1], 'yo', zorder=10)
plt.plot(pose[0],
pose[1],
'ro',
zorder=10,
label='Current Location')
# k = True
# for pareto in res.X:
# pareto = np.round(pareto).astype(np.int)
# if self.map_data[pareto[1], pareto[0]] == 0:
# if k:
# plt.plot(pareto[0], pareto[1], 'Xb', zorder=10, label="Pareto Points")
# k = False
# else:
# plt.plot(pareto[0], pareto[1], 'Xb', zorder=10)
# plt.plot(new_pos[0], new_pos[1], 'Xg', zorder=10, label='Closest Pareto Point')
plt.plot(np.append(reg[:, 0], reg[0, 0]),
np.append(reg[:, 1], reg[0, 1]),
'-b',
zorder=10)
# plt.show(block=True)
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 = 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:, :]
if len(self.train_inputs) > 1:
# plt.plot(new_pos[0], new_pos[1], 'Xk', zorder=10, label='Next Meas. Location')
plt.legend(loc='lower left',
prop={'size': 17},
fancybox=True,
shadow=True,
frameon=True)
plt.show(block=True)
new_pos = np.append(new_pos, 0)
return new_pos