[datatype = bibtex] [fieldsource = abstract, match = ([^
])%, replace = X]

# Toward Certifying Maps for Safe Localization Under Adversarial Corruption

Johann Laconte1, Daniil Lisus1 and Timothy D. Barfoot1 1 University of Toronto
Institute for Aerospace Studies (UTIAS), 4925 Dufferin St, Ontario, Canada.
{johann.laconte<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

In this paper, we propose a way to model the resilience of the Iterative
Closest Point (ICP) algorithm in the presence of corrupted measurements. In
the context of autonomous vehicles, certifying the safety of the localization
process poses a significant challenge. As robots evolve in a complex world,
various types of noise can impact the measurements. Conventionally, this noise
has been assumed to be distributed according to a zero-mean Gaussian
distribution. However, this assumption does not hold in numerous scenarios,
including adverse weather conditions, occlusions caused by dynamic obstacles,
or long-term changes in the map. In these cases, the measurements are instead
affected by a large, deterministic fault. This paper introduces a closed-form
formula approximating the highest pose error caused by corrupted measurements
using the ICP algorithm. Using this formula, we develop a metric to certify
and pinpoint specific regions within the environment where the robot is more
vulnerable to localization failures in the presence of faults in the
measurements.

##  I Introduction

Reliable localization is a vital task for self-driving robots, as it plays a
key role in ensuring their safety and effective operation. However, sensors
are susceptible to making errors, necessitating the implementation of robust
safeguards to mitigate potential risks. In this paper, we focus on the domain
of range-based localization, a technique relying on range sensors such as
lidar or radar. In addition to being inherently noisy, these sensors can also
encounter large, deterministic errors that heavily deviate from the common
Gaussian noise assumption, posing significant challenges to accurate
localization.

Measurement corruptions can be caused by diverse sources. For example, an
occlusion may cause a portion of the environment to be completely blocked from
the sensor’s view, leading to missing or inaccurate range readings. Similarly,
adverse weather conditions, such as heavy snowstorms or fog, can distort the
sensor measurements in a nonrandom manner [1]. Consistent measurement errors
can also arise from long-term changes in an outdated map, including the
presence of new buildings or parked cars. These factors result in systematic
and deterministic errors that are significantly different from the statistical
properties of Gaussian noise.

The ICP algorithm is established as one of the most popular approaches for
estimating a robot’s pose using range sensors [2]. Depending on the map
against which ICP is trying to localize, a corruption of the pointcloud can be
more or less impactful. Figure 1 depicts some examples from the self-driving
application. In a large intersection, few structures are close enough to
provide good constraints for ICP. As such, any faulty measurement on the
structures would have a great impact on the localization output. On the
contrary, smaller streets with lots of houses and other obstacles provide a
much safer environment, as ICP can now rely on a variety of landmarks to
localize. Even though ICP is frequently equipped with robust outlier rejection
features, some faults can still be considered as inliers and impact the
accuracy of the localization process. While not necessarily large enough to
break the localization process, these inlier faults can degrade the
localization accuracy to a degree that may easily result in a crash in
autonomous driving scenarios

Figure 1:  In the context of pose estimation, specific environments entail
higher risks compared to others. For instance, a large intersection (left)
offers limited landmarks for localization, whereas a suburban area (right)
presents numerous houses and landmarks. Consequently, occlusions or map
alterations at the intersection may lead to a significantly larger pose
estimation error compared to the suburban area.

Our paper aims to quantify the map-dependent resilience of the ICP
localization algorithm. Our contributions are 1) a closed-form formula of the
worst possible error on the pose for a given amount of corrupted points; 2) a
visualization of the worst corruption by applying the faults to the measured
pointcloud; and 3) a quantification of the resilience of the ICP for a
specific map against corrupted measurements. We provide a quantitative
analysis of our framework, as well as the evaluation of both structured and
unstructured environments, showing that our framework can pinpoint dangerous
locations in the event of corrupted measurements.

##  II Related Work

Extensive research has been conducted in the field of aerospace engineering to
address safety considerations. In this context, the majority of safety
analyses are bound to the estimation of position using Global Navigation
Satellite System (GNSS) measurements. The safety of this estimation is based
on the Hazardously Misleading Information (HMI) metric, which looks at the
probability that the position estimate is sufficiently erroneous to be
considered hazardous, while no fault detectors have been triggered. The
detectors can take on various forms, but two are most prevalent: residuals and
solution-separation methods [3, 4]. The residuals-based methods look at the
measurement residuals coming from the estimation algorithm [5], such as the
innovation in a Kalman filter [6]. In the event such residuals are higher than
a threshold, a fault in the measurements is likely and an alarm is triggered.
On the other hand, solution-separation-based methods try to isolate and
discard a potentially faulty measurement from all measurements received at a
given time; this method quickly becomes computationally expensive as every
combination of measurements needs to be tested [7]. Recently, [8] proposed to
adapt these methods to robotics. They used the HMI framework to monitor the
safety of a landmark-based localization pipeline. Their work was adapted to
batch estimation by [9]. Finally, [10] used this framework to propose a way to
enhance the safety of maps by adding well-placed landmarks in the environment.
All of these previous efforts aim at certifying that the system will be able
to detect faults, which is fundamentally different from certifying the well-
being of the system. Indeed, a very poor map can be classified as safe as long
as is it easy for the system to detect that its estimated pose is not right.
As such, we propose a novel method to directly monitor the reliability of the
localization process, and not its capability to detect hazardous events.

In this context, the notion of localizability is the standard for range-based
localization. Localizability is defined as the capability of the localization
system to produce a good estimate of the robot’s pose. This information is
paramount in under-constrained environments, such as tunnel-like surroundings
[11], where the localization process is prone to yielding bad estimates. [12]
proposed a way to learn the localizability in underground environments. [13]
developed a method to monitor the degeneracy of ICP throughout the
localization process by looking at the condition of the measurement matrix
after linearizing the system. Similarly, [14] proposed to model the safety of
the localization in an environment using Fisher information. Finally, [15]
proposed to train a classifier to automatically label odometry estimates as
good or bad, showing that rejecting poor solutions leads to a better overall
estimate. [16] used a similar idea to classify the integrity in visual
localization, removing inaccurate estimates. However, the localizability
analysis does not encompass the possibility of corrupted measurements that can
arise in numerous situations, such as adverse weather or occlusion from other
dynamic obstacles. Indeed, the analysis relies solely on the measured
pointcloud, without taking into account the possibility that large errors can
add malicious information. We propose a way to not only examine the safety of
the system under nominal conditions, but also its performance in situations
where certain measurements may provide adversarial information to the robot.

Within the area of compromised data, different works were conducted to
explicitly assess the resistance of algorithms to corruption by generating,
altering, or removing some measurements. [17] designed a benchmark that
directly takes into account the corruption of the data coming from diverse
sources, such as the weather or sensor failures. This benchmark was used to
extensively test the algorithms and their resilience to different types of
noise. [18] proposed a method to generate and perturb pointclouds that lead a
classification network to mislabel the scanned object. [19] developed a method
to craft 3D obstacles that were not detected by deep learning algorithms
working with lidar data. [20] proposed novel metrics for the safety of
robotics algorithms, such as the robustness of a bad prior and the absence of
updates. [21] designed a method to characterize failures of lidar-based
perception systems in adverse weather conditions. Using physics-based
disturbances and reinforcement learning, they were able to find high-
likelihood failures with relatively small input disturbances. Finally, [22]
directly attacked a pointcloud to force the ICP algorithm to move to a desired
target position instead of the ground truth. Our work is similar in spirit to
these methods, but we propose a closed-form algebraic method to analyze the
resilience of the ICP algorithm through corruption of the measurements.

##  III Preliminaries

First, we offer a brief illustration of the process to transform pointcloud
alignment into an approximated linear problem. Next, we introduce the
methodology for modeling corruption in conventional estimation problems. Using
these formulations, we derive a closed-form formula representing the maximum
possible error in a pose estimate.

###  III-A ICP Formulation

We briefly show a linear simplification of ICP. For one iteration, the
measurement model is written as

$\displaystyle\bm{q}_{i}$
$\displaystyle=\bm{R}(\bm{p}_{i}+\bm{w}_{i})+\bm{t},$ (1)

where $\bm{p}_{i}$ are the points in the sensor frame, $\bm{q}_{i}$ are the
points in the map frame, and
$\bm{w}_{i}\sim\mathcal{N}(\bm{0},\sigma^{2}\bm{I})$ is the noise on the
measured point $\bm{p}_{i}$, $\bm{I}$ being the identity matrix. The matrix
$\bm{R}$ and vector $\bm{t}$, respectively, denote the rotation and
translation components of the pose. Assuming the rotation $\bm{R}$ between the
scan and map pointclouds is small, we can use the small-angle approximation
$\bm{R}\approx\bm{I}+\bm{\phi}^{\wedge}$ and linearize the relation as

$\displaystyle\bm{q}_{i}$
$\displaystyle\approx(\bm{I}+\bm{\phi}^{\wedge})\bm{p}_{i}+\bm{R}\bm{w}_{i}+\bm{t}$
(2)
$\displaystyle=\bm{p}_{i}-\bm{p}_{i}^{\wedge}\bm{\phi}+\bm{R}\bm{w}_{i}+\bm{t}$
$\displaystyle=\begin{bmatrix}\bm{I}&-\bm{p}_{i}^{\wedge}\end{bmatrix}\begin{bmatrix}\bm{t}\\\
\bm{\phi}\end{bmatrix}+\bm{p}_{i}+\bm{w}_{i}^{\prime},$

where $(\cdot)^{\wedge}$ is a cross-product operator transforming the vector
to a 3$\times$3 skew-symmetric matrix [23].

In the case of point-to-plane ICP with unit normals $\bm{n}_{i}$ in the map
frame, we project the measurement points $\bm{p}_{i}$ onto the associated
normals $\bm{n}_{i}$. Reworking (2) and stacking the $N$ measurements into one
vector $\bm{y}$, we have the linear system

$\underbrace{\begin{bmatrix}\bm{n}_{1}^{T}(\bm{q}_{1}-\bm{p}_{1})\\\ \vdots\\\
\bm{n}_{N}^{T}(\bm{q}_{N}-\bm{p}_{N})\end{bmatrix}}_{\bm{y}}=\underbrace{\begin{bmatrix}\bm{n}_{1}^{T}&-\bm{n}_{1}^{T}\bm{p}_{1}^{\wedge}\\\
\vdots&\vdots\\\ \bm{n}_{N}^{T}&-\bm{n}_{N}^{T}\bm{p}_{N}^{\wedge}\\\
\end{bmatrix}}_{\bm{A}}\underbrace{\begin{bmatrix}\bm{t}\\\
\bm{\phi}\end{bmatrix}}_{\bm{x}}+\,\bm{w},$ (3)

with $\bm{w}\sim\mathcal{N}(\bm{0},\sigma^{2}\bm{I})$. This formulation has
two main assumptions: known data association and a noniterative approach.
While these assumptions may appear restrictive, we demonstrate in Subsection
V-A that they actually establish a suitably conservative approximation of the
error of the ICP algorithm.

###  III-B Problem Statement

In robotics, many problems can be formed as a least-squares minimization. In
particular, we are interested in standard linear measurement problems of the
form

$\displaystyle\bm{y}=\bm{A}\bm{x}+\bm{w},$ (4)

where $\bm{y}\in\mathbb{R}^{m}$ is the measurement, $\bm{x}\in\mathbb{R}^{n}$
is the state to estimate, $\bm{A}\in\mathbb{R}^{m\times n}$ is the matrix
linking the state to the measurement, and $\bm{w}\in\mathbb{R}^{m}$ is random
Gaussian noise with covariance $\bm{\Sigma}$. This formulation assumes that
the error on the measurement can be modelled as a zero-mean, Gaussian random
variable. Perturbations resulting from inclement weather or occlusions from
other dynamic obstacles can contradict this assumption. Therefore, as is done
in [9], we assume a measurement can be subject to both noise and a
deterministic, possibly large, fault (corruption):

$\bm{y}=\bm{A}\bm{x}+\bm{w}+\bm{Q}\bm{f},$ (5)

where $\bm{f}\in\mathbb{R}^{n_{f}}$ are the faults, and
$\bm{Q}\in\mathbb{R}^{m\times n_{f}}$ is a sparse matrix of zeros and ones
applying the faults to the associated measurements. As such, a part of the
measurement is subject to a possibly large fault that can hinder the
estimation.

In the context of linear estimation, the solution to a least-squares problem
can be written as

$\displaystyle\hat{\bm{x}}$
$\displaystyle=\operatorname*{arg\,min}_{\bm{x}}\sum_{i=1}^{m}\alpha_{i}\cdot(\bm{y}_{i}-\bm{A}_{i}\bm{x})^{T}\bm{\Sigma}_{i}^{-1}(\bm{y}_{i}-\bm{A}_{i}\bm{x})$
(6)
$\displaystyle\quad=\operatorname*{arg\,min}_{\bm{x}}(\bm{y}-\bm{A}\bm{x})^{T}\bm{\Sigma}^{-1}(\bm{y}-\bm{A}\bm{x}),$

where $\bm{y}_{i}$ is the $i^{\text{th}}$ measurement with its associated
covariance $\bm{\Sigma}_{i}$, and $\bm{x}$ is the pose to estimate. Note that
each measurement is weighted by a factor $\alpha_{i}$, which is used to
control the impact of each measurement on the estimate and is incorporated
into the lifted matrix $\bm{\Sigma}$.

Robust filters have the task of discriminating between inlier and outlier
measurements. Among them, the trimmed distance filter is one of the most
popular [2]. Given an initial guess $\bm{x}_{0}$, the trimmed distance filter
removes any measurement that has a corresponding error that is larger than a
fixed distance $d$:

$\alpha_{i}=\begin{cases}1&\text{if
}\left\lVert\bm{y}_{i}-\bm{A}_{i}\bm{x}_{0}\right\rVert_{\infty}\leq d,\\\
0&\text{otherwise}.\end{cases}$ (7)

As such, any fault trying to corrupt a system making use of a trimmed distance
filter has to be small enough to be deemed an inlier. In the case of systems
of the form defined in (6), the solution is given by

$\hat{\bm{x}}=\bm{H}\bm{y},\quad\text{with
}\bm{H}=\left(\bm{A}^{T}\bm{\Sigma}^{-1}\bm{A}\right)^{-1}\bm{A}^{T}\bm{\Sigma}^{-1}.$
(8)

The corresponding state-estimation error can be computed as

$\displaystyle\bm{e}$ $\displaystyle=\hat{\bm{x}}-\bm{x}$ (9)
$\displaystyle=\bm{H}\bm{y}-\bm{x}$
$\displaystyle=\bm{H}\left(\bm{A}\bm{x}+\bm{w}+\bm{Q}\bm{f}\right)-\bm{x}$
$\displaystyle=\bm{H}\left(\bm{w}+\bm{Q}\bm{f}\right).$

The faults $\bm{Q}\bm{f}$, unlike the noise $\bm{w}$, are not zero-mean random
variables, but are unknown, possibly large, deterministic quantities. As such,
the faults generate a bias in the estimation that could be harmful. In the
following, we provide a method to compute the worst error that can occur given
corrupted measurements from a defined set.

###  III-C Safety Metric

Aiming at certifying a localization algorithm, we define a safety metric as
the error on the pose being below a certain threshold:

$|e_{j}|=|\bm{g}^{T}_{j}(\hat{\bm{x}}-\bm{x})|\leq r_{j},$ (10)

where $\hat{\bm{x}}$ is the estimated state, $\bm{x}$ is the ground truth,
$\bm{g}^{T}_{j}$ is a row matrix that extracts the $j^{\text{th}}$ coordinate
from the state, and $r_{j}$ is the error-specific threshold. As such, an
estimate is considered safe if each of its components has an error below a
certain threshold $r_{j}$. Note that we choose to certify each component of
the estimated state independently instead of looking at the $L_{2}$ norm of
the error. This is particularly useful in self-driving applications where, for
instance, the lateral error is often more important than the longitudinal one.
Using this definition, we seek to compute the probability that a pose estimate
is safe:

$p\left(|e_{j}|\leq r_{j}\right)\geq p_{\text{safe}}.$ (11)

In other words, a given estimate is said to be certified if its probability to
be in the safe zone of radius $r_{j}$ is above $p_{\text{safe}}$. In the
presence of faults, we are interested in the maximum number of faulted
measurements that can happen at the same time before (11) becomes false. This
quantity is defined as the resilience of the system.

##  IV Corrupted Measurements

In this section, we provide a closed-form formula for the worst pose estimate
given corrupted measurements from a defined set. This closed form is used to
compute the probability that a pose estimate is hazardous, as defined in (11).
Then, we define a metric to certify the resilience of ICP in different
scenarios. Finally, we show how to link the faults back to a meaningful
perturbation in the measured pointcloud.

###  IV-A Worst Pose Estimate

This section proposes a closed-form formula to compute the probability of a
pose estimate being hazardous, given corrupted measurements from a defined
set. First, we define the constraint that the faults need to satisfy so that
they are not trimmed by the outlier filter. As defined in (5), the measurement
$\bm{y}$ contains both a probabilistic zero-mean noise $\bm{w}$ and
deterministic faults from a defined set $\bm{Q}\bm{f}$. Faults that maximize
the pose error will be such that they are considered inliers according to (7),
but are otherwise maximally detrimental to the accuracy of the pose estimate.
Using (5) and (7), assuming the initial guess is close to the ground truth, we
have the following constraint on the faults:

$\displaystyle\left\lVert\bm{y}-\bm{A}\bm{x}_{0}\right\rVert_{\infty}\leq d$
(12) $\displaystyle\Leftrightarrow$
$\displaystyle\left\lVert\bm{w}+\bm{Q}\bm{f}\right\rVert_{\infty}\leq d,$

meaning that all errors on the measurements are below the outlier detection
threshold $d$. However, this constraint acts on the whole measurement vector
and not only on the corrupted subset. As such, we reduce the constraint to
only act on the subset of faulted measurements, as

$\left\lVert\bm{Q}^{T}\bm{w}+\bm{f}\right\rVert_{\infty}\leq d,$ (13)

where $\bm{Q}^{T}\in\mathbb{R}^{n_{f}\\!\times m}$ extracts the noise
components of the faulted measurements from the noise vector
$\bm{w}\in\mathbb{R}^{m}$. As such, the constraint (13) forces the faults to
be small enough that the faulted measurements are still considered inliers.
Using this constraint, we now seek to find the worst pose error that a defined
set of faulted measurements could cause. Following (9) and (10), the maximum
error that a set of faulted measurements can induce on the pose while being
undetected by the outlier detector is defined by

$\displaystyle\max_{\left\lVert\bm{Q}^{T}\bm{w}+\bm{f}\right\rVert_{\infty}\leq
d}|e_{j}|=\max_{\left\lVert\bm{Q}^{T}\bm{w}+\bm{f}\right\rVert_{\infty}\leq
d}|\bm{g}^{T}_{j}\bm{H}\left(\bm{w}+\bm{Q}\bm{f}\right)|$ (14)
$\displaystyle=\max_{\left\lVert\bm{Q}^{T}\bm{w}+\bm{f}\right\rVert_{\infty}\leq
d}|\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\bm{w}+\bm{g}^{T}_{j}\bm{H}\bm{Q}\left(\bm{Q}^{T}\bm{w}+\bm{f}\right)|,$

where $\bar{\bm{Q}}=\bm{I}-\bm{Q}\bm{Q}^{T}$. In order to find a closed form,
we recall that for all $a\in\mathbb{R}$ and $x\in[-m,m]$ we have

$\max_{x\in[-m,m]}|a+x|=\max\left\\{|a\pm m|\right\\}.$ (15)

As such, using (15), the worst error on the pose (14) can be rewritten as

$\max_{\left\lVert\bm{Q}^{T}\bm{w}+\bm{f}\right\rVert_{\infty}\leq
d}|e_{j}|=\max\\{\left|\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\bm{w}\pm m\right|\\},$
(16)

with

$\displaystyle m$
$\displaystyle=\max_{\left\lVert\bm{Q}^{T}\bm{w}+\bm{f}\right\rVert_{\infty}\leq
d}\bm{g}^{T}_{j}\bm{H}\bm{Q}\left(\bm{Q}^{T}\bm{w}+\bm{f}\right)$ (17)
$\displaystyle=d\left\lVert\bm{g}^{T}_{j}\bm{H}\bm{Q}\right\rVert_{\infty},$

where we used the fact that for any vectors $\bm{a},\bm{b}$, we have

$\max_{\left\lVert\bf{b}\right\rVert_{\infty}\leq
1}\bm{a}^{T}\bm{b}=\sum_{k}\left|a_{k}\right|=\left\lVert\bm{a}^{T}\right\rVert_{\infty},$
(18)

with $\bm{a}=\begin{bmatrix}a_{1}\cdots a_{N}\end{bmatrix}^{T}$. Plugging this
back into (16), we finally find that the worst error on the pose is defined by

$\displaystyle\max_{\left\lVert\bm{Q}^{T}\bm{w}+\bm{f}\right\rVert_{\infty}\leq
d}|e_{j}|$
$\displaystyle=\max\left\\{\left|\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\bm{w}\pm
d\left\lVert\bm{g}^{T}_{j}\bm{H}\bm{Q}\right\rVert_{\infty}\right|\right\\}$
(19) $\displaystyle=\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\bm{w}+s\cdot
d\left\lVert\bm{g}^{T}_{j}\bm{H}\bm{Q}\right\rVert_{\infty},$

with
$s=\operatorname{sgn}{\left(\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\bm{w}\right)}$.
Therefore, for a given noise and set of corrupted measurements, the maximum
error on the pose can be found in closed-form. Note, this equation yields the
maximum error on the pose and not the fault vector that induces it.

In order to find the probability distribution of the worst error $|e_{j}|$, we
use the notation $v=\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\bm{w}$ and do the
following manipulation:

$\displaystyle p\left(|e_{j}|>r_{j}\right)$
$\displaystyle=p\left(e_{j}>r_{j},v\geq
0\right)+p\left(e_{j}<-r_{j},v<0\right)$ (20)
$\displaystyle=2p\left(e_{j}>r_{j},v\geq 0\right),$

where we group the $v\geq 0$ and $v<0$ cases as $v$ is a linear function of
the noise $\bm{w}$ and thus a zero-mean Gaussian random variable. Expanding
the error $|e_{j}|$, we find

$\displaystyle p\left(|e_{j}|>r_{j}\right)$
$\displaystyle=2\,p\left(v+d\left\lVert\bm{g}^{T}_{j}\bm{H}\bm{Q}\right\rVert_{\infty}>r_{j},v\geq
0\right)$ (21)
$\displaystyle=2\,p\left(v>\max\left\\{r_{j}-d\left\lVert\bm{g}^{T}_{j}\bm{H}\bm{Q}\right\rVert_{\infty},0\right\\}\right)$
$\displaystyle=\min\left\\{2\left(1-\Phi_{\mu,\sigma}(r_{j})\right),1\right\\},$

where $\Phi_{\mu,\sigma}(\cdot)$ is the standard cumulative distribution
function of the normal distribution, with

$\displaystyle\mu$
$\displaystyle=d\left\lVert\bm{g}^{T}_{j}\bm{H}\bm{Q}\right\rVert_{\infty}$
(22) $\displaystyle\sigma^{2}$
$\displaystyle=\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\bm{\Sigma}_{\bm{w}}\left(\bm{g}^{T}_{j}\bm{H}\bar{\bm{Q}}\right)^{T}.$

In conclusion, this section provides a way to compute efficiently the
probability of having a hazardous estimate of the pose given a malicious set
of corrupted measurements, using (21). We show in the next section how to
certify a map using this metric, before demonstrating how the faults on the
point-to-plane measurements can be linked back to the measured lidar points.

###  IV-B Map Certification

In the preceding section, we demonstrated the ability to determine efficiently
the maximum localization error for a specific set of corrupted measurements.
Nevertheless, due to the large number of points in a lidar scan, exhaustively
testing every combination of corrupted points becomes impractical. To address
this challenge, we propose to model the lidar field of view as a collection of
angular sectors. All points within each sector are then either corrupted or
unaltered. Although this assumption may appear restrictive, numerous events
are inherently tied to a sector of corrupted points. For instance, cars parked
on the side of the street will result in a coherent shift of the measured
pointcloud compared to a map that was constructed without them present.
Occlusions can also lead to a sector-wide pointcloud alteration, thus
appearing as fully faulted sectors.

Using this modeling approach, we define the resilience $R$ at a specific
position on the map as the maximum proportion of the lidar scan that can be
corrupted before the validity of the certificate stated in (11) is
compromised.

###  IV-C Fault Visualization

Although computing the explicit faults is not required for the safety
assessment, it can be useful for visualization and evaluation purposes. As the
faults are applied on the measurement equation defined in (5), the corruption
is not linked to the measured points but rather the projected distance between
the lidar points and the map. As such, in this section, we show how to recover
a corruption on the pointcloud from a set of faults originally applied to the
point-to-plane measurements. First, we recall that the vector that maximizes
(18) is given by

$\bm{b}=\operatorname{sgn}{\bm{a}},$ (23)

where the sign function is extended to a vector, applying the sign function
component-wise. As such, from (17) and (19), we can deduce that the fault
vector $\bm{f}$ that maximizes the error on the pose is

$\bm{f}=s\cdot
d\cdot\operatorname{sgn}{\left(\bm{g}^{T}_{j}\bm{H}\bm{Q}\right)^{T}}-\bm{Q}^{T}\bm{w}.$
(24)

Using (24), we are able to construct a fault vector on the point-to-plane
measurements. However, this fault vector does not directly corrupt the
measured points, which are 3D quantities, but rather the projected distances.
To model point-wise corruption, we seek to find a perturbation
$\Delta\bm{p}_{k}$ for each measured point $\bm{p}_{k}$ subject to corruption,
with a perturbed point $\bm{p}^{\prime}_{k}$ defined as

$\bm{p}^{\prime}_{k}=\bm{p}_{k}+\Delta\bm{p}_{k}.$ (25)

Rewriting (3) for one corrupted point, we have

$\displaystyle\bm{n}_{k}^{T}(\bm{q}_{k}-\bm{p}^{\prime}_{k})=-\bm{n}_{k}^{T}{\bm{p}^{\prime}_{k}}^{\wedge}\bm{\phi}+\bm{n}_{k}^{T}\bm{t}+\bm{w}_{k}$
(26) $\displaystyle\Leftrightarrow$
$\displaystyle\bm{n}_{k}^{T}(\bm{q}_{k}-\bm{p}_{k})=-\bm{n}_{k}^{T}\bm{p}_{k}^{\wedge}\bm{\phi}+\bm{n}_{k}^{T}\bm{t}+\bm{w}_{k}$
$\displaystyle\hskip
80.00012pt+\bm{n}_{k}^{T}\Delta\bm{p}_{k}-\bm{n}_{k}^{T}\Delta\bm{p}_{k}^{\wedge}\bm{\phi}.$

Therefore, the fault on the point-to-plane measurement $f_{k}$ is linked to
the corrupted point by the relation

$\displaystyle
f_{k}=\bm{n}_{k}^{T}\Delta\bm{p}_{k}-\bm{n}_{k}^{T}\Delta\bm{p}_{k}^{\wedge}\bm{\phi},$
(27)

from which we extract a viable solution

$\Delta\bm{p}_{k}=f_{k}\bm{n}_{k},$ (28)

since $\bm{n}_{k}^{T}\bm{n}_{k}^{\wedge}=\bm{0}$. Note that (27) possesses
many solutions as we retrieve a 3D quantity from a 1D fault. From the
perspective of our framework, all solutions are equally good, as all possible
corruptions lead to the same corrupted point-to-plane measurements. However,
(28) is particularly interesting as it is independent of the state variable
$\bm{\phi}$. As such, a fault on the point-to-plane measurement can be seen as
a shift of the measured point along the associated map’s normal. We use this
fault modeling in Section V to show that the simplified ICP formulation used
in the derivation of the framework is similar in behavior to a real ICP
algorithm.

As examples, Figure 2 depicts two submaps of the DARPA Subterranean challenge
finals [24], where a sector of the lidar scan is corrupted in a way to
maximize the error in the $y$ component of the pose. These maps depict
underground environments with clear features that ICP is using to localize.
The inlier threshold distance has been set to $d=$1\text{\,}\mathrm{m}$$.
Using (24), we compute the worst faults on the $y$ component for the given
corrupted sector. The faults are then transformed back into a corruption on
the pointcloud via (28). In the first example (top), the corrupted points
shift and slightly rotate the pose estimate to achieve the worst $y$ component
estimate while evading an outlier detector. In the second example (bottom),
the corruption takes a slightly more complex appearance. One might intuitively
think that the worst pose estimate would come from shifting all the points in
the same direction. However, our framework finds a more complex modification
of the pointcloud that results in an even worse estimate. Indeed, a coherent
shift of all points in a translation-only alignment problem would result in
the worst estimate. However, as ICP has to estimate both the translation and
the rotation components of the pose, shifting the whole wall in the same
direction would make the ICP solution rotate instead of translate, as most of
the pointcloud is left uncorrupted. Accordingly, the worst corruption modifies
the pointcloud in such a way as to prevent rotation, resulting in a pure shift
along the $y$ axis.

Figure 2: Examples of faulted pointclouds from underground passages of the
DARPA Subterranean challenge finals [24]. The faults try to corrupt the $y$
component of the pose, with the corrupted pose estimated by ICP in red, and
the ground truth pose in black. Black points correspond to the reference map.
The lidar point cloud is illustrated in green, with a sector (shaded blue)
being corrupted. The blue and red points correspond to the sector before and
after corruption, with gray arrows showing the shift induced by the
corruption. Points corresponding to the ceiling and floor are omitted for
better visualization.

##  V Experiments

First, we evaluate the quality of our assumptions in the context of a real,
iterative ICP. Then, we show the applicability of our method on real-world
maps using the Boreas [25] and Montmorency Forest Wintertime [26] datasets.

###  V-A Quality of Approximations

As is done in HMI-based safety analysis (e.g., [8, 9]), our method reduces ICP
to a linear, noniterative problem. This framework assumes a known data
association between the map and the scan, which is impossible to have in
practice. We show in this section that our framework yields an approximate
upper-bound to a real iterative ICP algorithm, in which data association must
be solved at each iteration. To do this, we sample $250\text{\,}$ submaps from
the Boreas dataset [25], in which we simulate a lidar scan from real lidar
maps by subsampling the pointcloud. In this evaluation,
$25\text{\,}\mathrm{\char 37\relax}$ of the lidar scan is corrupted and
perturbed using our framework, as this value is the highest resilience
recorded in the experiments presented in Subsection V-B. Then, we feed this
corrupted pointcloud to a vanilla ICP and compare the produced error with the
theoretical one given by (19). Vanilla ICP is equipped with a trimmed distance
filter, set to the same value $d$ as in our framework.

Figure 3 depicts the signed difference between the theoretical errors
predicted by our framework, and the real ICP errors generated from the
corrupted scans, for different inlier threshold distances $d$. Overall, the
difference in error exhibits a linear increase as the threshold distance $d$
increases. It can be seen that our framework yields higher errors compared to
those obtained by the ICP algorithm in more than $98\text{\,}\mathrm{\char
37\relax}$ of cases. In cases where our framework predicts a lower error than
the real ICP algorithm produces, the magnitude of error underestimation is on
the order of centimeters and milliradians. As shown in the next section, our
framework provides an estimate that is tight enough to pinpoint safe and
hazardous regions within the maps. Future work will focus on providing a
provable, tight upper bound.

For large inlier threshold distances, our framework overestimates the linear
error up to $80\text{\,}\mathrm{cm}$ in rare cases. We theorize this
discrepancy can be attributed to the fact that our framework assumes known
data association and optimizes the perturbations accordingly. As the inlier
threshold distance increases, the corrupted points deviate further from their
true associated points in the map. Consequently, the likelihood of incorrect
data associations also rises. In some instances where wrong data association
occurs, it unexpectedly favours the ICP algorithm, as the perturbations
applied were not optimized for that particular association. As a result, an
overall reduction in the pose error may occur. On the other hand, the
difference in rotational errors remains small. Owing to the nature of the
environment, the considered scans contain discernible features far away from
the robot. This makes it harder to corrupt the rotational component as
compared to the translational one.

As such, assuming a known data association yields a reasonably conservative
estimation on the error of ICP. This assumption is sensible as long as the
environment features clear planes, as incorrectly associated points with the
same plane will not affect the error terms. However, in the case of noisy
normal estimates or unstructured objects, an incorrect data association can
match points associated with different planes and thus returns a different
error term. Such behavior is seen when our framework corrupts sectors
containing bushes on the side of the road, or any other highly unstructured
obstacles. These cases are responsible for the long tails of the distributions
seen in Figure 3.

Figure 3: Signed difference of ICP errors between our theoretical estimate and
real ICP for different inlier threshold distances. The violins depict the
overall distribution of the difference, whereas the white dots and black boxes
stand for the median and quartiles. Our estimate typically yields larger
errors compared to real ICP.  Figure 4: Overall resilience of the Glenshield
trajectory. The vehicle starts on the top right of the map, before driving on
a large main road. It then leaves the road for smaller streets with better
features. Left: overview of the trajectory, coloured by its resilience $R$.
Right: Example of submaps with the colours representing the $z$ component of
the pointcloud for better visualization.

###  V-B Map Certification

To demonstrate the applicability of our framework for a given lidar map, we
use the Boreas dataset [25] and certify the Glenshield trajectory. We sample
poses along the trajectory and simulate a live scan by subsampling the map for
each pose. The scan is then used to build the matrices in (3). For this
experiment, we certify the map for both the longitudinal and lateral
directions, ensuring that the probability of deviating from the true position
by more than $20\text{\,}\mathrm{cm}$ is less than $1\text{\,}\mathrm{\char
37\relax}$. The outlier filter distance is set to $d=$30\text{\,}\mathrm{cm}$$
and the noise $\bm{w}$ on the lidar measurements is equal to
$\sigma=$10\text{\,}\mathrm{cm}$$. The lidar field of view is split into
$30\text{\,}$ sectors of approximately $0.1\text{\,}\mathrm{rad}$. We compute
the degree of resilience $R$ for each pose and depict it in Figure 4. The
vehicle starts in a parking lot before turning onto a main street (Figure
4.a). Then, the vehicle drives on smaller streets with better structural
definition (Figure 4.b). Overall, the map is robust to an average corruption
of around $15\text{\,}\mathrm{\char 37\relax}$ of the pointcloud, with some
exceptions. When turning onto the main road, the vehicle has to cross a large
crossroad with almost no structure present in the pointcloud. As such, a small
amount of faults is enough to push the estimate outside the safe bounds. While
on the main street, few structures are close enough to help constrain ICP in
the longitudinal direction, resulting in a drop of resilience to around
$12\text{\,}\mathrm{\char 37\relax}$. In contrast, once the vehicle is on
smaller streets with a lot of structural definition, ICP becomes resilient to
a greater amount of corruption. Even if $20\text{\,}\mathrm{\char 37\relax}$
of the scan is corrupted, there is still enough structure left untouched in
the pointcloud to constrain ICP. Note that the two pointclouds depicted in
Figure 4 correspond the same places pictured in Figure 1.

We additionally analyze a trajectory of the Montmorency Forest Wintertime
dataset [26]. This dataset was taken in subarctic environments in Northern
Quebec, Canada, and consists of both semi-structured surroundings and
unstructured trails in wintertime. Figure 5 depicts the resulting resilience
analysis on run A of the dataset. The robot starts at the bottom of the map
inside a garage. It then drives next to a building before accessing a ski
trail surrounded by tall pine trees. Overall, the resilience to faults is on
the same order as in urban environments, as long as the robot remains close to
structures. Once the robot reaches the ski trails and loses sight of the
building, the resilience drops. Intuitively, the resilience of ICP is high
when the robot is inside the garage, as there are good, clear structures
against which to match. As the robot leaves the garage, the only structured
obstacle is one wall of the building, as all other surrounding obstacles are
snowbanks. Since ICP relies heavily on one locally concentrated obstacle, a
fault in this part of the map leads to a large error in the pose estimate. As
noted in [26], such faulted measurements were in fact observed during the
recording of the dataset: a truck parked next to the wall resulted in the
measurements being offset by a small value, yielding the same type of
corruption as the one theorized in this paper. Harsh weather can also create
similar effects, such as in the case of snow accumulating next to a building
after a snowstorm overnight. As the robot drives toward the buildings on the
top part of the figure, the resilience increases. The ICP algorithm can indeed
rely on a diverse source of information to estimate the pose. As the robot
continues driving, enough buildings are visible in the distance to maintain
reasonable resilience even after the garage becomes out of view. The
resilience goes down once again when the robot enters the ski trails, as it
again only sees one part of a building. Once the robot drives along the
trails, the resilience tends to drop down as it loses sight of the buildings.

Figure 5: Path A of the Montmorency Forest Wintertime dataset, coloured by its
resilience to corruption $R$. The robot starts in a garage at the bottom of
the map, proceeds to drive by several buildings, and finally enters a narrow
ski trail surrounded by tall pine trees.

In conclusion, our resilience analysis is able to highlight dangerous
locations where a small amount of faults, coming from occlusion, noise, or
changes in the environment, could drastically hinder the localization process
and lead the robot to hazardous behaviors. These situations can either come
from a clear lack of good features in the environment, such as in the case of
the large crossroad, or result from ICP relying too much on a single
environmental feature. This second case can happen in semi-structured
environment, as seen in the Montmorecy Forest Wintertime dataset analysis.

##  VI Conclusion

In this paper, we present a novel way to analyze the resilience of the ICP
algorithm. Resilience is defined as the maximum amount of faults that can be
injected into the measurements before the localization estimate is dangerous
for the robot. We model faults as the most severe modifications to the
measurements that can go undetected by an outlier filter. Through this
framework, we verify the quality of maps in both structured and unstructured
environments, and demonstrate that environments lacking distinct, evenly
distributed structures are more susceptible to inaccurate estimates in the
event of measurement corruption.

Future research will focus on addressing the assumptions made in this paper.
We will account for the iterative nature of the ICP algorithm and explore a
broader range of robust cost functions. Also, this paper assumes that the
robot has a good initial guess. However, hazardous behavior can also occur
during a sequence of bad ICP solutions, each driving the robot farther from
the ground truth and feeding worse and worse initial guesses to the next
iteration. As such, taking into consideration a possibly wrong initial guess
will also help certify against a broader range of failures.

Finally, future work will involve expanding the framework to include other
types of sensor failures, such as scenarios where a portion of the lidar
becomes completely unavailable.

## ACKNOWLEDGMENT

We would like to thank the Natural Sciences and Engineering Research Council
of Canada (NSERC) and the Ontario Research Fund: Research Excellence (ORF-RE)
program for supporting this work. We also thank the Northern Robotics
Laboratory (Norlab) for their help with the datasets.

## References

  * [1] Clément Courcelle, Dominic Baril, François Pomerleau and Johann Laconte  “On the Importance of Quantifying Visibility for Autonomous Vehicles under Extreme Precipitation”  In _Towards Human-Vehicle Harmonization_ 3, 2022, pp. 239  URL: http://arxiv.org/abs/2209.03226
  * [2] François Pomerleau, Francis Colas and Roland Siegwart  “A Review of Point Cloud Registration Algorithms for Mobile Robotics”  In _Foundations and Trends in Robotics_ 4, 2015, pp. 1–104  DOI: 10.1561/2300000035ï
  * [3] Mathieu Joerger, Fang-Cheng Chan and Boris Pervan  “Solution separation versus residual-based RAIM”  In _NAVIGATION: Journal of the Institute of Navigation_ 61.4 Wiley Online Library, 2014, pp. 273–291
  * [4] Felipe A. De Oliveira, Frank Sill Torres and Alberto Garcia-Ortiz  “Recent Advances in Sensor Integrity Monitoring Methods - A Review”  In _IEEE Sensors Journal_ 22 Institute of ElectricalElectronics Engineers Inc., 2022, pp. 10256–10279  DOI: 10.1109/JSEN.2022.3169659
  * [5] Peng Zhao, Mathieu Joerger, Xiao Liang, Boris Pervan and Yongming Liu  “A New Method to Bound the Integrity Risk for Residual-Based ARAIM”  In _IEEE Transactions on Aerospace and Electronic Systems_ 57 Institute of ElectricalElectronics Engineers Inc., 2021, pp. 1378–1385  DOI: 10.1109/TAES.2020.3040527
  * [6] Joelle Al Hage, Philippe Xu, Philippe Bonnifait and Javier Ibanez-Guzman  “Localization Integrity for Intelligent Vehicles Through Fault Detection and Position Error Characterization”  In _IEEE Transactions on Intelligent Transportation Systems_ 23 Institute of ElectricalElectronics Engineers Inc., 2022, pp. 2978–2990  DOI: 10.1109/TITS.2020.3027433
  * [7] Qian Meng and Li Ta Hsu  “Integrity Monitoring for All-Source Navigation Enhanced by Kalman Filter-Based Solution Separation”  In _IEEE Sensors Journal_ 21 Institute of ElectricalElectronics Engineers Inc., 2021, pp. 15469–15484  DOI: 10.1109/JSEN.2020.3026081
  * [8] Guillermo Duenas Arana, Mathieu Joerger and Matthew Spenko  “Efficient integrity monitoring for KF-based localization”  In _2019 International Conference on Robotics and Automation (ICRA)_ , 2019, pp. 6374–6380  IEEE
  * [9] Osama Abdul Hafez, Guillermo Duenas Arana, Yihe Chen, Mathieu Joerger and Matthew Spenko  “On robot localization safety for fixed-lag smoothing: Quantifying the risk of misassociation”  In _2020 IEEE/ION Position, Location and Navigation Symposium (PLANS)_ , 2020, pp. 306–317  IEEE
  * [10] Yihe Chen, Osama Abdul Hafez, Boris Pervan and Matthew Spenko  “Landmark augmentation for mobile robot localization safety”  In _IEEE Robotics and Automation Letters_ 6.1 IEEE, 2020, pp. 119–126
  * [11] Turcan Tuna, Julian Nubert, Yoshua Nava, Shehryar Khattak and Marco Hutter  “X-ICP: Localizability-Aware LiDAR Registration for Robust Localization in Extreme Environments”  In _arXiv preprint arXiv:2211.16335_ , 2022  URL: https://youtu.be/f-i-br-l5QA
  * [12] Julian Nubert, Etienne Walther, Shehryar Khattak and Marco Hutter  “Learning-based localizability estimation for robust lidar localization”  In _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2022, pp. 17–24  IEEE
  * [13] Kamak Ebadi, Matteo Palieri, Sally Wood, Curtis Padgett and Ali-akbar Agha-mohammadi  “DARE-SLAM: Degeneracy-aware and resilient loop closing in perceptually-degraded environments”  In _Journal of Intelligent & Robotic Systems_ 102 Springer, 2021, pp. 1–25
  * [14] Zichao Zhang and Davide Scaramuzza  “Beyond point clouds: Fisher information field for active visual localization”  In _2019 International Conference on Robotics and Automation (ICRA)_ , 2019, pp. 5986–5992  IEEE
  * [15] Roberto Aldera, Daniele De Martini, Matthew Gadd and Paul Newman  “What could go wrong? Introspective radar odometry in challenging environments”  In _2019 IEEE Intelligent Transportation Systems Conference (ITSC)_ , 2019, pp. 2835–2842  IEEE
  * [16] Helen Carson, Jason J. Ford and Michael Milford  “Predicting to Improve: Integrity Measures for Assessing Visual Localization Performance”  In _IEEE Robotics and Automation Letters_ 7 Institute of ElectricalElectronics Engineers Inc., 2022, pp. 9627–9634  DOI: 10.1109/LRA.2022.3191205
  * [17] Lingdong Kong, Youquan Liu, Xin Li, Runnan Chen, Wenwei Zhang, Jiawei Ren, Liang Pan, Kai Chen and Ziwei Liu  “Robo3D: Towards robust and reliable 3D perception against corruptions”  In _arXiv preprint arXiv:2303.17597_ , 2023
  * [18] Chong Xiang, Charles R Qi and Bo Li  “Generating 3D adversarial point clouds”  In _2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2019, pp. 9136–9144
  * [19] Yulong Cao, Chaowei Xiao, Dawei Yang, Jing Fang, Ruigang Yang, Mingyan Liu and Bo Li  “Adversarial objects against lidar-based autonomous driving systems”  In _arXiv preprint arXiv:1907.05418_ , 2019
  * [20] Siqi Yi, Stewart Worrall and Eduardo Nebot  “Metrics for the Evaluation of localisation Robustness”  In _IEEE Intelligent Vehicles Symposium (IV)_ , 2019, pp. 1247–1253  URL: http://arxiv.org/abs/1904.08585
  * [21] Harrison Delecki, Masha Itkina, Bernard Lange, Ransalu Senanayake and Mykel J Kochenderfer  “How do we fail? Stress testing perception in autonomous vehicles”  In _2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , 2022, pp. 5139–5146  IEEE
  * [22] Kota Yoshida, Masaya Hojo and Takeshi Fujino  “Adversarial Scan Attack against Scan Matching Algorithm for Pose Estimation in LiDAR-Based SLAM”  In _IEICE Transactions on Fundamentals of Electronics, Communications and Computer Sciences_ 105.3 The Institute of Electronics, InformationCommunication Engineers, 2022, pp. 326–335
  * [23] T.D. Barfoot  “State Estimation for Robotics”  Cambridge, UK: Cambridge University Press, 2017
  * [24] Kamak Ebadi, Lukas Bernreiter, Harel Biggie, Gavin Catt, Yun Chang, Arghya Chatterjee, Christopher E Denniston, Simon-Pierre Deschênes, Kyle Harlow and Shehryar Khattak  “Present and future of SLAM in extreme underground environments”  In _arXiv preprint arXiv:2208.01787_ , 2022
  * [25] Keenan Burnett, David J Yoon, Yuchen Wu, Andrew Z Li, Haowei Zhang, Shichen Lu, Jingxing Qian, Wei-Kang Tseng, Andrew Lambert and Keith YK Leung  “Boreas: A multi-season autonomous driving dataset”  In _The International Journal of Robotics Research_ 42.1-2 SAGE Publications Sage UK: London, England, 2023, pp. 33–42
  * [26] Dominic Baril, Simon-Pierre Deschênes, Olivier Gamache, Maxime Vaidis, Damien LaRocque, Johann Laconte, Vladimír Kubelka, Philippe Giguère and François Pomerleau  “Kilometer-scale autonomous navigation in subarctic forests: challenges and lessons learned”  In _Field Robotics_ 2, 2022, pp. 1628–1660  URL: http://arxiv.org/abs/2111.13981

  *[ICP]: Iterative Closest Point
  *[GNSS]: Global Navigation Satellite System
  *[HMI]: Hazardously Misleading Information