def pixel_rays(mapshape, disc_step, disc_angle):
# Find intersection of points with the wallsi
points_start, angles, shape = sample_uniform(disc_step, disc_angle)
points_end = interesection_with_walls(points_start, angles)
# Convert to pixels
pixels_start = pose2pixel(points_start, mapshape)
pixels_end = pose2pixel(points_end, mapshape)
# Extract coordinates of lines between start and end points i.e. the rays
pixel_lines = [line(*p1, *p2) for p1, p2 in zip(pixels_start, pixels_end)]
return pixel_lines, shape
def draw_orientation(self, viz, pose, thickness=10):
# Start and end pose for arrow
stp, enp = self.orientation_vector(pose)
# Convert to pixel
st_pix = pose2pixel(stp, viz.shape)
en_pix = pose2pixel(enp, viz.shape)
# Draw orientation
cv2.arrowedLine(viz, point(st_pix), point(en_pix), GREEN, thickness=thickness)
return viz
def __init__(self, scanId, show_rays=True, show_grid=True):
self.scanId = scanId
self.root, self.target, agent_pose, goal_pose = scanId.split('-')
self.root = os.path.join(Files.basedir, dirname(self.root))
self.agent_pose_ini = to_array(agent_pose)
self.goal_pose_ini = to_array(goal_pose)
# Cache variables
self.pmc_var = None
# Visualization settnigs
self.show_rays = show_rays
self.show_grid = show_grid
# Floyd warshall algorithm for shortest path supervision
self.mapfile_var = join(self.root, Files.objfile)
self.planner = FloydWarshallRobotslang(self.mapfile_var)
# Navigation pixels for visualization
self.nav_pixels = self.planner.navigable_pixels
## Target poses
self.target_poses = get_targets()[scanId]
self.target_pixels = pose2pixel(np.asarray(self.target_poses), MC.mazeshape)
self.target_pixels = list(zip(["jar", "mug", "paddle"], self.target_pixels))
# Randomize location of agent and goal
self.agent = Particles(1)
self.goal = Particles(1)
# Start by resetting
self.reset()
def draw_rays(self, viz, pose):
stp, enp = self.orientation_vector(pose)
st_pix = pose2pixel(stp, viz.shape)
out = self.pmc.get_rays(pose)
for o in out:
cv2.line(viz, point(st_pix), point(o), WHITE, thickness=4)
return viz
def heatmap(self, viz):
viz = viz * 0
planner = self.env.planner
particles = Particles(planner.poses.shape[0])
particles.pose[:,:2] = planner.poses[:,:2]
particles.pose[:, 2] = self.env.agent.pose[:,2]
pixels = pose2pixel(particles.pose, MC.mazeshape)
weights = np.zeros(len(particles))
measurement = self.env.get_visual_obs(self.env.agent.pose)
for i, (pose, pix) in enumerate(zip(particles.pose, pixels)):
pose_measurement = self.env.get_visual_obs(pose)
weights[i] = (pose_measurement == measurement).sum()
#print(weights.max(), weights.min(), weights.mean())
#weights = softmax(weights, t=.05)
for i, (pose, pix) in enumerate(zip(particles.pose, pixels)):
c_indx = int((self.num_cgs-1) * weights[i]/weights.max())
color = color2bgr(self.color_gradient[c_indx])
self.env.draw_circle(viz, pix, rad=20, color=color)
self.env.draw_orientation(viz, pose, thickness=2)
for i, (pose, pix) in enumerate(zip(particles.pose, pixels)):
cv2.putText(viz, str(int(weights[i])), point(pix), cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2, cv2.LINE_AA)
return viz
def grid(self, radius):
# Make a grid
x = np.arange(0, MC.width, radius)
y = np.arange(0, MC.height, radius)
xx, yy = np.meshgrid(x, y)
xx, yy = xx.flatten(), yy.flatten()
poses = np.stack([xx, yy], 1)
pixels = pose2pixel(poses, MC.mazeshape)
return poses, pixels
def display(self):
viz = self.env.display().copy()
pixels = pose2pixel(self.particles.pose, MC.mazeshape)
for i, (pose, pixel) in enumerate(zip(self.particles.pose, pixels)):
#viz = self.env.draw_rays(viz, pose)
#viz = self.env.draw_orientation(viz, pose, thickness=2)
norm = self.particles.weights[i]/self.particles.weights.max()
c_indx = int((self.num_cgs-1) * norm)
color = color2bgr(self.color_gradient[c_indx])
viz = self.env.draw_circle(viz, pixel, rad=5, color=color)
# Make a heatmap
heatmap = self.heatmap(viz)
out = np.concatenate((viz, heatmap), 1)
show(out, 30)
return out
def draw_trajectory(self, viz, traj_poses, color=(59,181,207)):
if traj_poses is not None:
traj_pix = pose2pixel(traj_poses, viz.shape)
for i in range(len(traj_pix)-1):
cv2.line(viz, point(traj_pix[i]), point(traj_pix[i+1]), color, thickness=4)
return viz
def draw_agent(self, viz, pose):
stp, enp = self.orientation_vector(pose)
st_pix = pose2pixel(stp, viz.shape)
self.draw_circle(viz, st_pix, rad=MC.PRAD*2, color=BLUE)