* The laser scan data are represented as an array of distance measurements where each distance measurement $d_i$ is adjacent to a given scan angle $\theta_i$ with respect to the robot heading. Hence, the projection $\mathbf{p}_i$ of each scanned point is given as: $$\mathbf{p}_i = (x_i,y_i)^T = (d_i \cos(\theta_i), d_i \sin(\theta_i))^T$$

• The expected output of this step should be similar to the following video (8x speed-up)
  

• For each projected scan point $\mathbf{p}_i$, the odometry compensated point $\mathbf{o}_i$ is calculated according to the equation: $$\mathbf{o}_i = \mathbf{R}\cdot \mathbf{p}_i + \mathbf{T},$$ where $\mathbf{R}$ is the odometry orientation component represented by the rotation matrix (i.e. the orientation of the robot) and $\mathbf{T}$ is the odometry translational component (i.e. the position of the robot)
• The expected output of this step should be similar to the following video (8x speed-up)
  

• The map is in map coordinates that are given by the map origin $\mathbf{x}_\mathrm{origin}$ (encoded in Pose message grid_map.origin) and the map resolution $\Delta_\mathrm{res}$ (grid_map.resolution), i.e., each point in the map represents an area of $\Delta_\mathrm{res}\times\Delta_\mathrm{res}~\mathrm{m}$. Therefore we need to apply a similar approach to the previous step and also divide the real-world coordinates by the resolution $\Delta_\mathrm{res}$ and round it to the closest integer to obtain the grid map indices $\mathbf{m}_i$ as:$$\mathbf{m}_i = round((\mathbf{o}_i - \mathbf{x}_\mathrm{origin})/\Delta_\mathrm{res})$$
• You should also watch for any points that fall outside of the map and act accordingly. Either you need to extend the map and perhaps move its origin (harder version of this task), or simply trim the outlying points (sufficient for passing this task)
• The expected output of this step should be similar to the following video (8x speed-up)
  

• You need to obtain the list of all the grid map points to be updated (both free_points and occupied_points). This should be done using the raytracing. The simplest approach for raytracing on 2D grid is the Bresenham's line algorithm:

      def bresenham_line(self, start, goal):
          """Bresenham's line algorithm
          Args:
              start: (float64, float64) - start coordinate
              goal: (float64, float64) - goal coordinate
          Returns:
              interlying points between the start and goal coordinate
          """
          (x0, y0) = start
          (x1, y1) = goal
          line = []
          dx = abs(x1 - x0)
          dy = abs(y1 - y0)
          x, y = x0, y0
          sx = -1 if x0 > x1 else 1
          sy = -1 if y0 > y1 else 1
          if dx > dy:
              err = dx / 2.0
              while x != x1:
                  line.append((x,y))
                  err -= dy
                  if err < 0:
                      y += sy
                      err += dx
                  x += sx
          else:
              err = dy / 2.0
              while y != y1:
                  line.append((x,y))
                  err -= dx
                  if err < 0:
                      x += sx
                      err += dy
                  y += sy
          x = goal[0]
          y = goal[1]
          return line

• The algorithm will give you a list of all points between the start coordinate and goal coordinate. Where the start coordinate is for all the scan points the odometry of the robot, and goal coordinate is each scan projected scan point $\mathbf{m}_i$. It is enough to store the appropriate points in the lists, e.g.:

  #get the position of the robot in the map coordinates
  odom_map = world_to_map(odometry.position)
  #get the laser scan points in the map coordinates
  laser_scan_points_map = world_to_map(laser_scan_points_world)
   
  free_points = []
  occupied_points = []
   
  for pt in laser_scan_points_map:
    #raytrace the points
    pts = bresenham_line(odom_map, pt)
   
    #save the coordinate of free space cells
    free_points.extend(pts)
    #save the coordinate of occupied cell 
    occupied_points.append(pt)

• The expected output of this step should be similar to the following video (8x speed-up)
  

• In this step the Bayesian update of the map is performed according to the description in Lab03 - Grid and Graph based Path Planning.
• The Bayesian occupancy grid update is defined as:

$$ P(m_i = occupied \vert z) = \dfrac{p(z \vert m_i = occupied)P(m_i = occupied)}{p(z \vert m_i = occupied)P(m_i = occupied) + p(z \vert m_i = free)P(m_i = free)}, $$ where $P(m_i = occupied \vert z)$ is the probability of cell $m_i$ being occupied after the fusion of the sensory data; $P(m_i = occupied)$ is the previous probability of the cell being occupied and $p(z \vert m_i = occupied)$ is the model of the sensor which is usually modeled as: $$ p(z \vert m_i = occupied) = \dfrac{1 + S^z_{occupied} - S^z_{free}}{2}, $$ $$ p(z \vert m_i = free) = 1 - p(z \vert m_i = occupied), $$ where $S^z_{occupied}$ and $S^z_{free}$ are the sensory models for the occupied and free space respectively.

For the simulated LIDAR sensor, we will use the below stated model $$ S^z_{occupied} = \begin{cases} 0.95 \qquad\text{for}\qquad d = r\\ 0 \qquad\text{otherwise} \end{cases}, \qquad\qquad S^z_{free} = \begin{cases} 0.95 \qquad\text{for}\qquad d \in [0,r)\\ 0 \qquad\text{otherwise} \end{cases}, $$ where $r$ is the distance of the measured point and $\epsilon$ is the precision of the sensor. I.e. it is assumed that the space between the sensor and the measured point is free and the close vicinity of the measured point given by the sensor precision is occupied. * the recommended approach is to define two functions to help you with the Bayesian update

        def update_free(P_mi):
            """method to calculate the Bayesian update of the free cell with the current occupancy probability value P_mi 
            Args:
                P_mi: float64 - current probability of the cell being occupied
            Returns:
                p_mi: float64 - updated probability of the cell being occupied
            """
            p_mi = #TODO
            #never let p_mi get to 0
            return p_mi
 
        def update_occupied(P_mi)
            """method to calculate the Bayesian update of the occupied cell with the current occupancy probability value P_mi
            Args:
                P_mi: float64 - current probability of the cell being occupied
            Returns:
                p_mi: float64 - updated probability of the cell being occupied
            """
            p_mi = #TODO
            #never let p_mi get to 1
            return p_mi

• First, it is necessary to correctly set the cell values given the row-major order of OccupancyGrid representation. HINT code:

          #construct the 2D ggrid
          data = grid_map.data.reshape(grid_map_update.height, grid_map_update.width)
   
          #fill in the cell probability values
          #e.g.: data[y,x] = update_free(data[y,x])
   
          #serialize the data back (!watch for the correct width and height settings if you are doing the harder assignment)
          grid_map.data = data.flatten()

  • Second, the probability values shall never reach 0 or 1!!! The effect of letting this happen is inability of the map to dynamically add/delete obstacles as it is demonstrated in the following example:
  • Correct behavior

• Wrong behavior