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
def buildMAP(self, t, x, y, theta):
# MAP = self.MAP_
tj0 = self.getJointTime(t)
scan = self.lidar_.data_[t]['scan'][0]
neck_angle, head_angle = self.joints_.data_['head_angles'][:, tj0][
0], self.joints_.data_['head_angles'][:, tj0][1]
body_2_lidar_rot = tf.homo_transform(
np.dot(tf.rot_z_axis(neck_angle), tf.rot_y_axis(head_angle)),
[0, 0, 0])
ground_2_body = tf.homo_transform(tf.rot_z_axis(theta),
[x, y, self.h_lidar_])
#Got the transforms
homoscan = np.empty((4, (self.lidar_angles).shape[1]), dtype=np.float)
homoscan[0, :] = np.cos(self.lidar_angles) * scan
homoscan[1, :] = np.sin(self.lidar_angles) * scan
homoscan[2, :] = np.zeros((1, self.lidar_angles.shape[1]))
homoscan[3, :] = np.ones(self.lidar_angles.shape[1])
# xscan = np.cos(self.lidar_angles)*scan
# yscan = np.sin(self.lidar_angles)*scan
# zscan = np.zeros((1,xscan.shape[1]))
# Onescan = np.ones(zscan.shape)
ground_2_lidar = np.dot(ground_2_body, body_2_lidar_rot)
# homoscan = np.vstack((xscan,yscan,zscan,Onescan))
trans_scan = (np.dot(ground_2_lidar, homoscan)).astype(np.float16)
ground_zz = trans_scan[2] < 0.1
x_new = ((trans_scan[0] - self.MAP_['xmin']) //
self.MAP_['res']).astype(np.uint16)
y_new = ((trans_scan[1] - self.MAP_['ymin']) //
self.MAP_['res']).astype(np.uint16)
x_start = ((x - self.MAP_['xmin']) // self.MAP_['res']).astype(
np.uint16)
y_start = ((y - self.MAP_['xmin']) // self.MAP_['res']).astype(
np.uint16)
if not (self.psx == x_start and self.psy == y_start):
self.bresenDict = {}
self.psx = x_start
self.psy = y_start
for x_n, y_n, ground in zip(x_new, y_new, ground_zz):
if abs(x_n) < self.MAP_['sizex'] and abs(y_n) < self.MAP_['sizey']:
if (self.bresenDict.get((x_n, y_n)) is None):
ray_cells = bresenham2D(x_start, y_start, x_n, y_n)
self.bresenDict[(x_n, y_n)] = ray_cells
else:
ray_cells = self.bresenDict[(x_n, y_n)]
self.dict_use_count = self.dict_use_count + 1
# print("using dict")
x = np.asarray(ray_cells[0], dtype=int)
y = np.asarray(ray_cells[1], dtype=int)
x_end = x[-1]
y_end = y[-1]
x = x[:-1]
y = y[:-1]
self.log_odds_[x, y] = self.log_odds_[x, y] + np.log(
self.p_false_)
self.MAP_['map'][x, y] = self.log_odds_[x, y] > np.log(1.5)
self.log_odds_[x_end, y_end] = self.log_odds_[
x_end, y_end] + (1 - ground) * np.log(self.p_true_)
def ray_in_world(self,R_pose,head_angle, neck_angle,lidar_rays, theta):
lidar_rays_x = lidar_rays * np.cos(theta)
lidar_rays_y = lidar_rays * np.sin(theta)
homo_lidar_to_head = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0.15],[0,0,0,1]])
homo_head_to_body = tf.homo_transform(np.dot(tf.rot_z_axis(neck_angle), tf.rot_y_axis(head_angle)),np.array([0,0,0.33]))
homo_body_to_ground = tf.homo_transform(tf.rot_z_axis(R_pose[2]) , np.array([R_pose[0],R_pose[1],0.93]))
ray_ground = np.linalg.multi_dot([homo_body_to_ground , homo_head_to_body, homo_lidar_to_head, np.array( [lidar_rays_x, lidar_rays_y, np.zeros(lidar_rays_x.shape[0]), np.ones(lidar_rays_x.shape[0])] ) ])
#ray_ground = homo_body_to_ground @ homo_head_to_body @ homo_lidar_to_head @ np.array( [lidar_rays_x, lidar_rays_y, np.zeros(lidar_rays_x.shape[0]), np.ones(lidar_rays_x.shape[0])] )
return ray_ground[:,ray_ground[2,:] > 0.2]
def ray_in_world(self,R_pose,head_angle, neck_angle,lidar_rays, theta):
homo_lidar_to_head = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0.15],[0,0,0,1]])
homo_head_to_body = tf.homo_transform(np.dot(tf.rot_z_axis(neck_angle), tf.rot_y_axis(head_angle)),np.array([0,0,0.33]))
homo_body_to_ground = tf.homo_transform(tf.rot_z_axis(R_pose[2]) , np.array([R_pose[0],R_pose[1],0.93]))
homo_lidar_to_body = np.dot(homo_head_to_body , homo_lidar_to_head)
homo_lidar_to_ground = np.dot(homo_body_to_ground,homo_lidar_to_body)
#ray_angle = theta.reshape((1,theta.shape[0]))
lidar_rays_x = lidar_rays * np.cos(theta)#(ray_angle)
lidar_rays_y = lidar_rays * np.sin(theta)#(ray_angle)
#lidar_rays_z = np.zeros((1,lidar_rays_x.shape[1]))
#ones = np.ones((1,lidar_rays_x.shape[1]))
ray_ground = np.dot( homo_lidar_to_ground , np.array( [lidar_rays_x, lidar_rays_y, np.zeros(lidar_rays_x.shape[0]), np.ones(lidar_rays_x.shape[0])] )) #np.vstack( (lidar_rays_x, lidar_rays_y, lidar_rays_z, ones) ) )
return ray_ground[:,ray_ground[2,:] > 0.1]
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 _lidar_to_body_homo(self, head_angle, neck_angle):
rot_neck = tf.rot_z_axis(neck_angle)
# trans_neck = np.array([0,0,0])
neck_homo = tf.homo_transform(rot_neck, [0, 0, 0])
rot_head = tf.rot_y_axis(head_angle)
# trans_head = np.array([0,0,0])
head_homo = tf.homo_transform(rot_head, [0, 0, 0])
body_to_head_homo = np.array([[1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 1, 0.33], [0, 0, 0, 1]])
body_to_head = np.dot(np.dot(body_to_head_homo, neck_homo), head_homo)
head_to_lidar_homo = np.array([[1, 0, 0, 0], [0, 1, 0, 0],
[0, 0, 1, 0.15], [0, 0, 0, 1]])
lidar_to_body_homo = np.dot(body_to_head, head_to_lidar_homo)
return lidar_to_body_homo
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 _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
def Twoglobalpos(lidar_hit,
joint_angles,
body_angles,
pose=None,
Particles=None):
neck_angle = joint_angles[0] # yaw wrt body frame
head_angle = joint_angles[1] # pitch wrt body frame
roll_gb = body_angles[0]
pitch_gb = body_angles[1]
yaw_gb = body_angles[
2] # using imu's yaw has better performance than pose's yaw
# lidar wrt head
z_hl = 0.15
H_hl = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, z_hl],
[0, 0, 0, 1]]) # no rotation
# head wrt body
z_bh = 0.33
T_bh = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, z_bh], [0, 0, 0,
1]])
Rz = tf.rot_z_axis(neck_angle)
Rz = tf.homo_transform(Rz, np.array([0, 0, 1]))
Ry = tf.rot_y_axis(head_angle)
Ry = tf.homo_transform(Ry, np.array([0, 0, 1]))
R_bh = np.dot(Rz, Ry)
H_bh = np.dot(T_bh, R_bh)
if Particles is None: # for mapping
# body wrt world
x_gb = pose[0]
y_gb = pose[1]
z_gb = 0.93
T_gb = np.array([[1, 0, 0, x_gb], [0, 1, 0, y_gb], [0, 0, 1, z_gb],
[0, 0, 0, 1]])
Rb2g_yaw = tf.rot_z_axis(yaw_gb)
Rb2g_pitch = tf.rot_y_axis(pitch_gb)
Rb2g_roll = tf.rot_x_axis(roll_gb)
Hb2g_yaw = tf.homo_transform(Rb2g_yaw, np.array([0, 0, 1]))
Hb2g_pitch = tf.homo_transform(Rb2g_pitch, np.array([0, 0, 1]))
Hb2g_roll = tf.homo_transform(Rb2g_roll, np.array([0, 0, 1]))
R_gb = np.dot(np.dot(Hb2g_yaw, Hb2g_pitch), Hb2g_roll)
H_gb = np.dot(T_gb, R_gb)
# lidar wrt world
H_gl = H_gb.dot(H_bh).dot(H_hl)
lidar_hit = np.vstack((lidar_hit, np.ones(
(1, lidar_hit.shape[1])))) # 4*n
world_hit = np.dot(H_gl, lidar_hit)
# ground check, keep hits not on ground
not_floor = world_hit[2] > 0.1
world_hit = world_hit[:, not_floor]
return world_hit[:3, :]
else: # for particles update
nums = Particles.shape[1]
poses = Particles
particles_hit = []
lidar_hit = np.vstack((lidar_hit, np.ones((1, lidar_hit.shape[1]))))
for i in range(nums):
# body wrt world
T_gb = np.array([[1, 0, 0, poses[0, i]], [0, 1, 0, poses[1, i]],
[0, 0, 1, 0.93], [0, 0, 0, 1]])
yaw_gb = poses[2, i]
Rb2g_yaw = tf.rot_z_axis(yaw_gb)
Rb2g_pitch = tf.rot_y_axis(pitch_gb)
Rb2g_roll = tf.rot_x_axis(roll_gb)
Hb2g_yaw = tf.homo_transform(Rb2g_yaw, np.array([0, 0, 1]))
Hb2g_pitch = tf.homo_transform(Rb2g_pitch, np.array([0, 0, 1]))
Hb2g_roll = tf.homo_transform(Rb2g_roll, np.array([0, 0, 1]))
R_gb = np.dot(np.dot(Hb2g_yaw, Hb2g_pitch), Hb2g_roll)
H_gb = np.dot(T_gb, R_gb)
# lidar wrt world
H_gl = H_gb.dot(H_bh).dot(H_hl)
world_hit = np.dot(H_gl, lidar_hit)[:3, :]
not_floor = world_hit[2] > 0.1
particles_hit.append(world_hit[:, not_floor])
return np.transpose(np.asarray(particles_hit), (1, 2, 0))
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_
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
##Get data from Lidar data, joint data & time sync
ranges = self.lidar_.data_[self.t0]["scan"][0]
lidar_t = self.lidar_.data_[self.t0]["t"]
lidar_pose = self.lidar_.data_[self.t0]["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,y positions in lidar frame
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 & body2world
#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)
#body2world
world_2_body_rot = tf.rot_z_axis(lidar_yaw)
world_2_body_trans = np.array([lidar_pose[0], lidar_pose[1], 0.93])
world_2_part_homo = tf.homo_transform(world_2_body_rot,
world_2_body_trans)
p_world = np.dot(world_2_part_homo, p_body)
#ground removal
ground_ind = np.argwhere(p_world[2] < 0.1)
p_world = np.delete(p_world, ground_ind, 1)
p_world = p_world[0:2, :]
#map into map index
sx = np.ceil(
(lidar_pose[0] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
sy = np.ceil(
(lidar_pose[1] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
Ex = np.ceil(
(p_world[0, :] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
Ey = np.ceil(
(p_world[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
def _update(self, t, t0=0, fig='on'):
"""Update function where we update the """
# MAP = self.MAP_
if t == t0:
self._build_first_map(t0, use_lidar_yaw=True)
return
#TODO: student's input from here
tj0 = self.getJointTime(t)
neck_angle, head_angle = self.joints_.data_['head_angles'][:, tj0][
0], self.joints_.data_['head_angles'][:, tj0][1]
body_2_lidar_rot = tf.homo_transform(
np.dot(tf.rot_z_axis(neck_angle), tf.rot_y_axis(head_angle)),
[0, 0, 0])
scan = self.lidar_.data_[t]['scan'][0]
xscan = np.cos(self.lidar_angles) * scan
yscan = np.sin(self.lidar_angles) * scan
zscan = np.zeros((1, xscan.shape[1]))
Onescan = np.ones(zscan.shape)
corr = []
for i in range(self.num_p_):
pose_i = self.particles_[:, i]
ground_2_body = tf.homo_transform(tf.rot_z_axis(
pose_i[2]), [pose_i[0], pose_i[1], self.h_lidar_])
#Got the transforms
ground_2_lidar = np.dot(ground_2_body, body_2_lidar_rot)
homoscan = np.vstack((xscan, yscan, zscan, Onescan))
trans_scan = np.dot(ground_2_lidar, homoscan)
# vp = np.vstack((xscan,yscan))
# print("zz",min(trans_scan[1]))
trans_scan = trans_scan[:, abs(trans_scan[0]) < 20]
trans_scan = trans_scan[:, abs(trans_scan[1]) < 20]
x_ind, y_ind = self.getMapCoord(trans_scan[0], trans_scan[1])
occupied_indices = np.vstack((x_ind, y_ind))
corr.append(mapCorrelation(self.MAP_['map'], occupied_indices))
# print(self.num_p_)
# corr.append(self.getCorrMap(self.MAP_['map'],x_ind,y_ind))
# space = 0.02
# xs = np.arange(pose_i[0]-space,pose_i[0]+space,space/4)
# ys = np.arange(pose_i[1]-space,pose_i[1]+space,space/4)
#self.weights_[i] = self.weights_[i]*np.sum(mapCorrelation(self.MAP_['map'], np.array([self.MAP_['xmin'],self.MAP_['xmax']]), \
# np.array([self.MAP_['ymin'],self.MAP_['ymax']]), vp, xs, ys))
#self.weights_[i] = self.weights_[i]*mapCorrelation(self.MAP_['map'],occupied_indices)
#self.weights_ = self.weights_/np.sum(self.weights_)
corr = np.asarray(corr)
self.weights_ = prob.update_weights(self.weights_, corr)
best_p_i = np.argmax(self.weights_)
best_p = self.particles_[:, best_p_i]
x = best_p[0]
y = best_p[1]
theta = best_p[2]
self.buildMAP(t, x, y, theta)
self.best_p_[:, t] = best_p
self.best_p_indices_[:, t] = self.getMapCoord(x, y)
#End student's input
# self.MAP_ = MAP
return self.MAP_
