\section{Introduction}
	Predicting vehicle behavior is a major challenge and an active research domain for the automotive industry. The desire to develop intelligent vehicles that make good use of predictive vehicle behavior and contribute to the improvement of road safety can find a possible implementation by using the low cost compass sensor data.

	Complex planning algorithms have begun to factor in uncertainty that arise from variations in travel time, erratic communication between autonomous vehicles, imperfect sensor data or other situations. To consider such scenarios, a method for path planning is developed that also generates contingency vehicle paths, should the initial path prove too risky. By using the isochronous trajectory patterns it is also possible to identify particular conditions that consider sensor readings or delays in the measured data, that must trigger a switch to a particular contingency vehicle path.

\emph {The isochronous trajectory is defined to be a sequence of position vectors in the 3-dimensional Euclidean space, the notation used is $\Gamma$ and has the following representation:}

 \[  \Gamma_{t} = (x_{t},y_{t},z_{t})\] where t=20ms.

	The coordinate system used in vehicle dynamics modeling is according to SAE J670e.

\begin{figure}[htbp]
   \begin{minipage}{0.6\textwidth}
     \centering
     \includegraphics[width=.8\linewidth]{figure1}
     \caption{Vehicle path with isochronous data}\label{figure1}
   \end{minipage}\hfill
   \begin{minipage}{0.4\textwidth}
     \centering
     \includegraphics[width=.85\linewidth]{figure2}
     \caption{Vehicle axis system}\label{Vehicle axis}
   \end{minipage}
\end{figure}

	From the vehicle axis system it is possible to construct the isochronous surface. The X, Y and Z direction can be represented by one or multiple nodes that describe the isochronous surface.

	The vehicle is considered to be a rigid body, the position vector for a point P in the XYZ coordinates system, will have the following representation:
\[ \vec r = \vec r_t  \]
\[ \vec r = x_t*\vec i +  y_t*\vec j + z_t*\vec k\]

\begin{figure}[htbp]
   \begin{minipage}{0.5\textwidth}
     \centering
     \includegraphics[width=.85\linewidth]{figure2_2}
     \caption{OP positional vector}\label{Fig:OP positional vector}
   \end{minipage}\hfill
   \begin{minipage}{0.5\textwidth}
     \centering
     \includegraphics[width=.85\linewidth]{figure2_3}
     \caption{O'P' vector with new coordinates}\label{O'P' vector with new coordinates}
   \end{minipage}
\end{figure}

	The position vector for a point P' in the mobile X’Y’Z’ coordinates system can be localized trough:
\[ \vec r' = \vec r'_t  \]
\[ \vec r' = x'_t*\vec i' +  y'_t*\vec j' + z'_t*\vec k' \]

	Also, the origin O’ for the mobile X’Y’Z’ coordinates system in comparison with the origin O for the XYZ coordinates system can be represented trough the position vector:
\[ \vec r_0 = \vec r_{0_t} \]
\[ \vec r_0 = x_{0_t}*\vec i +  y_{0_t}*\vec j + z_{0_t}*\vec k \]
and thus:
\[ \vec r_t= \vec r_{0_t} + \vec r'_t \]

The relation between $ \vec r'_t $ and $ \vec r_t $ can be represented trough the following relation:
\[ i' = \alpha_{11}*i + \alpha_{12}*j + \alpha_{13}*k \]
\[ j' = \alpha_{21}*i + \alpha_{22}*j + \alpha_{23}*k \]
\[ k' = \alpha_{31}*i + \alpha_{32}*j + \alpha_{33}*k \]

And from this dependency the isochronous matrix that characterizes the state change for one time step is determined:
\[
A =
\begin{pmatrix}
\alpha_{11} & \alpha_{12} & \alpha_{13}\\
\alpha_{21} & \alpha_{22} & \alpha_{23}\\
\alpha_{31} & \alpha_{32} & \alpha_{33}
\end{pmatrix}
\]

From the above representation it is possible to derive the velocity isochronous matrix:
\[ \vec v = \frac{d \vec r}{dt} = \dot{\vec{r}} = \frac{d \vec r_{0}}{d t} + \frac{d \vec r'}{d t} = \dot{\vec{r_0}} + \dot{\vec{r'}} \]
\[ \dot{\vec{r_0}} = \dot{x_{0_t}} \vec i +  \dot{y_{0_t}} \vec j + \dot{z_{0_t}} \vec k \]
\[ \dot{\vec{r'}} = \dot {x}' \vec{i'} + \dot{y}'\vec{j'}+ \dot{z'} \vec{k'} \]

Each isochronous surface can have multiple nodes that characterize the state of respective surface. For one mini-node, the vehicle velocity isochronous matrix can be represented:
\begin{equation}  \label{eq:isochronous matrice}
V =
\begin{pmatrix}
\upsilon_{11} & \upsilon_{12} & \upsilon_{13}\\
\upsilon_{21} & \upsilon_{22} & \upsilon_{23}\\
\upsilon_{31} & \upsilon_{32} & \upsilon_{33}
\end{pmatrix}
\end{equation}

The equation \eqref{eq:isochronous matrice} can be applied to each mini-node that forms the isochronous surface. For when there are 9 mini-nodes, the vehicle velocity isochronous matrix will be:

\begin{equation}  \label{eq:isochronous main node}
V_{iso} =
\begin{pmatrix}
\begin{pmatrix}
\upsilon_{111} & \upsilon_{112} & \upsilon_{113}\\
\upsilon_{121} & \upsilon_{122} & \upsilon_{123}\\
\upsilon_{131} & \upsilon_{132} & \upsilon_{133}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{211} & \upsilon_{212} & \upsilon_{213}\\
\upsilon_{221} & \upsilon_{222} & \upsilon_{223}\\
\upsilon_{231} & \upsilon_{232} & \upsilon_{233}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{311} & \upsilon_{312} & \upsilon_{313}\\
\upsilon_{321} & \upsilon_{322} & \upsilon_{323}\\
\upsilon_{331} & \upsilon_{332} & \upsilon_{333}
\end{pmatrix} \\
\\
\begin{pmatrix}
\upsilon_{411} & \upsilon_{412} & \upsilon_{413}\\
\upsilon_{421} & \upsilon_{422} & \upsilon_{423}\\
\upsilon_{431} & \upsilon_{432} & \upsilon_{433}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{511} & \upsilon_{512} & \upsilon_{513}\\
\upsilon_{521} & \upsilon_{522} & \upsilon_{523}\\
\upsilon_{531} & \upsilon_{532} & \upsilon_{533}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{611} & \upsilon_{612} & \upsilon_{613}\\
\upsilon_{621} & \upsilon_{622} & \upsilon_{623}\\
\upsilon_{631} & \upsilon_{632} & \upsilon_{633}
\end{pmatrix} \\
\\
\begin{pmatrix}
\upsilon_{711} & \upsilon_{712} & \upsilon_{713}\\
\upsilon_{721} & \upsilon_{722} & \upsilon_{723}\\
\upsilon_{731} & \upsilon_{732} & \upsilon_{733}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{811} & \upsilon_{812} & \upsilon_{813}\\
\upsilon_{821} & \upsilon_{822} & \upsilon_{823}\\
\upsilon_{831} & \upsilon_{832} & \upsilon_{833}
\end{pmatrix} &
\begin{pmatrix}
\upsilon_{911} & \upsilon_{912} & \upsilon_{913}\\
\upsilon_{921} & \upsilon_{922} & \upsilon_{923}\\
\upsilon_{931} & \upsilon_{932} & \upsilon_{933}
\end{pmatrix}
\end{pmatrix}
\end{equation}

By considering multiple isochronous surfaces it is possible to have complex predictive trajectory paths.

\section{Random segmentation surfaces for isochronous trajectories}
It is also possible to define a specific isochronous surface type and flag such segmentation surfaces that lead to changes in the trajectory path. To that extent, for a random segmentation surface $X$, the following considerations are made:
\begin{itemize}
\item there are $n$ values that characterize the segmentation surfaces $S_1, S_2,..,S_n$
\item there are $n$ surfaces dispersals $D^2(X_1), D^2(X_2), ...,D^2(X_n) $ that characterize the segmentation surfaces distributions
\item there are $n(n-1)$ isochronous correlations defined as:
\[ K_{ij}=V_{{iso}_{ij}}[(X_i-m_i)(X_j-m_j)] \]
\end{itemize}

where:
\begin{itemize}
\item $X_i, X_j$ are the random segmentation surfaces
\item $m_i, m_j$ are the mean expected surface values
\item $V_{{iso}_{ij}}$ is the vehicle velocity isochronous matrix specific to the respective random segmentation surfaces
\end{itemize}

For situations when it is required to determine the distribution of a random segmentation surface event from the trajectory path, a particular case of the correlation can be used:
\[ D^2(X_i)=K_{ii}=V_{{iso}_{ij}}[(X_i-m_i)^2] \]

For the complete trajectory $\Gamma$ it is possible to construct a correlations distribution matrix that describes the distribution of random segmentation surfaces $S_1,S_2,...,S_n$:

\[
\Gamma_{ij} =
\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
K_{21} & K_{22} & ... & K_{2n}\\
...&...&...&...\\
K_{n1} & K_{n2} & ... & K_{nn}\\
\end{pmatrix}
\]

Since the defined correlation allows the equality $K_{ij}=K_{ji}$, the  distribution matrix will also contain this equality and can be represented in the following way:

\[
\Gamma_{ij} =
\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix}
\]

As a consequence the isochronous trajectory is expanded to include the random segmentation surfaces and the correlation distribution matrix for them:

\begin{equation}  \label{eq:isochronous trajectory with random segmentation}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} \right)
\end{equation}

\subsection{Domain of multiple random segmentation surfaces}
It is well known that a geometric mesh represents the discretization of the geometric domain into smaller and simpler shapes. For the two dimension, the triangles and quadrilaterals meshes are used. For the tree dimensions, the tetrahedral and hexahedral meshes are used as defined by \citet {key:3}.

	Since the trajectory path is a mesh of isochronous surfaces, based on previous recorded path data and the received node information, it is possible to detect path domains where movement obstacles might exist.

	Such an obstacle domain can be broken down into groups of segmentation surfaces with various sizes. The spacing between such surfaces can be identical with the isochronous surfaces space. The boundary for the obstacle domain will be:

\[OBS = S_1 \cup S_2 \]

where the $S_1$ and $S_2$ will have the following representation:
\[(S_1) \mapsto X= \zeta_1(Y,Z)\]
\[(S_2) \mapsto X= \zeta_2(Y,Z)\]

\begin{figure}[!h]
\centering
{\includegraphics[width=0.7\textwidth]{figure7}}
{\caption{Trajectory segmentation surfaces create a domain in the future trajectory path where risks exists.}}
\end{figure}

	The obstacle domain is named as $ \Omega $ and it is contained within the $ \Psi $ domain defined by the segmentation surface $X=C_3$ till $X=D_3$, where $C_3$ and $D_3$ are random surfaces that might or might not consider the time constraints that the isochronous surfaces follow.

	From this representation it is possible to define a function that defines the movement trough this section path SP. The resulting values can be found in the real numbers domain.

	\[ \hat f : SP \mapsto R  \]

\[ \hat f(x,y,z) =
\left\{
	\begin{array}{ll}
		f(x,y,z)  & \mbox{, if } (x,y,z) \in \Omega \\
		0 & \mbox{, if } (x,y,z) \in  $ $ $$\Psi$ / $\Omega$$
	\end{array}
\right.
\]

From this it is possible to determine the function for the path section domain:

\[ \iiint\limits_{SP} \hat f(x,y,z) dxdydz =  \iiint\limits_\Omega \hat f(x,y,z) dxdydz + \iiint\limits_\Psi \hat f(x,y,z) dxdydz  \]

\[ \iiint\limits_{SP} \hat f(x,y,z) dxdydz =  \iint\limits_{SP_{YZ}} \left( \int\limits_{C_3}^{D_3} \hat f(x,y,z) dx \right) dydz \]

Now it is possible to define the complete $ \Omega $ domain and the $ \Psi $ domain following the X-axis:

\[ \int\limits_{C_3}^{D_3} \hat f(x,y,z) dx = \int\limits_{C_3}^{\zeta_1(Y,Z)} \hat f(x,y,z) dx + \int\limits_{\zeta_1(Y,Z)}^{\zeta_2(Y,Z)} \hat f(x,y,z) dx + \int\limits_{\zeta_2(Y,Z)}^{D_3} \hat f(x,y,z) dx\]

	Or only considering the $\Omega$ domain:

\[ \int\limits_{C_3}^{D_3} \hat f(x,y,z) dx = 0 + \int\limits_{\zeta_1(Y,Z)}^{\zeta_2(Y,Z)} \hat f(x,y,z) dx + 0\]

Since the domain outside the $ \Omega $ domain is minimized to the 0 value, it can be considered that:

\[ \iiint\limits_{SP} \hat f(x,y,z) dxdydz =  \iiint\limits_\Omega f(x,y,z) dxdydz\]

At the same time, if multiple path section domains are to be encountered, the trajectory using isochronous surfaces can be described as:

\[ \iiint\limits_{SP} f(x,y,z) dxdydz =  \sum\limits_{q=1}^n \iiint\limits_{\Omega_q} f(x,y,z) dxdydz\]

From the defined isochronous surfaces it is possible to construct a trajectory mesh by using a mapping function. The coordinates of the nodes are known in the isochronous surface. The obtained mesh is then super imposed on the trajectory images.

As a consequence the isochronous trajectory is expanded to include the obstacle domain $\Omega$:

\begin{equation}  \label{eq:isochronous trajectory with obstacle domain}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} , \hat f(x,y,z) \right)
\end{equation}

\subsection{Probability of segmentation surfaces}
Since a possible change in the trajectory is initialized by the segmentation surface, the distribution of the trajectory change can be modeled using the Laplace–Gauss distribution.

The density of repartition is described in \citet {key:7} as:

\[ f(x; m; \sigma) = \frac {1}{\sigma \sqrt 2\pi}e^{-\frac{1}{2}\frac{(x-m)^2}{\sigma^2}}\]

where:
\begin{itemize}
\item $x$ real-valued random variable
\item $m$ maximum
\item $\sigma$ standard deviation
\end{itemize}

The probability that random segmentation surface $S_n$ takes a value between $(x_1, x_2)$ is given by:

\[ P(x_1 < S_n < x_2) = \int_{x_1}^{x_2} f(x; m; \sigma) dx = \frac {1}{\sigma \sqrt 2\pi}\int_{x_1}^{x_2} e^{-\frac{1}{2}\frac{(x-m)^2}{\sigma^2}}dx\]

If we make the variable change $y=\frac{x-m}{\sigma}$ then $x=\sigma y + m$, $dx=\sigma dy$.
\begin{itemize}
\item $x= x_1$ then $y=\frac{x_1-m}{\sigma}$
\item $x= x_2$ then $y=\frac{x_2-m}{\sigma}$
\end{itemize}

\[ P(x_1 < S_n < x_2) = \frac {\sigma}{\sigma \sqrt 2\pi}\int_{\frac{x_1-m}{\sigma}}^{\frac{x_2-m}{\sigma}} e^{-\frac{y}{2}}dy = \frac {1}{\sqrt 2\pi} \left( \int_{-\infty}^{\frac{x_2-m}{\sigma}} e^{-\frac{y}{2}}dy - \int_{-\infty}^{\frac{x_1-m}{\sigma}} e^{-\frac{y}{2}}dy \right)\]

If we consider the distribution function with the parameters 0 and 1, we can define the Laplace function $\Phi$:

\[ \Phi(x) = \int_{-\infty}^{x} f(y; 0; 1) dy = \frac {1}{\sqrt 2\pi}\int_{-\infty}^{x} e^{-\frac{y}{2}}dy\]

and the probability that random segmentation surface $S_n$ takes a value between $(x_1, x_2)$ is given by:

\[ P(x_1 < S_n < x_2) = \Phi\left(\frac{x_2-m}{\sigma}\right) - \Phi\left(\frac{x_1-m}{\sigma}\right) \]

As a consequence the isochronous trajectory is expanded to include the probability of random segmentation surfaces:

\begin{equation}  \label{eq:isochronous trajectory with probability}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} , \hat f(x,y,z), P(x_1 < S_n < x_2) \right)
\end{equation}

\section{Isochronous trajectory chords}

Since it is possible to start from a node, follow an edge to the next node and then select another edge that is not adjacent to the first edge to continue to another edge, it is possible to define the concept of isochronous surface chords. They can be closed or open curves and can produce self-intersections.

\begin{theorem}
If one of the chords is disturbed, the disturbance is instantaneous transmitted throughout the complete $\Gamma_{t}$.
\end{theorem}

By observing the amplitude of the chords disturbance it will be possible to build predictive paths for the remaining trajectory while at the same time updating the already known path.

As previously defined, the isochronous trajectory $\Gamma$, is composed of isochronous surfaces that interact with each other trough the chords.
The $\Gamma$ trajectory can be updated with new data from trajectories not yet travelled. The intent to change the already known $\Gamma$ trajectory with another $\Gamma$ trajectory, is captured by the "vibrations" of the isochronous surface chords defined as $\delta (t)$.

By analyzing the isochronous surface matrix of the velocity, acceleration and trajectory path updates it is possible to identify the isochronous surfaces that favour the appearance of chords vibrations.

\[ \delta (t) = \upsilon _{m,n,p}(t) +  \dot \upsilon _{m,n,p}(t) + \Omega _{m,n,p}(t)\]

where:
\begin{itemize}
\item m,n,p are the indexes of the mini-nodes for each isochronous surface
\item $\upsilon _{m,n,p}(t)$ is the velocity
\item $\dot  \upsilon _{m,n,p}(t)$ is the acceleration
\item $\Omega _{m,n,p}(t)$ the obstacle domains
\end{itemize}

By looking for the minimum $\delta (t)$ variation, it is possible to determine the complete trajectory profile of chords vibrations.

\[ \Gamma (t) = f({\delta _{t_1}, \delta _{t_2}, ..., \delta _{t_n}}) \]

where n is the number of isochronous surfaces with mini-nodes that have either chords vibrations or the probability of chords vibrations.

Since the trajectory to be traveled is not completely known, the chords vibrations have different propagation probabilities. For the path already traveled, the probability is $100\% $. For the path that is yet to be traveled, the chords vibrations propagation probability, will have the following form:

\[ \Gamma _{t} = f({\rho _{t_1}, \rho _{t_2}, ..., \rho _{t_n}}) \]

where:
\begin{itemize}
\item $\Gamma _{t}$ is the trajectory
\item $\rho _{t}$ is the chords vibrations propagation probability
\item t is the time step
\end{itemize}

If the $\rho _{t}$ is higher then a certain threshold $\rho _{t_{min}}$, then the corresponding isochronous surface is selected as a segmentation surface.

As a consequence the isochronous trajectory is expanded to include the chords vibrations and the chords vibrations propagation probability:

\begin{equation}  \label{eq:isochronous trajectory with chords}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} , \hat f(x,y,z), P(x_1 < S_n < x_2), \delta _{t_n}, \rho _{t_n} \right)
\end{equation}

\newpage

\section{Measurements}
	Instrumentation used in this study for the trajectory measurements consisted of a compass HDMM01 and a ArduinoUno.
The instruments used the factory calibration with no modifications. The sensor and ArduinoUno were fixed in a case that was placed on the vehicle floor. The 20 ms sample time between isochronous nodes was chosen in order to allow multiple readings within the target speed range of 0 to 50km/h. Also, the same driver was used to gather the data and the same trajectory was chosen to illustrate the method. Different hours and days where used to have a good sample of traffic influence on the trajectory isochronous patterns.

	The measurement system is implemented with the sensor connected with a two wire (i2C) cable to the Arduino. The data from compass is the raw result of measured acceleration along a reference axis. No correction is done on the signal, no zero bias, zero scale factor or random error correction is applied.

\begin{table}[htb]
\begin{tabular}{ l | l | l | l | l }
\hline
\multicolumn{5}{|c|}{Compass module}                                       \\
\hline \hline
\multicolumn{2}{|l|}{Type}                  & \multicolumn{3}{l|}{HDMM01,  Dual-axis Magnetic Sensor Module} \\
\hline
\multicolumn{2}{|l|}{Effective measurement range}                  & \multicolumn{3}{l|}{± 5 gausses} \\
\hline
\multicolumn{2}{|l|}{Sensitivity} & \multicolumn{3}{l|}{512counts/gauss @3.0V at 25°C} \\
\hline
\multicolumn{2}{|l|}{}                  & \multicolumn{3}{l|}{MIN: 461 counts/gauss} \\
\hline
\multicolumn{2}{|l|}{}                  & \multicolumn{3}{l|}{MAX: 563 counts/gauss} \\
\hline
\multicolumn{2}{|l|}{Supply Voltage}                  & \multicolumn{3}{l|}{2.7 … 5.25 V} \\
\hline
\multicolumn{2}{|l|}{Noise Density}                  & \multicolumn{3}{l|}{1 $\sim$ 25Hz, RMS, 600 \textmu gauss} \\
\hline
\multicolumn{2}{|l|}{Accuracy}                  & \multicolumn{3}{l|}{±5 deg} \\
\hline \hline
\multicolumn{5}{|c|}{Mechanical properties of the module}                                         \\
\hline
\multicolumn{2}{|l|}{L × l × H}                  & \multicolumn{3}{l|}{16,20 × 12,30 × 2,25 (mm)} \\
\hline
\end{tabular}
\end{table}

	From these readings it was possible to derive the specific trajectory horizon of predictive isochronous paths. The trajectory graph consists of nodes, which are usually represented as circles, and edges, which are represented as line segments connecting the nodes.

\begin{figure}[htb]
\centering
{\includegraphics[width=0.7\textwidth]{figure4}}
{\caption{Each isochronous surface can have multiple mini-nodes}}
\end{figure}

\begin{figure}[htb]
\centering
{\includegraphics[width=0.6\textwidth]{figure3}}
{\caption{Segment of vehicle path with isochronous surfaces}}
\end{figure}

	The forward direction can be derived from the corners of the isochronous surface or it can be derived from the center of the isochronous 9 mini-nodes representation. With the 16 mini-nodes representation, it is possible to have an increased precision for the trajectory prediction. Adding mini-nodes to each isochronous surface leads to improved prediction accuracy in situations like intersections or vehicle lane changing.

	From the increased number of mini-nodes for each isochronous surface it is possible to reverse to less complex isochronous surface representation when the vehicle is approaching a stop light or an immediate standstill situation.

	Each node of the trajectory map represents a decision point, such as, “Must I make a left or a right turn?”. The corners of isochronous surface will indicate if the probability of a left or right turn decision is going to be increased or not. The predictive paths can be evaluated according to the benefits they offer, like reaching the destination with current vehicle energy reserves or improve operating parameters if the path adds a delay of minutes. From these paths, the optimal plan can be one that maximizes the vehicle state of operation.

	Each mini-node and isochronous surface can give a certain probability that the vehicle will follow a specific path. A straight line trajectory can be represented with the green color, while an approaching vehicle path change can be represented from yellow to red. Previous compass data is used to provide predictive trajectory isochronous surfaces. Based on the compass data, the complexity of the isochronous surfaces can be reduced if there is a high probability that the vehicle will follow previous recorded paths.

\newpage

\begin{figure}[!h]
\centering
{\includegraphics[width=0.9\textwidth]{figure5}}
{\caption{Multiple isochronous trajectories.}{      Green (most likely), yellow (back-up path), red (not allowed)}\label{figure5}}
\end{figure}

	In figure \ref{figure5}, the vehicle will most likely follow the path no.1, as indicated by the color green in the isochronous trajectory surfaces. Nevertheless the path estimation can consider also path no.2 (red) and path no.3(yellow) as possible vehicle trajectories. The path no.2 (red) is a forbidden trajectory path. By having the video data and the compass data it is possible to validate the predictive trajectory isochronous surfaces.

	If the uncertainty level for a specific trajectory path reaches a certain level, it is possible to increase the number of mini-nodes for each isochronous surface. Thus, every small change in the sensor value can be captured and the prediction horizon for the vehicle trajectory can be increased.

The compass data can be used to determine also other variables that can characterize the vehicle behavior on the selected path:
\begin{itemize}
\item speed and shifting: slipping in and out of gears
\item acceleration and deceleration: the way a vehicle is maneuvered can influence how other vehicles react
\item changing lanes: it is possible to recognize lateral movements created by a vehicle and differentiate a left-lane change from a right-lane change
\end{itemize}

\begin{figure}[htbp]
\centering
{\includegraphics[width=0.8\textwidth]{figure6}}
{\caption{Measurement results}}
\end{figure}

	The equation \ref{eq:isochronous trajectory with chords} provides the necessary information to calculate and predict the trajectory path $\Gamma_{t}$. For day 1, since the profile of trajectory path is not yet known, the information about possible obstacle domain $\Omega$ can only be received from the road infrastructure. In case of such obstacle domains $\Omega$ in day 1, the $\Gamma_{t}$ will be prepared to encounter them in day 2 and day 3.
	In a similar way, the random segmentation surfaces, the correlation distribution matrix for them, the probability of random segmentation surfaces, the chords vibrations and the chords vibrations propagation probability are predicted with some degree of freedom in day 1. For day 2 and day 3 the accuracy of the $\Gamma_{t}$ predictability increases.

\section{Conclusions}
	In the present paper it was demonstrated the usage of predictive isochronus trajectories based on recorded compass sensor data. The measurement system is simple to construct, portable, cost efficient, low power consuming and reliable.

	By using predictive isochronus trajectories it is possible to determine vehicle path within a predefined time horizon. The mini-nodes that constitute the isochronus surfaces can provide great accuracy in predicting the desired trajectory.

	At the same time, energy savings can be obtain if the vehicle has a trajectory horizon estimation algorithm. Multiple energy systems can be synchronized to work together to achieve the best energy efficiency for the planned trajectory.

\newpage
\section{References}

\nocite{*}

\section{Introduction}
	Predicting vehicle behavior is a major challenge and an active research domain for the automotive industry. The desire to develop intelligent vehicles that make good use of predictive vehicle behavior and contribute to the improvement of road safety can find a possible implementation by using the low cost compass sensor data.

	Complex planning algorithms have begun to factor in uncertainty that arise from variations in travel time, erratic communication between autonomous vehicles, imperfect sensor data or other situations. To consider such scenarios, a method for path planning is developed that also generates contingency vehicle paths, should the initial path prove too risky. By using the isochronous trajectory patterns it is also possible to identify particular conditions that consider sensor readings or delays in the measured data, that must trigger a switch to a particular contingency vehicle path.

\emph {The isochronous trajectory is defined to be a sequence of position vectors in the 3-dimensional Euclidean space, the notation used is $\Gamma$ and has the following representation:}

 \[  \Gamma_{t} = (x_{t},y_{t},z_{t})\] where t=20ms.

	The coordinate system used in vehicle dynamics modeling is according to SAE J670e as shown in Figure 2.

\begin{figure}[htbp]
   \begin{minipage}{0.6\textwidth}
     \centering
     \includegraphics[width=.8\linewidth]{figure1}
     \caption{Vehicle path with isochronous data}\label{figure1}
   \end{minipage}\hfill
   \begin{minipage}{0.4\textwidth}
     \centering
     \includegraphics[width=.85\linewidth]{figure2}
     \caption{Vehicle axis system}\label{Vehicle axis}
   \end{minipage}
\end{figure}

	From the vehicle axis system it is possible to construct the isochronous surface. The X, Y and Z direction can be represented by one or multiple nodes that describe the isochronous surface.

	The vehicle is considered to be a rigid body, the position vector for a point P in the XYZ coordinates system, will have the following representation:
\[ \vec r = \vec r_t  \]
\[ \vec r = x_t*\vec i +  y_t*\vec j + z_t*\vec k\]

\begin{figure}[htbp]
   \begin{minipage}{0.5\textwidth}
     \centering
     \includegraphics[width=.85\linewidth]{figure2_2}
     \caption{OP positional vector}\label{Fig:OP positional vector}
   \end{minipage}\hfill
   \begin{minipage}{0.5\textwidth}
     \centering
     \includegraphics[width=.85\linewidth]{figure2_3}
     \caption{O'P' vector with new coordinates}\label{O'P' vector with new coordinates}
   \end{minipage}
\end{figure}

	The position vector for a point P' in the mobile X’Y’Z’ coordinates system can be localized trough:
\[ \vec r' = \vec r'_t  \]
\[ \vec r' = x'_t*\vec i' +  y'_t*\vec j' + z'_t*\vec k' \]

	Also, the origin O’ for the mobile X’Y’Z’ coordinates system in comparison with the origin O for the XYZ coordinates system can be represented trough the position vector:
\[ \vec r_0 = \vec r_{0_t} \]
\[ \vec r_0 = x_{0_t}*\vec i +  y_{0_t}*\vec j + z_{0_t}*\vec k \]
and thus:
\[ \vec r_t= \vec r_{0_t} + \vec r'_t \]

The relation between $ \vec r'_t $ and $ \vec r_t $ can be represented trough the following relation:
\[ i' = \alpha_{11}*i + \alpha_{12}*j + \alpha_{13}*k \]
\[ j' = \alpha_{21}*i + \alpha_{22}*j + \alpha_{23}*k \]
\[ k' = \alpha_{31}*i + \alpha_{32}*j + \alpha_{33}*k \]

And from this dependency the isochronous matrix that characterizes the state change for one time step is determined:
\[
A =
\begin{pmatrix}
\alpha_{11} & \alpha_{12} & \alpha_{13}\\
\alpha_{21} & \alpha_{22} & \alpha_{23}\\
\alpha_{31} & \alpha_{32} & \alpha_{33}
\end{pmatrix}
\]

From the above representation it is possible to derive the velocity isochronous matrix:
\[ \vec v = \frac{d \vec r}{dt} = \dot{\vec{r}} = \frac{d \vec r_{0}}{d t} + \frac{d \vec r'}{d t} = \dot{\vec{r_0}} + \dot{\vec{r'}} \]
\[ \dot{\vec{r_0}} = \dot{x_{0_t}} \vec i +  \dot{y_{0_t}} \vec j + \dot{z_{0_t}} \vec k \]
\[ \dot{\vec{r'}} = \dot {x}' \vec{i'} + \dot{y}'\vec{j'}+ \dot{z'} \vec{k'} \]

Each isochronous surface can have multiple nodes that characterize the state of respective surface. For one mini-node, the vehicle velocity isochronous matrix can be represented:
\begin{equation}  \label{eq:isochronous matrice}
V =
\begin{pmatrix}
\upsilon_{11} & \upsilon_{12} & \upsilon_{13}\\
\upsilon_{21} & \upsilon_{22} & \upsilon_{23}\\
\upsilon_{31} & \upsilon_{32} & \upsilon_{33}
\end{pmatrix}
\end{equation}

The equation \eqref{eq:isochronous matrice} can be applied to each mini-node that forms the isochronous surface. For when there are 9 mini-nodes, the vehicle velocity isochronous matrix will be:

\begin{equation}  \label{eq:isochronous main node}
V_{iso} =
\begin{pmatrix}
\begin{pmatrix}
\upsilon_{111} & \upsilon_{112} & \upsilon_{113}\\
\upsilon_{121} & \upsilon_{122} & \upsilon_{123}\\
\upsilon_{131} & \upsilon_{132} & \upsilon_{133}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{211} & \upsilon_{212} & \upsilon_{213}\\
\upsilon_{221} & \upsilon_{222} & \upsilon_{223}\\
\upsilon_{231} & \upsilon_{232} & \upsilon_{233}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{311} & \upsilon_{312} & \upsilon_{313}\\
\upsilon_{321} & \upsilon_{322} & \upsilon_{323}\\
\upsilon_{331} & \upsilon_{332} & \upsilon_{333}
\end{pmatrix} \\
\\
\begin{pmatrix}
\upsilon_{411} & \upsilon_{412} & \upsilon_{413}\\
\upsilon_{421} & \upsilon_{422} & \upsilon_{423}\\
\upsilon_{431} & \upsilon_{432} & \upsilon_{433}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{511} & \upsilon_{512} & \upsilon_{513}\\
\upsilon_{521} & \upsilon_{522} & \upsilon_{523}\\
\upsilon_{531} & \upsilon_{532} & \upsilon_{533}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{611} & \upsilon_{612} & \upsilon_{613}\\
\upsilon_{621} & \upsilon_{622} & \upsilon_{623}\\
\upsilon_{631} & \upsilon_{632} & \upsilon_{633}
\end{pmatrix} \\
\\
\begin{pmatrix}
\upsilon_{711} & \upsilon_{712} & \upsilon_{713}\\
\upsilon_{721} & \upsilon_{722} & \upsilon_{723}\\
\upsilon_{731} & \upsilon_{732} & \upsilon_{733}
\end{pmatrix}  &
\begin{pmatrix}
\upsilon_{811} & \upsilon_{812} & \upsilon_{813}\\
\upsilon_{821} & \upsilon_{822} & \upsilon_{823}\\
\upsilon_{831} & \upsilon_{832} & \upsilon_{833}
\end{pmatrix} &
\begin{pmatrix}
\upsilon_{911} & \upsilon_{912} & \upsilon_{913}\\
\upsilon_{921} & \upsilon_{922} & \upsilon_{923}\\
\upsilon_{931} & \upsilon_{932} & \upsilon_{933}
\end{pmatrix}
\end{pmatrix}
\end{equation}

By considering multiple isochronous surfaces it is possible to have complex predictive trajectory paths.

\section{Random segmentation surfaces for isochronous trajectories}
It is also possible to define a specific isochronous surface type and flag such segmentation surfaces that lead to changes in the trajectory path. To that extent, for a random segmentation surface $X$, the following considerations are made:
\begin{itemize}
\item there are $n$ values that characterize the segmentation surfaces $S_1, S_2,..,S_n$
\item there are $n$ surfaces dispersals $D^2(X_1), D^2(X_2), ...,D^2(X_n) $ that characterize the segmentation surfaces distributions
\item there are $n(n-1)$ isochronous correlations defined as:
\[ K_{ij}=V_{{iso}_{ij}}[(X_i-m_i)(X_j-m_j)] \]
\end{itemize}

where:
\begin{itemize}
\item $X_i, X_j$ are the random segmentation surfaces
\item $m_i, m_j$ are the mean expected surface values
\item $V_{{iso}_{ij}}$ is the vehicle velocity isochronous matrix specific to the respective random segmentation surfaces
\end{itemize}

For situations when it is required to determine the distribution of a random segmentation surface event from the trajectory path, a particular case of the correlation can be used:
\[ D^2(X_i)=K_{ii}=V_{{iso}_{ij}}[(X_i-m_i)^2] \]

For the complete trajectory $\Gamma$ it is possible to construct a correlations distribution matrix that describes the distribution of random segmentation surfaces $S_1,S_2,...,S_n$:

\[
\Gamma_{ij} =
\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
K_{21} & K_{22} & ... & K_{2n}\\
...&...&...&...\\
K_{n1} & K_{n2} & ... & K_{nn}\\
\end{pmatrix}
\]

Since the defined correlation allows the equality $K_{ij}=K_{ji}$, the  distribution matrix will also contain this equality and can be represented in the following way:

\[
\Gamma_{ij} =
\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix}
\]

As a consequence the isochronous trajectory is expanded to include the random segmentation surfaces and the correlation distribution matrix for them:

\begin{equation}  \label{eq:isochronous trajectory with random segmentation}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} \right)
\end{equation}

\subsection{Domain of multiple random segmentation surfaces}
It is well known that a geometric mesh represents the discretization of the geometric domain into smaller and simpler shapes. For the two dimension, the triangles and quadrilaterals meshes are used. For the tree dimensions, the tetrahedral and hexahedral meshes are used as defined by \citet {key:3}.

	Since the trajectory path is a mesh of isochronous surfaces, based on previous recorded path data and the received node information, it is possible to detect path domains where movement obstacles might exist.

	Such an obstacle domain can be broken down into groups of segmentation surfaces with various sizes. The spacing between such surfaces can be identical with the isochronous surfaces space. The boundary for the obstacle domain will be:

\[OBS = S_1 \cup S_2 \]

where the $S_1$ and $S_2$ will have the following representation:
\[(S_1) \mapsto X= \zeta_1(Y,Z)\]
\[(S_2) \mapsto X= \zeta_2(Y,Z)\]

\begin{figure}[!h]
\centering
{\includegraphics[width=0.7\textwidth]{figure7}}
{\caption{Trajectory segmentation surfaces create a domain in the future trajectory path where risks exists.}}
\end{figure}

	The obstacle domain is named as $ \Omega $ and it is contained within the $ \Psi $ domain defined by the segmentation surface $X=C_3$ till $X=D_3$, where $C_3$ and $D_3$ are random surfaces that might or might not consider the time constraints that the isochronous surfaces follow.

	From this representation it is possible to define a function that defines the movement trough this section path SP. The resulting values can be found in the real numbers domain.

	\[ \hat f : SP \mapsto R  \]

\[ \hat f(x,y,z) =
\left\{
	\begin{array}{ll}
		f(x,y,z)  & \mbox{, if } (x,y,z) \in \Omega \\
		0 & \mbox{, if } (x,y,z) \in  $ $ $$\Psi$ / $\Omega$$
	\end{array}
\right.
\]

From this it is possible to determine the function for the path section domain:

\[ \iiint\limits_{SP} \hat f(x,y,z) dxdydz =  \iiint\limits_\Omega \hat f(x,y,z) dxdydz + \iiint\limits_\Psi \hat f(x,y,z) dxdydz  \]

\[ \iiint\limits_{SP} \hat f(x,y,z) dxdydz =  \iint\limits_{SP_{YZ}} \left( \int\limits_{C_3}^{D_3} \hat f(x,y,z) dx \right) dydz \]

Now it is possible to define the complete $ \Omega $ domain and the $ \Psi $ domain following the X-axis:

\[ \int\limits_{C_3}^{D_3} \hat f(x,y,z) dx = \int\limits_{C_3}^{\zeta_1(Y,Z)} \hat f(x,y,z) dx + \int\limits_{\zeta_1(Y,Z)}^{\zeta_2(Y,Z)} \hat f(x,y,z) dx + \int\limits_{\zeta_2(Y,Z)}^{D_3} \hat f(x,y,z) dx\]

	Or only considering the $\Omega$ domain:

\[ \int\limits_{C_3}^{D_3} \hat f(x,y,z) dx = 0 + \int\limits_{\zeta_1(Y,Z)}^{\zeta_2(Y,Z)} \hat f(x,y,z) dx + 0\]

Since the domain outside the $ \Omega $ domain is minimized to the 0 value, it can be considered that:

\[ \iiint\limits_{SP} \hat f(x,y,z) dxdydz =  \iiint\limits_\Omega f(x,y,z) dxdydz\]

At the same time, if multiple path section domains are to be encountered, the trajectory using isochronous surfaces can be described as:

\[ \iiint\limits_{SP} f(x,y,z) dxdydz =  \sum\limits_{q=1}^n \iiint\limits_{\Omega_q} f(x,y,z) dxdydz\]

From the defined isochronous surfaces it is possible to construct a trajectory mesh by using a mapping function. The coordinates of the nodes are known in the isochronous surface. The obtained mesh is then super imposed on the trajectory images.

As a consequence the isochronous trajectory is expanded to include the obstacle domain $\Omega$:

\begin{equation}  \label{eq:isochronous trajectory with obstacle domain}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} , \hat f(x,y,z) \right)
\end{equation}

\subsection{Probability of segmentation surfaces}
Since a possible change in the trajectory is initialized by the segmentation surface, the distribution of the trajectory change can be modeled using the Laplace–Gauss distribution.

The density of repartition is described in \citet {key:7} as:

\[ f(x; m; \sigma) = \frac {1}{\sigma \sqrt 2\pi}e^{-\frac{1}{2}\frac{(x-m)^2}{\sigma^2}}\]

where:
\begin{itemize}
\item $x$ real-valued random variable
\item $m$ maximum
\item $\sigma$ standard deviation
\end{itemize}

The probability that random segmentation surface $S_n$ takes a value between $(x_1, x_2)$ is given by:

\[ P(x_1 < S_n < x_2) = \int_{x_1}^{x_2} f(x; m; \sigma) dx = \frac {1}{\sigma \sqrt 2\pi}\int_{x_1}^{x_2} e^{-\frac{1}{2}\frac{(x-m)^2}{\sigma^2}}dx\]

If we make the variable change $y=\frac{x-m}{\sigma}$ then $x=\sigma y + m$, $dx=\sigma dy$.
\begin{itemize}
\item $x= x_1$ then $y=\frac{x_1-m}{\sigma}$
\item $x= x_2$ then $y=\frac{x_2-m}{\sigma}$
\end{itemize}

\[ P(x_1 < S_n < x_2) = \frac {\sigma}{\sigma \sqrt 2\pi}\int_{\frac{x_1-m}{\sigma}}^{\frac{x_2-m}{\sigma}} e^{-\frac{y}{2}}dy = \frac {1}{\sqrt 2\pi} \left( \int_{-\infty}^{\frac{x_2-m}{\sigma}} e^{-\frac{y}{2}}dy - \int_{-\infty}^{\frac{x_1-m}{\sigma}} e^{-\frac{y}{2}}dy \right)\]

If we consider the distribution function with the parameters 0 and 1, we can define the Laplace function $\Phi$:

\[ \Phi(x) = \int_{-\infty}^{x} f(y; 0; 1) dy = \frac {1}{\sqrt 2\pi}\int_{-\infty}^{x} e^{-\frac{y}{2}}dy\]

and the probability that random segmentation surface $S_n$ takes a value between $(x_1, x_2)$ is given by:

\[ P(x_1 < S_n < x_2) = \Phi\left(\frac{x_2-m}{\sigma}\right) - \Phi\left(\frac{x_1-m}{\sigma}\right) \]

As a consequence the isochronous trajectory is expanded to include the probability of random segmentation surfaces:

\begin{equation}  \label{eq:isochronous trajectory with probability}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} , \hat f(x,y,z), P(x_1 < S_n < x_2) \right)
\end{equation}

\section{Isochronous trajectory chords}

Since it is possible to start from a node, follow an edge to the next node and then select another edge that is not adjacent to the first edge to continue to another edge, it is possible to define the concept of isochronous surface chords. They can be closed or open curves and can produce self-intersections.

\begin{theorem}
If one of the chords is disturbed, the disturbance is instantaneous transmitted throughout the complete $\Gamma_{t}$.
\end{theorem}

By observing the amplitude of the chords disturbance it will be possible to build predictive paths for the remaining trajectory while at the same time updating the already known path.

As previously defined, the isochronous trajectory $\Gamma$, is composed of isochronous surfaces that interact with each other trough the chords.
The $\Gamma$ trajectory can be updated with new data from trajectories not yet travelled. The intent to change the already known $\Gamma$ trajectory with another $\Gamma$ trajectory, is captured by the "vibrations" of the isochronous surface chords defined as $\delta (t)$.

By analyzing the isochronous surface matrix of the velocity, acceleration and trajectory path updates it is possible to identify the isochronous surfaces that favour the appearance of chords vibrations.

\[ \delta (t) = \upsilon _{m,n,p}(t) +  \dot \upsilon _{m,n,p}(t) + \Omega _{m,n,p}(t)\]

where:
\begin{itemize}
\item m,n,p are the indexes of the mini-nodes for each isochronous surface
\item $\upsilon _{m,n,p}(t)$ is the velocity
\item $\dot  \upsilon _{m,n,p}(t)$ is the acceleration
\item $\Omega _{m,n,p}(t)$ the obstacle domains
\end{itemize}

By looking for the minimum $\delta (t)$ variation, it is possible to determine the complete trajectory profile of chords vibrations.

\[ \Gamma (t) = f({\delta _{t_1}, \delta _{t_2}, ..., \delta _{t_n}}) \]

where n is the number of isochronous surfaces with mini-nodes that have either chords vibrations or the probability of chords vibrations.

Since the trajectory to be traveled is not completely known, the chords vibrations have different propagation probabilities. For the path already traveled, the probability is $100\% $. For the path that is yet to be traveled, the chords vibrations propagation probability, will have the following form:

\[ \Gamma _{t} = f({\rho _{t_1}, \rho _{t_2}, ..., \rho _{t_n}}) \]

where:
\begin{itemize}
\item $\Gamma _{t}$ is the trajectory
\item $\rho _{t}$ is the chords vibrations propagation probability
\item t is the time step
\end{itemize}

If the $\rho _{t}$ is higher then a certain threshold $\rho _{t_{min}}$, then the corresponding isochronous surface is selected as a segmentation surface.

As a consequence the isochronous trajectory is expanded to include the chords vibrations and the chords vibrations propagation probability:

\begin{equation}  \label{eq:isochronous trajectory with chords}
\Gamma_{t} = f\left((x_{t},y_{t},z_{t}),\begin{pmatrix}
K_{11} & K_{12} & ... & K_{1n}\\
       & K_{22} & ... & K_{2n}\\
 & &...&\\
 & & ... & K_{nn}\\
\end{pmatrix} , \hat f(x,y,z), P(x_1 < S_n < x_2), \delta _{t_n}, \rho _{t_n} \right)
\end{equation}

\newpage

\section{Measurements}
	Instrumentation used in this study for the trajectory measurements consisted of a compass HDMM01 and a ArduinoUno.
The instruments used the factory calibration with no modifications. The sensor and ArduinoUno were fixed in a case that was placed on the vehicle floor. The 20 ms sample time between isochronous nodes was chosen in order to allow multiple readings within the target speed range of 0 to 50km/h. Also, the same driver was used to gather the data and the same trajectory was chosen to illustrate the method. Different hours and days where used to have a good sample of traffic influence on the trajectory isochronous patterns.

	The measurement system is implemented with the sensor connected with a two wire (i2C) cable to the Arduino. The data from compass is the raw result of measured acceleration along a reference axis. No correction is done on the signal, no zero bias, zero scale factor or random error correction is applied.

\begin{table}[htb]
\begin{tabular}{ l | l | l | l | l }
\hline
\multicolumn{5}{|c|}{Compass module}                                       \\
\hline \hline
\multicolumn{2}{|l|}{Type}                  & \multicolumn{3}{l|}{HDMM01,  Dual-axis Magnetic Sensor Module} \\
\hline
\multicolumn{2}{|l|}{Effective measurement range}                  & \multicolumn{3}{l|}{± 5 gausses} \\
\hline
\multicolumn{2}{|l|}{Sensitivity} & \multicolumn{3}{l|}{512counts/gauss @3.0V at 25°C} \\
\hline
\multicolumn{2}{|l|}{}                  & \multicolumn{3}{l|}{MIN: 461 counts/gauss} \\
\hline
\multicolumn{2}{|l|}{}                  & \multicolumn{3}{l|}{MAX: 563 counts/gauss} \\
\hline
\multicolumn{2}{|l|}{Supply Voltage}                  & \multicolumn{3}{l|}{2.7 … 5.25 V} \\
\hline
\multicolumn{2}{|l|}{Noise Density}                  & \multicolumn{3}{l|}{1 $\sim$ 25Hz, RMS, 600 \textmu gauss} \\
\hline
\multicolumn{2}{|l|}{Accuracy}                  & \multicolumn{3}{l|}{±5 deg} \\
\hline \hline
\multicolumn{5}{|c|}{Mechanical properties of the module}                                         \\
\hline
\multicolumn{2}{|l|}{L × l × H}                  & \multicolumn{3}{l|}{16,20 × 12,30 × 2,25 (mm)} \\
\hline
\end{tabular}
\end{table}

	From these readings it was possible to derive the specific trajectory horizon of predictive isochronous paths. The trajectory graph consists of nodes, which are usually represented as circles, and edges, which are represented as line segments connecting the nodes.

\begin{figure}[htb]
\centering
{\includegraphics[width=0.7\textwidth]{figure4}}
{\caption{Each isochronous surface can have multiple mini-nodes}}
\end{figure}

\begin{figure}[htb]
\centering
{\includegraphics[width=0.6\textwidth]{figure3}}
{\caption{Segment of vehicle path with isochronous surfaces}}
\end{figure}

	The forward direction can be derived from the corners of the isochronous surface or it can be derived from the center of the isochronous 9 mini-nodes representation. With the 16 mini-nodes representation, it is possible to have an increased precision for the trajectory prediction. Adding mini-nodes to each isochronous surface leads to improved prediction accuracy in situations like intersections or vehicle lane changing.

	From the increased number of mini-nodes for each isochronous surface it is possible to reverse to less complex isochronous surface representation when the vehicle is approaching a stop light or an immediate standstill situation.

	Each node of the trajectory map represents a decision point, such as, “Must I make a left or a right turn?”. The corners of isochronous surface will indicate if the probability of a left or right turn decision is going to be increased or not. The predictive paths can be evaluated according to the benefits they offer, like reaching the destination with current vehicle energy reserves or improve operating parameters if the path adds a delay of minutes. From these paths, the optimal plan can be one that maximizes the vehicle state of operation.

	Each mini-node and isochronous surface can give a certain probability that the vehicle will follow a specific path. A straight line trajectory can be represented with the green color, while an approaching vehicle path change can be represented from yellow to red. Previous compass data is used to provide predictive trajectory isochronous surfaces. Based on the compass data, the complexity of the isochronous surfaces can be reduced if there is a high probability that the vehicle will follow previous recorded paths.

\newpage

\begin{figure}[!h]
\centering
{\includegraphics[width=0.9\textwidth]{figure5}}
{\caption{Multiple isochronous trajectories.}{      Green (most likely), yellow (back-up path), red (not allowed)}\label{figure5}}
\end{figure}

	In figure \ref{figure5}, the vehicle will most likely follow the path no.1, as indicated by the color green in the isochronous trajectory surfaces. Nevertheless the path estimation can consider also path no.2 (red) and path no.3(yellow) as possible vehicle trajectories. The path no.2 (red) is a forbidden trajectory path. By having the video data and the compass data it is possible to validate the predictive trajectory isochronous surfaces.

	If the uncertainty level for a specific trajectory path reaches a certain level, it is possible to increase the number of mini-nodes for each isochronous surface. Thus, every small change in the sensor value can be captured and the prediction horizon for the vehicle trajectory can be increased.

The compass data can be used to determine also other variables that can characterize the vehicle behavior on the selected path:
\begin{itemize}
\item speed and shifting: slipping in and out of gears
\item acceleration and deceleration: the way a vehicle is maneuvered can influence how other vehicles react
\item changing lanes: it is possible to recognize lateral movements created by a vehicle and differentiate a left-lane change from a right-lane change
\end{itemize}

\begin{figure}[htbp]
\centering
{\includegraphics[width=0.8\textwidth]{figure6}}
{\caption{Measurement results}}
\end{figure}

	The equation \ref{eq:isochronous trajectory with chords} provides the necessary information to calculate and predict the trajectory path $\Gamma_{t}$. For day 1, since the profile of trajectory path is not yet known, the information about possible obstacle domain $\Omega$ can only be received from the road infrastructure. In case of such obstacle domains $\Omega$ in day 1, the $\Gamma_{t}$ will be prepared to encounter them in day 2 and day 3.
	In a similar way, the random segmentation surfaces, the correlation distribution matrix for them, the probability of random segmentation surfaces, the chords vibrations and the chords vibrations propagation probability are predicted with some degree of freedom in day 1. For day 2 and day 3 the accuracy of the $\Gamma_{t}$ predictability increases.

\section{Conclusions}
	In the present paper it was demonstrated the usage of predictive isochronus trajectories based on recorded compass sensor data. The measurement system is simple to construct, portable, cost efficient, low power consuming and reliable.

	By using predictive isochronus trajectories it is possible to determine vehicle path within a predefined time horizon. The mini-nodes that constitute the isochronus surfaces can provide great accuracy in predicting the desired trajectory.

	At the same time, energy savings can be obtain if the vehicle has a trajectory horizon estimation algorithm. Multiple energy systems can be synchronized to work together to achieve the best energy efficiency for the planned trajectory.

\newpage
\section{References}

\nocite{*}


\section{Introduction}
A medial representation of an object encodes its solid geometry as the union of a collection of spheres of different radii and origins; see~\Figure{definitions}.
While volumetric or surface mesh representations are more commonly used in computer graphics and computer vision, since its introduction by~\citename{Blum}{blum}, medial representations have found applications in many 2D/3D geometric problems such as animation~\cite{pinocchio}, fabrication~\cite{musialski2016fabrication}, image processing~\cite{amat}, shape analysis~\cite{shapediam}, and real-time tracking~\cite{hmodel}.
The process of converting an input model into a medial representation is generally referred to as the Medial Axis Transform (MAT), and the collection of origins of the spheres in a medial representation form a {\em medial skeleton}.

\paragraph{Stability}
A common pitfall of medial axis methods is their sensitivity to noise: in a traditional medial representation, new branches of the medial skeleton may form at all negative local curvature extrema.
If our shape is represented via a piecewise linear boundary, such as a polygonal mesh in 3D, spurious branches form when the surface exhibits even minor levels of noise; see \cite[Fig.9]{skelstar}.
These issues are typically resolved by resorting to postprocessing, in which sets of spheres are filtered out according to various engineered criteria.
We present a new algorithm for computing the medial axis transform of an oriented point set which is naturally capable of handling noise, outliers, and other artifacts that create characteristic problems for traditional methods.

\paragraph{Definitions}
As summarized in \Figure{definitions} and \cite{skelstar}, the medial axis transform may be defined in a few ways, with each definition producing equally valid and useful
representations. In practice, each definition of the medial representation leads to a different way to compute the medial axis. For example, the \emph{normal flow} variant leads to the commonly employed voxel thinning algorithms~\cite{saha2016survey}, while the \emph{equidistance} definition leads to techniques leveraging Voronoi
diagrams~\cite{brandt92}. In this paper, we build over what is likely the most well known definition of the medial axis transform:
\begin{definition}
The Medial Axis Transform $\text{MAT}(\mathcal{O})$ of $\mathcal{O}$ is the set of centers $\mathcal{M}$ and corresponding radii $\mathcal{R}$ of all maximally inscribed circles/spheres in $\mathcal{O}$.
\label{def:mat}
\end{definition}
While previous work such as \citename{Ma}{bubble} has proposed methods to construct medial axis representations using this definition, it is interesting to note that they have treated the problem from a \emph{combinatorial geometry} standpoint. Instead, we consider the medial axis transform from a \emph{numerical geometry} perspective, where the medial axis is given by the solution of an optimization problem.
We achieve this by expressing the concepts of \emph{maximality} and \emph{inscription} from Def.~\ref{def:mat} in a least squares form.
The robustness of the approach to imperfections arises from the fact that least squares optimization attempts to find an approximate, rather than exact, solution to the given problem.
We are motivated in our approach by considering a least-squares problem as a maximum likelihood estimate of a function in the presence of noise obeying a Gaussian probability distribution.

\paragraph{Method outline}
Our method takes an \emph{oriented} point cloud as input, and produces an unconnected medial point cloud as output by minimizing a combination of a maximality energy, and an inscription energy.
Our key technical challenge is in formulating these energies correctly; our maximality energy is designed to have a constant magnitude, ensuring that the optimization energy of each medial sphere is constant regardless of its radius, and our inscription energy is based around a locally supported approximation of the signed distance function of the point cloud.
In order to prevent spheres from sliding towards local maxima of local shape thickness, we introduce a pinning constraint as a quadratic barrier energy.
The resulting optimization problem is quadratic, with differentiable but non-linear energy terms, and we solve it with an iterative Gauss-Newton solver.

\paragraph{Contributions and Evaluation}
Our main contributions are a novel interpretation of a problem that is classically solved by standard computational geometry as a numerical geometry problem, and a novel algorithm for computing the medial axis transform of an oriented point set that inherently handles imperfections in the input. We present a number of qualitative results throughout the paper for both 2D and 3D oriented point clouds. Finally, we also present side-by-side quantitative evaluations of the proposed algorithm against several state-of-the-art methods.

\section{Related Work}
\label{sec:related}
Many techniques exist to compute the medial axis transformation, as detailed in a recent survey~\cite{skelstar}.
We focus our discussion around two central aspects: methods that operate on \emph{surface} 2D/3D representations (e.g. point clouds and
triangular meshes), and methods that attempt to resolve the \emph{instability} of medial axis via post processing.

\subsection{Medial axis computation of sampled surfaces}
\label{sec:voronoi}
Assuming the surface is \emph{sampled} by a sufficiently dense set of points, the Voronoi diagram can be used to compute the medial axis transform with relative ease.
For a closed boundary in 2D, any interior Voronoi vertex, as well as Voronoi edges connecting interior vertices, approximate the medial axis with a convergence guarantee~\cite{brandt92}.
Unfortunately, \citename{Amenta}{amenta99} has shown how this property does not hold in 3D due to \emph{sliver tetrahedra}.
Thankfully, as we increase the sampling rate, the Voronoi poles (see definition in \cite{amenta99}) do converge to the medial axis, allowing the use of 3D Voronoi diagrams for the task at hand.
These methods suffer two fundamental shortcomings:
\circledone{} they are \emph{global} and optimize the entire set of centers at the same time, and
\circledtwo{} the extracted axis is heavily susceptible to even minor levels of noise; see \Figure{stability}.

\paragraph{Sphere-at-a-point methods}
Leveraging an \emph{oriented} sampling, \citename{Ma}{bubble} proposed an alternative way to compute the transform by marrying the maximally inscribed definition of \Figure{definitions}(a) to the bi-tangency of \Figure{definitions}(d). Unlike Voronoi methods, which consider the entire point set at once, their method can compute a maximal sphere at a point in \emph{isolation}, leading to extremely efficient GPU implementations~\cite{jalba_pami13}.
These methods represent the \emph{most efficient} way of computing the medial axis, but, similarly to Voronoi methods, they suffer stability issues; see \Figure{bubble}.
Another method for computing a local approximation to the axis was proposed by \citename{Shapira}{shapediam} via casting rays in a cone oriented along the anti-normal.
The distance between the point and the intersection with the surface is aggregated by a robust function (e.g. median) to estimate the \emph{shape diameter function}.
While this approximation has found widespread use as a shape descriptor, the radius estimate suffer of \emph{bias}, and the algorithm does not generalize to point clouds.

\input{fig/stability/item}
\paragraph{Shape approximation methods}
\label{sec:shapeapprox}
Recently, a new class of techniques has been proposed which attempt to \emph{approximate} watertight surfaces via \emph{Sphere~Meshes} -- linearly swept spherical primitives~\cite{spheremesh, anispheremesh}.
This is achieved via local mesh decimation relying on iterative edge collapses, where \emph{spherical quadrics} are employed in place of the traditional quadric metrics~\cite{qslim}.
In many cases the produced model \emph{resembles} a medial axis.
When executed on 3D data the result can contain tetrahedra, however, the medial axis of a 3D shape is known to consist only of points, curves, and surfaces.
Addressing these concerns, the \emph{Medial~Meshes} work by \citename{Sun}{medialmesh} extended sphere~meshes to decimate a medial axis mesh, and discard unstable branches. Marrying sphere~meshes to medial~meshes, \citename{Li}{qmat} followed up this work and proposed QMAT, a more computationally efficient version based on spherical quadrics.
While these techniques, in juxtaposition to our local method, are global, they can only cope with minor levels of noise:~\emph{``with very noisy input, however, the simplified medial mesh is not a stable representation''}; see \cite[Fig.9]{medialmesh}.
The follow-up work by~\citename{Li}{qmat} performs slightly better as it can optimize for sphere centers, but our algorithm can still cope with noise that is \emph{one order of magnitude} larger; see~\cite[Fig.16]{qmat}.

\subsection{Instability and filtering techniques}
Techniques that do not attempt to produce an \emph{approximation} suffer from instability when computing the medial axis.
Filtering techniques attempt to \emph{remove} portions of the medial axis that do not contribute significantly to the geometry \emph{reconstructed} as the union of medial spheres.
As shown in \cite[Fig.2]{scaleaxis3d}, this works fairly well for inputs with little or no noise.
Conversely, with large noise (and/or outliers), these methods become mostly inappropriate; see~\Figure{stability}.

\paragraph{Angle filtering -- $\theta$-medial axis}
One way to identify the significance of a point is the largest angle formed by the center of the corresponding maximal sphere and two of
its tangent points on the shape boundary. The $\theta$-medial axis of~\cite{foskey}, filters out balls associated with low aperture angle.
While this filtering can disconnect portions of the medial axis, see \Figure{stability}(c), the issue can be avoided by
homotopy-preserving pruning~\cite{attali}.

\paragraph{Distance filtering -- $\lambda$-medial axis}
Another metric for filtering discards a sphere whenever its tangent points lie on the surface below a certain
distance~\cite{lambda,powercrust}. As \Figure{stability}(d) illustrates, this results in a loss of features even before all noise has been
removed. This shortcoming makes these solutions inappropriate whenever the input shape contains structures at different scales.

\paragraph{Scale filtering -- $\sigma$-medial axis}
The scale axis transform introduced by \citename{Giesen}{scaleaxis} and extended to 3D by \citename{Miklos}{scaleaxis3d} are methods built over the \emph{maximality} property of the medial axis.
First medial spheres are scaled by a given $\sigma>1$ value, and spheres that are no longer maximal (i.e. contained in another sphere) get discarded.
The medial axis of the scaled union of balls is then computed, and its spheres unscaled by a factor $1/\sigma$.
This method is \emph{global} as it processes the whole point set at once, and computes multiple Voronoi diagrams in the process, resulting in reduced computational efficiency -- e.g. $\approx 2~min.$ for a mesh with $\approx 100k$ vertices, see~\cite[Tab.1]{scaleaxis3d}.

\input{fig/bubble/item.tex}
\input{fig/inscription/item.tex}
\section{Technical details}
\label{sec:algorithm}
In \Section{bubble}, we start by reviewing the highly relevant method of \citename{Ma}{bubble}, which computes the medial axis by searching for maximal spheres via \emph{local} analysis.
We then formulate our numerical optimization to compute medial spheres in \Section{optcore}.

\paragraph{Notation}
We are given in input an \emph{oriented} point cloud $\{(\mathbf{p}_n,\mathbf{n}_n)\}_{n=1 \dots N}$ drawn from the solid object $\mathcal{O}$ with watertight surface/boundary $\partial\mathcal{O}$.
The cloud is affected by noise with standard deviation $\sigma_\mathbf{p}$.
We give all numerical values of $\sigma_\mathbf{p}$ and other parameters which represent distance values as \textit{percentages} relative to the diagonal of the bounding box of the input shape.
With $\mathcal{M}$ we refer to the \emph{internal} medial axis of $\mathcal{O}$; see \cite[Fig.2]{skelstar}.
With $\mathbf{s}=(\mathbf{c},r)$ we indicate a sphere centered at $\mathbf{c}$ of radius $r$, and with $t$ the index of the solver iteration.

\subsection{The sphere-shrinking algorithm -- \Figure{bubble}}
\label{sec:bubble}
The algorithm proposed by \citename{Ma}{bubble} is an iterative method that shrinks an initially large sphere until it satisfies the medial axis properties.
Assuming $\mathbf{p}$ is a point on the boundary, this process leverages two properties:
\circledone{} that the sphere passing through $\mathbf{p}$ should be empty;
and \circledtwo{} that $\mathbf{p}-\mathbf{c}$ for a smooth input curve is parallel to the contact point normal $\mathbf{n}$.
The sphere-shrinking{} algorithm is initialized with a large sphere, passing through $\mathbf{p}$, containing the entire point set, and whose center $\mathbf{c}$ lies along the ray $(\mathbf{p}, -\mathbf{n})$.
Whenever the distance to the closest point $\mathbf{f}\in\partial\mathcal{O}$ from $\mathbf{c}$ is less than $r$ (i.e. the sphere is not empty), the center $\mathbf{c}$ is updated by computing the sphere tangent to $(\mathbf{p},\mathbf{n})$ and passing through $\mathbf{f}$; see \Figure{bubble}.
Finally, the loop terminates when the sphere radius change across iterations is below numerical precision.
This algorithm is highly efficient as each sample can be processed completely independently from the others, but as it treats all points as hard constraints in a combinatorial manner it does not cope well with noisy inputs.
Our formulation borrows from this one, but generalizes it by \circledone{} reformulating it into a \emph{continuous} optimization problem, and \circledtwo{} making it more \emph{robust} to noise and outliers.

\subsection{Optimizing for maximally inscribed spheres}
\label{sec:optcore}
We define a least-squares optimization capable of generating \emph{maximally inscribed spheres} given an \emph{oriented} point cloud $\mathcal{P}$.
Our optimization energy for a sphere $\mathbf{s}$ consists of the combination of two terms:
\newcommand{\text{maximal}}{\text{maximal}}
\newcommand{1}{1}
\newcommand{\text{inscribed}}{\text{inscribed}}
\newcommand{2}{2}
\begin{equation}
E_\text{medial} =
\omega_1\: E_\text{maximal} \: + \:
\omega_2 \: E_\text{inscribed}
\label{eq:maxball}
\end{equation}
We will now proceed to describe these terms in detail, and then provide comparison against alternative formulations in \Section{eval_maximal}.

\paragraph{Maximality}
The maximality energy creates a constant positive pressure term that tends to increase the sphere size at each iteration.
By design, we define this energy to have a \emph{constant magnitude}, that is, a contribution to the optimization energy that is constant regardless of the radius of the given medial sphere.
This is achieved by considering the radius at the previous optimization iteration with a constant offset $\epsilon$:
\begin{equation}
E_\text{maximal} = || r - (r^{t-1} + \epsilon) ||_2^2
\label{eq:maximal}
\end{equation}

\input{fig/iterations/item.tex}
\paragraph{Inscription}
Consider a signed distance function (SDF) $\Phi(\mathbf{x})$, where $\Phi(\mathbf{x})<0$ for an $\mathbf{x}$ inside the shape.
If the expression of this function were available to us, an inscription constraint could be easily expressed by the inequality $\Phi(\mathbf{c})<-r$, which we can convert in our least squares formalism as:
\begin{equation}
E_\text{inscribed} = \mathcal{R}( r+\Phi(\mathbf{c}) )^2
\label{eq:inscribed_exact}
\end{equation}
where the ramp function $\mathcal{R}(x)=max(x,0)$ only penalizes a sphere when it \emph{violates} an inscription constraint.
Unfortunately, an SDF function for our oriented point set $\mathcal{P}$ is not readily available without computing a full surface reconstruction of the point cloud~\cite{reconstar}.
However, the family of moving least square (MLS) methods are capable of building locally supported approximations in a neighborhood of $\partial\mathcal{O}$ -- that is, as $\Phi \rightarrow 0$.
For example, the MLS formulation by Kolluri~\cite{kolluri_talg08} approximates $\Phi$ as a weighted sum of locally supported point-to-plane functions:
\begin{equation}
\Phi(\mathbf{x}) = \frac
{\sum_n \varphi_n(\mathbf{x}, h) \: \mathbf{n}_n \cdot (\mathbf{x}-\mathbf{p}_n)}
{\sum_n \varphi_n(\mathbf{x}, h)}
\label{eq:kolluri}
\end{equation}
where in our case $\varphi_n(\mathbf{x}, h)=\varphi(\|\mathbf{x}-\mathbf{p}_n\|_2, h)$ and $\varphi(x, h)$ is a smoothly decaying radial basis function~\cite{apss} with compact support $[0, h]$:
\begin{equation}
\varphi(x, h) = b \left( \frac{x}{h} \right);
\quad
b(x) =
\begin{cases}
(1-x^2)^4& x<1 \\
0 & x\geq1
\end{cases}
\label{eq:kernel}
\end{equation}
Note that as \Equation{inscribed_exact} evaluates the function $\Phi$ at the medial center, which could be potentially far from $\partial\mathcal{O}$, this representation is not immediately appropriate.
However, in the context of registration, several works have shown how a quadratic approximation of $\Phi^2$ can be built by appropriately blending \emph{point-to-point} with \emph{point-to-plane} distance functions~\cite{sdfgeometry,mitra2004registration}.
In more detail, point-to-point distances are a good approximation of $\Phi^2$ in the \emph{far-field} (i.e. for $\Phi^2 \gg 0$), while point-to-plane distances are more suitable in the \emph{near-field} (i.e. for $\Phi^2 \rightarrow 0$).
Let us first define $\bar\mathbf{c}_n= \mathbf{c}^{t-1} - \mathbf{n}_n(\mathbf{c}^{t-1}-\mathbf{p}_n)\cdot\mathbf{n}_n$, the projection of the center on the hyperplane of point $\mathbf{p}_n$. We can use this to interpolate between the two metrics:
\begin{align}
\Phi_\text{blend}(\mathbf{s})^2 =
\text{mix}(\Phi_\text{plane}(\mathbf{s})^2, \Phi_\text{point}(\mathbf{s})^2, \varphi_n(\bar\mathbf{c}_n, h_\text{blend}))
\end{align}
where $\Phi_\text{plane}(\mathbf{s}) = \mathcal{R}(r - (\mathbf{p}_n-\mathbf{c})\cdot \mathbf{n}_n)$ represents the distance from the sphere to a plane, $\Phi_\text{point}(\mathbf{s}) = \mathcal{R}(r-\|\mathbf{p}_n-\mathbf{c}\|_2)$ is the euclidean distance from the sphere to a point, and $\text{mix}(a, b, x) = xa + (1-x)b$ is the linear interpolation operator; see \Figure{inscription}.
Based on the geometry of $\Phi_\text{blend}^2$, we can then re-formulate our inscription with respect to each oriented point $\mathbf{p}_n$.
Analogously to \Equation{kolluri}, this energy is accumulated over all points in $\mathcal{P}$:
\begin{align}
E_\text{inscribed} \approx \sum_n
\underbrace{\varphi(\mathcal{R}(||\mathbf{c}^{t-1} - \mathbf{p}_n|| - r^{t-1}), h_\text{support})}_{\neq 0 \text{ for a subset of the N points}}
\Phi_\text{blend}(\mathbf{s})^2
\label{eq:inscribed}
\end{align}
The parameter $h_\text{blend}$ defines the scale used for blending between distance types, and $h_\text{support}$ limits how far outside of a sphere a point may be before its contribution falls to zero.
As is typical in robust optimization (e.g. IRLS) the weights are computed with respect to the parameters $\mathbf{s}^{t-1}$ at the previous iteration -- inscription is evaluated only for points within, or in proximity of the sphere in its previous geometric configuration $\mathbf{s}^{t-1}$.

\input{fig/sliding/item.tex}

\paragraph{Pinned spheres}
The medial axis provides an estimate of the \emph{local thickness} of the shape through its sphere local radius.
However, as our variational formulation attempts to create \emph{larger} spheres, nothing prevents a sphere from traveling along medial branches wherever we have a non-vanishing medial radius gradient on $\mathcal{M}$; e.g. a sphere would slide from the tip of a cone to its base; see \Figure{sliding}.
We can avoid this issue by ``pinning'' medial spheres.
Generalizing the exact constraints of the sphere-shrinking algorithm by~\cite{bubble}, we subject the sphere associated with a given point $\mathbf{p}$ to the \emph{hard} constraint $\| \mathbf{c} - \mathbf{p} \| - r \leq d_\text{pin}$, which keeps the sphere in contact with a sphere of radius $d_\text{pin}$ centered at $\mathbf{p}$.
We include this constraint in our optimization via the penalty method~\cite{penalty}, yielding a quadratic barrier energy:
\begin{equation}
E_\text{pinning} = \mathcal{R}(\| \mathbf{c} - \mathbf{p} \| - (r + d_\text{pin}))^2
\label{eq:pinning}
\end{equation}

\paragraph{Optimization}
Similarly to~\cite{lop}, our optimization induces the definition of medial sphere as the \emph{fixed point solution} of an update equation:
\begin{equation}
\mathbf{s} = \mathcal{F}(\mathbf{s}) = \argmin_{\mathbf{s}} \:\: E_\text{medial} + E_\text{pinning}
\label{eq:update}
\end{equation}
Our optimization problem is quadratic, but while its energy terms are differentiable, they are not linear. Hence, we iteratively compute a solution via Gauss-Newton. This requires the linearization of the arguments of the quadratic functions with respect to $\mathbf{c}$ and $r$, which is straightforward in our setting.

\paragraph{Implementation}
Due to the independent nature of the per-sphere optimization, our implementation is straightforward. At each step we compute Jacobian matrices and residual vectors separately for each sphere and thus are only required to solve a $DxD$ linear system for each, where $D$ is the number of degrees of freedom of a single sphere (3 or 4 in our experiments). This property eliminates the need for any advanced solvers or factorizations and enables the implementation to be completely parallel over the spheres, as required for effective GPU acceleration. Additional performance optimization could be achieved by collecting the points for each sphere whose contribution is non-zero using accelerating data structures such as kd-trees \cite{friedman1977algorithm}.

\section{Results and evaluation}
\label{sec:eval}

We show medial representations generated by our method, in both 2D and 3D, throughout the paper and in \Figure{gallery}. As shown, our method produces valid medial representations for a wide variety of shapes and models, and in the presence of noise and outliers. Throughout the paper, we process all input with the same parameters whose values were set according to the analysis in \Section{paramanalysis}.
We evaluate our method in several ways. First, we consider variations of the inscription and maximality energy, and show how alternative formulations fail. Second, we evaluate the performance of our algorithm against ground truth on synthetic benchmarks contaminated by noise and outliers. Third, we compare our generated medial representations against the state-of-the-art in both 2D and 3D, again in the presence of noise. Finally, we provide an analysis of our algorithm parameters.

\input{fig/inscrvariants/item.tex}
\subsection{Variants of inscription energy -- \Figure{inscrvariants}}
We consider two modified formulations of \Equation{inscribed} in which we either penalize the squared distance to points, or the squared distance to half-planes as opposed to our blended formulation.
We investigate this behavior by randomly initializing the algorithm, and snapping $\varphi_n(\bar\mathbf{c}_n)$ to either zero or one for all points.
When $\varphi_n(\bar\mathbf{c}_n)=1$, point-to-point energy is used and the algorithm attempts to make \emph{spheres empty} in a least square sense. However, nothing prevents the optimization from generating maximal spheres \emph{outside} the shape in the ambient space.
When $\varphi_n(\bar\mathbf{c}_n)=1$, point-to-plane energy is used, and the algorithm attempts to make \emph{half-spaces empty}.
As illustrated in \Figure{inscrvariants} and the \textbf{supplemental video}, this results in difficulties for the algorithm in dealing with sharp concavities.
In the example above, we expect all centers to cluster to one of the two centers, but spheres in the neighborhood of the sharp concavity might read the normal of a point on the opposite side, with a halfplane requesting the radius of a sphere intersecting with it to be significantly smaller.
This problem is caused by the fact that point-to-point and point-to-plane energies approximate the squared SDF of a point set respectively in the far and near field. Our LSMAT formulation respects this geometric property, and deals with both issues at once.

\input{fig/maxvariants/item.tex}
\subsection{Variants of maximality energy -- \Figure{maxvariants}}
\label{sec:eval_maximal}
We consider two alternative formulations of the maximality condition in our optimization: penalizing the squared inverse of the sphere radius, expressed as $\|1/r\|^2$, or directly specifying $R_\text{max}$, the maximum size of a medial sphere, and optimizing $\|r-R_\text{max}\|^2$.
While intuitively these are potentially feasible solutions, they incur a significant limitation: the amount of ``pressure'' a medial sphere will apply will be dependent on its size.
That is, small spheres will be associated with a higher energy level.
For example, using the first formulation, as $r \rightarrow 0$ its gradient will tend to $-\infty$; as illustrated in \Figure{maxvariants}, this can cause spheres in the neighborhood of small features to bulge out.
Our solution, detailed in \Equation{maximal}, does not encounter this problem, as our energy is \emph{constant} regardless of the sphere size.

\input{fig/iterations-metric/item.tex}

\subsection{Quantitative evaluation metric -- \Figure{iterations-metric}}
\label{sec:metric}
We consider ground truth geometry polluted by noise, and evaluate the quality of an extracted axis by computing the distance of each medial center to the closest point on the ground truth axis~$\tilde{M}$:
\begin{align}
E_\text{avg} = \tfrac{1}{N} \sum_{n} \: \argmin_{\tilde\mathbf{c}_n \in \mathcal{\tilde{M}}} \| \mathbf{c}_n - \tilde\mathbf{c}_n \|_2
\\
E_\text{max} = \max_n \: \argmin_{\tilde\mathbf{c}_n \in \mathcal{\tilde{M}}} \| \mathbf{c}_n - \tilde\mathbf{c}_n \|_2
\end{align}
We compute $\tilde{M}$ via the medial axis module of the python \emph{skimage} package, derived from the ridges of the distance transform from the ground truth boundary $\partial\mathcal{O}$.
As we discretize images with a bounding box at a $1024^2$ resolution, the ``numerical precision'' of the metrics above is of one pixel.
To obtain scale invariance, we report these errors in relation to the diagonal of the bounding box.
In \Figure{iterations-metric}, we visualize the convergence of our iterative optimization scheme.

\input{fig/star2d/item.tex}
\subsection{State-of-the-art comparisons in 2D -- \Figure{star2d}}
Through the metric from \Section{metric}, we quantitatively evaluate the performance of LSMAT against local filtering methods, as well as the global scale axis transform.
\Figure{stability} shows how neither distance nor angle filtering is very effective; hence we employ a compound variant where we first filter by distance with $\lambda>\sigma_p$, and then by angle $\theta>110^\circ$.
For the scale axis, we set $\sigma=1.3$, and for our method, we use our default parameters.
We gradually increment the level of noise, and plot the corresponding error metric in~\Figure{star2d}, as well as a few example frames from the plot. Note that LSMAT correctly captures the shape of the ground truth boundary, whereas all other local methods fail.

\input{fig/star3d/item.tex}

\input{fig/qualparsweep/item.tex}
\subsection{State-of-the-art Comparisons in 3D -- \Figure{star3d}}
We qualitatively evaluate the performance of LSMAT in three dimensions by comparing our results to those generated by the SAT~\cite{scaleaxis3d}, QMAT~\cite{qmat}, and sphere-shrinking{}~\cite{bubble} methods.
To compare with methods that expect a mesh in input, we finely re-triangulate the surface, and apply normal displacement perturbation with $\sigma_\mathbf{p} \in [0\%, 2\%]$ relative to the diagonal bounding box.
On the \emph{fertility} model, LSMAT produces a convincing medial axis representation even when the input oriented point cloud is affected by extreme noise -- i.e. with a magnitude close to the one of the \emph{local feature size}.
For the \emph{vase} model, LSMAT still produces a smoother and more faithful medial axis representation than the existing state-of-the-art.
Even in the presence of minor amounts of noise (second row), LSMAT faithfully produces a medial axis representation with a smooth surface, and whose skeleton resembles that of the uncorrupted model.
The results are particularly encouraging on the horse dataset, where we attempted to use a very small number of target primitives for QMAT ($\approx 500$) and a large value of scale for SAT ($\approx 1.3$).
Our formulation is the only one capable of computing a relatively noiseless arrangement of medial spheres. The timings for our LSMAT and sphere shrinking results can be found in \Table{timings}. All experiments were run on a machine with an Intel Xeon E5-1650 CPU and an Nvidia GTX 1080 GPU.

\input{fig/timings/item.tex}

\subsection{Algorithm Parameter Analysis -- \Figure{qualparsweep}}
\label{sec:paramanalysis}
Our algorithm depends on five parameters: the kernel sizes $h_\text{blend}$ and $h_\text{support}$, the relative weight $\omega_1/\omega_2$, pinning distance $d_\text{pin}$, and radius expansion constant $\epsilon$. The effect of varying the first four given different levels of noise is shown in \Figure{qualparsweep}.
Note we only consider the ratio between the $\omega_*$, as the two energies balance each other.
As $h_\text{blend}$ increases, spheres near sharp concave corners begin to shrink as they eventually use the point-to-plane distance for all points in their neighborhood.
When $h_\text{support}$ grows significantly beyond the local feature size, the MLS formulation is no longer able to recover a meaningful surface.
As $\omega_1/\omega_2$ increases and the relative importance of maximality versus inscription increases, spheres expand until they no longer form a faithful medial representation and ultimately escape the point cloud.
Finally, as $d_\text{pin}$ grows, the spheres are allowed to slide further from their starting positions towards areas of locally maximal radius.
We assume that estimates of the input noise characteristics are available, and choose our default value for each of these four parameters based on empirically derived linear functions of $\sigma_\mathbf{p}$; see Appendix \ref{paramdefaults} for details.
Through experiments we observed that it is sufficient to choose a constant value for the fifth parameter $\epsilon$, as different values of it merely change the optimal relation between $\omega_1/\omega_2$ and $\sigma_\mathbf{p}$. For all our experiments we use $\epsilon = 100\%$.
We believe improved formulations of LSMAT could eventually coalesce some of these parameters hence improving the ease of use of our algorithm.

\section{Future works}
\input{fig/outliers/item.tex}
Expressing the Medial Axis Transform as a non-linear least squares problem opens up several interesting avenues for future research.

\paragraph{Robustness to outliers -- \Figure{outliers}}
Least squares problems can be interpreted as a maximum-likelihood (ML) estimation given a \emph{Gaussian} probability distribution of the noise variables.
If the input is corrupted by other forms of noise, one could replace \emph{Gaussian} with other error distributions, and derive the corresponding ML optimization scheme.
For example, if we assume the probability distribution of the noise to be Laplacian, the least-squares problems would simply be transformed into an $\ell^1$ (i.e. least norm) optimization.
However, these type of problems can still be computed with Gauss-Newton type methods by using iteratively re-weighted least squares~(IRLS) techniques~\cite{irls}.
As illustrated in \Figure{outliers} this results in an IR-LSMAT algorithm that can cope with significant amounts of outliers.
While these results are promising, the convergence speed of the optimization is severely reduced, as a much smaller value for $\varepsilon$ was needed to produce these results.
The generalizability of LSMAT to $\ell^p$ robust norms~\cite{sparseicp} is particularly interesting. More specifically, consider the optimization problem consisting only of the following energy:
\begin{equation}
\argmin_{\mathbf{c},r} \:\: \sum_n |\|\mathbf{c}-\mathbf{p}_n\| -r |^{p}.
\label{eq:sparsemat}
\end{equation}
For $p=2$ this simplified version of the sphere-fitting problem is convex, hence generating a single solution regardless of initialization.
However, as $p \rightarrow 0$ the problem is non-convex and the local minima reached by optimization depends on the initialization.
Our preliminary investigation revealed how these local minima correspond to spheres belonging to the \emph{symmetry set}, a superset of the medial axis; see \Figure{definitions}(d) and \cite[Sec.~2.1.4]{skelstar}.
How to exploit \eq{sparsemat} to efficiently compute the MAT of a point set is an interesting venue for future works.

\input{fig/smoothing/item.tex}
\paragraph{Smoothness priors -- \Figure{smoothing}}
\label{sec:postprocess}
Our pinning formulation is highly efficient, but its local nature can be also considered a intrinsic limitation.
For example, in the noisy \emph{maple leaf} example in \Figure{gallery}, at times the estimated medial spheres could be over and/or under-estimated in size, resulting in medial centers that do not necessarily sample the underlying piecewise-smooth manifold of the MAT.
However, if our input is a sampling of a smooth surface $\partial\mathcal{O}$, then we know that~\cite{matbook}: \circledone{} the medial centers $\mathbf{c}_*$ should be lying on a piecewise smooth manifold, and \circledtwo{} the sphere radius function on this manifold should vary smoothly.
Another desirable characteristic might be to have a uniform sampling of this manifold.
We can convert these priors into least-squares energies, resulting in a \emph{maximum a-posteriori} optimization.
In \Figure{smoothing} we illustrate a few iterations of this optimization on the \emph{moon} shape, where the ``pinning'' constraints from \Section{optcore} have been disabled.
While the optimization behaves as expected, this suffers similar shortcomings to those illustrated in \Figure{sliding}, and would result in a single sphere if executed for $t \rightarrow \infty$.
Modifying the variational LSMAT formulation to obtain a regularly sampled distribution of medial centers is an interesting venue for future works.

\paragraph{Optimization acceleration}
In our experiments, we initialize the optimization with random sphere positions and radii.
Nonetheless, given how a smooth object is composed of smooth piecewise manifolds $\mathcal{M}$, and a smooth radius function $\mathcal{R}$ defined thereon, one could easily envision a locally bootstrapped version of the algorithm, where unsolved medial balls are initialized with the $\mathbf{c}_*,r_*$ of their neighbors -- which could also be re-interpreted as a \emph{multi-scale} solver.
Notice that techniques such as the sphere-shrinking{} algorithm from \cite{bubble} do not permit this type of acceleration.

\paragraph{Shape approximation vs. reconstruction}
A number of methods leverage the medial axis for \emph{interpolatory} reconstruction~\cite{ireconstar}, and our work is a stepping stone towards the creation of \emph{approximating} reconstruction~\cite{reconstar} algorithms based on the medial axis.
Methods such as Medial~Meshes~\cite{medialmesh} and QMAT~\cite{qmat} assume a reconstruction of the watertight surface is \emph{already} available, and attempt to extract its \emph{approximation} via swept-spheres.
Conversely, our work could be extended to directly compute a \emph{reconstruction} of the input point cloud by minimizing data fitting metrics based on Hausdorff distances~\cite{medialmesh} or spherical quadrics~\cite{qmat}.
A ``topology surgery'' step could then be interweaved with our optimization to stitch medial spheres together and create a medial mesh with connectivity information.

\paragraph{Orienting a point set}
Finally, while our formulation is based on an oriented point set and an approximation of its SDF in the near/far field, an interesting variant of our algorithm could consider an unoriented point set, where the quantities to be optimized for would be the radii $[r_1, r_2]$, and contact plane $[\mathbf{k},\mathbf{n}]$ of a pair of \emph{twin spheres}.
The contact point $\mathbf{t}$ should then be then be optimized on the manifold $\partial\mathcal{O}$, while the complementary outside/inside label of each sphere be optimized to result in a \emph{smooth signing} of the environment space.
This approach would then provide a ``medial axis'' analogous to recent efforts in variational reconstruction of non-oriented point clouds~\cite{signing}.

\section{Conclusions}
\label{sec:conclusions}
We have introduced the Least Squares Medial Axis Transform, or LSMAT, a continuous relaxation of the medial axis transform that is not only stable, but also robust to high levels of noise even though it is based solely on \emph{local} optimization.
While in most of the paper we visualized the generated maximal spheres covering more or less the entire shape, we would like to remind the reader that the algorithm operates on each sphere \emph{independently}; the algorithm is therefore trivially parallelizable and particularly suitable for GPU implementations.
Our method produces results on noisy inputs that state-of-the-art methods fail to handle, without a reliance on ad-hoc postprocessing.
Our approach is efficient, parallelizable, and therefore suitable for real time applications where reliable medial representations are required, and where captured inputs are likely to be noisy.

\section*{Acknowledgements}
We would like to express our gratitude to Mathieu Desbrun, Leonidas Guibas, Justin Solomon, Keenan Crane, and especially Sofien Bouaziz for their ideas and suggestions.
In our experiments we use models provided courtesy of \cite{chen2009benchmark} and the AIM@SHAPE Shape Repository.

\bibliographystyle{eg-alpha}