Ask Your Question
0

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

asked 2019-11-18 06:57:29 -0600

Robotics_1920 gravatar image

updated 2019-11-18 07:01:12 -0600

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])

        occupied_mask = inside_perceptual_field & occupied_cells
        free_mask = inside_perceptual_field & free_cells

        occupied_mask = occupied_mask.T
        free_mask = free_mask.T

        self.logodds[free_mask] += self.l_free - self.l_0
        self.logodds[occupied_mask] += self.l_occ - self.l_0

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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-11-18 11:58:30 -0600

duck-development gravatar image

updated 2019-11-18 11:59:44 -0600

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

edit flag offensive delete link more

Comments

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.

Weasfas gravatar imageWeasfas ( 2019-11-21 05:55:44 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-11-18 06:57:29 -0600

Seen: 22 times

Last updated: Nov 18