def update (t, P, MAP, N):
# 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)
for i in range(N):
[X,Y,theta] = P[i,:]
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
vp = np.stack((x_w,y_w))
corr[i] = np.max(mapCorrelation(MAP['map'],x_im,y_im,vp,x_range,y_range))
W = softmax(corr)
ind = np.argmax(W)
[X,Y,theta] = P[ind,:]
state = [X,Y,theta]
# resample
if 1/(W**2).sum() < 0.1:
W, P = resampling (W, P, N)
# =============================================================================
# print(W)
# =============================================================================
return P,state,W
def update(mus, alphas, z, maps):
map_corr = np.zeros(len(mus))
for i in range(len(mus)):
x_old, y_old, theta_old = mus[i]
d_position = 2 * update_sigma_position / update_n_dposition
d_angle = 2 * update_sigma_angle / update_n_dangle
x_range = np.arange(-update_sigma_position,
update_sigma_position + d_position, d_position)
y_range = np.arange(-update_sigma_position,
update_sigma_position + d_position, d_position)
theta_range = np.arange(-update_sigma_angle, update_sigma_angle +
d_angle, d_angle) + theta_old
for theta in theta_range:
wTb = np.matrix([[np.cos(theta), -np.sin(theta), x_old],
[np.sin(theta),
np.cos(theta), y_old], [0, 0, 1]])
xs0w = wTb * np.vstack(
(z * np.cos(angles), z * np.sin(angles), np.ones(
angles.shape)))
Y = xs0w[:2]
x_im = np.arange(MAP['xmin'], MAP['xmax'] + MAP['res'],
MAP['res']) #x-positions of each pixel of the map
y_im = np.arange(MAP['ymin'], MAP['ymax'] + MAP['res'],
MAP['res']) #y-positions of each pixel of the map
c = map_utils.mapCorrelation(maps[i], x_im, y_im, Y, x_range,
y_range)
if map_corr[i] < c.max():
j, k = np.unravel_index(c.argmax(), c.shape)
mus[i][0] = x_range[j] + x_old
mus[i][1] = y_range[k] + y_old
mus[i][2] = theta
map_corr[i] = float(np.max(c))
alphas *= np.exp(map_corr - map_corr.max())
alphas /= alphas.sum()
return alphas, mus
def update(MAP, Sw, weight, ranges, angles, bTl):
# grid cells representing walls with 1
map_wall = ((1 - 1 / (1 + np.exp(MAP['map']))) > 0.5).astype(np.int)
x_im = np.arange(MAP['xmin'], MAP['xmax'] + MAP['res'],
MAP['res']) # x-positions of each pixel of the map
y_im = np.arange(MAP['ymin'], MAP['ymax'] + MAP['res'],
MAP['res']) # y-positions of each pixel of the map
# 9 x 9
x_range = np.arange(-4 * MAP['res'], 5 * MAP['res'],
MAP['res']) # x deviation
y_range = np.arange(-4 * MAP['res'], 5 * MAP['res'],
MAP['res']) # y deviation
# 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 in body frame
s_h = np.ones((4, np.size(ex)))
s_h[0, :] = ex
s_h[1, :] = ey
s_h = np.dot(bTl, s_h)
numofparticles = np.shape(Sw)[1]
correlation = np.zeros(numofparticles)
for i in range(numofparticles):
xt = Sw[:, i]
# 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]])
# transformed into world frame
s_w = np.dot(wTb, s_h)
ex_w = s_w[0, :]
ey_w = s_w[1, :]
Y = np.stack((ex_w, ey_w))
# calculate correlation
c = map_utils.mapCorrelation(map_wall, x_im, y_im, Y, x_range, y_range)
# find best correlation
correlation[i] = np.max(c)
# ind = np.unravel_index(np.argmax(c, axis=None), c.shape)
# Sw[0,:]+=x_range[ind[0]]
# Sw[1,:]+=x_range[ind[1]]
# update particle weight
ph = softmax(correlation)
weight = weight * ph / np.sum(weight * ph)
return Sw, weight
def predict(self, pose, scan):
'''
Prediction of particle filter.
Args:
pose: encoder pose (x, y, theta)
scan: lidar ranges
Returns:
Updated `cor_mle`.
'''
# particles
pose_diff = pose - self.prev_pose
self.prev_pose = pose
self.particles += pose_diff + randn(self.N, 3) * self.noise
self.particles[:, 2] %= 2 * np.pi
self.cor_mle = np.zeros(self.N)
# index of x and y coordinate
x_im, y_im = np.arange(
self.grid.shape[0]), np.arange(self.grid.shape[1])
# 5x5 window
xs, ys = np.arange(-2*self.res, 3*self.res,
self.res), np.arange(-2*self.res, 3*self.res, self.res)
# temp grid for computing map correlation
temp = np.zeros_like(self.grid)
temp[self.grid > 0] = 1
temp[self.grid < 0] = -1
# iterate N particles
for j in range(self.N):
# pose in world frame
theta = (self.lidar_angles + self.particles[j][2]).reshape((-1, 1))
x = scan * np.cos(theta) / self.res + self.grid.shape[0]//2
y = scan * np.sin(theta) / self.res + self.grid.shape[1]//2
# map correlation
mpcor = mapCorrelation(temp, x_im, y_im, np.vstack(
(x, y)), self.particles[j][0]+xs, self.particles[j][1]+ys)
self.cor_mle[j] = np.max(mpcor)
def update_particles(self, MAP, particles, lidar_range, angles):
'''
From current location of the particles and the lidar scan
update the new map
'''
from TEST import Mapping
mp = Mapping()
angles, ranges = mp.rm_ld(angles, lidar_range)
(x_l, y_l) = mp.pol2car(angles, ranges)
h_l = mp.car2hom(x_l, y_l)
h_b = mp.trans_l2b(h_l)
cor_max = []
for i in range(particles.shape[0]):
x_p = particles[i, 0]
y_p = particles[i, 1]
theta_p = particles[i, 2]
hp_w = mp.trans_b2w(h_b, x_p, y_p, theta_p)
xp_w, yp_w = mp.hom2car(hp_w)
# convert position in the map frame here
Y = np.stack((xp_w, yp_w))
x_im = np.arange(MAP['xmin'], MAP['xmax'] + MAP['res'],
MAP['res']) #x-positions of each pixel of the map
y_im = np.arange(MAP['ymin'], MAP['ymax'] + MAP['res'],
MAP['res']) #y-positions of each pixel of the map
x_range = np.arange(-0.2, 0.2 + 0.05, 0.05)
y_range = np.arange(-0.2, 0.2 + 0.05, 0.05)
cor = abs(
map_utils.mapCorrelation(MAP['map'], x_im, y_im, Y, x_range,
y_range))
cor_max.append(np.max(cor))
return cor, cor_max
def slam(X,W,encoder_stamps,encoder_pos,lidar_stamsp,lidar_ranges,grid,res,SLAM_MAP,WALK_MAP,Texture_map):
#video = cv2.VideoWriter('test.avi', -1, 1, (grid.shape[0], grid.shape[1]), isColor=3)
now_list = []
pred_pos = encoder_pos[0]
noise = np.array([0.02, 0.02, 0.5 * np.pi /180])
for t, lidar in enumerate(lidar_stamsp[::10]):
print('\n')
print(t)
scan = lidar_ranges[:,t*10]
# time alignment
encoder_idx = np.argmin(np.abs(lidar-encoder_stamps))
encoder_delta = encoder_pos[encoder_idx] - pred_pos
# predict
noises = np.random.randn(N,3)*noise # maybe scale here
X = X + encoder_delta + noises
X[:,2] %= 2*math.pi
x_im,y_im = np.arange(grid.shape[0]),np.arange(grid.shape[1])
xs = ys = np.arange(-res*4,res*4+res,res)
tem = np.zeros_like(grid)
tem[grid>0] = 1
tem[grid<0] = -1
corr_max = []
for i in range(len(X)):
angles = lidar_angle + X[i,2]
valid_scan = np.logical_and(scan>=0.1,scan<=30)
ranges = scan[valid_scan]
theta = angles[valid_scan]
# turn to Cartesian coordinate system
xx, yy = ranges * np.cos(theta), ranges * np.sin(theta)
# turn to cells
xx = xx/res + grid.shape[0]//2
yy = yy/res + grid.shape[1]//2
cor = mapCorrelation(grid,x_im,y_im,np.vstack((xx,yy)), (X[i,0]+xs)/res, (X[i,1]+ys)/res)
corr_max.append(np.max(cor))
# update particle weights
corr_max = W * np.array(corr_max)
e_x = np.exp(corr_max-np.max(corr_max))
W = e_x / e_x.sum()
# find best particle
best_particle = np.where(W == np.max(W))[0][0]
now_best = X[best_particle].copy()
now_best = now_best.ravel()
test = now_best.copy()
now_best[0]/= res
now_best[1]/= res
grid = mapping(grid,scan,now_best,res,lidar_angle)
img = np.zeros((grid_size, grid_size, 3))
img[:, :, 0] = grid
img[:, :, 1] = grid
img[:, :, 2] = grid
cv2.circle(img,(int(now_best[1]) + grid.shape[1]//2,int(now_best[0]) + grid.shape[0]//2),4,(0,0,255),-1)
if t% 50 == 1:
cv2.imwrite(f"data\\test_{t}.png",img.astype(np.uint8))
#video.write(img.astype(np.uint8))
if 1/(W**2).sum() < (0.85 * N):
idx = stratified_resample(W)
W.fill(1.0 / N)
X[:] = X[idx]
SLAM_MAP[int(now_best[0]) + SLAM_MAP.shape[0]//2, int(now_best[1]) + SLAM_MAP.shape[1]//2] = 1
WALK_MAP[int(encoder_pos[encoder_idx][0]/res) + WALK_MAP.shape[0]//2, int(encoder_pos[encoder_idx][1]/res) + WALK_MAP.shape[1]//2] = 1
now_list.append(now_best)
pred_pos = encoder_pos[encoder_idx]
cors = []
x_im, y_im = np.arange(grid.shape[0]), np.arange(grid.shape[1])
l = 1
xs, ys = np.arange(-res * l, res * l + res,
res), np.arange(-res * l, res * l + res, res)
temp = np.zeros_like(grid)
temp[grid > 0] = 1
temp[grid < 0] = -1
for i in range(len(X)):
world_angles = lidar_angles + X[i][2]
x, y = lidar * np.cos(world_angles), lidar * np.sin(world_angles)
x, y = x / res + grid.shape[0] // 2, y / res + grid.shape[1] // 2
cor = mapCorrelation(temp, x_im, y_im, np.vstack((x, y)),
(X[i][0] + xs) / res, (X[i][1] + ys) / res)
cors.append(np.max(cor))
cors = W * np.array(cors)
W = softmax(cors)
best = np.where(W == np.max(W))[0][0]
now = X[best].copy()
now[0] /= res
now[1] /= res
img_name = 'rgb%d_%d.png' % (dataset, cor_rgb[lidar_index])
depth_name = 'disparity%d_%d.png' % (dataset, cor_depth[lidar_index])
grid = mapping(grid, lidar, now, res, lidar_angles)
now_texture = now.copy()
def ParticleFilter(self,particles_number):
newranges = self.lidar_ranges
# take valid indices
valid = np.logical_and((newranges[:,0] < 30),(newranges[:,0]> 0.1))
newranges = newranges[valid]
angles = self.angles[valid]
newranges = uniformscandata(newranges,self.lidar_stamps,self.encoder_stamps)
lamda = np.zeros((self.frame_size,self.frame_size))
walkmap = np.zeros((self.frame_size,self.frame_size))
slammap = np.zeros((self.frame_size,self.frame_size))
now_tra = np.zeros((3,np.size(newranges,1)))
x,y,theta,v,w = self.InputMotion()
#particle number
N=particles_number
particle = np.zeros((N,3))
W = np.ones(N)/N
res = 0.05
dif = 2
x_im = np.arange(-30,30)
y_im = np.arange(-30,30)
x_dif,y_dif = np.arange(-dif*res,dif*res,res),np.arange(-dif*res,dif*res,res)
now = np.zeros((3,))
noise1 = np.array([0.05, 0.05, 0.1*np.pi/180])
noise2 = np.array([0.01, 0.01, 0.03*np.pi/180])
cors = []
for j in range(N):
noises = np.random.randn(1,3)*noise1
particle[j,:] = [x[0],y[0],theta[0]]+noises
#
for i in range(1,np.size(newranges,1)):
for j in range(N):
# print('particle'+str(j))
noises = np.random.randn(1,3)*noise2
dx,dy,dtheta = motion_diff(v,w,particle[j,2],i,self.encoder_stamps[i]-self.encoder_stamps[i-1])
particle[j,:] = [particle[j,0]+dx,particle[j,1]+dy,particle[j,2]+dtheta]+noises
particle[:,2] %= 2*np.pi
xs = newranges[:,i]*np.cos(angles)+self.lidaroriginx
ys = newranges[:,i]*np.sin(angles)+self.lidaroriginy
[xs,ys,z]= BodytoWorld(xs,ys,theta[i])
cors = []
temp = np.zeros_like(lamda)
temp[lamda>0] = 1
temp[lamda<0] = -1
for j in range(N):
particle_cor_x, particle_cor_y = x_dif+particle[j,0], y_dif+particle[j,1]
cor = mapCorrelation(temp,x_im,y_im,np.vstack((xs, ys)),particle_cor_x,particle_cor_y)
cors.append(np.max(cor))
cors = W * np.array(cors)
W = softmax(cors)
best = np.argmax(W)
now = particle[best,:].copy()
now_tra[:,i] = [now[0],now[1],now[2]]
now_map_x,now_map_y = WorldtoMap(now[0],now[1])
best_lidar_map_x,best_lidar_map_y = WorldtoMap(now[0]+0.298*math.cos(now[2]),now[1]+0.298*math.sin(now[2]))
xis,yis = WorldtoMap(xs+x[i],ys+y[i])
lamda = mapping(lamda,best_lidar_map_x,best_lidar_map_y,xis,yis)
bodyxis,bodyyis = WorldtoMap(x[i],y[i])
walkmap[bodyxis,bodyyis] = 1
slammap[now_map_x,now_map_y] = 1
return lamda,now_tra,walkmap,slammap
