* 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 the 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 (the orientation of the robot) and $\mathbf{T}$ is the odometry translational component (the position of the robot).
• The expected output of the step should be similar to the following video (8x speed-up)
  

• The map is in the map coordinates that are given by the map origin $\mathbf{x}_\mathrm{origin}$ (encoded in Pose grid_map.origin) and the map resolution $\Delta_\mathrm{res}$ (grid_map.resolution), where 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 the map and act accordingly. Either extend the map and move its origin (more demanding) or trim the outlying points (sufficient for the course tasks).
• The expected output of the 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). It can be done using raycasting. The straightforward approach for the raytracing on a 2D grid is Bresenham's line algorithm:

      def bresenham_line(self, start: (int, int), goal: (int, int)) -> (int, int):
          """Bresenham's line algorithm
          Args:
              start: (int, int) - start coordinate
              goal: (int, int) - goal coordinate
          Returns:
              Interlining 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 gives you a list of all points between the start coordinate and goal coordinate. Where the start coordinate is for all the scan points in the robot's odometry, and the goal coordinate is each scan projected scan point $\mathbf{m}_i$. It is enough to store the appropriate points in the lists, such as

  #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 the step should be similar to the video (8x speed-up)
  

• In this step, the Bayesian update of the map is performed according to the description in Map Building.
• The Bayesian occupancy grid update can be 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) = \dfrac{1 - S^z_{occupied} + S^z_{free}}{2} = 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.

* The recommended approach is to define two functions for 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 - the current probability of the cell being occupied
            Returns:
                p_mi: float64 - the 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 - the current probability of the cell being occupied
            Returns:
                p_mi: float64 - the updated probability of the cell being occupied
            """
            p_mi = #TODO
            #never let p_mi get to 1
            return p_mi

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

          #construct the 2D grid
          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()