def __init__(self, *, x_min, y_min, x_max, y_max, \
reso, init_val=0.0):
self.x_min=x_min
self.y_min=y_min
self.x_max=x_max
self.y_max=y_max
self.reso=reso
self.init_val=init_val
self.x_tick_num=my_round((x_max-x_min)/reso)+1
self.y_tick_num=my_round((y_max-y_min)/reso)+1
def __init__(self, *, max_range, min_angle, max_angle, angle_res): self.max_range = max_range self.min_angle = min_angle self.max_angle = max_angle self.angle_res = angle_res self.ray_cnt = my_round((max_angle - min_angle) / angle_res) + 1 self.ranges = np.zeros(self.ray_cnt)
def register_fixed_scan(self, *, pose, scans, map2d):
conf = map2d.get_map_config()
# Convert coordinate from global to map.
x_map_scan_st, y_map_scan_st = \
GridMap2D.convert_global_2_map(pose.x, pose.y, conf.x_min, conf.y_min, conf.reso)
map_scan_st = MapIntCoord2D(x=x_map_scan_st, y=y_map_scan_st)
map_scan_ends_world, map_scan_ends_map = self._convert_scan_hit_pnt_2_world_and_map_pnt(
pose, scans, conf)
min_pnt = MapIntCoord2D(x=0, y=0)
max_pnt = MapIntCoord2D(x=conf.x_tick_num - 1, y=conf.y_tick_num - 1)
for scan_idx, end_coord in enumerate(map_scan_ends_map):
if (map_scan_st.x == end_coord.x and map_scan_st.y == end_coord.y):
continue
path = RayTracing2D.ray_tracing(map_scan_st, end_coord, min_pnt,
max_pnt)
for index, coord in enumerate(path):
# When reacnes last element, break.
if (index == len(path) - 1):
break
x_map = my_round(coord.x)
y_map = my_round(coord.y)
if (not map2d.is_valid_map_coord(x_map, y_map)):
break
val = map2d.get_val_via_map_coord(x_map=x_map, y_map=y_map)
if (val != 1.0):
map2d.update_val_via_map_coord(x_map=x_map,
y_map=y_map,
value=0.0)
x_hit_map = my_round(path[len(path) - 1].x)
y_hit_map = my_round(path[len(path) - 1].y)
if (map2d.is_valid_map_coord(x_hit_map, y_hit_map)
and scans.ranges[scan_idx] < scans.max_range):
map2d.update_val_via_map_coord(x_map=x_hit_map,
y_map=y_hit_map,
value=1.0)
def __init__(self, *, x_min, x_max, \
reso, pred_sigma, meas_sigma, \
gt_landmark, init_pose=0.0):
self.x_min = x_min
self.x_max = x_max
self.reso = reso
self.init_pose = init_pose
self.x_tick_num = my_round((x_max - x_min) / reso) + 1
self.pred_sigma = pred_sigma
self.meas_sigma = meas_sigma
self.gt_landmark = gt_landmark
def __calc_t_dist_to_next_border(x0, x1):
dist = int(math.fabs(my_math.my_round(x0) - my_math.my_round(x1)))
if (x0 < x1):
dt_dx = 1 / (x1 - x0)
dx = ((math.floor(x0) + 1) - x0)
proc_dir = 1
next_t_dist = dt_dx * dx
elif(x1 < x0):
dt_dx = 1 / (x0 - x1)
dx = (x0 - math.floor(x0))
proc_dir = -1
next_t_dist = dt_dx * dx
else:
dt_dx = float('inf')
dx = 0.0
proc_dir = 0
next_t_dist = float('inf')
return next_t_dist, dt_dx, proc_dir, dist
def draw_point(map2d, x, y, width, value):
conf = map2d.get_map_config()
x_map, y_map = GridMap2D.convert_global_2_map(x, y, conf.x_min, conf.y_min, conf.reso)
half_width_map = my_round(width / conf.reso / 2.0)
map2d.fill_val_via_map_coord(
x_map_st=x_map-half_width_map,
y_map_st=y_map-half_width_map,
x_map_en=x_map+half_width_map,
y_map_en=y_map+half_width_map,
value=value)
def __init__(self,
*,
range_max,
min_angle,
max_angle,
angle_res,
sigma=0.0):
self.range_max = range_max
self.min_angle = min_angle
self.max_angle = max_angle
self.angle_res = angle_res
self.sigma = sigma
self.ray_cnt = int(my_round((max_angle - min_angle) / angle_res) + 1)
def ray_tracing(st_pnt, end_pnt, min_pnt, max_pnt):
next_t_dist_ver, dt_dx, proc_dir_x, dist_x =\
RayTracing2D.__calc_t_dist_to_next_border(st_pnt.x, end_pnt.x)
next_t_dist_hor, dt_dy, proc_dir_y, dist_y =\
RayTracing2D.__calc_t_dist_to_next_border(st_pnt.y, end_pnt.y)
total_dist = dist_x + dist_y + 1
x = my_math.my_round(st_pnt.x)
y = my_math.my_round(st_pnt.y)
path = np.zeros((2, total_dist), dtype=int)
path = [MapIntCoord2D() for i in range(total_dist)]
path[0].x = x
path[0].y = y
cnt = 1
for i in range(total_dist - 1, 0, -1):
if (next_t_dist_ver < next_t_dist_hor):
x = x + proc_dir_x
next_t_dist_ver = next_t_dist_ver + dt_dx
else:
y = y + proc_dir_y
next_t_dist_hor = next_t_dist_hor + dt_dy
if ((x < min_pnt.x or max_pnt.x < x) or \
(y < min_pnt.y or max_pnt.y < y)):
break
path[cnt].x = x
path[cnt].y = y
cnt = cnt + 1
return path[0:cnt]
def __init__(self, *, histogram_config):
self._config = histogram_config
self._last_prob = np.array(\
[0.0 for i in range(self._config.x_tick_num)])
self._new_prob = np.array(\
[0.0 for i in range(self._config.x_tick_num)])
st_pos = my_round((self._config.init_pose - \
self._config.x_min) / self._config.reso)
self._last_prob[st_pos] = 1.0 / self._config.reso
self._new_prob[st_pos] = 1.0 / self._config.reso
delta = self._config.reso * 0.00000001
self._x_ticks = np.mgrid[\
slice(self._config.x_min-self._config.reso/2.0, \
self._config.x_max+self._config.reso/2.0+delta, \
self._config.reso)]
def convert_global_2_map(x, y, min_x, min_y, reso): x_map = my_round((x - min_x)/reso) y_map = my_round((y - min_y)/reso) return x_map, y_map