PrognoseNet: A Generative Probabilistic Framework for Multimodal Position Prediction given Context Information

The input trajectory (up to time step $t$) is defined as the sequence:

with $\text{$\Delta$}x^{\left\langle t\right\rangle}=x^{\left\langle t\right\rangle}-x^{\left\langle t-1\right\rangle}$ and $\text{$\Delta$}y^{\left\langle t\right\rangle}=y^{\left\langle t\right\rangle}-y^{\left\langle t-1\right\rangle}$. For brevity in the following we will write: $\boldsymbol{x}^{\left\langle t\right\rangle}=(x^{\left\langle t\right\rangle},y^{\left\langle t\right\rangle})$. Both $\boldsymbol{x}^{\left\langle t\right\rangle}$ and $\boldsymbol{x}^{\left\langle t-1\right\rangle}$ in the definition of the deltas are expressed in the vehicle coordinate system of time step $t$ (please note that $\boldsymbol{x}^{\left\langle t\right\rangle}=(0,\ 0)$ in VCS of time step $t$). The terms $v^{\left\langle t\right\rangle}$, $h^{\left\langle t\right\rangle}$ are the velocity and the heading angle respectively. Please note that $t$ is an integer index denoting measurements, which were obtained using a sampling rate of 10Hz.

The static map for time step $t$ is denoted as $\mathcal{\mathscr{M}}^{\left\langle t\right\rangle}$. It is also expressed in the vehicle coordinate system of time step $t$, see Fig. 3. The static maps used in this paper contain road boundaries and center lines.

The benefit of using the vehicle coordinate system is that it reduces complexity of the input data. In the vehicle coordinate system, the two trajectories depicted above are identical, whereas in global coordinates they are not.

The ground truth for time step $t$ is $\boldsymbol{x}^{\left\langle t+\Delta\right\rangle}$, i.e. the position of the ego vehicle $\Delta$ time steps ahead. The ground truth position is expressed in the vehicle coordinate system of the current time step $t$. Since we are in the unsupervised learning setting, these points do not come with any labels. For each time step $t$ we can hence define the following input output pair: $(\mathscr{\mathscr{T}}^{\left\langle 0:t\right\rangle},\ \mathcal{\mathscr{M}}^{\left\langle t\right\rangle};\ \boldsymbol{x}^{\left\langle t+\text{$\Delta$}\right\rangle})$.

In this paper the ground truth is defined only as the future position of the ego vehicle. It can, however, be extended to encompass further information, i.e. predicting the future heading angle.

In the following, we will denote random variables by an uppercase letter, while their realizations will be denoted by a lowercase letter.

We wish to model the data in a generative manner¹¹1A generative model includes the distribution of the data itself, and tells you how likely a given example is. by approximating the following conditional density function:

which gives the probabilities of all possible positions at time step $t+\text{$\Delta$}$, given the input trajectory $\mathscr{T}^{\left\langle 0:t\right\rangle}$ up to time step t and the static map $\mathscr{M}^{\left\langle t\right\rangle}$ at time step $t$. Please note that $X^{\left\langle t+\text{$\Delta$}\right\rangle}$, $Y^{\left\langle t+\text{$\Delta$}\right\rangle}$ are expressed in the vehicle coordinate system of time step $t$.

We model the data by specifying a Gaussian mixture model for each time step $t$:

where $Z^{\left\langle t+\text{$\Delta$}\right\rangle}$ is a discrete latent random variable, which can take on $k$ different values. The $\theta_{1},\ \theta_{2}$ denote the parameters of the different parts of the Gaussian mixture model. The probability distribution:

in the upper equation is characterized by $k$ probability values $\phi_{j}$ satisfying $\phi_{j}\geq 0$ and $\sum_{i=1}^{k}\phi_{j}=1$. Furthermore for a Gaussian mixture model we assume:

In the subsequent formulas, for the sake of brevity, we won’t denote explicitly the dependency on $\mathscr{T}^{\left\langle 0:t\right\rangle}$, $\mathcal{\mathscr{M}}^{\left\langle t\right\rangle}$ when there is no risk of ambiguity.

Assuming that the $m$ training examples were generated independently, we can write down the log-likelihood of the parameters $\theta_{1}$ and $\theta_{2}$ for a single time step $t$ as:

where the superscripts in round brackets denote the sample number. We will design the latent random variable $Z^{\left\langle t+\text{$\Delta$}\right\rangle}$ in such a way that its true values $z^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ are known beforehand and can easily be deduced from the ground truth $\boldsymbol{x}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ . We can then write the log-likelihood as:

or using the ground truth of $Z^{\left\langle t+\text{$\Delta$}\right\rangle}$ we can rewrite the above expression as:

where $1\{\ldots\}$ is the indicator function indicating from which Gaussian each sample had come. Plugging in the definition of a multivariate Gaussian distribution and assuming $X^{\left\langle t+\text{$\Delta$}\right\rangle}$ and $Y^{\left\langle t+\text{$\Delta$}\right\rangle}$ to be independent we get:

with constant $\textrm{c}\textrm{=}\log 2\pi$. The final cost function is summed over all time steps $t$:

where $T_{\textrm{max}}$ is the number of time steps.

We have to choose the latent variable $z^{\left\langle t+\text{$\Delta$}\right\rangle}$ in a way, such that its ground truth value can be obtained beforehand. To this end we subdivide the static map into $N\times N$ subregions and assign an index $j=1,\text{\ldots},N^{2}$ to each of the subregions, as depicted in Fig. 5.

We can now easily determine in which of the $N^{2}$ different subregions the ground truth future position $\boldsymbol{x}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ is located. Choosing the subregion $j$ as our latent variable hence fulfills the required criteria.

Using the above definition of the latent variable let us dissect equation (7) to gain more insight. We will start with the first term:

which is recognized as the cross-entropy loss in a multiclass setting with $k=N^{2}$ classes. Hence our choice of the latent variable leads to a cost function which contains a classification problem.

The cross-entropy loss is used in neural networks which have softmax activations in the output layer. Hence we will realize $p(j\ |\ \mathcal{\mathscr{T}}^{\left\langle 0:t\right\rangle\left(i\right)},\ \mathcal{\mathscr{M}}^{\left\langle t\right\rangle\left(i\right)};\ \theta_{2})$ by a neural network with inputs $\mathcal{\mathscr{T}}^{\left\langle 0:t\right\rangle\left(i\right)}$ and $\mathscr{M}^{\left\langle t\right\rangle\left(i\right)}$ and a softmax activation at the output layer. Please remember that this part of the neural network outputs the probability distribution of the discrete latent variable $Z^{\left\langle t+\text{$\Delta$}\right\rangle}$, which is characterized by $k=N^{2}$ probability values $\phi_{j}$ satisfying $\phi_{j}\geq 0$ and $\sum_{i=1}^{k}\phi_{j}=1$. In the following we will hence denote the output of this network as $\phi_{j}^{\left\langle t+\text{$\Delta$}\right\rangle}$.

Each value $j$ which the latent variable $z^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ can assume, corresponds to a different spatial region. As has been shown in section. 1, the future positions of the ego vehicle are unequally distributed, see Fig. 1, which makes (8) a highly imbalanced classification problem. However, since we are in the classification domain, we have powerful tools to combat this issue, e.g. focal loss [14].

Let us come back to the first part of the cost function:

First of all please note that by design the ground truth value $\boldsymbol{x}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ is contained in the subregion indicated by $z^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$, see Fig. 5. Because of the indicator function in (9), for each sample $i$ only one Gaussian component $\boldsymbol{\mu}_{j}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$, $\boldsymbol{\sigma}_{j}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ is active, with the same index as the selected subregion $j=z^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$. During training, the Gaussian components will therefore learn to correspond to different subregions of the static map.

In order to minimize the above expression, the means $\boldsymbol{\mu}_{j}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ of the Gaussian components must come as close as possible to $\boldsymbol{x}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ , hence (9) can be interpreted as a (weighted) regression problem with $k=N^{2}$ regressors covering different spatial positions. The sigma terms $\boldsymbol{\sigma}_{j}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ are capturing how much noise there is in the outputs [12]. Since each regressor is covering different spatial positions, this helps to combat the imbalanced data problem even further.

In the next step, we will simplify the regression problem, thus emphasizing the classification task even further. Please note, that the classification part (8) is already predicting the future position of the ego vehicle, however quantized to the resolution of the subdivision. We can hence design the regression part (9) only to refine that prediction by providing an offset to the actual position. To this end, we define a grid consisting of the center positions of each subregion: $(\textrm{center}_{x,j},\textrm{center}_{y,j})$, with $j=1,\ldots,N^{2}$. Now we can substitute in (9):

This further reduces the complexity of the regression problem, since it has to predict only offsets, see Fig. 6.

At each time step $t$ the neural network outputs the parameters of a Gaussian mixture model:

This model estimates the probability of the vehicle of being at position $\boldsymbol{X}^{\left\langle t+\text{$\Delta$}\right\rangle}$ in $\Delta$ time steps:

given the input trajectory $\mathscr{T}^{\left\langle 0:t\right\rangle}$ and the static map $\mathscr{M}^{\left\langle t\right\rangle}$. We can visualize (11) by plugging values for $\boldsymbol{X}^{\left\langle t+\text{$\Delta$}\right\rangle}$ located on a dense uniform grid, see Fig. 7. The position of the vehicle at the current step is in the middle bottom position (green point). The ground truth position and orientation of the ego-vehicle in 2000 ms is shown as the blue arrow.

In many applications, however, instead of a heatmap we want to have a list containing the most probable future positions. The list should contain only distinct positions, thus reflecting the multimodality of the future.

As can be seen in Fig. 7 (a), the $N^{2}$ predictions made by our approach tend to form distinct clusters. Out of each cluster we want to keep only the most probable prediction and get rid of all nearly identical ones. Non-maximum suppression (NMS) is a way to make sure that each position is detected only once [2]. To easily apply the standard NMS, we will interpret the $\boldsymbol{\mu}_{j}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ as the centers of bounding boxes and choose the bounding box sizes to be proportional to $\boldsymbol{\sigma}_{j}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$ . The confidence required by NMS is obtained by using the fact that by design the Gaussian components are mostly bound to their corresponding subregions, yielding:

which gives the probability that the ego vehicle will be in subregion $j$ in $\Delta$ time steps. Please note that the number of predictions returned by NMS is changing, however at least one prediction is returned. We denote the distinct predictions as $\boldsymbol{\mu}_{k,\mathrm{nms}}^{\left\langle t+\text{$\Delta$}\right\rangle\left(i\right)}$, $k=1,\ldots,\mathrm{max}$, where $k=1$ is the most probable prediction, $k=2$ is the second most probable prediction and so on.

In this section we describe a basic architecture needed for realizing the model described in the previous sections. As described in section 3.1, the input of the network consists both of the sequence describing the dynamics of the vehicle $\{(\text{$\Delta$}x^{\left\langle t\right\rangle},\ \text{$\Delta$}y^{\left\langle t\right\rangle},\ v^{\left\langle t\right\rangle},\ h^{\left\langle t\right\rangle})\}_{t=0}^{T_{\mathrm{max}}}$ and the sequence of static maps $\{\mathcal{\mathscr{M}}^{\left\langle t\right\rangle}\}_{t=0}^{T_{\mathrm{max}}}$.

In order to generate sensible predictions, we need to aggregate the past values of $\text{$\Delta$}x^{\left\langle t\right\rangle},\ \text{$\Delta$}y^{\left\langle t\right\rangle},\ v^{\left\langle t\right\rangle},\ h^{\left\langle t\right\rangle}$. Hence a natural choice for processing of the input data (describing the dynamics of the vehicle) is a recurrent neural network (RNN), as depicted in Fig. 8. In the actual implementation, a member of the broad family of RNN architectures e.g. Long Short Term Memory (LSTM) and Gated Recurrent Unit (GRU), as well as their variants, can be used. The low dimensional input vector can optionally be embedded into a higher dimensional space.

The static map is represented as spatial data and is condensed to a feature vector (representing semantical information of the static map) using a convolutional neural network (CNN). The static maps only act as constraints for possible predictions and hence do not require an aggregation of past values, see Fig. 8. To account for shifts and rotations of the static map arising from the use of VCS, a spatial transformer network can be utilized [11].

Both the output of the RNN and CNN are fed into the predictor, whose output are five spatial maps consisting of $N$x$N$ cells. Each spatial map represents a different component of the Gaussian mixture model: $\phi_{j}^{\left\langle t+\Delta\right\rangle}$, $\Delta\mu_{x,j}^{\left\langle t+\Delta\right\rangle}$, $\sigma_{x,j}^{\left\langle t+\text{$\Delta$}\right\rangle}$, $\text{$\Delta$}\mu_{y,j}^{\left\langle t+\text{$\Delta$}\right\rangle}$ and $\sigma_{y,j}^{\left\langle t+\text{$\Delta$}\right\rangle}$. In a subsequent postprocessing step we add a constant grid to the coordinate offsets (delta terms), as described in (10).

As mentioned before the context information we use, encompasses only static context consisting of centerlines and the driveable area. For this reason, only a simple architecture is required to achieve reasonable results. More complex tasks e.g. with agents interaction, will require more complicated architectures (feature pyramid networks, transposed convolution etc.).

The input to the CNN network consists of a spatial map of size 128x128x2. The centerlines and the driveable area are in separate feature maps. The CNN network depicted in Fig. 8 consists of 5 layers. Each convolution layer is followed by a maximum pooling layer to half the size of the feature map. We use “tanh” as activation function in all layers. The output of the CNN is flattened to generate a feature vector of size 256.

The input to the recurrent part of the architecture consists of 4 values: the deltas between the current position and the last position, velocity and the heading angle, as described in section. 3.1. Prior to feeding the input into the RNN, we use an embedding network consisting of two layers. Again we use the “tanh” activation function. The resulting embedding has a dimension of 16, see Tab. I (b). The concrete implementation of the RNN network consists of a LSTM with two layers, see Tab. I (c).

Finally, the output of the LSTM and CNN is concatenated to a vector of size 256+150 which is fed to the Upsampling Network to obtain the output maps. The upsamling layer is implemented as a stack of three dense layers. The concrete implementation of the Upsampling Network depicted in Fig. 8 consists of 3 layers, where the last layer is a linear layer. The output is reshaped to form spatial maps of size 10x10x5.