def test_motion_model():
### Generate test input
np.random.seed(10000)
np.set_printoptions(precision=4, suppress=True)
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[:, :2] *= 10
h, w = occupancy_map.shape
flipped_occupancy_map = occupancy_map[::-1, :]
y, x = np.where(flipped_occupancy_map == 0)
valid_indices = np.where((350 <= y) & (y <= 450) & (350 <= x) & (x <= 450))[0]
ind = indices = np.random.choice(valid_indices, 1)[0]
y, x = y[ind], x[ind]
# y, x = ind // w, ind % w
theta = np.pi / 4
X = np.array([[x, y, theta]])
X[:, :2] *= 10
print(X)
# define motion control u, test on shift-only
theta1 = np.pi / 2
u_t1 = np.array([1049.833130, 28.126379, -2.664653])
u_t2 = np.array([1048.641235, 27.560303, -2.659024])
# motion model
motion = MotionModel()
X_t2 = motion.update(u_t1, u_t2, X)
print(X_t2)
print(f'X: {X}, X_t2: {X_t2}')
# plot
plt.figure(1)
plt.imshow(occupancy_map, cmap='Greys')
plt.axis([0, 800, 0, 800])
x_loc = X[:, 0] / 10.
y_loc = X[:, 1] / 10.
plt.scatter(x_loc, y_loc, c='r', marker='o')
x_loc = X_t2[:, 0] / 10.
y_loc = X_t2[:, 1] / 10.
plt.scatter(x_loc, y_loc, c='y', marker='o')
plt.show()
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 __init__(self, bag_info):
bag_name, map_name, _, _ = bag_info
self.map_reader = MapReader(map_name)
# Cameras
self.cam_ids = [1]
self.cameras = [
Camera(cam_id, camera_calibs[cam_id]) for cam_id in self.cam_ids
]
# Visualizers
self.camera_visualizers = {
camera.cam_id: CameraPoseVisualizer(camera)
for camera in self.cameras
}
def test_ground_truth_motion():
# Read map
np.random.seed(10000)
np.set_printoptions(precision=4, suppress=True)
map_obj = MapReader('../data/map/wean.dat')
occupancy_map = map_obj.get_map()
# specify ground truth location
X = np.array([[4200, 4000, np.pi*(170/180.)]])
motion_model = MotionModel()
with open('../data/log/robotdata1.log', 'r') as f:
logfile = f.readlines()
visualize_map(occupancy_map)
visualize_timestep(X, 0, '../tmp')
first_time_idx = True
for time_idx, line in enumerate(logfile):
meas_type = line[0]
meas_vals = np.fromstring(line[2:], dtype=np.float64, sep=' ')
odometry_robot = meas_vals[0:3]
time_stamp = meas_vals[-1]
if (meas_type == "L"):
# [x, y, theta] coordinates of laser in odometry frame
odometry_laser = meas_vals[3:6]
# 180 range measurement values from single laser scan
ranges = meas_vals[6:-1]
# print("Processing time step {} at time {}s".format(
# time_idx, time_stamp))
if first_time_idx:
u_t0 = odometry_robot
first_time_idx = False
continue
if meas_type == "O":
continue
u_t1 = odometry_robot
# Update motion
X = motion_model.update(u_t0, u_t1, X)
u_t0 = u_t1
visualize_timestep(X, time_idx, '../tmp')
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)
class LocalizationVisualizer:
def __init__(self, bag_info):
bag_name, map_name, _, _ = bag_info
self.map_reader = MapReader(map_name)
# Cameras
self.cam_ids = [1]
self.cameras = [
Camera(cam_id, camera_calibs[cam_id]) for cam_id in self.cam_ids
]
# Visualizers
self.camera_visualizers = {
camera.cam_id: CameraPoseVisualizer(camera)
for camera in self.cameras
}
def visualize(self, data, prediction):
vis_images = {}
# Get landmarks
localization = prediction['localization']
landmarks = self.map_reader.get_landmarks(localization)
# Visualize each camera fitting
for cam_id in self.cam_ids:
img = data[cam_id]
pose_info = {}
pose_info['localization'] = localization
pose_info['tcw'] = prediction['cam{}_tcw'.format(cam_id)]
cam_visualizer = self.camera_visualizers[cam_id]
vis_images[cam_id] = cam_visualizer.visualize(
img, pose_info, landmarks)
return vis_images
def show(self, vis_images):
for name in vis_images:
cv.imshow(name, vis_images[name])
cv.waitKey(1)
dist[oor_indices] = (self._max_range / 10.)
indices.append(oor_indices)
indices = np.concatenate(indices)
mask = np.zeros((X.shape[0], ), dtype=np.bool)
mask[indices] = True
return mask
def log_sum(p):
return np.sum(np.log(p), axis=1)
if __name__ == '__main__':
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
angle = np.pi * (95. / 180)
sensor = SensorModel(occupancy_map)
z_t_star = sensor.ray_casting_one_direction((x, y, theta), occupancy_map,
angle)
x0, y0 = x, y
import numpy as np
import math
import time
from matplotlib import pyplot as plt
from scipy.stats import norm
from scipy.stats import expon
import pdb
from map_reader import MapReader
from sensor_model import SensorModel
from collections import defaultdict
src_path_map = '../data/map/wean.dat'
map_obj = MapReader(src_path_map)
occupancy_map = map_obj.get_map()
_min_probability = 0.35
_max_range = 1000
bool_occ_map = (occupancy_map < _min_probability) & (occupancy_map >= 0)
walk_stride = 5
def ray_cast(laser_x_t1, laser_y_t1, laser_theta, walk_stride):
x_end = laser_x_t1
y_end = laser_y_t1
x_idx = min(int(np.around(x_end / 10)), 799)
y_idx = min(int(np.around(y_end / 10)), 799)
temp_location = bool_occ_map[y_idx][x_idx]