===== T1e-expl - Robotic information gathering - mobile robot exploration  ======
In the mobile-robot exploration, the robot has to select its goals. This goal selection rely on identification of information-rich regions of interest. A simple approach is to use free-edges (border between the free and unknown parts of the map) for exploration.  
The main task is to implement a function that will find the free-edge cells and cluster them to obtain list of free-edge representatives from which goals for exploration are further selected. 

|**Deadline**  |  November 06, 2022, 23:59 PST |
|**Points**  |  3 |
|**Label in BRUTE**  |  t1e-expl |
|**Files to submit**  |  archive with ''HexapodExplorer.py'' file |
|**Resources**  |  {{ :courses:uir:hw:b4m36uir_22_t1_resource_pack.zip | B4M36UIR_t1_resource_pack}}|

----

===Assignment===
In class ''HexapodExplorer.py'' implement the ''find_free_edge_frontiers'' function. The purpose of the function is to find the free-edge frontiers which are centroids of cells on the border between the free and unknown cells.
A free-edge cell is a free cell which borders:
  * at least one other free cell,
  * at least one unknown cell,
Free-edges are sets of free-edge cells connected into the connected component (8-neighborhood, a.k.a. 2 jump connected component, is considered).
Free-edge frontiers are centroids of the segmented free-edges.

The input parameters of the function is: 
  - ''grid_map'' - [[courses:uir:hw:support:messages|OccupancyGrid message]] - grid map after obstacle growing (to ensure the robot can reach the frontiers)

The function returns:
  - the [[courses:uir:hw:support:messages|Pose[] message]] - list of free-edge frontiers
  - ''None'' - if there are no free-edge frontiers

The ''find_free_edge_frontiers'' function in the ''HexapodExplorer.py'' class has a following prescription:
<code python>
    def find_free_edge_frontiers(self, grid_map):
        """Method to find the free-edge frontiers (edge clusters between the free and unknown areas)
        Args:
            grid_map: OccupancyGrid - gridmap of the environment
        Returns:
            pose_list: Pose[] - list of selected frontiers
        """
</code>

<note tip>In class ''HexapodExplorer.py'' you can change whatever you want. In evaluation, the given interfaces are fixed and the evaluation script is fixed.</note>

----

=== Approach ===
There are man different ways on how to accomplish the given task. The recommended approach for the detection of free edge frontiers consist of three steps:
  - detection of free-edge cells,
  - connecting free-edges into the connected components,
  - calculation of the free-edge centroids.
The detailed description of the individual steps follows.
----
 
== Free-edge cells detection ==
First, the detection of the free-edge cells is performed.
The free-edge cell $m_i$ is characterized by the following properties given the occupancy grid map:
  * a free-edge cell is a free cell, i.e., $P(m_i = occupied \vert z) < 0.5$,
  * a free-edge cell borders at least one other free cell, i.e., $\exists m_j \in 8\mathrm{-neighborhood\,\,of\,\,} m_i \vert P(m_j = occupied \vert z) < 0.5$,
  * a free-edge cell borders at least one unknown cell, i.e., $\exists m_j \in 8\mathrm{-neighborhood\,\,of\,\,} m_i \vert P(m_j = occupied \vert z) = 0.5$,
An efficient approach to free-cell detection is using the 2D convolution with subsequent thresholding. The task is to find a proper 2D convolution mask to distinguish between the different constitutions of the cell 8-neighborhood consisting of free, unknown, and occupied cells.++ Hint | 
You should draw the bounding examples of 8-neighborhood and think about the 2D-convolution as of a weighted sum of this neighborhood.  
++

The 2D convolution itself can be done easily using the ''scipy.ndimage'' library using the following code: 
<code python>
import scipy.ndimage as ndimg
data_c = ndimg.convolve(data, mask, mode='constant', cval=0.0)
</code>
In general, the expected output is a grid with ''1'' wherever the free-edge cell is and ''0'' elsewhere. An example of convolution result and subsequent thresholding to obtain the free cells is visualized in the following figure. 
++++ Figure (click to view) |
From left to right:
  * original gridmap,
  * gridmap after convolution,
  * thresholded positions of the free-cells.

{{:courses:uir:hw:t1e-example1.png?500|}}\\
{{:courses:uir:hw:t1e-example2.png?700|}}\\
{{:courses:uir:hw:t1e-example3.png?700|}}
++++

== Free-edge clustering ==
The second step is to perform the connected component analysis on the free-edge cells. A nice explanatory overview of the connected component analysis can be found [[https://datacarpentry.org/image-processing/09-connected-components/|here]]. The main idea is to connect the free-edge cells that coincide into the connected component to effectively reduce the number of frontiers and select only the representatives of the whole cluster. 

For the connectivity analysis you can use the following code:
<code python>
import skimage.measure as skm
labeled_image, num_labels = skm.label(free_cells, connectivity=2, return_num=True)
</code>

An example of correctly connected free-edge clusters is visualized in following figure together with free-edge centroids marked by red points.

{{:courses:uir:hw:t1e-example4.png?800|}}  

== Free-edge centroids ==
Finally, it is necessary to obtain the cluster centroids as the mean of $x, y$ coordinates of connected free-edge. 

----

=== Example behavior ===
{{:courses:uir:hw:t1e-map1.png?600|}}\\
{{:courses:uir:hw:t1e-map2.png?600|}}\\
{{:courses:uir:hw:t1e-map3.png?600|}}

----

=== Evaluation ===
The code can be evaluated using the following script (also attached as ''t1e-expl.py'').
<code python>
#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import matplotlib.pyplot as plt

import sys
import os
import math
import numpy as np

sys.path.append('hexapod_robot')
sys.path.append('hexapod_explorer')
 
#import hexapod robot and explorer
import HexapodRobot as hexapod
import HexapodExplorer as explorer

#import communication messages
from messages import *

#pickle
import pickle

if __name__=="__main__":

    explor = explorer.HexapodExplorer()

    dataset = pickle.load( open( "frontier_detection.bag", "rb" ) )

    for gridmap in dataset:
            #find free edges
            points = explor.find_free_edge_frontiers(gridmap)

            #plot the map
            fig, ax = plt.subplots()
            #plot the gridmap
            gridmap.plot(ax)
            #plot the cluster centers
            if points is not None:
                for p in points:
                    plt.plot([p.position.x],[p.position.y],'.', markersize=20)

            plt.xlabel('x[m]')
            plt.ylabel('y[m]')
            ax.set_aspect('equal', 'box')
            plt.show()

</code>