def mapping(t, state):
# mapping
# remove points
angles = np.arange(lidar_angle_min,
lidar_angle_max + lidar_angle_increment - 1e-3,
lidar_angle_increment)
ranges = lidar_ranges[:, t]
valid = np.logical_and((ranges < lidar_range_max),
(ranges > lidar_range_min))
angles = angles[valid]
ranges = ranges[valid]
# from lidar frame to world frame
x_b = ranges * np.cos(angles)
y_b = ranges * np.sin(angles)
x_w = x_b * np.cos(theta) - y_b * np.sin(theta) + X
y_w = x_b * np.sin(theta) + y_b * np.cos(theta) + Y
x = cell(X, MAP) # location of particle
y = cell(Y, MAP)
x_s = cell(x_w, MAP) # locaiton of sensors beam
y_s = cell(y_w, MAP)
# build map
for k in range(x_s.shape[0]):
xb, yb = bresenham2D(x, y, x_s[k], y_s[k])
indGood = np.logical_and((xb > 1), (yb > 1))
MAP['map'][xb[indGood].astype(np.int16),
yb[indGood].astype(np.int16)] += np.log(4) # MAP = 1
indGood = np.logical_and((x_s > 1), (y_s > 1))
MAP['map'][x_s[indGood], y_s[indGood]] -= np.log(4) # MAP = 0
MAP['map'][MAP['map'] > 0] = 0
return MAP
def mapping(grid, pose, scan, res, empty_odds, ocpy_odds):
'''
Update the grid map with log-odds.
Args:
grid (80, 80): grid map
scan (1081,): lidar ranges
pose (3,): pose (x,y,theta)
res (float or int): resolution
empty_odds (float): empty odds
ocpy_odds (float): occupied odds
Returns:
grid map (80, 80)
'''
# pixel value
value = 127
# convert from polar to Cartesian
x, y = pol2cart(scan, pose[2]) # 1-d array
# discretization (1-d array)
xi = (x / res).astype(int)
yi = (y / res).astype(int)
wall_set = np.zeros_like(grid)
empty_set = np.zeros_like(grid)
for i in range(len(xi)):
a, b = xi.item(i), yi.item(i)
x1 = a + int(pose[0]) + grid.shape[0] // 2
y1 = b + int(pose[1]) + grid.shape[1] // 2
if 0 <= x1 < grid.shape[0] and 0 <= y1 < grid.shape[1]:
wall_set[x1, y1] = 1
line = np.array(bresenham2D(0, 0, a, b)).astype(int)
for j in range(len(line[0]) - 1):
x2 = line[0][j] + int(pose[0]) + grid.shape[0] // 2
y2 = line[1][j] + int(pose[1]) + grid.shape[1] // 2
if 0 <= x2 < grid.shape[0] and 0 <= y2 < grid.shape[1]:
empty_set[x2, y2] = 1
grid[wall_set == 1] += ocpy_odds
grid[empty_set == 1] += empty_odds
grid[grid >= value] = value
grid[grid < -value] = -value
return grid
def mapupdate(MAP, xt, ranges, angles, bTl):
# wTb -- body to world transform
x_w = xt[0]
y_w = xt[1]
theta_w = xt[2]
wTb = np.array([[np.cos(theta_w), -np.sin(theta_w), 0, x_w],
[np.sin(theta_w), np.cos(theta_w), 0, y_w], [0, 0, 1, 0],
[0, 0, 0, 1]])
# start point of laser beam in grid units in world frame
sx = np.ceil((x_w - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
sy = np.ceil((y_w - MAP['ymin']) / MAP['res']).astype(np.int16) - 1
# end point of laser beam in phisical units in laser frame
ex = ranges * np.cos(angles)
ey = ranges * np.sin(angles)
# convert to homogenized coordinates
s_h = np.ones((4, np.size(ex)))
s_h[0, :] = ex
s_h[1, :] = ey
s_h[2, :] = 0.51435
# transformed into world frame
s_h = np.dot(wTb, np.dot(bTl, s_h))
# end point of laser beam in grid units in world frame
ex = s_h[0, :]
ey = s_h[1, :]
ex = np.ceil((ex - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
ey = np.ceil((ey - MAP['ymin']) / MAP['res']).astype(np.int16) - 1
# for each laser scan update the map
for i in range(np.size(ranges)):
passed_points = map_utils.bresenham2D(sx, sy, ex[i], ey[i])
xis = passed_points[0, :].astype(np.int16)
yis = passed_points[1, :].astype(np.int16)
indGood = np.logical_and(
np.logical_and(np.logical_and((xis > 1), (yis > 1)),
(xis < MAP['sizex'])), (yis < MAP['sizey']))
# update the log-odds map
MAP['map'][xis[indGood], yis[indGood]] += np.log(1 / 4)
# MAP['map'][xis,yis] += np.log(1/4)
if ((ex[i] > 1) and (ex[i] < MAP['sizex']) and (ey[i] > 1)
and (ey[i] < MAP['sizey'])):
MAP['map'][ex[i], ey[i]] += 2 * np.log(4)
# limit the range to prevent over-confidence
MAP['map'] = np.clip(MAP['map'], 10 * np.log(1 / 4), 10 * np.log(4))
return MAP
def mapping(lamda,originx,originy, scanx,scany):
ratio = 4
map2D = []
for i in range(len(scanx)):
map2D.append(bresenham2D(originx,originy,scanx[i],scany[i]))
for k in map2D:
[m,n] = np.shape(k)
for j in range(n):
# if k[0,j] <1601 and k[1,j] < 1601:
if j != n-1:
lamda[int(k[0,j]),int(k[1,j])] = lamda[int(k[0,j]),int(k[1,j])] + math.log(1/ratio)
else:
lamda[int(k[0,j]),int(k[1,j])] = lamda[int(k[0,j]),int(k[1,j])] + math.log(ratio)
return lamda
def mapping_particle(logodds, z, x, y, theta):
wTb = np.matrix([[np.cos(theta), -np.sin(theta), x],
[np.sin(theta), np.cos(theta), y], [0, 1, 0]])
zb = np.matrix(
(z * np.cos(angles), z * np.sin(angles), np.ones(angles.shape)))
zw = wTb * zb
x = x / MAP['res'] + MAP['sizex'] / 2
y = y / MAP['res'] + MAP['sizey'] / 2
zw[0] = zw[0] / MAP['res'] + MAP['sizex'] / 2
zw[1] = zw[1] / MAP['res'] + MAP['sizey'] / 2
for i in range(len(angles)):
x1, y1 = map_utils.bresenham2D(x, y, zw[0, i], zw[1, i])
for j, k in zip(x1[:-1], y1[:-1]):
logodds[int(j), int(k)] -= 2
logodds[int(x1[-1]), int(
y1[-1])] += 2 #If this fails, enlarge the map size. (physicalsize)
np.clip(logodds, -logodds_thresh, logodds_thresh, out=logodds)
return logodds
def mapping(log_odd, b_4, best_particle, w_T_b_best, MAP, b=4):
occu_max = 30
occu_min = -30
body_point = b_4
world_point = np.dot(w_T_b_best, body_point)
#bool_position = world_point[2,:] >0 #>= 0.1 + 0.93
#world_point = world_point[:, bool_position]
x_particle = np.ceil(
(best_particle[0] - MAP['xmin']) / MAP['res']).astype(np.int16) - 1
y_particle = np.ceil(
(best_particle[1] - MAP['ymin']) / MAP['res']).astype(np.int16) - 1
for i in range(world_point.shape[1]):
ex = np.ceil((world_point[0, i] - MAP['xmin']) / MAP['res']).astype(
np.int16) - 1 #in map coordinate to the pixel
ey = np.ceil((world_point[1, i] - MAP['ymin']) / MAP['res']).astype(
np.int16) - 1 #in map coordinate to the pixel
if ex > 1 and ey > 1 and ex < MAP['sizex'] and ey < MAP['sizey']:
pass
else:
continue
scan_section = map_utils.bresenham2D(
x_particle.item(), y_particle.item(), ex.item(),
ey.item()) #a beam section for just one end
scan_section = scan_section.astype(int)
log_odd[scan_section[0][-1], scan_section[1][-1:]] = \
log_odd[scan_section[0][-1], scan_section[1][-1]] + math.log(b)
log_odd[scan_section[0][1:-1], scan_section[1][1:-1]] = \
log_odd[scan_section[0][1:-1], scan_section[1][1:-1]] + math.log(1 / b)
log_odd[log_odd > occu_max] = occu_max
log_odd[log_odd < occu_min] = occu_min
P_occupied = 1 - expit(-log_odd)
bool_occupied_cells = P_occupied > 0.5
bool_free_cells = P_occupied < 0.5
mt = bool_free_cells * (-1) + bool_occupied_cells * 1
return log_odd, mt
def mapping(grid, lidar_scan, now, res, angle):
valid_scan = np.logical_and(lidar_scan >= 0.1, lidar_scan <= 30)
#valid_idx, = np.where(valid_scan==True)
empty_odds = np.log(0.9 / 0.1)
occuy_odds = np.log(0.9 / 0.1)
range_valid = lidar_scan[valid_scan]
theta = angle[valid_scan] + now[2]
x = range_valid * np.cos(theta)
y = range_valid * np.sin(theta)
x_cell = (x / res).astype(int)
y_cell = (y / res).astype(int)
e_cell = {}
o_cell = {}
for (a, b) in zip(x_cell, y_cell):
lines = bresenham2D(0, 0, a, b).astype(int)
xx = a + int(now[0]) + grid.shape[0] // 2
yy = b + int(now[1]) + grid.shape[1] // 2
o_cell[(xx, yy)] = True
for i in range(len(lines[0]) - 1): # the last col is wall
e_cell[lines[0, i], lines[1, i]] = True
for k, _ in o_cell.items():
if 0 <= k[0] < grid.shape[0] and 0 <= k[1] < grid.shape[1]:
grid[k[0], k[1]] += occuy_odds
for k, _ in e_cell.items():
xxx = k[0] + int(now[0]) + grid.shape[0] // 2
yyy = k[1] + int(now[1]) + grid.shape[1] // 2
if 0 <= xxx < grid.shape[0] and 0 <= yyy < grid.shape[1]:
grid[xxx, yyy] -= empty_odds
sat = 127
grid[grid > sat] = sat
grid[grid < -sat] = -sat
return grid
def mapping(grid, scan, now, res, map_angles):
free_odds = np.log(0.9 / 0.1) / 4
occup_odds = np.log(0.9 / 0.1)
saturated = 127
#valid = valid_scan(scan, map_angles)
distance = scan
theta = map_angles + now[2]
x, y = distance * np.cos(theta), distance * np.sin(theta)
xi = (x / res).astype(int)
yi = (y / res).astype(int)
free_set = {}
wall_set = {}
for (a, b) in zip(xi, yi):
line = np.array(bresenham2D(0, 0, a, b)).astype(int)
xx = a + int(now[0]) + grid.shape[0] // 2
yy = b + int(now[1]) + grid.shape[1] // 2
wall_set[xx, yy] = True
for j in range(len(line[0]) - 1):
free_set[(line[0][j], line[1][j])] = True
for k, _ in wall_set.items():
xx, yy = k[0], k[1]
if 0 <= xx < grid.shape[0] and 0 <= yy < grid.shape[1]:
grid[xx, yy] += occup_odds
#print('xx,yy', xx,yy)
for k, _ in free_set.items():
xx = k[0] + int(now[0]) + grid.shape[0] // 2
yy = k[1] + int(now[1]) + grid.shape[1] // 2
if 0 <= xx < grid.shape[0] and 0 <= yy < grid.shape[1]:
grid[xx, yy] -= free_odds
grid[grid > saturated] = saturated
grid[grid < -saturated] = -saturated
return grid
lidar_stamps[count[2]]
])
count[arg] = count[arg] + 1
# encoder
if arg == 0:
# time difference
tau = encoder_stamps[count[0] + 1] - encoder_stamps[count[0]]
[X, Y, theta] = state
u = drive_model(yaw, encoder_counts[:, count[0] + 1], tau)
state_0 = state
state = state + u
# trajectory
xt = cell(state[0], MAP)
yt = cell(state[1], MAP)
xtp = cell(state_0[0], MAP)
ytp = cell(state_0[1], MAP)
x_i, y_i = bresenham2D(xtp, ytp, xt, yt).astype(int)
MAP['track'][x_i, y_i] = -2000
traj.append(state)
elif arg == 1:
[_, _, yaw1] = imu_angular_velocity[:, count[1]]
[_, _, yaw2] = imu_angular_velocity[:, count[1] - 1]
yaw = (yaw1 + yaw2) / 2
else:
MAP = mapping(count[2], state)
if count[2] % 100 == 0: # show progress
print(count[2], 'lidar stamps')
plt.figure(figsize=(10, 10))
trajectory = np.asarray(traj)
plt.plot(trajectory[:, 0], trajectory[:, 1], '.k')
plt.savefig('dead%d.png' % dataset)