def _predict(self, t, use_lidar_yaw=True, DR=False):
logging.debug('\n-------- Doing prediction at t = {0}------'.format(t))
t -= 1
# DEAD-RECKONING
if DR:
p_curr = self.best_p_[:, t]
o_curr = self.lidar_.data_[t]['pose'][0, :]
o_curr[2] = self.lidar_.data_[t]['rpy'][0, 2]
o_next = self.lidar_.data_[t + 1]['pose'][0, :]
o_next[2] = self.lidar_.data_[t + 1]['rpy'][0, 2]
p_next = tf.twoDSmartPlus(p_curr,
tf.twoDSmartMinus(o_next, o_curr))
# LOCALIZATION PREDICTION
else:
o_curr = self.lidar_.data_[t]['pose'][0, :]
o_curr[2] = self.lidar_.data_[t]['rpy'][0, 2]
o_next = self.lidar_.data_[t + 1]['pose'][0, :]
o_next[2] = self.lidar_.data_[t + 1]['rpy'][0, 2]
w = np.random.multivariate_normal(np.zeros(3), self.mov_cov_, 1)[0]
for i in range(self.num_p_):
p_curr = self.particles_[:, i]
p_next = tf.twoDSmartPlus(p_curr,
tf.twoDSmartMinus(o_next, o_curr))
p_next = tf.twoDSmartPlus(p_next, w)
self.particles_[:, i] = p_next
def _predict(self, t, use_lidar_yaw=True):
logging.debug('\n -------- Doing prediction at t = {0} --------'.format(t))
#TODO Integrate odometry later
odom = tf.twoDSmartMinus(self.lidar_.data_[t]['pose'][0], self.lidar_.data_[t-1]['pose'][0])
noise_vector = np.random.multivariate_normal([0,0,0], self.mov_cov_, self.num_p_).T
for i in range(self.num_p_):
self.particles_[:,i] = tf.twoDSmartPlus(tf.twoDSmartPlus(self.particles_[:,i],odom), noise_vector[:,i])
def _predict(self, t, use_lidar_yaw=True):
#logging.debug('\n-------- Doing prediction at t = {0}------'.format(t))
#TODO: student's input from here
x_tminus1 = self.lidar_.data_[t - 1]['pose'][0]
x_tminus1[2] = self.lidar_.data_[t - 1]['rpy'][0, 2]
x_t = self.lidar_.data_[t]['pose'][0]
x_t[2] = self.lidar_.data_[t]['rpy'][0, 2]
#relative_movement = tf.twoDSmartMinus(self.lidar_.data_[t]['pose'][0] , self.lidar_.data_[t-1]['pose'][0])
relative_movement = tf.twoDSmartMinus(x_t, x_tminus1)
for i in range(0, self.num_p_):
self.particles_[:, i] = tf.twoDSmartPlus(self.particles_[:, i],
relative_movement)
temp = np.random.multivariate_normal(np.array([0, 0, 0]),
self.mov_cov_)
self.particles_[:, i] = tf.twoDSmartPlus(self.particles_[:, i],
temp)
def _predict(self, data_, t, mov_cov, scan_odom, scan_flag):
'''
Applies motion model on last pose in 'trajectory'
Returns predicted pose
'''
old_pose = self.trajectory_[:, -1]
if scan_flag[t - 1] and scan_flag[t]:
old_odom = scan_odom[:, t - 1]
new_odom = scan_odom[:, t]
else:
old_odom = data_._odom_at_lidar_idx(t - 1)
new_odom = data_._odom_at_lidar_idx(t)
odom_diff = tf.twoDSmartMinus(new_odom, old_odom)
noise = np.random.multivariate_normal(np.zeros(3), mov_cov,
1).flatten()
pred_pose = tf.twoDSmartPlus(old_pose, odom_diff)
pred_with_noise = tf.twoDSmartPlus(pred_pose, noise)
return scan_flag[t], pred_pose, pred_with_noise
def _predict(self, t, use_lidar_yaw=True):
logging.debug('\n-------- Doing prediction at t = {0}------'.format(t))
#TODO: student's input from here
# for i in range(self.num_p_):
# w_i = np.random.multivariate_normal(np.zeros(3),self.mov_cov_)
# self.particles_[:,i] = tf.twoDSmartPlus(self.particles_[:,i],w_i)
for i in range(self.num_p_):
#odo_diff = tf.twoDSmartMinus(self.lidar_.data_[t]['pose'][0],self.lidar_.data_[t-1]['pose'][0])
#delta = tf.twoDSmartPlus(np.random.multivariate_normal(np.zeros(3),self.mov_cov_),odo_diff)
delta = np.random.multivariate_normal(np.zeros(3), self.mov_cov_)
self.particles_[:, i] = tf.twoDSmartPlus(self.particles_[:, i],
delta)
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()
# 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
robot_pose = np.zeros((3, len(slam_inc.lidar_.data_)))
robot_pose[:, 0] = slam_inc.lidar_.data_[0]['pose'][0]
for t in tqdm.tqdm(range(1, num_steps)): #(range(t0, num_steps - t0)):
relative_movement = tf.twoDSmartMinus(
slam_inc.lidar_.data_[t]['pose'][0],
slam_inc.lidar_.data_[t - 1]['pose'][0])
robot_pose[:, t] = tf.twoDSmartPlus(robot_pose[:, t - 1],
relative_movement)
#print(robot_pose[:,0])
plt.figure(1)
plt.plot(robot_pose[0, :], robot_pose[1, :])
plt.show()
"""