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 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 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 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_circle(self, viz, px, rad=MC.ORAD, color=BLUE):
cv2.circle(viz, point(px), rad, tuple([int(c) for c in color]), thickness=-1)
return viz