def get_acq(self, pose=np.zeros((1, 2)), acq_func="gaussian_sei"):
if acq_func == "gaussian_sei":
return gaussian_sei(self.all_vector_pos,
self.gp,
np.min(self.data[1]),
c_point=pose[:2],
masked=self.acq_mod == "masked",
xi=1.0), self.main_sensor
elif acq_func == "maxvalue_entropy_search":
return maxvalue_entropy_search(
self.all_vector_pos,
self.gp,
np.min(self.data[1]),
c_point=pose[:2],
masked=self.acq_mod == "masked"), self.main_sensor
elif acq_func == "gaussian_pi":
return gaussian_pi(
self.all_vector_pos,
self.gp,
np.min(self.data[1]),
masked=self.acq_mod == "masked"), self.main_sensor
elif acq_func == "gaussian_ei":
return gaussian_ei(self.all_vector_pos,
self.gp,
np.min(self.data[1]),
c_point=pose[:2],
masked=False,
xi=1.0), self.main_sensor
def get_acq(self, pose=np.zeros((1, 2)), acq_func="gaussian_sei"):
if acq_func == "gaussian_sei":
return gaussian_sei(self.all_vector_pos,
self.gp,
np.max(self.data[1]),
c_point=pose[0][:2]), self.main_sensor
elif acq_func == "maxvalue_entropy_search":
return maxvalue_entropy_search(
self.all_vector_pos,
self.gp,
np.max(self.data[1]),
c_point=pose[0][:2]), self.main_sensor
elif acq_func == "gaussian_pi":
return gaussian_pi(self.all_vector_pos, self.gp,
np.max(self.data[1])), self.main_sensor
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)), idx=-1):
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
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")
# all_acq = gaussian_ei(self.vector_pos, self.gp, np.min(self.data[1]), xi=xi, return_grad=False)
new_pos = self.vector_pos[np.where(all_acq == np.nanmax(all_acq))][0]
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])
lx = np.round(
np.arange(pose[0], new_pos[0],
220 * np.cos(ang))[1:]).astype(np.int)
ly = np.round(
np.arange(pose[1], new_pos[1],
220 * 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))
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