def particle_SLAM(src_dir,
dataset_id=0,
split_name='train',
running_mode='test_SLAM',
log_dir='logs'):
'''Your main code is here.
'''
###############################################################################################
#* Student's input
#TODO: change the resolution of the map - the distance between two cells (meters)
map_resolution = 0.05
# Number of particles
#TODO: change the number of particles
num_p = 100
#TODO: change the process' covariance matrix
mov_cov = np.array([[1e-7, 0, 0], [0, 1e-7, 0], [0, 0, 1e-7]])
#TODO: set a threshold value of probability to consider a map's cell occupied
p_thresh = 0.7
#TODO: change the threshold of the percentage of effective particles to decide resampling
percent_eff_p_thresh = 0.05
#*End student's input
###############################################################################################
# Test prediction
if running_mode == 'test_prediction':
test_prediction(src_dir, dataset_id, split_name, log_dir)
exit(1)
if running_mode == 'test_update':
test_update(src_dir, dataset_id, split_name, log_dir, map_resolution)
exit(1)
# Test SLAM
# Create a SLAM instance
slam_inc = SLAM()
# Read data
slam_inc._read_data(src_dir, dataset_id, split_name)
num_steps = slam_inc.num_data_
# Characterize the sensors' specifications
slam_inc._characterize_sensor_specs(p_thresh)
# Initialize particles
slam_inc._init_particles(num_p,
mov_cov,
percent_eff_p_thresh=percent_eff_p_thresh)
# Iniitialize the map
slam_inc._init_map(map_resolution)
# Starting time index
t0 = 0
# Initialize the particle's poses using the lidar measurements at the starting time
slam_inc.particles_[:, 0] = slam_inc.lidar_.data_[t0]['pose'][0]
# Indicator to notice that map has not been built
build_first_map = False
# iterate next time steps
all_particles = deepcopy(slam_inc.particles_)
num_resamples = 0
for t in tqdm.tqdm(range(t0, num_steps - t0)):
# Ignore lidar scans that are obtained before the first IMU
if slam_inc.lidar_.data_[t]['t'][0][0] - slam_inc.joints_.data_['ts'][
0][0] < 0:
continue
if not build_first_map:
slam_inc._build_first_map(t)
t0 = t
build_first_map = True
continue
# Prediction
slam_inc._predict(t)
# Update
slam_inc._update(t, t0=t0, fig='off')
# Resample particles if necessary
num_eff = 1.0 / np.sum(np.dot(slam_inc.weights_, slam_inc.weights_))
logging.debug('>> Number of effective particles: %.2f' % num_eff)
if num_eff < slam_inc.percent_eff_p_thresh_ * slam_inc.num_p_:
num_resamples += 1
logging.debug('>> Resampling since this < threshold={0}| Resampling times/t = {1}/{2}'.format(\
slam_inc.percent_eff_p_thresh_*slam_inc.num_p_, num_resamples, t-t0 + 1))
[slam_inc.particles_,slam_inc.weights_] = prob.stratified_resampling(\
slam_inc.particles_,slam_inc.weights_,slam_inc.num_p_)
# Plot the estimated trajectory
if (t - t0 + 1) % 1000 == 0 or t == num_steps - 1:
# Save the result
log_file = log_dir + '/SLAM_' + split_name + '_' + str(
dataset_id) + '.pkl'
try:
with open(log_file, 'wb') as f:
pickle.dump(slam_inc, f, pickle.HIGHEST_PROTOCOL)
print(">> Save the result to: %s" % log_file)
except Exception as e:
print('Unable to write data to', log_file, ':', e)
raise
# Gen map + trajectory
MAP_2_display = genMap(slam_inc, t)
MAP_fig_path = log_dir + '/processing_SLAM_map_' + split_name + '_' + str(
dataset_id) + '.jpg'
cv2.imwrite(MAP_fig_path, MAP_2_display)
plt.title('Estimated Map at time stamp %d/%d' %
(t, num_steps - t0 + 1))
plt.imshow(MAP_2_display)
plt.pause(0.01)
logging.debug(">> Save %s" % MAP_fig_path)
# Return best_p which are an array of size 3xnum_data that represents the best particle over the whole time stamp
return slam_inc.best_p_
def particle_SLAM(src_dir, dataset_id=0, split_name='train', running_mode='test_SLAM', log_dir='logs'):
'''Your main code is here.
'''
###############################################################################################
#* Student's input
#TODO: change the resolution of the map - the distance between two cells (meters)
map_resolution = 0.05
# Number of particles
#TODO: change the number of particles
num_p = 100
#TODO: change the process' covariance matrix
mov_cov = np.array([[1e-8, 0, 0],[0, 1e-8, 0],[0, 0 , 1e-8]])
#TODO: set a threshold value of probability to consider a map's cell occupied
p_thresh = 0.6
#TODO: change the threshold of the percentage of effective particles to decide resampling
percent_eff_p_thresh = 0.5
#*End student's input
###############################################################################################
# Test prediction
if running_mode == 'test_prediction':
test_prediction(src_dir, dataset_id, split_name, log_dir)
exit(1)
if running_mode == 'test_update':
test_update(src_dir, dataset_id, split_name, log_dir, map_resolution)
exit(1)
# Test SLAM
# Create a SLAM instance
slam_inc = SLAM()
# with open('SLAM_train_1.pkl', 'rb') as pickle_file:
# slam_inc = pickle.load(pickle_file)
slam_inc.psx = 0
slam_inc.psy = 0
slam_inc.bresenDict = {}
slam_inc.dict_use_count = 0
# Read data
slam_inc._read_data(src_dir, dataset_id, split_name)
num_steps = int((slam_inc.num_data_)//1)
# Characterize the sensors' specifications
slam_inc._characterize_sensor_specs(p_thresh)
# Initialize particles
mov_cov = np.diag([0.1,0.1,0.1])/1000
slam_inc._init_particles(num_p, mov_cov, percent_eff_p_thresh=percent_eff_p_thresh)
# Iniitialize the map
slam_inc._init_map(map_resolution)
# Starting time index
t0 = 0
# Initialize the particle's poses using the lidar measurements at the starting time
slam_inc.particles_[:,0] = slam_inc.lidar_.data_[t0]['pose'][0]
# slam_inc.particles_[2,:] = slam_inc.particles_[2,:] +np.pi/12
# slam_inc.particles_[:2,:] = slam_inc.particles_[:2,:] + 0.1
# Indicator to notice that map has not been built
build_first_map = False
# iterate next time steps
all_particles = deepcopy(slam_inc.particles_)
num_resamples = 0
# num_steps = t0+400
for t in tqdm.tqdm(range(t0, num_steps)):
# Ignore lidar scans that are obtained before the first IMU
if slam_inc.lidar_.data_[t]['t'][0][0] - slam_inc.joints_.data_['ts'][0][0] < 0:
# print("skipping")
continue
if not build_first_map:
slam_inc._build_first_map(t)
# print("first mappp")
# t0 = t
# print(map_resolution)
build_first_map = True
continue
# MAP_2_display = genMap(slam_inc, t)
# MAP_fig_path = log_dir + '/0000processing_SLAM_map_t_9_34_a'+str((t-t0+1))+ split_name + '_' + str(dataset_id) + '.jpg'
# cv2.imwrite(MAP_fig_path, MAP_2_display)
# if (t-t0)<5000:
# slam_inc.mov_cov = np.diag([0.1,0.1,1])/10
# slam_inc.particles_[0,:] = slam_inc.particles_[0,:] - 0.001
# else:
# slam_inc.mov_cov = np.diag([0.1,0.1,0.1])/1000
# Prediction
slam_inc._predict(t)
# Update
slam_inc._update(t,t0=t0,fig='off')
# Resample particles if necessary;
num_eff = 1.0/np.sum(np.dot(slam_inc.weights_,slam_inc.weights_))
logging.debug('>> Number of effective particles: %.2f'%num_eff)
if num_eff < slam_inc.percent_eff_p_thresh_*slam_inc.num_p_:
num_resamples += 1
logging.debug('>> Resampling since this < threshold={0}| Resampling times/t = {1}/{2}'.format(\
slam_inc.percent_eff_p_thresh_*slam_inc.num_p_, num_resamples, t-t0 + 1))
[slam_inc.particles_,slam_inc.weights_] = prob.stratified_resampling(\
slam_inc.particles_,slam_inc.weights_,slam_inc.num_p_)
# Plot the estimated trajectory
if (t - t0 + 1)%1000 == 0 or t==num_steps-1:
print(t-t0)
print("use dict for ", slam_inc.dict_use_count)
# Save the result
log_file = log_dir + '/SLAM_' + split_name + '_' + str(dataset_id) + '.pkl'
try:
with open(log_file, 'wb') as f:
pickle.dump(slam_inc,f,pickle.HIGHEST_PROTOCOL)
print(">> Save the result to: %s"%log_file)
except Exception as e:
print('Unable to write data to', log_file, ':', e)
raise
# Gen map + trajectory
MAP_2_display = genMap(slam_inc, t)
MAP_fig_path = log_dir + '/processing_SLAM_map_t_6_51_p_W'+ split_name + '_' + str(dataset_id) + '.jpg'
#12.21 a: np.diag([0.1,0.1,1])/10000
#11.51 p: np.diag([0.1,0.1,1])/1000
#12.33 a: np.diag([0.1,0.1,1])/100000
#1.07 a: np.diag([0.1,0.1,1])/100
#1.30 a: np.diag([0.1,0.1,0.1])/100
#1.40 a: np.diag([0.1,0.1,0.1])/1000
#9.32 a: np.diag([0.1,0.1,0.1])/10000
#9.48 a: np.diag([0.1,0.1,0.1])/1000 (only noise)
#10.33 a: np.diag([0.1,0.1,0.1])/100 (only noise)
#12.28 p: np.diag([0.1,0.1,0.1])/100 (only noise, lower resampling)
#12.51 p: np.diag([0.1,0.1,0.1])/1000 (only noise, lower resampling, sin positive, vectorized) #WORKSSSSS
#7.31 p : reloaded pickle from 19500
#11.32 p: reloaded pickle from 19200
#3.03 a: trying more hack with x,y from 19800
cv2.imwrite(MAP_fig_path, MAP_2_display)
plt.title('Estimated Map at time stamp %d/%d'%(t, num_steps - t0 + 1))
plt.imshow(MAP_2_display)
plt.pause(0.01)
logging.debug(">> Save %s"%MAP_fig_path)
# Return best_p which are an array of size 3xnum_data that represents the best particle over the whole time stamp
return slam_inc.best_p_
def _update(self, t, t0=0, fig='on'):
"""Update function where we update the """
if t == t0:
self._build_first_map(t0, use_lidar_yaw=True)
return
#TODO: student's input from here
n_thresh = 5
MAP = self.MAP_
#trajectory update
ranges = self.lidar_.data_[t]["scan"][0]
lidar_t = self.lidar_.data_[t]["t"]
lidar_pose = self.lidar_.data_[t]["pose"][0]
#lidar_yaw = self.lidar_.data_[self.t0]["rpy"][0][2]
joint_ind = np.abs(self.joints_.data_["ts"][0] -
lidar_t).argmin() #time sync
angles = np.array([np.arange(-135, 135.25, 0.25) * np.pi / 180.])
#limit scan range
indv_range = np.logical_and((ranges < 30), (ranges > 0.1))
ranges = ranges[indv_range]
angles = angles[0]
angles = angles[indv_range]
x_lidar = ranges * np.cos(angles)
y_lidar = ranges * np.sin(angles)
length = len(x_lidar)
z_lidar = np.zeros(length)
w_lidar = np.ones(length)
p_lidar = np.vstack([x_lidar, y_lidar, z_lidar, w_lidar])
neck_angle = self.joints_.data_["head_angles"][0][joint_ind]
head_angle = self.joints_.data_["head_angles"][1][joint_ind]
#lidar2body
body_2_lidar_rot = np.dot(tf.rot_z_axis(neck_angle),
tf.rot_y_axis(head_angle))
# Transition from the body to the lidar frame (lidar is 15cm above the head. See config_slam.pdf for more details)
body_2_lidar_trans = np.array([0, 0, 0.15])
body_2_lidar_homo = tf.homo_transform(body_2_lidar_rot,
body_2_lidar_trans)
p_body = np.dot(body_2_lidar_homo, p_lidar)
corr = np.zeros(self.num_p_)
for i in range(self.num_p_):
pose_i = self.particles_[:, i]
world_2_body_rot = tf.rot_z_axis(pose_i[2])
world_2_body_trans = np.array([pose_i[0], pose_i[1], 0.93])
world_2_part_homo = tf.homo_transform(world_2_body_rot,
world_2_body_trans)
p_world_est = np.dot(world_2_part_homo, p_body)
ground_ind = np.argwhere(p_world_est[2] < 0.1)
p_world_est = np.delete(p_world_est, ground_ind, 1)
p_world_est = p_world_est[0:2, :]
Ex = np.ceil(
(p_world_est[0, :] - MAP['xmin']) / MAP['res']).astype(
np.int16) - 1
Ey = np.ceil(
(p_world_est[1, :] - MAP['ymin']) / MAP['res']).astype(
np.int16) - 1
obst = np.vstack([Ex, Ey])
corr[i] = prob.mapCorrelation(MAP["map"], obst)
#update weights
self.weights_ = prob.update_weights(self.weights_, corr)
max_ = np.argmax(self.weights_)
max_pose = self.particles_[:, max_]
self.best_p_[:, t] = max_pose
ind_x = np.ceil(
(max_pose[0] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
ind_y = np.ceil(
(max_pose[1] - MAP['ymin']) / MAP['res']).astype(np.int16) - 1
self.best_p_indices_[0, t] = ind_x
self.best_p_indices_[1, t] = ind_y
if t == 1:
self.best_p_indices_[0, t - 1] = ind_x
self.best_p_indices_[1, t - 1] = ind_y
n_eff = 1.0 / (np.sum(self.weights_)**2)
if n_eff < self.percent_eff_p_thresh_ * self.num_p_:
self.particles_, self.weights_ = prob.stratified_resampling(
self.particles_, self.weights_, self.num_p_)
#self.time_ = np.empty(self.num_data_)
#map_update
world_2_body_rot = tf.rot_z_axis(max_pose[2])
world_2_body_trans = np.array([max_pose[0], max_pose[1], 0.93])
world_2_part_homo = tf.homo_transform(world_2_body_rot,
world_2_body_trans)
p_world_est = np.dot(world_2_part_homo, p_body)
ground_ind = np.argwhere(p_world_est[2] < 0.1)
p_world_est = np.delete(p_world_est, ground_ind, 1)
p_world_est = p_world_est[0:2, :]
#Ex = np.ceil((p_world_est[0,:] - MAP['xmin']) / MAP['res'] ).astype(np.int16)-1
#Ey = np.ceil((p_world_est[1,:] - MAP['ymin']) / MAP['res'] ).astype(np.int16)-1
#map into map index
sx = np.ceil(
(max_pose[0] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
sy = np.ceil(
(max_pose[1] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
Ex = np.ceil((p_world_est[0, :] - MAP['xmin']) / MAP['res']).astype(
np.int16) - 1
Ey = np.ceil((p_world_est[1, :] - MAP['ymin']) / MAP['res']).astype(
np.int16) - 1
#brehensam 2D
num = len(Ex)
for i in range(num):
r = bresenham2D(sx, sy, Ex[i], Ey[i])
r = r.astype(np.int16)
self.log_odds_[r[0], r[1]] += np.log(self.p_false_)
self.log_odds_[Ex,
Ey] += (np.log(self.p_true_) - np.log(self.p_false_))
MAP["map"] = MAP["map"].astype(np.float64)
MAP["map"] += self.log_odds_
MAP["map"] = 1.0 - 1.0 / (1.0 + np.exp(MAP["map"]))
obst = MAP["map"] > self.p_thresh_
free = MAP["map"] < 0.2
unexp = (MAP["map"] > 0.2) & (MAP["map"] < self.p_thresh_)
MAP["map"][obst] = 0
MAP["map"][free] = 1
MAP["map"][unexp] = 0.5
#End student's input
self.MAP_ = MAP
return self.MAP_