def test_hyperparameter(): np.random.seed(10008) map_obj = MapReader('../data/map/wean.dat') occupancy_map = map_obj.get_map() # generate a random particle, then sent out beams from that location # indices = np.where(occupancy_map.flatten() == 0)[0] # ind = np.random.choice(indices, 1)[0] # y, x = ind // w, ind % w # theta = -np.pi / 2 # angle = np.pi * (40 / 180) X = init_particles_freespace(1, occupancy_map) sensor = SensorModel(occupancy_map) X = X.reshape(1, -1) z_t_star = sensor.ray_casting(X[:, :3], num_beams=10) print(z_t_star) print(z_t_star.max()) z = np.arange(sensor._max_range + 2).astype(np.float) p_hit, p_short, p_max, p_rand = sensor.estimate_density(z, z_t_star[0][5]) # plot(1, p_hit) # plot(2, p_short) # plot(3, p_max) # plot(4, p_rand) # w_hit = 3 # 99 / 2 / 2.5 / 4 # 1. # w_short = 0.05 # 2 * 198 / 4 / 2.5 / 4 # 1 # w_max = 0.1 # 49 / 2.5 / 4 # 0.5 # w_rand = 10 # 990 / 4 # 5 # self._z_hit = 99 / 2 / 2.5 / 4 # 1. # self._z_short = 198 / 4 // 2.5 / 4 # 1 # self._z_max = 49 / 2.5 / 4 # 0.5 # self._z_rand = 990 / 4 # 5 # w_hit = 1. # w_short = 0.1 # w_max = 0.5 # w_rand = 10 w_hit = 1000 # 99 / 2 / 2.5 / 4 # 1. w_short = 0.01 # 2 * 198 / 4 / 2.5 / 4 # 1 w_max = 0.03 # 49 / 4 / 4 # 0.5 w_rand = 12500 p = sensor._z_hit * p_hit + sensor._z_short * p_short + sensor._z_max * p_max + sensor._z_rand * p_rand plot(5, p) plt.show()
def test_raycasting_vectorize(): np.random.seed(10008) map_obj = MapReader('../data/map/wean.dat') occupancy_map = map_obj.get_map() # generate a random particle, then sent out beams from that location h, w = occupancy_map.shape indices = np.where(occupancy_map.flatten() == 0)[0] ind = np.random.choice(indices, 1)[0] y, x = ind // w, ind % w theta = np.pi / 2 X = np.array([[x, y, theta]]) X = np.repeat(X, 2, axis=0) X[:, :2] *= 10 num_beams = 180 sensor = SensorModel(occupancy_map) z_t_star = sensor.ray_casting(X, num_beams=num_beams) x0, y0 = X[0, :2] angle = np.arange(num_beams) * (np.pi / num_beams) angle = theta + angle - np.pi / 2 x1 = x0 + z_t_star * np.cos(angle) y1 = y0 - z_t_star * np.sin(angle) x0, y0 = x0 / 10, y0 / 10 x1, y1 = x1 / 10, y1 / 10 # plot figure fig = plt.figure() plt.imshow(occupancy_map) plt.scatter(x0, y0, c='red') plt.scatter(x1, y1, c='yellow') print(f'(x0, y0): ({x0}, {y0}), (x1, y1): ({x1}, {y1})') # plt.plot((x0, x1), (y0, y1), color='yellow') plt.show() print(z_t_star)