See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/350535662

Towards High-Performance Solid-State-LiDAR-Inertial Odometry and Mapping

Article  in  IEEE Robotics and Automation Letters · March 2021

DOI: 10.1109/LRA.2021.3070251

CITATIONS READS
112 352

3 authors, including:

Kailai Li Uwe D. Hanebeck

Linköping University Karlsruhe Institute of Technology
44 PUBLICATIONS   431 CITATIONS    614 PUBLICATIONS   8,233 CITATIONS   

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

CoCPN: Cooperative Cyber Physical Networking View project

Ubiquitous Spatio-Temporal Learning for Future Mobility (ULearn4Mobility) View project

All content following this page was uploaded by Kailai Li on 27 April 2021.

The user has requested enhancement of the downloaded file.

 1

Towards High-Performance
Solid-State-LiDAR-Inertial Odometry and Mapping
Kailai Li, Meng Li, and Uwe D. Hanebeck

Abstract—We present a novel tightly-coupled LiDAR-inertial

odometry and mapping scheme for both solid-state and mechan-
ical LiDARs. As frontend, a feature-based lightweight LiDAR
odometry provides fast motion estimates for adaptive keyframe
selection. As backend, a hierarchical keyframe-based sliding
window optimization is performed through marginalization for
directly fusing IMU and LiDAR measurements. For the Livox
Horizon, a newly released solid-state LiDAR, a novel feature
extraction method is proposed to handle its irregular scan pattern
during preprocessing. LiLi-OM (Livox LiDAR-inertial odometry
and mapping) is real-time capable and achieves superior accuracy
over state-of-the-art systems for both LiDAR types on public data
sets of mechanical LiDARs and in experiments using the Livox
Horizon. Source code and recorded experimental data sets are
available at https://github.com/KIT-ISAS/lili-om. Fig. 1: 3D map of Schloss Karlsruhe from LiLi-OM.
Index Terms—Sensor fusion, localization, mapping
Typical LiDAR scan rates are relatively low and the per-
ceived point clouds are in principle distorted due to sensor
I. I NTRODUCTION egomotion. Thus, performing LiDAR-only odometry is prone
to deterioration under fast motion or in complex scenes. Inertial
E STIMATING six-DoF egomotion plays a fundamental role
in a wealth of applications ranging from robot navigation
and inspection to virtual/augmented reality, see [1]–[4]. With
sensors, however, measure instant motion at a much higher
frequency and can bridge the gap between consecutive LiDAR
the booming of autonomous driving, light detection and ranging frames, improving the robustness and accuracy of LiDAR-based
(LiDAR) sensors have gained tremendous popularity [5], [6]. egomotion estimation. In [10], [11], an inertial measurement
Compared to visual sensors, 3D LiDARs provide lighting- unit (IMU) was used to de-skew point clouds and initialize
invariant and accurate perception of the surroundings with long motion estimates for scan-matching-based LiDAR odometry.
detection range and high robustness. Thus, they are broadly Though decoupled or loosely-coupled LiDAR-inertial fusion
deployed to mobile agents for odometry and mapping. schemes are appealing for runtime and deploying classic
Essentially, LiDAR-based odometry requires computing six- recursive filters (e.g., the EKF), they may cause information
DoF egomotion given consecutive frames of point clouds. loss and inaccurate estimates [13].
This can be performed via scan-matching algorithms, e.g., Thus, there has been a growing focus on tightly-coupled
the iterative closest point (ICP) [7] method. In typical mobile LiDAR-inertial odometry, where point cloud and IMU measure-
perception scenarios, 3D LiDARs output a large streaming ments are fused in a joint optimization or filtering framework.
volume of raw scans in the form of unorganized point clouds. Pre-integrated IMU readings are often employed for de-skewing
Meanwhile, real-time processing (e.g., at 10 Hz) is required the LiDAR scan per frame [14]. In [15], an optimization-based
given limited computing resources. To improve the performance approach was proposed for LiDAR-inertial odometry using a
of LiDAR odometry, much attention has been dedicated to scan- maximum a posteriori (MAP) formulation incorporating both
matching with points representing local geometric features [8], LiDAR and IMU residuals in a sliding window fashion. An
[9]. In [10], feature points are extracted from object edges additional method using rotational constraints was proposed to
and planes for scan-matching using point-to-edge and point-to- refine the final pose and map, which delivers similar or better
plane metrics, which enable accurate LiDAR odometry in real tracking accuracy as [10]. However, real-time processing is
time. Based thereon, in [11], an image-based approach [12] hard to achieve in practice as sensor readings of every frame
was further applied to preprocessing. A two-stage optimization are exploited.
scheme was tailored for ground vehicles to enable light-weight, To improve the runtime efficiency of LiDAR-inertial odome-
robust LiDAR odometry and mapping. try, an iterated error-state Kalman filter was introduced in [16]
This work was supported by German Research Foundation (DFG) under based on a robocentric formulation. It runs in real time and
grant HA 3789/16-1. shows superior tracking accuracy over existing LiDAR-only
The authors are with the Intelligent Sensor-Actuator-Systems Labora- odometry systems. In [17], a tightly-coupled LiDAR-inertial
tory (ISAS), Institute for Anthropomatics and Robotics, Karlsruhe Insti-
tute of Technology (KIT), Germany. E-mails: [email protected], odometry system was proposed based on the incremental
[email protected], [email protected] smoothing and mapping framework iSAM2 [18]. However,
2

the system relies heavily on [10] to produce LiDAR odometry

scan registration build local map
factors for further constraining the pre-integrated IMU states in backend fusion
the factor graph formulation. Unlike direct fusion of IMU and
LiDAR measurements within a unified scheme, this can result optimize global
feature extraction build local map pose graph
in loss of constraint information posed by landmarks. Also, scan-matching

correlations between LiDAR and IMU measurements might be deskew point cloud Y sliding window
optimization IMU pre-
IMU keyframe
largely discarded. To guarantee high odometry accuracy, the selection
N optimize local
integration
factor graph
system requires nine-axis IMU readings of high frequency (500
build global
Hz as used in [17]) to de-skew the point cloud and initialize Livox filtered point cloud pose graph
Horizon
LiDAR odometry. Fusion with additional sensor modalities preprocessing
global map detect loop closure

(e.g., GPS signals) is often needed at certain spots to achieve

Fig. 2: System pipeline
precise localization.
More importantly, conventional LiDARs rely on mechanical Contributions
spinning mechanisms to enable a 360-degree FoV, while the
vertical resolution is fairly limited. Though some products with Considering the state of the art above, we propose a novel
high vertical resolution emerged recently (e.g., from Hesai1 tightly-coupled LiDAR-inertial odometry and mapping scheme
and Velodyne2 ), their very high prices prohibit mass market with a specific variant for solid-state LiDARs (pipeline given
supply for robotics industry and research. in Sec. II). A novel feature extraction approach is tailored
to the irregular scan pattern of Livox Horizon (Sec. III). To
Very recently, solid-state LiDARs have hit the consumer
directly fuse LiDAR and IMU measurements in a unified
market with much better affordability based on various working
manner, a hierarchical keyframe-based scheme is proposed
principles. Existing types often have non-repetitive and irregular
using sliding window optimization (Sec. IV). The proposed
scan patterns with small FoVs to reach more uniform and higher
system is generically applicable for both conventional and the
resolution. So far, solid-state-LiDAR-based odometry has not
deployed solid-state LiDAR. It runs in real time and delivers
been well investigated. In [19], the LiDAR odometry system
superior odometry accuracy over existing systems (Sec. V).
in [10] was adapted to Livox Mid-403 , a solid-state LiDAR
We release the proposed system with open-source code and
with a circular FoV of 38.4◦ . Compared with its baseline [10],
new solid-state-LiDAR-inertial data sets recorded by Livox
it employs similar feature-based scan-matching and delivers
Horizon and Xsens MTi-670. Thanks to the low hardware
comparable tracking accuracy with improved runtime via paral-
costs and real-time performance on portable platforms, our
lelization. To the best of the authors’ knowledge, no published
system provides a cost-effective solution for mobile perception
research on tightly-coupled solid-state-LiDAR-inertial fusion
in various scenarios.
exists to date.
In this paper, we provide a specific study on solid-state-
LiDAR-inertial odometry and mapping. Instead of Livox Mid-
II. S YSTEM P IPELINE
40, we choose Livox Horizon4 (released in Q1, 2020), which is
designed for vehicular perception with an FoV of 81.7◦ ×25.1◦ .
The proposed LiDAR-inertial odometry and mapping scheme
Scanning at 10 Hz, it reaches a similar but more uniform FoV
is shown in Fig. 2. The 3D LiDAR (e.g., Livox Horizon)
coverage compared with typical 64-line mechanical LiDARs.
streams out point clouds at a typical frequency of 10 Hz and
Moreover, it is substantially cheaper than most existing 3D
is synchronized with a six-axis IMU providing gyroscope and
LiDARs of comparable performance.
accelerometer readings at higher frequency (e.g., 200 Hz for
Common feature extraction methods for spinning LiDARs Xsens MTi-6705 ). We want to estimate the six-DoF egomotion
are not applicable for solid-state ones due to their irregular of the LiDAR frame and obtain a globally consistent map
and non-repetitive scan patterns. For Livox Mid-40, scanning simultaneously. The raw point clouds from LiDAR scan are
of a single laser head is specially regulated to form a circular first downsampled and rotationally de-skewed using gyroscope
coverage. Hence, the approach in [19] traverses along the data. Then, feature points representing planes and edges are
incident and deflection angle to choose point candidates, at extracted (Sec. III-A). Given the preprocessed scans, a light-
which the local smoothness of the scan line is computed for weight scan-matching-based registration module runs in a
feature extraction as in [10]. This, however, cannot be applied frame-to-model manner for fast motion estimation with point-
to Livox Horizon as it sweeps in a rather unregulated manner. to-edge and point-to-plane metrics being exploited (Sec. III-B).
Such a pattern is more generic for reaching broader FoVs The obtained egomotion estimates are further used to de-
of uniform coverage, which can potentially become common skew the translational distortion of the current sweep as well
for future types of solid-state LiDARs. But the limited FoVs as select keyframes adaptively to scene transitions. Parallel
can still thwart odometry performance in some circumstances, to preprocessing and LiDAR odometry, LiDAR and IMU
especially under fast motion or with insufficient features. measurements are fused at the backend in a unified manner
via the proposed keyframe-based sliding window optimization
1 https://www.hesaitech.com/en/Pandar128
2 https://velodynelidar.com/products/alpha-prime
(Sec. IV).
3 https://www.livoxtech.com/mid-40-and-mid-100
4 https://www.livoxtech.com/de/horizon 5 https://www.xsens.com/products/mti-600-series
 3

raw sweep : invalid point : fitted edge

: valid point ∈ Pi : edge feature ∈ E
<latexit sha1_base64="pYfPyMO9Xm02oOV2FaPs4MY31ks=">AAAB9HicbVDLSgMxFL1TX7W+qi7dDBbBVZkRxe4siOCygn1AZyhJmmlDM5kxyRTK0O9w48Iibv0YF4L/IPgLZtoutPVA4HDOvdyTg2POlHacTyu3srq2vpHfLGxt7+zuFfcPGipKJKF1EvFItjBSlDNB65ppTluxpCjEnDbx4Drzm0MqFYvEvR7F1A9RT7CAEaSN5HtMeCHSfYzTm3GnWHLKzhT2MnHnpHT1Vfk+/5jgWqf47nUjkoRUaMKRUm3XibWfIqkZ4XRc8BJFY0QGqEfbhgoUUuWn09Bj+8QoXTuIpHlC21P190aKQqVGITaTWUK16GXif1470UHFT5mIE00FmR0KEm7ryM4asLtMUqL5yBBEJDNZbdJHEhFteiqYEtzFLy+TxlnZvSg7d06peg4z5OEIjuEUXLiEKtxCDepA4AEe4Rkm1tB6sl6s19lozprvHMIfWG8/1M6Whw==</latexit>

<latexit sha1_base64="L4T8c4wWxX7urIE0Syg2cAcSErs=">AAAB+HicbVDLSsNAFL3xWeujsS7dDC2Cq5KIosuCLlxWsA9oQ5hMJ+3QySTMTIQa+iVuXPjArXv/wJU7/8ZJ24W2Hhg4nHMv98wJEs6Udpxva2V1bX1js7BV3N7Z3SvZ++WWilNJaJPEPJadACvKmaBNzTSnnURSHAWctoPRZe6376hULBa3epxQL8IDwUJGsDaSb5d6TPQirIdBkDUmPvPtqlNzpkDLxJ2Tar3w+VG+eqk0fPur149JGlGhCcdKdV0n0V6GpWaE00mxlyqaYDLCA9o1VOCIKi+bBp+gI6P0URhL84RGU/X3RoYjpcZRYCbzjGrRy8X/vG6qwwsvYyJJNRVkdihMOdIxyltAfSYp0XxsCCaSmayIDLHERJuuiqYEd/HLy6R1UnPPas6NaeMUZijAIVTgGFw4hzpcQwOaQCCFB3iCZ+veerRerbfZ6Io13zmAP7DefwDyU5Yv</latexit>

: edge candidate ∈ Gi : plane feature ∈ F

<latexit sha1_base64="3eBPLfGqEmAXL8XUq2XmPWMu5uw=">AAAB9HicbVDLSgMxFL1TX7W+qi7dDBbBVZkRxe4sCOKygn1AZyhJmmlDM5kxyRTK0O9w48Iibv0YF4L/IPgLZtoutPVA4HDOvdyTg2POlHacTyu3srq2vpHfLGxt7+zuFfcPGipKJKF1EvFItjBSlDNB65ppTluxpCjEnDbx4Drzm0MqFYvEvR7F1A9RT7CAEaSN5HtMeCHSfYzTm3GnWHLKzhT2MnHnpHT1Vfk+/5jgWqf47nUjkoRUaMKRUm3XibWfIqkZ4XRc8BJFY0QGqEfbhgoUUuWn09Bj+8QoXTuIpHlC21P190aKQqVGITaTWUK16GXif1470UHFT5mIE00FmR0KEm7ryM4asLtMUqL5yBBEJDNZbdJHEhFteiqYEtzFLy+TxlnZvSg7d06peg4z5OEIjuEUXLiEKtxCDepA4AEe4Rkm1tB6sl6s19lozprvHMIfWG8/1lOWiA==</latexit>

<latexit sha1_base64="aGOuGFYEe3zUmSN9ijm0p2EAFL8=">AAAB+HicbVDLSsNAFL2pr1ofjXXpJrQIrkoiii4LCrqsYB/QhDCZTtqhk0mYmQg19EvcuPCBW/f+gSt3/o2TtgttPTBwOOde7pkTJIxKZdvfRmFldW19o7hZ2tre2S2be5W2jFOBSQvHLBbdAEnCKCctRRUj3UQQFAWMdILRRe537oiQNOa3apwQL0IDTkOKkdKSb5Zdyt0IqWEQZFcTn/pmza7bU1jLxJmTWqP4+VG5fKk2ffPL7cc4jQhXmCEpe46dKC9DQlHMyKTkppIkCI/QgPQ05Sgi0sumwSfWoVb6VhgL/biypurvjQxFUo6jQE/mGeWil4v/eb1UhedeRnmSKsLx7FCYMkvFVt6C1aeCYMXGmiAsqM5q4SESCCvdVUmX4Cx+eZm0j+vOad2+0W2cwAxFOIAqHIEDZ9CAa2hCCzCk8ABP8GzcG4/Gq/E2Gy0Y8519+APj/QfklJYm</latexit>

<latexit sha1_base64="AEqLnoC4eowdcSgEi3krOC2nrnU=">AAAB6HicbZDLSgMxFIbPeK31VnXpJlgEV2VGFN1ZcOOyBXuBdiiZ9Ewbm8kMSUYopU/gxoUidelb+BrufBszbRfa+kPg4//PIeecIBFcG9f9dlZW19Y3NnNb+e2d3b39wsFhXcepYlhjsYhVM6AaBZdYM9wIbCYKaRQIbASD2yxvPKLSPJb3ZpigH9Ge5CFn1FiryjuFoltypyLL4M2hePM5yfRe6RS+2t2YpRFKwwTVuuW5ifFHVBnOBI7z7VRjQtmA9rBlUdIItT+aDjomp9bpkjBW9klDpu7vjhGNtB5Gga2MqOnrxSwz/8taqQmv/RGXSWpQstlHYSqIiUm2NelyhcyIoQXKFLezEtanijJjb5O3R/AWV16G+nnJuyy5VbdYvoCZcnAMJ3AGHlxBGe6gAjVggPAEL/DqPDjPzpszmZWuOPOeI/gj5+MH7TSRbg==</latexit>

i
<latexit sha1_base64="lozYeCck+I3AQBqWtMPMNmxZKdQ=">AAACDHicbVDLSgMxFM3UV62vqks3wSK4KjOi6LLgxmUF+4BOKUmaaUMzmSG5I5RhPsCNv+LGhSJu/QB3/o2ZdgRtPRA4nHMuuffQWAoDrvvllFZW19Y3ypuVre2d3b3q/kHbRIlmvMUiGekuJYZLoXgLBEjejTUnIZW8QyfXud+559qISN3BNOb9kIyUCAQjYKVBteaPCaR+SGBMadrMsoHwTUINhx+tk9mUW3dnwMvEK0gNFWgOqp/+MGJJyBUwSYzpeW4M/ZRoEEzyrOInhseETciI9yxVJOSmn86OyfCJVYY4iLR9CvBM/T2RktCYaUhtMt/QLHq5+J/XSyC46qdCxQlwxeYfBYnEEOG8GTwUmjOQU0sI08LuitmYaMLA9lexJXiLJy+T9lndu6i7t+e1xnlRRxkdoWN0ijx0iRroBjVRCzH0gJ7QC3p1Hp1n5815n0dLTjFziP7A+fgGc22cdQ==</latexit>

P̂i ⊂ W unfolded sweep

<latexit sha1_base64="TdhceuJEBYjKXo3xI55L5DuuZc0=">AAAB6HicbZDLSgMxFIbPeK31VnXpJlgEV2VGFN1ZcOOyBXuBdiiZ9Ewbm8kMSUYopU/gxoUidelb+BrufBszbRfa+kPg4//PIeecIBFcG9f9dlZW19Y3NnNb+e2d3b39wsFhXcepYlhjsYhVM6AaBZdYM9wIbCYKaRQIbASD2yxvPKLSPJb3ZpigH9Ge5CFn1FirqjqFoltypyLL4M2hePM5yfRe6RS+2t2YpRFKwwTVuuW5ifFHVBnOBI7z7VRjQtmA9rBlUdIItT+aDjomp9bpkjBW9klDpu7vjhGNtB5Gga2MqOnrxSwz/8taqQmv/RGXSWpQstlHYSqIiUm2NelyhcyIoQXKFLezEtanijJjb5O3R/AWV16G+nnJuyy5VbdYvoCZcnAMJ3AGHlxBGe6gAjVggPAEL/DqPDjPzpszmZWuOPOeI/gj5+MH+tiRdw==</latexit>

(A) 0 - 25 ms (B) 25 - 50 ms

A B C
Fig. 4: Illustration of proposed feature extraction method.

Algorithm 1: Feature Extraction for Livox Horizon

Input: single sweep W
(C) 50 - 75 ms (D) 75 - 100 ms Output: edge feature set E, plane feature set F
1 E ← ∅, F ← ∅ ;
Fig. 3: Scan pattern of Livox Horizon with real-world scene.
2 {P̂i = {Xr }6r=1 | Xr ∈ R3×7 }τi=1 ← split (W) ;
Points are rendered according to time stamps with rainbow
3 for i ← 1 to τ do
color scale. Blue blobs depict the full sweep.
4 Pi ← getValidPoints (P̂i ) ;
The fusion window usually covers several (e.g., three) 5 Σi ← computeCovariance (Pi ) ;
keyframes. As the window slides, keyframe states (denoted 6 {(λ1 , λ2 , λ3 )} ← eig (Σi ); // λ1 ≤ λ2 ≤ λ3
with˘on top as follows) are optimized in the current window /* check plane feature */
> 7 if λ1 /λ2 < 0.3 then
∈ R3 × R3 × S3 × R6 ⊂ R16 . (1)

x̆ = t̆> , v̆> , q̆> , b̆> 8 F ← F ∪ Pi ;
9 else
Here, t̆ ∈ R and q̆ ∈ S denote the keyframe position
3 3
/* check edge feature */
and orientation (represented by unit quaternions), respectively.
10 Gi ← ∅ ;
v̆ ∈ R3 is the velocity and b̆ = [ b̆> > >
a , b̆g ] ∈ R6 denotes
11 for r ← 1 to 6 do
the term incorporating the IMU bias of the accelerometer
12 xr ← getCurv (Xr ) ;
(subscript ‘a’) and the gyroscope (subscript ‘g’). After being
13 Gi ← Gi ∪ Xr ;
slid over, the two oldest optimized keyframes are used as
constraints to optimize the in-between regular-frame poses via 14 Γi ← computeCovariance (Gi ) ;
factor graph optimization. Preintegrated inertial measurements 15 {(λ1 , λ2 , λ3 )} ← eig (Γi ) ;
are hereby exploited for pose initialization. A global pose graph 16 if λ2 /λ3 < 0.25 then
is maintained to incorporate all poses of LiDAR frames. Loop 17 E ← E ∪ Gi ;
closure is checked in a keyframe basis using ICP, and when
necessary, a global graph optimization is invoked to guarantee
the reconstructed map to be globally consistent. LiDARs. Compared with Mid-40 that regulates its single laser
head for circular FoV coverage, Horizon has a more irregular
scan pattern due to its rather unregulated sweeping motion.
III. F EATURE -BASED S OLID -S TATE L I DAR
S CAN -M ATCHING To extract plane and edge feature points for Livox Horizon,
we propose a new two-stage approach in Alg. 1 (illustrated
A. Feature extraction for irregular scan pattern in Fig. 4). Given a frame of raw scan, we unfold and split
Existing feature extraction methods for 3D LiDARs are not the sweep W in its time domain, where 6 × 7-point patches
well applicable for Livox Horizon [11], [19]. Fig. 3 illustrates are assigned one after another without overlapping (Alg. 1,
its scan pattern (at 10 Hz) by dividing one LiDAR sweep into line 2). In each raw patch P̂i , inconcrete point readings are
four stages. The Livox Horizon is instrumented with a multi- first removed (Alg. 1, line 4). For the valid points Pi , we
laser sensing module with an array of six vertically-aligned perform eigendecomposition of the covariance of their 3D
laser diodes sweeping back and forth through the prisms non- coordinates (Alg. 1, line 5-6). If the second largest eigenvalue
repetitively. Consequently, the six point readings obtained from is substantially larger than the smallest one (λ1 /λ2 < 0.3), then
the multi-laser transceiver are vertically aligned and perceived all points in the patch are extracted as plane features (Fig. 4-A
simultaneously (rendered as the same color). The scan shows and Alg. 1, line 7-8). For a non-plane patch, we search the
an irregular “brushing” pattern covering the 81.7◦ × 25.1◦ point with largest curvature on each scan line and perform
FoV uniformly. Over the integration time of 100 ms per frame, eigendecomposition for the six points (Alg. 1, line 9-15). If
the angular resolution reaches 0.2◦ to 0.4◦ horizontally and the largest eigenvalue is substantially larger than the second
vertically with a similar FoV coverage to 64-line spinning largest one (λ2 /λ3 < 0.25), then the points form a line and
4

they are extracted as edge features (Fig. 4-C and Alg. 1, line
16-17). Otherwise, no feature is extracted from the current
patch (Fig. 4-B).
We show an example of extracted features in one frame of
Livox Horizon scan in Fig. 5. Note that the proposed feature
extraction algorithm is purely performed in the time domain
of every sweep given time stamps of the perceived point
array. This is radically different from the approach in [19] that
computes local smoothness of point candidates selected through (A) extracted plane features (red)
spatial retrieval. Also for traditional spinning LiDARs, common
point cloud segmentation or feature extraction methods [10],
[11], [20] are performed in (transformed) spatial domains, e.g.,
extracting features w.r.t. the horizontal scan angles. For Livox
Horizon, the number of extracted plane features are usually
much more than edge features due to its uniform scan coverage
(statistics are given in Sec. V-D). We further associate each
feature point with its corresponding edge’s direction vector or
(B) extracted edge (green) and plane (red) features
plane’s normal vector to represent local geometries. It will be
further exploited for weighting the LiDAR residual term in the Fig. 5: Feature extraction for point cloud (blue) from Horizon.
backend fusion module. Here, we also have pw = R(q) pl + t.

B. Point-to-edge and point-to-plane metric C. Metric weighting function

Given the extracted edge and plane features, both the frontend In order to quantify the contribution of each LiDAR residual
registration module and the sliding window optimization for during sensor fusion, we propose a metric weighting function
backend fusion in Fig. 2 exploit the scan-matching formulation according to the association quality as follows
to incorporate LiDAR measurements for egomotion estimation. P5
v◦ (pl ) = λ · (ν◦ )> n◦ · exp − j=1 |γ(pl ) − γj | .


Following metrics are applied.
Point-to-edge metric: Suppose an edge feature pl ∈ E Here, ◦ indicates the type of feature correspondences, namely
is extracted from sweep W w.r.t. the LiDAR frame l and edges (subscript ‘e’) or planes (subscript ‘s’). For an edge
its associated unit vector νe indicating the edge direction feature correspondence, νe and ne denote the direction vector
is available. We first search its nearest five edge points in of the edge line at pl and the line approximated by its nearest
the corresponding local feature map Mw and compute their five edge features, respectively. Similarly, νs and ns denote
coordinates’ mean value ēw and covariance matrix (w.r.t. the the plane normal at point pl and the one formed by its
global frame w). Based thereon, an eigendecomposition is nearest five plane features, respectively. Moreover, γ(pl ) and
performed. If the largest eigenvalue is significantly larger than γj are the reflectance values of the feature point pl and its
the rest, the five points in Mw form a line with its direction associated nearest five features, respectively. Therefore, the
vector ne being the eigenvector corresponding to the largest proposed metric weighting function considers both geometric
eigenvalue. We then take two points éw = ēw + δne and and appearance consistencies of feature associations. We set
èw = ēw − δne on the fitted line and exploit a point-to-edge the constant λ = 15 from experience.
metric of the following form
k(pw − éw ) × (pw − èw )k D. Feature-based scan-matching
De (xw , pl , Mw ) = .
kéw − èw k
At the frontend, we perform a light-weight, feature-based
Here, pw = R(q) pl +t denotes the scan point w.r.t. the global scan-matching in a frame-to-model manner. A trust region
frame given current LiDAR pose x> = [ q> , t> ]> . R(q) is method is used to estimate the current pose xw by minimizing
the rotation matrix given by q. Typically, δ = 0.1 . X m n 
w 2 w 2
X
Point-to-plane metric: If the feature point pl indicates a De (x , pi , M ) +
w l
Ds (x , pj , M )
w l



min ,
xw
plane (with a normal νs ), we also transform it into its world i=1 j=1
coordinates pw and find its nearest five plane feature points
which incorporates all the edge (pli ∈ E) and plane (plj ∈ F)
in the corresponding local feature map w.r.t. the global frame,
correspondences between the current scan and the local map
namely Sw = {sw j } ⊂ M , with j = 1, ..., 5. Similar to the
w
Mw . Here, De and Ds denote the point-to-edge and point-to-
implementation for [10], we solve an overdetermined linear
plane metrics, respectively. The frontend local feature map Mw
equation As us = c via QR decomposition for plane fitting,
is updated with a width of typically 20 recent frames given
1 , ..., s5 ] ∈ R and c = [ −1, ..., −1 ] ∈ R5 .
5×3
with As = [ sw w
the optimized poses. The cost function above is similar to
We normalize the fitted normal vector as ns = us / ||us || and
other popular LiDAR odometry systems [10], [15], [17], [19].
the point-to-plane metric can be established as
However, we restrict the optimization time for fast motion
Ds (xw , pl , Mw ) = |u> w
 > w 
s p + 1|/ ||us || =  ns p + 1/ ||us ||  . estimation, based on which the translational scan distortion
 5

keyframes. For a sliding window of τw -keyframe width, the

: IMU measurement : plane feature : prior factor
optimal keyframe states X̆ = [ x̆> > >
1 , ...., x̆τw ] ∈ R15×τw are
: preintegrated IMU factor : edge feature
: regular frame
obtained by minimizing
sliding window
: keyframe optimization  τw
X τw
X 
w 2 w w
min kRP (X̆ )k + JL (x̆k ) + JI (x̆k ) (2)
X̆w
k=1 k=1

in the form of maximum a posterior (MAP). RP (X̆w ) denotes

the prior residual term representing the measurements that are
marginalized out due to window-sliding. JL (x̆w w
k ) and JI (x̆k )
denote the keyframe-wise LiDAR and IMU error terms. Details
local graph about the three components follow.
optimization
marginalization Prior factor: In order to bound the computational burden
without substantial information loss, we exploit marginaliza-
preintegration
tion in the sliding window optimization. Here, the oldest
Fig. 6: Proposed tightly-coupled LiDAR-inertial fusion scheme keyframe and its measurements are marginalized out via Schur-
using keyframe-based sliding window optimization. complement [22]. A new prior is computed accordingly and
is corrected and keyframes are selected. The accuracy of added on top of the existing prior factor to carry the estimate
state estimation is pursued using the proposed backend fusion from the removed keyframe to the next window.
scheme shown below. LiDAR term: The LiDAR term incorporates geometric
constraints from LiDAR measurements into the fusion scheme.
IV. T IGHTLY-C OUPLED L I DAR-I NERTIAL F USION VIA When aligning the observed edge (p̆lk,i ∈ Ĕk ) and plane
K EYFRAME -BASED S LIDING W INDOW O PTIMIZATION (p̆lk,j ∈ F̆k ) features to the local feature map M̆w
k observed by
A. Keyframe-based fusion hierarchy recent 30 keyframes, the term is defined as JL (x̆w k) =
Keyframe-based schemes have originally been proposed X m n
w 2 w 2
X
ṽe,i De (x̆w l
M̆ ṽs,j Ds (x̆wk , p̆k,j , M̆k ) .
l



and widely applied to visual odometry to achieve accurate k , p̆k,i , k ) +
tracking in real time [21]. In [17], keyframes from LiDAR i=1 j=1
odometry frontend [11] are fused to constrain IMU factors Here, De and Ds are the point-to-edge and point-to-plane
via iSAM2 (thus indirect fusion). The tightly-coupled LiDAR- metrics in Sec. III-B with fixed feature correspondences, re-
inertial odometry in [15] realized direct fusion of LiDAR spectively. ṽe and ṽs denote normalized weights among feature
and pre-integrated IMU measurements via sliding window correspondences of each type. The local map is updated given
optimization. However, real-time performance is not generally the optimized poses as the window slides.
achievable as the scheme fuses LiDAR sweeps of every frame. IMU term: The error term for IMU incorporates the relative
Therefore, it is of importance to maintain the sparsity of the motion constraints between keyframes into the fusion scheme.
optimization scheme for direct LiDAR-inertial fusion at the To avoid repropagating IMU states each time the optimization
backend. Shown in Fig. 6, the proposed fusion scheme exploits window slides, raw inertial readings are pre-integrated between
keyframes to establish sliding windows, where LiDAR and pre- two consecutive keyframes k and k + 1 as in [15], [22]. The
integrated IMU measurements at keyframes are fused in a uni- term is defined as
fied manner via nonlinear optimization. As the window slides w k 2 k
forward after optimization, we construct a local factor graph JI (x̆w
k ) = kRI (x̆k , ẑk+1 )kCk , with RI (x̆w k , ẑk+1 ) =
k+1
 
incorporating the two oldest keyframe poses as constraints and 1 w
∆τk − α̂kk+1
2


R̆w>
k t̆w w
k+1 − t̆k + 2 g ∆τk − vk
w
the regular frames poses initialized by IMU measurements. R̆w> w w w

k
k v̆k+1 + g ∆τk − v̆k − β̂
 
A small-scale factor graph optimization is invoked to obtain

w −1 w k −1
 k+1 
2 (qk ) ⊗ qk+1 ⊗ (γ̂k+1 ) vec
 
 
regular-frame poses at LiDAR frequency.
b̆a,k+1 − b̆a,k
 
 
Setting up keyframes can critically affect odometry accuracy
due to the IMU drift during the time interval of two consecutive b̆g,k+1 − b̆g,k
keyframes. We introduce two criteria for keyframe selection: being the preintegrated measurement residual at keyframe
(1) If the overlapping ratio between features of the current k. ẑkk+1 = [ α̂k> k> k> >
k+1 , β̂k+1 , γ̂k+1 ] is the pre-integrated IMU
frame and the local feature map is smaller than 60% or (2) if measurements incorporating gyroscope and accelerometer
the time difference to the last keyframe is more than a certain readings from keyframe k to k + 1. ∆τk denotes the time
number of (e.g., two) regular frames, the current frame is interval between consecutive keyframes k and k+1. We use the
then selected as a new keyframe. Here, restricting the frame operator [ · ]vec to take out the vector part of a quaternion. Due
interval between keyframes helps mitigate the drift issue for to space constraints, we do not provide the derivation of IMU
IMU pre-integration. pre-integration and the corresponding noise covariance Ckk+1
for the Mahalanobis norm above. A dedicated introduction can
B. Sliding window optimization for keyframes be found in [22].
We compute keyframe states of form (1) by directly fusing The nonlinear least square problem in (2) can be solved
LiDAR and pre-integrated IMU measurements, all observed at using typical solvers, e.g., the trust region method. Constrained
6

by the optimized keyframes, regular LiDAR frames in between

are obtained via local graph optimization. After the fusion
window slides over, the newly obtained LiDAR poses are HDL-64E

inserted into a global pose graph with only keyframe feature Xsens Mti-670

maps maintained for mapping purpose. To detect potential loop

closures, we search in the global graph, e.g., in a radius of 10 m, Livox Horizon

to find keyframe nodes that are spatially close but with enough
temporal distance (e.g., 20 keyframes). An ICP is performed (A) (B)
between the current feature scan and the candidate feature Fig. 7: Experimental setup for FR-IOSB data set.
map, from which a fitting score is computed for loop closure
detection. Once confirmed, a global pose graph optimization is For the remaining sequences, LIO-SAM still shows worse
invoked by imposing the constraint from the ICP. As depicted in tracking accuracy than LiLi-OM though it additionally exploits
Fig. 2, the keyframe local map M̆w at backend is then updated orientation measurements from a magnetometer. This mainly
by corrected poses to further incorporate the LiDAR constraint. results from the unified fusion scheme of LiLi-OM where
LiDAR and inertial measurements are directly fused. LIOM
fails on UrbanNav data sets (denoted as 7) and shows large drift
V. E VALUATION
on UTBM-1. It also cannot run in real time with recommended
A. Implementation and evaluation setup configurations. LOAM delivers large tracking errors on UTBM
We implement the proposed LiDAR-inertial odometry and as the implementation limits the iteration number in scan-
mapping system in C++ using ROS [23]. The three modules matching for real-time performance.
shown in Fig. 2 are structured as three individual nodes. The
nonlinear optimization problem in (2) is solved using the TABLE I: APE (RMSE) in meters on public data sets
Ceres Solver [24]. We use GTSAM [25] to perform factor dataset LOAM LeGO LIOM LINS LIO-SAM LiLi-OM?
UTBM-1 479.51 17.12 468.75 16.90 – 8.61
graph optimization for rectifying the global pose graph at loop UTBM-2 819.95 6.46 12.95 9.31 – 6.45
closures. Our system is developed for Livox Horizon with the UL-1 2.39 2.22 2.53 2.27 2.54 1.59
name LiLi-OM. It is, however, also applicable for conventional UL-2 2.58 2.30 2.00 2.99 2.50 1.20
UN-1 11.20 2.70 7 2.19 2.28 1.08
spinning LiDARs thanks to its generic backend fusion. Thus, UN-2 12.70 4.15 7 4.80 5.31 3.24
two versions of the system are evaluated: (1) the original LiLi-
OM for Livox Horizon with the proposed feature extraction
approach and (2) its variant LiLi-OM? using the preprocessing C. Experiment
module of [10] for spinning LiDARs. Evaluations are conducted To further test LiLi-OM in real-world scenarios, we set up
based on public data sets (recorded using conventional LiDARs) a sensor suite composed of a Livox Horizon and an Xsens
and experiments (including data sets from Livox Horizon). All MTi-670 IMU. The total cost is about 1700 Euros (Q1, 2020),
LiDAR frame rates are 10 Hz. which is much less than conventional LiDAR-inertial setups.
1) FR-IOSB data set: Shown in Fig. 7-(A), a mobile platform
B. Public data set is instrumented with the proposed Livox-Xsens suite. For
comparison with high-end mechanical spinning LiDARs, we
We deploy LiLi-OM? to compare with competing state-of- set up a Velodyne HDL-64E onboard and synchronize it with
the-art systems. These include works on (1) LiDAR odometry: an Xsens MTi-G-700 IMU (six-axis, 150 Hz). Three sequences
A-LOAM6 (open-source version of LOAM [10]), LeGO- were recorded at the Fraunhofer IOSB campus of Fig. 7-(B):
LOAM [11] (shortened as LeGO) and (2) LiDAR-inertial (1) Short for a short path in structured scenes, (2) Tree recorded
odometry: LIO-mapping (shortened as LIOM) [15], LINS [16], in bushes, and (3) Long for a long trajectory.
LIO-SAM [17]. For evaluation, we use the EU long-term data Both LiLi-OM and LiLi-OM? are tested. For comparison,
set (UTBM) that provides two long urban navigation sequences we run LOAM, LeGO and Livox-Horizon-LOAM (shortened
recorded by a Velodyne HDL-32E and a six-axis IMU (100 as LiHo)8 , a LOAM variant adapted to Livox Horizon with
Hz) [26]. LIO-SAM requires nine-axis IMU measurements. point clouds deskewed by IMU. Tab. II shows superior tracking
Thus, we include the UrbanLoco and UrbanNav data sets [5] accuracy (bold) of the proposed systems with our low-cost
recorded using a HDL-32E and Xsens MTi-10 IMU (nine-axis, hardware setup performing equally well as the high-end one
100 Hz). The RMSE of the absolute position error (APE) is in the same scenario. We show reconstructed maps (partial)
computed for the final estimated trajectory based on the ground on sequence Long in Fig. 8, where LiLi-OM? delivers superior
truth using the script in [27]. mapping quality using the proposed sensor fusion scheme.
Shown in Tab. I7 , the proposed LiLi-OM? achieves the best
tracking accuracy (bold) for all sequences in real time. LIO- TABLE II: End-to-end position error in meters on FR-IOSB
SAM [17] requires nine-axis IMU readings for de-skewing Velodyne HDL-64E Livox Horizon
and frontend odometry, thereby not applicable for UTBM. dataset length speed LOAM LeGO LiLi-OM? LiHo LiLi-OM
Short 0.49 km 2.15 m/s 0.78 0.25 0.34 5.04 0.25
6 https://github.com/HKUST-Aerial-Robotics/A-LOAM Tree 0.36 km 1.12 m/s 0.21 78.22 < 0.1 0.13 < 0.1
7 Data sets abbr.: UTBM-1: UTBM-20180719, UTBM-2: UTBM-20180418-RA, UL-1: Long 1.10 km 1.71 m/s 0.43 0.82 < 0.1 3.91 0.34
UrbanLoco-HK-20190426-1, UL-2: UrbanLoco-HK-20190426-2, UN-1: UrbanNav-HK-
20190314, UN-2: UrbanNav-HK-20190428. 8 https://github.com/Livox-SDK/livox_horizon_loam
 7

Livox-
Xsens
suite

312m
496m

(A) LOAM (B) LiLi-OM?

Fig. 8: Mapping result comparison on sequence Long. (A) (B)
Fig. 9: (A) Recording KA-Urban with proposed sensor suite.
2) KA-Urban data set: Shown in Fig. 9-(A), the proposed
(B) Map from LiLi-OM on Schloss-2 (path ends at origin).
Livox-Xsens sensor suite was further deployed onboard a
backpack platform for large-scale test in urban scenarios.
Five sequences were recorded in Karlsruhe, Germany with
end-to-end locations registered from satellite images. Beside
standard configuration of LiLi-OM, we deactivate its loop
closure module to evaluate the odometry accuracy without
global correction. LiHo is run for comparison.
Shown in Tab. III, LiLi-OM without loop closure (denoted
as LiLi-OM-O) delivers much less drift than LiHo. When LiLi-OM
LiLi-OM (OL)
exploiting loop closure constraints, LiLi-OM shows very small LH-LOAM

end-to-end errors. In order to justify the benefits of multi-sensor

fusion, we provide another special configuration of LiLi-OM
Fig. 10: Test results on sequence East.
with IMU constraints totally removed (including de-skewing).
Evaluations are done on long-distance sequences (the last three Preprocessing for feature extraction and scan registration at
ones) with loop closure both on and off. Denoted by the second frontend are light-weight. Runtime is dominated by backend
entry after “ / ”, this configuration is superior to LiHo (also fusion.
pure LiDAR odometry), but inferior to the standard one using
LiDAR-inertial fusion. TABLE IV: Runtime of LiLi-OM per frame in ms
The mapping result of LiLi-OM on Schloss-1 is given in Velodyne HDL Livox Horizon
node UTBM-2 UL-1 Long Long Schloss-2 East
Fig. 1, where the Schloss Karlsruhe is digitalized in high- preprocessing 12.72 13.31 30.14 9.99 10.21 11.76
precision point cloud using the proposed sensor suite. Result scan registration 22.29 23.71 16.71 22.69 27.15 25.30
of running LiLi-OM on Schloss-2 is visualized in Fig. 9-(B). backend fusion 50.27 57.62 60.92 58.86 54.56 41.81
An area of 496 m × 312 m is mapped accurately with global
consistency (compared to satellite image). Sequence East was Constructing the LiDAR constraints at backend fusion often
recorded while cycling through eastern Karlsruhe for a long takes substantial time. In Tab. V, we show statistics on feature
distance under fast and dynamic motion. Shown in Fig. 10, LiLi- extraction for LiLi-OM with typical configurations. Extracted
OM delivers accurate odometry and mapping results using the features are counted per frame at preprocessing, while only
proposed low-cost sensor suite. associated features are counted at backend fusion per keyframe.
Due to the relatively uniform FoV coverage of Livox Horizon,
TABLE III: End-to-end position error in meters on KA-Urban more plane features are extracted than edges on average. To
dataset length speed LiHo LiLi-OM-O LiLi-OM guarantee runtime efficiency, features are always downsampled
Campus-1 0.50 km 1.43 m/s 1.47 1.11 / – 0.13 / – using a voxel grid filter before providing the LiDAR constraints.
Campus-2 0.20 km 1.57 m/s 0.40 0.21 / – 0.19 / –
Schloss-1 0.65 km 1.03 m/s 1.55 0.95 / 1.21 0.15 / 0.24
Schloss-2 1.10 km 1.49 m/s 8.34 4.41 / 5.58 0.08 / 0.12 TABLE V: Average feature numbers
East 3.70 km 3.11 m/s 109.62 15.66 / 19.28 1.28 / 3.43 preprocessing / frame backend fusion / keyframe
dataset raw points edges planes edges planes
Schloss-1 22579 207 12791 162 3688
Schloss-2 22230 633 13128 425 2649
East 21544 337 13109 247 2363
D. Runtime
All evaluations are done on a laptop (Intel Core i5-7300HQ
CPU, 8GB RAM) with all four CPU cores involved. For all the VI. C ONCLUSION
data sets recorded with different devices in the evaluation, LiLi- In this work, we propose a novel sensor fusion method for
OM delivers real-time performance (at LiDAR frame rate). The real-time LiDAR-inertial odometry and mapping. A keyframe-
three nodes in Fig. 2 run in parallel and their average runtime based hierarchical scheme is established for directly fusing
per frame on representative sequences is collected in Tab. IV. LiDAR and (pre-integrated) IMU measurements via sliding
8

window optimization. Given the optimized keyframe states, [8] A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” in Proceedings
regular-frame poses are obtained via factor graph optimization. of the 2009 Robotics: Science and Systems (RSS 2009), vol. 2, no. 4,
Edinburgh, UK, June 2009.
The proposed LiDAR-inertial odometry and mapping system [9] Y. Chen and G. Medioni, “Object Modelling by Registration of Multiple
is universally applicable for both conventional LiDARs and Range Images,” Image and Vision Computing, vol. 10, no. 3, pp. 145–155,
solid-state LiDARs of small FoVs. For the latter use case, a 1992.
[10] J. Zhang and S. Singh, “LOAM: Lidar Odometry and Mapping in Real-
novel feature extraction method is designed for the irregular Time,” in Proceedings of the 2014 Robotics: Science and Systems (RSS
and unique scan pattern of Livox Horizon, a newly released 2014), vol. 2, no. 9, Berkeley, California, USA, July 2014.
spinning free, solid-state LiDAR with much lower price than [11] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and Ground-
Optimized Lidar Odometry and Mapping on Variable Terrain,” in
conventional 3D LiDARs. We conduct evaluations on both Proceedings of the 2018 IEEE/RSJ International Conference on Intelligent
public data sets of conventional LiDARs and experiments Robots and Systems (IROS 2018), Madrid, Spain, Oct. 2018, pp. 4758–
using the Livox Horizon. Results show that the proposed 4765.
[12] I. Bogoslavskyi and C. Stachniss, “Fast Range Image-Based Segmentation
system is real-time capable and delivers superior tracking and of Sparse 3D Laser Scans for Online Operation,” in Proceedings of
mapping accuracy over state-of-the-art LiDAR/LiDAR-inertial the 2016 IEEE/RSJ International Conference on Intelligent Robots and
odometry systems. The proposed system, LiLi-OM, is featured Systems (IROS 2016), Daejeon, Korea, Oct. 2016, pp. 163–169.
[13] J. Tang, Y. Chen, X. Niu, L. Wang, L. Chen, J. Liu, C. Shi, and J. Hyyppä,
as a cost-effective solution of high-performance LiDAR-inertial “LiDAR Scan Matching Aided Inertial Navigation System in GNSS-
odometry and mapping using solid-state LiDAR. Denied Environments,” Sensors, vol. 15, no. 7, pp. 16 710–16 728, 2015.
There is still much potential to exploit for the proposed [14] C. Le Gentil, T. Vidal-Calleja, and S. Huang, “IN2LAAMA: Inertial
Lidar Localization Autocalibration and Mapping,” IEEE Transactions on
system. The deployed Livox-Xsens suite is lightweight and Robotics, vol. 37, no. 1, pp. 275–290, 2021.
LiLi-OM is developed for universal egomotion estimation (not [15] H. Ye, Y. Chen, and M. Liu, “Tightly Coupled 3D Lidar Inertial Odometry
only for planar motion as [11]). Thus, it should be, for instance, and Mapping,” in Proceedings of the 2019 International Conference on
Robotics and Automation (ICRA 2019), Montreal, Canada, May 2019,
tested onboard unmanned aerial vehicles in applications such pp. 3144–3150.
as autonomous earth observation or environmental modeling [16] C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, “LINS:
coping with aggressive six-DoF egomotion. For large-scale A Lidar-Inertial State Estimator for Robust and Efficient Navigation,”
in Proceedings of the 2020 International Conference on Robotics and
odometry and mapping with limited computational resources, Automation (ICRA 2020), Paris, France, May 2020, pp. 8899–8906.
advanced map representations can be employed to improve [17] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and R. Daniela,
memory as well as runtime efficiency. Potential options include “LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and
Mapping,” in Proceedings of the 2020 IEEE/RSJ International Conference
volumetric mapping using TSDF (Truncated Signed Distance on Intelligent Robots and Systems (IROS 2020), Las Vegas, Nevada, USA,
Fields) [3] or mapping with geometric primitives (especially Oct. 2020, pp. 5135–5142.
in man-made environment [28]). [18] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert,
“iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree,” The
International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235,
ACKNOWLEDGMENT 2012.
[19] J. Lin and F. Zhang, “Loam Livox: A Fast, Robust, High-Precision
We would like to thank Thomas Emter from Fraunhofer LiDAR Odometry and Mapping Package for LiDARs of Small FoV,”
IOSB for providing the robot platform and assistance in in Proceedings of the 2020 International Conference on Robotics and
Automation (ICRA 2020), Paris, France, May 2020, pp. 3126–3131.
recording the FR-IOSB data set. [20] B. Wu, A. Wan, X. Yue, and K. Keutzer, “SqueezeSeg: Convolutional
Neural Nets with Recurrent CRF for Real-Time Road-Object Segmenta-
R EFERENCES tion from 3D LiDAR Point Cloud,” in Proceedings of the 2018 IEEE
International Conference on Robotics and Automation (ICRA 2018),
[1] J. Engel, J. Sturm, and D. Cremers, “Camera-Based Navigation of a Low- Brisbane, Australia, May 2018, pp. 1887–1893.
Cost Quadrocopter,” in Proceedings of the 2012 IEEE/RSJ International [21] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale,
Conference on Intelligent Robots and Systems (IROS 2012), Vilamoura, “Keyframe-Based Visual–Inertial Odometry Using Nonlinear Optimiza-
Portugal, Oct. 2012, pp. 2815–2821. tion,” The International Journal of Robotics Research, vol. 34, no. 3, pp.
[2] S. Bultmann, K. Li, and U. D. Hanebeck, “Stereo Visual SLAM Based 314–334, 2015.
on Unscented Dual Quaternion Filtering,” in Proceedings of the 22nd [22] T. Qin, P. Li, and S. Shen, “VINS-Mono: A Robust and Versatile
International Conference on Information Fusion (Fusion 2019), Ottawa, Monocular Visual-Inertial State Estimator,” IEEE Transactions on
Canada, July 2019, pp. 1–8. Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[3] V. Reijgwart, A. Millane, H. Oleynikova, R. Siegwart, C. Cadena, and [23] M. Quigley, B. Gerkey, K. Conley, J. Faust, T. Foote, J. Leibs, E. Berger,
J. Nieto, “Voxgraph: Globally Consistent, Volumetric Mapping Using R. Wheeler, and A. Ng, “ROS: An Open-source Robot Operating System,”
Signed Distance Function Submaps,” IEEE Robotics and Automation ICRA Workshop on Open Source Software, vol. 3, no. 3.2, pp. 5–10,
Letters, vol. 5, no. 1, pp. 227–234, 2020. 2009.
[4] O. Kähler, V. A. Prisacariu, J. P. C. Valentin, and D. W. Murray, [24] S. Agarwal, K. Mierle, and Others, “Ceres Solver,” http://ceres-solver.org,
“Hierarchical Voxel Block Hashing for Efficient Integration of Depth 2016.
Images,” IEEE Robotics and Automation Letters, vol. 1, no. 1, pp. 192– [25] F. Dellaert, “Factor Graphs and GTSAM: A Hands-on Introduction,”
197, 2016. Georgia Institute of Technology, Atlanta, GA, USA, Tech. Rep. GT-RIM-
[5] W. Wen, Y. Zhou, G. Zhang, S. Fahandezh-Saadi, X. Bai, W. Zhan, CP&R-2012-002, Sep. 2012.
M. Tomizuka, and L.-T. Hsu, “Urbanloco: A Full Sensor Suite Dataset [26] Z. Yan, L. Sun, T. Krajnik, and Y. Ruichek, “EU Long-term Dataset with
for Mapping and Localization in Urban Scenes,” in Proceedings of the Multiple Sensors for Autonomous Driving,” in Proceedings of the 2020
2020 International Conference on Robotics and Automation (ICRA 2020), IEEE/RSJ International Conference on Intelligent Robots and Systems
Paris, France, May 2020, pp. 2310–2316. (IROS 2020), Las Vegas, Nevada, USA, Oct. 2020, pp. 10 697–10 704.
[6] A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision Meets Robotics: [27] M. Grupp, “evo: Python Package for the Evaluation of Odometry and
The KITTI Dataset,” The International Journal of Robotics Research, SLAM,” https://github.com/MichaelGrupp/evo, 2017.
vol. 32, no. 11, pp. 1231–1237, 2013. [28] H. Möls, K. Li, and U. D. Hanebeck, “Highly Parallelizable Plane
[7] F. Pomerleau, F. Colas, R. Siegwart, and S. Magnenat, “Comparing ICP Extraction for Organized Point Clouds Using Spherical Convex Hulls,”
Variants on Real-World Data Sets,” Autonomous Robots, vol. 34, no. 3, in Proceedings of the 2020 IEEE International Conference on Robotics
pp. 133–148, 2013. and Automation (ICRA 2020), Paris, France, May 2020, pp. 7920–7926.

View publication stats