def vis_ray_lookup(self, r_idx): hm = self.hit_map.copy() canvas = 255 * numpy.dstack((numpy.zeros_like(hm), hm, hm)).astype('uint8') dists = self.ray_lookup[r_idx, :] angles = numpy.arange(0, 2*numpy.pi, self.angle_bins_step) coords = [] start_coord = self.valid_coordinates[r_idx] color = [255, 0, 0] canvas[start_coord[0], start_coord[1], :] = [0, 255, 0] for angle_idx in range(self.ray_lookup.shape[1]): angle = angles[angle_idx] dist = dists[angle_idx] pose_delt = dist * numpy.array((math.cos(angle), math.sin(angle), 0.0)) / self.resolution new_coord = map(int, [start_coord[0] + pose_delt[0], start_coord[1] + pose_delt[1]]) new_xs, new_ys = eight_neighborhood(new_coord[0], new_coord[1]) canvas[new_xs, new_ys, :] = color xs, ys = eight_neighborhood(start_coord[0], start_coord[1]) # canvas[start_coord[0], start_coord[1], :] = [0, 255, 0] canvas[xs, ys, :] = [0, 255, 0] iu.v(canvas)
def vis_ray_lookup(self, r_idx): hm = self.hit_map.copy() canvas = 255 * numpy.dstack( (numpy.zeros_like(hm), hm, hm)).astype('uint8') dists = self.ray_lookup[r_idx, :] angles = numpy.arange(0, 2 * numpy.pi, self.angle_bins_step) coords = [] start_coord = self.valid_coordinates[r_idx] color = [255, 0, 0] canvas[start_coord[0], start_coord[1], :] = [0, 255, 0] for angle_idx in range(self.ray_lookup.shape[1]): angle = angles[angle_idx] dist = dists[angle_idx] pose_delt = dist * numpy.array( (math.cos(angle), math.sin(angle), 0.0)) / self.resolution new_coord = map( int, [start_coord[0] + pose_delt[0], start_coord[1] + pose_delt[1]]) new_xs, new_ys = eight_neighborhood(new_coord[0], new_coord[1]) canvas[new_xs, new_ys, :] = color xs, ys = eight_neighborhood(start_coord[0], start_coord[1]) # canvas[start_coord[0], start_coord[1], :] = [0, 255, 0] canvas[xs, ys, :] = [0, 255, 0] iu.v(canvas)
def vis_particles(self, particles): hm = self.hit_map.copy() canvas = 255 * numpy.dstack((numpy.zeros_like(hm), hm, hm)).astype('uint8') color = [255, 0, 0] for p in particles: coord = self.get_pose_coord(p.pose) xs, ys = eight_neighborhood(coord[0], coord[1]) xs = np.asarray(xs) ys = np.asarray(ys) xs[xs >= self.mapsize_x] = self.mapsize_x - 1 ys[ys >= self.mapsize_y] = self.mapsize_y - 1 xs[xs < 0] = 0 ys[ys < 0] = 0 canvas[xs, ys, :] = color iu.v(canvas)
def vis_particles(self, particles): hm = self.hit_map.copy() canvas = 255 * numpy.dstack( (numpy.zeros_like(hm), hm, hm)).astype('uint8') color = [255, 0, 0] for p in particles: coord = self.get_pose_coord(p.pose) xs, ys = eight_neighborhood(coord[0], coord[1]) xs = np.asarray(xs) ys = np.asarray(ys) xs[xs >= self.mapsize_x] = self.mapsize_x - 1 ys[ys >= self.mapsize_y] = self.mapsize_y - 1 xs[xs < 0] = 0 ys[ys < 0] = 0 canvas[xs, ys, :] = color iu.v(canvas)
def show_hit_map(self): iu.v(self.hit_map)
def show(self): iu.v((self.grid + 1.0) / 2.0)