# 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[0][i][j] - self.pose1[0]
#distance from cell center of mass y-coordinate to laser center y-coordinate
y_dist = self.grid_cells_center[1][i][j] - self.pose1[1]
#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[2]
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[0] - self.pose1[0]
#distance from cell center of mass y-coordinate to laser center y-coordinate
dist[1, :, :] = self.grid_cells_center[1] - self.pose1[1]
#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[2]

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.2745289803


Time: 12.6104559898
Time: 13.7843110561
Time: 12.5238349438
Time: 14.500497818
Time: 13.8072030544
Time: 13.7702701092


I am using the same values for map width and height: 30m, grid size: 0.2 and laser scanner wide angle: 180º which is equivalent to 720 laser beams.

My goal is to make the map update function faster since as you can see it is pretty slow for this grid size and I need the grid size to be this small for the map to have good detail. But since I'm working with booleans and the second implementation with masks has a lot less iterations (= to the number of laser beams) I don't know how else I can optimize it.

Originally posted by Robotics_1920 on ROS Answers with karma: 3 on 2019-11-18

Post score: 0

you may implement it in C++ may this help, or use somethink like Cuda, OpenCL, OpemMP, TBB, multi Thread

Originally posted by duck-development with karma: 1999 on 2019-11-18

This answer was ACCEPTED on the original site

Post score: 1

Comment by Weasfas on 2019-11-21:
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.