def particle_update(self, l, m, t):
x_im = np.arange(m.xmin, m.xmax + m.res,
m.res) # x-positions of each pixel of the map
y_im = np.arange(m.ymin, m.ymax + m.res,
m.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)
# For positive x, y>0.5
# binary_map = np.zeros((m.size, m.size), dtype=np.float16)
binary_map = (np.copy(m.grid) > np.log(4)).astype(np.int)
corr = []
for n in range(self.num):
scan = l.scan_to_world(self.pose[n], t)
scan_copy = np.copy(scan)
c = p2_utils.mapCorrelation(binary_map, x_im, y_im, scan_copy,
x_range, y_range)
max_c = c.max()
corr.append(np.copy(max_c))
corr = np.array(corr)
# print("corr:", corr)
corr = corr / 2
# p_obs = softmax(corr - np.max(corr))
# self.weights = self.weights * p_obs / np.sum(self.weights * p_obs)
log_w = np.log(self.weights) + corr
log_w -= np.max(log_w) + logsumexp(log_w - np.max(log_w))
self.weights = np.exp(log_w)
def get_corr(l0, j0, particle, i, map):
# get 0-1 map
map_inv = np.zeros(map.shape)
map_inv[np.where(map > 0)] = 1
map_inv[np.where(map < 0)] = 0
#if i % 2000 == 0:
# plt.imshow(map_inv)
# plt.show()
# number of particle
shape = np.shape(particle)
Np = shape[1]
# init corr-matrix
C = np.zeros((1, Np))
for j in range(Np):
laser_pose_inWorld = mapping.get_laser_pose_inWorld(
l0, j0, i, particle[:, j:j + 1])
x_im = np.arange(-30, 30 + 0.05,
0.05) # x-positions of each pixel of the map
y_im = np.arange(-30, 30 + 0.05,
0.05) # y-positions of each pixel of the map
x_range = np.arange(
-0.2, 0.2 + 0.05,
0.05) #physical x,y,positions you want to evaluate "correlation"
y_range = np.arange(
-0.2, 0.2 + 0.05,
0.05) #physical x,y,positions you want to evaluate "correlation"
c = p2.mapCorrelation(
map_inv, x_im, y_im, laser_pose_inWorld[0:3, :], x_range, y_range
) # get corr of physical x,y,positions you want to evaluate "correlation"
corr_new = np.max(c) # value of max corr
x_index = np.where(c == np.max(c)) # index of the max corr
# adjust particle according the index of max corr
x_index = np.sum(x_index, axis=1) / len(x_index[0])
x_index = np.array([[x_index[0]], [x_index[1]]])
x_new = (x_index - 4) * 0.05
particle[0:2, j:j + 1] += x_new
C[0][j] = corr_new
# get observation function pz
pz = ss.softmax(C)
return particle, pz
def update(self, MAP, laser, robotPose, headAngles):
# Single Particle Update
laserScan = laser['scan']
y = self.getWorldFrameLaserMap(laserScan, robotPose, headAngles)
y = np.concatenate([y, np.zeros((1, y.shape[1]))], axis=0)
x_im = np.arange(self.xmin, self.xmax + self.resolution,
self.resolution)
y_im = np.arange(self.ymin, self.ymax + self.resolution,
self.resolution)
correlation = util.mapCorrelation(MAP['map'], x_im, y_im, y[0:3, :],
self.xRange, self.yRange)
ind = np.argmax(correlation)
indx = ind / 3
indy = ind % 3
corr = correlation[indx.astype(int), indy.astype(int)]
robotPose[0] += self.xRange[indx.astype(int)]
robotPose[1] += self.yRange[indy.astype(int)]
return robotPose, corr
def update(self, weights, MAP, laser, robotPose, headAngles):
# Single Particle Update
laserScan = laser['scan']
x_im = np.arange(
self.xmin, self.xmax + self.resolution,
self.resolution) # x-positions of each pixel of the map
y_im = np.arange(
self.ymin, self.ymax + self.resolution,
self.resolution) # y-positions of each pixel of the map
for i in range(robotPose.shape[0]):
y = self.getWorldFrameLaserMap(laserScan, robotPose[i, :],
headAngles)
y = np.concatenate([y, np.zeros((1, y.shape[1]))], axis=0)
correlation = util.mapCorrelation(MAP['map'], x_im, y_im,
y[0:3, :], self.xRange,
self.yRange)
ind = np.argmax(correlation)
indx = ind / 3
indy = ind % 3
corr = correlation[indx.astype(int), indy.astype(int)]
weights[i] = weights[i] * np.exp(corr)
return weights
def update_and_map(ranges,pose,MAP,head_angles,update_log_odds=False,plot_en=0,cur_scan=0):
#dataIn = io.loadmat("lidar/train_lidar0.mat")
angles = np.array([np.arange(-135,135.25,0.25)*np.pi/180.]).T
ranges = ranges.reshape((ranges.shape[0],1))
#ranges = np.double(dataIn['lidar'][0][110]['scan'][0][0]).T
# take valid indices
valid_range = np.logical_and((ranges < 30),(ranges> 0.1))
ranges = ranges[valid_range]
angles = angles[valid_range]
# xy position in the sensor frame
xs0 = np.array([ranges*np.cos(angles)])
ys0 = np.array([ranges*np.sin(angles)])
scan_ranges = np.vstack((xs0,ys0))
dummy = np.vstack((xs0*0,ys0*0))
scan_ranges = np.vstack((scan_ranges,dummy))
world = convert2world_frame(scan_ranges,pose,head_angles)
######## Drop Points that have negative z value in world frame ########
world_T = world.T
world_corrected = world_T[np.where(world[2,:] > 0)]
world_corrected = world_corrected.T
xs0 = world_corrected[0,:].reshape(1,world_corrected.shape[1])
ys0 = world_corrected[1,:].reshape(1,world_corrected.shape[1])
# convert position in the map frame here
# convert from meters to cells
xis = np.ceil((xs0 - MAP['xmin']) / MAP['res'] ).astype(np.int16)-1
yis = np.ceil((ys0 - MAP['ymin']) / MAP['res'] ).astype(np.int16)-1
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 Correlation Matrix ###
x_range = np.arange(-map_res * 4,map_res * 4 + map_res,map_res)
y_range = np.arange(-map_res * 4,map_res * 4 + map_res,map_res)
map_shape = MAP['log_odds'].shape
######## Update Log-Odds #########
pix_thrs = -num_true_events * np.log(lidar_confidence) ### Use this pixel only it was detected as free more than two times
if(update_log_odds == True):
free_cells = np.zeros(map_shape)
occupied_cells = np.zeros(map_shape)
################## Bug of the year was found here ##############
pose_x = np.ceil((pose[0] - MAP['xmin']) / MAP['res'] ).astype(np.int16)-1
pose_y = np.ceil((pose[1] - MAP['ymin']) / MAP['res'] ).astype(np.int16)-1
for scan in range(xis.shape[1]):
if(yis[0,scan] > MAP['sizey'] or xis[0,scan] > MAP['sizex']) :
pass
else:
free_cells = cv2.line(free_cells,(pose_x,pose_y),(xis[0,scan],yis[0,scan]),color = 1,thickness=1)
occupied_cells[yis[0,scan],xis[0,scan]] = 1
sensor_confidence = np.full(map_shape,np.log(lidar_confidence))
MAP["log_odds"] += 2 * occupied_cells * sensor_confidence #Because we are subtracting one value at occupied free cell
MAP["log_odds"] = MAP['log_odds'] - free_cells * sensor_confidence #Because we are subtracting one value at occupied free cell
MAP["log_odds"] = np.where(MAP["log_odds"] > log_thrs, np.full(map_shape,log_thrs),MAP["log_odds"])
MAP["log_odds"] = np.where(MAP["log_odds"] < -log_thrs, np.full(map_shape,-log_thrs),MAP["log_odds"])
MAP['x_loc'].append(pose_x)
MAP['y_loc'].append(pose_y)
MAP['theta'].append(pose[2])
################# Plot ################
plot_en_ = False
if(plot_en):
fig = plt.figure(figsize=(18,10))
map_threshold = np.zeros((map_shape[0],map_shape[1],3))
map_threshold[:,:,0] = np.where(MAP['log_odds'] <= pix_thrs, np.full(map_shape,1),np.zeros(map_shape))
map_threshold[:,:,1] = np.where(MAP['log_odds'] <= pix_thrs, np.full(map_shape,1),np.zeros(map_shape))
map_threshold[:,:,2] = np.where(MAP['log_odds'] <= pix_thrs, np.full(map_shape,1),np.zeros(map_shape))
plt.scatter(MAP['x_loc'],MAP['y_loc'],s=0.1,c='r')
plt.scatter(pose_x,pose_y,s=1,c='b')
img_name = '/datasets/home/94/594/adkulkar/sensing/SLAM/images/part_20_threshold_50/' + 'img_' + str(cur_scan) + '.png'
title_name = "Occupancy Map" + "Scan Number : " + str(cur_scan)
plt.title(title_name)
plt.xlabel("x")
plt.ylabel("y")
plt.imshow(map_threshold)
plt.savefig(img_name)
plt.show()
if(update_log_odds == True):
return MAP
####### Perform Correlation #######
Y = np.vstack((xs0,ys0))
map_threshold = np.where(MAP['log_odds'] >= -pix_thrs, np.ones(map_shape),np.zeros(map_shape))
correlation = mapCorrelation(map_threshold,x_im,y_im,Y,x_range,x_range)
return MAP,correlation