\section{Supplied mass-balance schemes}
\subsection{Overview}
The user is, of course, free to supply their own mass-balance model for use
with GLIDE. However, GLIMMER includes within it a annual positive-degree-day model
for mass balance, shortly to be augmented by a similar daily model and an
hourly energy balance model. This section gives details of how to configure
and call these models.
\subsection{Annual PDD scheme}
\label{ug.mbal.pdd_scheme}
The annual PDD scheme is contained in the f90 module \texttt{glimmer\_pdd},
and the model parameters are contained in the derived type
\texttt{glimmer\_pdd\_params}. Configuration data is contained in a standard
GLIMMER config file, which needs to be read from file before initialising the
mass-balance model. The model is initialised by calling the subroutine
\texttt{glimmer\_pdd\_init}, and the mass-balance may be calculated annually
by calling \texttt{glimmer\_pdd\_mbal}.

\subsubsection{Example of use:}
\begin{verbatim}
  use glimmer_pdd
  use glimmer_config

  ...

  type(glimmer_pdd_params) :: pdd_scheme
  type(ConfigSection),pointer :: config

  ...

  call glimmer_pdd_init(pdd_scheme,config)

  ...

  call glimmer_pdd_mbal(pdd_scheme,artm,arng,prcp,ablt,acab)
\end{verbatim}
In the subroutine call to \texttt{glimmer\_pdd\_mbal}, apart from the
parameter variable \texttt{pdd\_scheme}, there are three input fields
(\texttt{artm}, \texttt{arng} and \texttt{prcp}), which are, respectively, the
annual mean air temperature, annual temperature half-range, and annual
accumulated precipitation fields. The final two arguments are output fields
--- annual ablation (\texttt{ablt}) and annual mass-balance
(\texttt{acab}). All arrays are of type \texttt{real(sp)}. Temperatures are
degrees Celcius, and precipitation, ablation and mass-balance are measured in
m of water equivalent.
%
%
%
\subsubsection{Day-degree calculation}
%
The greater part of the information held in the \texttt{glimmer\_pdd\_params}
derived type comprises a look-up table (the \emph{PDD table}). The model is
implemented this way for computational efficiency.

The table has two dimensions: mean annual air temperature ($T_a$)
(as the second index) and annual air temperature half range (i.e.,
from July's mean to the annual mean $\Delta T_a$) (as the first
index).  Following \emph{Huybrechts and others} [1991], daily air
temperatures ($T_a^\prime$) are assumed to follow a sinusoidal
cycle
\begin{equation}
    T_a^\prime = T_a + \Delta T_a \cos \left( \frac{2 \pi t}{A}
    \right) + \textbf{R}(0,\sigma)
\end{equation}
where $A$ is the period of a year and $R$ is a random fluctuation
drawn from a normal distribution with mean 0 $^\circ$C and
standard deviation $\sigma$ $^\circ$C. \emph{Huybrechts and
others} [1991] indicate that the number of positive degree days
(D, $^\circ$C days) for this temperature series can be evaluated
as
\begin{equation}\label{pdd}
    D = \frac{1}{\sigma \sqrt{2 \pi}}
    \int\limits_0^A
    \int\limits_0^{T_a^\prime+2.5\sigma}
    T_a \times \exp \left( \frac{-(T_a-T_a^\prime)^2}{2 \sigma^2} \right) dT
    dt
\end{equation}
where $t$ is time.  The table is completed by evaluating this integral using a
public-domain algorithm (Romberg integration), by {\it Bauer} [1961]. The
inner and outer integrals are coded as two subroutines
(\texttt{inner\_integral} and \texttt{pdd\_integrand}), which call the Romburg
integration recursively.

The main parameter needed is the assumed standard deviation of
daily air temperatures, which can be set in the configuration file (the
default is 5 $^\circ $C).

The positive-degree days are then looked up in the table (as a function of
$T_a$ and $\Delta T_a$). We take care to check that this look up is in done
within the bounds of the table.  The final value of $P$ is determined using
bi-linear interpolation given the four nearest entries in the table to the
actual values of $T_a$ and $\Delta T_a$.

The remainder of the loop completes the calculation of the
ablation and accumulation given this value for $P$.
%
\subsubsection{Mass balance calculation}
%
We use the following symbols: $a$ is total annual ablation; $a_s$
is potential snow ablation; $b_0$ is the capacity of the snowpack
to hold meltwater by refreezing; the total number of positive
degree days ($D$); day-degree factors for snow and ice ($f_s$ and
$f_i$); and the fraction of snowfall that can be held in the
snowpack as refrozen meltwater ($W_max$). Note that the
day-degree factors have been converted from ice to water
equivalents using the ratio of densities.

 First,
determine the depth of superimposed ice ($b_0$) that would have to
be formed before runoff (mass loss) occurs as a constant fraction
($W_{max}$) of precipitation ($P$)
\begin{equation}
    b_0=W_{max} P.
\end{equation}
Now determine the amount of snow melt by applying a constant
day-degree factor for snow to the number of positive day-degrees
\begin{equation}
    a_s=f_s D.
\end{equation}
We now compare the potential amount of snow ablation with the
ability of the snow layer to absorb the melt.  Three cases are
possible. First, all snow melt is held within the snowpack and no
runoff occurs ($a=0$).  Second, the ability of the snowpack to
hold meltwater is exceeded but the potential snow ablation is
still less than the total amount of precipitation so that
$a=a_s-b_0$. Finally, the potential snow melt is greater than the
precipitation (amount of snow available), so that ice melt ($a_i$)
has to be considered as well.  The total ablation is therefore the
sum of snow melt (total precipitation minus meltwater held in
refreezing) and ice melt (deduct from total number of degree days,
the number of degree days needed to melt all snowfall and convert
to ice melt)
\begin{equation}
    a=a_s + a_i = P - b_0 + f_i \left( D-\frac{P}{f_s} \right).
\end{equation}
We now have a total annual ablation, and can find total net mass
balance as the difference between the total annual precipitation
and the total annual ablation.

Note that this methodology is fairly standard and stems from a
series of Greenland papers by Huybrechts, Letreguilly and Reeh in
the early 1990s.
%
%
%
\subsubsection{Configuration}
The annual PDD scheme is configured using a single section in the
configuration file:
\begin{center}
  \tablefirsthead{%
    \hline
  }
  \tablehead{%
    \hline
    \multicolumn{2}{|p{0.98\textwidth}|}{\emph{\small continued from previous page}}\\
    \hline
  }
  \tabletail{%
    \hline
    \multicolumn{2}{|r|}{\emph{\small continued on next page}}\\
    \hline}
  \tablelasttail{\hline}
  \begin{supertabular}{|l|p{11cm}|}
%%%%
    \hline
    \multicolumn{2}{|l|}{\texttt{[GLIMMER annual pdd]}}\\
    \hline
    \multicolumn{2}{|p{0.98\textwidth}|}{Specifies parameters for the PDD
    table and mass-balance calculation}\\
    \hline
    \texttt{dx} & Table spacing in the $x$-direction ($^{\circ}$C) (default=1.0)\\
    \texttt{dy} & Table spacing in the $y$-direction ($^{\circ}$C) (default=1.0)\\
    \texttt{ix} & Lower bound of $x$-axis ($^{\circ}$C) (default=0.0)\\
    \texttt{iy} & Lower bound of $y$-axis ($^{\circ}$C) (default=-50.0)\\
    \texttt{nx} & Number of values in x-direction (default=31)\\
    \texttt{ny} & Number of values in x-direction (default=71)\\
    \texttt{wmax} & Fraction of melted snow that refreezes (default=0.6) \\
    \texttt{pddfac\_ice} & PDD factor for ice (m day$^{-1}\,^{\circ}$C$^{-1}$)
    (default=0.008)\\
    \texttt{pddfac\_snow} & PDD factor for snow (m day$^{-1}\,^{\circ}$C$^{-1}$) (default=0.003)\\
  \end{supertabular}
\end{center}
\subsubsection{References}

\noindent Bauer (1961) \emph{Comm. ACM} \textbf{4}, 255.

\noindent Huybrechts, Letreguilly and Reeh (1991) \emph{Palaeogeography,
Palaeoclimatology, Palaeoecology (Global and Planetary Change)}
\textbf{89}, 399-412.

\noindent Letreguilly, Reeh and  Huybrechts (1991) \emph{Palaeogeography,
Palaeoclimatology, Palaeoecology (Global and Planetary Change)}
\textbf{90}, 385-394.

\noindent Letreguilly, Huybrechts  and  Reeh (1991) \emph{Journal of
Glaciology} \textbf{37}, 149-157.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Daily PDD scheme
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
\subsection{Daily PDD scheme}
\label{ug.mbal.daily_pdd_scheme}
The other PDD scheme supplied with GLIMMER is a daily scheme. This is simpler than the annual scheme in that it does not incorporate any stochastic variations. The mass-balance is calculated on a daily basis, given the daily mean temperature and half-range, and assuming a sinusoidal diurnal cycle. Consequently, the firn model is more sophisticated than with the annual scheme, and includes a snow-densification parameterization.
%
\subsubsection{Configuration}
The daily PDD scheme is configured using a single section in the configuration file:
\begin{center}
  \tablefirsthead{%
    \hline
  }
  \tablehead{%
    \hline
    \multicolumn{2}{|p{0.98\textwidth}|}{\emph{\small continued from previous page}}\\
    \hline
  }
  \tabletail{%
    \hline
    \multicolumn{2}{|r|}{\emph{\small continued on next page}}\\
    \hline}
  \tablelasttail{\hline}
  \begin{supertabular}{|l|p{11cm}|}
%%%%
    \hline
    \multicolumn{2}{|l|}{\texttt{[GLIMMER daily pdd]}}\\
    \hline
    \multicolumn{2}{|p{0.98\textwidth}|}{Specifies parameters for the PDD
    table and mass-balance calculation}\\
    \hline
    \texttt{wmax} & Fraction of melted snow that refreezes (default=0.6) \\
    \texttt{pddfac\_ice} & PDD factor for ice (m day$^{-1}\,^{\circ}$C$^{-1}$)
    (default=0.008)\\
    \texttt{pddfac\_snow} & PDD factor for snow (m day$^{-1}\,^{\circ}$C$^{-1}$) (default=0.003)\\
    \texttt{rain\_threshold} & Temperature above which precipitation is held to be rain ($^{\circ}$C) (default=1.0)\\
    \texttt{whichrain} & {\raggedright
      Which method to use to partition precipitation into rain and snow:\\
      \begin{tabular}{lp{7cm}}
        {\bf 1} & Use sinusoidal diurnal temperature variation \\
        2 & Use mean temperature only\\
      \end{tabular}}\\
    \texttt{tau0} & Snow densification timescale (s) (default=10 years)\\
    \texttt{constC} & Snow density profile factor C (m$^{-1}$) (default=0.0165)\\
    \texttt{firnbound} & Ice-firn boundary as fraction of density of ice (default=0.872)\\
    \texttt{snowdensity} & Density of fresh snow ($\mathrm{kg}\,\mathrm{m}^{-3}$) (default=300.0)\\
  \end{supertabular}
\end{center}


%%
%%  chapter05.tex - Obstacle Detection and Planning for Autonomous Vehicles based on Computer Vision Techniques
%%
%%  Copyright 2014 Néstor Morales <nestor@isaatc.ull.es>
%%
%%  This work is licensed under a Creative Commons Attribution 4.0 International License.
%%

\graphicspath{{./images/chapter05/bmps/}{./images/chapter05/vects/}{./images/chapter05/}}

\chapter{Particle Filter Based Object Tracking}\label{ch:chapter05}

In the previous chapter, we saw a method able to simplify the world reconstructed from a stereo pair, in an attempt to save computational time. However, this implies loosing information, which means that there could be certain situations in which we need more information than that provided by stixels. Furthermore, we can not always ensure that we will find a situation in which the assumptions are valid (for example, the supposition of having a planar road). For that reason, we also explored some strategies in which a full reconstruction is used.

In this chapter, we will describe a method able to detect obstacles from a point cloud generated from a pair of moving stereo cameras, and model them as a set of voxels. From these voxels, we predict the direction in which objects are moving to. We define a voxel as the minimal volume in the three-dimensional space, from which we will build the detected objects. The method is inspired in the work of \cite{danescu2012particle}, since we take their idea of using a particle filter over an occupancy grid in order to detect the obstacles and their directions. However, in their method they use a two-dimensional grid which is then converted to polar coordinates, with the inconvenient that just the top-view information of the input clouds is used for the segmentation of the scene. As said in section \ref{ch:chapter00_02_05} of the \emph{Introduction} chapter, this approach falls inside the so named 2.5D methods. The main disadvantages of this kind of methods is that they consider all the obstacles as if they were lying on the ground, and are represented as a convex cuboid that doesn't take into account the complexities of certain obstacles.

As commented in previous sections, Verdino is thought to work in crowded areas, like pedestrian streets or touristic complexes, in which an overestimation of the size of the obstacles can lead to an inefficient behavior. So we decided to extend the original method of \cite{danescu2012particle} in order to make it fully three-dimensional, by using a voxelized grid instead of the original cartesian/polar grid, and including some improvements, most of them with the aim of reducing the number of fake positives.

An implementation of the method described in this chapter is available at \url{https://github.com/nestormh/PolarGridTracking}.

\section{Method}\label{ch:chapter05_01}

In general terms, the method starts with the generation of a voxelized occupancy grid in which the world surrounding the vehicle is divided into a discrete number of voxels of the same size. For each voxel, an occupancy probability is assigned, based on the number of points inside it and its neighborhood. Also, during the process, a set of particles will be assigned dynamically to each voxel during the execution, based on a 3D generalization of the weighting and resampling mechanism described in \cite{isard1998condensation}. These particles will have a double function:
\begin{enumerate}
 \item Denoting hypotheses (as happens with classical particle filters).
 \item Being used as the building blocks of the world model.
\end{enumerate}

At each frame, the set of particles obtained in the previous frame (each of these with a certain pose ${(x, y, z)}$ and speed ${(vx, vy, vz)}$) will be evolved using their movement model and assigned to the corresponding voxel, attending to the time passed between frames and the ego-motion. Then, particles are re-weighted and resampled.

Particles that survived to the resampling process are used to construct the objects that model the environment, by joining all these voxels that share a similar orientation and speed. This object reconstruction is done by using a flood fill approach, following a similar process to that described in \cite{broggi2013}, but using the vectors of each voxel, instead of color information, as done in their work.

This particle-based approach inherits some advantages from the previous work by \cite{danescu2012particle}:
\begin{itemize}
 \item \emph{We do not need to estimate the probability distribution of the speed or the orientation.} These distributions, which in the past have been approximated as histograms (\cite{chen2006dynamic}), Gaussian mixtures (\cite{gindele2009bayesian}) or higher dimensions (\cite{coue2006bayesian}), are not required anymore due to the use the particles. This distribution is obtained directly from the surviving particles of each voxel.
 \item \emph{Easy encoding of the past and present knowledge from sensor data.} The usage of a voxel grid makes this easier, and allows updating it dynamically when new information is available with little computational cost.
\end{itemize}

Also, we added some new advantages in our approach:
\begin{itemize}
 \item \emph{Hierarchical object pose, orientation and speed detection.} Object units are detected at lower level as individual voxels, for which we know their individual speed and pose. Joining all these voxels together, we obtain the whole obstacle, whose pose and speed is directly dependent on the voxels which compose it.
 \item \emph{Fully 3D perception of the object detected.} No previous assumption on the shape of the obstacles is taken, providing an accurate estimation of complex obstacles. The usage of voxels instead of cuboids allows having a more clear idea of the actual shape of an obstacle, avoiding an overestimation, which is not acceptable for some applications. By using the whole dimensions, we can have an accurate idea of the 3D boundaries of an obstacle.
 \item \emph{Simplification of the world.} Despite it seems incompatible with the previous point, in the same way that we simplified the world through the use of stixels in the previous chapter, in this chapter this simplification is done through the use of voxels. This does not prevent us from computing the whole point cloud from the stereo pair, but we can save a lot of time by using its voxel-based simplification. In the other hand, there is still information loss, but we can determine the quantity of information we are willing to loose by tuning the voxels resolution.
 \item \emph{No color information is used.} As we do not use color information coming from the input 3D point cloud, our implementation is ready to accept data from other sensors different from a stereo pair (for example, a \ac{LIDAR} device) without modifications.
 \item \emph{Collaborative update of the grid.} The usage of a voxel grid also allows combining several input sources at the same time. Due to some decisions taken during the implementation of the method, it is ready for such collaborative update.
\end{itemize}

Method pipeline is based on six different steps, depicted in figure \ref{fig:cp05_pipeline_general}:

\begin{figure}[h!]
  \centering
  \includegraphics[height=\textwidth, width=\textwidth]{pipeline_general}
  \caption{Pipeline of the different steps of the method.}
  \label{fig:cp05_pipeline_general}
\end{figure}

\begin{enumerate}
 \item \emph{3D point cloud generation.} Our implementation is able to work from data coming from any kind of sensor. Anyway, in our tests we have used point clouds obtained from stereo cameras. In this step we obtain the 3D point set that will be the input for our algorithm. At implementation level, this step is done in a separate process, so the point cloud can be used for other tasks in the system, or even be computed in a different computer or device in the future. More details are given at section \ref{ch:chapter05_01_01}.
 \item \emph{Ego-motion.} As the car is moving, the orientation and speeds computed for the different objects is biased by the speed of the own vehicle. To compensate the ego-motion, we need to compute our own movement. In section \ref{ch:chapter05_01_02}, this process is better explained.
 \item \emph{Voxelization.} Each of the 3D points obtained in the first step is assigned to a certain voxel. Each voxel has a certain resolution, which covers a parameterized section of the world. In our tests, we have used voxels of a resolution of $(0.25\times0.25\times0.25\,m)$ in the $X$, $Y$ and $Z$ dimensions, respectively, covering a volume going from $-4$\,m to $+4$\,m in the $X$ axis; $0$ to $+24$\,m in the $Y$ axis; and $0$ to $3.5$\,m in the $Z$ axis. This process, and how probabilities are assigned to each voxel is explained at section \ref{ch:chapter05_01_03}.
 \item \emph{Voxel pose and speed computation.} Based on a \ac{PF} and in the probabilities assigned in the previous step, we calculate the pose and speed of each of the voxels in the grid, as explained in section \ref{ch:chapter05_01_04}.
 \item \emph{Object reconstruction.} Once we know the pose and speed of each voxel, and based on a flood fill procedure inspired in the work of \cite{broggi2013}, together with the vectors already obtained, similar voxels are assigned to a common final obstacle. For each segmented object, orientation and speed is computed based on the vectors belonging to each of the associated voxels (See section \ref{ch:chapter05_01_05}).
%  \item \emph{Planning and obstacle avoidance.} Once we know the exact position of the vehicles, and their future movement, we can integrate the results into the rest of the system and use it for the calculation of safe and smooth paths. This process will be described in more depth in the following chapters, in sections \ref{ch:chapter05_01_06}.
\end{enumerate}

In the following sections, all these steps will be explained in a bigger detail.

\subsection{3D point cloud generation}\label{ch:chapter05_01_01}

As we have seen, in this stage a pair of calibrated images is received and transformed into a disparity map, that will allow us to get the corresponding 3D point cloud. Using the methods described in chapter \ref{ch:chapter03}, we evaluated a set of algorithms, including some pre- and post-processing filters, in order to know which of these is the most suitable for outdoor autonomous navigation applications. From this evaluation, we concluded that best results were given by the configuration we named as \emph{Census-SGM Conf 2} \citep{Hirschmuller2005}. Unfortunately, we had not access to the optimized version of this configuration at the moment of developing the method explained in this chapter. In the other hand, in the same comparison, we saw that both \emph{BT-SGM} and \emph{ELAS} algorithms had also a good response, and we had an implementation of both of them. The question is: what is the most suitable algorithm for our approach?

In section \ref{ch:chapter05_02_01}, a comparison between both \emph{BT-SGM} and \emph{ELAS} algorithms is performed. After this evaluation, we concluded that for the method presented in this chapter, ELAS is the most suitable. If we attend to the average error shown in the \ac{LGT} tests in section \ref{ch:chapter03_04}, the values obtained are reasonable; but specially, its performance is quite better, as shown in the chart at figure \ref{fig:cp05_comparison_bt_sgm_vs_elas}.

\begin{figure*}[h!]
        \centering
        \includegraphics[width=\textwidth]{intervals}
        \caption{Representation of how the voxel grid is computed. Also $\{X, Y, Z\}$, $\{DX, DY, DZ\}$, $\{W, L, H\}$ and $\{(I_{min}(d), I_{max}(d)) ~|~ d \in \{X, Y, Z\}\}$ are represented, for the sake of clarity.}\label{fig:cp05_intervals}
\end{figure*}

In figure \ref{fig:cp05_intervals}, we can observe the relation between the point cloud generated and the coordinates frame located in the center of the left camera. In this coordinate system, $X$ axis is pointing to the right, $Y$ is the depth of the scene, and $Z$ the height.

% \begin{figure*}[t]
%         \centering
%         \begin{subfigure}[b]{0.45\textwidth}
%                 \centering
%                 \caption{Full Point Cloud}
%                 \includegraphics[width=\textwidth,trim=0 50 0 0,clip]{fullPointCloud}\label{fig:cp05_full_pointcloud}
%         \end{subfigure}%
%         ~
%         \begin{subfigure}[b]{0.45\textwidth}
%                 \centering
%                 \caption{Filtered Point Cloud}
%                 \includegraphics[width=\textwidth,trim=0 50 0 0,clip]{filteredPointCloud}\label{fig:cp05_filtered_pointcloud}
%         \end{subfigure}%
%         \caption{Comparison of a point cloud obtained using \emph{ELAS}, before and after the filtering.}\label{fig:cp05_full_filtered_pointcloud}
% \end{figure*}

\subsubsection{Point cloud filtering}\label{ch:chapter05_01_01_01}

Once we have obtained the point cloud, we segment the salient volumes. We remove the points below a plane defined by $XY$, as well as those points that are too far from the origin centered at the left camera to fall inside the voxel grid. This distance, and the resolution of each voxel, is parameterized for each dimension, and determines the size of the grid, so

\begin{equation}\label{eq:cp05_filter_limits}
\begin{cases}
W = (I_{max}(X) - I_{min}(X)) / {DX} \\
L = (I_{max}(Y) - I_{min}(Y)) / {DY} \\
H = (I_{max}(Z) - I_{min}(Z)) / {DZ}
\end{cases}
\end{equation}

Here, $W$, $L$ and $H$ are the width, length and height of the voxel grid. $DX$, $DY$ and $DZ$, are the parameterized resolutions of each voxel at dimensions $X$, $Y$ and $Z$, respectively. $I_{min}(d)$ and $I_{max}(d)$ represent the minimal and maximal values of the interval taken in consideration at each dimension $d$. As depicted in the diagram at figure \ref{fig:cp05_intervals}, the left camera is in the center of the grid in the $X$ and $Z$ dimensions, and at the minimal depth in the dimension $Y$.

At the end of this process, we will have the filtered point cloud $\mathcal{P}$, which will be the input for the 3D reconstruction stage.

\subsubsection{Using other sensors}\label{ch:chapter05_01_01_02}

As already said, although we have oriented this work to the usage of a stereo camera as input device, the point cloud generation and filter steps have been implemented as separate processes and, as we will see later, no color information is used. So other sources providing a 3D point cloud stream are acceptable for our algorithm. Also several sensors can provide data simultaneously.

\subsection{Ego-motion}\label{ch:chapter05_01_02}

By using a point cloud referred to the left camera (or a given sensor), speeds and orientations are biased by the movement of the vehicle between frames. This movement must be compensated, so we need to get the ego-motion. This can be done, in one hand, using the localization method used by Verdino (\cite{Perea2013mcl}), based on the information obtained from an odometric sensor, which combined with the \acs{GPS} signal and an \ac{IMU} device, gives a precise localization. Other way to do this is by using a visual odometry system, like that proposed by \cite{geiger2011stereoscan}. In section \ref{ch:chapter05_02_02}, a comparison between a sensor-based odometric system and a vision-based odometry system is performed, concluding that both approaches can be indistinctly used without affecting to the results of our method. As some datasets available in the network do not include this odometry information, we have had to use the visual odometry method in our evaluation tests.

In figure \ref{fig:cp05_tfs}, an historic of the positions of the vehicle in previous frames is shown.

\begin{figure}[t]
  \centering
  \includegraphics{tfs}
  \caption{Historic of the positions of the vehicle in previous frames.}\label{fig:cp05_tfs}
\end{figure}

\subsection{Voxelization}\label{ch:chapter05_01_03}

This is the first stage of the object reconstruction process. As said, we have implemented our application using four different and independent processes, able to run in separate threads, making it more modular. Three of them have been already described in the previous sections (See \ref{ch:chapter05_01_01}, 3D point cloud generation and Point cloud filtering; and \ref{ch:chapter05_01_02}, Ego-motion). This is the fourth.

As input, we receive the odometry information obtained from the process described in \ref{ch:chapter05_01_02}, and the point cloud $\mathcal{P}$ obtained in section \ref{ch:chapter05_01_01_01} (See figure \ref{fig:cp05_pipeline_general}). This point cloud is processed, assigning each point $p_i = (p_x, p_y, p_z), i=1..N_p$ to the corresponding voxel $g_j=(g_w, g_l, g_h), j=1..N_g$, through the expression:

\begin{equation}\label{eq:cp05_point_to_voxel}
\begin{cases}
g_w = (p_x - I_{min}(X)) / DX\\
g_l = (p_y - I_{min}(Y)) / DY\\
g_h = (p_z - I_{min}(Z)) / DZ
\end{cases}
\end{equation}

$N_p$ is the number of points in $\mathcal{P}$, $N_g = W * L * H$ is the total number of voxels in the grid $\mathcal{G}$ and $w$, $l$ and $h$ are the integer coordinates inside the grid at $X$, $Y$ and $Z$ dimensions. The grid is represented in the way that the centroid of the voxel $g_j=(0,0,0)$ is located at position $(I_{min}(X) - (DX/2), I_{min}(Y) - (DY/2), I_{min}(Z) - z_{camera})$, where $z_{camera}$ is the height of the camera.

On each voxel, we store information related to:
\begin{itemize}
 \item The 3D associated points, assigned with the expression at equation \ref{eq:cp05_point_to_voxel}.
 \item The centroids $c=(c_x, c_y, c_z)$ of each voxel.
 \item Uncertainty of the stereo reconstruction values $\sigma_{c_x}$, $\sigma_{c_y}$ and $\sigma_{c_z}$, which will be explained in the \emph{Conditional Probabilities Calculation} stage.
 \item Weights $\omega_{occupied}$ and $\omega_{free}$, described also in the same section.
 \item Density of 3D points $N_p$.
 \item Main orientation and speed vectors $v=(v_x, v_y, v_z)$ and their associated yaw $\psi$, pitch $\theta$ and magnitude $\|v\|$.
 \item Stored particles $q_i = (x, y, z, v_x, v_y, v_z) \in \mathcal{Q}$, with their associated position $x$, $y$, $z$ and velocities $v_x$, $v_y$ and $v_z$.
 \item Obstacle identifier $o_i \in \mathcal{O}$, referencing the obstacle it belongs to. The process in which this identifier is assigned is described in section \ref{ch:chapter05_01_05}.
\end{itemize}

\subsubsection{Conditional Probabilities Calculation}\label{ch:chapter05_01_03_01}

Both posterior and conditional probabilities of the received measurement are obtained a method similar to that described in \cite{isard1998condensation}, but extended for the three-dimensional case. Probabilities are directly related to the existence or absence of 3D points in each of the voxels. These probabilities will help us, in the next stages, to weight the particles.

For each voxel, we need to know the conditional probability of the associated measurements, under the occupied $p(m(w,l,h)~|~occupied)$ or free $p(m(w,l,h)~|~free)$ assumption. For that, we must take into account the specificities of each sensor. In our case, we have based our tests on a stereo camera. In this kind of sensors, uncertainties grow as depth increases. So we need to compute the uncertainty for the centroid $c=(c_x, c_y, c_z)$ associated to each voxel. This uncertainty is given by:

\begin{equation}\label{eq:cp05_uncertainty_distance_reconstruction}
\sigma_{c_y}={{{c_y}^2 \cdot \sigma_d} \over {b \cdot f}}
\end{equation}

Here, $\sigma_d$ is the error in disparity computation; $b$ is the baseline of the stereo system; and $f$ is the focal distance (in pixels).

Based on equation \ref{eq:cp05_uncertainty_distance_reconstruction}, we can derive the lateral and height positioning error $\sigma_w$ and $\sigma_h$:

\begin{equation}\label{eq:cp05_uncertainty_lateral_and_height_reconstruction}
\begin{align}
\sigma_{c_x}={{{c_x} \cdot \sigma_l} \over {c_y}} \\
\sigma_{c_z}={{{c_z} \cdot \sigma_l} \over {c_y}}
\end{align}
\end{equation}

Errors must be mapped into voxel errors. This is just done by dividing them by the voxel size at each dimension.

\begin{equation}\label{eq:cp05_uncertainty_voxel_errors}
\begin{align}
\sigma_w={{\sigma_{c_x}} \over {DX}} \\
\sigma_l={{\sigma_{c_y}} \over {DY}} \\
\sigma_h={{\sigma_{c_z}} \over {DZ}}
\end{align}
\end{equation}

Based on these errors, we find an approximation for the conditional probabilities of the measurement voxels under the occupied/free assumption. For that, we count the occupied neighbor voxels in the grid. The distance for which we consider a voxel as neighbor comes from the values of $\sigma_w$, $\sigma_l$ and $\sigma_h$. The total number of occupied neighbors is divided by the total number of voxels explored, obtaining the final value of the conditional probability.

\begin{equation}\label{eq:cp05_conditional_prob}
p(m(w,l,h)~|~occupied) = {
{\sum \limits_{w'=w-\sigma_w}^{w+\sigma_w} \sum \limits_{l'=l-\sigma_l}^{l+\sigma_l} \sum \limits_{h'=h-\sigma_h}^{h+\sigma_h} O(w',l',h')}
\over
{(2 \cdot \sigma_w + 1) \cdot (2 \cdot \sigma_l + 1) \cdot (2 \cdot \sigma_h + 1)}}
\end{equation}

, where

\begin{equation}\label{eq:cp05_occupied}
\begin{align*}
 O(w', l', h') &=
  \begin{cases}
   1        & \text{if } %
   %
   {\exists p(x, y, z) \in \mathcal{P} ~|~ p \text{ belongs to } \mathcal{G}(w', l', h')}%
   \\
   0        & \text{otherwise}
  \end{cases}
\end{align*}
\end{equation}

The conditional probability of the measurement given the “free” assumption is

\begin{equation}\label{eq:cp05_conditional_prob_2}
p(m(w,l,h)~|~occupied) = 1 - p(m(w,l,h)~|~free)
\end{equation}

In figure \ref{fig:cp05_voxelization} the obtained voxels are represented. The occupation probability is represented through the alpha channel of each of them, so the opacity is directly related to this probability, being the most transparent those with a low value.

\begin{figure}[t]
  \centering
  \includegraphics{voxelization}
  \caption{Voxelization of a given point cloud. Voxels with a bigger occupation probability are more opaque.}\label{fig:cp05_voxelization}
\end{figure}

\subsection{Voxel pose and speed computation}\label{ch:chapter05_01_04}

The pipeline of this step is represented in figure \ref{fig:cp05_voxel_pose_speed_computation}. One of the advantages of using a grid of voxels is that most of the computation is performed separately for each of them, making it suitable for a parallel implementation.

\begin{figure}[h!]
  \centering
  \includegraphics[width=\textwidth, height=\textwidth]{voxelPoseAndSpeedComputation}
  \caption{Pipeline of the voxel pose and speed computation stage.}\label{fig:cp05_voxel_pose_speed_computation}
\end{figure}

\subsubsection{Prediction}\label{ch:chapter05_01_04_01}

In this step, we compute the present particle distribution by taking into account each particle motion model and the time passed between frames. With this new distribution, data will be ready for the next task. In the method developed by \cite{danescu2012particle}, they updated the particle set in a step quite close to this one. However, in their work, prediction equations used odometric information in order to compensate the ego-motion. In our approach, we do not perform this compensation at this point. This doesn't means that we do not care about the bias introduced by the vehicle movement. Instead, this movement will be compensated at the end of the process, once we have the whole orientation and speed of each obstacle. This give us some advantages:
\begin{itemize}
 \item We do not need to rotate and translate the whole grid between iterations.
 \item We just need to apply the motion equations in order to update the particle set.
\end{itemize}

These advantages allow saving a lot of resources. This is the most computationally expensive of the tasks in the method, which is highly dependent on the parameter $N_g$, that controls the number of particles. If we speed up this step, the system will be able to deal with a larger number of particles, improving results.

Each particle $q_i = (x, y, z, v_x, v_y, v_z) \in \mathcal{Q}$ is updated as follows. First, we need a motion model that allows to transform the particles based on the increment of time $\Delta t$ and the position and speed parameters of the particle. For this purpose, we introduce the state transition matrix $S$:

\begin{equation}\label{eq:cp05_state_transition_matrix}
S =
\left( \begin{array}{cc}
I_{3\times3} & \Delta t \cdot I_{3\times3} \\
0_{3x3} & I_{3\times3} \end{array} \right)
\end{equation}

There, $0_{3x3}$ is a $3 \times 3$ matrix of $0s$. We also introduce the matrix $\Delta$, which models the stochastic diffusion caused by the uncertainties in the motion model.

\begin{equation}\label{eq:cp05_state_motion_model_uncertainties}
\Delta =
\left( \begin{array}{cccccc}
\delta x & \delta y & \delta z & \delta v_x & \delta v_y & \delta v_z
\end{array} \right)^T
\end{equation}

Here, $\delta x$, $\delta y$, $\delta z$, $\delta v_x$, $\delta v_y$ and $\delta z$, are randomly drawn from a Gaussian distribution of zero mean and a covariance matrix Q equivalent to the state transition covariance matrix of a Kalman filter.

So, the new position of each particle is given by the expression

\begin{equation}\label{eq:cp05_particle_update}
q_{t + 1} = S \cdot q_{t} + \Delta
\end{equation}

\FloatBarrier

\subsubsection{Weighting and resampling}\label{ch:chapter05_01_04_02}

This step is quite similar to the homonym stage described in \cite{danescu2012particle}, with some minor differences, mainly related to the addition of a new dimension. For optimization purposes, in this stage we also perform the calculation of the main orientation and speed vectors associated to each voxel.

As said before, particles are the base of our system. They are used both as hypotheses and as building blocks of the different obstacles. From these particles, we will calculate the main vectors of each voxel, which will be used in later steps for the reconstruction and segmentation of final obstacles.

\begin{figure}[h!]
  \centering
  \includegraphics{sphericalHist}
  \caption{Diagram representing an spherical histogram.}\label{fig:cp05_spherical_hist}
\end{figure}

But in this step we are more interested in their role as hypotheses. A particle in a voxel is a hypothesis saying that it is occupied, and that its speed is equal to the particle's velocity. The more particles in a voxel, the more chances to be occupied. If there are not many particles in a voxel, the hypothesis of the voxel being free is supported. If the occupied hypothesis is supported by the measured data, particles are multiplied; if the free hypothesis is supported, then they are deleted.

This step is composed by two steps: Weighting and Resampling, which for optimization reasons also include the main vectors calculation.

\paragraph{Main vectors calculation}\label{ch:chapter05_01_04_02_01}

Once we have computed the survival particles falling inside an occupied voxel, main vectors are obtained. This is done through an spherical histogram. The idea, represented in figure \ref{fig:cp05_spherical_hist}, works at follows:
\begin{itemize}
 \item The possible values of yaw $\psi$ that the voxel can take ($[0\dots2\pi]$) is divided in intervals of size $\Delta\psi$; we do the same for the pitch $\theta$, with intervals of size $\Delta\theta$. In our tests, both $\Delta\psi$ and $\Delta\theta$ are set to $5^{\circ}$.
 \item For each particle, we obtain the corresponding value of $\psi$ and $\theta$, and the corresponding bin is increased in one unit. Each bin stores, also, the average of the speeds related to each particle.

 \begin{figure}[h!]
  \centering
  \includegraphics{obstacleSpeed}
  \caption{Main vectors computed.}\label{fig:cp05_main_vectors_per_voxel}
\end{figure}
 \item Speed and orientation will be given by the bin with the biggest number of related particles.
\end{itemize}

After this process, each voxel will know their estimated speed and direction. These must be obtained using just the surviving particles after the prediction process. The reason for this is that since weighting and resampling is an stochastic process, results could be biased by wrongly duplicated or eliminated particles. In figure \ref{fig:cp05_main_vectors_per_voxel} we can view the main vectors obtained for the surviving voxels after the segmentation process.

\paragraph{Weighting}\label{ch:chapter05_01_04_02_02}

\begin{algorithm}[h!]
\caption{Weighting and Resampling}
\label{alg:cp05_weighting_resampling}
\begin{algorithmic}
\Function{MeasurementBasedUpdate}{$\mathcal{G}$}
  \For {\textbf{each} voxel $g \in \mathcal{G}$}
    \State \textbf{Main vectors computation}
      \State \indent {$g$.computeMainVectors()}
    \State
    \State \textbf{Weighting}
      \State \indent Compute $N_{og}$ and $P_{og}$
      \State
    \State \textbf{Resampling}
      \State \indent Compute resampled number of particles $N_{rg}$
      \State \indent $N_{rg} \gets P_{og} \cdot N_g$
      \State \indent $f_g \gets {{N_{rg}} \over {N_{og}}}$
    \For {\textbf{each} particle $p$ belonging to $g$}
      \If { $f_g > 1$ } \Comment { The number of particles will be increased.}
	\State {$F_n \gets \lfloor f_g\rfloor$} \Comment {Integer part}
	\State {$F_f \gets f_g - \lfloor f_g\rfloor$} \Comment {Fractional part}
	\For {$k = 1$ to $F_n - 1$}
	  \State {$g$.makeCopy($p$)}
	\EndFor
	\State $r \gets \text{random value in the range [0 \ldots 1]}$
	\If {$r < F_f$}
	  \State {$g$.makeCopy($p$)}
	\EndIf
      \Else \Comment {$f_g < 1$, the number of particles will be decreased.}
	\State $r \gets \text{random value in the range [0 \ldots 1]}$
	\If {$r > F_g$}
	  \State {$g$.remove($p$)}
	\EndIf
      \EndIf
    \EndFor
  \EndFor
\EndFunction
\end{algorithmic}
\end{algorithm}

The number of particles $N_g$ is assumed to be constant, assuming that the real particles inside a voxel have the occupancy value \emph{true}. Absence of real particles is considered as the presence of virtual particles with an occupancy value equal to \emph{false}. Based on this idea, as measurement data does not include speed information (we are using as input pure 3D point clouds), weight of the particles depends only on the \emph{occupied} hypothesis.

So, for each voxel, we are interested in getting the total posterior probability of being occupied or free. In order to obtain these probabilities, we will need the measurement conditional probabilities (weights) of each voxel and the number of free/occupied hypotheses. Total posterior probabilities are given by:

\begin{equation}\label{eq:cp05_total_posterior_probabilities}
\large
\begin{array}{l}
P_{og}={{\omega_{occupied} \cdot N_{og}} \over {\omega_{occupied} \cdot N_{og} + \omega_{free} \cdot N_{fg}}} \\
P_{fg}={{\omega_{free} \cdot N_{fg}} \over {\omega_{occupied} \cdot N_{og} + \omega_{free} \cdot N_{fg}}}
\end{array}
\end{equation}

In these equations, the values of the weights $\omega_{occupied}$ and $\omega_{free}$ are already calculated during the process described at section \ref{ch:chapter05_01_03_01}, since

\begin{equation}\label{eq:cp05_occupancy_weights}
\begin{array}{l}
\omega_{occupied} = p(m(w,l,h)~|~occupied) \\
\omega_{free} = p(m(w,l,h)~|~free)
\end{array}
\end{equation}

$N_{og}$ and $N_{fg}$ are the number of particles having the \emph{occupied} hypothesis (real number of particles in the voxel), and those having the \emph{free} hypothesis, respectively. As said, we consider the absence of particles as the presence of a set of virtual particles with the hypothesis \emph{free}:

\begin{equation}\label{eq:cp05_number_of_particles}
\begin{array}{l}
N_{og} = | \{ p_i \in \mathcal{P} ~|~ p_i \text{ belongs to voxel } g \} | \\
N_{fg} = N_g - N_{og}
\end{array}
\end{equation}

\FloatBarrier

\paragraph{Resampling}\label{ch:chapter05_01_04_02_03}

Once the weighting step is finished, we can start doing the resampling step. After this, the population of particles will be properly updated, so we can use them for the reconstruction of the obstacles. As we do not take into account the free voxels, just real particles (those with the \emph{occupied} hypothesis) will be considered. For each of these particles, we will decide if they are destroyed or multiplied. After the process of resampling, each voxel will have the proper number of particles.

In algorithm \ref{alg:cp05_weighting_resampling}, we describe the resampling process, including weighting and main vectors computation.

Unlike the method proposed by \cite{danescu2012particle}, due to optimization purposes we do not split the whole process into two different loops. Instead of having the set of particles separated from the voxels, each voxel knows exactly where its particles are. In the original algorithm, it is necessary to compute and store the value of $f_g$ for absolutely all the voxels in $\mathcal{G}$. After that, the program iterates over all the particles in $\mathcal{P}$. For each of these particles $p$, we need to know the related voxel and look for the computed value $f_g$. In our approach, we assign each particle to its voxel directly at the \emph{initialization} and \emph{prediction} steps, so we can perform all the weighting and resampling at once, saving computational time.

The resampling process depends on the value of the ratio between the actual number of particles and the number of resampled particles ($f_g$). Based on this value, particles are duplicated ($f_g > 1$) or removed ($f_g < 1$). In figure \ref{fig:cp05_weight_and_resample}, we can see an example where surviving particles after this step are shown. In this case, pedestrians are moving in the negative direction of $X$ and $Y$ axis, so the most of surviving particles show this trend. These will be used for the calculation of the main vectors of the voxels in the next iteration.

\begin{figure}[t]
  \centering
  \includegraphics{weightAndResample}
  \caption{Surviving particles after the weighting and resampling step.}\label{fig:cp05_weight_and_resample}
\end{figure}

After finishing this process, we can proceed to the initialization stage.

\subsubsection{Initialization}\label{ch:chapter05_01_04_03}

In this stage, empty voxels are initialized with a number of random particles proportional to the occupancy probability. This situation can happen due to three different reasons:
\begin{itemize}
 \item It is the first iteration, so none of the voxels has been initialized already.
 \item After the weighting and resampling process, none of the particles survived.
 \item A new object appears in the scene, so the associated voxels were not initialized in previous iterations.
\end{itemize}

This stage is in charge of ensuring that the number of particles never becomes zero. Here, we just check each of the voxels $g \in \mathcal{G}$. If there are not particles in the voxel (that is, $N_p = 0$), or the occupancy probability is bigger than the occupancy threshold $\tau_{o}$ ($\omega_{occupied} > \tau_{o}$), a number of particles between 0 and $N_p$ are initialized in the voxel.

Given the voxel $g \in \mathcal{G}$, each particle is initialized with the position $(x, y, z) = (g.c_x, g.c_y, g.c_z)$, and a random speed $(v_x, v_y, v_z)$ in the range $[0\dots v_{max}(d)]$, being $v_{max}(d)$ an user defined parameter that determines the maximal speed at each dimension $d \in \{ X, Y, Z\}$ expected for each voxel.

In this implementation, we allow vertical motion, which is controlled by the parameter $v_{max}(Z)$. However, in certain situations (like planar grounds) this movement is not relevant, so it is possible to set it to $0$, in the way that particles can restrict their movement to the $XY$ plane, simplifying the problem.

\FloatBarrier

\subsection{Object reconstruction}\label{ch:chapter05_01_05}

Based on the main vectors obtained for each voxel, we can assume that all adjacent voxels with a similar direction and speed belong to the same obstacle, or at least to the same group of obstacles (for example, a group of people walking together. As they walk in the same direction and without traversable gaps between them, they are considered as a single obstacle).

In this sense, we have developed a method inspired in the color-based clustering described by \cite{broggi2013}. However, in our case, the similarity function is not based in color information due to the reasons explained before, so we split the similarity function into three parts:

\begin{equation}\label{eq:cp05_similarity_functions}
\begin{array}{l}
f_1(o,g)=\| |\vec{v}|_o - |\vec{v}|_g \|\\
f_2(o,g)=\| \psi_o - \psi_g \|\\
f_3(o,g)=\| \theta_o - \theta_g \|
\end{array}
\end{equation}

, where $|\vec{v}|_o$ and $|\vec{v}|_g$ are the magnitudes of the speeds obtained for an obstacle $o \in \mathcal{O}$ and for a voxel $g \in \mathcal{G}$, respectively. Same applies for the yaw $\psi_o$ and $\psi_g$, and the pitch $\theta_o$ and $\theta_g$ measurements.

The use of these similarity functions has several advantages. The main is that we can get the point cloud from a sensor different from a stereo pair, as we are not using color information. In fact, we could even combine them. The employed flood-fill based clustering algorithm is described in algorithm \ref{alg:cp05_clustering}.

\begin{algorithm*}[h!]
\caption{Clustering algorithm}
\label{alg:cp05_clustering}
\begin{algorithmic}
\Function{Clustering}{$\mathcal{G}$}
\State {$\mathcal{G'} \gets \mathcal{G}$} \Comment {$\mathcal{G'}$ is the set of not-checked voxels. }
\For {\textbf{each} voxel $g \in \mathcal{G'}$}
  \State {$\mathcal{G'} \gets \mathcal{G'} - g$}
  \State {$o \gets \text{ new object containing }g$}
  \State {$\text{insert } g \text{ in } queue$}
  \While {$queue \ne \emptyset$}
    \State {$g' \gets \text{extract first element from the } queue$}
    \For {\textbf{each} $\{ g'' | g'' \in o\} \in N(g')$}
      \If {$ f_1(o,g) < \tau_{v} \textbf{ and } f_2(o,g) < \tau_{\psi} \textbf{ and } f_3(o,g) < \tau_{\theta} $}
	\State {$\text{insert } g'' \text{ in } o$}
	\State {$\text{insert } g'' \text{ in } queue$}
      \EndIf
    \EndFor
  \EndWhile
  \If {$|o| >= \tau_{min\_voxels\_per\_obstacle} \textbf{ and }$\\
  \pushcode\pushcode$~~~~o.sz(Z) >= \tau_{min\_obstacle\_height} \textbf{ and }$\\
  \pushcode\pushcode$~~~~o.min(Z) == I_{min}(Z)$}
      \State {$o \text{.updateVectors()}$}
      \State {$\text{insert } o \text{ in } \mathcal{O}$}
  \EndIf
\EndFor
\EndFunction
\end{algorithmic}
\end{algorithm*}

The neighborhood of a voxel $g$ represented by the function $N(g)$ is performed using the Chebyshev distance \citep{broggi2013}. When a new voxel $g''$ is added to an obstacle $o$, the voxel identifier is updated. From the obstacle point of view, we just add the voxel to a list associated to each obstacle. Once an obstacle is about to be added to the final set of obstacles, this list will be used for updating the information related to the obstacle centroid, speed and limits on each dimension (See section \ref{ch:chapter05_01_05_01}).

Before adding an obstacle to the final list of obstacles $\mathcal{O}$, we perform a filtering stage in which, for optimization reasons, the segmentation process is also integrated. This filtering procedure is based on a set of assumptions about the obstacles expected size and their position regarding to the ground. An obstacle is accepted in the final list if:

\begin{itemize}
 \item It is big enough. That is, if the number of voxels (which is directly proportional to its volume) is over the user-defined parameter $\tau_{min\_voxels\_per\_obstacle}$.
 \item It is tall enough. The minimal height is controlled by the parameter\\ $\tau_{min\_obstacle\_height}$
 \item The obstacle is on the ground. This helps to remove flying obstacles like traffic lights or the top of some trees, in which we are not interested.
\end{itemize}

\subsubsection{Obstacle vectors computation}\label{ch:chapter05_01_05_01}

Once the list of voxels that compose each obstacle is obtained, we use it for the calculation of their speed direction and magnitude. We need to compensate the motion of our vehicle in order to know their real speed. Let $ego_{vx}$, $ego_{vy}$ and $ego_{vz}$ be the vectors that represent the motion of the vehicle regarding to the left camera coordinates frame. Also, $o_{vx}$, $o_{vy}$ and $o_{vz}$ will be the motion vectors of each obstacle and $g_{vx}$, $g_{vy}$ and $g_{vz}$ those of each of the voxels in the list for that obstacle. Then, the final speed of the obstacle $o$ will be obtained by applying:

\begin{equation}\label{eq:cp05_obstacle_vectors_computation}
\begin{cases}
o_{vx} = {{\sum\limits_{g \in \mathcal{G}_o} g_{vx}} \over {|\mathcal{G}_o|}} + ego_{vx}\\
o_{vy} = {{\sum\limits_{g \in \mathcal{G}_o} g_{vy}} \over {|\mathcal{G}_o|}} + ego_{vy}\\
o_{vz} = {{\sum\limits_{g \in \mathcal{G}_o} g_{vz}} \over {|\mathcal{G}_o|}} + ego_{vz}
\end{cases}
\end{equation}

Here, $\mathcal{G}_o$ is the list of voxels associated to the obstacle $o$. In figure \ref{fig:cp05_obstacle_vectors_computation}, we  show a simulation of the future movement of a group of people based on the obstacle speed computed in this stage.

Examples of the output of the method are shown at the end of the pipelines described in the videos at \url{http://youtu.be/7A0kyc8jW64} and \url{http://youtu.be/DDv3xWnl7Jo}. Moreover, an implementation of this method is available at \url{https://github.com/nestormh/PolarGridTracking}.

\section{Summary}\label{ch:chapter05_03}

In this chapter, we have introduced a valid method we used for the detection and tracking of obstacles in the environment using a 3D point cloud as input. This solution solves the main problems we had from the first approach we used for this purpose:

\begin{figure}[h!]
  \centering
  \includegraphics{fakePointCloud}
  \caption{Simulation of the obstacles future movement using the computed vectors.}\label{fig:cp05_obstacle_vectors_computation}
\end{figure}

\begin{itemize}
 \item It is able to do the detection of obstacles and locate them in world coordinates, not just in image coordinates.
 \item It is able to work with moving cameras.
 \item It is able to estimate the direction and speed of each obstacle.
 \item It is able to do that in real time.
\end{itemize}

The method comes with some advantages in comparison with other methods. Through the use of a particle filter based approach, the method is able to avoid complex probabilities-related calculations that can be simplified through the use of this tool. It is also able to work in three-dimensions without increasing the complexity, thanks to the use of a voxel grid. This allows also a more specific knowledge of the obstacle, which is not just limited to the most external boundaries of the obstacle, as would happen if we used cuboids. Finally, our implementation is quite modular, allowing to use as input any kind of point cloud, as no color information is used. This implementation is also fast, being able to work at 6-7\,$Hz$.

In the future, we plan to do further research in order to improve the results given by the method. Some ideas are related to the use of color (although it would affect to the modularity of our system) as using more information should improve the output. We also think that using a polar voxel grid instead of a cartesian one would improve some results. Also, it would be a good idea to keep an historic of the voxels motion, allowing to know the past movement of the obstacles. That would improve the detection of their movement by applying, for example, a Kalman filter \citep{kalman1960new}. Finally, we could take advantage of the fact that most of operations are done individually for each voxel, and carry on a parallelized implementation. This will allow improving the algorithm frequency or the use of a bigger number of particles.

In the next part of the thesis, we will introduce the way in which we command the vehicle in order to get the desired motion behavior. In the next chapter, we will see a \ac{SVM} based method used to compute an efficient and smooth trajectory, that will allow the vehicle to reach a certain point. Then, we will describe the path planning algorithm used by the vehicle to follow that trajectory. We will also see how the obstacles detected using the methods described in the first part of this document are integrated into the path planning scheme.


\documentclass[8pt,letter]{article}

%% \usepackage[fleqn]{amsmath}
\usepackage[margin=1in]{geometry}
\usepackage{amsmath,amsfonts,amsthm,bm}
\usepackage{breqn}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{tikz}
\usepackage[ruled,vlined,linesnumbered,lined,boxed,commentsnumbered]{algorithm2e}
\usepackage{siunitx}
\usepackage{graphicx}
\usepackage{subcaption}
%% \usepackage{datetime}
\usepackage{multirow}
\usepackage{multicol}
\usepackage{mathrsfs}
\usepackage{fancyhdr}
\usepackage{fancyvrb}
\usepackage{parskip} %turns off paragraph indent
\pagestyle{fancy}

\usepackage{xcolor}
\usepackage{mdframed}

\usetikzlibrary{arrows}

\DeclareMathOperator*{\argmin}{argmin}
\newcommand*{\argminl}{\argmin\limits}

\newcommand{\mathleft}{\@fleqntrue\@mathmargin0pt}
\newcommand{\R}{\mathbb{R}}
\newcommand{\Z}{\mathbb{Z}}
\newcommand{\N}{\mathbb{N}}
\newcommand{\ppartial}[2]{\frac{\partial #1}{\partial #2}}
\newcommand{\p}{\partial}
\newcommand{\te}[1]{\text{#1 }}
\newcommand{\norm}[1]{\|#1\|}

\setcounter{MaxMatrixCols}{20}

% remove excess vertical space for align equations
\setlength{\abovedisplayskip}{0pt}
\setlength{\belowdisplayskip}{0pt}
\setlength{\abovedisplayshortskip}{0pt}
\setlength{\belowdisplayshortskip}{0pt}

\newtheorem{theorem}{Theorem}[section]
\newtheorem{corollary}{Corollary}[theorem]
\newtheorem{lemma}[theorem]{Lemma}

% \newtheorem{mdtheorem}{Theorem}
% \newenvironment{theorem}
% {\begin{mdframed}[
%   backgroundcolor=green!10,
%   topline=false,
%   rightline=false,
%   bottomline=false,
%   leftline=false
%   ]\begin{mdtheorem}}
%     {\end{mdtheorem}\end{mdframed}}

\begin {document}

\lhead{Notes - Numerical Optimization}

\begin{multicols*}{2}

  \section{General}

  line search conditions (1st and 2nd order), how to choose step length and direction:\\

  sufficient decrease, proportional to $\alpha_k$ step length and directional derivative $\nabla f_k^T p_k$:
  \begin{align}
    f_{k+1}^T & \leq f_k + c_1 \alpha_k \nabla f_k^T p_k\\
    \nabla f_k^T p_k & \leq 0\ //by\ construction
  \end{align}
  curvature:
  \begin{align}
    \nabla f_{k+1}^T p_k & \geq c_2 \nabla f_k^T p_k\\
    \frac{\partial \phi(\alpha_k)}{\partial \alpha_k} & \geq \frac{\partial \phi(0)}{\partial \alpha_k}\\
              &c_1,\alpha_k \in (0,1)\\
    &0<c_1<c_2<1
  \end{align}
  \begin{align*}
  \text{where}:& f_{k} = f(x_k)\\
  & f_{k+1} = f(x_k+\alpha_k p_k)\\
  & \phi(\alpha_k) = f(x_k + \alpha_k p_k)\\
  & \frac{\partial \phi(\alpha_k)}{\partial \alpha_k} = \nabla f_{k+1}^T p_k\\
  & \frac{\partial \phi(0)}{\partial \alpha_k} = \nabla f_{k}^T p_k //initial\ slope
  \end{align*}

  strong Wolfe curvature condition, to restrict large positive derivative:
  \begin{align}
    | \nabla f_{k+1}^T p_k| \leq c_2 |\nabla f_k^T p_k|
  \end{align}

  Form of search direction:
  \begin{align}
    p_k = - B_k^{-1} \nabla f_k
  \end{align}

  $B_k$ symmetric, non-singular, postive definite $\implies$ $p_k$ is a descent direction:
  \begin{align}
    \nabla f_k^T(-B_k^{-1} \nabla f_k) < 0
  \end{align}

  Goldstein condition (may miss minimizer of $f$):
  \begin{align}
    &f_k+(1-c) \alpha_k \nabla f_k^T p_k \leq f_{k+1} \leq f_k + c \alpha \nabla f_k^T p_k\\
    &c \in (0,\frac{1}{2})
  \end{align}

  \begin{algorithm}[H]
    \SetKwInOut{Input}{$f, x, d, c_1, \alpha, \beta$}\SetKwInOut{Output}{$\alpha$}
    \Input{function, x, direction, gradient threshold, initial step length, contraction}
    \Output{found step length}
    \While{$f(x + \alpha d) > f(x) + c_1 \alpha \nabla f(x)^T d$}{
      $\alpha \leftarrow \alpha * \beta$\\
    }
    return $\alpha$
    \caption{Line Search\label{Algo_LineSearch}}
  \end{algorithm}

  % Todo: add section on derivatives

  \section{Quasi Newton}

  \subsection{Concept}
  properties: $O(n^2)$, self correcting, slightly more iterations than Newton Method, linear convergence order and superlinear rate of convergence

  Derivation, using 2nd order model, with $B_k$ SPSD:
  \begin{align*}
    m_k(p)=f_k + \nabla f_k^T p + \frac{1}{2} p^T B_k p
  \end{align*}
  taking gradient wrt. $p$ and solve, assuming minimum exists:
  \begin{align*}
    0 & = \nabla f_k + B_k p\\
    p & = - B_k^{-1} \nabla f_k
  \end{align*}
  update equation:
  \begin{align*}
    x_{k+1} & = x_k + \alpha_k p_k\\
    x_{k+1} & = x_k - \alpha_k B_k^{-1} \nabla f_k
  \end{align*}
  pick $\alpha$ to satisfy Wolfe conditions

  updated model at next iterate $x_{k+1}$:
  \begin{align*}
    m_{k+1}(p)=f_{k+1} + \nabla f_{k+1}^T p + \frac{1}{2} p^T B_{k+1} p
  \end{align*}
  impose reasonable conditions:
  \begin{enumerate}
  \item gradient of $m_{k+1}$ matches gradient of objective function $f$ at iterate $x_k$
  \item gradient of $m_{k+1}$ matches gradient of objective function $f$ at iterate $x_{k+1}$
  \end{enumerate}
  \begin{align*}
    \nabla m_{k+1}(0) & = \nabla f_{k+1} \\
    & \implies m_{k+1} \text{ condition 1 satisfied}
  \end{align*}

  condition 2 (gradient of $m_{k+1}$ match gradient of objective $f$ at iterate $x_k$):
  \begin{align*}
    \nabla m_{k+1}(-\alpha_k p_k) &= \nabla f_{k+1} - \alpha_k B_{k+1} p_k = \nabla f_k\\
    \nabla f_{k+1} - \nabla f_k &= \alpha_k B_{k+1} p_k\\
    y_k &= \nabla f_{k+1} - \nabla f_k\\
    s_k &= x_{k+1} - x_k = \alpha_k p_k\\
    y_k &= B_{k+1} s_k \text{ [secant equation]}
  \end{align*}

  $B_{k+1}$ SPD $\implies s_k^T B_{k+1} s_k = s_k^T y_k > 0$\\
  $f$ strongly convex $\implies s_k^T y_k > 0$ satisfied for any 2 points $x_k$ and $x_{k+1}$.\\
  $f$ nonconvex $\implies$ need to enforce $s_k^T y_k$ by Wolfe conditions, and use line search for step length $\alpha$.

  Using line search ensures curvature condition:
  \begin{proof}
    \begin{align*}
      &s_k=x_{k+1} - x_k = \alpha_k p_k\\
      &y_K = \nabla f_{k+1} - \nabla f_k\\
      &\text{Wolfe curvature condition:}\\
      &\nabla f_{k+1}^T p_k \geq c_2 \nabla f_k^T p_k, c_2 \in (0,1)\\
      &y_k^T s_k = (\nabla f_{k+1} - \nabla f_k)^T (\alpha_k p_k)\\
      &y_k^T s_k = \alpha_k (\nabla f_{k+1} - \nabla f_k)^T p^k\\
      &y_k^T s_k = \alpha_k (c_2 \nabla f_k - \nabla f_k)^T p^k\\
      &y_k^T s_k = \alpha_k (c_2-1) \nabla f_k^T p^k\\
      &\alpha > 0 \wedge c_2-1 < 0 \wedge \nabla f_k^T p_k < 0 (p_k \text{ is a descent dir})\\
      &\implies y_k^T s_k > 0
    \end{align*}
    Curvature condition holds when Wolfe line search is performed.
  \end{proof}

  Choosing approximate Hessian, $B$:

  Force a unique solution among infinite many due to extra degrees of freedom in the matrix, few (n) constraining conditions imposed by secand equation, few (n) constraining conditions of PD. One approach is solving a optimization problem to make row rank update to previous iterate:
  \begin{align*}
    \min_B & ||B-B_k||\\
    s.t.\ &B=B^T \\
    &Bs_k=y_k \text{ [seccant equation]}\\
    &B \succ 0
  \end{align*}
  alternatively, constrain $B$'s inverse, $H$:
  \begin{align*}
    \min_H & ||H-H_k||\\
    s.t.\ &H=H^T\\
    &Hy_k=s_k
    &H_{k+1}y_k & = s_k \text{ [secant equation]}\\
  \end{align*}

  Different norms can be used. One choice: weighted Frobenius norm:
  \begin{align*}
    ||A||_W & := ||W^{1/2}AW^{1/2}||_F\\
    ||X||_F & := (\sum_i \sum_j (X_{ij})^2)^{1/2}
  \end{align*}

  \subsection{DFT update algorithm}
  let $\bar{G_k}$ be the average Hessian:
  \begin{align*}
    \bar{G_k} = \int_0^1 \nabla^2 f(x_k + \tau \alpha_k p_k) d \tau
  \end{align*}
  using Taylor's theorem:\\
  $y_k = \bar{G_k} s_k = \bar{G_k} \alpha_k p_k$\\
  let $W=\bar{G_k}^{-1}$\\
  solve optimization problem:
  \begin{align*}
    &\min_B \| B - B_k \|_W\\
    s.t.\ &W=\bar{G_k}^{-1}\\
    &Bs_k=y_k\\
    &B=B^T
  \end{align*}

  Solution:
  \begin{align*}
    B_{k+1} &= (I-\rho_k y_k s_k^T)B_k(I - \rho_k s_k y_k^T) + \rho_k y_k y_k^T\\
    \rho_k &= \frac{1}{y_k s_k}
  \end{align*}

  Computation simplification: inverse Hessian used in update of search direction: $p+k = - B_k^{-1} \nabla f_k$\\
  Use Sherman-Morrison-Woodbury formula to obtain inverse.\\
  let $H_k = B_k^{-1}$, then DFP update becomes:\\
  \begin{align*}
    H_{k+1} = H_k - \frac{H_k y_k y_k^T H_k}{y_k^T H_k y_k} + \frac{s_k s_k^T}{y_k^T s_k}
  \end{align*}
  This is a rank 2 modification to previous iterate for efficiency.

  \subsection{BFGS update algorithm}

  Idea: impose condition on inverse of Hessian instead of Hessian approximation.\\

  let $H$ be approximate inverse of Hessian, then impose:
  \begin{align*}
    H_{k+1} \succ 0\\
    H_{k+1} = H_{k+1}^T\\
    H_{k+1} y_k = s_k
  \end{align*}

  solve for $H$ in the optimization problem:
  \begin{align*}
    &\min_H \|H-H_k\|_W\\
    s.t.\ &H=H^T\\
    &H y_k = s_k\\
  \end{align*}
  where $y_k=W s_k$\\

  let $W$ be the average Hessian:\\
  $\bar{G_k}=\int_0^1 \nabla^2 f(x_k + \tau \alpha_k p_k) d \tau$\\

  solve to obtain:
  \begin{align*}
    H_{k+1} & = (I-\rho_k s_k y_k^T) H_k (I - \rho_k y_k s_k^T) + \rho_k s_k s_k^T\\
    \rho_k & = \frac{1}{y_k^T s_k}
  \end{align*}

%%   Weight matrix $W$ satisfies $Ws_k=y_k$.\\
%%   $W$ is the average Hessian $\bar{G}$:
%%   \begin{align*}
%%     \bar{G} = \int_0^1 \nabla^2 f(x_k+\tau \alpha_k p_k) d\tau
%%   \end{align*}

%%   solution given by:
%%   \begin{align*}
%%     H_{k+1} & = (I-\rho_k s_k y_k^T)H_k(I-\rho_ky_ks_k^T) \\
%%             &+ \rho_k s_k^T s_k\\
%%     \rho_k & = \frac{1}{y_k^Ts_k}
%%   \end{align*}

  initial $H_0$ can be chosen approximately:
  \begin{itemize}
  \item finite diferences
  \item $\alpha I$
  \end{itemize}

  \begin{algorithm}[H]
    \SetKwInOut{Input}{$H_0,x_0,\epsilon>0$}\SetKwInOut{Output}{$x$}
    \Input{inverse Hessian approx., initial point, convergence tolerance}
    \Output{solution}
    $k \leftarrow$ 0\\
    $p_k \leftarrow -B^{-1} \nabla f(x_k) = -H\nabla f(x_k)$\\
    \While{$||\nabla f_k|| > \epsilon$}{
      $\alpha_k \leftarrow $ LineSearch(..)\\
      $x_{k+1} \leftarrow x_k + \alpha_k p_k$\\
      $s_k \leftarrow x_{k+1} - x_k$\\
      $y_k \leftarrow \nabla f_{k+1} - \nabla f_k$\\
      $\rho_k \leftarrow \frac{1}{y_k^Ts_k}$\\
      $H_{k+1} \leftarrow (I-\rho_k s_k y_k^T)H_k(I-\rho_ky_ks_k^T) + \rho_k s_k s_k^T$\\
      $k \leftarrow k+1$\\
    }
    return $x$
    \caption{BFGS Algorithm\label{Algo_BFGS}}
  \end{algorithm}

  Cost: $O(n^2)$ + cost of eval $f(.)$ + cost of eval $\nabla f(.)$.\\
  Order of convergence: superlinear, worse than Newton but more computationally efficient than Newton.\\

  Using Sherman-Morrison-Woodbury formula to obtain Hessian update equation,
  \begin{align*}
    B_{k+1} & = B_k - \frac{B_ks_k s_k^T B_k}{s_k^T B_k s_k} + \frac{y_k y_k^T}{y_k^T s_k}
  \end{align*}
  , but is it more efficient to use the inverse version.\\

  Proper line search is required so that BFGS algo captures curvature information.\\

  Inaccurate line search can be used to reduce computation cost.
  \vfill\null

  % \pagebreak

  \section{Trust Region Methods}
  idea:
  \begin{itemize}
  \item models local behaviour of the objective function (eg: 2nd order Taylor series)
  \item set local region to explore, then simultaneously find direction and step size to take
  \item region size adaptively set using results from previous iterations
  \item step may fail due to inadequately set region, which need to be adjusted
  \item superlinear convergence when approximate model Hessian is equal to true Hessian
  \end{itemize}

  using 2nd order Taylor series model with symmetric matrix approximating Hessian
  \begin{align*}
    \min_{p \in \R^n} m_k(p) & = f_k + g_k^Tp + \frac{1}{2}p^TB_kp,\ st.\ ||p||\leq \Delta_k\\
    \Delta_k & :=\text{trust region radiius}\\
    g_k & =\nabla f(x_k)\\
    B_k & \succeq 0
  \end{align*}

  full step is ($p_k=-B_k^{-1}g_k)$ taken when $B\succ 0 $ and $||B_k^{-1} g_k|| \leq \Delta_k$

  evaluate goodness of model with actual function by:
  \begin{align*}
    &\rho_k = \frac{f(x_k)-f(x_k+p_k)}{m_k(0)-m_k(p_k)}\\
    &action \leftarrow
      \begin{cases}
        \text{expand trust region} &, \rho_k \approx 1\ (agreement)\\
        \text{shrink trust region} &, \rho_k  < 0 + \text{thresh}\\
        \text{keep trust region} &, o/w
      \end{cases}
  \end{align*}

  \begin{algorithm}[H]
    $k \leftarrow$ 0\\
    \While{$||\nabla f_k|| > \epsilon$}{
      $p_k \leftarrow
      \argminl_{p} f_k + g_k^Tp + \frac{1}{2}p^TB_kp,\ st.\ ||p||\leq \Delta_k$\\
      $\rho_k = \frac{f(x_k)-f(x_k+p_k)}{m_k(0)-m_k(p_k)} //ratio test$\\
      //change trust region for next iterate\\
      \uIf{$\rho_k<\gamma (:\frac{1}{4})$}{
        $\Delta_{k+1} \leftarrow \alpha (:\frac{1}{4}) \Delta_k //shrink$
      }
      \uElseIf{$\rho_k> \beta (:\frac{3}{4})\ and\ ||p_k|| == \Delta_k$}{
        $\Delta_{k+1} \leftarrow min(2 \Delta_k, \hat{\Delta}) //expand$
      }
      \Else{
        $\Delta_{k+1} \leftarrow \Delta_k //keep$
      }

      //accept/reject for current iterate\\
      \uIf{$\rho_k > \eta (: \in [0, \frac{1}{4}))$}{
        $x_{k+1} \leftarrow x_k + p_k$
      }
      \Else{
        $x_{k+1} \leftarrow x_k$
      }
    }
    \caption{Trust Region Algorithm\label{Algo_TrustRegion}}
  \end{algorithm}

  Minimizer of the 2nd order Taylor series satisfy the following as a global solution for trust region iff:
  \begin{align*}
    (B+\lambda I)p^*=-g\\
    \text{complementary slackness:}\\
    \lambda(\Delta - ||p^*||)=0\\
    (B+\lambda I) \succeq 0\\
    \lambda \geq 0
  \end{align*}

  \begin{align*}
    \lambda \ge 0 \implies p \text{ parallel to negative gradient of model}
  \end{align*}

  Solving 2nd order Taylor series using approx methods:
  \begin{itemize}
  \item dogleg
  \item 2-D subspace minimization
  \item conjugate gradient based, effective when $B$ large, sparse
  \end{itemize}
  These reduce at least as much as Cauchy Point.\\

  Cauchy Point (sufficient reduction in model value):\\
  Use 1st order approx. of model and gradient descent to get get next iterate, bounded within trust region.\\

  \subsection{Dogleg method}
  if $B \succ 0$:\\
  $p^B=-B^{-1}g$\\
  $p^*=p^B$ if $\Delta \geq \|p^B\|$\\
  $p^U=\frac{-g^Tg}{g^TBg}g$ (intermediate point along direction of steepest descent)\\
  Resulting algo, interpolate between $p^U$ and $p^B$:\\
  $\tilde{p}(\tau) =
  \begin{cases}
    \tau p^U &, \tau_\in [0,1]\\
    p^U + (\tau-1)(p^B-p^U) &, \tau \in [1,2]
  \end{cases}
  $
  \begin{align*}
    B \succ 0 \implies & \|\tilde{p}(\tau)\| \text{increases wrt. }\tau\ \land\\
    & m(\tilde{p}(\tau)) \text{decreases wrt. } \tau
  \end{align*}

  if $\|p^B\| \leq \Delta$: $p$ chosen at $p^B$\\
  else $p$ chosen at intersection of $\tilde{p}(\tau)$ and trust region boudnary by solving:\\
  $\|p^U+(\tau-1)(p^B-p^U)\|^2 = \|\Delta^2\|$

  $p_k^S=\argminl_p f_k+g_k^Tp, \|p\| \leq \Delta_k$\\
  $\tau_k = \argminl_{\tau\geq0} m_k(\tau p_k^S), \|\tau p_k^S\| \leq \Delta_k$\\
  $p_k^S=\frac{-\Delta_k g_k|}{\|g_k\|}$\\
  $p_k^C=\tau_k p_k^S$\\
  $p_k^C=-\tau_k \frac{\Delta_k g_k}{\|g_k\|}$\\
  $\tau_k=\begin{cases}
    1 &, g_k^T B_k g_k \leq 0\\
    min(\frac{\|g_k|^3}{\Delta_k g_k^T B_k g_k}, 1) &, o/w
  \end{cases}$

  \vfill\null
  % \pagebreak

  \subsection{Iterative Solution}
  Idea: solve subproblem $\min_{\|p\| \leq \Delta} m(p)$ by applying Newton's method to find $\lambda$ that matches trust region radius. This is slightly more accurate per step compared to Dogleg. Use $(B+\lambda I)p^*=-g$ to solve $\min_{\|p\| \leq \Delta} m(p)$ for $\lambda$.

  If $\lambda=0$ and $(B+\lambda I)p^*=-g, \|p^*\| \leq \Delta$ and $(B+\lambda I) \succeq 0$: return $\lambda$\\
  Else: find $\lambda$ s.t. $(B+\lambda I) \succeq 0$ and $\|p(\lambda)\| = \Delta, p(\lambda)=-(B+\lambda I)^{-1}g$. Solve and return $\lambda$.

  Solve $\|p(\lambda)\|-\Delta=0, \lambda > \lambda_1$ via Newton's method (root finding). Approx. this to nearly a linear problem for easy solving:\\

  \begin{algorithm}[H]
    \For{$l=0,1,..$}{
      solve $B+\lambda ^l I = R^T R$\\
      $R^TR p_l = -g$\\
      $R^T q_l = p_l$\\
      $\lambda^{l+1} \leftarrow \lambda^l + (\frac{\|p_l\|}{\|q_l\|})^2(\frac{\|p_l\|-\Delta)}{\Delta})$
      check $\lambda \geq \lambda_1$
    }
    \caption{Subproblem Algo\label{Algo_TrustRegionIterativeSubproblem}}
  \end{algorithm}

  \vfill\null
  \pagebreak

  \section{Conjugate Gradient}

  \subsection{linear method}
  Assuming unconstrained problem with strict convex quadratic objective function:
  \begin{align*}
    \frac{1}{2} x^TAx-b^Tx, A \succ 0, A^T=A
  \end{align*}

  $\nabla(\frac{1}{2} x^TAx-b^Tx) = Ax-b$, thus $\min_x x^TAx - b^Tx$ transformed to solving $Ax-b=0$.

  let $x_{k+1} = x_k + \alpha_k p_k$, solve for $\alpha$:
  \begin{align*}
    %%     &\frac{\partial}{\partial \alpha} (\frac{1}{2}(x_k + \alpha_k p_k)^T A (x_k + \alpha_k p_k) - b^T(x_k + \alpha_k p_k)) = 0\\
    &Ax_{k+1}-b=0\\
    &A(x_k+\alpha_k p_k) - b=0\\
    &\alpha_k A p_k = b - Ax_k\\
    &r_k = Ax-b\\
    &\alpha_k A p_k = -r_k\\
    &\alpha_k p_k^T A p_k = - p_k^T r_k\\
    &\alpha_k = - \frac{p_k^T r_k}{p_k^T A p_k}
  \end{align*}
  \subsection{Conjugate Direction}

  Enforce by construction for search directions linearly independent wrt. A.:
  \begin{align*}
    (\forall i\not=j) p_i^T A p_j = 0
  \end{align*}
  Properties:
  \begin{itemize}
  \item Residual elimnated one direction at a time, resulting in max of n iterations.
  \item Optimal if Hessian is diagonal, if not can try preconditioning.
  \item Current residual is orthogonal to all previous search directions.
  \item Any set of conjugate directions can be used:
    \begin{itemize}
    \item eigenvectors
    \item Gram-Schmidt
    \item conjugate gradient algo.
    \end{itemize}
  \end{itemize}

  \vfill\null
  \columnbreak

  \subsubsection{Termination Steps}
  \begin{theorem}[Conjugate Direction Termination]\label{thm:CD_Term}
    Conjugate direction algorithm converges to solution $x \in \R^n$ of linear system in n steps. Theorem [T5.1] in Num. Opt. book.
  \end{theorem}
  \begin{proof}
    \begin{align*}
      &\text{given:}\\
      &x_{k+1} = x_k + \alpha_k p_k\\
      &\alpha_k = - \frac{r_k^T p_k}{p_k^T A p_k}\\
      &(\forall i \neq j) p_i^T A p_j = 0\ \text{[A conjugate]}\\
      &\text{then:}\\
      &x^*-x_0 = \sum_{i=0}^{n-1} \sigma_i p_i\\
      &p_k^T A(x^*-x_0) = p_k^T A(\sum_{i=0}^{n-1} \sigma_i p_i)\\
      &\sigma_i = \frac{p_k^T A(x^*-x_0)}{p_k^T A p_k}\ \text{(from conjugacy)}\\
      &x_k = x_0 + \sum_{i=0}^{k-1} \alpha_i p_i\\
      &p_k^T A x_k = p_k^T A (x_0+\alpha_0 p_0 + ..)\\
      &p_k^T A x_k = p_k^T A x_0\ \text{(from conjugacy)}\\
      &p_k^T A (x_k-x_0) = 0\\
      &p_k^T A(x^*-x_0) = p_k^T A(x^*-x_k)\\
      &p_k^T A(x^*-x_0) = p_k^T(b-Ax_k) = -p_k^T r_k\\
      &\sigma_i = \frac{-p_k^T r_k}{p_k^T A p_k} = \alpha_k\\
      &\alpha_k\ \text{is 1D minimizer of kth coordinate by construction}
    \end{align*}
    Conjugate direction algo. terminates in n steps.
  \end{proof}

  If A is not diagonal, can transform coordinates:
  \begin{align*}
    S\hat{x} &= x\\
    S &=[p_0\ p_1\ ..\ p_{n-1}],\ (\forall i \neq j)\ p_i A p_j =0\\
    \phi(x) &= \frac{1}{2} x^T A x - b^T x\\
    \phi(\hat{x}) &= \frac{1}{2} \hat{x}^T S^T A S \hat{x} - (S^T b)^T \hat{x}\\
    S^T A S\ diagonal & \implies \text{can optimize one coordinate at a time}\\
  \end{align*}

  \vfill\null
  \columnbreak

  \subsubsection{Expanding subspace minimizer}
  Using conjugate directions to generate sequence $\{x\}$, then:\\
  $r_k^T p_i = 0, \forall i < k$, $x_k$ is minimizer of $\frac{1}{2} x^TAx - b^Tx$ over $\{ x | x= x_0 + span\{p_0,...p_{k-1}\}$
  \begin{proof}
    \begin{align*}
      &\tilde{x} = x_0 + \sum_i \sigma_i p_i\\
      &\tilde{x} \text{ minimizes over }\{x_0 + span\{ p_0, ... p_{k-1}\}\} \iff r(\tilde{x})^Tp_i = 0\\
      &h(\sigma) = \phi(\tilde{x})\\
      &\phi(x) =\frac{1}{2}x^TAx-b^Tx\\
      &\text{h is also strictly convex quadratic,}\\
      &\text{with unique }\sigma^*\text{ satisfying:}\\
      &\frac{\partial h(\sigma^*)}{\partial \sigma_i} = 0, i=[0,k-1]\\
      &\frac{\partial h(\sigma^*)}{\partial \sigma_i} = \nabla \phi(\tilde{x})^T p_i = 0, i = [0,k-1]\\
      &\nabla \phi(x) = Ax-b=r\\
      &r(\tilde{x})^T p_i = 0, i=[0,k-1]\\
    \end{align*}
  \end{proof}
  $p_i^T r_k=0, i=[0,k-1]$ via induction:
  \begin{proof}
    \begin{align*}
      &\text{base case of k=1, i=0}: x_1 = x_0 + \alpha_0 p_0 \text{ minimizes } \phi \text{ along } p_0\\
      &\implies r_1^T p_0 = (r_0 + \alpha_0  A p_0)^T p_0= 0 \\
      &\text{case: }r_{k-1}^T p_i = 0, i=[0,k-2]:\\
      &r_k = r_{k-1} + \alpha_{k-1} A p_{k-1}\\
      &(\forall i\in[0,k-2]) p_i^T r_k = p_i^T r_{k-1} + \alpha_{k-1} p_i^T A p_{k-1}\\
      &(\forall i=[0,k-2]) p_i^T A p_{k-1}=0\ \text{(A-conjugacy by construction)}\\
      &(\forall i=[0,k-2]) p_i^T r_{k-1} = 0\ \text{(by induction hypothesis)}\\
      &p_i^T r_k = 0, i=[0,k-1]
    \end{align*}
  \end{proof}

  \vfill\null
  \pagebreak

  \subsection{Conjugate Gradient Method}
  Idea:
  \begin{itemize}
  \item uses only previous search direction to compute current search direction
  \item $p_k$ set to linear combination of $-r_k$ and $p_{k-1}$
  \item impose $p_k^T A p_{k-1}=0$
  \end{itemize}
  \begin{align*}
    p_k &= -r_k + \beta_k p_{k-1}\\
    p_{k-1}^T A p_k &= -p_{k-1}^T A r_k + \beta p_{k-1}^T A p_{k-1}\\
    0 &= -p_{k-1}^T A r_k + \beta p_{k-1}^T A p_{k-1}\\
    \beta &= \frac{p_{k-1}^T A r_k}{p_{k-1}^T A p_{k-1}}\\
    p_0& = -(Ax_0-b)=-r_0
  \end{align*}
  \begin{algorithm}[H]
    $r_0 \leftarrow Ax_0-b$\\
    $p_0 = -r_0$\\
    \For{$k=[0,..n-1]$}{
      \uIf{$r_k==0$}{
        return $x_k$
      }
      \Else{
        $\alpha_k \leftarrow \frac{-r_k^Tp_k}{p_k^T A p_k} = \frac{r_k^T r_k}{p_k^T A p_k}$\\
        $x_{k+1} \leftarrow x_k + \alpha_k p_k$\\
        $r_{k+1} \leftarrow Ax_{k+1} - b$\\
        $\beta_{k+1} \leftarrow \frac{p_k^T A r_{k+1}}{p_k^T A p_k}=\frac{r_{k+1}^T r_{k+1}}{r_k^T r_k}$\\
        $p_{k+1} \leftarrow -r_{k+1} + \beta_{k+1} p_k$
      }
    }
    \caption{Basic Conjugate Gradient Algorithm\label{Algo_CGBasic}}
  \end{algorithm}
  $p_k$ and $r_k$ is within krylov subspace:\\
  $K(r_0;k) =span\{r_0, Ar_0, ..A^k r_0\}$\\
  if $r_k \not= 0$:\\
  $r_k^T r_i = 0, i=[0,k-1]$\\
  $span\{r_0,..,r_k\} = span\{r_0,Ar_0,..,A^k r_0\}$\\
  $span\{p_0,..,p+k\}=span\{r_0,Ar_0,..,A^k r_0\}$\\
  $p_k^T A p_i=0, i=[0,k-1]$\\
  then, $\{x_k\} \rightarrow x^*$ in at most n steps.\\

  \vfill\null
  \columnbreak

  Simplification:
  \begin{align*}
    &p_{k+1} \leftarrow -r_{k+1} + \beta_{k+1} p_k\\
    &\alpha_k \leftarrow \frac{-r_k^Tp_k}{p_k^T A p_k}\\
    &\alpha_k \leftarrow \frac{-r_k^T(-r_{k} + \beta_{k} p_{k-1})}{p_k^T A p_k}\\
    &(\forall i=[0,k-1]) r_k^T p_i=0 \implies \beta_k r_k^T p_{k-1}=0\\
    &\alpha_k \leftarrow \frac{r_k^T r_k}{p_k^T A p_k} \ \text{(simplified)}
  \end{align*}
  \begin{align*}
    &r_{k+1}=r_k+\alpha_k A p_k\\
    &A p_k = \frac{r_{k+1}-r_k}{\alpha_k}\\
    &\beta = \frac{p_k^T A r_{k+1}}{p_k^T A p_k}\\
    &p_k^T A p_k = p_k^T \frac{r_{k+1}-r_k}{\alpha_k}= \frac{-p_k^T r_k}{\alpha} (\text{conjugacy})\\
    &p_k^T A p_k = -\frac{(- r_k + \beta_k p_{k-1})^T r_k}{\alpha}=\frac{r_k^T r_k}{\alpha} (\text{conjugacy})\\
    &p_k^T A r_{k+1} = r_{k+1}^T A p_k\\
    &p_k^T A r_{k+1} = r_{k+1}^T \frac{r_{k+1}-r_k}{\alpha_k}\\
    &r_k \in span\{p_k,p_{k-1}\}\ and\ r_{k+1}^T p_i = 0, i=[0,k] \implies\\
    &p_k^T A r_{k+1} = \frac{r_{k+1}^T r_{k+1}}{\alpha_k}\\
    &\beta_{k+1} \leftarrow \frac{r_{k+1}^T r_{k+1}}{r_k^T r_k} \ \text{(simplified)}
  \end{align*}

  \subsection{Nonlinear Method}
  Minimize general convex function or nonlinear function. Variants: FR, PR.

  \subsubsection{FR (Fletcher Reaves)}
  Modify linear CG by:
  \begin{itemize}
  \item replace residual by gradient of nonlinear objective, $r_k \rightarrow \nabla f_k$
  \item replace $\alpha_k$ computation by a linear search to find approx. minimum along search direction of $f$
  \end{itemize}

  Equivalent to linear CG if objective is strongly convex quadratic.\\

  Linear search for $\alpha_k$ with strong Wolfe condition to ensure $p_k$'s are descent directions wrt. objective function.\\

  \subsubsection{PR}
  Replace $\beta_{k+1}$ computation in FR with:
  \begin{align*}
    \beta_{k+1}^{PR} & \leftarrow \frac{\nabla f_{k+1}^T (\nabla f_{k+1} - \nabla f_k)}{\nabla f_k^T \nabla f_k}\\
    \beta_{k+1}^+ & \leftarrow \max(\beta_{k+1}^{PR}, 0)
  \end{align*}

  CG Gradient Algo. Property [T.5.3]:
  \begin{align*}
    &(\forall i=[0,k-1])\ r_k^T r_i=0\\
    &span\{r_0,..,r_k\} = span\{r_0,Ar_0,..,A^kr_0\}\\
    &span\{p_0,..,p_k\} = span\{r_0,Ar_0,..,A^kr_0\}\\
    &(\forall i=[0,k-1])\ p_k^T A p_i = 0\\
    &\implies \{x_k\}\ \text{converges to } x^*\ \text{in at most n steps}
  \end{align*}
  Proof by induction to show generated search direcitons are A-conjugate. Then apply Theorem \ref{thm:CD_Term} to conclude algo terminates within n steps.

  \vfill\null
  \columnbreak

  \section{Proximal Algorithm}
  Idea:
  \begin{itemize}
  \item reliance on easy to evaluate proximal operators
  \item separability allows parallel evaluation
  \item generalization of projection based algorithms
  \end{itemize}

  \begin{align*}
    prox_{\lambda f}(v) = \argminl_{x} f(x) + \frac{1}{2 \lambda} \|x-v\|_2^2
  \end{align*}

  Resolvent of subdifferential operator:
  \begin{align*}
    &z=prox_{\lambda f}(x) \implies z \in (I+\lambda \partial f)^{-1}(x)\\
    &(I+\lambda \partial f)^{-1} :=\text{resolvent of operator }\partial f
  \end{align*}

  \subsection{Proximal Gradient Method}
  Solve $\min_{x} g(x)+f(x)$, where $f,g$ are closed, convex functions and $f$ differentiable
  \begin{align*}
    &x^* = prox_{\lambda^k g}(x^k-\lambda^k \nabla f(x^k)) \\
    &=\argminl_{x} g(x) + \frac{1}{2 \lambda^k} \|x - (x^k - \lambda^k \nabla f(x^k))\|_2^2\\
    &\text{tradeoff between g and and gradient step}\\
    &g=I_C(x) \implies \text{projected gradient step}\\
    &g=0 \implies \text{gradient descent}\\
    &f=0 \implies \text{proximal minimization}
  \end{align*}

  Relation to Pixed Point:\\
  $x^*$ is a fixed point solution of $\min_{x} g(x)+f(x)$ iff $0 \in \nabla f(x^*) + \partial g(x^*)$ iff $x^* = (I+\lambda \partial g)^{-1}(I-\lambda \nabla f)(x^*)$\\

  Forward Euler, Backward Euler stepping is same as the proximal gradient iteration, $prox_{\lambda^k g}(x^k-\lambda^k \nabla f(x^k))$\\

  \subsection{Accelerated Proximal Gradient Method}
  Introduce extrapolation:
  \begin{align*}
    &y^{k+1} = x^k + w^k(x^k-x^{k-1})\\
    &x^{k+1} = prox_{\lambda^k g}(y^{k+1}-\lambda^k \nabla f(y^{k+1}))\\
    &w^k \in [0,1)
  \end{align*}

  Example: $w^k = \frac{k}{k+3}, w^0 = 0, \lambda^k \in (0,1/L], L:= \text{Lipschitz constant of }\nabla f,\ or\ \lambda^k$ found via line search.
  Line search for $\lambda^k$ (Beck and Teboulle):

  \begin{algorithm}[H]
    $\hat{f}(x,y):=f(y) + \nabla f(y)^T(x-y) + \frac{1}{2\lambda}\|x-y\|^2_2$\\
    \While{True}{
      $z = prox_{\lambda g}(y^k-\lambda \nabla f(y^k))$\\
      \uIf{$f(x) \leq \hat{f}(z,y^k)$}{
        break
      }
      $\lambda = \beta \lambda$
    }
    return $\lambda^k := \lambda, x^{k+1} := z$\\
    \caption{Proximal Gradient Algorithm\label{Algo_ProximalGradient}}
  \end{algorithm}

  \subsection{Types of Proximal Operators}
  \begin{itemize}
  \item quadratic functions
    \begin{align*}
      f&=\frac{1}{2} \|.\|_x^2 \implies prox_{\lambda f}(v) = (\frac{1}{1+\lambda})v\\
      f&=\frac{1}{2}x^T A x + b^T x + c, A \in S_+^n \implies\\
      &prox_{\lambda f}(v)=(I+\lambda)^{-1}(v-\lambda b)
    \end{align*}
  \item unconstrained problem: use gradient methods such as Newton, Quasi-Newton
  \item constrained: use projected subgradient for non-smooth, projected gradient or interior method for smooth
  \item separable function: if scalar, may be solved analytically, eg:
    L1 norm separable to:
    \begin{align*}
      f(x) = |x| \implies prox_{\lambda f}(v) =
      \begin{cases}
        v-\lambda ,& v \geq \lambda\\
        0,& |v| \leq \lambda\\
        v+\lambda, & v \leq -\lambda
      \end{cases}
    \end{align*}
    \begin{align*}
      f(x) = -log(x) \implies prox_{\lambda f}(v) = \frac{v+\sqrt{v^2+4\lambda}}{2}
    \end{align*}
  \item general scalar function
    \begin{itemize}
    \item localization: using a subgradient oracle and bisection algorithm
    \item twice continuously differentiable: guarded Newton method
    \end{itemize}
  \item polyhedra constraint, quadratic objective: solve as QP problem
    \begin{itemize}
    \item duality to reduce number of variables to solve if possible
    \item gram matrix caching
    \item affine constraint($Ax=b$): use pseudo-inverse, $A^{+}$:
      \begin{align*}
        \Pi_C(v) &= v - A^{+}(Av-b)\\
                 &A \in R^{m \times n}, m < n \implies A^{+} = A^T(AA^T)^{-1}\\
                 &A \in R^{m \times n}, m > n \implies A^{+} = (A^TA)^{-1}A^T
      \end{align*}
    \item hyperplane constaint($a^Tx=b$):
      \begin{align*}
        \Pi_C(v) = v + (\frac{b-a^Tb}{\|a\|_2^2})a
      \end{align*}
    \item halfspace
      \begin{align*}
        \Pi_C(v) = v - \frac{max(a^T v -b,0)}{\|a\|_2^2}a
      \end{align*}
    \item box($l\leq x \leq u$)
      \begin{align*}
        \Pi_C(v)_k = min(max(v_k, l_k), u_k)
      \end{align*}
    \item probability simplex($1^Tx= 1, x \geq 0$)\\
      bisection algo on $\nu$:
      \begin{align*}
        \Pi_C(v) = (v- \nu 1)_+\\
        \text{intial }[l_k,u_k] = [\max_i v_i-1, \max_i vi]
      \end{align*}
      analytically solve when bounded in between 2 adjacent v_i's
    \end{itemize}
  \item cones ($\kappa$: proper cone)\\
    problem of the form:
    \begin{align*}
      min_x \|x-v\|_2^2\\
      s.t.: x\in \kappa
    \end{align*}
    \begin{align*}
      x \in \kappa\\
      v=x-\lambda\\
      \lambda\in\kappa^*\\
      \lambda^Tx=0
    \end{align*}
    \begin{itemize}
    \item cone $C=\R_+^n$
      \begin{align*}
        \Pi_C(v)=v_+
      \end{align*}
    \item 2nd order cone $C=\{(x,t)\in \R^{n+1}: \|x\|_2 \leq t\}$
      \begin{align*}
        \Pi_C(v,s) = \begin{cases}
          0,& \|v\|_2 \leq -s\\
          (v,s),& \|v\|_2 \leq s\\
          \frac{1}{2}(1+\frac{s}{\|v\|_2})(v,\|v\|_2),& \|v\|_2 \geq |s|
        \end{cases}
      \end{align*}
    \item PSD cone $S_+^n$
      \begin{align*}
        \Pi_C(V)&=\sum_i (\lambda_i)_+ u_i u_i^T\\
        V&=\sum_i \lambda_i u_i u_i^T\ (eigendecomp)
      \end{align*}
    \item exponential cone\\
      Todo
    \end{itemize}
  \item pointwise supremum
    \begin{itemize}
    \item max function
    \item support function
    \end{itemize}
  \item norms
    \begin{itemize}
    \item L2
    \item L1
    \item L-$\inf$
    \item elastic net
    \item sum of norms
    \item matrix norm
    \end{itemize}
  \item sublevel set
  \item epigraph
  \item matrix functions
    Todo
  \end{itemize}

  \section{Subgradient Method}
  todo

  \end{multicols*}
\end {document}


\subsection{Types, Type Applications and Type functions} % (cvr)
\label{tyfun-sec}
A type $\tau$ is an {\sl equality type},\index{22.3} or {\sl admits equality}, if it is
of one of the forms
\begin{itemize}
\item $\alpha$, where $\alpha$ admits equality;
\item $\{\lab_1\mapsto\tau_1,\ \cdots,\ \lab_n\mapsto\tau_n\}$,
      where each $\tau_i$ admits equality;
% \item $\tauk\t$, where $t$ and all members of $\tauk$ admit equality; % (cvr)
\item $\apptype{\tauk}{\typeapp}$, where \typeapp\ and all members of $\tauk$ admit equality; % (cvr)

\item $(\tau')\REF$.\index{23.1}
\end{itemize}
% (cvr
(Note that  if $\tau$ is a package type \longpackagetype\ then
it \emph{does not} admit equality.)

A type application $\typeapp$ is an {\sl equality type application},
\index{22.3} or {\sl admits equality}, if it is
of one of the forms
\begin{itemize}
\item $\t$, where $\t$ admits equality;
\item $\apptypeapp{\typeapp'}{\typefnc}$, where \typeapp\ and \typefnc\ both
       admit equality.
\end{itemize}

A type function $\typefnc$ is an {\sl equality type function},
\index{22.3} or {\sl admits equality}, if it is
of one of the forms
\begin{itemize}
\item \typetypefnc{\alphak}{\typefnc'}, where, when the type variables $\alphak$
      are chosen to admit equality, then $\typefnc'$ also admits equality;
\item \typefnctypefnc{\t}{\typefnc'}, where,  when the type name $\t$
      is chosen to admit equality, then $\typefnc'$ also admits equality;
\item \typeapptypefnc{\typeapp}, where \typeapp\ admits equality.
\end{itemize}

% cvr)
% (cvr
% \label{tyfcn-lab}
% A type function $\theta=\Lambda\alphak.\tau$\index{23.2}
%  has arity $k$; it must be
% {\sl closed} -- i.e.
% $\TyVarFcn(\tau)\subseteq\alphak$ -- and the bound variables must
% be distinct. Two type functions are considered equal
% if they only differ in their choice of bound variables (alpha-conversion).
% In particular, the equality attribute has no significance in a
% bound variable of a type function; for example, $\Lambda\alpha.\alpha\to
% \alpha$ and $\Lambda\beta.\beta\to\beta$ are equal type functions
% even if $\alpha$ admits equality but $\beta$ does not.
% %poly
% Similarly, the imperative attribute has no significance
% in the bound variable of a type function.
% If $t$ has arity $k$, then we write $t$ to mean $\Lambda\alphak.\alphak\t$
% (eta-conversion); thus $\TyNames\subseteq\TypeFcn$. $\theta=\Lambda\alphak.\tau$
% is an {\sl equality} type function, or {\sl admits equality}, if when the
% type variables $\alphak$ are chosen to admit equality then $\tau$ also admits
% equality.
\label{tyfcn-lab}
A type function $\theta=\Lambda\alphak.\tau$\index{23.2}
 has the arity $\k$ as its kind; its bound variables must
be distinct.
A type function $\typefnc=\typefnctypefnc{\t[\K]}{\typefnc'}$\index{23.2}
 has kind $\Karr{\K}{\K'}$, provided $\typefnc'$ has kind $\K'$.

Two type functions are considered equal if they have the same kind
and differ in their choice of bound variables or
type names.
In particular, the equality attribute has no significance in a
bound variable or type name of a type function;
for example, $\Lambda\alpha.\alpha\to\alpha$
(\typefnctypefnc{\t[\K]}{\apptypeapp{\t}{\typefnc}})
and $\Lambda\beta.\beta\to\beta$
 (\typefnctypefnc{\u[\K]}{\apptypeapp{\u}{\typefnc}}) are equal type functions
even if $\alpha$ ($\t$) admits equality but $\beta$($\u$) does not (provided
$\t,\u \not \in \TyNamesFcn \typefnc$).

If the type application \typeapp\ has kind \k\ then we
identify the type function $\typefnc = \typeapp$ with the type function
\typetypefnc{\alphak}{\apptype{\alphak}{\typeapp}}\
(provided $(\TyVarsFcn \alphak) \cap (\TyVarsFcn \typeapp) = \emptyset$) (eta-conversion).
If the type application \typeapp\ has kind \Karr{\K}{\K'} then we
identify the type function $\typefnc = \typeapp$ with the type function
\typefnctypefnc{\t[\K]}{\typeapptypefnc{\apptypeapp{\typeapp}{\typeapptypefnc{\t}}}}\
 (provided $\t\not \in \TyNamesFcn \typeapp$) (eta-conversion).

For convenience, when $\t$ has arity $k$, we
shall write the type name $\t$ to mean
the type function $\typetypefnc{\alphak}{\apptype{\alphak}{\tynametypeapp{\t}}}$.

% cvr)

We write the application of a type function $\typefnc[\k]$ to a vector
$\tauk$ of types as $\tauk\theta$.
If $\typefnc=\typetypefnc{\alphak}{\tau}$ we set $\tauk\theta=\tau\{\tauk/\alphak\}$
(beta-conversion).

We write $\tau\{\thetak/\t^{(\k)}\}$ for the result of substituting type
functions $\thetak$ for type names $\t^{(k)}$ in $\tau$.
We assume that all beta-conversions
are carried out after substitution, so that for example
\[(\tauk\t)\{\typetypefnc{\alphak}{\tau}/\t\}=\tau\{\tauk/\alphak\}.\]
(assuming $\t \not \in \TyNamesFcn \tauk$); and
\[(\typeapptypefnc{\apptypeapp{\t}{\typefnc}})\{\typefnctypefnc{\u}{\typefnc'}/\t\}=
     \typefnc'\{\typefnc/\u\}.\]
(assuming $\t \not \in \TyNamesFcn \typefnc$).