% ------------------------------------------------------------------------------------------------
%  Talking about plane model road segmentation
% ------------------------------------------------------------------------------------------------

Depois de filtrar devidamente a nuvem de pontos, é necessário segmentá-la, segundo
a continuação do trabalho. O princípio de segmentação tridimensional consiste na partição da nuvem de pontos
em diferentes zonas conhecidas, para simplificação do problema, de forma semelhante ao que acontece em problemas
de visão convencionais. 

Para ilustrar o conceito de segmentação envolvido, retome-se 
a título de exemplo a figura \ref{subfig:voxel_example} que ilustra um cenário
característico obtido quando o ATLASCAR está em movimento.
Para proceder à devida segmentação da nuvem de pontos, é necessário 
recorrer à análise da informação que é comum aos diversos obstáculos.
Neste caso, o conjunto de pontos que une a maioria dos obstáculos,
correspondem aqueles que representam o chão.
Desta forma, é necessário criar um procedimento capaz de efetuar a segmentação dos pontos do
chão, para que deixe de existir contacto entre os pontos que representam 
os objetos. Esta forma de segmentação tem a vantagem de permitir
a redução da quantidade de dados, já que, na maioria dos casos, os pontos que representam 
o chão constituem uma grande percentagem de informação.
Note-se que, caso seja possível segmentar o conjunto de pontos
que representam o chão $P_c$ numa nuvem de pontos inicial $P$,
então é possível obter uma nuvem de pontos $P_o$ com a informação 
que representa os diversos obstáculos pela subtração das duas,
isto é, $P_o=P \cap \overline{P_c}$. Desta forma $P_o$ deve constituir um conjunto de 
pontos de diversos obstáculos sem que haja contacto entre eles.
Por este motivo, a primeira etapa de segmentação consiste 
na separação dos pontos do chão dos restantes. 

% \subsection{Modelo \textit{RANSAC}}  \label{subsec:ransac}

A primeira abordagem para remoção dos pontos do chão da nuvem de pontos que poderia ser adotada
consiste em descartar todos os pontos de coordenada \textit{z} abaixo de um determinado 
valor (figura \ref{fig:ref_frames}). Este método simplista tem a vantagem
de ser bastante rápido, contudo só funcionaria para os casos em que o veículo
se encontre paralelo ao plano do chão, o que à partida limitaria o processo. 
Uma forma para contornar o problema
seria monitorizar o estado da suspensão do veículo, para estimar uma transformação
geométrica que compensasse desvios em situações de desvio de orientações do veículo ao plano do chão 
como em rotundas, lombas de quebra de velocidade,
depressões no piso, ou qualquer outra situação semelhante.
Visto que o sistema de monitorização da suspensão está ainda em fase de desenvolvimento, 
é necessário recorrer a outra metodologia para a deteção do plano do chão. 

Outra forma de proceder à segmentação do plano do chão
é através da aplicação do modelo RANSAC (\textit{RANdom SAmple Consensus}).
A aplicação do método consiste na consideração de um modelo teórico,
neste caso um plano, e de uma amostra de dados que pode ser aproximada
a esse modelo, neste caso, a nuvem de pontos \cite{RANSAC}.

Este método é iterativo e proporciona resultados corretos com uma determinada probabilidade, 
influenciada diretamente pelo número de iterações \cite{PCL}.

O princípio do algoritmo consiste na análise aleatória de
amostras de pontos da nuvem e pelo teste hipotético dos parâmetros do modelo \cite{PCL}.
O resultado do algoritmo é aceite quando a quantidade de pontos
que representam o modelo é considerada suficientemente grande.
Por ser uma forma rápida e robusta de estimar geometrias em nuvens de pontos,
a biblioteca PCL possui uma classe escrita em C++ para determinação de alguns modelos 
matemáticos através do método RANSAC. 
O algoritmo existente é capaz de devolver os coeficientes ($a$,$b$,$c$ e $d$) do
plano médio que se aproxima aos pontos do chão, representado através da equação \ref{eq:plane_model}:

\begin{equation}
 ax+by+cz+d=0
\label{eq:plane_model}
\end{equation}

Considere-se a representação da figura \ref{fig:projection} 
em que o plano $A$ representa o plano do chão, e $B$ que representa
o plano do veículo.

\begin{figure}[h!]
\centering
\includegraphics[scale=0.3]{Images/projection.eps}
\caption{Geometria dos planos: veículo ($B$) e chão ($A$).}
\label{fig:projection}
\end{figure}

A partir da equação \ref{eq:plane_model}, é possível determinar a 
transformação geométrica entre o referencial $XYZ$ do carro e o referencial $xyz$ do chão (figura \ref{fig:projection}).
Para tal, considere-se o vetor $\overrightarrow{v}$ normal ao plano $A$ 
dado pelos coeficiente $a$,$b$,$c$ e $d$ da equação \ref{eq:plane_model}:

\begin{displaymath}
\overrightarrow{v}= \begin{bmatrix} a \\ b\\ c \end{bmatrix}
\end{displaymath}

e um vetor $\overrightarrow{w}$ dado por:

\begin{displaymath}
\overrightarrow{w}= -\begin{bmatrix} x-x_0 \\ y-y_0 \\ z-z_0 \end{bmatrix}
\end{displaymath}

em que $x_0$,$y_0$,$z_0$ representam coordenadas de um ponto genérico representado no referencial $XYZ$.
Então a distância $D$ do ponto $(x_0, y_0, z_0)$ ao plano é dada por \cite{PROJECTION}:

\begin{displaymath}
 D=\dfrac{|\overrightarrow{v}.\overrightarrow{w}|}{||\overrightarrow{v}||}
\end{displaymath}

substituindo as respetivas variáveis a distância $D$ pode ser determinada pela equação \ref{eq:distance}. 

\begin{equation}
D=\dfrac{|a \times x_0 + b \times y_0 + c \times z_0 +d|}{\sqrt{a^2+b^2+c^2}}
\label{eq:distance}
\end{equation}

A partir da projeção da origem do referencial $XYZ$ no plano $A$ e de dois pontos
conhecidos $(x_1, 0, 0)$ e $(0, y_1, 0)$ é possível determinar a localização do referencial $xyz$
e a orientação dos ângulos de \textit{Euler} ($RPY$).
O ângulo $R$ é estimado através do ângulo entre os dois vetores $\overrightarrow{Z}$ e $\overrightarrow{z}$,
enquanto que o ângulo $P$ é determinado pelo ângulo entre os vetores $\overrightarrow{X}$ e $\overrightarrow{x}$.
A expressão \ref{eq:RPY} permite obter o ângulo entre dois vetores $\overrightarrow{v}$ e $\overrightarrow{w}$ \cite{PROJECTION}.

\begin{equation}
\alpha = \arccos \left(    \dfrac{\overrightarrow{v}.\overrightarrow{w}}{||\overrightarrow{v}|| \times ||\overrightarrow{w}||}  \right)
 \label{eq:RPY}
\end{equation}

Repare-se que o terceiro ângulo de \textit{Euler} ($Y$) não necessita de ser determinado pela equação
\ref{eq:RPY}, podendo assumir imediatamente o valor nulo, já que a orientação entre os 
vetores $\overrightarrow{Y}$ e $\overrightarrow{y}$ não é relevante para a determinação dos pontos da estrada,
ou seja, os dois vetores são paralelos.

Com base no referencial $xyz$, é possível criar um volume com uma margem para extração
dos pontos que fazem parte do chão, completando esta etapa de trabalhos.
A figura \ref{fig:plane_model_road_segmentation} representa um exemplo da extração
de todos os pontos que se encontrem a uma margem de dez centímetros do 
plano médio estimado pelo método RANSAC. 

\begin{figure}[h!]
\begin{minipage}[b]{0.35\linewidth}
\centering
\subfigure[Cenário com vários planos.]
{
  \includegraphics[width=\textwidth]{Images/truck_image.eps}
  \label{subfig:xb3_truck}
}
\end{minipage}
\begin{minipage}[b]{0.65\linewidth}
\centering
\subfigure[Nuvem de pontos filtrada e segmentada]
{
  \includegraphics[width=\textwidth]{Images/plane_model_road_segmentation.eps}
  \label{subfig:plane_model_road_segmentation}
}
\end{minipage}
\caption{Exemplo da aplicação do algoritmo na presença de múltiplos planos.}
\label{fig:plane_model_road_segmentation}
\end{figure}

Apesar da eficácia e robustez do método, há algumas ressalvas na sua aplicação que convém
salientar. Em primeiro lugar, apesar do método ser 
significativamente rápido na execução computacional, não é possível estimar o tempo que o algoritmo
exige, já que o método é iterativo. 
Outra desvantagem da aplicação,
é que o método só consegue determinar uma solução. Isto quer dizer que, se na nuvem 
de pontos fornecida existirem dois planos candidatos, o método pode não devolver a solução 
ótima. Tal fato pode consistir um problema, no caso de existir um plano maior na nuvem de pontos que não o
do chão. Considere-se o exemplo da figura \ref{subfig:xb3_truck} que representa o deslocamento de um 
camião de traseira  e lateral planas a uma distancia reduzida do veículo.
Neste caso, é possível que o algoritmo devolva os coeficientes do plano que se aproximam 
à parte traseira do camião, em vez de devolver os coeficientes do plano médio do chão.
Por isso, antes de validar o resultado da aplicação do método RANSAC, são avaliadas 
as coordenadas e ângulos do referencial $xyz$ obtido. Se os valores não respeitarem determinadas
margens, o resultado é rejeitado e é considerado o último referencial determinado. 
Desta forma, é possível obter uma aplicação capaz de extrair
de forma significativamente robusta o conjunto de pontos que pertencem ao chão, o que constitui
uma etapa fundamental na segmentação de nuvens de pontos.