def write_traj_map(self, trajmap: TrajectoryMapManager):
full_map = trajmap.traj_map
half_size = 16 # half of map size, in map size
if not self.rotate:
center_pos = trajmap.real2map(self.traj_train[-1])
else:
center_pos = trajmap.real2map(self.traj_original[self.obs_length])
original_map = cv2.resize(
full_map[np.maximum(center_pos[0] - 2 * half_size, 0):np.
minimum(center_pos[0] + 2 * half_size, full_map.shape[0]),
np.maximum(center_pos[1] - 2 * half_size, 0):np.
minimum(center_pos[1] +
2 * half_size, full_map.shape[1]), ],
(4 * half_size, 4 * half_size))
final_map = original_map[half_size:3 * half_size,
half_size:3 * half_size]
if self.reverse:
final_map = np.flip(original_map[half_size:3 * half_size,
half_size:3 * half_size])
if self.rotate:
final_map = cv2.warpAffine(
original_map,
cv2.getRotationMatrix2D(
(2 * half_size, 2 * half_size),
self.rotate,
1,
),
(4 * half_size, 4 * half_size),
)
final_map = final_map[half_size:3 * half_size,
half_size:3 * half_size]
self.traj_map = final_map
def write_traj_map_for_neighbors(self, trajmap: TrajectoryMapManager):
self.traj_map_neighbors = []
full_map = trajmap.traj_map
half_size = 16
for nei_traj in self.get_neighbor_traj():
center_pos = trajmap.real2map(nei_traj[-1, :])
original_map = cv2.resize(
full_map[np.maximum(center_pos[0] - 2 * half_size, 0):np.
minimum(center_pos[0] +
2 * half_size, full_map.shape[0]),
np.maximum(center_pos[1] - 2 * half_size, 0):np.
minimum(center_pos[1] +
2 * half_size, full_map.shape[1]), ],
(4 * half_size, 4 * half_size))
final_map = original_map[half_size:3 * half_size,
half_size:3 * half_size]
self.traj_map_neighbors.append(final_map)