# Contact processes on fragmented domains

I. Ibagon<EMAIL_ADDRESS>Departamento de Física, ICEx, Universidade
Federal de Minas Gerais, C. P. 702, 30123-970, Belo Horizonte, Minas Gerais,
Brazil  A. P. Furlan<EMAIL_ADDRESS>Ronald Dickman
<EMAIL_ADDRESS>National Institute of Science and Technology for
Complex Systems,Brazil

(August 28, 2024)

###### Abstract

Motivated by recent findings of enhanced species survival when fragmented
habitats are reconnected through narrow strips of land [S. Pimm, and C. N.
Jenkins, Am. Sci. 107(3), 162 (2019).], we study the effect of a corridor
connecting two square regions on the survival time of three models exhibiting
extinction/survival phase transitions: the basic contact process (CP), the
diffusive contact process, and the two-species symbiotic contact process
(2SCP). We find that connecting the regions generally increases the survival
time for $\lambda\geq\lambda_{c}$, where $\lambda$ is the reproduction rate
and $\lambda_{c}$ its critical value. The enhancement of the survival time
increases with $\lambda$, and is largest in the 2SCP.

###### keywords:

Contact process , Nonequilibrium statistical physics , Fragmentation ,
Corridor Ecology

##  1 Introduction

Human activities such as construction and agriculture are responsible for
habitat loss and fragmentation [1]. An alarming consequence of fragmentation
is that species richness decreases, with smaller fragments losing species more
rapidly [2, 3]. Since habitat fragments—even when protected— are often too
small to maintain viable populations of species, corridors connecting the
fragments are used as a conservation strategy [4, 2]. Most studies of the
effect of corridors on populations are empirical. Beier and Noss [5] reviewed
32 studies published from 1988 to 1997 and concluded that they provide
evidence on the benefit of corridors to populations in real fragmented
landscapes, increasing immigration and colonization rates or facilitating
movement of individuals between fragments. Similarly, a meta-analysis of 78
experiments published between 1988 and 2008 found a 50% increase in movement
between habitat patches connected with corridors in comparison with habitat
patches without corridors [6]. Haddad et al. [7] reviewed studies on the
negative effects of corridors, such as increased dispersal of antagonistic
species to the conservation target, increased dispersal of exotic species,
spread of disturbances like fire, and edge effects. However, no consistent
evidence of negative effect of corridors was found.

Theoretical work in this area includes a two-population model[8] and
metapopulation models [9, 10]. In Ref. [8], two logistically growing
populations within two patches are connected by migration, which depends on
the presence or absence of the corridor[8]. In spatial metapopulation models,
the dynamics of the fraction of occupied patches, representing local
populations, is governed by colonization and extinction rates that depend on
the patch area and the distance between patches [9, 10]. Given the growing use
of corridors in conservation ecology we believe the area might benefit from
analysis of simple models with explicit spatial structure, which may help to
understand which characteristics of the corridor and/or the population are
important for this strategy to succeed. In this work we study the effect of
linking fragmented regions on species survival times in the contact process
and related models.

The contact process (CP) [11] is a simple stochastic system that can be
interpreted as a birth-and-death process with spatial structure. Since the CP
undergoes a phase transition between extinction and survival as the
reproduction rate $\lambda$ is varied, it is a reasonable first choice to
model the effect of connecting habitable fragments. In addition, the CP is
readily adapted to model other factors affecting population dynamics, such as
disorder[12, 13, 14] or symbiotic interactions [15, 16, 17].

Here, we study the effect of a corridor linking two square regions on the
survival time of the basic CP, the diffusive CP, and the two-species symbiotic
CP (2SCP)[15]. The rest of this paper is organized as follows. In Sec. 2 we
introduce the models, in Sec. 3 we present a semianalytic solution of the
basic CP on a complete graph, and in Sec. 4 present our simulation results. We
conclude and discuss our findings in Sec. 5.

##  2 Models

The basic CP [11] is a Markov process defined on a lattice. Each lattice site
$i$ is either occupied(active) [$\sigma_{i}=1$] or vacant(inactive)
[$\sigma_{i}=0$]. Transitions from $\sigma_{i}=0$ to 1 occur at a rate
$r\lambda$, where $r$ is the fraction of nearest neighbors (NNs) of site $i$
that are occupied and $\lambda$ is the reproduction rate. The transition from
$\sigma_{i}=1$ to 0 is independent of the neighboring sites and occurs at a
rate of unity (Thus, our time unit corresponds to the mean life of an
organism). The state $\sigma_{i}=0$ for all $i$ is absorbing. The model
exhibits a phase transition from the active to the absorbing state at a
certain critical value $\lambda_{c}$; for the two-dimensional square lattice
studied here $\lambda_{c}=1.64877(3)$ [18]. In finite systems, the only
stationary state is the absorbing state, and one therefore studies the
quasistationary state which describes the statistical properties of surviving
realizations at long times. After an initial stage whose duration depends on
the system size $L$, and on $\Delta=\lambda-\lambda_{c}$, averages over
surviving realizations attain steady values. Considering the evolution
starting from a fully occupied lattice, the survival time $\tau(\Delta,L)$ is
defined such that the survival probability
$P(t)\propto\exp{[-t/\tau(\Delta,L)]}$ for long times $t$. At the critical
point ($\Delta=0$), $\tau(0,R)\sim L^{\nu_{\parallel}/\nu_{\perp}}$, where
$\nu_{\parallel}$ and $\nu_{\perp}$ are critical exponents. In the active
phase ($\Delta>0$), the scaling form for $\tau(L,\Delta)$ in $d$ dimensions is
given by[13],

$\tau(\Delta,L)\sim
L^{\nu_{||}/\nu_{\perp}}\exp[c(L\Delta^{\nu_{\perp}})^{d}],$ (1)

where $c>0$ is a constant. Thus the survival time grows exponentially with the
number of sites in the supercritical regime. In the subcritical regime
($\Delta<0$), the survival time is expected to follow [19],

$\tau(\Delta,L)\sim\left|\Delta\right|^{\nu_{\parallel}}\mathcal{F}\left(\left|\Delta\right|L^{1/\nu_{\perp}}\right),$
(2)

where the scaling function $\mathcal{F}(x)\propto x^{\nu_{\parallel}}$ for
small $x$. In this regime, the survival time approaches a constant value as
$L\to\infty$ .

In addition to the basic CP, we study two variants, each of which includes
features relevant to real populations and/or habitats: (1) The diffusive
contact process in which, in addition to creation and death, each individual
attempts to hop to one of its NN sites at rate $D$ [20, 21]; and (2) the two-
species symbiotic CP (2SCP)[15].

In the 2SCP, two species A and B, can occupy the sites of the lattice. The
allowed states for a site $i$, ($\sigma_{i}$, $\eta_{i}$), are (0,0), (0,1),
(1,0) and (1,1), where $\sigma_{i}$ and $\eta_{i}$ indicate the occupation of
site $i$ by species A and B, respectively. Transitions (0,1) to (0,0) and
(1,0) to (0,0) occur at a rate of unity, while (1,1) to (1,0) or to (0,1)
occur at a rate $\mu$. Transitions (0,0) to (1,0) and (0,1) to (1,1) occur at
a rate $r_{A}\lambda$, where $r_{A}$ is the fraction of NNs containing a
particle of species $A$. Similarly, transitions (0,0) to (0,1) and (1,0) to
(1,1) occur at a rate $r_{B}\lambda$ with $r_{B}$ the fraction of NNs
containing a particle of species $B$. For $\mu=1$, this set of transition
rates describes a pair of independent contact processes cohabiting the
lattice, but for $\mu<1$ the annihilation rate is reduced at sites with both
species, representing symbiosis between the species. Details of the simulation
algorithm can be found in Ref. [15].

The nondiffusive models better describe sessile organisms such as plants. We
interpret one lattice site as corresponding to the area needed to support a
single organism. For example, if we use the largest value for the density of
Eucalyptus salmonophloia reported in Ref. [22], i.e., 170.6 trees per ha, one
site of our simulation corresponds to an area of approx. 60 m2, so that a
square of 40$\times$40 represents an area of about 0.1 km2.

##  3 Contact process on a complete graph

Since the CP is not exactly soluble on finite-dimensional lattices, a reliable
approach to determine the effect of domain connection on lifetime is via Monte
Carlo simulation. Before turning to simulation, we note that a semianalytic
(“numerically exact”) solution is possible for the CP on a complete graph,
that is, a graph of $N$ nodes, all of which which are neighbors. This can be
seen as representing a situation in which organisms are highly mobile,
visiting all parts of their habitat on a time scale much shorter than their
mean lifetime. In this case the state of the system is fully specified by $n$,
the number of active sites. The critical reproduction rate is $\lambda_{c}=1$.
The quasistationary probability distribution $Q(n)$ is readily determined via
an iterative procedure [23]. Then the QS lifetime is $\tau=1/Q(1)$. (In the QS
distribution, $Q(0)$ is zero by definition since the process has never visited
the state $n=0$.)

To study the effect of connecting separate domains, we consider a pair of
complete graphs, of size $N_{1}$ and $N_{2}$ and a single node A, which
represents the corridor. The latter is connected to all nodes in each of the
complete graphs. Thus an organism in region 1 can produce an offspring in A,
which can then produce an offspring in region 2. (Note that there are no
direct connections between regions 1 and 2.)

Let $\tau_{1}$ and $\tau_{2}$ be the mean survival times of the CP populations
restricted to their respective regions when there is no corridor between them.
The mean survival time of the composite region is
max$[\tau_{1},\tau_{2}]\equiv\tau_{0}$. If we connect the two regions via site
A, the survival time grows to $\tau_{c}$. In the case of complete graphs 1 and
2 connected by node A, we can again solve for the quasistationary probability
distribution $Q(n_{1},n_{2},n_{A})$, where $n_{1}$ ranges from zero to $N_{1}$
and similarly for $n_{2}$, and $n_{A}$ is either zero or one. The QS survival
time of the connected system is

$\tau_{c}=\frac{1}{Q(1,0,0)+Q(0,1,0)+Q(0,0,1)}.$ (3)

Figure 1 shows, for a complete graph of $N=50$ nodes, the mean survival time
$\tau_{0}$ as a function of $\lambda-\lambda_{c}$ (red curve), and the
survival time $\tau_{c}$ of the composite system (two complete graphs of 50
nodes linked by a single site - the blue curve). (Recall that the survival
time for two disconnected graphs of the same size is the same as for a single
graph.) Also plotted is the survival time for a single complete graph of 101
sites (black curve). We see that the survival time on the connected system
approaches that of the single nonfragmented habitat.

Figure 2 plots the enhancement ratio $\tau_{c}/\tau_{0}$ versus $\tau_{0}$.
For $\tau_{0}\leq 100$, connecting the two fragments leads to an enhancement
in lifetime of up to a factor of nearly 20.

Figure 1: Contact process on a complete graph: Quasistationary survival time
on a pair of disconnected complete graphs of 50 nodes (red circles), on a
similar pair connected by a single site (blue squares), and on a complete
graph of 101 nodes (black triangles). Figure 2: CP on a complete graph:
Enhancement ratio $\tau_{c}/\tau_{0}$ of the quasistationary lifetime on a
pair of graphs of 50 sites connected by a single site to that in the absence
of the connecting site.

##  4 Simulation results

Figure 3: A pair of square regions $L_{1}=L_{2}=20$ connected by a link of
length $l=5$ and width $w=3$.

We perform Monte Carlo simulations of the three models described in Sec. 2 on
a pair of square regions of sides $L_{1}$ and $L_{2}$ connected by a corridor
of length $l$ and width $w$ (see Fig. 3). The initial configuration is
maximally occupied, i.e., all sites occupied in the CP and the diffusive CP,
and all sites doubly occupied in the 2SCP. The sizes $L_{1}$ and $L_{2}$ range
from 20 to 80; the relative width $\bar{w}=w/L_{1}$, and relative length
$\bar{l}=l/L_{1}$ are varied from $0.05$ to $1.0$. We calculate averages over
samples of 50000 independent realizations. The simulations typically required
up to 6 hours for each value of $\lambda$ and $L_{1}=L_{2}=20$ and up to 350
hours for $L_{1}=L_{2}=40$.

###  4.1 Contact process

We studied the basic CP for reproduction rate values $\lambda>\lambda_{c}$
($\lambda_{c}=1.64874(4)$). As mentioned in Sec. 2, following an initial stage
during which $P(t)=1$, the survival probability decays exponentially with
survival time $\tau$, i.e., $P(t)\propto\exp{[-t/\tau]}$ and the survival time
$\tau$ can be obtained from linear fits to $\ln P(t)$. We first determine the
survival time $\tau_{0}$ of two disconnected regions as a function of the
system size $L$. Fig. 4 shows $\tau_{0}$ for $L=10-60$ and $\lambda=1.66$ and
$1.67$. For fixed reproduction rate, smaller fragments have smaller survival
times, as expected. Since survival times are already large for rather small
systems, we focus on system sizes $L=20$ and 40. Figure 5 shows scaling plots
for $\tau$ on square lattices with periodic boundaries confirming the scaling
form in Eq. 1, and $\tau_{0}$ in the disconnected regions which is equal to
the survival time on a square lattice with open boundaries.

Figure 4:  Basic CP: Survival time $\tau_{0}$ vs system size $L$ for two
disconnected square regions $L=L_{1}=L_{2}$. Figure 5:  Basic CP: Scaling plot
of the survival time $\tau_{0}$ (see Eg. 1)) on a square lattice with periodic
boundaries (PBC) and open boundaries (NPBC). System sizes:  $L=20$,  $L=40$,
$\blacktriangle L=80$.

Figure 6 shows a semilogarithmic plot of the the survival probability P(t) for
$\lambda=1.72$ for two regions, $L_{1}=L_{2}=20$, connected by a corridor of
length $\bar{l}=l/L_{1}=0.05$ and varying width $\bar{w}=w/L_{1}$. Note that
$\bar{w}=0$ corresponds to two disconnected regions and $\bar{w}=1$ to a
rectangular region of $20\times 41$ lattice sites. For fixed reproduction rate
$\lambda$ the survival time increases with the corridor width.

Figure 7 shows the survival time $\tau$ as a function of $\lambda-\lambda_{c}$
for two square regions, (a) $L_{1}=L_{2}=20$ and (b) $L_{1}=20,L_{2}=30$,
connected by a corridor of length $\bar{l}=0.05$ and varying width $\bar{w}$.
The inset shows the enhancement ratio
$e_{\bar{w},\bar{l}}=\tau_{\bar{w},\bar{l}}/\tau_{0}$, where
$\tau_{\bar{w},\bar{l}}$ is the survival time of a pair of regions connected
by a corridor of width $\bar{w}$ and length $\bar{l}$, and $\tau_{0}$ is the
survival time of the disconnected regions ($\bar{w}=0$). Connecting two equal
regions can lead to an enhancement in the survival time by up to a factor of 2
for a corridor of width $\bar{w}=0.05$; the enhancement grows as $\bar{w}$ is
increased. For $L_{1}=20$ and $L_{2}=30$, survival times are greater because
the systems are larger (see Eq. (1)), but the enhancement due to the corridor
is smaller for $\bar{w}=0.05$ and $0.15$ (see inset of Fig. 7(b)).

Figure 6: Survival probability $P$ versus time in the basic CP for
$\lambda=1.72$ ($\lambda-\lambda_{c}=0.0712$) on a pair of regions
($L_{1}=L_{2}=20$) connected by a corridor of length $\bar{l}=l/L_{1}=0.05$
and width (lower to upper) $\bar{w}=w/L_{1}=0,0.05,0.15,0.5$ and
$1$.($\bar{w}=0$ corresponds to a habitat consisting of disconnected regions).

.

Figure 7: Survival time $\tau$ of the basic contact process on a pair of
square regions connected by a corridor of length $\bar{l}=l/L_{1}=0.05$ and
varying widths $\bar{w}=w/L_{1}$ for (a) $L_{1}=L_{2}=20$ and (b)
$L_{1}=20,L_{2}=30$. Symbols: $\bar{w}=0$ ( ); $\bar{w}=0.05$ ( );
$\bar{w}=0.15$ $(\leavevmode\hbox to6.09pt{\vbox
to6.09pt{\pgfpicture\makeatletter\hbox{\hskip 0.2pt\lower-0.2pt\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{
}\definecolor{pgfstrokecolor}{rgb}{0,0,0}\pgfsys@color@rgb@stroke{0}{0}{0}\pgfsys@invoke{
}\pgfsys@color@rgb@fill{0}{0}{0}\pgfsys@invoke{
}\pgfsys@setlinewidth{0.4pt}\pgfsys@invoke{ }\nullfont\hbox
to0.0pt{\pgfsys@beginscope\pgfsys@invoke{ }{{}{{}}{} {{}{}}{}
{}{}\pgfsys@beginscope\pgfsys@invoke{
}\definecolor[named]{pgfstrokecolor}{rgb}{0,1,0}\pgfsys@color@cmyk@stroke{1}{0}{1}{0}\pgfsys@invoke{
}\definecolor[named]{pgffillcolor}{rgb}{0,1,0}\pgfsys@color@cmyk@fill{1}{0}{1}{0}\pgfsys@invoke{
}{}\pgfsys@moveto{0.0pt}{0.0pt}\pgfsys@lineto{5.69055pt}{0.0pt}\pgfsys@lineto{2.84528pt}{5.69055pt}\pgfsys@fillstroke\pgfsys@invoke{
} \pgfsys@invoke{\lxSVG@closescope }\pgfsys@endscope}
\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope{}{}{}\hss}\pgfsys@discardpath\pgfsys@invoke{\lxSVG@closescope
}\pgfsys@endscope\hss}}\lxSVG@closescope\endpgfpicture}})$; $\bar{w}=0.5$ ( );
$\bar{w}=1$ ( ). Insets show the enhancement ratio
$e_{\bar{w},\bar{l}=0.05}=\tau_{\bar{w},\bar{l}=0.05}/\tau_{0}$.

We also studied corridors with dimensions: $\bar{w}=\bar{l}=0.15$ and
$\bar{w}=0.2$, $\bar{l}=0.1$. Figure 8 summarizes the results for the
enhancement ratios $e_{\bar{w},\bar{l}}$ for corridor geometries with
$0.05\leq\bar{w}\leq 0.2$ and $0.05\leq\bar{l}\leq 0.15$ and $L_{1}=L_{2}=20$
and $40$. We focus on these corridor sizes because in real-world applications
the goal is often cost-effective restoration of key corridors ensuring
conservation without purchasing large areas [2]. As expected from Eq. (1),
$\tau$ increases with system size and with $\Delta=\lambda-\lambda_{c}$ as
does the enhancement ratio. Thus, for $L=L_{1}=L_{2}=40$ we need a smaller
$\Delta$ to obtain the same enhancement as in $L=20$ (see Fig. 8). For fixed
corridor length $\bar{l}$ the enhancement increases with $\bar{w}$. By
contrast, when we increase the length of the corridor for a fixed width
$\bar{w}$ the survival time approaches that of the disconnected regions, as
expected (see Fig. 9 (a)). The global quasistationary density $\bar{\rho}$
exhibits similar tendencies (see Fig. 9 (b)).

Figure 8:  Basic CP: Enhancement ratio
$e_{\bar{w},\bar{l}}=\tau_{\bar{w},\bar{l}}/\tau_{0}$. (a) $L_{1}=L_{2}=20$;
(b) $L_{1}=L_{2}=40$. Different symbols indicate different corridor
geometries, as per the legend in panel (a).

Figure 10 shows heat maps of the local density in the quasistationary state
for $L_{1}=L_{2}=40$ and $\lambda-\lambda_{c}=0.0512$ ($\lambda=1.70$), and
different corridor geometries: (a) $\bar{l}=0.05$, $\bar{w}=0.05$, (b)
$\bar{l}=0.05$, $\bar{w}=0.2$, (c) $\bar{l}=0.2$, $\bar{w}=0.05$ and (d)
$\bar{l}=0.2$,$\bar{w}=0.2$. The density in the center of the corridor
increases with $\bar{w}$ and decreases with $\bar{l}$ (see Fig. 9 (c)). The
narrower the corridor the lower the density at its center . As expected, the
local density is smaller near the borders. For $\bar{l}=0.2$, $\bar{w}=0.05$,
the density in the corridor is close to zero (0,0087 in its center), and the
associated enhancement in lifetime is also negligible. Thus a relatively long
narrow corridor represents a barrier for propagation between regions.

Figure 9: Basic CP: $L_{1}=L_{2}=40$ and $\lambda-\lambda_{c}=0.0512$
($\lambda=1.70$).(a) Heat map of the enhancement ratio
$e_{\bar{w},\bar{l}}=\tau_{\bar{w},\bar{l}}/\tau_{0}$; (b) Heat map of the
average global quasi-stationary density $\bar{\rho}$, varying $\bar{w}$ and
$\bar{l}$; (c) Heat map of the density at the center of the corridor
$\rho_{cc}$. Figure 10: Basic CP: Local quasistationary density for
$L_{1}=L_{2}=40$ and $\lambda-\lambda_{c}=0.0512$($\lambda=1.70$). (a)
$\bar{l}=0.05$,$\bar{w}=0.05$; (b) $\bar{l}=0.05$,$\bar{w}=0.2$; (c)
$\bar{l}=0.2$,$\bar{w}=0.05$; (d) $\bar{l}=0.2$,$\bar{w}=0.2$.

###  4.2 Diffusive contact process

Since the critical reproduction rate, $\lambda_{c}(D)$, has not been
determined previously for this model on the square lattice, we begin by
estimating this value in spreading studies. In these studies, performed on a
square lattice of size $L=4000$, each realization begins with a single active
site, and runs to a maximum time $t_{m}=5\times 10^{4}$, if the absorbing
state is not reached prior to this time; averages are calculated over $5\times
10^{5}$ realizations. At the critical point, one expects power-law behavior of
the survival probability $P(t)\sim t^{-\delta}$ and the number of active sites
$n(t)\sim t^{\eta}$, where $\delta$ and $\eta$ are critical exponents [24,
25]. Deviations from power law behavior indicate off-critical values of the
reproduction rate. Fig. 11 (a) illustrates this behavior for $D=2$. We
obtained precise estimates of $\lambda_{c}$, $\delta$ and $\eta$ by analyzing
the local slopes $\delta(t)$ and $\eta(t)$. $\delta(t)$ and $\eta(t)$ are
calculated using linear fits to the data, on the logarithmic scale, on the
interval $[t/a,ta]$ (Here $a=5$). Off-critical values are characterized by a
curvature in plots of local slopes vs $1/t$. Fig. 11 (b) shows the behavior of
$\delta(t)$ and $\eta(t)$ for $D=2$. The estimates for the spreading exponents
are summarized in Table 1 and are in good agreement with the values for the
directed percolation (DP) universality class.

| $\lambda_{c}$ | $\delta$ | $\eta$
---|---|---|---
$D=2$ | 1.37235(5) | 0.46670(2) | 0.22343(3)
$D=5$ | 1.24105(5) | 0.45932(3) | 0.20987(7)
$D=10$ | 1.1596(1) | 0.49202(2) | 0.22411(3)
CP or DP | 1.64877(3) | 0.4523(10) | 0.2293(4)
Table 1: Two-dimensional Diffusive CP: results from spreading simulations.

With the values of $\lambda_{c}(D)$ in hand, we can investigate the effect of
a corridor on the survival time, following the same procedure as in the basic
CP. Figure 12 shows the enhancement ratio $e_{\bar{w},\bar{l}}$ vs
$\lambda-\lambda_{c}$ for two corridor geometries and four values of the
diffusion rate ($D=0,2,5$ and 10) in systems of size $L_{1}=L_{2}=40$. $D=0$
corresponds to the basic CP. Increasing the diffusion rate decreases the
critical reproduction rate $\lambda_{c}$, but do not change the enhancement
ratio $e_{\bar{w},\bar{l}}$, when plotted as function of
$\lambda-\lambda_{c}$, for the corridor geometries studied here.

Figure 11: Diffusive CP on the square lattice: (a) survival probability $P(t)$
and number of active sites $n(t)$ in spreading simulations; (b) Local slopes
$\delta(t)$ and $\eta(t)$ vs $1/t$. $D=2$ and (from bottom to top)
$\lambda=1.3721,1.3723$ and $1.3725$.  Figure 12:  Diffusive CP: Enhancement
ratio $e_{\bar{w},\bar{l}}=\tau_{\bar{w},\bar{l}}/\tau_{0}$ for different
diffusion rates $D$ and two corridor geometries, $\bar{w},\bar{l}$, as
indicated. $L_{1}=L_{2}=40$. $D=0$ corresponds to the basic CP.

###  4.3  Two-species symbiotic contact process

For the two-species symbiotic contact process (2SCP), we studied two values of
the death rate of doubly-occupied sites, $\mu=0.9$ and $0.5$, with critical
reproduction rates $\lambda_{c}=1.64515(5)$ and $\lambda_{c}=1.47290(5)$,
respectively [15]. $\mu=0.9$ is a relatively small variation of the basic
CP($\mu=1$) whereas $\mu=0.5$ represents a rather strong symbiotic
interaction. In the 2SCP the behavior of the survival probability $P(t)$ is
the same as in the basic CP: after an initial period in which it is equal to
one, it decays exponentially with a characteristic survival time $\tau$ (see
Fig. 6); the survival time is obtained from linear fits of $\ln P(t)$.

Figure 13 shows the enhancement ratio $e_{\bar{w},\bar{l}}$ for $\mu=0.9$ and
$\mu=0.5$ and $L_{1}=L_{2}=20$. The enhancement in survival time is grater for
the stronger symbiotic interaction and exceeds that of the basic CP for the
corresponding parameter values (see Table 2).

| CP | 2SCP | 2SCP
---|---|---|---
|  | ($\mu=0.9$) | ($\mu=0.5$)
$\tilde{\lambda}$ | 0.0674 | 0.0634 | 0.0388
$e_{\bar{w}=0.2,\bar{l}=0.05}$ | 3.2 | 4.5 | 6.5
$e_{\bar{w}=0.2,\bar{l}=0.1}$ | 2.8 | 3.8 | 5.3
Table 2: Enhancement ratio
$e_{\bar{w},\bar{l}}=\tau_{\bar{w},\bar{l}}/\tau_{0}$ for the CP and the 2SCP
for two regions $L_{1}=L_{2}=20$. Here
$\tilde{\lambda}=(\lambda-\lambda_{c})/\lambda_{c}$).

Figure 13: 2SCP: Enhancement ratio
$e={\bar{w},\bar{l}}=\tau_{\bar{w},\bar{l}}/\tau_{0}$ for death rates (a)
$\mu=0.9$ and (b) $\mu=0.5$. Different symbols indicate different corridor
geometries. $L_{1}=L_{2}=20$.

##  5 Conclusions

Motivated by the use of corridors to counter extinction of species whose
habitat is fragmented [2, 4], we study the effect of connecting regions on the
survival time in the basic contact process and two variants, the diffusive
contact process, and the two-species symbiotic contact process (2SCP). We
compared the survival time of two square regions connected by corridors of
width $0<\bar{w}\leq 0.2$ and length $0<\bar{l}\leq 0.2$ with that of two
disconnected regions, i.e., $\bar{w}=0$. For the three processes short and
wide corridors provide the best enhancement of the survival time.

In the diffusive CP, $\lambda_{c}$ decreases when the diffusion rate $D$
increases. The enhancement in the survival time, when plotted as function of
$\lambda-\lambda_{c}$, does not vary with $D$ for the same corridor geometry
and unbiased diffusion. The case in which the diffusion is biased toward the
other fragment might be more realistic and will be addressed in future work.

In the 2SCP, increasing the strength of the symbiotic interaction, i.e.,
decreasing the death rate $\mu$ at sites occupied for both species, decreases
$\lambda_{c}$ and increases the enhancement in the survival time. This
suggests that populations with symbiotic interactions enjoy greater benefit
when disconnected fragments are linked via corridors.

The contact processes studied here are extreme simplifications of ecological
systems but offer a qualitative hint of the impact of corridors on population
survival times. In particular, the spatial structure captures the negative
effects of fragmentation such as reduced area and isolation[3, 26]. For the
three variants studied, the survival time of the connected regions,
$\tau_{\bar{w},\bar{l}}$, is in general greater than that of the disconnected
regions, $\tau_{0}$. For the values of $\lambda$, $L_{1}$, $L_{2}$, $\bar{w}$
and $\bar{l}$ considered, the enhancement ratio $e_{\bar{w},\bar{l}}$ ranges
from 3 to 7 in the most favorable cases. This result agrees with field
observations showing that corridors have a positive impact on populations,
aiding in their conservation. Future investigations should address the role of
competition between species, e.g., predator-prey dynamics [27] and
multispecies models [28, 29], as well as the influence of patch arrangement on
species richness and persistence [30, 31, 32, 33].

##  6 Acknowledgments

We thank T. V. Rosembach and D. L. P. Lima for sharing their notes and codes,
S. Pimm and C.N. Jenkins for helpful communication, and Ricardo Martinez-
Garcia and M. M. de Oliveira for helpful comments and discussions. Authors
acknowledge financial support from CAPES (I.I. and A.P.F.). R.D. is grateful
to CNPq for financial support under project number 303766/2016-6.

## References

  * [1] D. World Resources Institute, Washington, Millennium ecosystem assessment, 2005\. ecosystems and human well-being: Biodiversity synthesis.
  * [2] S. L. Pimm, C. N. Jenkins, Connecting habitats to prevent species extinctions, American Scientist 107 (3) (2019) 162–169.  doi:10.1511/2019.107.3.162.
  * [3] N. M. Haddad, L. A. Brudvig, J. Clobert, K. F. Davies, A. Gonzalez, R. D. Holt, T. E. Lovejoy, J. O. Sexton, M. P. Austin, C. D. Collins, W. M. Cook, E. I. Damschen, R. M. Ewers, B. L. Foster, C. N. Jenkins, A. J. King, W. F. Laurance, D. J. Levey, C. R. Margules, B. A. Melbourne, A. O. Nicholls, J. L. Orrock, D.-X. Song, J. R. Townshend, Habitat fragmentation and its lasting impact on earth’s ecosystems, Science Advances 1 (2) (2015).  doi:10.1126/sciadv.1500052.
URL https://advances.sciencemag.org/content/1/2/e1500052

  * [4] J. A. Hilty, A. Keeley, W. Z. L. Jr., A. M. Merenlender, Corridor Ecology: Linking Landscapes for Biodiversity Conservation and Climate Adaptation, Island Press, Washington, DC, 2019.
  * [5] P. Beier, R. F. Noss, Do habitat corridors provide connectivity?, Conservation Biology 12 (6) (1998) 1241–1252.  doi:10.1111/j.1523-1739.1998.98036.x.
  * [6] L. Gilbert-norton, R. Wilson, J. R. Stevens, K. H. Beard, A meta-analytic review of corridor effectiveness, Conservation Biology 24 (3) (2010) 660–668.  doi:https://doi.org/10.1111/j.1523-1739.2010.01450.x.
URL
https://conbio.onlinelibrary.wiley.com/doi/abs/10.1111/j.1523-1739.2010.01450.x

  * [7] N. M. Haddad, L. A. Brudvig, E. I. Damschen, D. M. Evans, B. L. Johnson, D. J. Levey, J. L. Orrock, J. Resasco, L. L. Sullivan, J. J. Tewksbury, S. A. Wagner, A. J. Weldon, Potential negative ecological effects of corridors, Conservation Biology 28 (5) (2014) 1178–1187.  doi:https://doi.org/10.1111/cobi.12323.
URL https://conbio.onlinelibrary.wiley.com/doi/abs/10.1111/cobi.12323

  * [8] B. Hudgens, N. Haddad, Predicting which species will benefit from corridors in fragmented landscapes from population growth models., The American Naturalist 161 (5) (2003) 808–820, pMID: 12858286\.  doi:10.1086/374343.
URL https://doi.org/10.1086/374343

  * [9] I. Hanski, O. Ovaskainen, The metapopulation capacity of a fragmented landscape, Nature 404 (6779) (2000) 755–758.  doi:10.1038/35008063.
URL https://doi.org/10.1038/35008063

  * [10] O. Ovaskainen, I. Hanski, Spatially structured metapopulation models: Global and local assessment of metapopulation capacity, Theoretical Population Biology 60 (4) (2001) 281–302.  doi:https://doi.org/10.1006/tpbi.2001.1548.
URL https://www.sciencedirect.com/science/article/pii/S0040580901915485

  * [11] T. E. Harris, Contact Interactions on a Lattice, The Annals of Probability 2 (6) (1974) 969 – 988\.  doi:10.1214/aop/1176996493.
URL https://doi.org/10.1214/aop/1176996493

  * [12] A. G. Moreira, R. Dickman, Critical dynamics of the contact process with quenched disorder, Phys. Rev. E 54 (1996) R3090–R3093.  doi:10.1103/PhysRevE.54.R3090.
URL https://link.aps.org/doi/10.1103/PhysRevE.54.R3090

  * [13] R. Dickman, A. G. Moreira, Violation of scaling in the contact process with quenched disorder, Phys. Rev. E 57 (1998) 1263–1268.  doi:10.1103/PhysRevE.57.1263.
URL https://link.aps.org/doi/10.1103/PhysRevE.57.1263

  * [14] T. Vojta, A. Farquhar, J. Mast, Infinite-randomness critical point in the two-dimensional disordered contact process, Phys. Rev. E 79 (2009) 011111.  doi:10.1103/PhysRevE.79.011111.
URL https://link.aps.org/doi/10.1103/PhysRevE.79.011111

  * [15] M. M. de Oliveira, R. V. Dos Santos, R. Dickman, Symbiotic two-species contact process, Phys. Rev. E 86 (2012) 011121.  doi:10.1103/PhysRevE.86.011121.
URL https://link.aps.org/doi/10.1103/PhysRevE.86.011121

  * [16] M. M. de Oliveira, R. Dickman, Phase diagram of the symbiotic two-species contact process, Phys. Rev. E 90 (2014) 032120.  doi:10.1103/PhysRevE.90.032120.
URL https://link.aps.org/doi/10.1103/PhysRevE.90.032120

  * [17] C. I. N. Sampaio Filho, T. B. dos Santos, N. A. M. Araújo, H. A. Carmona, A. A. Moreira, J. S. Andrade, Symbiotic contact process: Phase transitions, hysteresis cycles, and bistability, Phys. Rev. E 98 (2018) 062108.  doi:10.1103/PhysRevE.98.062108.
URL https://link.aps.org/doi/10.1103/PhysRevE.98.062108

  * [18] R. Dickman, Reweighting in nonequilibrium simulations, Phys. Rev. E 60 (1999) R2441–R2444.  doi:10.1103/PhysRevE.60.R2441.
URL https://link.aps.org/doi/10.1103/PhysRevE.60.R2441

  * [19] M. M. de Oliveira, R. Dickman, Quasi-stationary simulation: the subcritical contact process, Brazilian Journal of Physics 36 (3A) (2006) 685–689.
  * [20] T. M. Liggett, Interacting Particle Systems, Springer-Verlag Berlin Heidelberg, Berlin, 1985.
  * [21] I. Jensen, R. Dickman, Time-dependent perturbation theory for diffusive non-equilibrium lattice models, Journal of Physics A: Mathematical and General 26 (4) (1993) L151–L157.  doi:10.1088/0305-4470/26/4/005.
URL https://doi.org/10.1088/0305-4470/26/4/005

  * [22] D. A. Norton, R. J. Hobbs, L. Atkins, Fragmentation, disturbance, and plant distribution: Mistletoes in woodland remnants in the western australian wheatbelt, Conservation Biology 9 (2) (1995) 426–438.  doi:https://doi.org/10.1046/j.1523-1739.1995.9020426.x.
URL
https://conbio.onlinelibrary.wiley.com/doi/abs/10.1046/j.1523-1739.1995.9020426.x

  * [23] R. Dickman, R. Vidigal, Quasi-stationary distributions for stochastic processes with an absorbing state, Journal of Physics A: Mathematical and General 35 (5) (2002) 1147–1166.  doi:10.1088/0305-4470/35/5/303.
URL https://doi.org/10.1088/0305-4470/35/5/303

  * [24] P. Grassberger, A. de la Torre, Reggeon field theory (schlögl’s first model) on a lattice: Monte carlo calculations of critical behaviour, Annals of Physics 122 (2) (1979) 373–396.  doi:https://doi.org/10.1016/0003-4916(79)90207-0.
URL https://www.sciencedirect.com/science/article/pii/0003491679902070

  * [25] J. Marro, R. Dickman, Nonequilibrium Phase Transitions in Lattice Models, Cambridge University Press, Cambridge, 1999.
  * [26] I. Ibáñez, D. S. W. Katz, D. Peltier, S. M. Wolf, B. T. C. Barrie, Assessing the integrated effects of landscape fragmentation on plants and plant communities: the challenge of multiprocess-multiresponse dynamics, Journal of Ecology 102 (4) (2014) 882–895, full publication date: July 2014.
URL http://www.jstor.org/stable/24541544

  * [27] J. E. Satulovsky, T. Tomé, Stochastic lattice gas model for a predator-prey system, Phys. Rev. E 49 (1994) 5073–5079.  doi:10.1103/PhysRevE.49.5073.
URL https://link.aps.org/doi/10.1103/PhysRevE.49.5073

  * [28] S. Pigolotti, M. Cencini, D. Molina, M. A. Muñoz, Stochastic spatial models in ecology: A statistical physics approach, Journal of Statistical Physics 172 (1) (2018) 44–73.  doi:10.1007/s10955-017-1926-4.
URL https://doi.org/10.1007/s10955-017-1926-4

  * [29] R. Martinez-Garcia, C. López, F. Vazquez, Species exclusion and coexistence in a noisy voter model with a competition-colonization tradeoff, Phys. Rev. E 103 (2021) 032406.  doi:10.1103/PhysRevE.103.032406.
URL https://link.aps.org/doi/10.1103/PhysRevE.103.032406

  * [30] E. Tjørve, How to resolve the sloss debate: Lessons from species-diversity models, Journal of Theoretical Biology 264 (2) (2010) 604–612.  doi:https://doi.org/10.1016/j.jtbi.2010.02.009.
URL https://www.sciencedirect.com/science/article/pii/S0022519310000767

  * [31] V. Rösch, T. Tscharntke, C. Scherber, P. Batáry, Biodiversity conservation across taxa and landscapes requires many small as well as single large habitat fragments, Oecologia 179 (1) (2015) 209–222.  doi:10.1007/s00442-015-3315-5.
URL https://doi.org/10.1007/s00442-015-3315-5

  * [32] L. Fahrig, Ecological responses to habitat fragmentation per se, Annual Review of Ecology, Evolution, and Systematics 48 (1) (2017) 1–23.  doi:10.1146/annurev-ecolsys-110316-022612.
URL https://doi.org/10.1146/annurev-ecolsys-110316-022612

  * [33] J. A. de Lima Filho, R. J. Vieira, C. A. de Souza, F. F. Ferreira, V. M. de Oliveira, Effects of habitat fragmentation on biodiversity patterns of ecosystems with resource competition, Physica A: Statistical Mechanics and its Applications 564 (2021) 125497.  doi:https://doi.org/10.1016/j.physa.2020.125497.
URL https://www.sciencedirect.com/science/article/pii/S0378437120307950


# Enhanced Laser-Scan Matching
with Online Error Estimation
for Highway and Tunnel Driving

Matthew McDermott  Jason Rife  Tufts University

## biography

Matthew McDermottis a student in the Mechanical Engineering Ph.D. program at
Tufts University in Medford, MA. He works in the Automated Systems and
Robotics Laboratory (ASAR) with Dr. Jason Rife. He received his B.S. and M.S.
degrees in Mechanical Engineering at Tufts University.

Jason Rifeis a Professor and Chair of the Department of Mechanical Engineering
at Tufts University in Medford, Massachusetts. He directs the Automated
Systems and Robotics Laboratory (ASAR), which applies theory and experiment to
characterize integrity of autonomous vehicle systems. He received his B.S. in
Mechanical and Aerospace Engineering from Cornell University and his M.S. and
Ph.D. degrees in Mechanical Engineering from Stanford University.

## Abstract

Lidar data can be used to generate point clouds for the navigation of
autonomous vehicles or mobile robotics platforms. Scan matching, the process
of estimating the rigid transformation that best aligns two point clouds, is
the basis for lidar odometry, a form of dead reckoning. Lidar odometry is
particularly useful when absolute sensors, like GPS, are not available. Here
we propose the Iterative Closest Ellipsoidal Transform (ICET), a scan matching
algorithm which provides two novel improvements over the current state-of-the-
art Normal Distributions Transform (NDT). Like NDT, ICET decomposes lidar data
into voxels and fits a Gaussian distribution to the points within each voxel.
The first innovation of ICET reduces geometric ambiguity along large flat
surfaces by suppressing the solution along those directions. The second
innovation of ICET is to infer the output error covariance associated with the
position and orientation transformation between successive point clouds; the
error covariance is particularly useful when ICET is incorporated into a
state-estimation routine such as an extended Kalman filter. We constructed a
simulation to compare the performance of ICET and NDT in 2D space both with
and without geometric ambiguity and found that ICET produces superior
estimates while accurately predicting solution accuracy.

##  1 Introduction

This paper introduces the Iterative Closest Ellipse Transform (ICET), a novel
lidar scan matching algorithm. ICET is superior to current state-of-the-art
methods of scan matching due to improvements in performance and
interpretability. To the best of our knowledge, there is no existing algorithm
that can, in addition to estimating transformation between point clouds, both
produce online error estimates and predict ambiguous directions as a function
of scene geometry. To understand the utility of the contributions of ICET, it
is important to consider existing algorithms for lidar scan matching.

One of the earliest scan matching methods is the Iterative Closest Point (ICP)
algorithm [1]. ICP works by finding transformations that minimize the
distances between corresponding points in two scans [2]. Though simple, ICP
comes with a number of drawbacks, most importantly the challenge of data
association, where each point in the second scan must be matched correctly to
a point in the first scan. Point-to-Plane ICP simplifies data association by
fitting planes to groups of points and matching those planes between scans
[3]; however, the assumption of planar structure is not often representative
of real-world scenes.

The Normal Distribution Transform is an alternative to ICP and ICP Point-to-
Plane that simplifies data association using an unstructured representation of
the scene: a voxel grid [4]. The grid enforces spatial relationships among
grid clusters without imposing specific geometric models to represent objects
in the scene. NDT also replaces individual points with a density function
thereby reducing sensitivity to noise, particularly during data association.
The most simple implementation of NDT involves optimizing correspondences
between distributions in the reference scan and individual points in the later
scan (P2D-NDT), though more recent implementations have shown an increase in
computational speed with similar performance by optimizing the correspondences
between distribution centers of the two scans (D2D-NDT) [5]. One common
disadvantage of all variants of NDT, however, is the lack of a quantitative
indication of solution quality.

Figure 1: Visualization of ICP, ICP point-to-plane, and NDT. Dotted lines
represent correspondences between various features in each scan.

In many applications, machine-learning based approaches have been shown to be
superior to traditional geometric analyses. With regards to scan matching,
recent machine learning approaches such as LO-Net have achieved performance in
dead reckoning similar to that of geometric based approaches like ICP and NDT
[6]. Unfortunately, the nature of end-to-end neural networks results in
uninterpretable models where it is difficult to predict patterns of error. In
the automotive application, it is particularly important to preserve some
level of human readable information in order to develop a rigorous safety
case.

Our new algorithm ICET addresses limitations of ICP, NDT and LO-Net. To
overcome the data association issues of ICP, ICET uses a voxelized density
function (much like NDT). To enable performance estimation, however, ICET
leverages a nonlinear least-squares solution (as opposed to the optimization
methods used in NDT). Error covariance matrices can be obtained from least-
squares processing, but these covariance matrices are only made meaningful by
introducing a new scene interpretation and dimension reduction step, a step
not used in either ICP or NDT. The resulting algorithm enhances accuracy and
characterizes errors; moreover, in contrast with LO-Net, ICET remains fully
interpretable at every step.

The remainder of this paper presents the ICET algorithm and simulation results
demonstrating its function. Section II highlights how geometric ambiguity
arises and undermines existing scan matching methods. Section III describes
our approach to scan matching using a linear least-squares formulation.
Section IV introduces a technique to eliminate ambiguities from contributing
to scan matching solution. Section V describes simulations to illustrate the
algorithm’s performance. Results of the simulation are discussed in Section
VI. A final section summarizes the paper.

##  2 Geometric Ambiguity

The effectiveness of a scan matching technique is highly dependant on both the
transformation between the two scans and the underlying geometry of the scene.
While it is intuitive that too large a transformation between subsequent scans
may make accurate estimation of their relationship impossible, it should be
noted that the same is true even for small displacements in scenes that lack
distinctive features in one or more direction. Figure 2 illustrates an
example.

Figure 2: Geometrically ambiguous extended surface

The figure visualizes an extended feature, which in this case is a wall. Two
snapshots are taken at different locations along the wall. A scan matching
algorithm would most likely suggest that the two snapshots describe
essentially the same physical location in the world, even though they are in
fact drawn from distinct, non-overlapping locations. However, the same
ambiguity would exist even if the scans were mildly overlapping, because the
scans can only reliably be localized in the direction perpendicular to the
wall and not along it. Similar scenes occur during driving on some highways,
in urban canyons, and in tunnels. In these environments, dominant terrain
features (like the wall in Figure 2) may be aligned with the road such that
scan matching algorithms are only reliable perpendicular to the road and not
along it. An example of an environment in which the dominant features are
aligned with the road is shown in Figure 3.

This ambiguity associated with extended objects, sometimes called the aperture
problem [7], is commonly observed in vision-based and lidar-based
localization. Although the aperture problem is well understood in some
domains, its impact on lidar scan matching algorithms like NDT is not well
understood. One of the primary goals of this paper is to introduce a new
approach to mitigate the aperture problem when lidar scan points are voxelized
as shown in Figure 4.

Figure 3: Road section with potential for high geometric ambiguity along the
axis aligned with the road, courtesy of Rene Schwietzke [8] Figure 4:
Voxelization of Lidar scans for three objects of different sizes: small,
voxel-sized, and extended.

The flaw in the NDT algorithm is in its assumption that the lidar points
within a voxel belong to a random distribution that lies entirely inside the
voxel. This flaw is twofold in that the distribution is neither entirely
random nor necessarily confined within a given voxel. These issues are
visualized in Figure 4, which visualizes three objects (top row), simulated
lidar scans without noise (middle row), and lidar scans with simulated noise
(last row). For a point-like object, one much smaller than the size of a grid
voxel, the NDT assumptions are reasonable. In this case (left column of
figure), the distribution of lidar samples is governed almost entirely by
random sensor noise. For an object on the scale of a voxel (middle column), it
becomes more clear that the distribution of scan points has a largely
deterministic shape, a right angle shape for the vehicle visualized in the
figure. In this middle case, NDT’s assumption that the distribution is driven
by purely random noise begins to break down; however, the Gaussian random-
noise distribution assumed by NDT still provides a meaningful description of
the object. When the object is extended (right column), however, NDT’s
assumptions break down completely. NDT represents the wall as multiple local
Guassian distributions, each independent from its neighbors. Along the wall
direction, localization errors larger than a voxel result in aliasing, with
bad data association caused by lidar points slipping into the next cell along
the wall. This aliasing problem means that NDT implicitly creates an
artificial upper bound for the noise along the length of the extended object.
Because maximum distribution length is clipped by the length of a voxel,
solutions from NDT are calculated with an implied standard deviation of error
in the extended direction of less than one half of the voxel width. This issue
compounds the fact that no single error metric is generated by the NDT
algorithm. In fact, without resolving the aperture problem, it is clear that
it would be very difficult to extract a meaningful accuracy estimate directly
from NDT. Our approach is to resolve these issues with a least-squares based
approach that incorporates a mechanism for mitigating the aperture problem and
that, thereby, enables meaningful online estimates of the accuracy of a lidar
scan match.

##  3 Scan Matching as a Linearized Least-Squares Problem

This section re-envisions voxelized scan matching, replacing the optimization
approach of NDT with a least-squares solution in order to introduce an
analytical approach for estimating solution accuracy. Obtaining an accuracy
estimate for a voxelized lidar scan match is the first novel feature of our
ICET algorithm.

ICET works by estimating the solution vector x that best represents the
transformation between two point clouds. This paper will focus on the 2D case,
where x consists of one rotation angle and two scalar translations: $\theta$,
$x$ and $y$.

$\textbf{x}=\begin{bmatrix}x\\\ y\\\ \theta\\\ \end{bmatrix}$ (1)

For the 3D case, by comparison, the vector would consist of a generalized
rotation description (e.g. Euler angles or quaternions) and a 3D translation
vector. In both the 2D and 3D cases, the transformation vector $\mathbf{x}$
maps the points in the second (or new) scan into the coordinate system used by
the first (or reference) scan. We assign 2D positions ${}^{(i)}\textbf{p}$ to
describe the location of each point $i$ in the new scan using Cartesian {x,y}
coordinates defined in the body-fixed frame at the time of the new scan. The
goal of the scan match is to transform the coordinates ${}^{(i)}\textbf{p}$
into an alternative set of coordinates ${}^{(i)}\textbf{q}$, corresponding to
the original body-fixed frame (at the time reference scan). The
${}^{(i)}\textbf{q}$ are related to the states in (1) as described below.

$^{(i)}\textbf{q}(\textbf{x})=\begin{bmatrix}cos(\theta)&-sin(\theta)\\\
sin(\theta)&cos(\theta)\\\ \end{bmatrix}{}^{(i)}\textbf{p}-\begin{bmatrix}x\\\
y\\\ \end{bmatrix}$ (2)

Averaging the ${}^{(i)}\textbf{q}(\textbf{x})$ over a voxel gives the voxel
mean. Concatenating all of those voxel means into a larger vector gives a
nonlinear observer function $\textbf{y}=\textbf{h}(\textbf{x})$, which can be
compared to a similar concatenated voxel-mean vector $\mathbf{y}_{0}$ from the
original image. Note that here, and subsequently in this paper, variable pairs
like $\mathbf{y}$ and $\mathbf{y}_{0}$ are defined such that the case with no
subscript indicates the new scan and the case with the “$0$” subscript, the
reference scan. Ideally, the voxel-mean vectors are identical for both scans
($\mathbf{y}=\mathbf{y}_{0}$), so $\mathbf{x}$ can be estimated by inverting
the observation equation to solve

$\mathbf{h}(\mathbf{x})=\mathbf{y}_{0}.$ (3)

The goal of ICET is to solve (3) using an iterative Newton-Raphson approach.
The approach works by linearizing the nonlinear equation via a Taylor series
expansion.

$\textbf{y}_{0}=\textbf{h}(\hat{\textbf{x}})+\textbf{H}\delta\textbf{x}+\mathcal{O}(\delta\textbf{x}^{2})$
(4)

Here H is the Jacobian, a matrix of first derivatives of the function h with
respect to each element of x. In the Taylor Series expansion, the current best
estimate is $\hat{\textbf{x}}$. A linear correction $\delta\textbf{x}$ can be
computed assuming that the higher-order terms are zero:

$\textbf{H}\delta\textbf{x}=\Delta\textbf{y}$ (5)

Here $\Delta\textbf{y}=\textbf{y}_{0}-\textbf{h}(\hat{\textbf{x}})$.
Introducing a weighting matrix W, the standard weighted least-squares solution
[9] is

$\delta\textbf{x}=(\textbf{H}^{T}\textbf{WH})^{-1}\textbf{H}^{T}\textbf{W}\Delta\textbf{y}.$
(6)

After computing $\delta\textbf{x}$ using (6), we can update the estimate
$\hat{\textbf{x}}$ according to the following equation, and iterate to
convergence.

$\hat{\textbf{x}}\xrightarrow[]{}\hat{\textbf{x}}+\delta\textbf{x}$ (7)

In order to implement this approach, it is necessary to specify certain terms
used above, by providing the precise definitions of the voxel means y and
$\textbf{y}_{0}$, the Jacobian H, and the weighting matrix W. First consider
the voxel means. A subset of points $i$ from a scan belong to a given voxel
$j$. For the original scan these are $i\in^{(j)}\mathcal{I}_{0}$ and for the
new scan, $i\in^{(j)}\mathcal{I}$. For a given voxel, the mean
${}^{(j)}\textbf{y}_{0}$ of the new-scan points and the mean
${}^{(j)}\textbf{y}$ of the reference-scan points are computed as follows.

$^{(j)}\textbf{y}_{0}=\frac{1}{|^{(j)}\mathcal{I}_{0}|}{\sum_{i\in^{(j)}\mathcal{I}_{0}}}^{(i)}\mathbf{q_{0}}$
(8)

$^{(j)}\textbf{y}(\hat{\textbf{x}})=\frac{1}{|^{(j)}\mathcal{I}|}{\sum_{i\in^{(j)}\mathcal{I}}}^{(i)}\mathbf{q(\hat{\textbf{x}})}$
(9)

The number of points in each subset is the cardinality of that subset,
indicated by the “$|*|$” notation. Statistics are only computed for those
voxels where the number of lidar points exceeds a minimum cutoff;
correspondences are then generated for voxels, such that there remains a
subset of voxels, indexed $j\in J$, matched between the new and reference
scan. The corresponding means (for voxels $j\in J$) are concatenated to form
the larger observation vectors: $\mathbf{y}_{0}$ and
$\mathbf{y}=\mathbf{h}(\hat{\textbf{x}})$.

$\mathbf{y}_{0}=\begin{bmatrix}{}^{(1)}\textbf{y}_{0}\\\
{}^{(2)}\textbf{y}_{0}\\\ ...\\\ {}^{(J)}\textbf{y}_{0}\\\ \end{bmatrix}$ (10)

$\textbf{h}(\hat{\textbf{x}})=\begin{bmatrix}{}^{(1)}\textbf{y}(\hat{\textbf{x}})\\\
{}^{(2)}\textbf{y}(\hat{\textbf{x}})\\\ ...\\\
{}^{(J)}\textbf{y}(\hat{\textbf{x}})\\\ \end{bmatrix}$ (11)

Computing the $\delta\mathbf{x}$ correction in (6) requires differencing the
above two vectors to give
$\Delta\textbf{y}=\textbf{y}_{0}-\textbf{h}(\hat{\textbf{x}})$. If the
dimension of the scan is $N$, with $N=2$ in the 2D case and $N=3$ in the 3D
case, then $\Delta\textbf{y}\in\mathbb{R}^{JN}$. Taking the derivative of (11)
with respect to each variable in $\mathbf{x}$, we can write the Jacobian H in
terms of a submatrix ${}^{(j)}\textbf{H}$ for each $j\in J$.

$\textbf{H}=\begin{bmatrix}{}^{(1)}\textbf{H}\\\ ...\\\ {}^{(J)}\textbf{H}\\\
\end{bmatrix}$ (12)

For the 2D case, the ${}^{(j)}\textbf{H}$ are obtained by substituting (2)
into (9) and taking derivatives with respect to each state in (1).

${}^{(j)}\textbf{H}=\left(\begin{array}[]{cc|c}-1&0&{}^{(j)}\mathbf{H}_{\theta}\\\
0&-1\\\ \end{array}\right)$ (13)

Here the vector ${}^{(j)}\mathbf{H}_{\theta}$ is computed by evaluating
derivatives of (3) with respect to $\theta$ and summing over
$i\in{}^{(j)}\mathcal{I}$.

${}^{(j)}\mathbf{H}_{\theta}=\frac{1}{|^{(j)}\mathcal{I}|}\sum_{i\in{}^{(j)}\mathcal{I}}\begin{bmatrix}-sin(\theta)&-cos(\theta)\\\
cos(\theta)&-sin(\theta)\\\ \end{bmatrix}{}^{(i)}\textbf{p}$ (14)

To evaluate the $\delta\mathbf{x}$ correction in (6), we must still define the
weighting matrix $\mathbf{W}$. For an optimal solution [9], $\Delta\mathbf{y}$
should be unbiased and the covariance of $\Delta\mathbf{y}$ should be inverted
to form the weighting matrix $\mathbf{W}$. The covariance of the observation
vector is typically called the sensor-noise covariance and labeled
$\mathbf{R}$. Thus we expect the weighting matrix to have this form:

$\textbf{W}=\textbf{R}^{-1}$ (15)

In many navigation applications, the matrix $\mathbf{R}$ is determined
strictly from a model of sensor noise. In the case of scan matching, we have a
luxury in that each voxel mean is computed statistically from many data
points, and so we can directly estimate the covariance from the data. For a
given voxel $j$, define the reference-scan covariance estimate (computed
statistically as the central second moment over the point locations) to be
${}^{(j)}\textbf{Q}_{0}$, and define the new-scan estimate to be
${}^{(j)}\textbf{Q}$.

${}^{(j)}\textbf{Q}_{0}=\frac{1}{|{}^{(j)}\mathcal{I}_{0}|-1}{\sum_{i\in{}^{(j)}\mathcal{I}_{0}}}\Big{(}{}^{(i)}\mathbf{q}_{0}-\mathbf{y}_{0}\Big{)}\Big{(}{}^{(i)}\mathbf{q}_{0}-\mathbf{y}_{0}\Big{)}^{T}$
(16)

${}^{(j)}\textbf{Q}(\hat{\mathbf{x}})=\frac{1}{|{}^{(j)}\mathcal{I}|-1}{\sum_{i\in{}^{(j)}\mathcal{I}}}\Big{(}{}^{(i)}\mathbf{q}(\hat{\mathbf{x}})-\mathbf{y}\Big{)}\Big{(}{}^{(i)}\mathbf{q}(\hat{\mathbf{x}})-\mathbf{y}\Big{)}^{T}$
(17)

A well-known result is that the covariance ${}^{(j)}\mathbf{\Sigma}_{y_{0}}$
of the sample mean vector ${}^{(j)}\mathbf{y}_{0}$ can be related to the true
covariance, using (8) and the central limit theorem [10]. Given a sufficient
number of samples in a voxel, the estimated covariances of (16) and (17) can
be substituted for the true covariances. Taking this approach, we estimate
${}^{(j)}\mathbf{\Sigma}_{y_{0}}={}^{(j)}\mathbf{Q}_{0}/|{}^{(j)}\mathcal{I}_{0}|$
and ${}^{(j)}\mathbf{\Sigma}_{y}={}^{(j)}\mathbf{Q}/|{}^{(j)}\mathcal{I}|$.

Next, we note $\mathbf{R}$ describes the variance of $\Delta\mathbf{y}$, which
is formed by differencing and concatenating the mean vectors
${}^{(j)}\mathbf{y}$ and ${}^{(j)}\mathbf{y}_{0}$ for all voxels $j\in J$. For
each voxel, a local covariance ${}^{(j)}\mathbf{R}$ can be defined considering
contributions from ${}^{(j)}\mathbf{y}$ and ${}^{(j)}\mathbf{y}_{0}$. In other
words,
${}^{(j)}\mathbf{R}={}^{(j)}\mathbf{\Sigma}_{y_{0}}+{}^{(j)}\mathbf{\Sigma}_{y}$.
Assuming point distributions are uncorrelated across voxels, then the full
covariance matrix $\mathbf{R}$ is block diagonal with diagonal elements
${}^{(j)}\mathbf{R}$. That is:

$\mathbf{R}=\begin{bmatrix}{}^{(1)}\mathbf{R}&\mathbf{0}&\dots&\mathbf{0}\\\
\mathbf{0}&{}^{(2)}\mathbf{R}&\dots&\mathbf{0}\\\
\vdots&\vdots&\ddots&\vdots\\\
\mathbf{0}&\mathbf{0}&\dots&{}^{(J)}\mathbf{R}\end{bmatrix}$ (18)

When the number of voxels is large, the covariance matrix $\mathbf{R}$ is
rather sparse, so it is inefficient to construct the full matrix. Instead, a
more efficient mechanism for evaluating (6) is to recognize that

$\mathbf{H}^{T}\mathbf{WH}={\sum_{j\in J}}\Bigg{[}{}^{(j)}\mathbf{H}^{T}\,\
{}^{(j)}\mathbf{R}^{-1}\,\ {}^{(j)}\mathbf{H}\Bigg{]}$ (19)

and

$\textbf{H}^{T}\textbf{W}\Delta\textbf{y}={\sum_{j\in
J}}\Bigg{[}{}^{(j)}\mathbf{H}^{T}\,\ {}^{(j)}\mathbf{R}^{-1}\,\
\Big{(}{}^{(j)}\mathbf{y}_{0}-{}^{(j)}\mathbf{y}(\hat{\mathbf{x}})\Big{)}\Bigg{]}.$
(20)

Substituting (19) and (20) into (6) greatly increases efficiency for computing
the correction $\delta\mathbf{x}$. Equation (19) is also very useful for
computing the covariance $\mathbf{P}$, which describes the error of the
estimated state $\hat{\mathbf{x}}$ relative to the true state
$\mathbf{x}_{true}$.

$\mathbf{P}=E[(\hat{\mathbf{x}}-\mathbf{x}_{true})(\hat{\mathbf{x}}-\mathbf{x}_{true})^{T}]$
(21)

Assuming errors are small perturbations around the solution, it is well known
[9] that the linearized equations result in the following expression for the
covariance.

$\mathbf{P}=(\mathbf{H}^{T}\mathbf{WH})^{-1}$ (22)

This last equation represents ICET’s prediction of its own accuracy, a
prediction that automatically accounts for the geometric distribution of
useful voxels and for the quality of the scan points in those voxels.

##  4 Ambiguity Management via Dimension Reduction

One limitation of the prior section is that the methods assume the lidar
points within each voxel are scattered about the mean due to purely random
sensor noise. In fact, objects and natural terrain shape the point
distribution, such that the observed scan has some degree of deterministic
structure. The weighted least-squares solution is optimal only if the point
distribution is random, so this deterministic structure degrades solution
accuracy.

Our approach to limiting the impact of unmodeled deterministic structure is to
exclude measurements in directions where the covariance matrix approaches the
size of the voxel. Deterministic structure is particularly problematic for
objects that extend across multiple voxels (e.g. a long wall), because the
within-voxel covariance provides a misleadingly optimistic estimate of
measurement accuracy. To detect large variance values and identify their
direction, we perform an eigendecomposition on the reference-scan covariance
matrix ${}^{(j)}\mathbf{Q}_{0}$ for each voxel $j\in J$. Each
${}^{(j)}\textbf{Q}_{0}$ can be coverted to an eigenvalue matrix
${}^{(j)}\mathbf{\Lambda}$ and an eigenvector matrix ${}^{(j)}\mathbf{U}$ and
decomposed as follows.

${}^{(j)}\mathbf{Q}_{0}={}^{(j)}\mathbf{U}\,\,\,{}^{(j)}\mathbf{\Lambda}\,\,\,{}^{(j)}\mathbf{U}^{T}$
(23)

The eigenvalue matrix describes the principal axis lengths (squared) for the
variance ellipse. The eigenvector matrix describes the directions of those
principal axes. By testing each eigenvalue to see if it exceeds a reasonable
threshold, we can identify overly extended distributions. The eigenvalue
threshold $T$ is based on the voxel width $a$. It is well known that extended
objects of uniform density along their length (e.g. a uniform-density bar)
have a variance in that direction of $a^{2}/12$. This result is tabulated in
introductory mechanics textbooks like [11], where the variance, or central
second moment, is called a moment of inertia. Noting covariance matrices
computed from lidar data are stochastic, the threshold is reduced slightly to
avoid missed detection of extended objects, to a value of $T=a^{2}/16$. We
also restrict the minimum voxel width to be significantly wider than the
standard deviation of the lidar noise, so that the false detection risk is low
for small objects and normal to surfaces.

Without loss of generality, we can partition the eigenvalue matrix into two
blocks, a first block $\mathbf{\Lambda}_{P}$ that includes the values less
than the threshold $T$ and a second block $\mathbf{\Lambda}_{N}$ that includes
the values greater than or equal to the threshold.

$\mathbf{\Lambda}=\begin{bmatrix}\mathbf{\Lambda}_{P}&0\\\
0&\mathbf{\Lambda}_{N}\\\ \end{bmatrix}$ (24)

Similarly, we can partition the corresponding eigenvectors into two
submatrices, the eigenvectors $\mathbf{U}_{P}$ describing the dimensions
preserved and the eigenvectors $\mathbf{U}_{N}$ describing the dimensions
eliminated.

$\mathbf{U}=\begin{bmatrix}\mathbf{U}_{P}&\mathbf{U}_{N}\\\ \end{bmatrix}$
(25)

We can then project each voxel-mean vector into the preserved directions,
eliminating the other directions. This operation is only necessary when the
eigenvalue dimension is reduced yet not trivial (i.e. when
$\mathbf{\Lambda}\in\mathbb{R}^{N\times N}$,
$\mathbf{\Lambda}_{P}\in\mathbb{R}^{n\times n}$, and $0<n<N$). To account for
all cases, we introduce a modified voxel mean, indicated by a tilde, as in
${}^{(j)}\tilde{\mathbf{y}}$.

${}^{(j)}\tilde{\mathbf{y}}=\begin{cases}{}^{(j)}\mathbf{y},&n=N\\\
{}^{(j)}\mathbf{U}_{P}^{T}\,\ {}^{(j)}\mathbf{y},&0<n<N\\\
\emptyset,&n=0\end{cases}$ (26)

The third instance of (26), the null result, occurs only for distributions
wide in all directions, as in the case of loose foliage.

To compute our solution via (6), the dimension-reduction process must also be
applied to other variables including ${}^{(j)}\mathbf{y}_{0}$,
${}^{(j)}\mathbf{H}$ and ${}^{(j)}\mathbf{R}$. First, the voxel means
${}^{(j)}\mathbf{y}_{0}$ for the reference scan become
${}^{(j)}\tilde{\mathbf{y}}_{0}$, which has a structure analagous to (26), but
with ${}^{(j)}\mathbf{y}_{0}$ substituted for ${}^{(j)}\mathbf{y}$. Second,
reducing the dimensions of ${}^{(j)}\mathbf{H}$ and ${}^{(j)}\mathbf{R}$ gives
${}^{(j)}\tilde{\mathbf{H}}$ and ${}^{(j)}\tilde{\mathbf{R}}$, defined as
follows.

${}^{(j)}\tilde{\mathbf{H}}=\begin{cases}{}^{(j)}\mathbf{H},&n=N\\\
{}^{(j)}\mathbf{U}_{P}^{T}\,\ {}^{(j)}\mathbf{H},&0<n<N\\\
\emptyset,&n=0\end{cases}$ (27)

${}^{(j)}\tilde{\mathbf{R}}=\begin{cases}{}^{(j)}\mathbf{R},&n=N\\\
{}^{(j)}\mathbf{U}_{P}^{T}\,\ {}^{(j)}\mathbf{R}\,\
{}^{(j)}\mathbf{U},&0<n<N\\\ \emptyset,&n=0\end{cases}$ (28)

In all instances, the tilde indicates the dimension-reduction process. Using
the dimension-reduced variables, solution steps (19) and (20) can be
rewritten, respectively, as follows.

$\mathbf{H}^{T}\mathbf{WH}={\sum_{j\in
J}}\Bigg{[}{}^{(j)}\tilde{\mathbf{H}}^{T}\,\
{}^{(j)}\tilde{\mathbf{R}}^{-1}\,\ {}^{(j)}\tilde{\mathbf{H}}\Bigg{]}$ (29)

$\textbf{H}^{T}\textbf{W}\Delta\textbf{y}={\sum_{j\in
J}}\Bigg{[}{}^{(j)}\tilde{\mathbf{H}}^{T}\,\
{}^{(j)}\tilde{\mathbf{R}}^{-1}\,\
\Big{(}{}^{(j)}\tilde{\mathbf{y}}_{0}-{}^{(j)}\tilde{\mathbf{y}}\Big{)}\Bigg{]}.$
(30)

Equations (29) and (30) can be substituted into (6) and, in most cases,
iterated to obtain the Newton-Raphson solution. There is one corner case to
consider, however. If all of the information in one direction is removed (e.g.
for the case where the only feature is a straight wall that stretches across
the entire scan), then (29) will be rank deficient and therefore non-
invertible. A method for handling this special case is discussed in the
Appendix.

The full algorithm, including both the basic solution and dimension reduction,
is summarized in the table labeled Algorithm 1.

1:Initialize solution vector x $\triangleright$ (1)

2:Break down reference scan into voxels

3:Calculate mean and covariance matrix for voxels in reference scan with more
than $\tau$ points $\triangleright$ (8), (16)

4:Calculate $U$ and $L$ matrices for the reference scan in each qualifying
voxel $\triangleright$ (23)

5:while $\delta\textbf{x}$ is converging do

6: Transform second point cloud by x $\triangleright$ (2)

7: Voxelize transformed point cloud using the same grid used to subdivide the
reference scan

8: Compute the new-scan mean and covariance of lidar points in each qualifying
voxel $\triangleright$ (9),(17)

9: Define a nearest-neighbor correspondence between distributions in each scan

10: for each correspondence j do

11: Compute the reduced-dimension means ${}^{(j)}\tilde{\mathbf{y}}_{0}$ and
${}^{(j)}\tilde{\mathbf{y}}$$\triangleright$ (26)

12: Calculate ${}^{(j)}\tilde{\mathbf{H}}$ $\triangleright$ (27)

13: Calculate the sensor-noise matrix
${}^{(j)}\tilde{\mathbf{R}}$$\triangleright$ (28)

14: Compute intermediate quantities as running sums $\triangleright$ (29),(30)

15: end for

16: Calculate condition number of $\mathbf{H}^{T}\mathbf{WH}$ $\triangleright$
(31)

17: if condition number below cutoff then

18: Determine the state correction $\delta\mathbf{x}$ $\triangleright$ (6)

19: $\hat{\mathbf{x}}\xrightarrow{}\hat{\mathbf{x}}+\delta\mathbf{x}$
$\triangleright$ (7)

20: Calculate state-error covariance $\mathbf{P}$ $\triangleright$ (22)

21: else (See Appendix for details)

22: Determine the subspace correction $\delta\mathbf{z}$ and update
$\mathbf{x}^{\prime}$ $\triangleright$ (33),(34)

23:
$\hat{\mathbf{x}}\xrightarrow{}\hat{\mathbf{x}}+\mathbf{x}^{\prime}$$\triangleright$
(35)

24: Calculate subspace-error covariance $\mathbf{\Gamma}_{P}^{-1}$
$\triangleright$ (37)

25: end if

26:end while

Algorithm 1 ICET

##  5 Simulation

To demonstrate the central contributions of this paper, ICET was benchmarked
against NDT. An implementation of NDT was programmed from scratch using the
standard algorithm and optimization routine provided by Biber [4]. Monte-Carlo
(MC) simulations of NDT and ICET were conducted through two scenarios
featuring simple, simulated driving environments, as illustrated in Figure 5.
The first scenario represents a T-intersection. The second scenario represents
a straight tunnel or roadway. The T-intersection case involves a scene in
which there are both along-track and cross-track features present in the
frame. The straight-tunnel case involves a scene of high geometric ambiguity
in which there are only along-track features. Both cases were represented in
two-dimensions, with vertical walls assumed on the sides of the roadway. The
units of the environment were somewhat arbitrary (but each unit length is
meant to correspond to approximately 0.05 cm of physical distance).

Each MC simulation considered only one environment. For a given environment,
each MC trial considered a pair of scans: a reference scan and a new scan
depicting the scene after movement. Examples of scan pairs are shown in Figure
5, where the reference scan is green and the new scan, blue. Grid lines in the
figure correspond to voxel boundaries, with voxels of about 50 units on a side
(about 2.5 m). Mean and covariance of lidar points in each voxel is ilustrated
by a two-sigma ellipse. The illustrations indicate that the motion was small,
with a translation of about 5 units laterally and 10 units vertically (about
0.25 m laterally and 0.5 m vertically). The yaw rotation between scans was 0.1
radian. The same translation and rotation was applied between scans in all
trials, but lidar noise was re-sampled for each trial. Simulated lidar scans
were constructed with 4200 samples. Gaussian noise with a standard deviation
of 2 units in the $x$ and $y$ directions was applied to each sample.

Note that both the ICET and NDT algorithms were implemented with two
correspondence methods. The baseline approach defines correspondences only for
means co-located within the same grid cell. A modified approach defines
correspondences using the 1-NN or ”nearest neighbor” method, which provides
more robust initialization. Simulations were run using both correspondence
methods with no statistically significant difference in solution accuracy.
Therefore, only results from simulations using the nearest-neighbor approach
are presented below.

Figure 5: Sample transformations for T-intersection and straight tunnel case.
Voxels shown as square grid cells. Each ellipse represents the two-sigma
covariance ellipse for a grid cell. Lines are drawn across dimensions of
distributions in the first scan that have not been reduced.

##  6 Results and Discussion

Results were computed from MC simulations with 1000 trials for each
environment. In each trial, the error was computed as
$\hat{\mathbf{x}}-\mathbf{x}_{true}$. Mean and variance for lateral (or
$x$-direction) error, longitudinal (or $y$-direction) error, and rotational
errors were computed for each environment, for both the NDT and ICET
algorithms. A predicted state-error covariance matrix was also computed for
ICET.

The distributions appeared zero centered, with no evidence of a systematic
mean error across trials. Standard deviations (labeled std error) are
tabulated below (in simulation units) for the two translation directions and
for rotation.

The tunnel geometry was specially configured to suppress all geometric
information in one dimension, in order to create a singularity in the
computation of (6). To address this issue, a specialty solution was applied,
as defined by (33) in the Appendix. In all of the tunnel-geometry trials, ICET
was able to correctly identify the direction of geometric ambiguity and
suppress the solution along that axis. For this reason the $y$ direction error
is listed as N/A for ICET; the measurements were identified as ambiguous, so
no solution was attempted in that direction.

The error standard deviations for ICET were consistently better by nearly an
order of magnitude as compared to NDT, presumably because the dimension
reduction process eliminates processing in ambiguous directions, which NDT
believes to be well-characterized (e.g. modeling point-cloud uncertainty based
on the voxel dimension, even for environmental features that extend across
many voxels). The extreme case of this trend occurred for the tunnel
environment, where NDT delivered a lateral error two orders of magnitude
larger than the longitudinal error, without enunciating any warning about a
potential fault caused by ambiguity associated with extended wall features.

The predictive error-covariance matrices computed internally by the ICET
algorithm (labeled as ”ICET Predicted”) were consistently representative of
the true error distribution (labeled ”ICET Actual”).

### T-Intersection Case

Algorithm |  std error $x$ |  std error $y$ |  std error $\theta$ (rad)
---|---|---|---
NDT Actual |  0.554 |  0.490 |  0.00251
ICET Actual |  0.1047 |  0.0545 |  0.00035
ICET Predicted |  0.101 |  0.0602 |  0.00035

### Straight Tunnel Case (High Geometric Ambiguity)

Algorithm |  std error $x$ |  std error $y$ |  std error $\theta$ (rad)
---|---|---|---
NDT Actual |  0.330 |  39.26 |  0.00280
ICET Actual |  0.0437 |  N/A |  0.00031
ICET Predicted |  0.0454 |  Excluded from Solution |  0.00032

The results in the table indicate, at least in this simple test case, that
ICET predicts accuracy well and that the algorithm identifies instances of
geometric ambiguity, thereby verifying our two major claims about the ICET
algorithm. It is interesting, too, that the accuracy of ICET, at least on this
data set, is noticeably higher than the accuracy of NDT.

While ICET effectively negates the effects of ambiguity from extended surfaces
(e.g. walls), ambiguity may still occur from other geometric conditions.
Regularly spaced features of equal size (e.g. columns) may contribute to
aliasing. However, extended flat surfaces remain the most common form of
geometric ambiguity in automotive applications [12].

The ability for ICET to output a predicted error covariance makes the
algorithm particularly useful in the context of Bayesian filtering algorithms,
such as the extended Kalman filter. Constructing a Bayesian filter requires
modeling sensor noise. Currently error models are available for individual
lidar data points [13] but relatively little information is available [14] to
characterize the output of a lidar algorithm such as NDT. The ICET algorithm
sidesteps this gap in the literature by directly estimating the accuracy of
its output states. Moreover, the ICET accuracy prediction automatically adapts
to reflect different environments and terrains, so it is not necessary to rely
on the simplistic (though convenient) assumption that algorithm accuracy is
the same under all conditions.

One limitation of ICET is that performance is somewhat dependent on the
resolution and alignment of the voxel grid. Rectangular voxels are an
attractive choice in that their regularity makes it easy to index and assign
lidar points to individual voxels; however, rectangular voxels also have
practical limitations, particularly in cases in which extended world features
are not aligned with the grid. This issue can be resolved for NDT by using an
overlapping grid [15]. For ICET, a new problem is that our dimension reduction
approach assumes the extended object cuts through the middle of a cell rather
than, say, across its corner. The apparent width of a wall passing through the
corner of a voxel would appear to be small, and the feature would not
necessarily be excluded if the wall were aligned at an arbitrary angle
relative to the grid. This is expected to somewhat reduce the benefits of
ICET’s dimension-reduction step when applied to arbitrary data sets.

##  7 Summary

A new technique for matching point clouds, the Iterative Closest Ellipsoidal
Transform (ICET), was introduced which provides two novel improvements over
existing voxelized scan-match methods such as the Normal Distributions
Transform (NDT). The first contribution of ICET is the ability to estimate
solution-error covariance (and thus the expected standard deviation of error
for each component of translation and rotation in the solution vector) as a
function of scene geometry. This is accomplished by first subdividing each
scan into voxel grids and treating the distribution of points in each voxel as
a probability density function. A linear least-squares solution is then
calculated to determine the optimal transformation to minimize the differences
between distribution centers for the new scan and a reference scan. This
process is iterated multiple times to account for nonlinearities. The second
major contribution of ICET allows the algorithm to ignore contributions to the
solution vector from locally ambiguous directions (such as along the surface
of a long smooth wall) while keeping information in useful directions (such
normal to the wall).

Performance of ICET was verified in a simulated 2D environment against a
benchmark implementation of NDT. In Monte-Carlo simulations of 1000 trials,
ICET achieved an 80% reduction in estimation error while successfully
predicting the standard deviation of error for all components of rotation and
translation. The ability to estimate accuracy has utility for sensor fusion
algorithms (e.g. a Kalman Filter), especially since ICET can dynamically
adjust accuracy estimates on the fly, which avoids reliance on static - and
likely overconservative - accuracy estimates. Furthermore, in a pathological
straight-tunnel simulation, ICET was able to successfully identify the
ambiguous dimension in all trials and suppress the solution along that axis.
By contrast, in the same situation, NDT naively provides inaccurate estimates
without warning that an ambiguity is present. The ambiguity-handling
properties of ICET are especially important for autonomous vehicle
applications, where tunnels and urban canyons may introduce ambiguity in the
form of the aperture problem.

## acknowledgements

The authors wish to acknowledge and thank the U.S. Department of
Transportation Joint Program Office (ITS JPO) and the Office of the Assistant
Secretary for Research and Technology (OST-R) for sponsorship of this work.
Opinions discussed here are those of the authors and do not necessarily
represent those of the DOT or other affiliated agencies.

## Appendix

As discussed in Section 4, the dimension-reduction process can, in rare cases,
result in a solution singularity. These special cases can be detected
numerically by analyzing (29) to compute the matrix condition number, which
for this symmetric and positive-definite matrix is the ratio of its maximum to
minimum eigenvalue. Matrices with high condition number are not practical to
invert, and so in the case where the condition number is high (larger than,
say, $10^{5}$), then the lowest eigenvalue of (29) can be eliminated
iteratively until the condition number falls below the threshold. This process
eliminates dimensions from the solution, in contrast with the prior dimension-
reduction step, which eliminated dimensions from the measurement.

In order to reduce the dimension of the solution, start by defining an
eigendecomposition for (29), involving the eigenvector matrix $\mathbf{V}$ and
the eigenvalue matrix $\mathbf{\Gamma}$.

$\mathbf{H}^{T}\mathbf{WH}=\mathbf{V}\mathbf{\Gamma}\mathbf{V}^{T}$ (31)

Again the eigenvalues can be partitioned into a submatrix of preserved
eigenvalues $\mathbf{\Gamma}_{P}$, which satisfy the condition number
requirement, and a submatrix of eliminated eigenvalues $\mathbf{\Gamma}_{N}$,
which includes the approximately singular eigenvalues, meaning the low
eigenvalues removed to meet the condition number requirement. The eigenvector
matrix $\mathbf{V}$ can also be partitioned into the preserved and eliminated
eigenvectors, $\mathbf{V}_{P}$ and $\mathbf{V}_{N}$, respectively. We can then
solve a modified version of (6) in the well-conditioned subspace. To this end,
we define coordinates $\delta\mathbf{z}$ for the well-condition subspace where

$\delta\mathbf{z}=\mathbf{V}_{P}^{T}\ \delta\mathbf{x}.$ (32)

Substituting (31) into into (6), multiplying by $\mathbf{V}_{P}^{T}$, and
introducing (32), the iterative Newton-Raphson correction (6) becomes

$\delta\mathbf{z}=\mathbf{\Gamma}_{P}^{-1}\ \mathbf{V}_{P}^{T}\
(\mathbf{H}^{T}\mathbf{W}\Delta\mathbf{y})$ (33)

The corrections are accumulated into the variable $\mathbf{x}^{\prime}$
(initialized to zero), such that

$\mathbf{x}^{\prime}\xrightarrow[]{}\mathbf{x}^{\prime}+\mathbf{V}_{P}\
\delta\mathbf{z}.$ (34)

At each Newton-Raphson stage, the estimated state $\hat{\mathbf{x}}$ is
computed as the initial state $\bar{\mathbf{x}}$ plus the correction from
(34).

$\hat{\mathbf{x}}=\bar{\mathbf{x}}+\mathbf{x}^{\prime}$ (35)

When the iterations converge, there is only confidence in the solution in the
coordinates of the well-conditioned subspace, associated with the directions
of the vectors $\mathbf{V}_{P}$. For this reason, it is helpful to map the
correction term $\mathbf{x}^{\prime}$ back into the well-conditioned subspace
as $\mathbf{z}$, in order to accurately express the solution covariance. This
mapping has the form of a general observation equation, characterized by an
observation matrix $\mathbf{C}$.

$\mathbf{z}=\mathbf{C}\mathbf{x}^{\prime}$ (36)

In this case, the observation matrix is simply the projection matrix
$\mathbf{C}=\mathbf{V}_{P}^{T}$. The uncertainty associated with this the
difference between the estimate $\hat{\mathbf{z}}$ and the true value
$\mathbf{z_{true}}$ is characterized by the following covariance.

$E[(\hat{\mathbf{z}}-\mathbf{z}_{true})(\hat{\mathbf{z}}-\mathbf{z}_{true})^{T}]=\mathbf{\Gamma}^{-1}_{P}$
(37)

Result (37) has a dimension that is smaller than the size of the full-state
covariance (22), reflecting the fact that there is no confidence in the
solution in the eliminated directions, those associated with $\mathbf{V}_{N}$.

## References

  * [1] P.. Besl and N.. McKay  “A method for registration of 3-D shapes”  In _IEEE Transactions on Pattern Analysis and Machine Intelligence_ 14.2, 1992, pp. 239–256  DOI: 10.1109/34.121791
  * [2] Y. Chen and G. Medioni  “Object modeling by registration of multiple range images”  In _Proceedings. 1991 IEEE International Conference on Robotics and Automation_ , 1991, pp. 2724–2729 vol.3  DOI: 10.1109/ROBOT.1991.132043
  * [3] K.. Low  “Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration”, 2004
  * [4] P. Biber and W. Straßer  “The Normal Distributions Transform: A New Approach to Laser Scan Matching”  In _IEEE International Conference on Intelligent Robots and Systems_ 3, 2003, pp. 2743–2748 vol.3  DOI: 10.1109/IROS.2003.1249285
  * [5] Min Jiang et al.  “Scan registration for mechanical scanning imaging sonar using kD2D-NDT”, 2018, pp. 6425–6430  DOI: 10.1109/CCDC.2018.8408259
  * [6] Q. Li et al.  “LO-Net: Deep Real-Time Lidar Odometry”, 2019, pp. 8465–8474  DOI: 10.1109/CVPR.2019.00867
  * [7] Shinsuke Shimojo, Gerald H Silverman and Ken Nakayama  “Occlusion and the solution to the aperture problem for motion”  In _Vision research_ 29.5 Elsevier, 1989, pp. 619–626
  * [8] Rene Schwietzke  “Interstate I-93 Tunnel in Boston, part of the Big Dig”, 2005  URL: https://commons.wikimedia.org/wiki/File:Tunnel-large.jpg
  * [9] D. Simon  “Optimal State Estimation: Kalman, H Infinity, and Nonlinear Approaches”  Wiley & Sons, 2006
  * [10] NIST  “Normal Distribution”  In _NIST/ SEMATECH e-Handbook of Statistical Methods_ , 2013, pp. 1.3.6.6  DOI: 10.18434/M32189
  * [11] Ferdinand Pierre Beer, Ralph E Flori and E Russell Johnston  “Mechanics for engineers: dynamics”  McGraw-Hill, 2007
  * [12] Y.. Park, H. Jang and A. Kim  “I-LOAM: Intensity Enhanced LiDAR Odometry and Mapping”  In _2020 17th International Conference on Ubiquitous Robots (UR)_ , 2020, pp. 455–458  DOI: 10.1109/UR49135.2020.9144987
  * [13] Silvère Bonnabel, Martin Barczyk and François Goulette  “On the covariance of ICP-based scan-matching techniques”  In _2016 American Control Conference (ACC)_ , 2016, pp. 5498–5503  DOI: 10.1109/ACC.2016.7526532
  * [14] Ashwin Vivek Kanhere and Grace Xingxin Gao  “LiDAR SLAM utilizing normal distribution transform and measurement consensus”  In _Proceedings of the 32nd International Technical Meeting of the Satellite Division of The Institute of Navigation (ION GNSS+ 2019)_ , 2019, pp. 2228–2240
  * [15] H. Hong, H. Kim and B.. Lee  “Accuracy Evaluation of Registration of 3D Normal Distributions Transforms Interpolated by Overlapped Regular Cells”  In _2018 18th International Conference on Control, Automation and Systems (ICCAS)_ , 2018, pp. 1616–1619