% ------------------------------------------------------------------------------------------------
%  A brief introduction on segmentation
% ------------------------------------------------------------------------------------------------

Após a aplicação da metodologia de segmentação da nuvem de pontos 
através da análise de um plano (secção \ref{sec:plane_model_road_segmentation}),
é possível obter duas nuvens de pontos distintas: uma com todos os pontos que pertencem
ao chão; e outra com os restantes que caracterizam o ambiente circundante.
A etapa seguinte consiste na análise dos dados 
obtidos anteriormente, para dar continuidade ao processo de segmentação.
O objetivo nesta etapa é classificar o conjunto de pontos que caracterizam o ambiente
em duas partes distintas: uma que contém todos os pontos dentro da zona navegável,
e outra que contém os pontos que estão fora dessa zona. Esta segunda 
segmentação tem especial importância na medida em que permite uma primeira classificação dos
obstáculos. No final, o algoritmo deve ser capaz de distinguir, por exemplo, um peão
a atravessar numa passadeira de um edifício que se encontre fora da área navegável.

\subsection{Cálculo do perímetro navegável}

Neste trabalho considera-se por zona navegável
o contorno da zona vista pela câmara, num determinado 
instante, para a qual o veículo se pode deslocar.
Considere-se uma nuvem de pontos $P$ segmentada de acordo
com a metodologia aplicada anteriormente, em que se 
obtém o conjunto de pontos do plano do chão $P_c$ e o
conjunto de pontos resultante da subtração, $P_o=P \cap \overline{P_c}$.
As etapas seguintes consistem numa
abordagem adotada para determinar a nuvem
de pontos dentro da zona navegável.

\begin{enumerate}
 \item Projeção de dos pontos de $P_c$ no plano médio do chão ( previamente obtidos pelo método RANSAC);
 \item Cálculo do \textit{convex hull} da nuvem projetada para estimar o perímetro navegável;
 \item Criação de um volume de extração com base no \textit{convex hull}.
\end{enumerate}


\begin{figure}[h!]
\begin{minipage}[b]{0.4\linewidth}
\centering
\subfigure[Imagem.]
{
  \includegraphics[width=\textwidth]{Images/navegable_zone_image.eps}
  \label{subfig:nav_zone_image}
}
\end{minipage}
\begin{minipage}[b]{0.6\linewidth}
\centering
\subfigure[Nuvem de pontos filtrada e segmentada.]
{
  \includegraphics[width=\textwidth]{Images/navegable_zone_cloud.eps}
  \label{subfig:nav_zone_cloud}
}
\end{minipage}
\caption{Exemplo da aplicação do algoritmo de classificação da zona navegável.}
\label{fig:projection}
\end{figure}

A figura \ref{fig:projection} representa a aplicação da metodologia descrita.
O conjunto de pontos representado a azul é determinado através da projeção de
todos os pontos no plano estimado pelo método RANSAC (secção \ref{sec:plane_model_road_segmentation})
que representa o plano do chão. Esta projeção é interessante,
porque permite o cálculo do \textit{convex hull} do conjunto de pontos representado na figura 
\ref{subfig:nav_zone_cloud} por pequenas esferas alaranjadas.
Um \textit{convex\_hull} de um conjunto de pontos $P$ é definido como a menor
geometria convexa que contém $P$. Então, se $P$ é finito o \textit{convex\_hull} pode ser
definido pela expressão \ref{eq:qhull}, tal que para todos os pontos $p_i \in P$~\cite{QHULL}:

\begin{equation}
Ap_i +b \le [0,...],
\label{eq:qhull}
\end{equation}

em que cada linha de $A$ e cada elemento de $b$ definem a equação de um segmento de reta do \textit{convex\_hull}.
O conjunto de pontos definido pelo \textit{convex\_hull} constitui aquilo que pode ser designado
neste trabalho como perímetro navegável. A determinação deste perímetro
permite estimar um volume extrudido na vertical 
que divide a nuvem de pontos em duas partes distintas: uma com o conjunto de pontos
dentro da zona navegável, e outra com os pontos fora da zona navegável
(figura \ref{subfig:nav_zone_cloud} a vermelho e a verde, respetivamente).
Esta abordagem traduz uma etapa de segmentação interessante porque permite,
em determinadas situações, distinguir e classificar a nuvem de pontos.

\subsection{Segmentação por distância euclidiana} \label{subsec:euclidean_cluster_extraction}

Até esta etapa, a metodologia de segmentação consiste numa abordagem de divisão
de toda a nuvem de pontos em três zonas conhecidas, nomeadamente a zona do chão e dos pontos
dentro e fora da zona navegável.
Os métodos descritos anteriormente constituem uma forma de segmentação
para simplificação do problema. Contudo, até esta fase não é possível obter informação
acerca do número de obstáculos existentes, nem da sua localização e geometria.
Desta forma, é necessário recorrer a uma segmentação a outro nível,
para determinar a quantidade de obstáculos que se encontram dentro
da zona navegável e as propriedades de cada um.
Surge então a necessidade de analisar as distâncias
entre o conjunto de pontos no espaço euclidiano.
Define-se por distância euclidiana $d$ ao comprimento de um 
segmento de reta que une dois pontos, o que matematicamente
pode ser escrito através da expressão \ref{eq:euclideam_distance},
em que $p=(p_1,p_2,p_3,...,p_n)$ e $q=(p_1,p_2,p_3,...,p_n)$~\cite{STATSOFT}.

\begin{equation}
 d=\sqrt{ \sum\limits_{i=1}^n (q_i-p_i)^2},
  \label{eq:euclideam_distance}
\end{equation}

Para realizar uma segmentação num ponto de vista
euclidiano, é necessário recorrer à decomposição
espacial do conjunto de dados para os poder analisar
quanto à sua proximidade. 

% Uma forma rápida e eficaz de realizar essa decomposição
% é utilizar uma grelha tridimensional com tamanho constante
% aplicada à nuvem de pontos, ou seja, uma estrutura de dados
% \textit{octree}. Contudo esta metodologia apenas faz sentido para 
% aplicações em que o espaço deva ser dividido em partes
% iguais~\cite{RUSU}. Para situações em que em que os segmentos
% tem tamanhos diferentes, que é o caso que aqui se observa,
% é necessário um algoritmo mais complexo.

De uma forma geral, o sistema tem de detetar segmentos
e distingui-los. Matematicamente, um segmento pode ser definido da seguinte forma:
Se o conjunto de pontos de entrada for representado por $P$,
então o segmento $O_i= \{ p_i : p_i \in P \} $ é distinto de
$O_j= \{p_j : p_j \in P \} $ se:

\begin{equation}
 min ||p_i-p_j|| \ge d_{th},
\label{eq:segmentation}
\end{equation}

em que $d_{th}$ é um limite máximo imposto de distância euclidiana~\cite{RUSU}.
A equação \ref{eq:segmentation} traduz que, se a distância mínima entre um conjunto de 
pontos $p_i \in P$ e outro conjunto $p_j \in P$ é maior que um determinado 
valor, então os pontos que pertencem a $p_i$ pertencem a um segmento $O_i$
e os pontos do conjunto $p_j$ pertencem a outro segmento $O_j$, distinto do anterior.

% \subsubsection{\textit{Octree} e \textit{kd-tree}}

% \textbf{-> Pontos principais a definir:}

% \textbf{-> Descrição geral de octree e kd-tree}

% \textbf{-> Definir os princípios básicos envolvidos.}

% \textbf{-> Referir a importância para a procura de vizinhança de pontos.}

% \textbf{-> Integração no PCL.}
 
% Antes de proceder à aplicação do conceito de distância euclidiana 
% para definir os diversos segmentos, é importante ainda definir 
% o conceito de vizinhança e compreender a forma como o uso de \textit{octrees}
% e \textit{kd-trees}, pode otimizar o processo de segmentação.
% 
% As nuvens de pontos consistem em estruturas de dados
% não organizados, por isso surge a necessidade da criação
% de uma representação estrutural da informação contida na 
% nuvem de pontos, para melhorar o desempenho de certas operações.
% Uma forma de proceder à representação estruturada dos dados 
% da nuvem de pontos, consiste na utilização de \textit{octrees}.
% Uma \textit{octree} consiste na agregação de dados que têm valores
% semelhantes para economizar tempo de processamento. O conceito
% deriva do termo \textit{quadtree}, generalizado a três dimensões.
% O princípio consiste na divisão consecutiva do nodo raíz em oito
% partes de dimensões iguais, até que para cada nodo 
% a informação agregada seja semelhante.
% O uso de \textit{octrees} é limitado na medida em que o particionamento 
% do espaço é feito sempre em partes iguais. 

% A figura \ref{fig:octree} ilustra a estruturação de nodos em \textit{octree}.
% 
% \begin{figure}[h!]
% \begin{minipage}[b]{\linewidth}
% \centering
% \subfigure[Raíz.]
% {
%   \includegraphics[scale=0.5]{Images/octree/level_1.eps}
%   \label{subfig:level_1}
% }
% \end{minipage}
% \begin{minipage}[b]{\linewidth}
% \centering
% \subfigure[Nível 1.]
% {
%   \includegraphics[scale=0.5]{Images/octree/level_2.eps}
%   \label{subfig:level_2}
% }
% \end{minipage}
% \begin{minipage}[b]{\linewidth}
% \centering
% \subfigure[Nível 2.]
% {
%   \includegraphics[scale=0.5]{Images/octree/level_3.eps}
%   \label{subfig:level_2}
% }
% \end{minipage}
% \caption{Estrutura hierárquica de \textit{octrees}.}
% \label{fig:octree}
% \end{figure}

\subsubsection{Implementação}

A definição dos conceitos de distância euclidiana e de segmento
é de especial importância para a compreensão do algoritmo de segmentação.
Para proceder à implementação do algoritmo, é necessário recorrer
à análise de vizinhança de pontos. Calcular e analisar as distâncias 
diretamente entre todos os pontos numa nuvem de pontos desorganizada,
é uma tarefa extremamente exigente a nível computacional. Desta forma,
surge a necessidade de representar a nuvem de pontos numa forma estruturada.
A biblioteca de funções permite a utilização de uma representação em árvore
(\textit{kd-tree}), cujo método de procura é baseado na biblioteca de 
funções escritas em C++ FLANN (\textit{Fast Library for Approximate Nearest Neighbor}),
que possibilita a procura rápida de vizinhança de pontos~\cite{PCL}.
As etapas do algoritmo são as seguintes, considerando $P$ a nuvem de pontos a segmentar~\cite{RUSU}:

\begin{enumerate}
 \item Criar uma representação estruturada em \textit{kd-tree} da nuvem de pontos;
 \item Criar uma lista vazia de segmentos $C$, e uma lista de pontos que têm de ser verificados $Q$;
 \item Para cada ponto $p_i \in P $ efetuar os seguintes passos:
    
    \begin{enumerate}
     \item adicionar $p_i$ à lista de pontos que têm de ser verificados, $Q$;
     \item para cada ponto $p_i \in Q$ proceder da seguinte forma:

	\begin{enumerate}
	 \item procurar o conjunto de pontos vizinhos ($p_i^k$) de $p_i$ dentro do raio $r < d_{th}$;
	 \item para cada vizinho ($p_i^k$) de $p_i$, verificar se o ponto já foi processado, e caso contrário adicioná-lo à lista $Q$;
	\end{enumerate}
     \item Quando a lista de todos os pontos em $Q$ estiver processada, adicionar $Q$ à lista de segmentos $C$ e apagar todos os pontos em $Q$;
    \end{enumerate}
 
 \item O algoritmo termina quando todos os pontos $p_i \in P$ estão processados
e fazem agora parte da lista de segmentos $C$.
\end{enumerate}

A aplicação do algoritmo encontra-se implementada na biblioteca de funções PCL
que devolve a lista de índices dos segmentos $C$.
Ao proceder à extração da lista de índices $C$ na nuvem de pontos 
inicial $P$ e com a aplicação conjunta das ferramentas disponibilizadas pelo ROS
para visualização, pode-se obter uma representação do volume ocupado pelos segmentos.
A visualização dos segmentos é efetuada a partir do cálculo do centróide de cada segmento
da lista $C$, e dos valores mínimo e máximo determinados no conjunto de pontos. Com
esta informação, representa-se um marcador no visualizador de mensagens do ROS.
A figura \ref{fig:euclidean_cluster} representa 
a segmentação euclidiana de uma situação típica de navegação urbana.

\begin{figure}[h!]
\begin{minipage}[b]{0.4\linewidth}
\centering
\subfigure[Imagem.]
{
  \includegraphics[width=\textwidth]{Images/euclidean_cluster.eps}
  \label{subfig:euclidean_image}
}
\end{minipage}
\begin{minipage}[b]{0.6\linewidth}
\centering
\subfigure[Nuvem de pontos filtrada e segmentada.]
{
  \includegraphics[width=\textwidth]{Images/euclidean_cluster_cloud.eps}
  \label{subfig:euclidean_cluster}
}
\end{minipage}
\caption{Exemplo da aplicação da segmentação euclidiana.}
\label{fig:euclidean_cluster}
\end{figure}

% A aplicação do algoritmo é relativamente rápida em termos de computação
% uma vez que recorre a representação de nuvens de pontos por \textit{kd-trees}.
% Este tipo de representação constitui um passo importante na segmentação
% de nuvens de pontos já que permite a divisão de dados para um processamento mais eficaz.

Apesar da eficácia da aplicação de segmentação por distância euclidiana, há algumas restrições do método
que convém referir. A principal desvantagem é que o algoritmo exige que todos os pontos que existem na 
nuvem sejam processados. Tal facto pode constituir um problema por dois motivos principais; 
em primeiro lugar nem todos os pontos são informativos acerca do ambiente que rodeia o veículo, e 
em segundo lugar o processamento de todos os pontos da nuvem implica um custo computacional relativamente elevado.

\subsection{Algoritmo de crescimento de regiões} \label{subsec:region_growing}

A análise ao algoritmo de segmentação euclidiana conduziu a definição de uma outra forma
de proceder à segmentação da nuvem de pontos.
O método consiste na aplicação da segmentação euclidiana a dados bidimensionais,
e, de seguida, extrudir para a informação tridimensional a partir de um algoritmo de crescimento de regiões.

O ATLASCAR está equipado com dois lasers \textit{Sick LMS100} situados no para choques frontal do veículo, um do lado
esquerdo e outro do lado direito (figura \ref{fig:ref_frames}). Os lasers são capazes de publicar 
informação para o sistema ROS a uma taxa de $50 Hz$, 
um valor maior que o da nuvem de pontos do sensor estéreo (cerca de $5 Hz$
dependendo do processamento do computador utilizado). 
Uma vantagem em utilizar a informação bidimensional fornecida pelos lasers
é que a quantidade de pontos obtida é signicativamente menor, logo o processamento
deste tipo de informação é mais reduzido que os dados de sensores tridimensionais. 
Desta forma, foi proposta a realização de uma aplicação que partisse dos dados
bidimensionais fornecidos pelos lasers e, aplicar o algoritmo de crescimento de regiões
para obter posteriormente a representação tridimensional de obstáculos
a partir dos dados estéreo. 
Considere-se por $P_L$ e $P_R$ as nuvens de pontos fornecidas pelos lasers esquerdo
e direito, respetivamente, em que a ordem de operações do algoritmo proposto se enumera da seguinte forma:

\begin {enumerate}
 \item "Soma" das nuvens de pontos dos dois lasers, $P=P_L \cup P_R$;
 \item Aplicação de um filtro de distâncias à nuvem de pontos $P$ de acordo com os dados do sensor estéreo (secção \ref{subsec:distance_filtering});
 \item Aplicar a segmentação euclidiana aos dados do laser consoante o método descrito na secção \ref{subsec:euclidean_cluster_extraction}
para a obtenção de uma lista de segmentos $C$;
 \item Para cada segmento $C$, obtido na etapa anterior, aplicar o algoritmo de crescimento de regiões para obtenção da representação tridimensional.
\end {enumerate}
 
\subsubsection{Implementação do algoritmo} \label{subsubsec:region_growing_impl}

Ao contrário do algoritmo de segmentação por distância euclidiana, o algoritmo de
crescimento de regiões não se encontra implementado na biblioteca PCL.
Desta forma, surge a necessidade da implementação de uma classe escrita em 
\textit{C++} capaz de aplicar o cálculo do crescimento de regiões.
Tendo em conta a notação anterior e denominando por $P_S$
o conjunto de pontos do sensor estéreo, a descrição matemática
do algoritmo pode ser listada da seguinte forma:

\begin{enumerate}
 \item Criar uma representação estruturada em \textit{kd-tree} da nuvem de pontos do sensor estéreo $P_S$;
 \item Criar um vetor de pontos processados $T$ e um vetor de pontos que precisam de ser verificados $Q$;
 \item Para cada ponto $p_j \in P_L$ efetuar as seguintes operações:
      \begin{enumerate}
	  \item Procurar por todos os vizinhos de $p_{j}^{k} \in P_S$ num raio $r<r_{th}$, em que $r_{th}$ é um limite imposto; 
	  \item Adicionar todos os pontos $p_{j}^{k}$ encontrados anteriormente à lista de pontos que têm de ser verificados $Q$;
      \end{enumerate}
 \item Para cada ponto $p_i \in Q$ realizar as seguintes operações:
      \begin{enumerate}
	  \item Verificar se o ponto já foi processado, ou seja, $p_i \in T$;
	  \item Se $p_i \notin T$:
	      \begin{enumerate}
		  \item Procurar por todos os vizinhos de $p_i$ num raio $r<r_{th}$ e adicioná-los à lista de pontos que necessitam de ser procurados $Q$;
		  \item Adicionar $p_i$ à lista de pontos processados $T$;
	      \end{enumerate}
      \end{enumerate}
  \item O algoritmo termina quando todos os pontos na lista $Q$ estão verificados, ou seja, quando a região deixa de crescer.
\end{enumerate}

Esta foi a estrutura adotada para a implementação do algoritmo,
já que dispensa a representação da informação do laser 
em \textit{kd-tree}. Desta forma, a criação da lista que constitui as sementes
do algoritmo de crescimento de regiões é determinado pela procura dos vizinhos
de $P_L$ em $P_S$, e o algoritmo de crescimento de regiões começa a partir desse ponto. 

Neste caso, a aplicação do algoritmo de crescimento de regiões
é feita para cada segmento de forma individual.
Contudo, a sua aplicação pode ser feita para vários
segmentos em simultâneo, desde que estejam espaçados de uma distância 
superior a $r_{th}$.

A figura \ref{fig:region_growing} representa um exemplo da aplicação 
do algoritmo no cenário da figura \ref{subfig:euclidean_image}.
Os pontos representados a vermelho representam a informação
dos dados dos lasers sobre os quais o método é aplicado para a determinação
dos segmentos tridimensionais.

\clearpage

\begin{figure}[h!]
\centering
\includegraphics[width=\textwidth]{Images/region_growing_cloud.eps}
\caption{Exemplo da aplicação do algoritmo de crescimento de regiões.}
\label{fig:region_growing}
\end{figure}

Através da fusão dos sensores, é possível estimar o volume ocupado pelos objetos encontrados
de uma forma bastante credível. Uma vantagem na utilização de múltiplos sensores 
é que a informação repetida é duplamente credível, ou seja, se um 
determinado conjunto de pontos $P_0$ é detetado pelo sensor $A$, e um 
outro conjunto $P_1$ é detetado pelos sensores $A$ e $B$, a probabilidade
da informação de $P_1$ corresponder à realidade é relativamente superior à de $P_0$.

A utilização de informação proveniente de diversos 