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)
def show(self):
iu.v((self.grid + 1.0) / 2.0)
def show_hit_map(self):
iu.v(self.hit_map)
