Data-Driven Protection Levels for Camera and 3D Map-based Safe Urban Localization

Aleatoric uncertainty refers to the uncertainty from noise present in the camera image measurements $I_{t}$ and the environment map $\mathcal{M}$, due to which a precise value of the position error $\Delta\mathbf{x}_{t}$ cannot be determined. Existing DNN-based localization approaches model the aleatoric uncertainty as a covariance matrix with only diagonal entries [18, 21, 22] or with both diagonal and off-diagonal terms [37, 38]. Similar to the existing approaches, we characterize the aleatoric uncertainty by using a DNN to model the covariance matrix $\Sigma_{t}$ associated with the position error $\Delta\mathbf{x}_{t}$. We consider both nonzero diagonal and off-diagonal terms in $\Sigma_{t}$ to model the correlation between $x,y$ and $z$-dimension uncertainties, such as along the ground plane.

Aleatoric uncertainty by itself does not accurately represent the uncertainty in determining the position error. This is because aleatoric uncertainty assumes that the noise present in training data also represents the noise in all future inputs and that the DNN approximation is error-free. These assumptions fail in scenarios when the input at evaluation time is different from the training data or when the input contains features that occur rarely in the real world [26]. Thus, relying purely on aleatoric uncertainty can lead to an overconfident estimates of the position error uncertainty [18].

Epistemic uncertainty relates to the inaccuracies in the model for determining the position error $\Delta\mathbf{x}_{t}$. In our approach, we characterize the epistemic uncertainty by leveraging a geometrical property of the position error $\Delta\mathbf{x}_{t}$, where for the same camera image $I_{t}$, $\Delta\mathbf{x}_{t}$ can be obtained by linearly combining the position error $\Delta\mathbf{x}^{\prime}_{t}$ computed for any candidate state $\mathbf{s}^{\prime}_{t}$ and the relative position of $\mathbf{s}^{\prime}_{t}$ from the state estimate $\mathbf{s}_{t}$ (Fig. 2). Hence, using known relative positions and orientations of $N_{C}$ candidate states $\{\mathbf{s}_{t}^{1},\ldots,\mathbf{s}_{t}^{N_{C}}\}$ from $\mathbf{s}_{t}$, we transform the different position errors $\{\Delta\mathbf{x}_{t}^{1},\ldots,\Delta\mathbf{x}_{t}^{N_{C}}\}$ determined for the candidate states into samples of the state estimate position error $\Delta\mathbf{x}_{t}$. The empirical distribution comprised of these position error samples characterizes the epistemic uncertainty in the position error estimated using the DNN.

A local representation of the 3D LiDAR map of the environment captures the environment information in the vicinity of the state estimate $\mathbf{s}_{t}$ at time $t$. By comparing the environment information captured in the local map with the camera image $I_{t}\in\mathbb{R}^{l\times w\times 3}$ using a DNN, we estimate the position error $\Delta\mathbf{x}_{t}$ and covariance $\Sigma_{t}$ in the state estimate $\mathbf{s}_{t}$. For computing the local maps, we utilize the LiDAR-image generation procedure described in [15]. Similar to their approach, we generate the local map $L(\mathbf{s},\mathcal{M})\in\mathbb{R}^{l\times w}$ associated with a vehicle state $\mathbf{s}$ and LiDAR environment map $\mathcal{M}$ in two steps.

1. 1.

   First, we determine the rigid-body transformation matrix $H_{\mathbf{s}}$ in the special Euclidean group $\textrm{SE}(3)$ corresponding to the vehicle state $\mathbf{s}$

   $$H_{\mathbf{s}}=\left[\begin{matrix}R_{\mathbf{s}}&T_{\mathbf{s}}\\ \mathbf{0}_{1\times 3}&1\end{matrix}\right]\in\textrm{SE}(3),$$

   (2)

   where

   1. –

      $R_{\mathbf{s}}$ denotes the rotation matrix corresponding to the orientation quaternion elements $\mathbf{o}=[o_{1},o_{2},o_{3},o_{4}]$ in the state $\mathbf{s}$

   2. –

      $T_{\mathbf{s}}$ denotes the translation vector corresponding to the position elements $\mathbf{x}=[x,y,z]$ in the state $\mathbf{s}$.

   Using the matrix $H_{\mathbf{s}}$, we rotate and translate the points in the map $\mathcal{M}$ to the map $\mathcal{M}_{\mathbf{s}}$ in the reference frame of the state $\mathbf{s}$

   $$\mathcal{M}_{\mathbf{s}}=\{\left[\begin{matrix}I_{3\times 3}&\mathbf{0}_{3\times 1}\end{matrix}\right]\cdot H_{\mathbf{s}}\cdot\left[\begin{matrix}\mathbf{p}^{\top}&1\end{matrix}\right]^{\top}\mid\mathbf{p}\in\mathcal{M}\},$$

   (3)

   where $I$ denotes the identity matrix.

   For maintaining computational efficiency in the case of large maps, we use the points in the LiDAR map $\mathcal{M}_{\mathbf{s}}$ that lie in a sub-region around the state $\mathbf{s}$ and in the direction of the vehicle orientation.

2. 2.

   In the second step, we apply the occlusion estimation filter presented in [39] to identify and remove occluded points along rays from the camera center. For each pair of points $(\mathbf{p}^{(i)},\mathbf{p}^{(j)})$ where $\mathbf{p}^{(i)}$ is closer to the state $\mathbf{s}$, $\mathbf{p}^{(j)}$ is marked occluded if the angle between the ray from $\mathbf{p}^{(j)}$ to the camera center and the line from $\mathbf{p}^{(j)}$ to $\mathbf{p}^{(i)}$ is less than a threshold. Then, the remaining points are projected to the camera image frame using the camera projection matrix $K$ to generate the local depth map $L(\mathbf{s},\mathcal{M})$. The $i$th point $\mathbf{p}^{(i)}$ in $\mathcal{M}_{\mathbf{s}}$ is projected as

   	$\displaystyle[\begin{matrix}p_{x}&p_{y}&c\end{matrix}]^{\top}$	$\displaystyle=K\cdot\mathbf{p}^{(i)}$
   	$\displaystyle[L(\mathbf{s},\mathcal{M})]_{(\lceil p_{x}/c\rceil,\lceil p_{y}/c\rceil)}$	$\displaystyle=[\begin{matrix}0&0&1\end{matrix}]\cdot\mathbf{p}^{(i)},$		(4)

   where

   1. –

      $p_{x},p_{y}$ denote the projected 2D coordinates with scaling term $c$

   2. –

      $[L(\mathbf{s},\mathcal{M})]_{(p_{x},p_{y})}$ denotes the $(p_{x},p_{y})$ pixel position in the local map $L(\mathbf{s},\mathcal{M})$.

The local depth map $L(\mathbf{s},\mathcal{M})$ for state $\mathbf{s}$ visualizes the environment features that are expected to be captured in a camera image obtained from the state $\mathbf{s}$. However, the obtained camera image $I_{t}$ is associated with the true state $\mathbf{s}^{*}_{t}$ that might be different from the state estimate $\mathbf{s}_{t}$. Nevertheless, for reasonably small position and orientation differences between the state estimate $\mathbf{s}_{t}$ and true state $\mathbf{s}^{*}_{t}$, the local map $L(\mathbf{s},\mathcal{M})$ contains features that correspond with some of the features in the camera image $I_{t}$ that we use to estimate the position error.

We use a DNN to estimate the position error $\Delta\mathbf{x}_{t}$ and associated covariance matrix $\Sigma_{t}$ by implicitly identifying and comparing the positions of corresponding features in camera image $I_{t}$ and the local depth map $L(\mathbf{s}_{t},\mathcal{M})$ associated with the state estimate $\mathbf{s}_{t}$.

The architecture of our DNN is given in Fig. 3. Our DNN comprises of two separate modules, one for estimating the position error $\Delta\mathbf{x}_{t}$ and other for the parameters of the covariance matrix $\Sigma_{t}$. The first module for estimating the position error $\Delta\mathbf{x}_{t}$ is based on CMRNet [15]. CMRNet was originally proposed as an algorithm to iteratively determine the position and orientation of a vehicle using a camera image and 3D LiDAR map, starting from a provided initial state. For determining position error $\Delta\mathbf{x}_{t}$ using CMRNet, we use the state estimate $\mathbf{s}_{t}$ as the provided initial state and the corresponding DNN translation $\Delta\tilde{\mathbf{x}}_{t}$ and rotation $\Delta\tilde{\mathbf{r}}$ error output for transforming the state $\mathbf{s}_{t}$ towards the true state $\mathbf{s}^{*}_{t}$. Formally, given any state $\mathbf{s}$ and camera image $I_{t}$ at time $t$, the translation error $\Delta\tilde{\mathbf{x}}$ and rotation error $\Delta\tilde{\mathbf{r}}$ are expressed as

$$\Delta\tilde{\mathbf{x}},\Delta\tilde{\mathbf{r}}=\textrm{CMRNet}(I_{t},L(\mathbf{s},\mathcal{M})).$$

(5)

CMRNet estimates the rotation error $\Delta\tilde{\mathbf{r}}$ as a unit quaternion. Furthermore, the architecture determines both the translation error $\Delta\tilde{\mathbf{x}}$ and rotation error $\Delta\tilde{\mathbf{r}}$ in the reference frame of the state $\mathbf{s}$. Since the protection levels depend on the position error $\Delta\mathbf{x}$ in the reference frame from which the camera image $I_{t}$ is captured (the vehicle reference frame), we transform the translation error $\Delta\tilde{\mathbf{x}}$ to the vehicle reference frame by rotating it with the inverse of $\Delta\tilde{\mathbf{r}}$

$$\Delta\mathbf{x}=-\tilde{R}^{\top}\cdot\Delta\tilde{\mathbf{x}},$$

(6)

where $\tilde{R}$ is the $3\times 3$ rotation matrix corresponding to the rotation error quaternion $\Delta\tilde{\mathbf{r}}$.

In the second module, we determine the covariance matrix $\Sigma$ associated with $\Delta\mathbf{x}$ by first estimating the covariance matrix $\tilde{\Sigma}$ associated with the translation error $\Delta\tilde{\mathbf{x}}$ obtained from CMRNet and then transforming it to the vehicle reference frame using $\Delta\tilde{\mathbf{r}}$.

We model the covariance matrix $\tilde{\Sigma}$ by following a similar approach to [37]. Since the covariance matrix is both symmetric and positive-definite, we consider the decomposition of $\tilde{\Sigma}$ into diagonal standard deviations $\boldsymbol{\sigma}=[\sigma_{1},\sigma_{2},\sigma_{3}]$ and correlation coefficients $\boldsymbol{\eta}=[\eta_{21},\eta_{31},\eta_{32}]$

	$\displaystyle[\tilde{\Sigma}]_{ii}$	$\displaystyle=\sigma_{i}^{2}$
	$\displaystyle[\tilde{\Sigma}]_{ij}$	$\displaystyle=[\Sigma]_{ji}=\eta_{ij}\sigma_{i}\sigma_{j},$		(7)

where $i,j\in\{1,2,3\}$ and $j<i$. We estimate these terms using our second DNN module (referred to as CovarianceNet) which has a similar network structure as CMRNet, but with $256$ and $6$ artificial neurons in the last two fully connected layers to prevent overfitting. For stable training, CovarianceNet produces logarithm of the standard deviation output, which is converted to the standard deviation by then taking the exponent. Additionally, we use tanh function to scale the correlation coefficient outputs $\boldsymbol{\eta}$ in CovarianceNet between $\pm 1$. Formally, given a vehicle state $\mathbf{s}$ and camera image $I_{t}$ at time $t$, the standard deviation $\boldsymbol{\sigma}$ and correlation coefficients $\boldsymbol{\eta}$ is approximated as

$$\boldsymbol{\sigma},\boldsymbol{\eta}=\textrm{CovarianceNet}(I_{t},L(\mathbf{s},\mathcal{M})).$$

(8)

Using the constructed $\tilde{\Sigma}$ from the obtained $\boldsymbol{\sigma},\boldsymbol{\eta}$, we obtain the covariance matrix $\Sigma$ associated with $\Delta\mathbf{x}$ as

$$\Sigma=\tilde{R}^{\top}\cdot\tilde{\Sigma}\cdot\tilde{R}.$$

(9)

We keep the aleatoric uncertainty restricted to position domain errors in this work for simplicity, and thus treat $\Delta\tilde{\mathbf{r}}$ as a point estimate. The impact of errors in estimating $\Delta\tilde{\mathbf{r}}$ on protection levels is taken into consideration as epistemic uncertainty, and is discussed in more detail in Section V.5 and V.7.

The feature extraction modules in CovarianceNet and CMRNet are separate since the two tasks are complementary: for estimating position error, the DNN must learn features that are robust to noise in the inputs while the variance in the estimated position error depends on the noise itself.

The loss function for training the DNN must penalize position error outputs that differ from the corresponding ground truth present in the dataset, as well as penalize covariance that overestimates or underestimates the uncertainty in the position error predictions. Furthermore, the loss muss incentivize the DNN to extract useful features from the camera image and local map inputs for predicting the position error. Hence, we consider three additive components in our loss function $\mathcal{L}(\cdot)$

$$\mathcal{L}=\alpha_{\textrm{Huber}}\mathcal{L}_{\textrm{Huber}}(\Delta\tilde{\mathbf{x}}^{*},\Delta\tilde{\mathbf{x}})+\alpha_{\textrm{MLE}}\mathcal{L}_{\textrm{MLE}}(\Delta\tilde{\mathbf{x}}^{*},\Delta\tilde{\mathbf{x}},\tilde{\Sigma})+\alpha_{\textrm{Ang}}\mathcal{L}_{\textrm{Ang}}(\Delta\tilde{\mathbf{r}}^{*},\Delta\tilde{\mathbf{r}}),$$

(10)

where

1. –

   $\Delta\tilde{\mathbf{x}}^{*},\Delta\tilde{\mathbf{r}}^{*}$ denotes the vector-valued translation and rotation error in the reference frame of the state estimate $\mathbf{s}$ to the unknown true state $\mathbf{s}^{*}$

2. –

   $\mathcal{L}_{\textrm{Huber}}(\cdot)$ denotes the Huber loss function [41]

3. –

   $\mathcal{L}_{\textrm{MLE}}(\cdot)$ denotes the loss function for the maximum likelihood estimation of position error $\Delta\mathbf{x}$ and covariance $\tilde{\Sigma}$

4. –

   $\mathcal{L}_{\textrm{Ang}}(\cdot)$ denotes the quaternion angular distance from [15]

5. –

   $\alpha_{\textrm{Huber}},\alpha_{\textrm{MLE}},\alpha_{\textrm{Ang}}$ are coefficients for weighting each loss term.

We employ the Huber loss $\mathcal{L}_{\textrm{Huber}}(\cdot)$ and quaternion angular distance $\mathcal{L}_{\textrm{Ang}}(\cdot)$ terms from [15]. The Huber loss term $\mathcal{L}_{\textrm{Huber}}(\cdot)$ penalizes the translation error output $\Delta\tilde{\mathbf{x}}$ of the DNN

	$\displaystyle\mathcal{L}_{\textrm{Huber}}(\Delta\tilde{\mathbf{x}}^{*},\Delta\tilde{\mathbf{x}})$	$\displaystyle=\sum_{X=x,y,z}D_{\textrm{Huber}}(\Delta\tilde{X}^{*},\Delta\tilde{X})$
	$\displaystyle D_{\textrm{Huber}}(a^{*},a)$	$\displaystyle=\begin{cases}\frac{1}{2}(a-a^{*})^{2}&\textrm{for }|a-a^{*}|\leq\delta\\ \delta\cdot(|a-a^{*}|-\frac{1}{2}\delta)&\textrm{otherwise}\end{cases},$		(11)

where $\delta$ is a hyperparameter for adjusting the penalty assignment to small error values. In this paper, we set $\delta=1$. Unlike the more common mean squared error, the penalty assigned to higher error values is linear in Huber loss instead of quadratic. Thus, Huber loss is more robust to outliers and leads to more stable training as compared with squared error. The quaternion angular distance term $\mathcal{L}_{\textrm{Ang}}(\cdot)$ penalizes the rotation error output $\Delta\tilde{\mathbf{r}}$ from CMRNet

	$\displaystyle\mathcal{L}_{\textrm{Ang}}(\Delta\tilde{\mathbf{r}}^{*},\Delta\tilde{\mathbf{r}})$	$\displaystyle=D_{\textrm{Ang}}(\Delta\tilde{\mathbf{r}}^{*}\times\Delta\tilde{\mathbf{r}}^{-1})$
	$\displaystyle D_{\textrm{Ang}}(\mathbf{q})$	$\displaystyle=\operatorname{atan2}2\left(\sqrt{q_{2}^{2}+q_{3}^{2}+q_{4}^{2}},|q_{1}|\right),$		(12)

where

1. –

   $q_{i}$ denotes the $i$th element in quaternion $\mathbf{q}$

2. –

   $\Delta\mathbf{r}^{-1}$ denotes the inverse of the quaternion $\Delta\mathbf{r}$

3. –

   $\mathbf{q}\times\mathbf{r}$ here denotes element-wise multiplication of the quaternions $\mathbf{q}$ and $\mathbf{r}$

4. –

   $\operatorname{atan2}2(\cdot)$ is the two-argument version of the arctangent function.

Including the quaternion angular distance term $\mathcal{L}_{\textrm{Ang}}(\cdot)$ in the loss function incentivizes the DNN to learn features that are relevant to the geometry between the camera image and the local depth map. Hence, it provides additional supervision to the DNN training as a multi-task objective [42], and is important for the stability and speed of the training process.

The maximum likelihood loss term $\mathcal{L}_{\textrm{MLE}}(\cdot)$ depends on both the translation error $\Delta\tilde{\mathbf{x}}$ and covariance matrix $\tilde{\Sigma}$ estimated from the DNN. The loss function is analogous to the negative log-likelihood of the Gaussian distribution

$$\mathcal{L}_{\textrm{MLE}}(\Delta\tilde{\mathbf{x}}^{*},\Delta\tilde{\mathbf{x}},\tilde{\Sigma})=\frac{1}{2}\log|\tilde{\Sigma}|+\frac{1}{2}(\Delta\tilde{\mathbf{x}}^{*}-\Delta\tilde{\mathbf{x}})^{\top}\cdot\tilde{\Sigma}^{-1}\cdot(\Delta\tilde{\mathbf{x}}^{*}-\Delta\tilde{\mathbf{x}})$$

(13)

If the covariance output from the DNN has small values, the corresponding translation error is penalized much more than the translation error corresponding to a large valued covariance. Hence, the maximum likelihood loss term $\mathcal{L}_{\textrm{MLE}}(\cdot)$ incentivizes the DNN to output small covariance only when the corresponding translation error output has high confidence, and otherwise output large covariance.

To assess the uncertainty in the DNN-based position error estimation process as well as the uncertainty from environmental factors, we evaluate the DNN output at $N_{C}$ candidate states $\{\mathbf{s}^{1}_{t}\ldots,\mathbf{s}^{N_{C}}_{t}\}$ in the neighborhood of the state estimate $\mathbf{s}_{t}$.

For selecting the candidate states $\{\mathbf{s}^{1}_{t}\ldots,\mathbf{s}^{N_{C}}_{t}\}$, we randomly generate multiple values of translation offset $\{\mathbf{t}^{1},\ldots,\mathbf{t}^{N_{C}}\}$ and rotation offset $\{\mathbf{r}^{1},\ldots,\mathbf{r}^{N_{C}}\}$ about the state estimate $\mathbf{s}_{t}$, where $N_{C}$ is the total number of selected candidate states. The $i$th translation offset $\mathbf{t}^{i}\in\mathbb{R}^{3}$ denotes translation in $x,y$ and $z$ dimensions and is sampled from a uniform probability distribution between a specified range $\pm t_{max}$ in each dimension. Similarly, the $i$th rotation offset $\mathbf{r}^{i}\in\textrm{SU}(2)$ is obtained by uniformly sampling between $\pm r_{max}$ angular deviations about each axis and converting the resulting rotation to a quaternion. The $i$th candidate state $\mathbf{s}^{i}_{t}$ is generated by rotating and translating the state estimate $\mathbf{s}_{t}$ by $\mathbf{r}^{i}$ and $\mathbf{t}^{i}$, respectively. Corresponding to each candidate state $\mathbf{s}^{i}_{t}$, we generate a local depth map $L(\mathbf{s}^{i}_{t},\mathcal{M})$ using the procedure laid out in Section V.1.

Using each local depth map $L(\mathbf{s}^{i}_{t},\mathcal{M})$ and camera image $I_{t}$ for the $i$th candidate state $\mathbf{s}^{i}_{t}$ as inputs to the DNN in Section V.2, we evaluate the candidate state position error $\Delta\mathbf{x}^{i}_{t}$ and covariance matrix $\Sigma^{i}_{t}$. From the known translation offset $\mathbf{t}^{i}$ between the candidate state $\mathbf{s}^{i}_{t}$ and the state estimate $\mathbf{s}_{t}$ and the DNN-based rotation error $\Delta\tilde{\mathbf{r}}_{t}$ in $\mathbf{s}_{t}$, we compute the transformation matrix $H_{\mathbf{s}^{i}_{t}\to\mathbf{s}_{t}}$ for converting the candidate state position error $\Delta\mathbf{x}^{i}_{t}$ to the state estimate position error $\Delta\mathbf{x}_{t}$ in the vehicle reference frame

$$H_{\mathbf{s}^{i}_{t}\to\mathbf{s}_{t}}=\left[\begin{matrix}I_{3\times 3}&-\tilde{R}_{t}^{\top}\mathbf{t}^{i}\end{matrix}\right],$$

(14)

where $I_{3\times 3}$ denotes the identity matrix and $\tilde{R}_{t}$ is the $3\times 3$ rotation matrix computed from the DNN-based rotation error $\Delta\tilde{\mathbf{r}}_{t}$ between the state estimate $\mathbf{s}_{t}$ and the unknown true state $\mathbf{s}^{*}_{t}$. Note that the rotation offset $\mathbf{r}^{i}$ is not used in the transformation, since we are only concerned with the position errors from the true state $\mathbf{s}^{*}_{t}$ to the state estimate $\mathbf{s}_{t}$, which are invariant to the orientation of the candidate state $\mathbf{s}^{i}_{t}$. Using the transformation matrix $H_{\mathbf{s}^{i}_{t}\to\mathbf{s}_{t}}$, we obtain the $i$th sample of the state estimate position error $\Delta\mathbf{x}_{t}^{(i)}$

$$\Delta\mathbf{x}_{t}^{(i)}=H_{\mathbf{s}^{i}_{t}\to\mathbf{s}_{t}}\cdot[\begin{matrix}\Delta\mathbf{x}^{i}_{t}&1\end{matrix}]^{\top}=\Delta\mathbf{x}^{i}_{t}-\tilde{R}_{t}^{\top}\mathbf{t}^{i}.$$

(15)

We use parentheses in the notation $\Delta\mathbf{x}_{t}^{(i)}$ for the transformed samples of the position error between the true state $\mathbf{s}^{*}_{t}$ and the state estimate $\mathbf{s}_{t}$ to differentiate from the position error $\Delta\mathbf{x}^{i}_{t}$ between $\mathbf{s}^{*}_{t}$ and the candidate state $\mathbf{s}^{i}_{t}$. Next, we modify the candidate state covariance matrix $\Sigma^{i}_{t}$ to account for uncertainty in DNN-based rotation error $\Delta\tilde{\mathbf{r}}_{t}$. The resulting covariance matrix $\Sigma^{(i)}_{t}$ in terms of the covariance matrix $\Sigma^{i}_{t}$ for $\Delta\mathbf{x}^{i}_{t}$, $\tilde{R}_{t}$ and $\mathbf{t}^{i}$ is

$$\Sigma^{(i)}_{t}=\Sigma^{i}_{t}+\textrm{Var}[\tilde{R}_{t}^{\top}\mathbf{t}^{i}].$$

(16)

Assuming small errors in determining the true rotation offsets between state estimate $\mathbf{s}_{t}$ and the true state $\mathbf{s}^{*}_{t}$, we consider the random variable $R^{\prime}\tilde{R}_{t}^{\top}\mathbf{t}^{i}$ where $R^{\prime}$ represents the random rotation matrix corresponding to small angular deviations [43]. Using $R^{\prime}\tilde{R}_{t}^{\top}\mathbf{t}^{i}$, we approximate the covariance matrix $\Sigma^{(i)}_{t}$ as

	$\displaystyle\Sigma^{(i)}_{t}$	$\displaystyle\approx\Sigma^{i}_{t}+\mathbb{E}[(R^{\prime}-I)(\tilde{R}_{t}^{\top}\mathbf{t}^{i})(\tilde{R}_{t}^{\top}\mathbf{t}^{i})^{\top}(R^{\prime}-I)^{\top}]$
	$\displaystyle[\Sigma^{(i)}_{t}]_{i^{\prime}j^{\prime}}$	$\displaystyle\approx[\Sigma^{i}_{t}]_{i^{\prime}j^{\prime}}+\mathbb{E}[(\mathbf{r}^{\prime}_{i^{\prime}})^{\top}(\tilde{R}_{t}^{\top}\mathbf{t}^{i})(\tilde{R}_{t}^{\top}\mathbf{t}^{i})^{\top}(\mathbf{r}^{\prime}_{j^{\prime}})]$
		$\displaystyle=[\Sigma^{i}_{t}]_{i^{\prime}j^{\prime}}+\mathrm{Tr}\left((\tilde{R}_{t}^{\top}\mathbf{t}^{i})(\tilde{R}_{t}^{\top}\mathbf{t}^{i})^{\top}\mathbb{E}[(\mathbf{r}^{\prime}_{i^{\prime}})(\mathbf{r}^{\prime}_{j^{\prime}})^{\top}]\right)$
		$\displaystyle=[\Sigma^{i}_{t}]_{i^{\prime}j^{\prime}}+\mathrm{Tr}\left((\tilde{R}_{t}^{\top}\mathbf{t}^{i})(\tilde{R}_{t}^{\top}\mathbf{t}^{i})^{\top}Q_{i^{\prime}j^{\prime}}\right),$		(17)

where $(\mathbf{r}^{\prime}_{i})^{\top}$ represents the $i$th row vector in $R^{\prime}-I$. Since errors in $\tilde{R}$ depend on the DNN output, we specify $R^{\prime}$ through the empirical distribution of the angular deviations in $\tilde{R}$ as observed for the trained DNN on the training and validation data, and precompute the expectation $Q_{i^{\prime}j^{\prime}}$ for each $(i^{\prime},j^{\prime})$ pair.

The samples of state estimate position error $\{\Delta\mathbf{x}_{t}^{(1)},\ldots,\Delta\mathbf{x}_{t}^{(N_{C})}\}$ represent both inaccuracy in the DNN estimation as well as uncertainties due to environmental factors. If the DNN approximation fails at the input corresponding to the state estimate $\mathbf{s}_{t}$, the estimated position errors at candidate states would lead to a wide range of different values for the state estimate position errors. Similarly, if the environment map $\mathcal{M}$ near the state estimate $\mathbf{s}_{t}$ contains repetitive features, the position errors computed from candidate states would be different and hence indicate high uncertainty.

Since the candidate states $\{\mathbf{s}^{1}_{t}\ldots,\mathbf{s}^{N_{C}}_{t}\}$ are selected randomly, some position error samples may correspond to the local depth map and camera image pairs for which the DNN performs poorly. Thus, we compute outlier weights $\{\mathbf{w}^{(1)}_{t},\ldots,\mathbf{w}^{(N_{C})}_{t}\}$ corresponding to the position error samples $\{\Delta\mathbf{x}_{t}^{(1)},\ldots,\Delta\mathbf{x}_{t}^{(N_{C})}\}$ to mitigate the effect of these erroneous position error values in determining the protection levels. We compute outlier weights in each of the $x,y,$ and $z$-dimensions separately, since the DNN approximation might not necessarily fail in all of its outputs. An example of this scenario is when the input camera image and local map contain features such as building edges that can be used to robustly determine errors along certain directions but not others.

For computing the outlier weights $\mathbf{w}_{t}^{(i)}=[w^{(i)}_{x,t},w^{(i)}_{y,t},w^{(i)}_{z,t}]$ associated with the $i$th position error value $\Delta\mathbf{x}_{t}^{(i)}=[\Delta x_{t}^{(i)},\Delta y_{t}^{(i)},\Delta z_{t}^{(i)}]$, we employ the robust Z-score based outlier detection technique [31]. The robust Z-score is used in a variety of anomaly detection approaches due to its resilience to outliers [44]. We apply the following operations in each dimension $X=x,y,$ and $z$:

1. 1.

   We compute the Median Absolute Deviation statistic [31] ${M\negthinspace AD}_{X}$ using all position error values $\{\Delta X_{t}^{(1)},\ldots,\Delta X_{t}^{(N_{C})}\}$

   $${M\negthinspace AD}_{X}=\operatorname{median}(|\Delta X_{t}^{(i)}-\operatorname{median}(\Delta X_{t}^{(i)})|).$$

   (18)

2. 2.

   Using the statistic ${M\negthinspace AD}_{X}$, we compute the robust Z-score $\mathcal{Z}^{(i)}_{X}$ for each position error value $\Delta X_{t}^{(i)}$

   $$\mathcal{Z}^{(i)}_{X}=\frac{|\Delta X_{t}^{(i)}-\operatorname{median}(\Delta X_{t}^{(i)})|}{{M\negthinspace AD}_{X}}.$$

   (19)

   The robust Z-score $\mathcal{Z}^{(i)}_{X}$ is high if the position error $\Delta\mathbf{x}^{(i)}$ deviates from the median error with a large value when compared with the median deviation value.

3. 3.

   We compute the outlier weights $\{w^{(1)}_{X},\ldots,w^{(N_{C})}_{X}\}$ from the robust Z-scores $\{\mathcal{Z}^{(1)}_{X},\ldots,\mathcal{Z}^{(N_{C})}_{X}\}$ by applying the softmax operation [45] such that the sum of weights is unity

   $$w^{(i)}_{X,t}=\frac{e^{-\gamma\cdot\mathcal{Z}^{(i)}_{X}}}{\sum_{j=1}^{N_{C}}e^{-\gamma\cdot\mathcal{Z}^{(j)}_{X}}},$$

   (20)

   where $\gamma$ denotes the scaling coefficient in the softmax function. We set $\gamma=0.6745$ as the approximate inverse of the standard normal distribution evaluated at $3/4$ to make the scaling in the statistic consistent with the standard deviation of a normal distribution [31]. A small value of outlier weight $w^{(i)}_{X,t}$ indicates that the position error $\Delta X_{t}^{(i)}$ is an outlier.

For brevity, we extract the diagonal variances associated with each dimension for all position error samples

	$\displaystyle(\sigma^{2}_{x,t})^{(i)}$	$\displaystyle=[\Sigma^{(i)}_{t}]_{11}$
	$\displaystyle(\sigma^{2}_{y,t})^{(i)}$	$\displaystyle=[\Sigma^{(i)}_{t}]_{22}$
	$\displaystyle(\sigma^{2}_{z,t})^{(i)}$	$\displaystyle=[\Sigma^{(i)}_{t}]_{33}.$		(21)

We construct a probability distribution in each of the $X=x,y$ and $z$-dimensions from the previously obtained samples of position errors $\Delta X^{(i)}_{t}$, variances $(\sigma^{2}_{X,t})^{(i)}$ and outlier weights $w^{(i)}_{X,t}$. We model the probability distribution using the Gaussian Mixture Model (GMM) distribution [32]

$\displaystyle\mathbb{P}(\rho_{X,t})$

$\displaystyle=\sum_{i=1}^{N_{C}}w^{(i)}_{X,t}\mathcal{N}\left(\Delta X_{t}^{(i)},(\sigma^{2}_{X,t})^{(i)}\right),$

(22)

where

1. –

   $\rho_{X,t}$ denotes the position error random variable

2. –

   $\mathcal{N}(\mu,\sigma^{2})$ is the Gaussian distribution with mean $\mu$ and variance $\sigma^{2}$.

The probability distributions $\mathbb{P}(\rho_{x,t})$, $\mathbb{P}(\rho_{y,t})$ and $\mathbb{P}(\rho_{z,t})$ incorporate both aleatoric uncertainty from the DNN-based covariance and epistemic uncertainty from the multiple DNN evaluations associated with different candidate states. Both the position error and covariance matrix depend on the rotation error point estimate from CMRNet for transforming the error values to the vehicle reference frame. Since each DNN evaluation for a candidate state estimates the rotation error independently, the epistemic uncertainty incorporates the effects of errors in DNN-based estimation of both rotation and translation. The epistemic uncertainty is reflected in the multiple GMM components and their weight coefficients, which represent the different possible position error values that may arise from the same camera image measurement and the environment map. The aleatoric uncertainty is present as the variance in each possible value of the position error represented by the individual components.

We compute the protection levels along the lateral, longitudinal and vertical directions using the probability distributions obtained in the previous section. Since the position errors are in the vehicle reference frame, the $x,y$ and $z$-dimensions coincide with the lateral, longitudinal and the vertical directions, respectively. First, we obtain the cumulative distribution function $\textrm{CDF}(\cdot)$ for each probability distribution

$\displaystyle\textrm{CDF}(\rho_{X,t})$

$\displaystyle=\sum_{i=1}^{N_{C}}w^{(i)}_{X,t}\Phi\left(\frac{\rho_{X,t}-\Delta X_{t}^{(i)}}{(\sigma_{X,t})^{(i)}}\right)$

(23)

where $\Phi(\cdot)$ is the cumulative distribution function of the standard normal distribution. Then, for a specified value of the integrity risk $IR$, we compute the protection level $PL$ in lateral, longitudinal and vertical directions from equation 1 using the CDF as the probability distribution. For numerical optimization, we employ a simple interval halving method for line search or the bisection method [46]. To account for both positive and negative errors, we perform the optimization both using CDF (supremum) and $1-\textrm{CDF}$ (infemum) with $IR/2$ as the integrity risk and use the maximum absolute value as the protection level.

The computed protection levels consider heavy-tails in the GMM probability distribution of the position error that arise because of the different possible values of the position error that can be computed from the available camera measurements and environment map. Our method computes large protection levels when many different values of position error may be equally probable from the measurements, resulting in larger tail probabilities in the GMM, and small protection levels only if the uncertainty from both aleatoric and epistemic sources is small.

We use the KITTI visual odometry dataset [47] to evaluate the performance of the protection levels computed by our approach. The dataset was recorded around Karlsruhe, Germany over multiple driving sequences and contains images recorded by multiple on-board cameras, along with ground truth positions and orientations. Additionally, the dataset contains LiDAR point cloud measurements which we use to generate the environment map corresponding to each sequence. Since our approach for computing protection levels just requires a monocular camera sensor, we use the images recorded by the left RGB camera in our experiments. We use the sequences $00$, $03$, $05$, $06$, $07$, $08$ and $09$ from the dataset based on the availability of a LiDAR environment map. We use sequence $00$ for validation of our approach and the rest of the sequences are utilized in training our DNN. The experimental parameters are provided in Table 5.

To construct a precise LiDAR point cloud map $\mathcal{M}$ of the environment, we exploit the openly available position and orientation values for the dataset computed via Simultaneous Localization and Mapping [4]. Similar to [15], we aggregate the LiDAR point clouds across all time instances. Then, we detect and remove sparse outliers within the aggregated point cloud by computing Z-score [31] of each point in a $0.1$ m local neighborhood. We discarded the points which had a higher Z-score than $3$. Finally, the remaining points are down sampled into a voxel map of the environment $\mathcal{M}$ with resolution of $0.1$ m. The corresponding map for sequence 00 in the KITTI dataset is shown in Fig. 5. For storing large maps, we divide the LiDAR point cloud sequences into multiple overlapping parts and construct separate maps of roughly $500$ Megabytes each.

We generate the training dataset for our DNN in two steps. First, we randomly select a state estimate $s_{t}$ at time $t$ from within a $2$ m translation and a $10^{\circ}$ rotation of the ground truth positions and orientations in each driving sequence. The translation and rotation used for generating the state estimate is utilized as the ground truth position error $\Delta\mathbf{x}^{*}_{t}$ and orientation error $\Delta\mathbf{r}^{*}_{t}$. Then, using the LiDAR map $\mathcal{M}$, we generate the local depth map $L(\mathbf{s}_{t},\mathcal{M})$ corresponding to the state estimate $\mathbf{s}_{t}$ and use it as the DNN input along with the camera image $I_{t}$ from the driving sequence data. The training dataset comprises of camera images from $11455$ different time instances, with the state estimate selected at runtime so as to have different state estimates for the same camera images in different epochs.

Similar to the data augmentation techniques described in [15], we

1. 1.

   Randomly change contrast, saturation and brightness of images,

2. 2.

   Apply random rotations in the range of $\pm 5^{\circ}$ to both the camera images and local depth maps,

3. 3.

   Horizontally mirror the camera image and compute the local depth map using a modified camera projection matrix.

All three of these data augmentation techniques are used in training CMRNet in the first half of the optimization process. However, for training CovarianceNet, we skip the contrast, saturation and brightness changes during the second half of the optimization so that the DNN can learn real-world noise features from camera images.

We generate the validation and test datasets from sequence $00$ in the KITTI odometry dataset, which is not used for training. We follow a similar procedure as the one for generating the training dataset, except we do not augment the data. The validation dataset comprises of randomly selected $100$ time instances from sequence $00$, while the test dataset contains the remaining $4441$ time instances in sequence $00$.

We train the DNN using stochastic gradient descent. Directly optimizing via the maximum likelihood loss term $\mathcal{L}_{\textrm{MLE}}(\cdot)$ might suffer from instability caused by the interdependence between the translation error $\Delta\tilde{\mathbf{x}}$ and covariance $\tilde{\Sigma}$ outputs [48]. Therefore, we employ the mean-variance split training strategy proposed in [48]: First, we set $(\alpha_{\textrm{Huber}}=1,\alpha_{\textrm{MLE}}=1,\alpha_{\textrm{Ang}}=1)$ and only optimize the parameters of CMRNet till validation error stops decreasing. Next, we set $(\alpha_{\textrm{Huber}}=0,\alpha_{\textrm{MLE}}=1,\alpha_{\textrm{Ang}}=0)$ and optimize the parameters of CovarianceNet. We alternate between these two steps till validation loss stops decreasing. Our DNN is implemented using the PyTorch library [49] and takes advantage of the open-source implementation available for CMRNet [15] as well as the available pretrained weights for initialization. Similar to CMRNet, all the layers in our DNN use the leaky RELU activation function with a negative slope of $0.1$. We train the DNN on using a single NVIDIA Tesla P40 GPU with a batch size of $24$ and learning rate of $10^{-5}$ selected via grid search.

We evaluate the lateral, longitudinal and vertical protection levels computed using our approach using the following three metrics (with subscript $t$ dropped for brevity):

1. 1.

   Bound gap measures the difference between the computed protection levels $PL_{lat},PL_{lon},PL_{vert}$ and the true position error magnitude during nominal operations (protection level is less than the alarm limit and greater than the position error)

   	$\displaystyle BG_{lat}$	$\displaystyle=\textrm{avg}(PL_{lat}-|\Delta x^{*}|)$
   	$\displaystyle BG_{lon}$	$\displaystyle=\textrm{avg}(PL_{lon}-|\Delta y^{*}|)$
   	$\displaystyle BG_{vert}$	$\displaystyle=\textrm{avg}(PL_{vert}-|\Delta z^{*}|),$		(24)

   where

   1. –

      $BG_{lat},BG_{lon}$ and $BG_{vert}$ denote bound gaps in lateral, longitudinal and vertical dimensions respectively

   2. –

      $\textrm{avg}(\cdot)$ denotes the average computed over the test dataset for which the value of protection level is greater than position error and less than the alarm limit

   A small bound gap value $BG_{lat},BG_{lon},BG_{vert}$ is desirable since it implies that the algorithm both estimates the position error magnitude during nominal operations accurately and has low uncertainty in the prediction. We only consider the bound gap for nominal operations, since the estimated position is declared unsafe when the protection level exceeds the alarm limit.

2. 2.

   Failure rate measures the total fraction of time instances in the test data sequence for which the computed protection levels $PL_{lat},PL_{lon},PL_{vert}$ are smaller than the true position error magnitude

   	$\displaystyle FR_{lat}$	$\displaystyle=\frac{1}{T_{\textrm{max}}}\sum_{t=1}^{T_{\textrm{max}}}\mathbbm{1}_{t}\left(PL_{lat}<|\Delta x^{*}|\right)$
   	$\displaystyle FR_{lon}$	$\displaystyle=\frac{1}{T_{\textrm{max}}}\sum_{t=1}^{T_{\textrm{max}}}\mathbbm{1}_{t}\left(PL_{lon}<|\Delta y^{*}|\right)$
   	$\displaystyle FR_{vert}$	$\displaystyle=\frac{1}{T_{\textrm{max}}}\sum_{t=1}^{T_{\textrm{max}}}\mathbbm{1}_{t}\left(PL_{vert}<|\Delta z^{*}|\right),$		(25)

   where

   1. –

      $FR_{lat},FR_{lon}$ and $FR_{vert}$ denote failure rates for lateral, longitudinal and vertical protection levels, respectively

   2. –

      $\mathbbm{1}_{t}(\cdot)$ denotes the indicator function computed using the protection level and true position error values at time $t$. The indicator function evaluates to $1$ if the event in its argument holds true, and otherwise evaluates to $0$

   3. –

      $T_{\textrm{max}}$ denotes the total time duration of the test sequence

   The failure rate $FR_{lat},FR_{lon},FR_{vert}$ should be consistent with the specified value of the integrity risk $IR$ to meet the safety requirements.

3. 3.

   False alarm rate is computed for a specified alarm limit $AL_{lat},AL_{lon},AL_{vert}$ in the lateral, longitudinal and vertical directions and measures the fraction of time instances in the test data sequence for which the computed protection levels $PL_{lat},PL_{lon},PL_{vert}$ exceed the alarm limit $AL_{lat},AL_{lon},AL_{vert}$ while the position error magnitude is within the alarm limits. We first define the following integrity events

   	$\displaystyle\Omega_{lat,PL}$	$\displaystyle=(PL_{lat}>AL_{lat})$
   	$\displaystyle\Omega_{lat,PE}$	$\displaystyle=(|\Delta x^{*}|>AL_{lat})$
   	$\displaystyle\Omega_{lon,PL}$	$\displaystyle=(PL_{lon}>AL_{lon})$
   	$\displaystyle\Omega_{lon,PE}$	$\displaystyle=(|\Delta y^{*}|>AL_{lon})$
   	$\displaystyle\Omega_{vert,PL}$	$\displaystyle=(PL_{vert}>AL_{vert})$
   	$\displaystyle\Omega_{vert,PE}$	$\displaystyle=(|\Delta z^{*}|>AL_{vert}).$		(26)

   The complement of each event is denoted by $\bar{\Omega}$. Next, we define the counts for false alarms $N_{X,FA}$, true alarms $N_{X,TA}$ and the number of times the position error exceeds the alarm limit $N_{X,PE}$ with $X=lat,lon$ and $vert$

   	$\displaystyle N_{X,FA}$	$\displaystyle=\sum_{t=1}^{T_{\textrm{max}}}\mathbbm{1}_{t}\left(\Omega_{X,PL}\cap\bar{\Omega}_{X,PE}\right)$
   	$\displaystyle N_{X,TA}$	$\displaystyle=\sum_{t=1}^{T_{\textrm{max}}}\mathbbm{1}_{t}\left(\Omega_{X,PL}\cap\Omega_{X,PE}\right)$
   	$\displaystyle N_{X,PE}$	$\displaystyle=\sum_{t=1}^{T_{\textrm{max}}}\mathbbm{1}_{t}\left(\Omega_{X,PE}\right).$		(27)

   Finally, we compute the false alarm rates $FAR_{lat},FAR_{lon},FAR_{vert}$ after normalizing the total number of position error magnitudes lying above and below the alarm limit $AL$

   $\displaystyle FAR_{X}$

   $\displaystyle=\frac{N_{X,FA}\cdot({T_{\textrm{max}}}-N_{X,PE})}{N_{X,FA}\cdot({T_{\textrm{max}}}-N_{X,PE})+N_{X,TA}\cdot N_{X,PE}}.$

   (28)

Fig. 6 shows the lateral and longitudinal protection levels computed by our approach on two $200$ s subsets of the test sequence. For clarity, protection levels are computed at every $5$th time instance. Similarly, Fig. 7 shows the vertical protection levels along with the vertical position error magnitude in a subset of the test sequence. As can be seen from both the figures, the computed protection levels successfully enclose the position error magnitudes at a majority of the points ($\sim 99\%$) in the visualized subsequences. Furthermore, the vertical protection levels are observed to be visually closer to the position error as compared to the lateral and longitudinal protection levels. This is due to the superior performance of the DNN in determining position errors along the vertical dimension, which is easier to learn since all the camera images in the dataset are captured by a ground-based vehicle.