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

edit

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