def inv_sensor_model(self, scan, pose):
map_update = np.zeros(self._grid_map.shape)
rob_trans = vector2transform2D(pose)
robot_pose_map_frame = GridMap.world_to_map_coordinates(
pose[0:2, :], self._grid_size, self._offset)
laser_end_points = GridMap.laser_as_cartesian(scan, 30)
laser_end_points = rob_trans * laser_end_points
laser_end_map_frame = GridMap.world_to_map_coordinates(
laser_end_points[0:2, :], self._grid_size, self._offset)
for col in range(laser_end_map_frame.shape[1]):
rx = int(robot_pose_map_frame.item(0))
ry = int(robot_pose_map_frame.item(1))
lx = int(laser_end_map_frame.item((0, col)))
ly = int(laser_end_map_frame.item((1, col)))
bres_points = bresenham_line((rx, ry), (lx, ly))
for point in bres_points:
px, py = point
map_update[py, px] = self._grid_map[py, px] + \
prob2log(self._prob_free)
map_update[py, px] = self._grid_map[py, px] + \
prob2log(self._prob_occ)
return map_update, robot_pose_map_frame, laser_end_map_frame
def raycast(self, pose, scan_endpoints, occ_prob):
prob_map = self.get_prob_map()
pose_transf = vector2transform2D(pose)
scan_endpoints = pose_transf * scan_endpoints
pose_map = GridMap.world_to_map_coordinates(pose[0:2, :],
self._grid_size,
self._offset)
scan_endpoints_map = GridMap.world_to_map_coordinates(
scan_endpoints[0:2, :], self._grid_size, self._offset)
log_odds = prob2log(occ_prob)
result_x = np.zeros(scan_endpoints_map.shape[1])
result_y = np.zeros(scan_endpoints_map.shape[1])
rx = int(pose_map.item(0))
ry = int(pose_map.item(1))
for col in range(scan_endpoints_map.shape[1]):
lx = int(scan_endpoints_map.item((0, col)))
ly = int(scan_endpoints_map.item((1, col)))
bres_points = bresenham_line((rx, ry), (lx, ly))
last_point = None
for point in bres_points:
px, py = point
last_point = point
if self._grid_map[py, px] >= log_odds:
break
result_x[col] = last_point[0]
result_y[col] = last_point[1]
return np.vstack([result_x, result_y])
def update(self, robot_pose, laser_scan):
map_update, pose_map_frame, laser_map_frame = self.inv_sensor_model(
laser_scan, robot_pose)
log_odds_prior = prob2log(self._prior)
map_update = map_update - log_odds_prior * np.ones(map_update.shape)
self._grid_map = self._grid_map + map_update
def init_gridmap(self, map_size):
self._offset = (map_size[0] / 2.0, map_size[1] / 2.0)
self._map_size_meters = map_size
self._map_size = tuple([
math.ceil(dim / self._grid_size) for dim in self._map_size_meters
])
log_odds_prior = prob2log(self._prior)
self._grid_map = np.ones(self._map_size) * log_odds_prior
def init_gridmap_from_data(self, gridmap):
pose_x_min = np.min(self._poses[:, 0])
pose_x_max = np.max(self._poses[:, 0])
pose_y_min = np.min(self._poses[:, 1])
pose_y_max = np.max(self._poses[:, 1])
map_borders = (pose_x_min - gridmap._border,
pose_x_max + gridmap._border,
pose_y_min - gridmap._border,
pose_y_max + gridmap._border)
offset_x = map_borders[0]
offset_y = map_borders[2]
gridmap._offset = (offset_x, offset_y)
gridmap._map_size_meters = (map_borders[1] - offset_x,
map_borders[3] - offset_y)
gridmap._map_size = tuple([
math.ceil(dim / gridmap._grid_size)
for dim in gridmap._map_size_meters
])
log_odds_prior = prob2log(gridmap._prior)
gridmap._grid_map = np.ones(gridmap._map_size) * log_odds_prior