class SLAM(object):
def __init__(self):
self._characterize_sensor_specs()
def _read_data(self, src_dir, dataset=0, split_name='train'):
self.dataset_ = str(dataset)
if split_name.lower() not in src_dir:
src_dir = src_dir + '/' + split_name
print('\n------Reading Lidar and Joints (IMU)------')
self.lidar_ = LIDAR(dataset=self.dataset_,
data_folder=src_dir,
name=split_name + '_lidar' + self.dataset_)
print('\n------Reading Joints Data------')
self.joints_ = JOINTS(dataset=self.dataset_,
data_folder=src_dir,
name=split_name + '_joint' + self.dataset_)
self.num_data_ = len(self.lidar_.data_)
# Position of odometry
self.odo_indices_ = np.empty((2, self.num_data_), dtype=np.int64)
def _characterize_sensor_specs(self, p_thresh=None):
# High of the lidar from the ground (meters)
self.h_lidar_ = 0.93 + 0.33 + 0.15
# Accuracy of the lidar
self.p_true_ = 0.9
self.p_false_ = 1.0 / 9
#TODO: set a threshold value of probability to consider a map's cell occupied
self.p_thresh_ = 0.6 if p_thresh is None else p_thresh # > p_thresh => occupied and vice versa
# Compute the corresponding threshold value of logodd
self.logodd_thresh_ = prob.log_thresh_from_pdf_thresh(self.p_thresh_)
def _init_particles(self,
num_p=0,
mov_cov=None,
particles=None,
weights=None,
percent_eff_p_thresh=None):
# Particles representation
self.num_p_ = num_p
#self.percent_eff_p_thresh_ = percent_eff_p_thresh
self.particles_ = np.zeros(
(3, self.num_p_),
dtype=np.float64) if particles is None else particles
# Weights for particles
self.weights_ = 1.0 / self.num_p_ * np.ones(
self.num_p_) if weights is None else weights
# Position of the best particle after update on the map
self.best_p_indices_ = np.empty((2, self.num_data_), dtype=np.int64)
# Best particles
self.best_p_ = np.empty((3, self.num_data_))
# Corresponding time stamps of best particles
self.time_ = np.empty(self.num_data_)
# Covariance matrix of the movement model
tiny_mov_cov = np.array([[1e-8, 0, 0], [0, 1e-8, 0], [0, 0, 1e-8]])
self.mov_cov_ = mov_cov if mov_cov is not None else tiny_mov_cov
# To generate random noise: x, y, z = np.random.multivariate_normal(np.zeros(3), mov_cov, 1).T
# this return [x], [y], [z]
# Threshold for resampling the particles
self.percent_eff_p_thresh_ = percent_eff_p_thresh
def _init_map(self, map_resolution=0.05):
'''*Input: resolution of the map - distance between two grid cells (meters)'''
# Map representation
MAP = {}
MAP['res'] = map_resolution #meters
MAP['xmin'] = -20 #meters
MAP['ymin'] = -20
MAP['xmax'] = 20
MAP['ymax'] = 20
MAP['sizex'] = int(
np.ceil((MAP['xmax'] - MAP['xmin']) / MAP['res'] + 1)) #cells
MAP['sizey'] = int(
np.ceil((MAP['ymax'] - MAP['ymin']) / MAP['res'] + 1))
# binary representation of math
MAP['map'] = np.zeros((MAP['sizex'], MAP['sizey']),
dtype=np.int8) #DATA TYPE: char or int8
self.MAP_ = MAP
# log_odds representation to simplify the math
self.log_odds_ = np.zeros((self.MAP_['sizex'], self.MAP_['sizey']),
dtype=np.float64)
# occupancy probability
self.occu_ = np.ones(
(self.MAP_['sizex'], self.MAP_['sizey']), dtype=np.float64) * 0.5
# Number of measurements for each cell
self.num_m_per_cell_ = np.zeros(
(self.MAP_['sizex'], self.MAP_['sizey']), dtype=np.uint64)
def good_lidar_idx(self, lidar_scan, L_MIN=0.001, L_MAX=30):
#TODO: Truncate > L_Max and less than L_MIN
lidar_scan = np.squeeze(lidar_scan)
good_idxs = (lidar_scan > L_MIN) & (lidar_scan < L_MAX)
return good_idxs
def _polar_to_cart(self, lidar_scan, res_rad=0.004359):
"""
lidar_scan: m
returns: 3 x m
"""
angle_min = -2.35619 #rad
angle_max = +2.35619 #rad
lidar_angles = np.arange(angle_min, angle_max, res_rad)
x = lidar_scan * np.cos(lidar_angles)
y = lidar_scan * np.sin(lidar_angles)
z = np.zeros_like(x)
lidar_cart = np.vstack((x, y, z))
return lidar_cart
def _global_to_map_cell(self, g_pose, MAP):
cell_x = max(
min(MAP['xmax'], int(np.ceil(g_pose[0] / MAP['res'] + 1))),
MAP['xmin'])
cell_y = max(
min(MAP['ymax'], int(np.ceil(g_pose[1] / MAP['res'] + 1))),
MAP['ymin'])
return (cell_x + 20, cell_y + 20)
def _build_first_map(self, t0=0, use_lidar_yaw=True):
"""Build the first map using first lidar"""
self.t0 = t0
# Extract a ray from lidar data
MAP = self.MAP_
print('\n--------Doing build the first map--------')
#TODO: student's input from here
#0) Extract Params from LiDAR and Joints
lidar_scan, lidar_ts = self.lidar_._get_scan(idx=t0)
neck_angle, head_angle, _ = self.joints_._get_head_angles(ts=lidar_ts)
good_lidar_idxs = self.good_lidar_idx(lidar_scan)
l_lidar_pts = self._polar_to_cart(lidar_scan,
res_rad=self.lidar_.res_rad)
l_lidar_pts = l_lidar_pts[:, good_lidar_idxs]
homo_l_lidar_pts = np.ones((4, l_lidar_pts.shape[1]), dtype=np.float64)
homo_l_lidar_pts[:3, :] = l_lidar_pts
if use_lidar_yaw:
yaw = self.lidar_.data_[t0]['rpy'][0, 2]
t_gb_x, t_gb_y, _ = self.best_p_[:, t0]
else:
t_gb_x, t_gb_y, yaw = self.best_p_[:, t0]
p_curr = np.array([t_gb_x, t_gb_y, yaw], dtype=np.float64)
#1) Transform LiDAR Scan to global world frame
#a) lidar -> body
R_bl = np.dot(tf.rot_z_axis(neck_angle), tf.rot_y_axis(head_angle))
T_bl = np.array([0, 0, 0.15], dtype=np.float64)
H_bl = tf.homo_transform(R_bl, T_bl)
#b) body -> global (only considering yaw atm)
R_gb = tf.rot_z_axis(yaw)
T_gb = np.array([t_gb_x, t_gb_y, self.h_lidar_])
H_gb = tf.homo_transform(R_gb, T_gb)
#c) apply to lidar_pts
H_gl = H_gb @ H_bl
g_lidar_pts = H_gl @ homo_l_lidar_pts
#d) remove ground (all points with global y < 0.0)
non_ground_idx = g_lidar_pts[2, :] > 0.0
g_lidar_pts = g_lidar_pts[:, non_ground_idx]
#e) Use bresenham2D to get free/occupied cell locations
g_curr_pose = p_curr
m_curr_pose = self.lidar_._physicPos2Pos(MAP, g_curr_pose[:2])
for ray in range(g_lidar_pts.shape[1]):
m_lidar_pt = self.lidar_._physicPos2Pos(MAP, g_lidar_pts[:2, ray])
ret = bresenham2D(m_curr_pose[0], m_curr_pose[1], m_lidar_pt[0],
m_lidar_pt[1]).astype(int)
free_coords = ret[:, :-1]
occupied_coords = ret[:, -1]
#f) Update Log Odds Map (increase all the free cells)
log_pos = np.log(self.p_true_ / (1 - self.p_true_))
log_neg = np.log(self.p_false_ / (1 - self.p_false_))
self.log_odds_[tuple(occupied_coords)] += log_pos
self.log_odds_[tuple(free_coords)] += log_neg
MAP['map'][self.log_odds_ >= self.logodd_thresh_] = 1
MAP['map'][self.log_odds_ < self.logodd_thresh_] = 0
# plt.imshow(MAP['map'])
self.best_p_[:, min(self.num_data_ - 1, t0)] = g_curr_pose[:3]
self.best_p_indices_[:, min(self.num_data_ - 1, t0)] = m_curr_pose
self.MAP_ = MAP
def _mapping(self, t=0, use_lidar_yaw=True):
"""Build the map """
t -= 1
# Extract a ray from lidar data
MAP = self.MAP_
print('\n--------Building the map--------')
#0) Extract Params from LiDAR and Joints
lidar_scan, lidar_ts = self.lidar_._get_scan(idx=t)
neck_angle, head_angle, _ = self.joints_._get_head_angles(ts=lidar_ts)
good_lidar_idxs = self.good_lidar_idx(lidar_scan)
l_lidar_pts = self._polar_to_cart(lidar_scan,
res_rad=self.lidar_.res_rad)
l_lidar_pts = l_lidar_pts[:, good_lidar_idxs]
homo_l_lidar_pts = np.ones((4, l_lidar_pts.shape[1]), dtype=np.float64)
homo_l_lidar_pts[:3, :] = l_lidar_pts
if use_lidar_yaw:
yaw = self.lidar_.data_[t]['rpy'][0, 2]
t_gb_x, t_gb_y, _ = self.best_p_[:, t]
else:
t_gb_x, t_gb_y, yaw = self.best_p_[:, t]
p_curr = np.array([t_gb_x, t_gb_y, yaw], dtype=np.float64)
#1) Transform LiDAR Scan to global world frame
#a) lidar -> body
R_bl = np.dot(tf.rot_z_axis(neck_angle), tf.rot_y_axis(head_angle))
T_bl = np.array([0, 0, 0.15], dtype=np.float64)
H_bl = tf.homo_transform(R_bl, T_bl)
#b) body -> global (only considering yaw atm)
R_gb = tf.rot_z_axis(yaw)
T_gb = np.array([t_gb_x, t_gb_y, self.h_lidar_])
H_gb = tf.homo_transform(R_gb, T_gb)
#c) apply to lidar_pts
H_gl = H_gb @ H_bl
g_lidar_pts = H_gl @ homo_l_lidar_pts
#d) remove ground (all points with global y < 0.0)
non_ground_idx = g_lidar_pts[2, :] > 0.0
g_lidar_pts = g_lidar_pts[:, non_ground_idx]
#e) Use bresenham2D to get free/occupied cell locations
g_curr_pose = p_curr
m_curr_pose = self.lidar_._physicPos2Pos(MAP, g_curr_pose[:2])
m_lidar_pts = self.lidar_._physicPos2PosVec(MAP, g_lidar_pts[:2, :])
for ray in range(g_lidar_pts.shape[1]):
m_lidar_pt = m_lidar_pts[:, ray]
ret = bresenham2D(m_curr_pose[0], m_curr_pose[1], m_lidar_pt[0],
m_lidar_pt[1]).astype(int)
free_coords = ret[:, :-1]
occupied_coords = ret[:, -1]
#f) Update Log Odds Map (increase all the free cells)
log_pos = np.log(self.p_true_ / (1 - self.p_true_))
log_neg = np.log(self.p_false_ / (1 - self.p_false_))
self.log_odds_[tuple(occupied_coords)] += log_pos
self.log_odds_[tuple(free_coords)] += log_neg
MAP['map'][self.log_odds_ >= self.logodd_thresh_] = 1
MAP['map'][self.log_odds_ < self.logodd_thresh_] = 0
self.MAP_ = MAP
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
# self.best_p_[:, t] = p_next
# p_next_idx = self.lidar_._physicPos2Pos(self.MAP_, p_next[:2])
# self.best_p_indices_[:, t] = p_next_idx
#TODO: student's input from here
#End student's input
def _update(self, t, t0=0, fig='on'):
"""Update function where we update the """
MAP = self.MAP_
t -= 1
# if t == t0:
# self._build_first_map(t0,use_lidar_yaw=True)
# return
correlations = np.empty(self.num_p_)
for i in range(self.num_p_):
#0) Extract Params from LiDAR and Joints
lidar_scan, lidar_ts = self.lidar_._get_scan(idx=t)
neck_angle, head_angle, _ = self.joints_._get_head_angles(
ts=lidar_ts)
good_lidar_idxs = self.good_lidar_idx(lidar_scan)
l_lidar_pts = self._polar_to_cart(lidar_scan,
res_rad=self.lidar_.res_rad)
l_lidar_pts = l_lidar_pts[:, good_lidar_idxs]
homo_l_lidar_pts = np.ones((4, l_lidar_pts.shape[1]),
dtype=np.float64)
homo_l_lidar_pts[:3, :] = l_lidar_pts
p_curr = self.particles_[:, i]
# p_curr[2] = self.lidar_.data_[t]['rpy'][0, 2]
#1) Transform LiDAR Scan to global world frame
#a) lidar -> body
R_bl = np.dot(tf.rot_z_axis(neck_angle), tf.rot_y_axis(head_angle))
T_bl = np.array([0, 0, 0.15], dtype=np.float64)
H_bl = tf.homo_transform(R_bl, T_bl)
#b) body -> global (only considering yaw atm)
R_gb = tf.rot_z_axis(p_curr[2])
T_gb = np.array([p_curr[0], p_curr[1], self.h_lidar_])
H_gb = tf.homo_transform(R_gb, T_gb)
#c) apply to lidar_pts
H_gl = H_gb @ H_bl
g_lidar_pts = H_gl @ homo_l_lidar_pts
#d) remove ground (all points with global y < 0.0)
non_ground_idx = g_lidar_pts[2, :] > 0.0
g_lidar_pts = g_lidar_pts[:, non_ground_idx]
#e) Convert to map cell coords & Compute corr(m_t, y_t)
m_lidar_pts = (self.lidar_._physicPos2PosVec(
MAP, g_lidar_pts[:2, :])).astype(np.int64)
corr = prob.mapCorrelation(MAP['map'], m_lidar_pts)
correlations[i] = corr
self.weights_ = prob.update_weights(self.weights_, correlations)
#Update the best particle position
max_idx = np.argmax(self.weights_)
g_particle = self.particles_[:, max_idx]
self.best_p_[:, t + 1] = g_particle
m_particle = self.lidar_._physicPos2Pos(MAP, g_particle[:2]).astype(
np.int64)
self.best_p_indices_[:, t + 1] = m_particle
self.MAP_ = MAP
return MAP
class SLAM(object):
def __init__(self):
self._characterize_sensor_specs()
def _read_data(self, src_dir, dataset=0, split_name='train'):
self.dataset_ = str(dataset)
if split_name.lower() not in src_dir:
src_dir = src_dir + '/' + split_name
print('\n------Reading Lidar and Joints (IMU)------')
lidar_data_ = split_name + '_lidar' + self.dataset_
self.lidar_ = LIDAR(dataset=self.dataset_, data_folder=src_dir, name=lidar_data_)
print('\n------Reading Joints Data------')
joint_data_ = split_name + '_joint' + self.dataset_
self.joints_ = JOINTS(dataset=self.dataset_, data_folder=src_dir, name=joint_data_)
self.num_data_ = len(self.lidar_.data_)
# Position of indices
self.odo_indices_ = np.empty((2, self.num_data_), dtype=np.int64)
def _characterize_sensor_specs(self, p_thresh=None):
# Height of the lidar from the ground
self.h_lidar_ = 0.93 + 0.33 + 0.15
self.h_com_ = 0.93
# Accuracy of the lidar
self.p_true_ = 9
self.p_false_ = 1.0/9
#TODO set a threshold value of probability to consider a map's cell occupied
self.p_thresh_ = 0.6 if p_thresh is None else p_thresh
# Compute the corresponding threshold value of logodd
self.logodd_thresh_ = prob.log_thresh_from_pdf_thresh(self.p_thresh_)
self.ground_threshold_ = 0.2
def _init_particles(self, num_p=0, mov_cov=None, particles=None, weights=None, percent_eff_p_thresh=None):
self.num_p_ = num_p
# Initialize particles
self.particles_ = np.zeros((3, self.num_p_), dtype=np.float64) if particles is None else particles
# Weights for the particles
self.weights_ = 1.0/self.num_p_*np.ones(self.num_p_) if weights is None else weights
# Position of the best particle after update on the map
self.best_p_indices_ = np.zeros((2, self.num_data_), dtype=np.int64)
# Best particles
self.best_p_ = np.zeros((3, self.num_data_))
# Corresponding time stamps of best particles
self.time_ = np.zeros(self.num_data_)
# Covariance matrix of motion model
tiny_mov_cov = np.array([[1e-8, 0, 0],[0, 1e-8, 0],[0, 0, 1e-8]])
self.mov_cov_ = mov_cov if mov_cov is not None else tiny_mov_cov
# Threshold for resampling the particles
self.percent_eff_p_thresh_ = percent_eff_p_thresh
def _init_map(self, map_resolution=0.05):
""" Input: resolution of map """
MAP = {}
MAP['res'] = map_resolution
MAP['xmin'] = -20
MAP['ymin'] = -20
MAP['xmax'] = 20
MAP['ymax'] = 20
MAP['sizex'] = int(np.ceil((MAP['xmax'] - MAP['xmin']) / MAP['res'] + 1))
MAP['sizey'] = int(np.ceil((MAP['ymax'] - MAP['ymin']) / MAP['res'] + 1))
MAP['map'] = np.zeros((MAP['sizex'], MAP['sizey']), dtype=np.int8)
self.MAP_ = MAP
self.log_odds_ = np.zeros((self.MAP_['sizex'], self.MAP_['sizey']), dtype=np.float64)
self.occu_ = np.ones((self.MAP_['sizex'], self.MAP_['sizey']), dtype=np.uint64)
# Number of measurements for each cell
self.num_m_per_cell_ = np.zeros((self.MAP_['sizex'], self.MAP_['sizey']), dtype=np.uint64)
def _build_first_map(self, t0=0, use_lidar_yaw=True):
""" Build the first map using first lidar"""
self.t0 = t0
MAP = self.MAP_
print('\n -------- Building the first map --------')
joint_idx = np.argmin(abs(self.lidar_.data_[t0]['t'][0] - self.joints_.data_['ts'][0]))
neck_angle, head_angle = self.joints_.data_['head_angles'][:,joint_idx]
R_pose = self.lidar_.data_[t0]['pose'][0]
ray_l = self.lidar_.data_[t0]['scan'][0]
ray_angles = np.linspace(-2.355, 2.355, len(ray_l))
valid_id = np.logical_and((ray_l >= 0.1), (ray_l <= 30))
ray_l = ray_l[valid_id]
ray_angles = ray_angles[valid_id]
lidar_to_body = self.lidar_._lidar_to_body_homo(head_angle, neck_angle)
world_to_body_trans = np.array([R_pose[0], R_pose[1], self.h_com_])
world_to_body_rot = tf.rot_z_axis(R_pose[2])
world_to_body_homo = tf.homo_transform(world_to_body_rot, world_to_body_trans)
x_val = ray_l * np.cos(ray_angles)
y_val = ray_l * np.sin(ray_angles)
z_val = np.zeros_like(x_val)
ones = np.ones_like(x_val)
points = np.vstack((x_val, y_val, z_val, ones))
points_body = np.matmul(lidar_to_body, points)
points_world = np.matmul(world_to_body_homo, points_body)
points_world = points_world[:2,points_world[2,:] > self.ground_threshold_]
map_cells = np.array(self.lidar_._physicPos2Pos(MAP, points_world))
valid_map_id = np.logical_and(np.logical_and((map_cells[0] >= 0), (map_cells[0] < MAP['sizex'])), np.logical_and((map_cells[1] >= 0), map_cells[1] < MAP['sizey']))
map_cells = map_cells[:,valid_map_id]
self.log_odds_[map_cells[0], map_cells[1]] += (2 * np.log(9))
R_pose_cells = self.lidar_._physicPos2Pos(MAP, R_pose[:2])
self.best_p_[:, t0] = R_pose
self.best_p_indices_[:, t0] = R_pose_cells
x = np.append(map_cells[0], R_pose_cells[0])
y = np.append(map_cells[1], R_pose_cells[1])
contors = np.array([y, x]).T.astype(np.int)
mask = np.zeros_like(self.log_odds_)
cv2.drawContours(image=mask, contours=[contors], contourIdx=0, color=np.log(self.p_false_), thickness=cv2.FILLED)
self.log_odds_ += mask
MAP['map'] = 1*(self.log_odds_>self.logodd_thresh_)
# End code
self.MAP_ = MAP
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])
# new_particles[:,i] = tf.twoDSmartPlus(self.particles_[:,i], noise_vectors[:,i])
# new_particles[:,i] = tf.twoDSmartPlus(self.particles_[:,i], tf.twoDSmartMinus(odom_curr, odom_prev))
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
# for one particle
MAP = self.MAP_
# joint_idx = np.where(self.lidar_.data_[t]['t'][0]<=self.joints_.data_['ts'][0])[0][0]
joint_idx = np.argmin(abs(self.lidar_.data_[t]['t'][0] - self.joints_.data_['ts'][0]))
neck_angle, head_angle = self.joints_.data_['head_angles'][:, joint_idx]
ray_l = self.lidar_.data_[t]['scan'][0]
ray_angles = np.linspace(-2.355, 2.355, len(ray_l))
valid_id = np.logical_and((ray_l >= 0.1), (ray_l <= 30))
ray_l = ray_l[valid_id]
ray_angles = ray_angles[valid_id]
lidar_to_body = self.lidar_._lidar_to_body_homo(head_angle, neck_angle)
x_val = ray_l * np.cos(ray_angles)
y_val = ray_l * np.sin(ray_angles)
z_val = np.zeros_like(x_val)
ones = np.ones_like(x_val)
points = np.vstack((x_val, y_val, z_val, ones))
points_body = np.matmul(lidar_to_body, points)
correlations = np.zeros(self.num_p_)
for i in range(self.num_p_):
R_pose = self.particles_[:,i]
world_to_body_trans = np.array([R_pose[0], R_pose[1], self.h_com_])
world_to_body_rot = tf.rot_z_axis(R_pose[2])
world_to_body_homo = tf.homo_transform(world_to_body_rot, world_to_body_trans)
points_world = np.matmul(world_to_body_homo, points_body)
points_world = points_world[:2,points_world[2,:] > self.ground_threshold_]
map_cells = np.array(self.lidar_._physicPos2Pos(MAP, points_world))
valid_map_id = np.logical_and(np.logical_and((map_cells[0] >= 0), (map_cells[0] < MAP['sizex'])),
np.logical_and((map_cells[1] >= 0), map_cells[1] < MAP['sizey']))
map_cells = map_cells[:, valid_map_id]
correlations[i] = prob.mapCorrelation(MAP['map'], map_cells)
self.weights_ = prob.update_weights(self.weights_, correlations)
best_particle_id = np.argmax(self.weights_)
R_pose = self.particles_[:,best_particle_id]
#if t % 100 == 0:
# print(self.weights_)
self.best_p_[:,t] = R_pose
self.best_p_indices_[:,t] = self.lidar_._physicPos2Pos(MAP, R_pose[:2])
world_to_body_trans = np.array([R_pose[0], R_pose[1], self.h_com_])
world_to_body_rot = tf.rot_z_axis(R_pose[2])
world_to_body_homo = tf.homo_transform(world_to_body_rot, world_to_body_trans)
points_world = np.matmul(world_to_body_homo, points_body)
points_world = points_world[:2, points_world[2, :] > self.ground_threshold_]
map_cells = np.array(self.lidar_._physicPos2Pos(MAP, points_world))
valid_map_id = np.logical_and(np.logical_and((map_cells[0] >= 0), (map_cells[0] < MAP['sizex'])),
np.logical_and((map_cells[1] >= 0), map_cells[1] < MAP['sizey']))
map_cells = map_cells[:, valid_map_id]
self.log_odds_[map_cells[0], map_cells[1]] += (2 * np.log(9))
R_pose_cells = self.best_p_indices_[:,t]
x = np.append(map_cells[0], R_pose_cells[0])
y = np.append(map_cells[1], R_pose_cells[1])
contors = np.array([y, x]).T.astype(np.int)
mask = np.zeros_like(self.log_odds_)
cv2.drawContours(image=mask, contours=[contors], contourIdx=0, color=np.log(self.p_false_),
thickness=cv2.FILLED)
self.log_odds_ += mask
MAP['map'] = self.log_odds_
self.MAP_ = MAP
return MAP