#  Gravity-constrained point cloud registration

Vladimír Kubelka, Maxime Vaidis and François Pomerleau1 *This research was
supported by the Natural Sciences and Engineering Research Council of Canada
(NSERC) through the grant CRDPJ 527642-18 SNOW (Self-driving Navigation
Optimized for Winter).1The authors are with Northern Robotics Laboratory,
Université Laval, Québec City, Canada, $\\{$vladimir.kubelka, maxime.vaidis,
francois.pomerleau$\\}$ @norlab.ulaval.ca

###### Abstract

Visual and lidar Simultaneous Localization and Mapping (SLAM) algorithms
benefit from the Inertial Measurement Unit (IMU) modality. The high-rate
inertial data complement the other lower-rate modalities. Moreover, in the
absence of constant acceleration, the gravity vector makes two attitude angles
out of three observable in the global coordinate frame. In visual odometry,
this is already being used to reduce the 6- Degrees Of Freedom (DOF) pose
estimation problem to 4-DOF. In lidar SLAM, the gravity measurements are often
used as a penalty in the back-end global map optimization to prevent map
deformations. In this work, we propose an Iterative Closest Point (ICP)-based
front-end which exploits the observable DOF and provides pose estimates
aligned with the gravity vector. We believe that this front-end has the
potential to support the loop closure identification, thus speeding up
convergences of global map optimizations. The presented approach has been
extensively tested in large-scale outdoor environments as well as in the
Subterranean Challenge organized by Defense Advanced Research Projects Agency
(DARPA). We show that it can reduce the localization drift by 30 % when
compared to the standard 6-DOF ICP. Moreover, the code is readily available to
the community as a part of the libpointmatcher library.

##  I Introduction

Among the SLAM approaches for mobile robots, cameras and Light Detection And
Ranging (lidar) modalities are still the dominant front-end choices [1].
Modern visual SLAM and visual odometry solutions benefit from the
complementary nature of IMU modalities, which provide high-rate linear
acceleration and rotational velocity between captured camera frames [2].
Moreover, the acceleration sensed by IMUs allows the detection of the gravity
vector. This vector can be used to observe two attitude angles out of three in
a global coordinate frame, thus reducing the visual odometry pose estimation
problem from 6-DOF to 4-DOF [3]. Similarly, in the domain of lidar SLAM, using
IMUs is beneficial for precise point cloud alignment [4]. The two observable
attitude angles are often used as penalties in SLAM back-ends during the
global map optimization step. These penalties have the benefit of aligning the
map with the gravity vector [5, 6, 7]. This technique reduces map
deformations, especially in large environments with a limited number of
possible loop closures, such as in underground mines [5]. We will refer to the
use of gravity vector in a SLAM back-end as a loosely-coupled solution.

SLAM front-end solutions relying on lidar typically build an incremental 3D
map of the environment by registering a current scan using a variant of the
original ICP algorithm [8]. The ICP algorithm estimates a rigid transformation
by iteratively finding corresponding points between two point clouds and
minimizing an alignment error. This solution ensures local crisp maps, yet
inevitably suffers from global drift [9]. This phenomenon is shown in Figure
1, where a robot navigated autonomously on a $1.5\text{\,}\mathrm{km}$ path
crossing a forest, while only localizing on subsequent scans. This drift
problem can be mitigated by SLAM back-ends applying pose graph optimization
[10, 1]. Global cues, such as loop closures, are used as constraints to
optimize all estimated transformations between the individual lidar scans to
ensure global consistency. SLAM back-ends rely on pose-graph optimizers, which
require uncertainty estimations in the form of covariance matrices for all
transformations. Unfortunately, when it comes to adding constraints produced
by ICP, it was shown that, although the uncertainty can be learned or sampled,
the Gaussian distribution assumption does not hold up well in complex 3D
environments [11].

Figure 1:  Example of a drifting localization-based solely on a front-end
relying on lidar registration. The top view shows the 3D reconstruction of the
forest path over $1.5\text{\,}\mathrm{km}$. Color is used to highlight the
structure of the environment, with yellow representing the ground. The side
view of the same path shows reconstruction drifting by $55\text{\,}\mathrm{m}$
in elevation with red points and a gravity constrained map in green.

In this paper, we present a tightly-coupled variant of the ICP algorithm,
which exploits the directly observable DOFs in the IMU orientation. This way,
we obtain pose estimates aligned with the gravity vector. The proposed
modification is intended as a lidar SLAM front-end that limits mapping drift.
We believe that this front-end has the potential to support the loop closure
identification, thus speeding up convergences of global map optimizations. Our
solution has been extensively tested in large-scale outdoor environments as
well as in the DARPA Subterranean Challenge
(SubT).111https://www.darpa.mil/program/darpa-subterranean-challenge Moreover,
the code is readily available to the community as a part of the
libpointmatcher library.222https://github.com/ethz-asl/libpointmatcher

The remainder of the paper is structured as follows: Section II provides a
brief overview of the SLAM algorithms that benefit from the IMU modality.
Section III introduces the modified 4-DOF ICP algorithm including the
derivation of the internal optimization step. The following Section IV
presents our experimental setup and Section V discusses the experiment
results. Finally, Section VI summarizes our findings and concludes the paper.

##  II Related work

In this section, we first briefly present the visual-inertial SLAM and
odometry solutions. Then, the lidar-based systems follow. We divide them into
two subgroups according to the manner the IMU modality is exploited. The first
subgroup consists of solutions that pre-integrate inertial data into pose
increments and use them as penalties in state estimation. The second subgroup
is specific by exploiting the gravity force vector that serves as an
additional global attitude measurement. Finally, we put our topic, lidar
registration, in context with these works.

In the field of visual odometry, the works of [12] and [13] presented
filtering methods for motion estimation from monocular camera supported by
inertial measurements. Besides pose estimation, the algorithms allowed online
self-calibration of the camera and IMU coordinate frames. The following
extensive research in the field gradually evolved towards graph-based
optimization methods, which became computationally feasible thanks to
incremental smoothing [14]. To include inertial information into the factor
graphs, [2] improved the IMU preintegration technique, originally proposed by
[15]. The preintegration allows condensing high-rate inertial measurements
into compact inter-frame motion penalties. These, in turn, allow real-time
operation of visual-inertial odometry and SLAM. When the IMU is not subject to
a constantly accelerated motion, the sensed gravitational force makes two of
the six pose DOF observable. As stated in [3], in a visual-inertial odometry
framework, the gravity vector simplifies the problem to a 4-DOF pose
estimation. These DOFs are three positions and one rotation around the
vertical axis in a gravity-aligned global reference frame. In our work, we
leverage these observations and propose a solution to include this very
principle in the lidar-scan-matching process instead of camera-based
frameworks.

In the field of lidar SLAM, the IMU modality has attracted attention as well.
[4] tightly integrated IMU, lidar and Global Positioning System (GPS) in a
factor graph optimized by the iSAM2 [14] algorithm. The GPS and the ability to
identify loop closures lead to globally consistent metric maps. A lightweight
lidar-inertial ego-motion estimator was presented by [16]. They used an
iterated error-state Kalman filter to achieve real-time performance, while
still gaining from the benefits of tight integration of the sensory data.
Similarly, a solution for an Unmanned Aerial Vehicle (UAV) that integrated
lidar, IMU and ultra-wide-band ranging modalities was proposed by [17]. Each
modality provided penalties for a sliding-window optimization carried out by
the Ceres solver.333http://ceres-solver.org We see that these works treat the
pre-integrated IMU pose increments as penalties in the state estimation
process. Closer to our application, [18] introduced their lidar odometry
system used in the DARPA SubT Challenge. The IMU modality is integrated more
loosely, but it is important for removing motion distortion from lidar scans.
They also stressed the necessity of robust operation in harsh conditions and
their design dynamically handles sensor modality failures.

Representing the second subgroup, the following works explicitly use the
gravity force measurement for stabilizing the vertical orientation of their
world coordinate frame and thus their map representation. [5] proposed a lidar
mapping solution for underground mines. Apart from benefiting from the
inertial measurements as a prior for matching point clouds, they explicitly
used gravity vector constraints to ensure the vertical orientation. The
gravity vector is one of several linear constraints in their optimization
process that smooths prior trajectory. In a tightly integrated lidar-inertial
SLAM system for autonomous vehicles, [6] used rotation constraints in the
global optimization step. The team COSTAR from the SubT competition added
explicit gravity factors in their graph representation used in global
optimization [7]. In our work, we benefit from the two observable orientation
DOFs that also rise from the gravity measurement.

Our solution focuses on the front-end of SLAM systems relying principally on
lidar (i.e., on the point-cloud registration). The ICP algorithm iteratively
finds corresponding points between two point clouds and looks for a rigid
transformation minimizing the alignment error. ICP ensures local consistency,
yet inevitably suffers from global drift [9]. This drift can be mitigated by
SLAM techniques of global map optimization based on spatial (e.g., loop
closures) and measurement constraints (e.g., GPS measurements) [1]. We propose
to modify directly the ICP minimization process to exploit observable DOFs
from the gravity vector. This leads to searching for pose estimates in a 4-DOF
space while limiting the vertical and axial drifts of the local pose
estimates.

##  III Theory

This section first provides a high-level overview of the mapping framework
that we use to evaluate the proposed ICP modification. Then, it derives the
4-DOF minimization equations for ICP itself.

###  III-A ICP Mapper

The mapping and subsequent localization are achieved using the freely
available norlab_icp_mapper.444https://github.com/norlab-
ulaval/norlab_icp_mapper The mapper is lightweight and essentially wraps the
ICP functionality while performing input lidar data filtering and memory
management for the point cloud map. No global optimization is performed in our
setting, the point cloud map grows incrementally as the robot navigates
through the environment. The mapper performs the following steps:

  1. 1.

Transform the input lidar scan into the world frame according to the initial
pose estimate $\widecheck{\bm{T}}$

  2. 2.

Register the scan with the map using the ICP

  3. 3.

Insert the scan inside the map

The initial estimate $\widecheck{\bm{T}}$ is composed of a translation vector
based on the robot odometry and of an orientation quaternion based on the IMU.
The input lidar scans are randomly sub-sampled to maintain real-time
performance on mobile robots. The input scans are also corrected for the
motion distortion according to the odometry and IMU data. Finally, only a
subset of the aligned scan points is merged into the map to keep its density
bounded.

###  III-B 4-DOF ICP

The ICP algorithm aims at estimating a rigid transformation $\widehat{\bm{T}}$
that best aligns a set of 3D points $\mathcal{Q}$ (i.e., a _map_ point cloud)
with a second set of 3D points $\mathcal{P}$ (i.e., _scan_ point cloud), by
minimizing the error function $e$:

$\widehat{\bm{T}}=\arg\min_{\bm{T}}\>e\left(\mathcal{Q},\mathcal{P}\right).$
((1))

In our approach, we choose the _point-to-plane_ definition of $e$ [19] since
it outperforms the _point-to-point_ definition in most cases [9]. However, the
4-DOF modification can be implicitly extended to the _point-to-Gaussian_ error
definition since the latter can be translated to the _point-to-plane_ as shown
by [20]. We first define our transformation parameter $\bm{T}$ as a 4D vector
$\bm{\tau}$:

$\bm{\tau}=\left[\begin{array}[]{c}\gamma\\\
\bm{t}\end{array}\right]=\left[\begin{array}[]{c}\gamma\\\ t_{x}\\\ t_{y}\\\
t_{z}\end{array}\right],$ ((2))

where $\gamma$ is the rotational component along the z axis, and $t_{x}$,
$t_{y}$ and $t_{z}$ are the translation components. Compared to the classical
6-DOF definition of $e$, please note that $\bm{\tau}$ misses the rotational
components along the x and y axes. We then follow with the standard definition
of the point-to-plane error function

$e=\sum\limits_{k=1}^{K}\left\|\left[(\bm{R}\bm{p}_{k}+\bm{t})-\bm{q}_{k}\right]\cdot\bm{n}_{k}\right\|_{2},$
((3))

where $\bm{n}_{k}$ is the normal vector representing the surface at the point
$\bm{q}_{k}$, $\bm{R}=R(\gamma)$ is a rotation matrix and the index $k$
represents paired points from $\mathcal{Q}$ and $\mathcal{P}$. Closely
following the minimization derivation from [21], yet modified for 4-DOF, the
rotation matrix $\bm{R}$ performs only the yaw rotation. The other two
rotations are not necessary since the $\mathcal{P}$ point cloud had already
been pre-aligned by the mapper according to the initial $\widecheck{\bm{T}}$
pose estimate. The minimization method relies on rotation matrix
linearization. This linearization can be achieved using the small-angle
approximation:

$\bm{R}=R(\gamma)\approx\underbrace{\left[\begin{array}[]{ccc}0&-1&0\\\
1&0&0\\\ 0&0&0\end{array}\right]}_{\bm{\Gamma}}\gamma+\bm{I},$ ((4))

where $\bm{I}$ is a unit matrix. In the context of ICP, the impact of
linearization is reduced through the iterative process of the whole
registration algorithm. Combining (3) with (4), we can approximate the
objective function as

$\displaystyle e$
$\displaystyle\approx\sum\limits_{k=1}^{K}\left\|[(\bm{\Gamma}\gamma+\bm{I})\bm{p}_{k}+\bm{t}-\bm{q}_{k}]\cdot\bm{n}_{k}\right\|_{2}$
$\displaystyle\approx\sum\limits_{k=1}^{K}\left\|\gamma(\bm{\Gamma}\bm{p}_{k})\cdot\bm{n}_{k}+\bm{p}_{k}\cdot\bm{n}_{k}+\bm{t}\cdot\bm{n}_{k}-\bm{q}_{k}\cdot\bm{n}_{k}\right\|_{2},$

which can be rewritten using the _scalar triple product_ and by reorganizing
the terms, such that

$\displaystyle e$
$\displaystyle\approx\sum\limits_{k=1}^{K}\left\|\gamma\underbrace{(\bm{\Gamma}\bm{p}_{k})\cdot\bm{n}_{k}}_{{\text{{\c{c}}}_{k}}}+\bm{t}\cdot\bm{n}_{k}-\underbrace{(\bm{q}_{k}-\bm{p}_{k})}_{\bm{d}_{k}}\cdot\bm{n}_{k}\right\|_{2}$
$\displaystyle\approx\sum\limits_{k=1}^{K}\left\|\gamma\,\text{{\c{c}}}_{k}+\bm{t}\cdot\bm{n}_{k}-\bm{d}_{k}\cdot\bm{n}_{k}\right\|_{2}.$

We can then minimize the error $e$ with respect to $\bm{r}$ and $\bm{t}$ and
setting the partial derivatives to zero

$\displaystyle\frac{\partial e}{\partial\gamma}$
$\displaystyle=\sum\limits_{k=1}^{K}2\text{{\c{c}}}_{k}(\gamma\,\text{{\c{c}}}_{k}+\bm{t}\cdot\bm{n}_{k}-\bm{d}_{k}\cdot\bm{n}_{k})=\bm{0}$
$\displaystyle\frac{\partial e}{\partial\bm{t}}$
$\displaystyle=\sum\limits_{k=1}^{K}2\bm{n}_{k}(\gamma\,\text{{\c{c}}}_{k}+\bm{t}\cdot\bm{n}_{k}-\bm{d}_{k}\cdot\bm{n}_{k})=\bm{0}.$

We can assemble these derivatives under the linear form
$\bm{A}\bm{\tau}=\bm{b}$, by bringing the independent variables on the right
side of the equation

$\displaystyle\sum\limits_{k=1}^{K}\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}(\gamma\text{{\c{c}}}_{k})+\text{{\c{c}}}_{k}(\bm{t}\cdot\bm{n}_{k})\\\
\bm{n}_{k}(\gamma\text{{\c{c}}}_{k})+\bm{n}_{k}(\bm{t}\cdot\bm{n}_{k})\end{array}\right]$
$\displaystyle=\sum\limits_{k=1}^{K}\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}(\bm{d}_{k}\cdot\bm{n}_{k})\\\
\bm{n}_{k}(\bm{d}_{k}\cdot\bm{n}_{k})\end{array}\right]$
$\displaystyle\sum\limits_{k=1}^{K}\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}^{2}\gamma+\text{{\c{c}}}_{k}\bm{n}_{k}^{\top}\bm{t}\\\
\bm{n}_{k}\text{{\c{c}}}_{k}\gamma+\bm{n}_{k}\bm{n}_{k}^{\top}\bm{t}\end{array}\right]$
$\displaystyle=\sum\limits_{k=1}^{K}\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}(\bm{d}_{k}\cdot\bm{n}_{k})\\\
\bm{n}_{k}(\bm{d}_{k}\cdot\bm{n}_{k})\end{array}\right]$
$\displaystyle\sum\limits_{k=1}^{K}\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}^{2}&\text{{\c{c}}}_{k}\bm{n}_{k}^{\top}\\\
\text{{\c{c}}}_{k}\bm{n}_{k}&\bm{n}_{k}\bm{n}_{k}^{\top}\end{array}\right]\left[\begin{array}[]{cc}\gamma\\\
\bm{t}\end{array}\right]$
$\displaystyle=\sum\limits_{k=1}^{K}\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}\\\
\bm{n}_{k}\end{array}\right](\bm{d}_{k}\cdot\bm{n}_{k}),$

which brings us to the linear system of equations

$\displaystyle\underbrace{\sum\limits_{k=1}^{K}\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}\\\
\bm{n}_{k}\end{array}\right]\left[\begin{array}[]{cc}\text{{\c{c}}}_{k}&\bm{n}_{k}^{\top}\\\
\end{array}\right]}_{\bm{A}_{4\times
4}}\bm{\tau}=\underbrace{\sum\limits_{k=1}^{K}\left[\begin{array}[]{c}\text{{\c{c}}}_{k}\\\
\bm{n}_{k}\\\ \end{array}\right](\bm{d}_{k}\cdot\bm{n}_{k})}_{\bm{b}_{4\times
1}}.$ ((10))

Once the matrix $\bm{A}$ and the vector $\bm{b}$ can be constructed, the
linear system of Equation (10) can be resolved for $\bm{\tau}$ using the
Cholesky decomposition. Implementing such solution will require a loop for the
summations over $K$ to build $\bm{A}$ and $\bm{b}$. An alternative formulation
relying on dense matrix multiplication can be computed by assembling

$\bm{G}=\underbrace{\left[\cdots\begin{array}[]{c}(\bm{\Gamma}\bm{p}_{k})\cdot\bm{n}_{k}\\\
\bm{n}_{k}\end{array}\cdots\right]}_{4\times
K},\;\;\;\bm{h}=\underbrace{\left[\begin{array}[]{c}\vdots\\\
(\bm{q}_{k}-\bm{p}_{k})\cdot\bm{n}_{k}\\\ \vdots\end{array}\right]}_{K\times
1},$

leading to

$\bm{A}\bm{\tau}=\bm{b}\;\;\;\Leftrightarrow\;\;\;\bm{G}\bm{G}^{\top}\bm{\tau}=\bm{G}\bm{h},$
((11))

which is the final form we use in our implementation. The ICP algorithm
iteratively adjusts $\bm{\tau}$ according to (11) until the convergence of
$e$, which indicates the best achievable alignment. Since our definition of
$\bm{\tau}$ does not involve the rotations in the x-y plane, the resulting
alignment and consequently the estimated pose follows the initial IMU values
(i.e., the roll and pitch angles remain the same.)

##  IV Experiments

Firstly, the 4-DOF point-to-plane minimizer is readily available in the
libpointmatcher library under the force4DOF option and can be tested with the
norlab_icp_mapper.555https://github.com/norlab-ulaval/norlab_icp_mapper

In this section, we present two sets of experiments that demonstrate the
performance of the 4-DOF ICP variant and the following Section V then
discusses the achieved results. The first set of experiments consists of two
runs recorded during the post-event testing session at the DARPA SubT finals
(see Figure 3). The second set of experiments covers a
$1.5\text{\,}\mathrm{k}\mathrm{m}$ long forest trail in the Montmorency
research forest.666https://www.foretmontmorency.ca/en/ A large tracked robot
repeated this path five times in both directions during several days in March
2021.

###  IV-A DARPA SubT underground environment

Our laboratory, as a member of the CTU-CRAS-NORLAB team, deployed the 4-DOF
ICP variant in the Urban circuit and during the competition finals. Its
necessity became apparent after the first Mine circuit, where the mapping and
localization drifted in a long entrance tunnel. This drift would jeopardize
the localization of the scored artifacts in the competition. In this
publication, we present two experiments recorded after the competition finals
in Kentucky, during the post-event testing. The robot shown in Figure 2 was
driven through the underground environment in two separate runs, once through
the Urban section (green trajectory in Figure 3) and then through the Mine and
Cave sections (purple trajectory). We recorded all sensory data and we are
thus able to compare the ICP variants offline. Nevertheless, the mapper
performs the same way when launched directly on the robot in real-time.
Furthermore, the competition organizers provide a precise, point-cloud map of
the environment. It allows us to compute ground-truth reference poses with
uncertainty comparable to the lidar sensor noise. For this purpose, we
initialize the mapper with the reference map and disable the mapping
functionality, thus letting the mapper localize within the reference map.

Figure 2:  The tracked robot driving through the Mine underground environment.
It is equipped with an Ouster OS0 128-beam lidar and an Xsens MTI-30 IMU.
Figure 3:  The underground environment of the final SubT round. The green and
violet trajectories denote the _Urban_ and _Mine/Cave_ experiments recorded
during the post-event testing.

###  IV-B Montmorency forest large-scale outdoor environment

During the winter season of 2021, we deployed a large tracked Clearpath
Warthog robot (Figure 4) in the research forest. The purpose of this
expedition was the development and testing of a Teach&Repeat functionality for
autonomous outdoor navigation (the detailed report [22] is in a review process
at the moment). From the data recorded there, we choose a
$1.5\text{\,}\mathrm{km}$ forest trail to demonstrate the impact of our
approach to mapping and localization in the large-scale outdoor environment of
a sub-arctic forest. The robot passed the trail shown in Figure 5 five times
in separate experiments during three days. We tested both directions and the
weather conditions varied between sub-zero snowy weather and above-zero rain,
therefore altering the character of the snow cover. Thanks to accurate
reference positioning by RTK-GPS receivers on the robot, we can provide the
ground truth positioning and thus evaluate the localization error.

Figure 4: The Clearpath Warthog tracked robot. The navigation is based on the
front 32-beam RS-LiDAR-32, Xsens MTI-100 IMU and wheel odometry. The robot is
also equipped with two Emlid Reach RS+ RTK-GPS receivers used for reference
positioning. The other installed sensors were not used for the experiment.
Figure 5: The $1.5\,$km forest trail in the Montmorency forest autonomously
navigated in both directions. The map was adapted from ©Google Earth.

##  V Results

We evaluate the effect of the 4-DOF ICP on localization accuracy by comparing
the mapper output with the available ground truth. The metric we choose is the
Absolute Trajectory Error (ATE) following [23]. We show its z component and
also its modulus.

###  V-A DARPA SubT underground environment

Our general experience from the underground environment is that it is well
suited for the point-to-plane ICP error function. In the case of the Mine and
Cave experiment, the maximum ATE reaches only three to four meters on a
trajectory $950\text{\,}\mathrm{m}$ long, depending on the 4-DOF or classical
6-DOF ICP choice (Figure 6). The shorter Urban experiment performs similarly
well, less than one meter in $450\text{\,}\mathrm{m}$ (Figure 7). We attribute
this to the modern 360-degree lidars and plentiful surfaces in this particular
underground environment. Also, the course did not contain long uniform
tunnels, which are challenging for this type of SLAM. Nevertheless, we observe
a trend when switching between 6-DOF and 4-DOF localization: the drift in the
z coordinate is suppressed in both experiments, see Figure 6 and Figure 7.
Both approaches drift on the x-y plane, but this is expected since the IMU
does not add any constraints in this plane.

Figure 6: Comparison of the 4-DOF and 6-DOF ATE during the Mine and Cave
experiment in the underground environment. The 4-DOF suppresses the vertical
drift (top) which improves the overall ATE (bottom).

In the Mine and Cave experiment, the ATE of the 4-DOF variant (see Figure 6)
momentarily overshoots the 6-DOF error at around $200\text{\,}\mathrm{m}$
distance due to drift in the x-y plane, but drift in z eventually dominates
when the robot reaches the edge of the competition course. In the Urban
experiment, the z drift is the dominant contributor to ATE along the whole
trajectory and thus the 4-DOF variant helps by suppressing it below
$25\text{\,}\mathrm{cm}$ in the worst case. In both experiments, the ATE drops
back to zero as the robot returns to the starting point.

Figure 7: ATE in the Urban part of the underground environment. The vertical
drift was the dominant contributor to ATE in this experiment.

###  V-B Montmorency forest large-scale outdoor environment

Figure 1 demonstrates the difference between the 4-DOF represented with a
green map and the 6-DOF represented in red. Towards the end of the
$1.5\text{\,}\mathrm{km}$ trail, the elevation error can reach
$55\text{\,}\mathrm{m}$. Unconstrained, the map drifts mainly in the narrow
forest corridors, possibly due to the fuzzy vegetation geometry. Incorporating
the gravity information in the case of the 4-DOF ICP suppresses this effect.
Figure 8 assembles all five runs in one plot, each run with its unique line
style. Although the error differs between each run and two runs went in the
opposite direction, the trend is clear. Both 4-DOF and 6-DOF drift in the x-y
plane, but the 4-DOF always limits the z drift. This translates to the total
ATE shown in Figure 8 with the slower 4-DOF localization error growth compared
to the counterparts in 6-DOF.

Figure 8: ATE of the Montmorency forest experiments. Five runs shown together,
each with its unique line style. The effect of 4-DOF is evident in the Z axis
error.

###  V-C Normalized ATE analysis

To examine what is the expected localization drift with 4-DOF and the standard
6-DOF ICP variants, we have assembled ATE from all poses and experiments and
normalized it by the distance traveled. Figure 9 shows the resulting
distributions. 4-DOF limits the median value from $1.48\text{\,}\%$ to
$1.05\text{\,}\%$ and also the upper half of the distribution gets compressed
with the new third quartile $2.29\text{\,}\%$ instead of $4.19\text{\,}\%$.
This result shows that we cannot eliminate the drift, but we can significantly
reduce it by exploiting the IMU modality, which is usually available on
robotic platforms anyway.

Figure 9: Normalized ATE of the 4-DOF and 6-DOF variants over all poses and
experiments. The first quartile, median and third quartile values are
$(0.26|1.05|2.29)$% and $(0.3|1.48|4.19)$%, respectively. The black whiskers
extend to datapoints in the 1.5x inter-quartile range.

###  V-D Practical considerations

The presented benefits of the 4-DOF modification are conditioned by correctly
estimated roll and pitch angles, and by precise IMU-lidar coordinate frame
calibration. In our experiments, we use the Xsens MTI-30 and MTI-100 IMUs
which provide sub-degree orientation accuracy in the roll and pitch angles.
More importantly, imprecise alignment between the IMU and lidar frames can
lead to worse drift than with the classical 6-DOF localization. Our practical
approach to the calibration is to use a sufficiently large, flat floor and to
adjust the transformation between the sensor frames until the captured point
cloud appears flat when transformed to the inertial coordinate frame. The
calibration can be further fine-tuned by performing a mapping run in a
sufficiently large loop (i.e., more than $500\text{\,}\mathrm{m}$) and
observing the resulting elevation mismatch. The tangent between the loop
length and the error elevation is the final correction of the lidar coordinate
frame pitch angle in the inertial frame. This correction can be as low as
$0.1\text{\,}\mathrm{\SIUnitSymbolDegree}$ and is sufficient to achieve the
results presented in this paper.

##  VI Conclusion

In this paper, we have proposed a tightly-coupled variant of the ICP
algorithm, which exploits directly observable DOFs in the IMU orientation. It
reduces the median normalized ATE by $30\text{\,}\%$ compared to the standard
6-DOF ICP by limiting the vertical drift. The method requires accurate IMU
orientation estimates and IMU-lidar calibration. This condition is however
achievable with standard sensors and tools. Moreover, the code is readily
available to the community as a part of the libpointmatcher library. We
believe it can be useful as a module in complex graph-based SLAM systems as
well as a lightweight lidar odometry with reduced vertical drift.

The 4-DOF ICP has been derived for the point-to-plane error function and
implicitly supports the point-to-gaussian error function. Yet, the family of
ICP variants is vast and it is for future work to extend this idea to more of
them. Moreover, it would be practical to replace the manual IMU-lidar
calibration be an automatic one based on additional external measurements,
such as GPS or global map optimization.

## Acknowledgment

The trajectory analysis in this paper was done using _Python package for the
evaluation of odometry and SLAM_ available at
https://github.com/MichaelGrupp/evo.

## References

  * [1] Cesar Cadena, Luca Carlone, Henry Carrillo, Yasir Latif, Davide Scaramuzza, Jose Neira, Ian Reid and John J. Leonard  “Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age”  In _IEEE Transactions on Robotics_ 32, 2016, pp. 1309–1332  DOI: 10.1109/TRO.2016.2624754
  * [2] Christian Forster, Luca Carlone, Frank Dellaert and Davide Scaramuzza  “On-Manifold Preintegration for Real-Time Visual-Inertial Odometry”  In _IEEE Transactions on Robotics_ 33, 2017  DOI: 10.1109/TRO.2016.2597321
  * [3] Zichao Zhang, Guillermo Gallego and Davide Scaramuzza  “On the Comparison of Gauge Freedom Handling in Optimization-Based Visual-Inertial State Estimation”  In _IEEE Robotics and Automation Letters_ 3, 2018  DOI: 10.1109/LRA.2018.2833152
  * [4] Tixiao Shan, Brendan Englot, Drew Meyers, Wei Wang, Carlo Ratti and Daniela Rus  “LIO-SAM: Tightly-coupled lidar inertial odometry via smoothing and mapping”  In _IEEE International Conference on Intelligent Robots and Systems_ , 2020  DOI: 10.1109/IROS45743.2020.9341176
  * [5] Robert Zlot and Michael Bosse  “Efficient Large-scale Three-dimensional Mobile Mapping for Underground Mines”  In _Journal of Field Robotics_ 31, 2014, pp. 758–779  DOI: 10.1002/rob.21504
  * [6] Haoyang Ye, Yuying Chen and Ming Liu  “Tightly coupled 3D Lidar inertial odometry and mapping”  In _IEEE International Conference on Robotics and Automation_ 2019-May, 2019  DOI: 10.1109/ICRA.2019.8793511
  * [7] Ali Agha et al.  “NeBula: Quest for Robotic Autonomy in Challenging Environments; TEAM CoSTAR at the DARPA Subterranean Challenge”, arXiv preprint arXiv:2103.11470, 2021  arXiv:2103.11470 [cs.RO]
  * [8] P.. Besl and N.. McKay  “A method for registration of 3-D shapes”  In _TPAMI_ 14.2, 1992, pp. 239–256
  * [9] F. Pomerleau, F. Colas, R. Siegwart and S. Magnenat  “Comparing ICP variants on real-world data sets: Open-source library and experimental protocol”  In _Autonomous Robots_ 34.3, 2013, pp. 133–148
  * [10] S. Thrun and M. Montemerlo  “The GraphSLAM Algorithm with Applications to Large-Scale Mapping of Urban Structures”  In _IJRR_ 25.5-6, 2006, pp. 403–429
  * [11] David Landry, Francois Pomerleau and Philippe Giguère  “CELLO-3D: Estimating the Covariance of ICP in the Real World”  In _2019 International Conference on Robotics and Automation (ICRA)_ , 2019, pp. 8190–8196  DOI: 10.1109/ICRA.2019.8793516
  * [12] Jonathan Kelly and Gaurav S. Sukhatme  “Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor Self-calibration”  In _International Journal of Robotics Research_ 30, 2011  DOI: 10.1177/0278364910382802
  * [13] Eagle S. Jones and Stefano Soatto  “Visual-inertial navigation, mapping and localization: A scalable real-time causal approach”  In _International Journal of Robotics Research_ 30, 2011  DOI: 10.1177/0278364910388963
  * [14] Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard and Frank Dellaert  “iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering”  In _2011 IEEE International Conference on Robotics and Automation_ IEEE, 2011, pp. 3281–3288  DOI: 10.1109/ICRA.2011.5979641
  * [15] Todd Lupton and Salah Sukkarieh  “Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions”  In _IEEE Transactions on Robotics_ 28, 2012  DOI: 10.1109/TRO.2011.2170332
  * [16] Chao Qin, Haoyang Ye, Christian E. Pranata, Jun Han, Shuyang Zhang and Ming Liu  “LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation”  In _IEEE International Conference on Robotics and Automation_ , 2020  DOI: 10.1109/ICRA40945.2020.9197567
  * [17] Thien-Minh Nguyen, Muqing Cao, Shenghai Yuan, Yang Lyu, Thien Hoang Nguyen and Lihua Xie  “LIRO: Tightly Coupled Lidar-Inertia-Ranging Odometry”  In _2021 IEEE International Conference on Robotics and Automation (ICRA)_ , 2021, pp. 14484–14490  DOI: 10.1109/ICRA48506.2021.9560954
  * [18] Matteo Palieri et al.  “LOCUS: A Multi-Sensor Lidar-Centric Solution for High-Precision Odometry and 3D Mapping in Real-Time”  In _IEEE Robotics and Automation Letters_ 6, 2021  DOI: 10.1109/LRA.2020.3044864
  * [19] Yang Chen and Gérard Medioni  “Object modelling by registration of multiple range images” Range Image Understanding  In _Image and Vision Computing_ 10.3, 1992, pp. 145–155  DOI: https://doi.org/10.1016/0262-8856(92)90066-C
  * [20] P. Babin, P. Dandurand, V. Kubelka, P. Giguère and F. Pomerleau  “Large-scale 3D Mapping of Subarctic Forests”  In _Proceedings of the Conference on Field and Service Robotics (FSR). Springer Tracts in Advanced Robotics_ , 2019
  * [21] F. Pomerleau, F. Colas and R. Siegwart  “A Review of Point Cloud Registration Algorithms for Mobile Robotics”  In _Foundations and Trends in Robotics_ 4.1 Now Publishers, 2015, pp. 1–104
  * [22] D. Baril, S. Deschênes, O. Gamache, M. Vaidis, D. LaRocque, J. Laconte, V. Kubelka, P. Giguère and F. Pomerleau  “Kilometer-scale autonomous navigation in subarctic forests: challenges and lessons learned”, arXiv preprint arXiv:2111.13981, 2021
  * [23] Jrgen Sturm, Nikolas Engelhard, Felix Endres, Wolfram Burgard and Daniel Cremers  “A benchmark for the evaluation of RGB-D SLAM systems”  In _IEEE International Conference on Intelligent Robots and Systems_ , 2012  DOI: 10.1109/IROS.2012.6385773

  *[SLAM]: Simultaneous Localization and Mapping
  *[IMU]: Inertial Measurement Unit
  *[DOF]: Degrees Of Freedom
  *[ICP]: Iterative Closest Point
  *[DARPA]: Defense Advanced Research Projects Agency
  *[lidar]: Light Detection And Ranging
  *[IMUs]: Inertial Measurement Unit
  *[DOFs]: Degrees Of Freedom
  *[GPS]: Global Positioning System
  *[UAV]: Unmanned Aerial Vehicle