Rewrite the inverse_sensor_model. Before that, the occupied cells couldn't be shown

plotters/MappingPlotter.py

+from math import log
 class MappingPlotter:
 
     def __init__(self, slam_mapping, viewer, frame_number):
@@ -25,9 +26,14 @@ class MappingPlotter:
         for j in range(H):
             for i in range(W):
                 val = map[j, i]  # the occupancy probability
-                if val >= 0.5:
+                if abs(val - 0.5) < 0.0001: # unknown area
                     x, y = self.__to_meter(i, j)
-                    frame.add_rectangle([x, y], self.pixel_size, self.pixel_size, color=(0.5, 0.5, 0.5), alpha=0.5)
+                    frame.add_rectangle([x, y], self.pixel_size, self.pixel_size, color=(0.5, 0.5, 0.5), alpha=0.3)
+                elif val > 0.5:  # occupied area
+                    x, y = self.__to_meter(i, j)
+                    val = 1-val
+                    frame.add_rectangle([x, y], self.pixel_size, self.pixel_size, color=(val, val, val), alpha=0.8)
+
         self.draw_path_planning_to_frame(frame)
         self._draw_goal_to_frame(frame)
 

plotters/SlamPlotter.py

@@ -23,6 +23,7 @@ from matplotlib import pyplot as plt
 import numpy as np
 
 from supervisor.slam.EKFSlam import EKFSlam
+from supervisor.slam.FastSlam import FastSlam
 
 
 class SlamPlotter:
@@ -52,7 +53,7 @@ class SlamPlotter:
 
         # draw all the obstacles
         for landmark in self.slam.get_landmarks():
-            frame.add_circle((landmark[0], landmark[1]), self.radius, "black", alpha=0.6)
+            frame.add_circle((landmark[0], landmark[1]), self.radius, "darkgreen", alpha=0.8)
 
         if self.viewer.draw_invisibles and isinstance(self.slam, EKFSlam):
             self.__draw_confidence_ellipse(frame)

supervisor/slam/mapping.py

@@ -57,8 +57,8 @@ class OccupancyGridMapping2d(Mapping):
         self.max_range = supervisor_interface.proximity_sensor_max_range()
 
         self.prob_unknown = 0.5 # prior
-        self.prob_occ = 0.99 # probability perceptual a grid is occupied
-        self.prob_free = 0.01 # probability perceptual a grid is free
+        self.prob_occ = 0.9 # probability perceptual a grid is occupied
+        self.prob_free = 0.1 # probability perceptual a grid is free
 
         self.reset() # initialize the algorithm
 
@@ -91,7 +91,7 @@ class OccupancyGridMapping2d(Mapping):
             x0, y0 = self.__to_gridmap_position(x0, y0) # from position in meters to position in pixels
             x1, y1 = self.__to_gridmap_position(x1, y1) # from position in meters to position in pixels
             points = bresenham_line(x0, y0, x1, y1) # a list of points on the line
-            occ_probs = self.__calc_prob(points)  # calculate the occupancy probabilities on this line
+            occ_probs = self.__inverse_sensor_model(points)  # calculate the occupancy probabilities on this line
             observed_pixs += occ_probs
 
         inverse_sensor = np.copy(self.L0) # initialize the inverse-sensor-model term by prior
@@ -150,31 +150,33 @@ class OccupancyGridMapping2d(Mapping):
             else:
                 self.path = list()
 
-    def __calc_prob(self, points):
+    def __inverse_sensor_model(self, points):
         """
-        Calculate the probability the occupied points on a segment
+        Inverse sensor model. Calculate the probability the occupied points on a segment
         :param points: points on a segment in pixels
         :return:  A list of occupancy probabilities of the grids on the segment.
                     A single item is (x, y, prob) where (x, y) is the prosition of a grid, and prob is the occupancy probability.
         """
+        a = 0.3*self.resolution # the thick of the obstacles
         x0, y0 = points[0] # start point position
-        xn, yn = points[-1] # end point position
-        seg_length = sqrt((xn - x0)**2 + (yn - y0)**2) # the length of the segment
-        max_range_seg =  self.max_range*self.resolution
+        xn, yn = points[-1] # end point position, the position of a obstacle
+        z_t = sqrt((xn - x0)**2 + (yn - y0)**2) # the length of the segment, z_t
+        z_max =  self.max_range*self.resolution
         probs = [] # occupancy probabilities of the grids on the segment.
         for xi, yi in points:
+            r = sqrt((xi - x0) ** 2 + (yi - y0) ** 2)  # a distance of a cell from the robot
             prob = self.prob_unknown
-            if seg_length >= max_range_seg:     # out of range
-                prob = self.prob_unknown        # assign prob with prior
-            if seg_length < max_range_seg:    # likely to be an object
-                prob = self.prob_occ            # assign prob with the occupied probability
-
-            distance = sqrt((xi - x0)**2 + (yi - y0)**2)
-            if distance < seg_length:                  # free space
+            if r > min(z_max, z_t + a*0.5): # return a prior
+                prob = self.prob_unknown
+            if z_t < z_max and abs(r-z_t) < a*0.5: # return a occ probability
+                prob = self.prob_occ
+            if r < z_t:
                 prob = self.prob_free
             probs.append((int(xi), int(yi), prob))
+
         return probs
 
+
     def blur(self, image):
         """
         Blur the map