# How to get a real-time map using the laser tf and laser scans

Hello, I have written code based on the algorithm for Occupancy grid mapping presented in Probabilistic Robotics by Sebastian Thrun, Wolfram Burgard and Dieter Fox. However, I have ran into some issues as the mapping is extremely slow, since it iterates through every cell. The code:

def update_map(self):
#for each line of cells
for i in range(self.map.info.height):
#for each cell in that line of cells
for j in range(self.map.info.width):
self.logodds[i][j] += self.inverse_sensor_model(i, j) - self.l_0

def inverse_sensor_model(self, j, i):
#distance from cell center of mass x-coordinate to laser center x-coordinate
x_dist = self.grid_cells_center[i][j] - self.pose1
#distance from cell center of mass y-coordinate to laser center y-coordinate
y_dist = self.grid_cells_center[i][j] - self.pose1
#Calculate r (relative range), phi (relative bearing) and k (beam index of the closet beam to cell mi)
r = sqrt((x_dist**2)+(y_dist**2))
phi = np.arctan2(y_dist, x_dist) - self.pose1
k = np.argmin(np.abs(self.scanner_bearings - phi))

out_of_perceptual_field = (r > min(self.z_max, self.scanner_ranges[k] + self.alpha/2.0)) or(abs(phi - self.scanner_bearings[k]) > self.beta/2.0) or (self.scanner_ranges[k] > self.z_max) or (self.scanner_ranges[k] < self.z_min)
condition_occ = (self.scanner_ranges[k] < self.z_max) and (abs(r - self.scanner_ranges[k]) < self.alpha/2.0)
condition_free = r <= self.scanner_ranges[k]
inside_perceptual_field = not (out_of_perceptual_field)

if out_of_perceptual_field:
return self.l_0
elif (inside_perceptual_field and condition_occ):
return self.l_occ
elif (inside_perceptual_field and condition_free):
return self.l_free


So I found somewhere on the internet an example using masks to access only the required cells and implemented that too, however it did not save more than 3 seconds from the time it takes for each update to finish. The Code:

def update_map(self):

dist = self.grid_cells_center.copy()
#distance from cell center of mass x-coordinate to laser center x-coordinate
dist[0, :, :] = self.grid_cells_center - self.pose1
#distance from cell center of mass y-coordinate to laser center y-coordinate
dist[1, :, :] = self.grid_cells_center - self.pose1
#Calculate r (relative range), phi (relative bearing) and k (beam index of the closet beam to cell mi)
self.relative_ranges = scipy.linalg.norm(dist, axis = 0)
self.relative_bearings = np.arctan2(dist[1, :, :], dist[0, :, :]) - self.pose1

for i in range(len(self.scanner_ranges)):

out_of_perceptual_field = (self.relative_ranges > min(self.z_max, self.scanner_ranges[i] + self.alpha/2.0)) | (np.abs(self.relative_bearings - self.scanner_bearings[i]) > self.beta/2.0) | (self.scanner_ranges[i] > self.z_max) | (self.scanner_ranges[i] < self.z_min)
inside_perceptual_field = ~(out_of_perceptual_field)

occupied_cells = (np.abs(self.relative_ranges - self.scanner_ranges[i]) < self.alpha/2.0)
free_cells = (self.relative_ranges <= self.scanner_ranges[i])



Timing cell by cell update_map() function returned:

Time: 15.9444479942
Time: 16.2612850666
Time: 16.1209139824
Time: 15.9952340126
Time: 16.3198771477
Time: 16 ...
edit retag close merge delete

Sort by » oldest newest most voted you may implement it in C++ may this help, or use somethink like Cuda, OpenCL, OpemMP, TBB, multi Thread

more

1

Indeed, Python is a Scripting language, thus any Python implementation usually produces low performance results.

It is get worse when we talk about the SLAM problem when you have multiple things running, processing and computing. The advice given by @duck-development is accurate because having a program in a compilation language will give you better result when processing, iterating and computing things.