def bresenham(self, a, b, c, d):
cells = p2_utils.bresenham2D(c, d, a, b).astype(int)
self.grid_occ(cells[0, -1], cells[1, -1])
cells = cells[:, :-1]
self.v_grid_free(cells[0], cells[1])
def get_Occmap(l0, j0, xmin, xmax, ymin, ymax, res, laser_index, body_Wpose):
# get laser points in world frame
laser_Wpose = get_laser_pose_inWorld(l0, j0, laser_index, body_Wpose)
# measurements of valid laser points
map_meas = len(laser_Wpose[0])
# init map
map = np.zeros(
(int((ymax - ymin) / res + 1), int((xmax - xmin) / res + 1)))
for i in range(map_meas):
# transform world frame to map frame
lx = (np.ceil((laser_Wpose[0][i] - xmin) / 0.05)).astype(np.int16) - 1
ly = (np.ceil((laser_Wpose[1][i] - ymin) / 0.05)).astype(np.int16) - 1
bx = (np.ceil((body_Wpose[0][0] - xmin) / 0.05)).astype(np.int16) - 1
by = (np.ceil((body_Wpose[1][0] - ymin) / 0.05)).astype(np.int16) - 1
# use bresenham algorithm to get free cells
r = p2.bresenham2D(bx, by, lx, ly)
# log-odds
map[lx, ly] += np.log(4)
map[r[0].astype(np.int16), r[1].astype(np.int16)] += np.log(0.8)
return map
def update(MAP, best_particle, location):
# wTb of the best particle
best_wTb = np.array([[np.cos(best_particle[2]), -np.sin(best_particle[2]), 0, best_particle[0]],
[np.sin(best_particle[2]), np.cos(best_particle[2]), 0, best_particle[1]],
[0, 0, 1, 0.93], [0, 0, 0, 1]])
# start point sx and sy in image
sx = np.ceil((best_particle[0] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
sy = np.ceil((best_particle[1] - MAP['ymin']) / MAP['res']).astype(np.int16) - 1
endpoint = np.dot(best_wTb, location)
bool_endpoint = endpoint[2, :] >= 0.01 # remove the points that hit the ground
endpoint = endpoint[:, bool_endpoint]
# end point ex and ey in image
ex = np.ceil((endpoint[0] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
ey = np.ceil((endpoint[1] - MAP['ymin']) / MAP['res']).astype(np.int16) - 1
for i in range(len(ex)):
# bresenham2D
points = p2.bresenham2D(sx, sy, ex[i], ey[i])
[xis, yis] = points.astype(np.int16)
indGood = np.logical_and(np.logical_and(np.logical_and((xis > 1), (yis > 1)), (xis < MAP['sizex'])),
(yis < MAP['sizey']))
# To simplify
MAP['map'][ex[i], ey[i]] = 1
MAP['map'][xis[indGood], yis[indGood]] = 1
# MAP['map'][xis[indGood], yis[indGood]] += np.log(1 / 4)
# MAP['map'][ex[i], ey[i]] += np.log(4)
# MAP['map'] = np.clip(MAP['map'], 5 * np.log(1 / 4), 5 * np.log(4)) # Avoid over confidence
return MAP
def Transform(self, z_min,z_max):
self.load_data()
self._degrees = np.linspace(-135, 135.25, 1081)*np.pi/180.
indValid = np.logical_and((self._lidar['scan'][0] < z_max), (self._lidar['scan'][0] > z_min))
self._lidar['scan'] = [self._lidar['scan'][0][indValid]]
self._degrees = self._degrees[indValid]
xs0 = np.array([self._lidar['scan'] * np.cos(self._degrees)])
ys0 = np.array([self._lidar['scan'] * np.sin(self._degrees)])
zs0 = np.array([0.15]*len(indValid))
self._lidar['coordinate'] = np.array([xs0[0], ys0[0], zs0[0]])
neck_angle = self._neck
head_angle = self._head
particle_position = self._particles.Prob_particle() #obtain the position of the most probably particle to update the map
self.__record[self.__iteration, 0] = self._lidar["ts"]
self.__record[self.__iteration, 1] = particle_position[0]
self.__record[self.__iteration, 2] = particle_position[1]
self.__record[self.__iteration, 3] = particle_position[2]
self.__record[self.__iteration, 4] = neck_angle
self.__record[self.__iteration, 5] = head_angle
self._position_in_map_x = np.ceil((particle_position[0] - self.__MAP['xmin']) / self.__MAP['res']).astype(np.int16) #x
self._position_in_map_y = np.ceil((particle_position[1] - self.__MAP['ymin']) / self.__MAP['res']).astype(np.int16)#y
angle_w2b = particle_position[2] #angle_body_to_world
Rotation_matrix = compute_rotation_matrix(wx = 0, wy = head_angle, wz = neck_angle)
self._lidar['coordinate_b'] = Rotation_matrix.dot(self._lidar['coordinate'])
self._lidar['coordinate_b'][2] += 0.33
z_ground = -0.93 #contact points with the ground
indValid = self._lidar['coordinate_b'][2] > z_ground
self._lidar['coordinate_b'][0] = self._lidar['coordinate_b'][0][indValid] #x
self._lidar['coordinate_b'][1] = self._lidar['coordinate_b'][1][indValid] #y
self._lidar['coordinate_b'][2] = self._lidar['coordinate_b'][2][indValid] #z
if self.__iteration > 0:
self._particles.weight_update(self._lidar['coordinate_b'][0:2],self.__MAP['map']) #update particle weights
Rotation_matrix = compute_rotation_matrix(wx = 0, wy = 0, wz = angle_w2b)
self._lidar['coordinate_w'] = Rotation_matrix.dot(self._lidar['coordinate_b'])
self._lidar['x'] = self._lidar['coordinate_w'][0]
self._lidar['y'] = self._lidar['coordinate_w'][1]
xs0 = self._lidar['x'].squeeze()
ys0 = self._lidar['y'].squeeze()
xis = np.ceil((xs0 - self.__MAP['xmin']) / self.__MAP['res']).astype(np.int16) - 1 + np.ceil(self._position_in_map_x - (self.__MAP['sizex'] - 1) / 2).astype(np.int16)
yis = np.ceil((ys0 - self.__MAP['ymin']) / self.__MAP['res']).astype(np.int16) - 1 + np.ceil(self._position_in_map_y - (self.__MAP['sizey'] - 1) / 2).astype(np.int16)
indGood = np.logical_and(np.logical_and(np.logical_and((xis > 1), (yis > 1)), (xis < self.__MAP['sizex'])),(yis < self.__MAP['sizey']))
xis = xis[indGood]
yis = yis[indGood]
for index in range(0, len(xis)):
x_point = xis[index]
y_point = yis[index]
stack = bresenham2D(self._position_in_map_x, self._position_in_map_y, x_point, y_point).astype(np.int16)
#stack[0] -> x stack[1] -> y stack[0][-1] -> point on the boundary
# saturate to [-10,10]
#if self.__MAP['Probability'][stack[1][-1], stack[0][-1]] <= self.__saturu:
self.__MAP['Probability'][stack[1][-1], stack[0][-1]] += self.__threshold_o
#invalid = self.__MAP['Probability'][stack[1][0:-1], stack[0][0:-1]] >= self.__saturl
#self.__MAP['Probability'][stack[1][0:-1][invalid], stack[0][0:-1][invalid]] += self.__threshold_f
self.__MAP['Probability'][stack[1][0:-1], stack[0][0:-1]] += self.__threshold_f
self.__MAP['map'][np.logical_and(self.__MAP['Probability'] > 0, self.__MAP['map'] != 2)] = 1
self.__MAP['map'][np.logical_and(self.__MAP['Probability'] < 0, self.__MAP['map'] != 2)] = 0
self.__MAP['map'][self._position_in_map_y.astype(np.int16),self._position_in_map_x.astype(np.int16)] = 2