This paper has been accepted for publication in _IEEE Robotics And Automation
Letters_ (RA-L).

DOI: 10.1109/LRA.2021.3080633

IEEE Explore: https://ieeexplore.ieee.org/document/9431754

Please cite the paper using the following bibtex code:

    @article{nguyen2021miliom,
      title={MILIOM: Tightly Coupled Multi-Input Lidar-Inertia Odometry and Mapping},
      author={Nguyen, Thien-Minh
              and Yuan, Shenghai
              and Cao, Muqing
              and Yang, Lyu
              and Nguyen, Thien Hoang
              and Xie, Lihua},
      journal={IEEE Robotics and Automation Letters},
      volume={6},
      number={3},
      pages={5573--5580},
      year={2021},
      publisher={IEEE}
    }

# MILIOM: Tightly Coupled Multi-Input Lidar-Inertia Odometry and Mapping

Thien-Minh Nguyen1, , Shenghai Yuan1, Muqing Cao1, Lyu Yang1,
Thien Hoang Nguyen1, and Lihua Xie1 Manuscript received: February 24, 2021;
Revised April 21, 2021; Accepted May 12, 2021.This paper was recommended for
publication by Editor Javier Civera upon evaluation of the Associate Editor
and Reviewers’ comments. This work was supported by the Wallenberg AI,
Autonomous Systems and Software Program (WASP) funded by the Knut and Alice
Wallenberg Foundation, under the Grant Call 10013 - Wallenberg-NTU
Presidential Postdoctoral Fellowship 2020. (Corresponding Author: Thien-Minh
Nguyen)1The authors are with School of Electrical and Electronic Engineering,
Nanyang Technological University, Singapore 639798, 50 Nanyang Avenue.
(e-mail: {thienminh.nguyen@, shyuan@, mqcao@, lyu.yang<EMAIL_ADDRESS>elhxie@}ntu.edu.sg).Digital Object Identifier (DOI): see top of this page.

###### Abstract

In this paper we investigate a tightly coupled Lidar-Inertia Odometry and
Mapping (LIOM) scheme, with the capability to incorporate multiple lidars with
complementary field of view (FOV). In essence, we devise a time-synchronized
scheme to combine extracted features from separate lidars into a single
pointcloud, which is then used to construct a local map and compute the
feature-map matching (FMM) coefficients. These coefficients, along with the
IMU preinteration observations, are then used to construct a factor graph that
will be optimized to produce an estimate of the sliding window trajectory. We
also propose a key frame-based map management strategy to marginalize certain
poses and pointclouds in the sliding window to grow a global map, which is
used to assemble the local map in the later stage. The use of multiple lidars
with complementary FOV and the global map ensures that our estimate has low
drift and can sustain good localization in situations where single lidar use
gives poor result, or even fails to work. Multi-thread computation
implementations are also adopted to fractionally cut down the computation time
and ensure real-time performance. We demonstrate the efficacy of our system
via a series of experiments on public datasets collected from an aerial
vehicle.

###### Index Terms:

SLAM, Range Sensing, Aerial Systems: Perception and Autonomy

##  I Introduction

Over the years, 3D lidars have proved to be a reliable and accurate
localization solution for autonomous systems. Compared with the other class of
onboard self-localization (OSL) based on camera, lidar clearly possesses many
superior characteristics. In terms of sensing capability, lidar can provide
360 degree observation of the environment, with metric-scaled features, while
the best fisheye cameras still only have up to $180^{0}$ field of view (FOV),
and can only extract features of unknown scale. Even when compared to multi-
camera or RGBD camera systems, which are supposed to solve the scale problem,
lidar still has a much higher sensing range, is almost unaffected by lighting
conditions, and requires very little calibration effort. Nevertheless, high
cost and weight has been one of lidar’s main weaknesses, and has mostly
restricted its application to ground robot platforms. However, in recent
years, both the cost and weight factors have been significantly reduced thanks
to many technological innovations. Hence, we have observed an uptick of 3D
lidar applications in the robotics community.

Recently some researchers have started investigating the use of multiple 3D
lidars for better coverage of the environment, especially in the field of
autonomous driving [1, 2, 3, 4, 5, 6]. In fact, though a single lidar mounted
on the top of the vehicle can be cost sufficient, there are many scenarios
where the use of multiple lidars can be advantageous. For example, in some
complex urban environments [3, 1], the combined FOV from multiple lidars
provides a more comprehensive observation of the surrounding, which allows the
vehicle to navigate through the environment and avoid obstacles more flexibly.
In a challenging subterranean scenario [2], where the robot has to travel
through some narrow and irregular terrains that can cause the lidar’s FOV to
be blocked, a multi-lidar based localization technique was shown to be the
solution to overcome such challenge.

Figure 1: Illustration of the localization drift by using single lidar in a
challenging environment.

In the aforementioned works, we note that lidar was used mostly on ground
robots operating in 2D space, hence it is usually assumed that the ground
plane can be observed most of the time. This is no longer the case with aerial
robots, which is the targeted application of this work. Fig. 1 gives an
example of such a scenario, in which, there are two lidars mounted on an
Unmanned Aerial Vehicle (UAV), and the only nearby objects that can be
observed are the building facade and the ground. Hence with a single
horizontal lidar, a commonly known Lidar Odometry and Mapping (LOAM)
method111https://github.com/HKUST-Aerial-Robotics/A-LOAM gives the result that
expectedly drifts in the vertical direction (blue path in Fig. 1), as it
cannot observe the ground. Similarly, when using the vertical lidar, the
result exhibits significant drift in the lateral direction (orange path Fig.
1), as it lacks observations to counter drift in that direction. In this work
our main goal is to show that when these lidars are combined in a tighly
coupled manner, the challenge can be effectively overcome.

##  II Related Works

Many 3D lidar-based localization methods have been proposed in the last
decade. Most notable is the work by Zhang et al [7, 8]. In [7], the authors
proposed a standard framework for lidar-based odometry and mapping. Indeed,
their technique to calculate smoothness and determine plane and edge features
from lidar scans remains a popular approach that is still widely adopted to
this day. In [8], the authors proposed a loosely coupled lidar-visual-inertial
localization and mapping system, which was also demonstrated to work on a UAV.
However, several techniques used in this work seem to be no longer efficient
compared to current state-of-the-art. For example, the edge and plane features
have separate factors with high non-linearity, while in recent works, it has
been shown that one edge factor can be substituted by two plane factors [3, 9]
with relatively simpler formulation. In addition, the voxel-based map
management scheme appears to be quite complex. In contrast, inspired by
contemporary VIO frameworks, in this work we propose a more efficient
framework based on so-called key frame, which simplifies the query as well as
update on the map.

Besides, to the best of our knowledge, few works have satisfactorily
investigated the tight coupling of IMU preintegration factors with lidar
factors in the literature, despite its extensive applications in visual-
inertial systems [10, 11, 12, 13, 14]. In [15], a method based on this idea
was proposed, however the lidar processing part assumes the presence of
distinct planes that can be extracted via RANSAC in the environments. In [16],
a tightly-coupled framework called LIO-Mapping was proposed. Inspired by the
VIO frameworks, both of the aforementioned works use a sliding window, and
employ IMU preintegration and lidar features to construct cost factors that
couple the measurements with the robot poses in the sliding window. Later,
Shan et al. released the LIO-SAM package [16] which optimizes a pose graph
leveraging IMU preintegration and pose priors as the cost factors.
Unfortunately, LIO-Mapping performs very poorly when forced to run in real-
time, which is also reported in other works [17, 9, 2]. We believe this is due
to several inefficient implementation designs. On the other hand, LIO-SAM
requires 9DoF IMU, and should be better categorized as a loosely coupled
method, as it uses pose priors that are obtained from a LOAM process with a
separate LM-based optimization process proposed earlier[18], instead of
jointly optimizing the lidar feature and IMU preintegration factors together.
Besides, we also note that the aforementioned works do not generalize to
multi-lidar case, which poses a new challenge as it requires us to design a
stable synchronization scheme, as well as a time-efficient frontend to ensure
real-time performance. As will be shown later, despite integrating multiple
lidars, and employing tightly coupled scheme, our method can still operate in
real-time and provide accurate estimate, thanks to the use of multi-thread
implementation and low programming overheads.

The main contribution of this work can be listed as follows:

  * •

We propose a general scheme to combine multiple lidars with complementary FOV
for feature-based Lidar-Inertia Odometry and Mapping (LIOM) application.

  * •

We propose a tightly-coupled, key frame-based, multi-threaded multi-input LIOM
framework to achieve robust localization estimate.

  * •

We demonstrate the advantages of the method over existing methods via
extensive experiments on an advanced UAV platform.

The remainder of the paper is organized as follows: in Sec. III, we lay out
the basic definitions, notations and formulations of the problem; Sec. IV then
presents an overview of the software structure and the flow of information.
Sec. V and Sec. VI discuss in details how lidar data and IMU are processed.
Next we describe the construction of the local map and the feature-to-map
matching (FMM) process that produces the FMM coefficients for constructing the
lidar factors. Sec. VIII explains the key frame management procedures. We
demonstrate the capability of our method via several experiments on UAV
datasets in Sec. IX. Finally, Sec. X concludes our work.

##  III Preliminaries

###  III-A Nomenclature

In this paper we use $(\cdot)^{\top}$ to denote the transpose of an algebraic
vector or matrix under $(\cdot)$. For a vector
${\bm{\mathbf{x}}}\in\mathbb{R}^{m}$,
$\left\lVert{\bm{\mathbf{x}}}\right\rVert$ stands for its Euclidean norm, and
$\left\lVert{\bm{\mathbf{x}}}\right\rVert_{{\bm{\mathbf{G}}}}^{2}$ is short-
hand for
$\left\lVert{\bm{\mathbf{x}}}\right\rVert_{{\bm{\mathbf{G}}}}^{2}={\bm{\mathbf{x}}}^{\top}{\bm{\mathbf{G}}}{\bm{\mathbf{x}}}$.
For two vectors $\mathbf{v}_{1}$, $\mathbf{v}_{2}$,
$\mathbf{v}_{1}\times\mathbf{v}_{2}$ denotes their cross product. In later
parts, we denote $\mathbf{R}\in\mathrm{SO(3)}$ as the rotation matrix, and
$\mathbf{T}\in\mathrm{SE(3)}$ as the transformation matrix. We denote
$\mathbb{Q}$ as the set of unit quaternions, with $\mathbb{q}$ as its identity
element. Given $\mathbf{q}\in\mathbb{Q}$, $\mathcal{R}(\mathbf{q})$ denotes
its corresponding rotation matrix, and ${\bm{\mathbf{vec}}}(\mathbf{q})$
returns its vector part. Also, in the later parts, we refer to the mapping
$\mathcal{E}\colon\mathbb{R}^{3}\mapsto\mathbb{Q}$ and its inverse
$\mathcal{E}^{-1}\colon\mathbb{Q}\to\mathbb{R}^{3}$ to convert a rotation
vector, i.e. the angle-axis representation of rotation, to its quaternion
form, and vice versa. The formulae for these operations can be found in [12].

When needed for clarity, we attach a left superscript to an object to denote
its coordinate frame. For example ${}^{\texttt{A}}\mathbf{v}$ implies the
coordinate of the vector $\mathbf{v}$ is in reference to the frame A, and
${}^{\texttt{B}}\mathcal{F}$ indicates that the coordinate of the points in
the pointcloud $\mathcal{F}$ is w.r.t. to the frame B. A rotation matrix and
transformation matrix between two coordinate frames are denoted with the
frames attached as the left-hand-side superscript and subscript, e.g.,
${}^{\texttt{A}}_{\texttt{B}}\mathbf{R}$ and
${{}^{\texttt{A}}_{\texttt{B}}\mathbf{T}}$ are called the rotation and
transform matrices from frame A to B, respectively. When the coordinate frames
are the body frame at different times, depending on the context, we may omit
and rearrange the superscript and subscripts to keep the notation concise,
e.g.,
${}^{k}\mathbf{R}_{k+1}\triangleq{}^{\texttt{B}_{k}}_{\texttt{B}_{k+1}}\mathbf{R}$,
or
${}^{w}_{m}\mathbf{T}\triangleq{}^{\texttt{B}_{w}}_{\texttt{B}_{m}}\mathbf{T}$.

\begin{overpic}[width=433.62pt]{frames.png} \put(70.0,35.0){
${}^{\texttt{B}_{0}}\vv{y}$} \put(81.0,34.0){ ${}^{\texttt{L}}\vv{y}$}
\put(84.0,18.0){ ${}^{\texttt{L}}\vv{x}$} \put(84.0,26.0){
${}^{\texttt{B}_{0}}\vv{x}$} \put(68.0,40.0){ ${}^{\texttt{L}}\vv{z}$}
\put(73.0,4.5){ ${}^{\texttt{B}_{0}}\vv{z}$} \par\put(61.0,3.0){ $\vv{g}$}
\par\put(48.0,34.0){ ${}_{\texttt{B}_{w}}^{\texttt{L}}\mathbf{T}$}
\put(30.0,29.0){ ${}_{\texttt{B}_{j}}^{\texttt{L}}\mathbf{T}$}
\put(27.0,16.0){ ${}_{\texttt{B}_{k}}^{\texttt{L}}\mathbf{T}$} \end{overpic}
Figure 2: The local frame L defined via the initial body frame
$\texttt{B}_{0}$, and the poses of the robot at later time instances.

Throughout this paper we define a local frame L whose origin coincides with
the position of the body frame at the initial time, denoted as
$\texttt{B}_{0}$. In addition, the ${}^{\texttt{L}}\vv{z}$ axis of L points
towards the opposite direction of the gravity vector $\vv{g}$ (see Fig. 2),
its ${}^{\texttt{L}}\vv{x}$ axis points to the same direction of the
projection of ${}^{\texttt{B}_{0}}\vv{x}$ on a plane perpendicular with
${}^{\texttt{L}}\vv{z}$ axis, and the ${}^{\texttt{L}}\vv{y}$ axis can be
determined by the right-hand rule. Indeed, these calculations can be done by
simply using the initial acceleration reading from the IMU, which contains
only the coordinate of the vector $\vv{g}$ in $\texttt{B}_{0}$, to determine
the pitch and roll angles of $\texttt{B}_{0}$ with respect to L, assuming that
the error due to acceleration bias is negligible.

###  III-B State estimates

In reference to Fig. 2, we define the robot states to be estimated at time
$t_{k}$ as:

$\displaystyle\mathcal{X}_{k}$ $\displaystyle=\Big{(}\mathbf{q}_{k},\
\mathbf{p}_{k},\ \mathbf{v}_{k},\ \mathbf{b}^{\omega}_{k},\
\mathbf{b}^{a}_{k}\Big{)},$ (1)

where $\mathbf{q}_{k}\in\mathbb{Q}$, $\mathbf{p}_{k}\in\mathbb{R}^{3}$,
$\mathbf{v}_{k}\in\mathbb{R}^{3}$ are respectively the orientation quaternion,
position and velocity of the robot w.r.t. the local frame L at time $t_{k}$;
$\mathbf{b}^{a}_{k},\ \mathbf{b}^{\omega}_{k}\in\mathbb{R}^{3}$ are
respectively the IMU accelerometer and gyroscope biases. Hence, we denote the
state estimate at each time step $k$, and the sliding window of size $M$ as
follows:

$\displaystyle\hat{\mathcal{X}}_{k}$
$\displaystyle=\Big{(}\hat{\mathbf{q}}_{k},\ \hat{\mathbf{p}}_{k},\
\hat{\mathbf{v}}_{k},\ \hat{\mathbf{b}}^{\omega}_{k},\
\hat{\mathbf{b}}^{a}_{k}\Big{)},$ (2) $\displaystyle\hat{\mathcal{X}}$
$\displaystyle=\left(\hat{\mathcal{X}}_{w},\ \hat{\mathcal{X}}_{w+1},\dots,\
\hat{\mathcal{X}}_{k}\right),\ w\triangleq k-M+1.$ (3)

Note that in this work we assume the extrinsic parameters have been manually
calibrated, and each lidar’s pointcloud has been transformed to the body frame
before being processed further. Moreover, depending on the context we may also
refer to $\hat{\mathbf{R}}_{k}\triangleq\mathcal{R}(\hat{\mathbf{q}}_{k})$ as
the rotation matrix estimate, and
$(\hat{\mathbf{R}}_{k},\hat{\mathbf{p}}_{k})$ or
$(\hat{\mathbf{q}}_{k},\hat{\mathbf{p}}_{k})$ as the pose estimate.

##  IV General Framework

Fig. 3 presents a snapshot of our MILIOM system at some IMU sample time $t$
past $t_{k}$, where $t_{k}$ is the start time of the skewed combined feature
cloud (SCFC) $\breve{\mathcal{F}}_{k}$ (more details are given in Sec. V).
Indeed, there are two types of features that can be extracted from the lidar
scans, namely edge feature and plane feature. Thus when we refer to a feature
cloud $\mathcal{F}$, we do mean the compound
$\mathcal{F}\triangleq(\mathcal{F}^{e},\ \mathcal{F}^{p})$, where
$\mathcal{F}^{e}$ is the set of edge features, and $\mathcal{F}^{p}$ is the
set of plane features. Whenever the context requires, we can make the notation
more explicit.

The inputs of the system are the IMU measurements, i.e. acceleration
$\mathbf{a}_{t}$ and angular velocity $\bm{\omega}_{t}$, and lidar raw
pointclouds $\mathcal{P}_{m}^{i}$. The outputs are the optimized estimates of
the sliding window state estimates, denoted as
$\\{\hat{\mathcal{X}}_{w},\hat{\mathcal{X}}_{w+1},\dots,\hat{\mathcal{X}}_{k}\\}$,
and the IMU-predicted state estimate up to time $t$, denoted as
$\hat{\mathcal{X}}_{t}$.

\begin{overpic}[width=433.62pt]{overview.png}
\put(95.0,43.0){\footnotesize$\hat{\mathcal{X}}_{t}$}
\put(16.6,45.0){\footnotesize$\begin{array}[]{c}\bm{\omega}_{t},\\\
\mathbf{a}_{t}\end{array}$}
\put(18.8,31.0){\footnotesize$\mathcal{P}^{i}_{m}$}
\put(45.0,28.0){\footnotesize$\breve{\mathcal{F}}_{k}$}
\put(68.0,43.5){\footnotesize$\mathcal{I}_{k}$}
\put(82.5,16.5){\footnotesize$\\{\dots\mathcal{L}^{i}_{m}\\}$}
\put(-2.0,16.5){\footnotesize$\\{{}^{\texttt{L}}\mathcal{F}_{w},{}^{\texttt{L}}\mathcal{F}_{w+1},\dots,{}^{\texttt{L}}\mathcal{F}_{k}\\}$}
\put(65.8,12.0){\footnotesize$\mathcal{M}_{w}$}
\put(50.0,1.75){\footnotesize$\\{\hat{\mathcal{X}}_{w},\hat{\mathcal{X}}_{w+1},\dots,\hat{\mathcal{X}}_{k}\\}$}
\end{overpic} Figure 3: Main processes and available quantities in the system
by the time $t$. Note that lines of different colors are not connected.

In the above framework, the joint optimization process consumes the most
computation time, while most other processes are meant to extract the key
information used for constructing the factors that will be optimized in the
following cost function:

$\displaystyle f(\hat{\mathcal{X}})$ $\displaystyle\triangleq\Bigg{\\{}\mkern
9.0mu\sum_{m=w+1}^{k}\left\lVert\bm{r}_{\mathcal{I}}(\hat{\mathcal{X}}_{m-1},\hat{\mathcal{X}}_{m},\mathcal{I}_{m})\right\rVert^{2}_{{\bm{\mathbf{P}}}_{\mathcal{I}_{m}}^{-1}}$
$\displaystyle\qquad+\sum_{m=w}^{k}\sum_{i=1}^{\left\lvert\mathcal{C}_{m}\right\rvert}\rho\left(\left\lVert\bm{r}_{\mathcal{L}}(\hat{\mathcal{X}}_{m},\mathcal{L}_{m}^{i})\right\rVert^{2}_{{\bm{\mathbf{P}}}_{\mathcal{L}_{m}^{i}}^{-1}}\right)\Bigg{\\}},$
(4)

where $\bm{r}_{\mathcal{I}}(\cdot)$ and $\bm{r}_{\mathcal{L}}(\cdot)$ are the
IMU and lidar residuals, $\rho(\cdot)$ is the Huber loss function to suppress
outliers, ${\bm{\mathbf{P}}}_{\mathcal{I}_{m}}$ and
${\bm{\mathbf{P}}}_{\mathcal{L}_{m}^{i}}$ are the covariance matrix of their
measurements, $\mathcal{I}_{m}$ and $\mathcal{L}_{m}$ are respectively IMU
preintegration and lidar FMM coefficients, $\mathcal{C}_{m}$ denotes the set
of FMM features derived from the lidar feature cloud at time $t_{m}$. Note
that each IMU factor in (4) is coupled with two consecutive states, while each
lidar factor is coupled with only one. In this paper, the IMU factors are
calculated via an integration scheme as described in our previous work [12].
Hence we only recall some important steps due to page constraints. In the next
sections we will elaborate on the processing of lidar and IMU data for use in
the optimization of (4).

##  V Lidar processing

Fig. 4 presents an illustration of the synchronization between the lidars.
Specifically, for a lidar $i$, we can obtain a sequence of pointclouds
$\mathcal{P}^{i}_{m}$, each corresponds to a 3D scan of the environment over a
fixed period (which is around 0.1s for the
Ouster222https://ouster.com/products/os1-lidar-sensor/ sensor used in this
work). We assume that lidar 1 is the primary unit, whose start times are used
to determine the time instances of the state estimates on the sliding window,
and others are referred to as secondary lidars. Each pointcloud input is put
through a feature extraction process using the smoothness criteria as in [7]
to obtain the corresponding feature cloud. We then merge all feature clouds
whose start times fall in the period $[t_{k-1},t_{k})$ to obtained the SCFC
$\breve{\mathcal{F}}_{k}$.

\begin{overpic}[width=433.62pt]{lidar_lidar_sync.png}
\put(3.0,42.0){\footnotesize$\mathcal{P}^{1}_{m}$}
\put(3.0,36.5){\footnotesize$\mathcal{P}^{2}_{m}$}
\put(3.0,31.0){\footnotesize$\mathcal{P}^{n}_{m}$}
\par\put(2.0,22.5){\footnotesize$\breve{\mathcal{F}}_{w}$}
\put(2.0,17.0){\footnotesize$\breve{\mathcal{F}}_{w+1}$}
\put(2.0,11.5){\footnotesize$\dots$}
\put(2.0,6.5){\footnotesize$\breve{\mathcal{F}}_{k}$}
\par\put(10.5,0.0){\footnotesize$t_{w}$} \put(29.1,0.0){\footnotesize$\dots$}
\put(49.0,0.0){\footnotesize$t_{k-1}$} \put(67.0,0.0){\footnotesize$t_{k}$}
\par\put(91.0,0.0){\footnotesize$t_{k}^{\prime}$} \end{overpic} Figure 4:
Illustration of the feature extraction and synchronization between the lidar
scans. Assuming that there are $n$ lidars, $\mathcal{P}^{i}_{m}$ refers to the
pointclouds of lidar $i\in\\{1,2,\dots,n\\}$. Note that
$\breve{\mathcal{F}}_{k}$ is the last SCFC that comes out from the feature
extraction and synchronization block with the start time $t_{k}$ and the end
time $t_{k}^{\prime}$.

Note that if the scan period of a lidar is 0.1s, then a SCFC can stretch over
a maximum of 0.2s period. This is a significant time-span where the robot
motion can affect the pointcloud in the scan, however since all of the points
in the pointclouds are timestamped, we can use the IMU-based state propagation
to compensate for the robot motion, which we refer to as the ”deskew” process.
Specifically, for a feature point ${}^{\texttt{B}_{t_{s}}}{\bm{\mathbf{f}}}$
in the SCFC $\breve{\mathcal{F}}_{k}$ stretching over the time span $[t_{k},\
t_{k}^{\prime})$, its coordinates w.r.t. the robot’s pose at time $t_{k}$ can
be calculated by the interpolation:

${}^{\texttt{B}_{t_{s}}}{\bm{\mathbf{f}}}\triangleq\mathcal{R}\left(\mathrm{slerp}\left(\mathbb{q},{}^{t_{k}}_{t_{k}^{\prime}}\breve{\mathbf{q}},s\right)\right){}^{\texttt{B}_{t_{s}}}{\bm{\mathbf{f}}}+s{}^{t_{k}}_{t_{k}^{\prime}}\breve{\mathbf{p}},$
(5)

where $s=\frac{t_{s}}{t_{k}^{\prime}-t_{k}}$ and
$\mathrm{slerp}(\mathbf{q}_{1},\mathbf{q}_{2},s)$ is the spherical linear
interpolation operation on quaternions [19],
${}^{t_{k}}_{t_{k}^{\prime}}\breve{\mathbf{q}}$ and
${}^{t_{k}}_{t_{k}^{\prime}}\breve{\mathbf{p}}$ are the orientation and
position of $\texttt{B}_{t_{k}^{\prime}}$ w.r.t. $\texttt{B}_{t_{k}}$. More
information on the IMU propagation is given in Sec. VI-B.

After deskew, a SCFC $\breve{\mathcal{F}}_{m}$ becomes
${}^{\texttt{B}}\mathcal{F}_{m}$, the so-called deskewed combined feature
cloud, or simply CFC for short. We further use the state estimates
$\\{\hat{\mathcal{X}}_{w},\hat{\mathcal{X}}_{w+1},\dots,\hat{\mathcal{X}}_{k-1}\\}$
and the IMU-propagated states $\breve{\mathbf{q}}_{k}$,
$\breve{\mathbf{p}}_{k}$ (see Fig. VI-B) to transform
$\\{{}^{\texttt{B}}\mathcal{F}_{w},{}^{\texttt{B}}\mathcal{F}_{w+1},\dots,{}^{\texttt{B}}\mathcal{F}_{k}\\}$
to
$\\{{}^{\texttt{L}}\mathcal{F}_{w},{}^{\texttt{L}}\mathcal{F}_{w+1},\dots,{}^{\texttt{L}}\mathcal{F}_{k}\\}$,
which are employed in the construction of the local map and the FMM process in
Sec. VII.

######  Remark 1.

Our synchronization scheme differs from LOCUS [2] in that the merging of the
pointclouds is made on full-size poinclouds in [2], while ours is made on
feature pointclouds. The reason for choosing this merging scheme is because
the feature extraction part is done on distinct rings, using the information
on the horizontal and vertical angular resolution of the lidar sensors. Hence
if the merging happens before the feature extraction, the ring by ring
structure will be lost and one cannot trace the smoothness of the points on
the ring to extract feature. On the other hand, LOCUS uses General Iterative
Closest Point (GICP) for scan matching, which does not concern features,
however it requires good IMU and/or wheel-inertial odometry (WIO) information
for initial guess of the relative transform between the two pointclouds.

##  VI IMU Processing

In Fig. 5, we illustrate the process of synchronizing the IMU data with the
lidar pointcloud. Recall that $t_{k}^{\prime}$ is the end time of the last
SCFC, and that during the period $[t_{k-1},\ t_{k}^{\prime})$, we obtain the
following IMU samples
$\\{(\bm{\omega}_{\tau_{m}},\mathbf{a}_{\tau_{m}}),(\bm{\omega}_{\tau_{m+1}},\mathbf{a}_{\tau_{m+1}}),\dots,(\bm{\omega}_{\tau_{N^{\prime}}},\mathbf{a}_{\tau_{N^{\prime}}})\\}$.
These samples will be used for two operations below.

\begin{overpic}[width=433.62pt]{lidar_imu_sync.png}
\par\put(0.0,38.0){\footnotesize${}^{\texttt{L}}\mathcal{F}_{w}$}
\put(0.0,33.0){\footnotesize${}^{\texttt{L}}\mathcal{F}_{w+1}$}
\put(0.0,27.0){\footnotesize${}^{\texttt{L}}\mathcal{F}_{k-1}$}
\put(0.0,21.5){\footnotesize$\breve{\mathcal{F}}_{k}$}
\par\put(0.0,13.0){\footnotesize IMU} \par\put(31.4,7.0){\footnotesize
Preintegration} \put(75.0,7.0){\footnotesize Propagation}
\par\put(9.0,1.5){\footnotesize$t_{w}$} \put(30.0,1.5){\footnotesize$\dots$}
\put(48.75,1.5){\footnotesize$t_{k-1}$} \put(69.0,1.5){\footnotesize$t_{k}$}
\par\put(93.0,1.0){\footnotesize$t_{k}^{\prime}$} \end{overpic} Figure 5:
Synchronization between the CFC and the IMU. The light blue circles represent
the interpolated samples at the start and end times of the CFCs.

###  VI-A Preintegration

First, we use the samples in the sub-interval $[t_{k-1},\ t_{k})$ to calculate
the so-called preintegration observations using the zero-order-hold (ZOH)
technique:

$\displaystyle[left=\empheqlbrace]$
$\displaystyle\breve{{{\bm{\mathbf{\gamma}}}}}_{\tau_{m+1}}=\breve{{{\bm{\mathbf{\gamma}}}}}_{\tau_{m}}\circ\mathcal{E}\left(\Delta\tau_{m}\bar{\bm{\omega}}_{\tau_{m}}\right),$
(6a)
$\displaystyle\breve{{{\bm{\mathbf{\beta}}}}}_{\tau_{m+1}}=\breve{{{\bm{\mathbf{\beta}}}}}_{\tau_{m}}+\Delta\tau_{m}\mathcal{R}(\breve{{{\bm{\mathbf{\gamma}}}}}_{\tau_{m}})\bar{\mathbf{a}}_{\tau_{m}},$
(6b)
$\displaystyle\breve{{{\bm{\mathbf{\alpha}}}}}_{\tau_{m+1}}=\breve{{{\bm{\mathbf{\alpha}}}}}_{\tau_{m}}+\Delta\tau_{m}\breve{{{\bm{\mathbf{\beta}}}}}_{\tau_{m}}+\frac{\Delta\tau_{m}^{2}}{2}\mathcal{R}(\breve{{{\bm{\mathbf{\gamma}}}}}_{\tau_{m}})\bar{\mathbf{a}}_{\tau_{m}},$
(6c)
$\displaystyle\breve{{{\bm{\mathbf{\gamma}}}}}_{\tau_{0}}\triangleq\mathbb{q},\
\breve{{{\bm{\mathbf{\beta}}}}}_{\tau_{0}}\triangleq{\bm{\mathbf{0}}},\
\breve{{{\bm{\mathbf{\alpha}}}}}_{\tau_{0}}\triangleq{\bm{\mathbf{0}}},$ (6d)
$\displaystyle\bar{\bm{\omega}}_{\tau_{m}}\triangleq({\bm{\omega}}_{\tau_{m}}-\hat{\mathbf{b}}^{\omega}_{k-1}),\
\bar{\mathbf{a}}_{\tau_{m}}\triangleq({\mathbf{a}}_{\tau_{m}}-\hat{\mathbf{b}}^{a}_{k-1}),$
(6e) $\displaystyle\Delta\tau_{m}\triangleq\tau_{m+1}-\tau_{m},\
m\in\\{0,1,\dots,N\\},$ (6f) $\displaystyle\tau_{0}=t_{k-1},\
\tau_{N+1}=t_{k},$ (6g)

where $\circ$ is the quaternion product, and $\mathcal{E}(\cdot)$ is the
mapping from rotation vector to quaternion described in Sec. III.

At the end of the preintagration process (6), we can obtain the preintegration
observations
$\mathcal{I}_{k}\triangleq(\breve{{{\bm{\mathbf{\alpha}}}}}_{t_{k}},\breve{{{\bm{\mathbf{\beta}}}}}_{t_{k}},\breve{{{\bm{\mathbf{\gamma}}}}}_{t_{k}})$.
The use of this observation in an optimization framework has been described in
depth in our previous work [12], and we refer to that for more details.

###  VI-B State Propagation

Given the IMU samples over $[t_{k-1},\ t_{k}^{\prime})$ and the state estimate
$\hat{\mathcal{X}}_{k-1}$, we propagate the state further in time and obtain
the IMU-predicted poses at time $t_{k}$ and $t_{k}^{\prime}$, denoted as
$(\breve{\mathbf{p}}_{k},\breve{\mathbf{q}}_{k})$ and
$(\breve{\mathbf{p}}_{t_{k}^{\prime}},\breve{\mathbf{q}}_{t_{k}^{\prime}})$,
for the deskew of the SCFC. The propagation is done using the ZOH technique as
follows:

$\displaystyle[left=\empheqlbrace]$
$\displaystyle\breve{\mathbf{q}}_{\tau_{m+1}}=\breve{\mathbf{q}}_{\tau_{m}}\circ\mathcal{E}\left(\Delta\tau_{m}\bar{\bm{\omega}}_{\tau_{m}}\right),$
(7a)
$\displaystyle\breve{\mathbf{v}}_{\tau_{m+1}}=\breve{\mathbf{v}}_{\tau_{m}}+\Delta\tau_{m}\left[\mathcal{R}(\breve{\mathbf{q}}_{\tau_{m}})\bar{\mathbf{a}}_{\tau_{m}}-\mathbf{g}\right],$
(7b)
$\displaystyle\breve{\mathbf{p}}_{\tau_{m+1}}=\breve{\mathbf{p}}_{\tau_{m}}+\Delta\tau_{m}\breve{\mathbf{v}}_{\tau_{m}}\dots$
$\displaystyle\qquad\qquad\quad\mkern
12.0mu+\frac{(\Delta\tau_{m})^{2}}{2}\left[\mathcal{R}(\breve{\mathbf{q}}_{\tau_{m}})\bar{\mathbf{a}}_{\tau_{m}}-\mathbf{g}\right],$
(7c) $\displaystyle\breve{\mathbf{q}}_{\tau_{0}}=\hat{\mathbf{q}}_{k-1},\
\breve{\mathbf{v}}_{\tau_{0}}=\hat{\mathbf{v}}_{k-1},\
\breve{\mathbf{p}}_{\tau_{0}}=\breve{\mathbf{p}}_{k-1},$ (7d)
$\displaystyle\Delta\tau_{m}\triangleq\tau_{m+1}-\tau_{m},\
m\in\\{0,1,\dots,N,\dots,N^{\prime}\\},$ (7e) $\displaystyle\tau_{0}=t_{k-1},\
\tau_{N+1}=t_{k},\ \tau_{N^{\prime}+1}=t,$ (7f)

where we reuse the definition of $\bar{\bm{\omega}}_{\tau_{m}}$ and
$\bar{\mathbf{a}}_{\tau_{m}}$ in (6e).

Hence we obtain the relative transform from time $t_{k}$ to $t_{k}^{\prime}$
by
${}^{t_{k}}_{t_{k}^{\prime}}\breve{\mathbf{q}}\triangleq\breve{\mathbf{q}}_{t_{k}}^{-1}\breve{\mathbf{q}}_{t_{k}^{\prime}}$
and
${}^{t_{k}}_{t_{k}^{\prime}}\breve{\mathbf{p}}\triangleq-\mathcal{R}(\breve{\mathbf{q}}_{t_{k}}^{-1})(\breve{\mathbf{q}}_{t_{k}^{\prime}}-\breve{\mathbf{q}}_{t_{k}})$.
This transform is used for the deskew of the SCFC in Sec. V.

######  Remark 2.

It might be useful to note the difference between IMU propagation and IMU
preintegration. On the physical sense, IMU propogation uses IMU measurement to
project the system’s state from time $t_{k}$ to time $t$, while preintegration
is the direct integration of IMU measurements to obtain some pseudo-
observation [20]. In a technical sense, preintegration does not involve
cancelling the gravity from the acceleration measurement before integration
(when comparing (6b), (6c) with (7b), (7c)), nor does it require the initial
state
$(\hat{\mathbf{q}}_{k-1},\hat{\mathbf{v}}_{k-1},\breve{\mathbf{p}}_{k-1})$
(when comparing (6d) with (7d)).

##  VII Local Map and FMM Processes

The local map $\mathcal{M}_{w}$ is used as a prior map to calculate the FMM
coefficients for the joint optimization process. In the beginning, when fewer
than $M$ key frames have been stored to the memory, we directly merge the CFCs
of the first $M-1$ steps obtained in Sec. V to obtain $\mathcal{M}_{w}$. On
the other hand, when enough key frames have been created, we will use the
latest IMU-predicted state estimate $\breve{\mathbf{p}}_{k}$ to search for the
$M$ nearest key frames, and merge the corresponding pointclouds to obtain
$\mathcal{M}_{w}$.

Next, for each CFC ${}^{\texttt{L}}\mathcal{F}_{m}$, $m=w,w+1,\dots,k$, we
calculate the FMM coefficients for each of its features by using Algorithm 1.
In steps 1-1 of this algorithm, for each plane feature ${\bm{\mathbf{f}}}$ we
find a set $\mathcal{N}_{\bm{\mathbf{f}}}$ of neighbouring plane features in
the local map using KNN, then calculate the Hesse normal ${\bm{\mathbf{n}}}$
of a plane, whose sum of squared distances to the points in
$\mathcal{N}_{\bm{\mathbf{f}}}$ is minimal (this optimization problem has
closed-form solution). We then calculate the ”fitness score” $s$ in step 1. If
the fitness score is above a threshold in, we can admit the tuple
$({}^{m}{\bm{\mathbf{f}}}^{i},g\bar{{\bm{\mathbf{n}}}},g)$ to the set of FMM
coefficients of CFC $\mathcal{F}_{m}$, denoted as $\mathcal{C}_{m}$.

For steps 1-1, we follow a similar procedure to calculate the coefficients of
an edge feature. Our strategy here is to construct the two planes that
intersect at the presumed edge line, as such, if ${\bm{\mathbf{f}}}$ belongs
to the edge line, it belongs to both of these planes and vice versa. To find
the edge line, we find the fittest line going through the neighbor set
$\mathcal{N}_{\bm{\mathbf{f}}}$ and its centroid $\bar{\mathbf{p}}$, which
corresponds to steps 1-1. From step 1 to step 1, using some geometrical
manipulations, we compute the Hesse normal vectors of the two planes, one that
goes through this line and ${\bm{\mathbf{f}}}$, and another that also goes
through the edge line but is perpendicular to the first plane. Hence, we
compute the fitness score to decide whether to add the tuples
$({}^{m}{\bm{\mathbf{f}}}^{i},g\bar{{\bm{\mathbf{n}}}}_{1},gc_{1})$ and
$({}^{m}{\bm{\mathbf{f}}}^{i},g\bar{{\bm{\mathbf{n}}}}_{2},gc_{2})$ to the set
$\mathcal{C}_{m}$. Fig. 6 illustrates the vectors and planes computed over
these steps.

It should be noted that the FMM process can be quite computationally
expensive. However, since Algorithm 1 does not modify the local map, the FMM
process can be split into multiple threads for each CFC. Thanks to this
strategy, we can cut down the computation time to about 10m to 20ms compared
to hundreds of ms when using a single thread.

Input: $\mathcal{F}_{m}=(\mathcal{F}_{m}^{p},\ \mathcal{F}_{m}^{e})$,
$\mathcal{M}_{w}=(\mathcal{M}_{w}^{p},\ \mathcal{M}_{w}^{e})$,
$\hat{\mathbf{T}}_{m}$.

Output:
$\mathcal{C}_{m}\triangleq\\{\mathcal{L}^{1}_{m},\mathcal{L}^{2}_{m},\dots,\\}$,
$\mathcal{L}^{i}_{m}\triangleq({}^{m}{\bm{\mathbf{f}}}^{i},{\bm{\mathbf{n}}}^{i},c^{i})$.

1 for  _$\mathrm{each}$ ${\bm{\mathbf{f}}}\in\mathcal{F}_{m}$_ do

2 Compute ${}^{m}{\bm{\mathbf{f}}}^{i}$ from ${\bm{\mathbf{f}}}^{i}$ and
$\hat{\mathbf{T}}_{m}$;

3 if  _ ${\bm{\mathbf{f}}}\in\mathcal{F}_{m}^{p}$ _ then

4 Find $\mathcal{N}_{{\bm{\mathbf{f}}}}=\text{KNN}({\bm{\mathbf{f}}},\
\mathcal{M}_{w}^{p})$;

5 Find
$\bar{{\bm{\mathbf{n}}}}=\operatorname*{argmin}_{{\bm{\mathbf{n}}}}\sum_{{\bm{\mathbf{x}}}\in\mathcal{N}_{\bm{\mathbf{f}}}}||{\bm{\mathbf{n}}}^{\top}{\bm{\mathbf{x}}}+1||^{2}$;

6 Compute
$s=\left[1-0.9\frac{\left\lvert\bar{{\bm{\mathbf{n}}}}^{\top}{\bm{\mathbf{f}}}+1\right\rvert}{\left\lVert\bar{{\bm{\mathbf{n}}}}\right\rVert\left\lVert{\bm{\mathbf{f}}}\right\rVert}\right]$
and $g=\frac{s}{\left\lVert\bar{{\bm{\mathbf{n}}}}\right\rVert}$;

7 If $s>0.1$, add $({}^{m}{\bm{\mathbf{f}}}^{i},g\bar{{\bm{\mathbf{n}}}},g)$
to $\mathcal{C}_{m}$;

8

9 else if  _${\bm{\mathbf{f}}}\in\mathcal{F}_{m}^{e}$_ then

10 Find the set
$\mathcal{N}_{\bm{\mathbf{f}}}=\text{KNN}({\bm{\mathbf{f}}},\mathcal{M}_{w}^{e})$,
and its centroid
$\bar{\mathbf{p}}=\frac{1}{\left\lvert\mathcal{N}_{\bm{\mathbf{f}}}\right\rvert}\sum_{{\bm{\mathbf{x}}}\in\mathcal{N}_{\bm{\mathbf{f}}}}{\bm{\mathbf{x}}}$;

11 Compute:
${\bm{\mathbf{A}}}\triangleq\frac{1}{\left\lvert\mathcal{N}_{\bm{\mathbf{f}}}\right\rvert}\sum_{{\bm{\mathbf{x}}}\in\mathcal{N}_{\bm{\mathbf{f}}}}({\bm{\mathbf{x}}}-\bar{\mathbf{p}})({\bm{\mathbf{x}}}-\bar{\mathbf{p}})^{\top}$;

12 Find the eigenvector $\mathbf{v}_{\max}$ corresponding to the largest
eigenvalue of ${\bm{\mathbf{A}}}$;

13 Compute: ${\bm{\mathbf{x}}}_{0}={\bm{\mathbf{f}}}$,
${\bm{\mathbf{x}}}_{1}=\bar{\mathbf{p}}+0.1\mathbf{v}_{\max}$,
${\bm{\mathbf{x}}}_{2}=\bar{\mathbf{p}}-0.1\mathbf{v}_{\max}$,
${\bm{\mathbf{v}}}_{01}={\bm{\mathbf{x}}}_{0}-{\bm{\mathbf{x}}}_{1}$,
${\bm{\mathbf{v}}}_{02}={\bm{\mathbf{x}}}_{0}-{\bm{\mathbf{x}}}_{2}$,
${\bm{\mathbf{v}}}_{12}={\bm{\mathbf{x}}}_{1}-{\bm{\mathbf{x}}}_{2}$, ;

14 Compute:
${\bm{\mathbf{n}}}_{1}={\bm{\mathbf{v}}}_{12}\times({\bm{\mathbf{v}}}_{10}\times{\bm{\mathbf{v}}}_{02})$,
${\bm{\mathbf{n}}}_{1}\leftarrow{\bm{\mathbf{n}}}_{1}/\left\lVert{\bm{\mathbf{n}}}_{1}\right\rVert$,
${\bm{\mathbf{n}}}_{2}={\bm{\mathbf{v}}}_{12}\times{\bm{\mathbf{n}}}_{1}$;

15 Compute:
${\bm{\mathbf{f}}}_{\bot}={\bm{\mathbf{f}}}-({\bm{\mathbf{n}}}_{1}{\bm{\mathbf{n}}}_{1}^{\top}){\bm{\mathbf{v}}}_{01}$;

16 Compute: $c_{1}=-{\bm{\mathbf{n}}}_{1}^{\top}{\bm{\mathbf{f}}}_{\bot}$ and
$c_{2}=-{\bm{\mathbf{n}}}_{2}^{\top}{\bm{\mathbf{f}}}_{\bot}$;

17 Compute:
$s=\left[1-\frac{0.9\left\lVert{\bm{\mathbf{x}}}_{01}\times{\bm{\mathbf{x}}}_{02}\right\rVert}{\left\lVert{\bm{\mathbf{x}}}_{12}\right\rVert}\right]$,
$g=s/2$;

18 If $s>0.1$, add
$({}^{m}{\bm{\mathbf{f}}}^{i},g\bar{{\bm{\mathbf{n}}}}_{1},gc_{1})$ and
$({}^{m}{\bm{\mathbf{f}}}^{i},g\bar{{\bm{\mathbf{n}}}}_{2},gc_{2})$ to
$\mathcal{C}_{m}$;

19

20

21 end for

Algorithm 1 Calculation of FMM coefficients Figure 6: Illustrations of the
vectors and the two planes computed in step 1 to step 1 of Algorithm 1.

The result of the FMM is a list of coefficients used to create a lidar factor
in (4). Specifically, for each tuple of FMM coefficients
$\mathcal{L}^{i}_{m}=({}^{m}{\bm{\mathbf{f}}}^{i},{\bm{\mathbf{n}}}^{i},c^{i})$,
we can calculate the corresponding residual as follows:

$\bm{r}_{\mathcal{L}}(\hat{\mathcal{X}}_{m},\mathcal{L}_{m}^{i})=({\bm{\mathbf{n}}}^{i})^{\top}\left[\mathcal{R}(\hat{\mathbf{q}}_{m}){}^{m}{\bm{\mathbf{f}}}^{i}+\hat{\mathbf{p}}_{m}\right]+c^{i}.$
(8)

######  Remark 3.

Note that both ${}^{\texttt{L}}\mathcal{F}_{m}$ and $\mathcal{M}_{w}$ in
Algorithm 1 are w.r.t. to the L frame, thus we omit the superscript for more
concise notation. Also, either $\mathcal{M}_{w}$ or $\mathcal{F}_{m}$ or both
can be down-sampled to reduce the computational load. In this paper we chose a
0.4m leaf size for the plane feature pointclouds and 0.2m for the edge feature
pointclouds.

##  VIII Key frame Management

The key frame in this case refers to marginalized pose estimates, so-called
key poses, and their corresponding CFCs, so-called key pointclouds. After each
joint optimization step, we will consider admitting the middle pose estimate,
i.e. $(\hat{\mathbf{q}}_{v},\hat{\mathbf{p}}_{v}),\ v\triangleq k-M/2$ to the
list of key poses. Specifically, given $\hat{\mathbf{p}}_{v}$, we will find
its KNN among the key poses and create the sets
$\mathcal{N}_{p}=\\{\mathbf{p}_{0},\mathbf{p}_{1},\dots,\mathbf{p}_{K}\\}$ and
$\mathcal{N}_{q}=\\{\mathbf{q}_{0},\mathbf{q}_{1},\dots,\mathbf{q}_{K}\\}$
that contain the positions and quaternions of these key poses, respectively.
Hence, we check the following:

  * •

$\left\lVert\hat{\mathbf{p}}_{v}-\mathbf{p}\right\rVert>1.0,\
\forall\mathbf{p}\in\mathcal{N}_{p}$,

  * •

$\left\lVert\mathcal{E}^{-1}(\hat{\mathbf{q}}_{v}^{-1}\mathbf{q})\right\rVert>\pi/18,\
\forall\mathbf{q}\in\mathcal{N}_{q}$,

where $\mathcal{E}^{-1}$ is the inverse of $\mathcal{E}$, which returns the
rotation vector from a quaternion. If either one of the two conditions above
is true, then we will admit this pose estimate to the list of key poses and
save its corresponding CFC $\mathcal{F}_{v}$ to the buffer for later use in
the construction of local map.

##  IX Experiment

In this section we describe our implementation of the MILIOM method and seek
to demonstrate its effectiveness through a series of experiments on UAV
datasets. The software system is developed on the ROS framework, employs the
ceres-solver for the joint optimization process and other libraries such as
PCL, Eigen, etc for their utilities. All experiments were done on a core-i7
computer with 6 cores. Video recording of some experiments can be viewed at
https://youtu.be/dHXYWC2KJyo.

###  IX-A NTU VIRAL Datasets

We employ our recently published NTU VIRAL dataset [21]333https://ntu-
aris.github.io/ntu_viral_dataset/, which to the best of our knowledge is the
first UAV dataset that contains data from multiple lidars (besides multiple
cameras, IMUs, Ultra-wideband (UWB) ranging sensors). The configuration of the
two lidars can be seen in Fig. 7. More specifically, one so-called horizontal
lidar can scan the front, back, left, right, while the vertical lidar can scan
the front, back, above and below sides of the UAV. Both lidars have 16
channels with $32^{o}$ vertical FOV.

Figure 7: Hardware setup of the viral dataset, with one horizontal and one
vertical lidar whose fields of view are complementary.

.

We compare our algorithms with the two latest LIO based methods, which is LIO-
Mapping [16] (henceforth referred to as LIO-M) and LIO-SAM [9], and the
MLOAM444https://github.com/gogojjh/M-LOAM [3] method, which integrates
multiple lidars but does not employ IMU measurements. We note that both LIO-M
and LIO-SAM are designed for single lidar configuration. More specifically,
they assume the pointcloud input to be of regular shape and use the vertical
and angular steps to extract the pointcloud features. In theory, we can modify
their code to replace the internal feature extraction process with our feature
extraction & merging part (Fig. 3) and inject the CFC into their backend.
However this would involve too much modification into the original code
structure. Therefore, we opt to configure our method as well as the others to
work with a single horizontal lidar and report the result in Tab. I. We then
configure our method to work with both lidars and append the result to Tab. I.
The details of these configurations will be provided on the NTU VIRAL
dataset’s website.

TABLE I:  ATE of the lidar-based localization methods over the NTU VIRAL datasets. The best result is highlighted in bold, the second best is underlined. All values are in m. Dataset | 1-Lidar | 2-Lidar
---|---|---
| LIO-M | LIO-SAM | Ours | MLOAM | Ours
eee_01 | 1.0542 | 0.0915 | 0.1042 | 0.3558 | 0.0666
eee_02 | 0.7234 | 0.0815 | 0.0650 | 0.1945 | 0.0656
eee_02 | 1.0314 | 0.1176 | 0.0628 | 0.2996 | 0.0518
nya_01 | 2.2436 | 0.0899 | 0.0832 | 0.1555 | 0.0565
nya_02 | 1.9664 | 0.1068 | 0.0721 | 0.2334 | 0.0668
nya_03 | 2.9934 | 0.3655 | 0.0577 | 0.2859 | 0.0423
sbs_01 | 1.6737 | 0.0966 | 0.0764 | 0.1925 | 0.0658
sbs_02 | 1.8056 | 0.0961 | 0.0806 | 0.1778 | 0.0816
sbs_03 | 2.0006 | 0.0960 | 0.0884 | 0.1863 | 0.0933

All parameters such as the standard deviation of IMU noises, window size,
downsampling resolution, etc., are kept uniform. All methods are required to
run at the full lidar rate, which is 10 Hz. However, for LIO-M, we reduce the
rate of the lidar topic to one third of the actual rate (10 Hz) to allow the
algorithm to run in real-time. Even with this we also have to limit the time
for the optimization process when the buffered data becomes too large. We run
each algorithm on each dataset once and record the IMU-predicted estimates.
The estimated and grountruth trajectories are then synchronized and aligned
with each other. The resulting Absolute Trajectory Error (ATE) of each test
and each method is computed and reported in Tab. I. These tasks are done by
the matlab scripts accompanying the NTU VIRAL data suite [21].

It is clearly shown in Tab. I that our system consistently achieves better
performance compared to existing methods, as MILIOM with a single lidar
already performs better than other methods, including multi-lidar MLOAM
method. When configured to work with both lidars, the result is also
consistently improved compared to the single lidar case. In Fig. 8, we can
observe a maximum error of 10cm by MILIOM method with two lidars. We also
visualize our mapping and key frame management schemes in Fig. 9.

Figure 8: Estimation error by different methods on the eee_02 dataset. Figure
9: Visualization of the trajectory estimate by MILIOM method (blue line),
ground truth (red line), key frame positions (yellow circles), activated key
frames used for constructing the local map (light green), and the global map
built by merging all of the key pointclouds. This experiment is done over the
sbs_03 dataset.

Fig. 10 reports the computation time for the main processes when running the
eee_02 dataset. The frontend processing time, which is mainly consumed by the
FMM process, has a mean of 14.56 ms. While the backend processing time,
dominated by the optimization time of the ceres solver, has a mean of about
62.9902 ms. The mean of the loop time is 99.99 ms, which exactly matches the
lidar rate. The fluctuation in ${\Delta t}_{\text{loop}}$ is reflective of the
soft synchronization scheme when sometimes we have to wait for some extra time
for data from all lidars to arrive, and sometimes we can immediately start on
new frontend processing task when the current backend completes just when all
lidar inputs have arrived at the buffer.

Figure 10:  Computation times of the main MILIOM processes in eee_02 dataset:
${\Delta t}_{\text{loop}}$ is the time period between the beginning of one
joint optimization process to the next, ${\Delta t}_{\text{backend}}$ is the
time for the ceres-solver to actually solve the joint optimization problem
(4), ${\Delta t}_{\text{frontend}}$ is the computation time for all of the
preliminary processes including deskew, transform and FMM processes before the
optimization starts.

###  IX-B Building Inspection Trials

TABLE II:  ATE of the lidar-based localization methods on some object inspection trials. The best result is highlighted in bold, the second best is underlined. All values are in m.  Dataset | 1-Lidar | 2-Lidar
---|---|---
| LIO-M | LIO-SAM | Ours | MLOAM | Ours
test_01 | 3.6611 | - | 2.2460 | 3.9609 | 0.2524
test_02 | 5.0644 | - | 2.0184 | 0.6699 | 0.3378
test_03 | 5.7629 | - | 4.4470 | 3.4921 | 0.1444
Figure 11: Visualization of the result in on the building inspection test_01.

We also conduct several trials to test the ability of the method in actual
inspection scenarios, one of which was shown earlier in Fig. 1. Indeed, these
operations can be more challenging than the NTU VIRAL datasets for several
reasons. First, there are only a building facade and the ground plane where
reliable features can be extracted. Second, the building is much taller than
the structures in the NTU VIRAL datasets, and when flying up to that high,
there might be very few features that can be extracted. Finally, the
trajectory of the UAV is mostly confined to the YZ plane, instead of having
diverse motions as in the NTU VIRAL datasets. Hence there can be some
observability issues with the estimation.

Tab. II summarizes the results of these inspection trials. In general, the
aforementioned challenges do increase the error for all methods. We couldn’t
get LIO-SAM to work in these datasets till the end without divergence. Even
for the MILIOM method with a single lidar, there is also a significant
increase in the error. On the other hand, contrary to its lower performance
over NTU VIRAL datasets, in these challenging building inspection trials,
MLOAM can take advantage of complementary FOVs of both lidar, thus achieving
significantly higher accuracy than single-lidar MILIOM. Nevertheless, the
results of the two-lidar MILIOM approach are still most accurate, which
demonstrates the robustness and accuracy of the multi-input lidar-inertia
approach in critical conditions. Fig. 11 presents the 3D plot of the
trajectory estimates, the global map and groundtruth in one of these datasets.

##  X Conclusion and Future Works

In this paper we have developed a tightly-coupled, multi-threaded, multi-
input, key frame based LIOM framework. We demonstrated our system’s capability
to estimate the position of a UAV and compared the performance with other
state of the art methods. The results show that our method can outperform
existing methods in single lidar case, and the use of multiple lidars can
further improve the accuracy as well as the robustness of the localization
process. We shown that our method can guarantee real time processing
capability.

In the future, we will improve this localization system by combining it with
other types of sensor such as visual features and ranging measurements. In
addition, loop closure and bundle adjustment are also being investigated.

## References

  * [1] J. Jeong, Y. Cho, Y.-S. Shin, H. Roh, and A. Kim, “Complex urban dataset with multi-level sensors from highly diverse urban environments,” _The International Journal of Robotics Research_ , vol. 38, no. 6, pp. 642–657, 2019\.
  * [2] M. Palieri, B. Morrell, A. Thakur, K. Ebadi, J. Nash, A. Chatterjee, C. Kanellakis, L. Carlone, C. Guaragnella, and A.-a. Agha-mohammadi, “Locus: A multi-sensor lidar-centric solution for high-precision odometry and 3d mapping in real-time,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 421–428, 2020.
  * [3] J. Jiao, H. Ye, Y. Zhu, and M. Liu, “Robust odometry and mapping for multi-lidar systems with online extrinsic calibration,” _arXiv preprint arXiv:2010.14294_ , 2020.
  * [4] J. Geyer, Y. Kassahun, M. Mahmudi, X. Ricou, R. Durgesh, A. S. Chung, L. Hauswald, V. H. Pham, M. Mühlegg, S. Dorn _et al._ , “A2d2: Audi autonomous driving dataset,” _arXiv preprint arXiv:2004.06320_ , 2020.
  * [5] S. Agarwal, A. Vora, G. Pandey, W. Williams, H. Kourous, and J. McBride, “Ford multi-av seasonal dataset,” _The International Journal of Robotics Research_ , vol. 39, no. 12, pp. 1367–1376, 2020.
  * [6] P. Sun, H. Kretzschmar, X. Dotiwalla, A. Chouard, V. Patnaik, P. Tsui, J. Guo, Y. Zhou, Y. Chai, B. Caine _et al._ , “Scalability in perception for autonomous driving: Waymo open dataset,” in _Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition_ , 2020, pp. 2446–2454.
  * [7] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in _Robotics: Science and Systems_ , vol. 2, no. 9, 2014.
  * [8] ——, “Laser–visual–inertial odometry and mapping with high robustness and low drift,” _Journal of Field Robotics_ , vol. 35, no. 8, pp. 1242–1264, 2018.
  * [9] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and R. Daniela, “Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2020, pp. 5135–5142.
  * [10] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual–inertial odometry,” _IEEE Transactions on Robotics_ , vol. 33, no. 1, pp. 1–21, 2016.
  * [11] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 1004–1020, 2018.
  * [12] T.-M. Nguyen, M. Cao, S. Yuan, Y. Lyu, T. H. Nguyen, and L. Xie, “Viral-fusion: A visual-inertial-ranging-lidar sensor fusion approach,” _Submitted to IEEE Transactions on Robotics_ , 2021.
  * [13] T. H. Nguyen, T.-M. Nguyen, and L. Xie, “Tightly-coupled single-anchor ultra-wideband-aided monocular visual odometry system,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 665–671.
  * [14] ——, “Range-focused fusion of camera-imu-uwb for accurate and drift-reduced localization,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 1678 – 1685, 2021.
  * [15] P. Geneva, K. Eckenhoff, Y. Yang, and G. Huang, “Lips: Lidar-inertial 3d plane slam,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 123–130.
  * [16] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial odometry and mapping,” in _2019 International Conference on Robotics and Automation (ICRA)_. IEEE, 2019, pp. 3144–3150.
  * [17] T.-M. Nguyen, M. Cao, S. Yuan, Y. Lyu, T. H. Nguyen, and L. Xie, “Liro: Tightly coupled lidar-inertia-ranging odometry,” _2021 IEEE International Conference on Robotics and Automation (ICRA), Accepted_ , 2020.
  * [18] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in _2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_. IEEE, 2018, pp. 4758–4765.
  * [19] E. B. Dam, M. Koch, and M. Lillholm, “Quaternions, interpolation and animation,” University of Copenhagen, Tech. Rep., 07 1998.
  * [20] T. Lupton and S. Sukkarieh, “Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions,” _IEEE Transactions on Robotics_ , vol. 28, no. 1, pp. 61–76, 2011.
  * [21] T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, “Ntu viral: A visual-inertial-ranging-lidar dataset, from an aerial vehicle viewpoint,” _Submitted to IJRR_ , 2021.