monthyeardate[] ,

# Probabilistic Scan Matching:
Bayesian Pose Estimation from Point Clouds

Rico Mendrzik∗ and Florian Meyer†
∗Ibeo Autmotive Systems GmbH, Hamburg, Germany
†University of California San Diego, San Diego, USA
Email<EMAIL_ADDRESS><EMAIL_ADDRESS>
###### Abstract

Estimating position and orientation change of a mobile platform from two
consecutive point clouds provided by a high-resolution sensor is a key problem
in autonomous navigation. In particular, scan matching algorithms aim to find
the translation and rotation of the platform such that the two point clouds
coincide. The association of measurements in point cloud one with measurements
in point cloud two is a problem inherent to scan matching. Existing methods
perform non-probabilistic data association, i.e., they assume a single
association hypothesis. This leads to overconfident pose estimates and reduced
estimation accuracy in ambiguous environments. Our probabilistic scan matching
approach addresses this issue by considering all association hypotheses with
their respective likelihoods. We formulate a holistic Bayesian estimation
problem for both data association and pose estimation and present the
corresponding joint factor graph. Near-optimum maximum a posteriori (MAP)
estimates of the sensor pose are computed by performing iterative message
passing on the factor graph. Our numerical study shows performance
improvements compared to non-probabilistic scan matching methods that are
based on the normal distributions transform (NDT) and implicit moving least
squares (IMLS).

##  I Introduction

High-resolution sensors for situational awareness such as Light Detection and
Ranging (LiDAR) and imaging sonar are paramount for autonomous navigation due
to their unique sensing capabilities. LiDAR sensors can provide extremely high
resolution accompanied by precise depth information and are thus ideal for
automated driving and advanced driver assistance systems (ADAS) [1]. Imaging
sonar generates accurate echo intensity profiles of the scanned area and can
enable autonomous underwater vehicles (AUV) to navigate in an unknown and
possibly unstructured environment [2]. Beyond object tracking and
classification or environmental perception, high-resolution sensors can be
cornerstones for ego-state estimation which can help navigating in challenging
environments [3, 4, 5, 6, 7, 8, 9, 10]. By matching two scans obtained at
different time instances, information on the change of position and
orientation (also known as relative pose) of the sensor can be extracted. A
simplistic example from the LiDAR domain is depicted in Fig. 1.

State-of-the-art scan matching approaches [3, 4, 5, 6, 7, 8, 9, 10] typically
use heuristics that require a good initial guess on the relative pose of the
sensor. Moreover, they rely on a key assumption: The association of scan data
can be described by a single deterministic association hypothesis. This
assumption can lead to poor and overconfident estimates of the relative pose
if the scans contain ambiguous data or non-associable artifacts. State-of-the-
art algorithms typically fail if the scan data association is erroneous, the
generating environment does not allow for an unambiguous estimation of the
relative pose, or scan points originate from detection errors belong to
dynamic objects. Another frequently-occurring challenge of the scan matching
paradigm, is partial observability of the environment, which describes the
phenomenon that two scans recorded at different poses almost never depict the
identical scene.

![width=.999keepaspectratio]Figures/scenario/scenario

Figure 1: Scenario: Our goal is to infer the relative pose,
$\Delta\bm{\mathsfbr{P}}$, of LiDAR sensor (concentric circles) by matching
the source point cloud $\bm{s}=[\bm{s}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\bm{s}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{s}^{(N_{\mathrm{S}})\mathrm{T}}]^{\mathrm{T}}$ (green dots) and
destination surface, consisting of scan points
$\bm{d}=[\bm{d}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{d}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{d}^{(N_{\mathrm{D}})\mathrm{T}}]^{\mathrm{T}}$ (magenta diamonds)
and normal vectors $\bm{n}=[\bm{n}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\bm{n}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{n}^{(N_{\mathrm{D}})\mathrm{T}}]^{\mathrm{T}}$ (magenta arrows).
Laser rays are indicated by thin green and magenta lines originating from the
LiDAR sensor.

In contrast to state-of-the-art method, we formulate scan matching as a
Bayesian inference problem. This approach aims to increase resilience to
sensing artifacts and partial observability by performing scan associations
probabilistically [11, 12, 13, 14]. Our main contributions can be summarized
as follows:

  * •

We derive a stochastic model which entangles both scan association and pose
estimation in a holistic fashion.

  * •

We present a Bayesian inference algorithm for relative pose estimation based
on probabilistic scan matching.

Notation: Random variables are displayed in sans serif, upright fonts; their
realizations in serif, italic fonts. Vectors and matrices are denoted by bold
lowercase and uppercase letters, respectively. For example, a random variable
and its realization are denoted by $\mathsfbr{x}$ and $x$; a random vector and
its realization are denoted by $\bm{\mathsfbr{x}}$ and $\bm{x}$. Sets are
denoted by calligraphic font, e.g. ${\cal{X}}$. Furthermore,
${\bm{x}}^{\text{T}}$ denotes the transpose of vector $\bm{x}$. $f(\bm{x})$
denotes the probability density function (PDF) or probability mass function
(PMF) of continuous or discrete random vector $\bm{\mathsfbr{x}}$. Finally,
$\bm{0}_{n}$ is the row vector of size $n$ containing all zeros.

##  II Fundamentals and Problem Formulation

We define the notion of poses, describe our sensor and measurement models, and
present our problem formulation.

###  II-A Preliminaries

####  II-A1 Pose Representation

We refer to the position and orientation of a rigid object as pose. The pose
of the sensor in $N_{\mathrm{dim}}$-dimensional space can be conveniently
represented by the following matrix from the special Euclidean group
$\textit{SE}(N_{\text{dim}})$[15]

$\bm{\mathsfbr{P}}=\begin{pmatrix}\bm{\mathsfbr{R}}&\bm{\mathsfbr{t}}\\\
\bm{0}_{N_{\text{dim}}}&1\end{pmatrix}\vspace{0mm}$

where $\bm{\mathsfbr{R}}$ is a $N_{\text{dim}}\times N_{\text{dim}}$ rotation
matrix and $\bm{\mathsfbr{t}}\in\mathbb{R}^{N_{\text{dim}}}$ is a translation
vector. From source pose $\bm{\mathsfbr{P}}_{\mathrm{s}}$ and destination pose
$\bm{\mathsfbr{P}}_{\mathrm{d}}$, we can determine the relative pose
$\Delta\bm{\mathsfbr{P}}\in\textit{SE}(N_{\text{dim}})$ as

$\Delta\bm{\mathsfbr{P}}=\bm{\mathsfbr{P}}_{\mathrm{d}}\bm{\mathsfbr{P}}_{\mathrm{s}}^{-1}=\begin{pmatrix}\Delta\bm{\mathsfbr{R}}&\Delta\bm{\mathsfbr{t}}\\\
\bm{0}_{N_{\text{dim}}}&1\end{pmatrix}$

where the $N_{\text{dim}}\times N_{\text{dim}}$ rotation matrix
$\Delta\bm{\mathsfbr{R}}$ and the translation vector
$\Delta\bm{\mathsfbr{t}}\in\mathbb{R}^{N_{\text{dim}}}$ describe the change of
orientation and displacement between $\bm{\mathsfbr{P}}_{\mathrm{s}}$ and
$\bm{\mathsfbr{P}}_{\mathrm{d}}$, respectively.

####  II-A2 Sensor Characteristics

We consider a high-resolution sensors that illuminates the environment and
uses reflected and scattered signal components to infer spatial information on
the observed scene. Typically, sets of scan points (so-called point clouds)
are the results of sensing. Here, we consider two distinct sets: source and
destination  point clouds. We define the destination point cloud as

$\bm{d}\triangleq[\bm{d}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{d}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{d}^{(N_{\mathrm{D}})\mathrm{T}}]^{\mathrm{T}}.$

where $\bm{d}^{(l)}\in\mathbb{R}^{N_{\text{dim}}}$ denotes the $l^{\text{th}}$
noisy scan point in Cartesian coordinates. Similarly, we introduce the source
point cloud $\bm{s}\triangleq[\bm{s}^{(1)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\bm{s}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{s}^{(N_{\mathrm{S}})\mathrm{T}}]^{\mathrm{T}}.$

####  II-A3 Surface Information

Surface normal vectors are a common method to preserve information on the
shapes of the underlying surfaces without the need of having explicit
representations of them [16, 17, 18]. We build upon this idea and augment
every destination scan point $\bm{d}^{(i)}$ by a normal vector
$\bm{n}^{(i)}\in\mathbb{R}^{N_{\mathrm{dim}}},i=1,2,...,N_{\mathrm{D}}$. The
normal vector $\bm{n}^{(i)}$ of destination scan point $\bm{d}^{(i)}$ is
determined based on the set of neighboring destination scan points
${\cal{N}}_{\mathrm{D}}^{(i)}=\\{\bm{d}^{(i^{\prime})}|\|\bm{d}^{(i^{\prime})}-\bm{d}^{(i)}\|\leqslant
d_{\mathrm{TH}}\\}$ where the threshold $d_{\mathrm{TH}}$ controls the size of
the neighborhood. The literature on the computation of surface normal vectors
is rich and the interested reader can find an overview in [18]. In our work,
we determine $i^{\mathrm{th}}$ surface normal vector, $\bm{n}^{(i)}$, as the
eigenvector which corresponds to the smallest eigenvalue of the sample
covariance matrix of ${\cal{N}}^{(i)}$. Hereafter, we will refer to
destination scan points and their associated normal vector as destination
surface points.

###  II-B Measurement Model

Before introducing our measurement model, it is worth noting that all scan
points are considered in homogeneous coordinates to cope with our pose
representation, i.e.,
$\bm{d}^{(i)}_{\mathrm{H}}\triangleq[\bm{d}^{(i)\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip 0.85358pt1]^{\mathrm{T}}$,
$\bm{n}^{(i)}_{\mathrm{H}}\triangleq[\bm{n}^{(i)\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt0]^{\mathrm{T}},i=1,2,\ldots,N_{\mathrm{D}}$, and
$\bm{s}^{(j)}_{\mathrm{H}}\triangleq[\bm{s}^{(j)\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt1]^{\mathrm{T}},j=1,2,\ldots,N_{\mathrm{S}}$. For mathematical
convenience, we introduce stacked vectors of source points, destination points
and their normals as follows:
$\bm{s}_{\mathrm{H}}=[\bm{s}_{\mathrm{H}}^{(1)\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{s}_{\mathrm{H}}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{s}_{\mathrm{H}}^{(N_{\mathrm{S}})\mathrm{T}}]^{\mathrm{T}}$,
$\bm{d}_{\mathrm{H}}=[\bm{d}_{\mathrm{H}}^{(1)\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{d}_{\mathrm{H}}^{(2)\mathrm{T}}\ldots\hskip 0.85358pt\hskip
0.85358pt\hskip
0.85358pt\bm{d}_{\mathrm{H}}^{(N_{\mathrm{D}})\mathrm{T}}]^{\mathrm{T}}$, and
$\bm{n}_{\mathrm{H}}=[\bm{n}_{\mathrm{H}}^{(1)\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{n}_{\mathrm{H}}^{(2)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{n}_{\mathrm{H}}^{(N_{\mathrm{D}})\mathrm{T}}]^{\mathrm{T}}$.

####  II-B1 Point-to-plane Error Metric

We can link the relative pose $\Delta\bm{\mathsfbr{P}}$ to source scan points,
$\bm{s}^{(j)},\forall j$, and destination surface points,
$\\{\bm{d}^{(i)},\bm{n}^{(i)}\\},\forall i$, via a point-to-plane metric [19].
In particular, our measurement model is given by

$\bm{n}^{(i)}_{\mathrm{H}}\left(\bm{d}^{(i)}_{\mathrm{H}}-\Delta\bm{\mathsfbr{P}}\bm{s}^{(j)}_{\mathrm{H}}\right)=e^{(j)}\quad
j=1,2,\ldots,N_{\mathrm{S}}$ (1)

in which $e^{(j)}$ is an error term with known111In partice, full knowledge of
$f\big{(}e^{(j)}\big{)}$ is typically unavailable and the PDF needs to be
approximated by its most significant moments. PDF $f\big{(}e^{(j)}\big{)}$.
The left-hand side of (1) is typically referred to as pseudo measurements
[20]. For future reference, we also introduce the joint measurement vectors
$\bm{z}^{(i,j)}\triangleq[\bm{d}_{\mathrm{H}}^{(i)\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{n}_{\mathrm{H}}^{(i)\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\bm{s}_{\mathrm{H}}^{(j)\mathrm{T}}]^{\mathrm{T}},$
$\bm{z}^{(i)}=[\bm{d}_{\mathrm{H}}^{(i)\mathrm{T}}$
$\bm{n}_{\mathrm{H}}^{(i)\mathrm{T}}\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{s}_{\mathrm{H}}^{\mathrm{T}}]^{\mathrm{T}}$, and
$\bm{z}=[\bm{d}_{\mathrm{H}}^{\mathrm{T}}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\bm{n}_{\mathrm{H}}^{\mathrm{T}}\hskip
0.85358pt\hskip 0.85358pt\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\bm{s}_{\mathrm{H}}^{\mathrm{T}}]^{\mathrm{T}}$. Note that from (1)
and the error statistics $f(e^{(j)})$, we can directly obtain conditional PDFs
$f(\bm{z}^{(i,j)}|\Delta\bm{P})$ for each pair $(i,j).$

The measurement model (1) is briefly discussed in what follows. Suppose we
knew the relative pose $\Delta\bm{\mathsfbr{P}}$ and the association $(i,j)$
of source point $\bm{s}^{(j)}_{\mathrm{H}}$ and destination surface point
$\\{\bm{d}^{(i)}_{\mathrm{H}},\bm{n}^{(i)}_{\mathrm{H}}\\}$. Then, the
expression within the parenthesis on the left-hand side of (1) implies that we
can transform the source scan point $\bm{s}^{(j)}_{\mathrm{H}}$ via relative
pose $\Delta\bm{\mathsfbr{P}}$ and we will retrieve the destination scan point
$\bm{d}^{(i)}_{\mathrm{H}}$. Note that the sensor does not measure the exact
same point on a surface from different poses due to its finite resolution (see
Fig. 1). We account for this by embedding the local information of the
surface. This is achieved by multiplying with the surface normal
$\bm{n}^{(i)}_{\mathrm{H}}$. Thus, the deviation of the transformed source
point from destination point is measured only in the direction of the surface
normal. The remaining error is given by $e^{(j)}$.

####  II-B2 Associability of Scan Points

High-resolution sensing, with effects such as occlusion and finite sensing
range, make scenes appear differently even if they are observed from similar
poses. As a result, the number of source points $N_{\mathrm{S}}$ and
destination points $N_{\mathrm{D}}$ are generally different and not all scan
points are associable. For robust scan matching, we need to account for scan
points which cannot be associated. In particular, we introduce the
destination-oriented data association vector
$\bm{\mathsfbr{a}}=[\mathsfbr{a}^{(1)}\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\mathsfbr{a}^{(2)}\hskip 0.85358pt\ldots\hskip
0.85358pt\mathsfbr{a}^{(N_{\mathrm{D}})}]^{\mathrm{T}}$ in which
$\mathsfbr{a}^{(i)},i\in\\{1,2,\ldots,N_{\mathrm{D}}\\}$ refers to the
association variable of the $i^{\mathrm{th}}$ destination surface point.
$\mathsfbr{a}^{(i)}$ can take any value from the set
$\\{0,1,\ldots,N_{\mathrm{S}}\\}$, where $\mathsfbr{a}^{(i)}=j$ means that the
$i^{\mathrm{th}}$ destination surface point relates to the $j^{\mathrm{th}}$
source point. Moreover, $\mathsfbr{a}^{(i)}=0$ means that the destination
surface point $\\{\bm{d}_{\mathrm{H}}^{(i)},\bm{n}_{\mathrm{H}}^{(i)}\\}$ is
not associable to any source point $\bm{s}_{\mathrm{H}}^{(j)},\forall j$.

###  II-C Problem Formulation

Fig. 1 graphically depicts the problem we attempt to solve. Our goal is to
infer the relative pose $\Delta\bm{\mathsfbr{P}}$ of the sensor based on the
source and destination point clouds by transforming the source point cloud
onto the destination point cloud. Note that the associations between source
scan points and destination surface points $\bm{\mathsfbr{a}}$ are unknown and
need to be inferred jointly with the relative pose $\Delta\bm{\mathsfbr{P}}$
which is a random matrix on the manifold $\textit{SE}(N_{\text{dim}})$.

In the pursuit of solving this problem robustly, we resort to the
probabilistic domain. Our goal is to obtain a Bayes-optimum estimate of the
relative pose $\Delta\bm{\mathsfbr{P}}$ considering all valid data association
hypotheses $\bm{\mathsfbr{a}}$. First, we marginalize the joint posterior
distribution $f(\Delta\bm{P},\bm{a}|\bm{z})$ to obtain
$f(\Delta\bm{P}|\bm{z})$ which factors in all valid data association
hypotheses. Then, we derive an maximum a posteriori (MAP) estimate by
maximizing the marginal posterior distribution, i.e.,

$\Delta\hat{\bm{P}}_{\mathrm{MAP}}=\operatorname*{arg\,max}_{\Delta\bm{P}}f(\Delta\bm{P}|\bm{z}).\vspace{-1.2mm}$
(2)

Note that determining $f(\Delta\bm{P}|\bm{z})$ by direct marginalization
$\big{(}\text{i.e.,
}f(\Delta\bm{P}|\bm{z})=\sum_{\bm{a}}f(\Delta\bm{P},\bm{a}|\bm{z})\big{)}$ is
infeasible due to the high-dimensionality of the summation space as discussed
in Section III. We consider feasible approximate calculation based on the
notion of message passing [21, 22, 23].

##  III Stochastic Modeling

Here, we present our novel stochastic model which couples data association,
sensor pose, and point cloud measurements.

###  III-A Modeling of the Point-to-Surface-Point Associations

We attempt to associate $N_{\mathrm{S}}$ source points to $N_{\mathrm{D}}$
destination surface points. Recall, though, that not all points in the source
and destination scan are associable. We assume the number of non-associable
source points, $\mathsfbr{N}_{\mathrm{NA}}$, to be random and Poisson
distributed with mean $\lambda_{\mathrm{NA}}$. Hence the total number of
source points, $\mathsfbr{N}_{\mathrm{S}}$, is also random and given by

$\mathsfbr{N}_{\mathrm{S}}=\mathsfbr{N}_{\mathrm{NA}}+|{\cal{D}}_{\bm{\mathsfbr{a}}}|,$
(3)

where
${\cal{D}}_{\bm{\mathsfbr{a}}}\triangleq\\{i\in\\{1,2,\ldots,N_{\mathrm{D}}\\}:\mathsfbr{a}^{(i)}\neq
0\\}$.

Valid associations are those in which each source point
$\bm{s}_{\mathrm{H}}^{(j)}$ is associated to at most one destination surface
point $\\{\bm{d}_{\mathrm{H}}^{(i)},\bm{n}_{\mathrm{H}}^{(i)}\\}$, and each
destination surface point
$\\{\bm{d}_{\mathrm{H}}^{(i)},\bm{n}_{\mathrm{H}}^{(i)}\\}$ is associated to
at most one source point $\bm{s}_{\mathrm{H}}^{(j)}$. This assumptions imposes
constraints on $\bm{\mathsfbr{a}}$. In particular, $\bm{\mathsfbr{a}}$ is
valid if and only if $\mathsfbr{a}^{(i)}=j\in\\{1,\ldots,N_{\mathrm{S}}\\}$,
$\mathsfbr{a}^{(i^{\prime})}\neq j,\forall i\neq i^{\prime}$. These
constraints can be conveniently expressed by

$\Psi(\bm{a})=\begin{cases}0,\quad&a^{(i)}=a^{(i^{\prime})}=j,i\neq
i^{\prime},j\neq 0\\\ 1,\quad&\text{otherwise}.\end{cases}$ (4)

Note that the cardinality of valid hypothesis is huge, rendering enumeration
of all hypotheses intractable [12, 24]. To address this issue, we introduce a
redundant formulation of $\bm{\mathsfbr{a}}$: the source-oriented data
association vector $\bm{\mathsfbr{b}}=[\mathsfbr{b}^{(1)}\hskip
0.85358pt\hskip 0.85358pt\hskip 0.85358pt\mathsfbr{b}^{(2)}\hskip
0.85358pt\hskip 0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\mathsfbr{b}^{(N_{\mathrm{S}})}]^{\mathrm{T}}$, in
which $\mathsfbr{b}^{(j)}\in\\{1,\ldots,N_{\mathrm{D}}\\}$ and
$\mathsfbr{b}^{(j)}=i$ means that the $j^{\mathrm{th}}$ source point is
associated to the $i^{\mathrm{th}}$ destination surface point. If
$\mathsfbr{b}^{(j)}=0$, the $j^{\mathrm{th}}$ source point is non-associable.
For source-oriented data association vectors $\bm{\mathsfbr{b}}$ we have
$\mathsfbr{b}^{(j)}=i\in\\{1,\ldots,N_{\mathrm{D}}\\}$,
$\mathsfbr{b}^{(j^{\prime})}\neq i,\forall j^{\prime}\neq j$. Note that the
set of all valid vectors $\bm{\mathsfbr{b}}$ contains exactly the same
information as that of all valid vectors $\bm{\mathsfbr{a}}$.

The reason for introducing this redundant data association representation is
that validity of association hypotheses can now be described via pairs of
$\mathsfbr{a}^{(i)}$ and $\mathsfbr{b}^{(j)}$ instead of the entire vector
$\bm{\mathsfbr{a}}$, making evaluation tractable. This is described in more
detail in [12, 24]. It can be easily verified that valid data associations can
be described by

$\Psi(\bm{a},\bm{b})=\prod_{i=1}^{N_{\mathrm{D}}}\prod_{j=1}^{N_{\mathrm{S}}}\psi_{i,j}(a^{(i)},b^{(j)}),\vspace{-2mm}$
(5)

where

$\vspace{-1mm}\psi_{i,j}(a^{(i)},b^{(j)})\triangleq\begin{cases}0,\quad&a^{(i)}=j,b^{(j)}\neq
i\\\ &b^{(j)}=i,a^{(i)}\neq j\\\ 1,\quad&\text{otherwise}.\end{cases}$ (6)

Note that we treat both $\bm{\mathsfbr{a}}$ and $\bm{\mathsfbr{b}}$ as random
vectors that are inferred jointly with the relative pose
$\Delta\bm{\mathsfbr{P}}$.

$f(\Delta\bm{P},\bm{a},\bm{b}|\bm{z})\propto
f(\Delta\bm{P})v(\Delta\bm{P};\bm{s}_{\mathrm{H}})\prod_{i=1}^{N_{\mathrm{D}}}q_{i}(\Delta\bm{P},a^{(i)};\bm{z}^{(i)})\prod_{j=1}^{N_{\mathrm{S}}}\psi_{i,j}(a^{(i)},b^{(j)})$
(9)

![width=1keepaspectratio]Figures/factorGraph/factorGraph

Figure 2: Factor graph of the joint posterior distribution in (9). The graph
can be sub-divided into a data association layer and a pose inference layer.
In the latter, our goal is to infer the marginal posterior distribution
$f(\Delta\bm{P}|z)$ via message passing (messages are indicated with grey
arrows). This PDF will maximized in a subsequent step to obtain an MAP
estimate of the relative pose $\Delta\bm{\mathsfbr{P}}$. We use the following
shorthand notations within the graph: $f_{\Delta\bm{P}}\triangleq
f(\Delta\bm{P})$, $q_{i}\triangleq q_{i}(\Delta\bm{P},a^{(i)};\bm{z}^{(i)})$,
$\psi_{i,j}\triangleq\psi_{i,j}(a^{(i)},b^{(j)})$, $v_{\mathrm{NA}}\triangleq
v(\Delta\bm{P};\bm{s}_{\mathrm{H}})$.

###  III-B Joint Posterior Distribution

In what follows, we derive the joint posterior distribution
$f(\Delta\bm{P},\bm{a},\bm{b}|\bm{z})$ and the corresponding factor graph. For
$\bm{z}$ observed and thus fixed, we obtain

$f(\Delta\bm{P},\bm{a},\bm{b}|\bm{z})\propto
f(\bm{z},\bm{a},\bm{b},N_{\mathrm{S}}|\Delta\bm{P})f(\Delta\bm{P}).\vspace{0mm}$
(7)

$f(\Delta\bm{P})$ is a random matrix pdf with support
$\textit{SE}(N_{\text{dim}})$ that incorporates prior knowledge on the
relative pose $\Delta\bm{\mathsfbr{P}}$. The conditional PDF
$f(\bm{z},\bm{a},\bm{b},N_{\mathrm{S}}|\Delta\bm{P})$ is given by

$\displaystyle f(\bm{z},\bm{a},\bm{b},N_{\mathrm{S}}|\Delta\bm{P})$ (8)
$\displaystyle=\\!\frac{e^{-\lambda_{\mathrm{NA}}}}{N_{\mathrm{NA}}!}\Psi(\bm{a},\bm{b})v(\Delta\bm{P};\bm{s}_{\mathrm{H}})\prod_{i=1}^{N_{\mathrm{D}}}q_{i}(\Delta\bm{P},a^{(i)};\bm{z}^{(i)})$

where we introduced
$v(\Delta\bm{P};\bm{s}_{\mathrm{H}})\triangleq\prod_{j=1}^{N_{\mathrm{S}}}f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P})$
and

$\displaystyle q_{i}(\Delta\bm{P},a^{(i)};\bm{z}^{(i)})$
$\displaystyle=\begin{cases}f_{\mathrm{A}}(\Delta\bm{P})\frac{f(\bm{z}^{(i,a^{(i)})}|\Delta\bm{P})}{\lambda_{\mathrm{NA}}f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(a^{(i)})}|\Delta\bm{P})}\quad&a^{(i)}\neq
0\\\ 1-f_{\mathrm{A}}(\Delta\bm{P})\quad&a^{(i)}=0\end{cases}$ (10)

and used $\Psi(\bm{a},\bm{b})$ from (5). A detailed derivation of (8) together
with the underlying assumptions is provided in Appendix A. By using (8) in
(7), we obtain the joint posterior distribution
$f(\Delta\bm{P},\bm{a},\bm{b}|\bm{z})$ shown in (9).

Let us discuss the expression in (9) to gain further insights into our model.
Suppose that we have a destination surface point $i$,
$\\{\bm{\mathsfbr{d}}_{\mathrm{H}}^{(i)},\bm{\mathsfbr{n}}_{\mathrm{H}}^{(i)}\\}$,
that is associated with source point $\bm{\mathsfbr{s}}_{\mathrm{H}}^{(j)}$,
i.e., $a^{(i)}=j$. Then, we have
$q_{i}(\Delta\bm{P},j,\bm{z}^{(i)})=f_{\mathrm{A}}(\Delta\bm{P})f(\bm{z}^{(i,j)}|\Delta\bm{P})/(\lambda_{\mathrm{NA}}f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P}))$,
where $f_{\mathrm{A}}(\cdot)$ accounts for the fact that destination surface
point $i$ is associable and contributes pseudo measurement $\bm{z}^{(i,j)}$
with likelihood $f(\bm{z}^{(i,j)}|\Delta\bm{P})$. Lastly, the term in the
denominator, $f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P})$,
reflects the fact that source point $j$, $\bm{s}_{\mathrm{H}}^{(j)}$, is
associable as well since it cancels the $j^{\text{th}}$ term in
$v(\Delta\bm{P};\bm{s}_{\mathrm{H}})$. On the contrary, if the
$i^{\mathrm{th}}$ destination surface point is not associated with any source
point, we have
$q_{i}(\Delta\bm{P},0,\bm{z}^{(i)})=1-f_{\mathrm{A}}(\Delta\bm{P})$. This term
takes the likelihood of non-associability, $1-f_{\mathrm{A}}(\Delta\bm{P})$,
of destination surface point $i$ into account. Finally, $\Psi(\bm{a},\bm{b})$
enforces validity of associations.

Algorithm 1 Obtaining the Marginal Posterior $f(\Delta\bm{P}|\bm{z})$

1:Initialize prior information

2: $\displaystyle\mu_{f_{\Delta\bm{P}}\rightarrow\Delta\bm{P}}(\Delta\bm{P})$
$\displaystyle=f(\Delta\bm{P})$
$\displaystyle\mu_{v_{\mathrm{NA}}\rightarrow\Delta\bm{P}}(\Delta\bm{P})$
$\displaystyle=v(\Delta\bm{P};\bm{s}_{\mathrm{H}})$

3:Forward prior information

4:For $i=1,2,\ldots,N_{\mathrm{D}}$

5: $\displaystyle\mu_{\Delta\bm{P}\rightarrow
q_{i}}(\Delta\bm{P})=\mu_{f_{\Delta\bm{P}}\rightarrow\Delta\bm{P}}(\Delta\bm{P})\mu_{v_{\mathrm{NA}}\rightarrow\Delta\bm{P}}(\Delta\bm{P})$

6:End For

7:Evaluate associations

8:For $i=1,2,\ldots,N_{\mathrm{D}}$

9: $\displaystyle\mu_{q_{i}\rightarrow a^{(i)}}(a^{(i)})$
$\displaystyle\\!=\\!\int
q_{i}(\Delta\bm{P},a^{(i)};\bm{z}^{(i)})\mu_{\Delta\bm{P}\rightarrow
q_{i}}(\Delta\bm{P})\mathrm{d}\Delta\bm{P}$
$\displaystyle\approx\sum^{N_{\mathrm{P}}}_{p=1}q_{i}(\Delta\bm{P}^{(p)},a^{(i)};\bm{z}^{(i)})v(\Delta\bm{P};\bm{s}_{\mathrm{H}})$

10:where $\Delta\bm{P}^{(p)}$, $p=1,2,\dots,N_{\mathrm{P}}$ are random samples
drawn from
$\mu_{f_{\Delta\bm{P}}\rightarrow\Delta\bm{P}}(\Delta\bm{P})=f(\Delta\bm{P})$.

11:End For

12:Perform $N_{\mathrm{DA}}$ data association iterations

13:For $l=1,2,\ldots,N_{\mathrm{DA}}$

14: $\displaystyle\text{ Message exchange between }a^{(i)}\text{ and
}b^{(j)}\text{ see
\cite[cite]{[\@@bibref{}{WilliamsLau:J14,MeyerKropfreiterWilliamsLauHlawatschBracaWin:J18}{}{}]}}$
$\displaystyle(\mu^{(l)}_{a_{i}\rightarrow\psi_{i,j}},\mu^{(l)}_{\psi_{i,j}\rightarrow
b^{(j)}},\mu^{(l)}_{b^{(j)}\rightarrow\psi_{i,j}},\mu^{(l)}_{\psi_{i,j}\rightarrow
a^{(i)}})$

15:End For

16:Forward data association result

17:For $i=1,2,\ldots,N_{\mathrm{D}}$

18: $\displaystyle\mu_{a^{(i)}\rightarrow
q_{i}}(a^{(i)})=\prod_{j=1}^{N_{\mathrm{S}}}\mu^{(N_{\mathrm{DA}})}_{\psi_{i,j}\rightarrow
a_{i}}(a^{(i)})$

19:End For

20:Marginalize associations

21:For $i=1,2,\ldots,N_{\mathrm{D}}$

22:
$\displaystyle\mu_{q_{i}\rightarrow\Delta\bm{P}}(\Delta\bm{P})=\sum_{a^{(i)}=0}^{N_{\mathrm{S}}}q_{i}(\Delta\bm{P},a^{(i)};\bm{z}^{(i)})\mu_{a^{(i)}\rightarrow
q_{i}}(a^{(i)})$

23:End For

24:Determine marginalized posterior $f(\Delta\bm{P}|\bm{z})$

25: $\displaystyle f(\Delta\bm{P}|\bm{z})\approx
f(\Delta\bm{P})v(\Delta\bm{P};\bm{s}_{\mathrm{H}})\prod_{i=1}^{N_{\mathrm{D}}}\mu_{q_{i}\rightarrow\Delta\bm{P}}(\Delta\bm{P})$
(11)

##  IV Graph-based Inference

The factor graph of the joint posterior distribution in (9) is depicted in
Fig. 2. Our goal is to infer the marginal posterior distribution
$f(\Delta\bm{P}|\bm{z})=\sum_{\bm{a}}\sum_{\bm{b}}f(\Delta\bm{P},\bm{a},\bm{b}|\bm{z})$
needed for the calculation of an MAP estimate
$\Delta\hat{\bm{P}}_{\mathrm{MAP}}$. We employ the notation of message passing
according to the sum-product rules [21, 22, 23] to calculate an approximation
of the marginal posterior distribution $f(\Delta\bm{P}|\bm{z})$. The messages
passed along the edges of the factor graph are indicated via grey arrows in
the factor graph in Fig. 2. Messages that involve integration
$\int\mathrm{d}\Delta\bm{P}$ over the sensor pose $\Delta\bm{P}$ are
approximated by sampling-based integration [25]. We present the steps of our
inference method in Algorithm 1.

First, we perform Algorithm 1 to compute an accurate approximation of the
marginal posterior $f(\Delta\bm{P}|\bm{z})$. Then, our goal is to derive the
MAP estimate,
$\Delta\hat{\bm{P}}_{\mathrm{MAP}}=\operatorname*{arg\,max}_{\Delta\bm{P}}f(\Delta\bm{P}|\bm{z})$,
by maximizing (11) with $N_{\mathrm{IT}}$. To that end, we employ iterative
numerical solvers that exploit the structure of
$\textit{SE}(N_{\text{dim}})$[26]. As initial guess, we use the pose sample
that maximizes the marginal posterior, i.e.,

$\Delta\bm{P}_{\mathrm{init}}=\operatorname*{arg\,max}_{\Delta\bm{P}^{(p)}}f(\Delta\bm{P}^{(p)}|\bm{z}),\quad
p\in\\{1,2,\ldots N_{\mathrm{P}}\\}$ (12)

where $\Delta\bm{P}^{(p)}$, $p=1,2,\dots,N_{\mathrm{P}}$ are random samples
drawn from $f(\Delta\bm{P})$.

We emphasize that our approach determines the full marginal posterior
distribution $f(\Delta\bm{P}|\bm{z})$. Obtaining an estimate by finding its
mode is by no means the only information that we can extract from it. We could
derive, e.g., uncertainty information on our estimate that we can use to
reason about the quality of matching. Such knowledge is typically required for
closing loops in simultaneous localization and mapping (SLAM) applications.
Beyond scan-to-scan matching, our proposed stochastic model can be embedded
into online or offline factor graph-based trajectory estimation.

The computational complexities in the pose inference and data association
layers (see Fig. 2) as well as for optimization are given by
$\mathcal{O}(N_{\mathrm{D}}N_{\mathrm{S}}N_{*})$,
$*\in\\{\mathrm{P,DA,IT}\\}$. To avoid high complexity, segmentation of point
clouds [27] can be used to generate semantic clusters (e.g., road surface,
guardrails, poles, etc.) where each cluster contains significantly fewer
points than the original point cloud. Running multiple instances (one for each
cluster) of our proposed algorithm in parallel reduces the complexity as
$N_{\mathrm{D},i}N_{\mathrm{S},i}\ll N_{\mathrm{D}}N_{\mathrm{S}}$, where
$N_{\mathrm{D},i}$ and $N_{\mathrm{S},i}$ denote the number of destination and
source points in cluster $i$, respectively.

Relative Trans. Error |  Proposed |  NDT [3] |  IMLS [6]
---|---|---|---
$e_{\mathrm{trans},50}$ |  1.87$\%$ |  1.800$\%$ |  6.60$\%$
$e_{\mathrm{trans},95}$ |  7.723$\%$ |  94.65$\%$ |  28.06$\%$
Relative Rotation Error |  Proposed |  NDT [3] |  IMLS [6]
$e_{\mathrm{rot},50}$ |  0.0349∘/m |  0.0203∘/m |  0.2487∘/m
$e_{\mathrm{rot},95}$ |  0.1123∘/m |  0.0759 ∘/m |  2.2016∘/m

TABLE I: Error quantiles of simulated pose estimation methods.

##  V Numerical Evaluation

In this section, we evaluate the our scan matching approach based on simulated
data and present the results.

###  V-A Simulation Scenario

We consider a vehicle that drives in the urban environment depicted in Fig. 3.
The vehicle moves with a constant velocity of $10$ m/s along the indicated
trajectory (red line). It is equipped with a $360^{\circ}$-LiDAR sensor that
generates scans every $\Delta T=80$ ms. We treat the current scan as the
source scan and the previous scan as the destination scan to obtain pose
estimates that are inline with the motion of the vehicle.

The sensor has an angular resolution of $1$ deg and a maximum sensing range of
$100$ m. The scanner obtains range and bearing estimates that are corrupted by
zero-mean Gaussian noise with standard deviation
$\sigma_{\mathrm{range}}=0.05$ m and $\sigma_{\mathrm{bearing}}=0.5$ deg,
respectively. Non-associable source points are assumed to be uniformly
distributed in range and bearing, being independent of
$\Delta\bm{\mathsfbr{P}}$. The number of non-associable source points is
assumed Poisson distributed with mean $\lambda_{\mathrm{NA}}=1$. For surface
normal computation, we use $d_{\mathrm{TH}}=2$ m. An exemplary scan is
depicted in Fig. 3. We use the following parameters for our proposed
algorithm. The probability of associability is constant
$f_{\mathrm{A}}(\Delta\bm{P})=0.8$. For each
$j\in\\{1,2,\ldots,N_{\mathrm{S}}\\}$, the noise statistics of $f(e^{(j)})$
are assumed zero-mean Gaussian distributed with standard deviation
$\sigma_{e^{(j)}}=0.03$ m. Translation and rotation prior are considered
uniform within the intervals $[-10,10]$ m and $[-90,90]$ ∘. We performed
$N_{\mathrm{MC}}=100$ Monte Carlo trials.

###  V-B Performance Comparison

We compare our probabilistic scan matching approach to normal distributions
transform (NDT) scan matching [3] and implicit moving least squares (IMLS)
scan matching [6]. In our implementation of IMLS, we set the neighborhood
parameter to $h=2$ and obtained estimates by minimizing [6, Eq. (1)]. We
employ the following error metrics to evaluate the translation and rotation
drift:
$e_{\mathrm{trans}}=\|\Delta\hat{\bm{t}}_{\mathrm{MAP}}-\Delta\bm{t}\|/\|\Delta\bm{t}\|$
and
$e_{\mathrm{rot}}=\arccos((\mathrm{Tr}(\Delta\hat{\bm{R}}_{\mathrm{MAP}}^{-1}\Delta\bm{R})-1)/2)/\|\Delta\bm{t}\|$,
where $\Delta\hat{\bm{t}}_{\mathrm{MAP}}$ and
$\Delta\hat{\bm{R}}_{\mathrm{MAP}}$ are the estimated translation and rotation
extracted from $\Delta\hat{\bm{P}}_{\mathrm{MAP}}$, $\Delta\bm{t}$ and
$\Delta\bm{R}$ are the ground truth translation and rotation, and
$\mathrm{Tr}(\cdot)$ is the trace operator.

![width=keepaspectratio]Figures/urbanEnvironment/urbanEnvironmentCar

Figure 3: Simluation scenario and results: The ground truth trajectory is
shown against the accumulated pose change estimates of the our approach and
its benchmarks. Scan points obtained at the first time instance are depicted
by yellow dots to visualize the characteristics of the LiDAR sensor.

Results are depicted in Tab. I. Here, $e_{\mathrm{trans},q}$ refers to an
error threshold such that $q\%$ of all relative observed translation errors
$e_{\mathrm{trans}}$ are below this threshold. The same logic applies to
$e_{\mathrm{rot},q}$. For example, $e_{\mathrm{trans},50}$ is the median
relative translation error. From Tab. I we observe that median relative
translation and rotation errors of NDT and our proposed approach both show low
drifts. However, considering $e_{\mathrm{trans},q}$, NTD shows a significantly
larger drift compared to our proposed probabilistic approach. IMLS scan
matching shows large drifts, presumably due to the absence of a good initial
guess provided to the optimizers. Exemplary estimated trajectories obtained
via a concatenation of relative pose estimates are also shown in Fig. 3 where
we can see how closely our approach attains ground truth.

##  VI Conclusions

We derived a probabilistic scan matching model which jointly describes the
pose change of a high-resolution sensor along with the unknown data
associations of observed scan points. Moreover, we presented an inference
algorithm for that model which determines the posterior distribution of pose
change given scan data. Compared to state-of-the-art scan matching algorithms,
our approach considers all data association hypotheses and allows for
uncertainty quantification, as the full posterior distribution is available.
Numerical studies show precise pose change estimates characterized by low
drift in terms of both translation and rotation.

##  Appendix A Derivation of
$f(\bm{z},\bm{a},\bm{b},N_{\mathrm{S}}|\Delta\bm{P})$

We make the following assumptions:

  1. A1)

Source points, $\bm{s}_{\mathrm{H}}^{(j)},\forall j$, are either associable or
non-associable. Every associable source point can be associated with at most
one destination surface point and every destination surface point can be
associated to at most one associable source point.

  2. A2)

Valid source-to-destination associations according to A1) are uniformly
distributed a priori.

  3. A3)

Associability of destination points, $\bm{\stheta}$, and the number of non-
associable source points, $\mathsfbr{N}_{\mathrm{NA}}$, are statistically
independent given the relative pose, $\Delta\bm{\mathsfbr{P}}$.

  4. A4)

The associability of destination surface points $i$ and $i^{\prime},\forall
i\neq i^{\prime}$, $\stheta^{(i)}$ and $\stheta^{(i^{\prime})}$, are
independent Bernoulli-distributed random variables given the relative pose
$\Delta\bm{\mathsfbr{P}}$. The probability of a destination surface point $i$
to be associable ($\stheta^{(i)}=1$) is $0\leq
f_{\mathrm{A}}(\Delta\bm{P})<1,\forall i$.

  5. A5)

The number of non-associable source points, $\mathsfbr{N}_{\mathrm{NA}}$, is
Poisson-distributed with intensity $\lambda_{\mathrm{NA}}$.

  6. A6)

A non-associable source point, $\bm{s}_{\mathrm{H}}^{(j)}$, is statistically
independent of an associable source points,
$\bm{s}_{\mathrm{H}}^{(j^{\prime})}$. This holds for all associable and non-
associable points.

  7. A7)

Given the relative pose $\Delta\bm{\mathsfbr{P}}$, pseudo measurements,
$\bm{z}^{(i,j^{\prime})},\forall i,j^{\prime}$, corresponding to associable
source points, $\bm{s}_{\mathrm{H}}^{(j^{\prime})},\forall j^{\prime}$, are
independent of each other and distributed according to
$f(\bm{z}^{(i,j^{\prime})}|\Delta\bm{P})$.

  8. A8)

Non-associable source points, $\bm{s}_{\mathrm{H}}^{(j)},\forall j$, are
independent and identically distributed (iid) with PDF
$f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P})\neq
0,\Delta\bm{P}\in\textit{SE}(N_{\text{dim}})$.

As a preliminary step, we derive the conditional PDF
$f(\bm{z},\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$. This is closely related to the
derivation of the joint probabilistic data association (JPDA) filter [11]. We
start by decomposing $f(\bm{z},\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$ as

$f(\bm{z},\bm{a},N_{\mathrm{S}}|\Delta\bm{P})=f(\bm{z}|\bm{a},N_{\mathrm{S}},\Delta\bm{P})f(\bm{a},N_{\mathrm{S}}|\Delta\bm{P}).$
(13)

Let us next define the vector $\bm{\stheta}\triangleq[\stheta^{(1)}\hskip
0.85358pt\hskip 0.85358pt\hskip 0.85358pt\stheta^{(2)}\hskip 0.85358pt\hskip
0.85358pt\hskip 0.85358pt\ldots\hskip 0.85358pt\hskip 0.85358pt\hskip
0.85358pt\stheta^{(N_{\mathrm{D}})}]^{\mathrm{T}}$ that indicates which
destination surface point $i\in\\{1,2,\ldots,N_{\mathrm{D}}\\}$ is associable
($\stheta^{(i)}=1$) or non-associable ($\stheta^{(i)}=0$). Note that
$\stheta^{(i)}=1$ implies $\mathsfbr{a}^{(i)}\neq 0$ and $\stheta^{(i)}=0$
implies $\mathsfbr{a}^{(i)}=0$. Due to (3), the mapping between
($\bm{\mathsfbr{a}},\bm{\stheta},\mathsfbr{N}_{\mathrm{NA}}$) and
($\bm{\mathsfbr{a}},\mathsfbr{N}_{\mathrm{S}}$) is thus deterministic as well
as bijective and the factor $f(\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$ in (13)
can also be written as
$f(\bm{a},\bm{\theta},N_{\mathrm{NA}}|\Delta\bm{P})=f(\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$.

As a result of the chain rule and by using A3), we obtain

$\displaystyle f(\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$ $\displaystyle\hskip
19.91692pt=f(\bm{a},\bm{\theta},N_{\mathrm{NA}}|\Delta\bm{P})$
$\displaystyle\hskip
19.91692pt=f(\bm{a}|\bm{\theta},N_{\mathrm{NA}},\Delta\bm{P})f(\bm{\theta}|\Delta\bm{P},N_{\mathrm{NA}})f(N_{\mathrm{NA}}|\Delta\bm{P})$
$\displaystyle\hskip
19.91692pt=f(\bm{a}|\bm{\theta},N_{\mathrm{NA}},\Delta\bm{P})f(\bm{\theta}|\Delta\bm{P})f(N_{\mathrm{NA}}|\Delta\bm{P}).\vspace{.7mm}$
(14)

Note that A4) implies

$f(\bm{\theta}|\Delta\bm{P})=\left(\prod_{i\in{\cal{D}}_{\bm{a}}}f_{\mathrm{A}}(\Delta\bm{P})\right)\prod_{i^{\prime}\notin{\cal{D}}_{\bm{a}}}(1-f_{\mathrm{A}}(\Delta\bm{P}))$
(15)

and from A5), we obtain

$f(N_{\mathrm{NA}}|\Delta\bm{P})=\frac{\lambda_{\mathrm{NA}}^{N_{\mathrm{NA}}}}{N_{\mathrm{NA}}!}e^{-\lambda_{\mathrm{NA}}}.\vspace{0mm}$
(16)

Let us denote by ${\cal{A}}$ the set of all vectors $\bm{a}$ that do not
violate assumption A1). From basic results of combinatorics and A2), it is
straightforward to show that
$f(\bm{a}|\bm{\theta},N_{\mathrm{NA}},\Delta\bm{P})=N_{\mathrm{NA}}!/N_{\mathrm{S}}!$
for $\bm{a}\in{\cal{A}}$ and thus
$f(\bm{a}|\bm{\theta},N_{\mathrm{NA}},\Delta\bm{P})=\Psi(\bm{a})$
$N_{\mathrm{NA}}!/N_{\mathrm{S}}!$ for arbitrary
$\bm{a}\in\\{0,1,\dots,N_{\mathrm{S}}\\}^{N_{\mathrm{D}}}$. Using this result
as well as (15) and (16) in (14), we obtain

$\displaystyle f(\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$
$\displaystyle=\Psi(\bm{a})\hskip
0.85358pt\frac{\lambda_{\mathrm{NA}}^{N_{\mathrm{S}}-|{\cal{D}}_{\bm{a}}|}}{N_{\mathrm{S}}!}e^{-\lambda_{\mathrm{NA}}}\left(\hskip
1.13809pt\prod_{i\in{\cal{D}}_{\bm{a}}}f_{\mathrm{A}}(\Delta\bm{P})\right)$
$\displaystyle\hskip
28.45274pt\times\prod_{j\notin{\cal{D}}_{\bm{a}}}\big{(}1-f_{\mathrm{A}}(\Delta\bm{P})\big{)}.\vspace{.7mm}$
(17)

Furthermore, using A6)-A8) $f(\bm{z}|\bm{a},N_{\mathrm{S}},\Delta\bm{P})$ in
(13), can be expressed as

$\displaystyle f(\bm{z}|\bm{a},N_{\mathrm{S}},\Delta\bm{P})$
$\displaystyle\hskip 11.38109pt=\left(\hskip
1.13809pt\prod_{j=1}^{N_{\mathrm{S}}}f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P})\right)\prod_{i\in\mathcal{D}_{\bm{a}}}\frac{f\big{(}\bm{z}^{(i,a^{(i)})}|\Delta\bm{P}\big{)}}{f_{\mathrm{NA}}\big{(}\bm{s}_{\mathrm{H}}^{(a^{(i)})}\big{|}\Delta\bm{P}\big{)}}.$
(18)

Now, we use the expression for $f(\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$ in (17)
and the expression for $f(\bm{z}|\bm{a},N_{\mathrm{S}},\Delta\bm{P})$ in (18)
into (14) and obtain

$\displaystyle f(\bm{z},\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$
$\displaystyle\hskip 2.84526pt=\Psi(\bm{a})\hskip
0.85358pt\frac{\lambda_{\mathrm{NA}}^{N_{\mathrm{S}}-|{\cal{D}}_{\bm{a}}|}}{N_{\mathrm{S}}!}\hskip
0.85358pte^{-\lambda_{\mathrm{NA}}}\left(\hskip
1.13809pt\prod_{j=1}^{N_{\mathrm{S}}}f_{\mathrm{NA}}(\bm{s}_{\mathrm{H}}^{(j)}|\Delta\bm{P})\right)$
$\displaystyle\hskip
2.84526pt\times\left(\prod_{i\in\mathcal{D}_{\bm{a}}}\frac{f_{\mathrm{A}}(\Delta\bm{P})\hskip
0.85358ptf\big{(}\bm{z}^{(i,a^{(i)})}\big{|}\Delta\bm{P}\big{)}}{f_{\mathrm{NA}}\big{(}\bm{s}_{\mathrm{H}}^{(a^{(i)})}\big{|}\Delta\bm{P}\big{)}}\right)\prod_{j\notin{\cal{D}}_{\bm{a}}}\big{(}1-f_{\mathrm{A}}(\Delta\bm{P})\big{)}.$

Finally, the final expression for
$f(\bm{z},\bm{a},\bm{b},N_{\mathrm{S}}|\Delta\bm{P})$ in (8) is obtained from
the expression for $f(\bm{z},\bm{a},N_{\mathrm{S}}|\Delta\bm{P})$ in
(LABEL:eq:finalAppendix) by “opening” the factor $\Psi(\bm{a})$. In
particular, we introduce $\bm{b}$ as described in [24, Section V.B] and
replace $\Psi(\bm{a})$ by $\Psi(\bm{a},\bm{b})$ in (5).

## References

  * [1] A. Ziebinski, R. Cupek, H. Erdogan, and S. Waechter, “A survey of ADAS technologies for the future perspective of sensor fusion,” in _Computational Collective Intelligence_. Springer International Publishing, 2016, pp. 135–146.
  * [2] A. Mallios, P. Ridao, D. Ribas, F. Maurelli, and Y. Petillot, “EKF-SLAM for AUV navigation under probabilistic sonar scan-matching,” in _Proc. IEEE IROS-10_ , Taipei, Taiwan, 2010, pp. 4404–4411.
  * [3] P. Biber and W. Strasser, “The normal distributions transform: a new approach to laser scan matching,” in _Proc. IEEE IROS-03_ , Las Vegas, NV, USA, Oct. 2003.
  * [4] J. Zhang and S. Singh, “Low-drift and real-time lidar odometry and mapping,” _Auton. Robots_ , vol. 41, no. 2, pp. 401–416, Feb. 2016.
  * [5] R. A. Srivatsan, M. Xu, N. Zevallos, and H. Choset, “Probabilistic pose estimation using a Bingham distribution-based linear filter,” _Int J. Robot Res._ , vol. 37, no. 13-14, pp. 1610–1631, Jun. 2018.
  * [6] J.-E. Deschaud, “IMLS-SLAM: Scan-to-model matching based on 3D data,” in _Proc. IEEE ICRA-18_ , Brisbane, Australia, May 2018.
  * [7] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in _Proc. IEEE IROS-18_ , Madrid, Spain, Oct. 2018.
  * [8] J. Tang, Y. Chen, X. Niu, L. Wang, L. Chen, J. Liu, C. Shi, and J. Hyyppä, “LiDAR scan matching aided inertial navigation system in GNSS-denied environments,” _Sensors_ , vol. 15, no. 7, pp. 16 710–16 728, Jul. 2015\.
  * [9] E. Olson, “Real-time correlative scan matching,” in _2009 IEEE International Conference on Robotics and Automation_. IEEE, may 2009.
  * [10] A. Mallios, P. Ridao, D. Ribas, and E. Hernández, “Scan matching SLAM in underwater environments,” _Autonomous Robots_ , vol. 36, no. 3, pp. 181–198, jun 2013.
  * [11] Y. Bar-Shalom, P. K. Willett, and X. Tian, _Tracking and Data Fusion: A Handbook of Algorithms_. Storrs, CT, USA: Yaakov Bar-Shalom, 2011.
  * [12] J. L. Williams and R. Lau, “Approximate evaluation of marginal association probabilities with belief propagation,” _IEEE Trans. Aerosp. Electron. Syst._ , vol. 50, no. 4, pp. 2942–2959, Oct. 2014.
  * [13] F. Meyer and M. Z. Win, “Scalable data association for extended object tracking,” _IEEE Trans. Signal Inf. Process. Netw._ , vol. 6, pp. 491–507, 2020.
  * [14] F. Meyer and J. L. Williams, “Scalable detection and tracking of extended objects,” in _Proc. IEEE ICASSP 2020_ , May 2020, pp. 8916–8920.
  * [15] S. Thrun, W. Burgard, and D. Fox, _Probabilistic Robotics_. MIT Press Ltd, 2005.
  * [16] J. C. Carr, R. K. Beatson, J. B. Cherrie, T. J. Mitchell, W. R. Fright, B. C. McCallum, and T. R. Evans, “Reconstruction and representation of 3D objects with radial basis functions,” in _Proc. ACM SIGGRAPH-01_ , Los Angeles, CA, USA, Aug. 2001.
  * [17] J. Demantké, C. Mallet, N. David, and B. Vallet, “Dimensionality based scale selection in 3D lidar point clouds,” _ISPRS Archives_ , vol. XXXVIII-5/W12, pp. 97–102, Sep. 2012.
  * [18] K. Klasing, D. Althoff, D. Wollherr, and M. Buss, “Comparison of surface normal estimation methods for range sensing applications,” in _Proc. IEEE ICRA-09_ , Kobe, Japan, May 2009.
  * [19] K.-L. Low, “Linear least-squares optimization for point-to-plane ICP surface registration,” Department of Computer Science, University of North Carolina at Chapel Hill, Tech. Rep., 2004.
  * [20] P. Richards, “Constrained Kalman filtering using pseudo-measurements,” in _IEE CATT-95_ , London, UK, May 1995.
  * [21] H.-A. Loeliger, J. Dauwels, J. Hu, S. Korl, L. Ping, and F. R. Kschischang, “The factor graph approach to model-based signal processing,” _Proc. IEEE_ , vol. 95, no. 6, pp. 1295–1322, Jun. 2007.
  * [22] H. Loeliger, “An introduction to factor graphs,” _IEEE Signal Process. Mag._ , vol. 21, no. 1, pp. 28–41, Jan. 2004.
  * [23] F. Kschischang, B. Frey, and H.-A. Loeliger, “Factor graphs and the sum-product algorithm,” _IEEE Trans. Inf. Theory_ , vol. 47, no. 2, pp. 498–519, 2001.
  * [24] F. Meyer, T. Kropfreiter, J. L. Williams, R. Lau, F. Hlawatsch, P. Braca, and M. Z. Win, “Message passing algorithms for scalable multitarget tracking,” _Proc. IEEE_ , vol. 106, no. 2, pp. 221–259, Feb. 2018.
  * [25] A. Doucet, N. de Freitas, and N. Gordon, _Sequential Monte Carlo Methods in Practice_. New York, NY, USA: Springer, 2001.
  * [26] N. Boumal, “An introduction to optimization on smooth manifolds,” Published online, Aug 2020. [Online]. Available: http://www.nicolasboumal.net/book
  * [27] L. Tchapmi, C. Choy, I. Armeni, J. Gwak, and S. Savarese, “SEGCloud: Semantic segmentation of 3d point clouds,” in _2017 International Conference on 3D Vision (3DV)_. IEEE, oct 2017.

  *[LiDAR]: Light Detection and Ranging
  *[ADAS]: advanced driver assistance systems
  *[AUV]: autonomous underwater vehicles
  *[PDF]: probability density function
  *[PMF]: probability mass function
  *[PDFs]: probability density function
  *[MAP]: maximum a posteriori
  *[SLAM]: simultaneous localization and mapping
  *[iid]: independent and identically distributed
  *[JPDA]: joint probabilistic data association


# Sectional category à la Quillen

Urtzi Buijs  Departamento de Álgebra, Geometría y Topología, Universidad de
Málaga, Ap. 59, 29080 Málaga, Spain<EMAIL_ADDRESS>and  José Carrasquel
<EMAIL_ADDRESS>
###### Abstract.

In this note we give a characterization of the sectional category of a map
between rational spaces in terms of its Koszul-Quillen model.

## Introduction

The _sectional category_ of a continuous map $f\colon X\to Y$ is the least
number of open sets of $Y$, minus one, needed to cover $Y$ on each of which
$f$ has a local homotopy section [12]. This invariant of the weak homotopy
type of maps has several particular cases that have been studied for a long
time. For instance, the Lusternik-Schnirelmann (LS) category of a path-
connected space is the sectional category of its base point inclusion [10].
Other particular cases include Farber’s topological complexity [4] and the LS
category of a map [1, 7].

If $f$ is a map between rational spaces, then its sectional category depends
only on the rational homotopy type of $f$. This simple remark invites the
study of sectional category through models. The first step on this direction
was the celebrated work of Félix and Halperin [5] where they characterise the
LS category of a simply connected rational space of finite $\mathbb{Q}$-type
in terms of its minimal Sullivan model [6, 13]. This result was generalised in
[2] for sectional category of maps between such spaces admitting a homotopy
retraction.

In this paper, we will give characterisation for the sectional category of a
map in terms of its Koszul-Quillen model which we now state.

Let $(\mathbb{L}(Z),\partial)$ be the minimal Quillen model of a space $X$.
Then a minimal Quillen model for the cartesian product $X^{n}$ has the form
$(\mathbb{L}(Z^{n}),D)$ where $Z^{n}=\oplus_{j=1}^{n}Z^{n}_{j}$,

(0.1) $Z^{n}_{j}=\bigoplus_{1\leq i_{1}<\cdots<i_{j}\leq
n}s^{j-1}(Z_{i_{1}}\otimes\cdots\otimes Z_{i_{j}})$

and $Z=Z_{i}$ [14, VII.1.(2)]. Observe that in the case that
$(\mathbb{L}(Z),\partial)$ is a cellular Quillen model then so is
$(\mathbb{L}(Z^{n}),D)$ and the generators $Z^{n}$ correspond to the product
$CW$-structure of $X^{n}$. Observe also that the generators of $Z_{n}^{n}$
correspond to products of $n$ positive dimension cells of $X$.

######  Theorem.

Let $\varphi\colon(\mathbb{L}(V),\partial)\rightarrowtail(\mathbb{L}(V\oplus
W),\partial)$ be the Koszul-Quillen model of certain continuous map $f\colon
X\to Y$ between simply connected rational spaces. Then the sectional category
of $f$ is the smallest $n$ for which there exists a dgl map

$\alpha\colon(\mathbb{L}(V\oplus W),\partial)\to(\mathbb{L}((V\oplus
W)^{n+1}_{1}\oplus U^{n+1}),D)$

such that, for all $a\in V\oplus W$, $\alpha(a)=a_{1}+\cdots+a_{n+1}+\xi$ with

$\xi\in\mathbb{L}((V\oplus W)^{n+1}_{1})*\mathbb{L}^{+}(U^{n+1}).$

Here $U^{n+1}$ denotes the quotient vector space $(V\oplus W)^{n+1}_{\geq 2}$
by $s^{n}(W_{1}\otimes\cdots\otimes W_{n+1})$ and $a_{i}$ means the inclusion
of $a$ into $V_{i}\oplus W_{i}$.

Observe that $\alpha$ is in fact a model for the diagonal inclusion which does
not _touch_ the top cells.

The previous Theorem and the technical Lemmas included in Section 1 of this
work constitute the theoretical background necessary to develop the project
consisting of studying results in Rational Homotopy Theory related to
Cartesian products of spaces. For this purpose, in future works it will be
necessary to describe explicitly the Quillen models of Cartesian products used
in this article as well as a model for the diagonal map.

In view of the characterization of the sectional category used in this
article, these explicit models seem essential to understand invariants such as
the LS category or topological complexity from the point of view of rational
homotopy using Quillen models.

##  1\. Preliminaries

The sectional category of an inclusion of a sub CW-complex $f\colon
X\hookrightarrow Y$ can be characterize using the _fat wedge_

$T^{n}(i)=\left\\{(y_{1},\ldots,y_{n})\in Y^{n}\colon\ y_{l}\in f(X)\mbox{ for
at least one }l\in\\{1,\cdots n\\}\right\\}.$

######  Proposition 1.1.

[12] The sectional category of $f$ is the smallest $n$ for which the diagonal
$Y\to Y^{n+1}$ factors up to homotopy through the fat wedge $T^{n+1}(f)$.

Observe that the fat wedge can be written as

$T^{n+1}(f)=\bigcup_{i=0}^{n}Y^{i}\times X\times Y^{n-i},$

therefore, considering the product CW-structure, it can be seen as a sub CW-
complex of $Y^{n+1}$ where the cells of $Y^{n+1}/T^{n+1}(i)$ are the products
of $n+1$ cells of $Y/X$.

Using the notation of (0.1), the description of the model of the cartesian
product of [14, VII.1.(2)] can be modified to get

######  Proposition 1.2.

Let $X$ be a topological space and $\mathbb{L}(Z,\partial)$ be a minimal
Quillen model for $X$. Then there is a model for the diagonal inlusion
$X\hookrightarrow X^{n}$,

${\mathbb{L}(Z^{n}_{1}\oplus\cdots\oplus
Z^{n}_{n},D)}$${\mathbb{L}(Z,\partial)}$${\mathbb{L}(Z_{1},\partial)\times\cdots\times\mathbb{L}(Z_{n},\partial)}$$\scriptstyle{\varphi}$$\scriptstyle{\simeq}$$\scriptstyle{\Delta}$$\scriptstyle{\delta}$

  * (i)

$D(z_{i})=\partial z_{i}$ for $z_{i}\in Z_{i}$,

  * (ii)

$\Delta$ is the diagonal $n$ inclusion of dgls,

  * (iii)

$\varphi(Z^{n}_{2}\oplus\cdots Z^{n}_{n})=0$ and
$\varphi(z_{i})=(0,\ldots,0,z,0,\ldots,0)$ in $i$th position,

  * (iv)

for all $s(z_{i}\otimes z_{j})\in Z^{n}_{2}$, $D(s(z_{i}\otimes
z_{j}))=[z_{i},z_{j}]+\xi$ with
$\xi\in\mathbb{L}(Z^{n}_{1})*\mathbb{L}^{+}(Z^{n}_{2})$,

  * (v)

for $i\geq 3$, $D(Z^{n}_{i})\subset(\mathbb{L}(Z^{n}_{1}\oplus\cdots
Z^{n}_{i-2})*\mathbb{L}^{+}(Z^{n}_{i-1}\oplus Z^{n}_{i})$,

  * (vi)

for all $z\in Z$, $\delta(z)=z_{1}+\cdots+z_{n}+\xi$ with
$\xi\in\mathbb{L}(Z^{n}_{1})*\mathbb{L}^{+}(Z^{n}_{2}\oplus\cdots Z^{n}_{n})$.

One part of the proof comes from induction and the following modification of
[14, VII.1.(2)].

######  Proposition 1.3.

Let $(\mathbb{L}(V),\partial)$ and $(\mathbb{L}(W),\partial)$ be minimal
cellular Quillen models for $X$ and $Y$ respectively. Then the minimal
cellular Quillen model for $X\times Y$ has the form
$\varphi\colon\left(\mathbb{L}(V\oplus W\oplus s(V\otimes
W)),D\right)\stackrel{{\scriptstyle\simeq}}{{\longrightarrow}}(\mathbb{L}V,\partial)\times(\mathbb{L}W,\partial)$,
with $\varphi(v)=v$, $\varphi(w)=w$, $\varphi(s(v\otimes w))=0$,
$D(v)=\partial(v)$, $D(w)=\partial(w)$ and

$D(s(v\otimes w))=[v,w]+D^{+}(s(v\otimes w)),$

where $D^{+}(s(v\otimes w))\in I_{s}:=\mathbb{L}(V\oplus
W)*\mathbb{L}^{+}(s(V\otimes W))$.

In the proof of Proposition 1.3 we will constantly use the following results.

######  Lemma 1.4.

Let $L:=\left(\mathbb{L}(V\oplus W\oplus s(V\otimes W)),D\right)$ be as in
Proposition 1.3 and $\varphi\colon
L\rightarrow(\mathbb{L}(V),\partial)\times(\mathbb{L}(W),\partial)$ defined,
for $v\in V$ and $w\in W$, as $\varphi(v)=v$, $\varphi(w)=w$ and
$\varphi(s(v\otimes w))=0$. Then $\varphi$ is a quasi-isomorphism.

###### Proof.

We will use a spectral sequence argument. Define an increasing filtration on
$L$ as $F(k)_{r}=\oplus_{p=0}^{k}\left(\mathbb{L}(V\oplus
W)*\mathbb{L}^{r-p}s(V\otimes W)\right)_{r}$, where $r$ denotes degree.
Observe that $D(F(k))\subset F(k)$ because $s(V\otimes W)$ is concentrated in
positive degrees. Filter also $\mathbb{L}(V)\times\mathbb{L}(W)$ by degree,
that is $G(k)=\left(\mathbb{L}(V)\times\mathbb{L}(W)\right)_{\leq k}$. Observe
that $\varphi$ respects these filtrations. Indeed, if $x\in\mathbb{L}(V\oplus
W)*\mathbb{L}^{|x|-k}s(V\otimes W)$ then either $\varphi(x)=0$ or $|x|=k$.
Then the induced morphism is

$E^{0}(\varphi)\colon\left(\mathbb{L}(V\oplus W\oplus s(V\otimes
W)),D_{0}\right)\rightarrow(\mathbb{L}(V),0)\times(\mathbb{L}(W),0)$

with $D_{0}(v)=D_{0}(w)=0$ and $D(s(v\otimes w))=[v,w]$, for $v\in V$ and
$w\in W$. By [14, VII.7. (10)], $E^{0}(\varphi)$ is a quasi-isomorphism. ∎

We also need

######  Lemma 1.5.

Let $\left(\mathbb{L}(V\oplus W\oplus s(V\otimes W)),D\right)$ be as in
Proposition 1.3. Then for every $\xi\in\mathbb{L}^{+}(V)*\mathbb{L}^{+}(W)$
there exist $\beta(\xi),D^{+}(\beta(\xi))\in I_{S}$ such that
$D(\beta(\xi))=\xi+D^{+}(\beta(\xi))$.

###### Proof.

The result is obvious if $\xi\in V*W$. Now suppose the result is true for
elements of bracket length less than $n$ and let
$\xi\in\mathbb{L}^{+}(V)*\mathbb{L}^{+}(W)$ be of bracket length $n\geq 3$. By
Jacobi, $\xi$ is linear combination of elements of the form
$[\xi^{\prime},\omega]$ with
$\xi^{\prime}\in\mathbb{L}^{+}(V)*\mathbb{L}^{+}(W)$ of bracket length less
than $n$. Then, by induction

$D([\beta(\xi^{\prime}),\omega])=[\xi^{\prime},\omega]+[D^{+}(\beta(\xi^{\prime}),\omega]-(-1)^{|\xi^{\prime}|}[\beta(\xi^{\prime}),\partial(w)]$

with
$[D^{+}(\beta(\xi^{\prime}),\omega]-(-1)^{|\xi^{\prime}|}[\beta(\xi^{\prime}),\partial(w)]\in
I_{s}$. ∎

We may now proceed to the

###### Proof of Proposition 1.3.

First write $V=\oplus_{m\geq 0}V_{m}$ with $\partial
V_{m}\subset\mathbb{L}(V_{<m})$ and analogously for $W$. Now write
$L_{m}:=\left(\mathbb{L}(V\oplus W_{<m}\oplus s(V\otimes W_{<m})),D\right)$
and suppose we have

$\varphi_{m}\colon
L_{m}\stackrel{{\scriptstyle\simeq}}{{\longrightarrow}}(\mathbb{L}(V),\partial)\times(\mathbb{L}(W_{<m}),\partial)$

as in the statement of the proposition and such that, for every $n$ and
$m^{\prime}<m$, $\left(\mathbb{L}(V_{\leq n}\oplus W_{\leq m^{\prime}}\oplus
s(V_{\leq n}\otimes W_{\leq m^{\prime}}),D\right)$ is a sub dgl of $L$.
Observe that $\varphi_{1}$ exists by [9, Section 3]. We will construct
$\varphi_{m+1}$ inductively. Throughout the proof we will denote
$L_{n,m}:=\left(\mathbb{L}(V_{<n}\oplus W_{<m}\oplus s(V_{<n}\otimes
W_{<m})),D\right)$, $I_{s}^{n,m}$ the Lie ideal of $L_{n,m}$ generated by
$s(V_{<n}\otimes W_{<m})$ and

$\varphi_{n,m}\colon
L_{n,m}\to(\mathbb{L}(V_{<n}),\partial)\times(\mathbb{L}(W_{<m}),\partial)$

the obvious restriction. Observe that, by Lemma 1.4, $\varphi_{m,n}$ is a
quasi-isomorphism whenever it is defined.

Let $v\otimes w\in V_{0}\otimes W_{m}$. We have that
$\partial([v,w])=(-1)^{|v|}[v,\partial w]$ is a cycle in $\ker\varphi_{1,m}$.
Since $\ker\varphi_{1,m}$ is acyclic, there exists $\tau\in\ker\varphi_{1,m}$
such that $D(\tau)=D([v,w])$. Write $\tau=\tau^{\prime}+\tau^{\prime\prime}$
with $\tau^{\prime}\in\mathbb{L}^{+}(V)*\mathbb{L}^{+}(W_{<m})$ and
$\tau^{\prime\prime}\in I_{s}$. By Lemma 1.5 applied to $L_{1,m}$ , there
exist $\beta(\tau^{\prime})\in I_{s}^{1,m}$ such that
$D(\beta(\tau^{\prime}))=\tau^{\prime}+D^{+}(\beta(\tau^{\prime}))$. Then
$D^{+}(s(v\otimes w)):=D(\beta(\tau^{\prime}))-\tau\in I_{s}^{1,m}$ and
$D(D^{+}(s(v\otimes w)))=-D(\tau)=-D([v,w])$. Now define $D(s(v\otimes
w))=[v,w]+D^{+}(s(v\otimes w))$.

By doing this process over homogeneous basis of $V_{0}$ and $W_{m}$ we
construct a Koszul-Quillen extension
$L_{m}\rightarrowtail(L_{m}*\mathbb{L}(W_{m}\oplus s(V_{0}\otimes W_{m}),D)$
with $D$ verifying the properties of the proposition’s statement and all
possible $L_{n,m}$ being sub dgls. In particular, $\varphi_{1,m+1}$ is now
defined.

Now suppose we have constructed a Koszul-Quillen extension
$L_{m}\rightarrowtail(L_{m}*\mathbb{L}(W_{m}\oplus s(V_{<n}\otimes W_{m}),D)$
with $D$ as desired and all possible $L_{n,m}$ being sub dgls. In particular,
$\varphi_{n,m+1}$ and $\varphi_{n+1,m}$ are defined and are quasi-
isomorphisms.

Let $v\otimes w\in V_{n}\otimes W_{m}$. We have that $[\partial v,\partial w]$
is a cycle in the acyclic ideal $\ker\varphi_{n,m}$, this implies that there
exists $\psi\in\ker\varphi_{n,m}$ such that $D(\psi)=[\partial v,\partial w]$.
Now we have a cycle $[\partial v,w]+(-1)^{|v|}\psi$ in $\ker\varphi_{n,m+1}$.
As before, there exists an element $\omega\in\ker\varphi_{n,m+1}$, which can
actually be taken in $I_{s}^{n,m+1}$, such that $D(\omega)=[\partial
v,w]+(-1)^{|v|}\psi$. Analogously, there exists $\pi\in I_{s}^{n+1,m}$ such
that $D(\pi)=[v,\partial w]-\varphi$. Finally, define $D(s(v\otimes
w))=[v,w]-\omega-(-1)^{|v|}\pi\in\ker\varphi_{n+1,m+1}$.

By repeating in homogeneous basis, we extend to a Koszul-Quillen extension
$L_{m}\rightarrowtail(L_{m}*(\mathbb{L}W_{m}\oplus s(V_{<m+1}\otimes
W_{m})),D)$ that closes the induction. ∎

######  Remark 1.6.

We have proven that such model actually respects _cone-length_ filtration in
the sense that the restriction to $\left(\mathbb{L}(V_{\leq n}\oplus W_{\leq
n}\oplus s(V_{\leq n}\otimes W_{\leq m})),D\right)$ is quasi-isomorphic to
$(\mathbb{L}(V_{\leq n}),\partial)\times(\mathbb{L}(W_{\leq m}),\partial)$.
Moreover we have constructed it such that

$D(s(V_{n}\otimes W_{m}))\subset\mathbb{L}(V_{\leq n}\oplus W_{\leq m}\oplus
s(V_{<n}\otimes W_{\leq m})\oplus s(V_{\leq n}\otimes W_{<m})).$

Lastly, since the differentials $\partial$ are minimal, $D$ has no linear
part.

To finalize the proof of Proposition 1.2 we lift $\Delta$ through $\varphi$ to
get $\delta$ and then use Lemma 1.5, as in the proof of Lemma 1.3, to _fix_
$\delta$ such that it verifies (vi).

##  2\. Proof of Theorem

Let $\varphi\colon(\mathbb{L}(V),\partial)\rightarrowtail(\mathbb{L}(V\oplus
W),\partial)$ be a Koszul-Quillen model for $f$, then the inclusion
$T^{n+1}(i)\hookrightarrow Y^{n+1}$ is modelled by the inclusion

$\iota\colon(\mathbb{L}((V\oplus W)^{n+1}_{1}\oplus
U^{n+1}),D)\rightarrowtail(\mathbb{L}(V\oplus W)^{n+1}).$

Now suppose ${\rm secat}(f)=n$, then by Proposition 1.1, the $m+1$ diagonal
factors through the fat-wedge, by standard rational homotopy theory
techniques, there is a homotopy commutative diagram

${(\mathbb{L}((V\oplus W)^{n+1}_{1}\oplus U^{n+1}),D)}$${(\mathbb{L}(V\oplus
W),\partial)}$${(\mathbb{L}(V\oplus
W)^{n+1},D)}$$\scriptstyle{\iota}$$\scriptstyle{\alpha}$$\scriptstyle{\delta}$

Observe that the $\alpha$ verifies the conditions of Theorem because the
linear parts of homotopic morhpisms between minimal dgls coincide.

Conversely, suppose a map $\alpha$ of Theorem exists, then we have a
commutative diagram

${\mathbb{L}(Z^{n}_{1}\oplus\cdots\oplus
Z^{n}_{n},D)}$${\mathbb{L}(Z,\partial)}$${\mathbb{L}(Z_{1},\partial)\times\cdots\times\mathbb{L}(Z_{n},\partial)}$$\scriptstyle{\varphi}$$\scriptstyle{\simeq}$$\scriptstyle{\Delta}$$\scriptstyle{\iota\circ\alpha}$

which exposes $\iota\circ\alpha$ as a model for the diagonal $n+1$ inclusion.
Realizing $\iota\circ\alpha$ gives a homotopy factorization of the diagonal
through the fat-wedge.

Acknowledgements. The first author was partially supported by the MICINN grant
PID2020-118753GB-I00 of the Spanish Government and the Junta de Andalucía
grant ProyExcel-00827

This work has been supported by the National Science Centre grant
2016/21/P/ST1/03460 within the European Union’s Horizon 2020 research and
innovation programme under the Marie Skłodowska-Curie grant agreement No.
665778.

## References

  * [1] I. Berstein and T. Ganea. The category of a map and of a cohomology class. Fund. Math., 50 (3), 265–279, (1962).
  * [2] J.G. Carrasquel-Vera. The rational sectional category of certain maps. Ann. Sc. Norm. Super. Pisa Cl. Sci. (5), 17(2), 805–813, (2017).
  * [3] N. Dupont, A counterexample of the Lemaire-Sigrist conjecture, Topology 38(1), 189–196, (1999).
  * [4] M. Farber. Topological complexity of motion planning. Discrete Comput. Geom., 29(2), 211– 221, 2003.
  * [5] Y. Félix and S. Halperin, Rational L.-S. category and its applications, Trans. of the Amer. Math. Soc. 273(1). 1–37, (1982).
  * [6] Y. Félix, S. Halperin and J.-C. Thomas, Rational Homotopy Theory, Graduate Texts in Mathematics 205, Spinger (2000).
  * [7] R.H. Fox. On the Lusternik-Schnirelmann category. Ann. of Math. 42, 333–370, (1941).
  * [8] J.-M. Lemaire, and F. Sigrist, Sur les invariants d’homotopie rationnelle lié s à la L.S. category. Commentarii Mathematici Helvetici, 56, 103–122, (1981).
  * [9] G. Lupton and S. B. Smith, Rationalized evaluation subgroups of a map II: Quillen models and adjoint maps, J. Pure Appl. Algebra, 209 (1), 173–188, (2007).
  * [10] L. Lusternik and L. Schnirelmann. Méthodes topologiques dans les problèmes variationnels, Volume 188. Hermann, Paris, (1934).
  * [11] D. Quillen, Rational homotopy theory, Ann. of Math. (2) 90, 205–295, (1969).
  * [12] A. S. Schwarz. The genus of a fiber space. Amer. Math. Sci. Transl., 55, 49–140, (1966).
  * [13] D. Sullivan, Infinitesimal computations in topology, Publications Mathématiques de l’IHÉS, 47, 269–-331, (1977).
  * [14] D. Tanré, _Homotopie rationnelle: modèles de Chen, Quillen, Sullivan_ , Lecture Notes in Math. 1025, Springer, (1983).