IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION.

ACCEPTED JANUARY, 2022 1

Direct LiDAR Odometry:

Fast Localization with Dense Point Clouds
Kenny Chen1 , Brett T. Lopez2 , Ali-akbar Agha-mohammadi3 , and Ankur Mehta1

Abstract—Field robotics in perceptually-challenging environ-

ments require fast and accurate state estimation, but modern
LiDAR sensors quickly overwhelm current odometry algorithms.
To this end, this paper presents a lightweight frontend LiDAR
odometry solution with consistent and accurate localization for
computationally-limited robotic platforms. Our Direct LiDAR
Odometry (DLO) method includes several key algorithmic in-
novations which prioritize computational efficiency and enables
arXiv:2110.00605v3 [[Link]] 7 Jan 2022

the use of dense, minimally-preprocessed point clouds to pro-

vide accurate pose estimates in real-time. This is achieved
through a novel keyframing system which efficiently manages
historical map information, in addition to a custom iterative
closest point solver for fast point cloud registration with data
structure recycling. Our method is more accurate with lower
computational overhead than the current state-of-the-art and has
been extensively evaluated in multiple perceptually-challenging
environments on aerial and legged robots as part of NASA
JPL Team CoSTAR’s research and development efforts for the
DARPA Subterranean Challenge.
Index Terms—Localization, Mapping, SLAM, Field Robots
Fig. 1. Fast and lightweight LiDAR odometry. Two of Team CoSTAR’s
robotic platforms which have limited computational resources. (A) Our custom
I. I NTRODUCTION quadrotor platform which features an Ouster OS1 LiDAR sensor on top. (B) A
CCURATE state estimation and mapping in large, Boston Dynamics Spot robot with a mounted custom payload and a Velodyne

A perceptually-challenging environments have become crit-

ical capabilities for autonomous mobile robots. Whereas typi-
VLP-16 with protective guards. (C) Top-down view of a mapped limestone
mine using our lightweight odometry method on these robots during testing
and integration for the DARPA Subterranean Challenge.
cal visual SLAM approaches often perform poorly in dust, fog, guess, followed by a “scan-to-map” registration between the
or low-light conditions, LiDAR-based methods can provide current scan and past environmental knowledge to increase
more reliable localization due to the superior range and global pose consistency. Unfortunately, the large number of
accuracy of direct depth measurements [1]. However, recent data points per scan from modern LiDARs quickly over-
work on LiDAR odometry (LO) have revealed the challenges whelms computationally-limited processors and bottlenecks
of processing the large number of depth returns generated by performance during alignment, which can induce frame drops
commercial LiDAR sensors in real-time for high-rate state and ultimately cause poor pose estimation. More specifically,
estimation [2], [3]. This work presents several algorithmic scan-to-scan alignment requires a registration of corresponding
innovations that make real-time localization with dense LiDAR points between two clouds, but this process often involves a
scans feasible while also demonstrating the superiority of our nearest-neighbor search which grows exponentially with the
method in terms of accuracy and computational complexity number of points per scan. Feature-based methods [2], [4]–
when compared to the state-of-the-art. [6] attempt to mitigate this by using only the most salient
Current LO algorithms estimate a robot’s egomotion in two points, but these methods employ an often computationally-
stages: first, by performing a “scan-to-scan” alignment be- intensive feature extraction step and may accidentally dis-
tween adjacent LiDAR frames to recover an immediate motion card data which could otherwise help improve the quality
Manuscript received: September 9, 2021; Revised December 1, 2021; of downstream registration. Moreover, in scan-to-map align-
Accepted December 23, 2021. ment, keyed environmental history (which consists of all or
This paper was recommended for publication by Editor Sven Behnke upon a subset of past points) grows rapidly in size as new scans
evaluation of the Associate Editor and Reviewers’ comments. This work was
partially conducted at NASA JPL in support of the DARPA Subterranean are acquired and stored in memory. While aligning with a
Challenge. submap (rather than the full history of scans) helps increase
1 Kenny Chen and Ankur Mehta are with the Department of Electrical and
computational efficiency, the perpetual addition of points still
Computer Engineering, University of California Los Angeles, Los Angeles,
CA, USA. {kennyjchen, mehtank}@[Link] significantly expands the nearest-neighbor search space for
2 Brett T. Lopez is with the Department of Mechanical and Aerospace typical submap extraction methods. Tree-based data structures
Engineering, University of California Los Angeles, Los Angeles, CA, USA. have been shown to decrease this nearest-neighbor search cost
btlopez@[Link]
3 Ali-akbar Agha-mohammadi is with the NASA Jet Propulsion significantly [7], but the extraction of a local submap still
Laboratory, California Institute of Technology, Pasadena, CA, USA. involves too many points after just a few keyframes, thus
aliagha@[Link] preventing consistent performance for long-term navigation.
Digital Object Identifier (DOI): see top of this page.
2 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2022

Fig. 2. LiDAR odometry architecture. Our system first retrieves a relative transform between two temporally-adjacent scans of times k and k − 1 through
scan-to-scan (S2S) matching with RANSAC outlier rejection and an optional rotational prior from IMU. This initial estimate is then propagated into the world
frame and used as the initialization point for our secondary GICP module for scan-to-map optimization (S2M), which scan-matches the current point cloud
Pk with a derived submap Sk consisting of scans from nearby and boundary keyframes. The output of this is a globally-consistent pose estimate which is
subsequently checked against several metrics to determine if the current pose should be stored as a new keyframe.
In this letter, we present our Direct LiDAR Odometry [10] or learned networks [11] and might consist of planes [5],
(DLO) algorithm, a high-speed and computationally-efficient lines and edges [4], [6], or ground points [2]. These works
frontend localization solution which permits the direct use of aim to translate insights gained from visual odometry (VO)
dense point cloud scans without significant preprocessing. The techniques into the 3D domain. However, adding this step
main contribution of this work is a custom speed-first pipeline increases computational overhead and risks discarding data
which accurately resolves robot egomotion in real-time using points which could help with better correspondence matching
minimally-preprocessed LiDAR scans and an optional IMU for odometry accuracy. Alternatively, direct methods attempt
on consumer-grade processors. A key insight of our work to align dense point clouds but must heavily downsample to
is the link between algorithmic speed and accuracy, and achieve computational tractability [12], [13]. More recently,
our approach is comprised of three core innovations. First, a recursive filtering framework, e.g. Kalman filter, has been
an adaptive keyframing system which efficiently captures proposed [14], [15] to achieve real-time performance but at
significant environmental information through a novel spa- the potential expense of estimation accuracy.
ciousness metric. Second, a fast keyframe-based submapping A second stage immediately following scan alignment
approach via convex optimization which generates permissive between adjacent clouds has been shown to reduce global
local submaps for global pose refinement. Third, NanoGICP, drift by increasing pose estimation consistency with previous
a custom iterative closet point solver for lightweight point past scans [3], [13]. In the scan-to-map stage, a scan-to-
cloud scan-matching with data structure recycling to elim- scan transformation is further refined by aligning the current
inate redundant calculations. Our method has been exten- point cloud with an existing in-memory map; this submap is
sively evaluated in numerous challenging environments on typically derived by retrieving nearby map points within some
computationally-limited robotic platforms as part of Team radius of the robot’s current position. However, this search in
CoSTAR’s research and development efforts for the DARPA “point-space” can quickly explode in computational expense
Subterranean Challenge, and we have open-sourced our code due to the sheer number of operations needed to retrieve the
for benefit of the community1 . nearest neighbor data points. While there exists techniques
to mitigate this such as only incrementally storing map data
Related Work at keyed locations [6], this search still involves thousands of
calculations which can increase overall processor load and
LiDAR-based odometry is typically cast as a nonlinear
hence the potential to drop frames.
optimization problem to calculate a best-fit homogeneous
transform that minimizes the error across corresponding, i.e., To address these issues, our DLO algorithm is built around
matching, points and/or planes between two point clouds. a “speed-first” philosophy to permit the use of minimally-
Since correspondences are not known a priori, techniques preprocessed point clouds and provide accurate pose esti-
such as the iterative closest point (ICP) algorithm [8] or mates even for robots with limited computational resources
other variants like Generalized ICP (GICP) [9] have become (Fig. 2). The key contribution of our work lies in how we
the standard to align two point clouds; however, searching efficiently derive our submap for global refinement in scan-
over all data points can be computationally costly. Feature- to-map matching. That is, rather than extracting points within
based methods attempt to extract and use only the most a local vicinity of a robot’s current position as most works
salient points before scan-matching to reduce computation. do, DLO instead searches in keyframe-space by associating a
Such features are found either via manually tuned methods scan’s set of points with its corresponding keyframe position.
The submap is subsequently constructed by concatenating the
1 [Link] lidar odometry clouds from a subset of historic keyframes derived from nearby
CHEN et al.: DIRECT LIDAR ODOMETRY: FAST LOCALIZATION WITH DENSE POINT CLOUDS 3

keyframes and those which make up the convex hull; this Algorithm 1: Direct LiDAR Odometry
provides the current scan with both nearby and distant points W W
1 input: Pk , X̂k−1 ; initialize: X̂k−1 = I or gravityAlign()
in the submap to anchor to. In addition, a custom GICP solver W
2 output: X̂k , Mk
enables extensive reuse of data structures across multiple 3 while Pk 6= ∅ do
solver instantiations to eliminate redundant operations across // preprocessing
the two-stage process. Our system also optionally accepts 4 P̄k ← preprocessPoints(Pk ) ;
5 computeAdaptiveParameters(P̄k ) ;
an initialization prior from an IMU in a loosely-coupled // initialization
fashion to further improve accuracy during aggressive rota- 6 if k = 0 then
tional motions. The reliability of our approach is demonstrated 7 Tkt1 , Ckt1 ← NanoGICP1 .build(P̄k ) ;
W
through extensive tests on several computationally-limited 8 Kk ← updateKeyframeDatabase(X̂k−1 , P̄k ) ;
9 continue;
robotic platforms in multiple challenging environments. This 10 end
work was part of Team CoSTAR’s research and development // prior
L B L
efforts for the DARPA Subterranean Challenge in support of 11 if IMU then X̃k ← X̃k ; else X̃k ← I ;
// scan-to-scan
NASA Jet Propulsion Laboratory’s Networked Belief-aware 12 Tks1 , Cks1 ← NanoGICP1 .build(P̄k ) ;
Perceptual Autonomy (NeBula) framework [16], in which 13
L L
X̂k ← NanoGICP1 .align(Tks1 , Tkt1 , Cks1 , Ckt1 , X̃k ) ;
DLO was the primary state estimation component for our fleet W W L
14 X̃k ← X̂k−1 X̂k ;
of autonomous aerial vehicles (Fig. 1A). // scan-to-map
W
15 Qk ← getKeyframeNeighbors(X̂k−1 , Kk ) ;
W
II. M ETHODS 16 Hk ← getKeyframeHulls(X̂k−1 , Kk ) ;
17 Sk ← Qk ⊕ Hk ;
A. Notation
18 if Sk 6= Sk−1 then Tkt2 ← NanoGICP2 .build(Sk ) ; else
A point cloud, P, is composed of a set of points p ∈ P with Tkt2 ← Tk−1
t2
;
Cartesian coordinates pi ∈ R3 . We denote L as the LiDAR’s Tk ← Tk ; Cks2 ← Cks1 ; Ckt2 ← N
s2 s1 S
P
19 n Ck,n ;
W W
coordinate system, B as the robot’s coordinate system located 20
s2 t2 s2
X̂k ← NanoGICP2 .align(Tk , Tk , Ck , Ckt2 , X̃k ) ;
at the IMU frame, and W as the world coordinate system // update keyframe database and map
W
which coincides with B at the initial position. Note that in 21 Kk ← updateKeyframeDatabase(X̂k , P̄k ) ;
22 Mk ← Mk−1 ⊕ {Kk \ Kk−1 } ;
this work we assume L and B reference frames coincide. // propagate data structures
Submap, covariance, and kdtree structures are denoted as S, C 23 Tkt1 ← Tks1 ; Ckt1 ← Cks1 ;
W
and T , respectively. We adopt standard convention such that 24 return X̂k , Mk
x points forward, y points left, and z points upward, and 25 end
our work attempts to address the following problem: given
adjacent point clouds scans Pk and Pk−1 at time k, estimate an SE(3) transformation, which translates to the robot’s 6-
W
the robot’s current pose X̂k ∈ SE(3) and map Mk in W. DOF motion between consecutive LiDAR acquisitions. This
process is typically performed in two stages, first to provide a
B. Preprocessing best instantaneous guess, which is subsequently refined to be
more globally consistent with previous keyframe locations.
Our system assumes an input of 3D point cloud data
gathered by a 360◦ LiDAR such as an Ouster OS1 (20Hz) 1) Scan-to-Scan: In the first stage, the scan-to-scan match-
L
or a Velodyne VLP-16 (10Hz). To minimize information loss ing objective is to compute a relative transform X̂k between
s t t s
from the raw sensor data, only two filters are used during a source Pk and a target Pk (where Pk = Pk−1 ) captured in
preprocessing: first, we remove all point returns that may be L where
L
X̂k = arg min E XL s t


from the robot itself through a box filter of size 1m3 around k Pk , Pk . (1)
XL
the origin. This is especially important if an aerial robot’s k

propellers (Fig. 1A) or protective guards (Fig. 1B) are in the The residual error E from GICP is defined as
LiDAR’s field of view. The resulting cloud is then sent through N  −1
L>

X
a 3D voxel grid filter with a resolution of 0.25m to lightly E XL P
k k
s
, P t
k = d>
i C t
k,i + X L s
C X
k k,i k di , (2)
downsample the data for subsequent tasks while maintaining i
dominate structures within the surrounding environment. Note such that the overall objective for this stage is
that in this work we do not correct for motion distortion since
N −1
non-rigid transformations can be computationally burdensome, L X 
L>
X̂k = arg min d>
i
t
Ck,i + XL s
k Ck,i Xk di , (3)
and we directly use the dense point cloud rather than extracting XL
k i
features as most works do. On average, each cloud contains
∼10,000 points after preprocessing. for N number of corresponding points between point clouds
Pks and Pkt , where di = pti − XL s s s t t
k pi , pi ∈ Pk , pi ∈ Pk , ∀i, and
s t
Ck,i and Ck,i are the corresponding estimated covariance ma-
C. Scan Matching via Generalized-ICP trices associated with each point i of the source or target cloud,
LiDAR-based odometry can be viewed as the process of respectively. As will be further discussed in Section II-D, we
resolving a robot’s egomotion by means of comparing suc- can initialize the above objective function with a prior supplied
cessive point clouds and point clouds in-memory to recover by external sensors in an attempt to push the convergence
4 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2022

B
towards a global minimum. That is, for Eq. (3), if a prior X̃k
is available by means of IMU preintegration, we can set the
L B
initial guess X̃k = X̃k to create a loosely-coupled system. If
a prior is not available however, the system reverts to pure
L
LiDAR odometry in which X̃k = I and relies solely on point
cloud correspondence matching for this step.
2) Scan-to-Map: After recovering an initial robot motion
estimate, a secondary stage of scan-to-map matching is per-
formed and follows a similar procedure to that of scan-to-
scan. However, rather than computing a relative transform
between two instantaneous point clouds, the objective here
is to further refine the motion estimate from the previous step
to be more globally-consistent by means of matching with a
local submap. In other words, the task here is to compute an
W
optimal transform X̂k between the current source cloud Pks
and some derived submap Sk such that
W Fig. 3. Keyframe-based submapping. A comparison between the different
X̂k = arg min E XW s


k Pk , Sk . (4) submapping approaches, visualizing the current scan (white), the derived
XW
k submap (red), and the full map (blue). (A) A common radius-based submap-
ping approach of r = 20m retrieved in point cloud-space. (B) Our keyframe-
After similarly defining the residual error E from GICP as in based submapping approach, which concatenates a subset of keyed scans and
Eq. (2), the overall objective function for scan-to-map is helps anchor even the most distant points in the current scan (green box)
during the scan-to-map stage.
M −1
W

W>
X
X̂k = arg min d>
j C S
k,j + X W s
C X
k k,j k dj , (5) qk+1 = qk + ( 21 qk ⊗ ω k )∆t. Here, qk is initialized to identity
XW
k j prior to integration, ∆t is the difference in time between IMU
for M number of corresponding points between point cloud Pks measurements in seconds, and only gyroscopic measurements
and submap Sk , where Ck,jS
is the corresponding scan-stitched found between the current LiDAR scan and the previous one
covariance matrix for point j in the submap as defined later in are used. Note that we are only concerned with a rotational
Section II-F. Eq. (5) is initialized using the propagated result prior during IMU preintegration and leave the retrieval of a
from scan-to-scan in the previous section from L to W, i.e. translational prior via the accelerometer for future work. The
W W L resulting quaternion of this propagation is converted to an
X̃k = X̂k−1 X̂k , so that this prior motion can be compared
SE(3) matrix with zero translational component to be used
against historical map data for global consistency. The output B
W as X̃k , the scan-to-scan prior.
of this stage X̂k is the final estimated robot pose used for
downstream modules.
We note here that a key innovation of this work is how E. Fast Keyframe-Based Submapping
we derive and manage our submap for this stage. Whereas
previous works create a submap by querying the locality of A key innovation of this work lies in how our system
each individual point in a stored map, we associate scans to manages map information and derives the local submap in
keyframes and search rather in keyframe-space to stitch point scan-to-submap matching for global egomotion refinement.
clouds together and create Sk . The implications of this include Rather than working directly with point clouds and storing
a far faster and more consistent generation of a local submap, points into a typical octree data structure, we instead keep a
which is additionally more permissive as compared to a radius- history of keyframes to search within, in which each keyframe
based search and will be further discussed in Section II-E. is linked to its corresponding point cloud scan in a key-
value pair. The resulting local submap used for scan-to-submap
matching is then generated by concatenating the corresponding
D. Optimization Prior point clouds from a subset of the keyframes, rather than
Eq. (3) describes the scan-to-scan nonlinear optimization directly retrieving local points within some radius of the
problem and can be initialized with a prior to reduce the robot’s current position.
chances of converging into a sub-optimal local minima. This The implication of this design choice is twofold: first,
prior represents an initial guess of the relative motion between by searching in “keyframe-space” rather than “point cloud-
two LiDAR frames and can come from integrating angular space,” a much more computationally tractable problem is
velocity measurements from an inertial measurement unit obtained. Radius-based searches within a cumulative point
(IMU). More specifically, angular velocity measurements ω̂ k cloud map can require distance calculations against hundreds
is defined as ω̂ k = ω k + bω ω
k + nk measured in B with of thousands of points — a process that quickly becomes
ω
static bias bk and zero white noise nω k for convenience. infeasible even with an incremental octree data structure.
After calibrating for the bias, a relative rotational motion of Searching against keyframes, however, typically involves only
the robot’s body between two LiDAR frames can be com- a few hundred points even after long traversals and provides
puted via gyroscopic propagation of the quaternion kinematics much more consistent computational performance, reducing
CHEN et al.: DIRECT LIDAR ODOMETRY: FAST LOCALIZATION WITH DENSE POINT CLOUDS 5

TABLE I
S UMMARY OF DATA S TRUCTURE R ECYCLING
Element Scan-to-Scan Scan-to-Map
reuse from S2S
Tksource build −−−−−−−−→
Tktarget source
Tk−1 build when Sk 6= Sk−1
reuse from S2S
Cksource compute −−−−−−−−→
PN S
Cktarget source
Ck−1 n Ck,n

used once in the submap.

2) Adaptive Keyframing: The location of keyframes af-
fects the derived submap and can subsequently influence
accuracy and robustness of the odometry. Keyframe nodes
are commonly dropped using fixed thresholds (e.g., every
1m or 10◦ of translational or rotational change) [4], [6],
[13], but the optimal position can be highly dependent on
Fig. 4. Keyframe selection and adaptive thresholds. (A) Our method’s a surrounding environment’s structure. More specifically, in
submap (red) is generated by concatenating the scans from a subset of
keyframes (green spheres), which consists of K nearest neighbor keyframes large-scale settings, features captured by the point cloud
and those that construct the convex hull of the keyframe set. (B) An scan are much more prominent and can be depended on for
illustration of adaptive keyframing. In this scenario, the threshold decreases longer periods of time. Conversely, for narrow or small-scale
when traversing down a narrow ramp to better capture small-scale details.
environments, a smaller threshold is necessary to continually
the chances of dropping frames. Additionally, a keyframe- capture the small-scale features (i.e., tight corners) in the
based approach constructs a much more permissive submap submap for better localization. Thus, we choose to scale the
as compared to range-based methods. That is, since the size translational threshold for new keyframes according to the
of a submap derived from keyframe point clouds relies solely “spaciousness” in the instantaneous point cloud scan, defined
on the LiDAR sensor’s range rather than a predetermined as mk = αmk−1 + βMk , where Mk is the median Euclidean
distance, the derived submap can have a larger overlap with point distance from the origin to each point in the preprocessed
the current scan; this is illustrated in Fig. 3. In this example, point cloud, α = 0.95, β = 0.05, and mk is the smoothed signal
a submap of fixed radius r = 20m insufficiently overlaps with used to scale the keyframe threshold thk at time k such that
the current scan and can introduce drift over time due to 
containing only spatially-nearby points; however, a keyframe- 
 10m if mk > 20m

based approach covers most of the current scan which helps 5m if mk > 10m & mk ≤ 20m
thk = (6)
with better scan-to-map alignment. Expanding the radius size 
 1m if mk > 5m & mk ≤ 10m
may help increase this overlap for radius-based methods, but 

0.5m if mk ≤ 5m
doing so would significantly slowdown subsequent tasks such
as the GICP covariance calculations. with rotational threshold held fixed at 30◦ . Fig. 4B illustrates
the effects of this adaptive thresholding, which helps with
1) Keyframe Selection via kNN and Convex Hull: To con-
robustness to changing environmental dimension.
struct the submap Sk , we concatenate the corresponding point
clouds from a selected subset of environmental keyframes.
Let Kk be the set of all keyframe point clouds such that F. Algorithmic Implementation
Sk ⊆ Kk . We define submap Sk as the concatenation of K 1) Scan-Stitched Submap Normals: Generalized-ICP in-
nearest neighbor keyframe scans Qk and L nearest neighbor volves minimizing the plane-to-plane distance between two
convex hull scans Hk such that Sk = Qk ⊕ Hk , where the clouds, in which these planes are modeled by a computed
indices which specify the convex hull are defined by the set of covariance for each point in the scan. Rather than computing
keyframes which make up the intersection of all convex sets the normals for each point in the submap on every iteration
containing the keyframes which compose Kk . (which can be infeasible for real-time operation), we assume
The result of this is illustrated in Fig. 4A, in which the that the set of submap covariances CkS can be approximated
S
keyframes highlighted in green are those that compose the by concatenating the normals Ck,n from N keyframes which
PN S
extracted submap, indicated in red. Intuitively, extracting near- populate the submap such that CkS ≈ n Ck,n . As a conse-
est neighbor keyframes aims to help with overlap of nearby quence, each submap’s set of normals need not be explicitly
points in the current scan, while those from the convex hull — computed, but rather just reconstructed by stitching together
which contain boundary map points — increase the overlap those calculated previously.
with more distant points in the scan. This combination reduces 2) Data Structure Recycling: Expanding on the above,
overall trajectory drift by maximizing scan-to-map overlap and several algorithmic steps in current LiDAR odometry pipelines
provides the system with multiple scales of environmental can benefit from data structure sharing and reuse, drastically
features to align with. Note that keyframes which are classified reducing overall system overhead by removing unnecessary
as both a nearest neighbor and a convex hull index are only and redundant operations. As summarized in Table I, the
6 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2022

Fig. 7. Average convergence time. A comparison of average convergence

times across 100 benchmark alignments for each algorithm, including our
NanoGICP solver and two other open-source GICP packages.
Fig. 5. Alpha course map. Different views and angles of the dense 3D point
cloud map generated using our DLO algorithm on the Urban Alpha dataset.
Estimated positions at each timestamp were used to transform the provided
scan into a world frame; this was performed for all scans across the dataset
and concatenated / voxel filtered to generated the above images.

Fig. 8. Ablation study of data recycling schemes. Box plots of the

processing time and CPU usage for four different data recycling schemes,
ranging from no data structure reuse to partial reuse and full reuse.
Fig. 6. Error comparison. The absolute pose error plotted across a 1200s TABLE II
window of movement, showing the difference between radius and keyframe D ROPPED L I DAR S CANS PER R ECYCLING S CHEME
submapping schemes. Keyframe-based approaches do not have the range
restriction that radius-based approaches inherently contain, which directly
translates to a lower error in odometry due to more perceptive submapping. None KDTrees Covariances Both
Note that adaptive keyframing primarily helps with reliability and robustness % Scans 9.37% 4.51% 0.00% 0.00%
to changes in environmental dimension (Fig. 9).

system requires eight total elements to successfully perform kdtree data structures, which are subsequently used for point
scan-to-scan and scan-to-map matching. This includes kdtrees cloud correspondence matching by FastGICP. In practice, data
Tk used to search for point correspondences and covariance structure sharing is performed between two separate NanoG-
matrices Ck for GICP alignment for both source and target ICP instantiations with different hyperparameters — one to
clouds in each scan-matching process. target each scan-matching problem — and done procedurally
Out of the four required kdtrees data structures, only two as detailed in Algorithm 1.
need to be built explicitly. That is, the tree for the source
(input) cloud Tksource can be built just once per scan acquisition
III. R ESULTS
and shared between both modules (as the same scan is used
for both sources). For the scan-to-scan target tree Tktarget , this A. Component Evaluation
is simply just the previous iteration’s scan-to-scan source tree To investigate the impact of our system’s components,
source
Tk−1 and thus can be propagated. The scan-to-map target tree including keyframe-based submapping, submap normal ap-
needs to be built explicitly, but since the submap is derived proximation, and the reuse of data structures, we compare each
from a set of keyframes, this build only needs to be performed component with its counterpart using the Alpha Course dataset
when the set of selected keyframes via our kNN and convex from the Urban circuit of the DARPA Subterranean Challenge.
hull strategy changes from one iteration to the next, such that This dataset contains LiDAR scans from a Velodyne VLP-16
Sk 6= Sk−1 . Otherwise, the data structure can just be reused sensor, in addition to IMU measurements from a VectorNav
again for additional computational savings. Point covariances VN-100, collected across 60 minutes in an abandoned power-
Ck needed for GICP, on the other hand, only need to be plant located in Elma, WA which contains multiple perceptual
computed once per scan aquisition, and its data can be shared challenges such as large or self-similar scenes (Fig. 5). For
directly in the other three instances. these component-wise evaluations, data was processed using
3) Dual NanoGICP: To facilitate the cross-talking between a 4-core Intel i7 1.30GHz CPU.
scan-matching modules, we developed NanoGICP, a custom it- 1) Keyframe-Based Submapping: We compared the abso-
erative closest point solver which combines the FastGICP [17] lute pose error (APE), processing time, and CPU load across
and NanoFLANN [18] open-source packages with additional three submapping schemes, including: radius-based (r = 10m),
modifications for data structure sharing as described before. In keyframe-based with a 1m static threshold, and keyframe-
particular, NanoGICP uses NanoFLANN to efficiently build based with adaptive thresholding. For keyframe-based variants,
CHEN et al.: DIRECT LIDAR ODOMETRY: FAST LOCALIZATION WITH DENSE POINT CLOUDS 7

TABLE III
C OMPARISON ON B ENCHMARK DATASETS
Alpha Course (757.4m) Beta Course (631.5m) CPU Usage
Method APE [m] ME [m] APE [m] ME [m] No. of Cores
max mean std rmse max mean std rmse max mean
BLAM [12] 3.44 1.01 0.94 0.43 3.89 2.27 0.89 1.27 1.14 0.93
Cartographer [19] 5.84 2.91 1.60 1.05 2.64 1.37 0.67 0.31 1.75 0.88
LIO-Mapping [5] 2.12 0.99 0.51 0.45 1.60 1.18 0.22 0.61 1.80 1.53
LOAM [10] 4.33 1.38 1.19 0.60 2.58 2.11 0.44 0.99 1.65 1.41
LOCUS [13] 0.63 0.26 0.18 0.28 1.20 0.58 0.39 0.48 3.39 2.72
DLO 0.40 0.18 0.06 0.19 0.50 0.16 0.09 0.19 0.92 0.62

each kdtree and cloud covariance, computation time exceeded

LiDAR rate (10Hz for Velodyne) with a high average of
69.8ms per scan and nearly 10% of scans dropped due to
high processing time. Recycling kdtrees but not covariances
provides a slight improvement in processing time and CPU
percentage, while recycling covariances but not kdtrees pro-
vides a more prominent performance boost; this is reasonable
since our covariance recycling scheme is more aggressive than
kdtree reusage. Finally, using the full scheme as detailed in
Table I significantly decreases both metrics, with an average
processing time of 21.9ms and 9.5% CPU load, which prevents
any LiDAR frames from dropping.
3) NanoGICP: To compare NanoGICP with the state-of-
the-art, we use FastGICP’s [17] benchmark alignment code
found in the authors’ open-source repository. This benchmark
measures the average convergence time to align two LiDAR
scans across 100 runs, and we compare against PCL’s [20]
GICP implementation as well as FastGICP’s multithreaded
Fig. 9. Extreme environments. Top: A section of an underground mine in implementation. Note that we do not compare against the
Lexington, KY mapped autonomously using our custom drone while running voxelized FastGICP variant, since this method approximates
DLO. This environment contained challenging conditions such as: (A) low planes with groups of planes and decreases overall accuracy.
illuminance, (B) object obstructions, and (C) wet and muddy terrain. Bottom:
Top-down (D) and side (E) views of the three levels of an abandoned subway All tested algorithms were initialized with an identity prior,
located in Downtown Los Angeles, CA mapped via DLO using a Velodyne and as shown in Fig. 7, we observed that NanoGICP converged
VLP-16 on a quadruped. In this run, we manually tele-operated the legged faster on average (42.53ms) when compared to FastGICP
robot to walk up, down, and around each floor for a total of 856m.
(72.88ms) and PCL’s GICP (178.24ms).
we used 10 nearest-neighbor and 10 convex hull keyframes
for submap derivation. From Fig. 6, the influence of our
B. Benchmark Results
approach is clear: submapping in keyframe-space can signif-
icantly reduce positional error by considering more distant The odometry accuracy and CPU load of DLO was com-
points that would otherwise be outside the scope of a radius- pared to several LiDAR and LiDAR-IMU odometry methods
based approach. These additional points influence the outcome — including BLAM [12], Cartographer [19], LIO-Mapping
of the GICP optimization process as they are considered [5], LOAM [10], and LOCUS [13] — using the Alpha and
during error minimization for the optimal transform; this is Beta course dataset from the Urban Circuit of the Subterranean
especially important in purely frontend-based odometry, since Challenge (numbers and ground truth retrieved from [13]). We
any additional error in pose can quickly propagate over time note that LIO-SAM [6] and LVI-SAM [4], two state-of-the-
due to drift. Processing time and CPU load showed similar art tightly-coupled approach, could not be tested at the time of
trends: radius-based processed each scan notably slower at this work due to their sensitive calibration procedure and strict
74.2ms per scan with an average of 37.5% CPU load as input data requirements. We observed that our method’s CPU
compared to 21.6ms / 10.2% and 19.1ms / 9.1% for static load was measured to be far lower than any other algorithm,
and adaptive schemes, respectively. using less than one core both on average and at its peak.
This is likely a result how our system derives its submap,
2) Data Structure Recycling: To evaluate the effectiveness in addition to the extensive reuse of internal data structures.
of data reusage, we measured and compared the processing This observation can also explain DLO’s much lower absolute
time and CPU usage between different recycling schemes via pose error (APE) and mean error (ME), with similar trends
a box plot (Fig. 8) and percentage of dropped scans over the in the relative pose error. With this faster processing time,
dataset (Table II). In a naive system which explicitly calculates our method outperformed all other methods in both Alpha
8 IEEE ROBOTICS AND AUTOMATION LETTERS. PREPRINT VERSION. ACCEPTED JANUARY, 2022

between system modules, all of which is facilitated using our

custom NanoGICP cloud registration package. We demonstrate
the reliability of our approach through benchmarks and exten-
sive field experiments on multiple platforms operating in large-
scale perceptually-challenging environments, and we invite
others to use and evaluate our open-source code. DLO was
developed for and used on NASA JPL’s Team CoSTAR’s fleet
of quadrotors in the DARPA Subterranean Challenge (Fig. 10),
and in the future we are interested in tighter IMU integration
as well as motion distortion correction.
Acknowledgements: The authors would like to thank Team
CoSTAR teammates and colleagues, including Amanda
Bouman, Luca Carlone, Micah Corah, Kamak Ebadi, Seyed
Fakoorian, David Fan, Sung Kim, Benjamin Morrell, Joshua
Fig. 10. Mega Cavern. Different views of the Mega Cavern in Louisville, KY
mapped by our DLO algorithm, with a total estimated trajectory of 9057.66m. Ott, Andrzej Reinke, Toni Rosinol, and Patrick Spieler, for
Data is courtesy of Team Explorer. their valuable insight and productive discussions.
and Beta courses, having more than twice the accuracy in R EFERENCES
the Beta course for max, mean and standard deviation, even
[1] C. Cadena, L. Carlone et al., “Past, present, and future of simultaneous
without motion distortion correction. In addition to our more localization and mapping: Toward the robust-perception age,” IEEE
permissive submapping approach, we are less likely to drop Transactions on Robotics, 2016.
frames than other methods and have the processing capital to [2] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized
lidar odometry and mapping on variable terrain,” in International
match the dense point clouds at a higher resolution. Conference on Intelligent Robots and Systems, 2018.
[3] K. Ebadi, Y. Chang et al., “Lamp: Large-scale autonomous mapping
and positioning for exploration of perceptually-degraded subterranean
C. Field Experiments environments,” in IEEE International Conference on Robotics and
We additionally tested and implemented our solution on Automation, 2020.
[4] T. Shan, B. Englot et al., “Lvi-sam: Tightly-coupled lidar-visual-inertial
several custom robotic platforms for real-world field opera- odometry via smoothing and mapping,” in IEEE International Confer-
tion. Specifically, we integrated DLO onto an aerial vehicle ence on Robotics and Automation, 2021.
(Fig. 1A) with an Ouster OS1 and a Boston Dynamics Spot [5] H. Ye, Y. Chen, and M. Liu, “Tightly coupled 3d lidar inertial odometry
and mapping,” in International Conference on Robotics and Automation,
(Fig. 1B) with a Velodyne VLP-16. Both systems contained 2019.
a VectorNav VN-100 IMU rigidly mounted below the base [6] T. Shan, B. Englot et al., “Lio-sam: Tightly-coupled lidar inertial odom-
of the LiDAR and processed data on an Intel NUC Board etry via smoothing and mapping,” in IEEE/RSJ International Conference
on Intelligent Robots and Systems, 2020.
NUC7i7DNBE 1.9GHz CPU. We conducted both manual [7] N. Bhatia, “Survey of nearest neighbor techniques,” International Jour-
and autonomous traversals in two perceptually-challenging nal of Computer Science and Information Security, 2010.
environments: in an underground limestone cave in Lexington, [8] Y. Chen and G. Medioni, “Object modelling by registration of multiple
range images,” Image and Vision Computing, 1992.
KY and at an abandoned subway in Los Angeles, CA (Fig. 9). [9] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics:
Both locations contained environmental properties which often Science and Systems (RSS), 2009.
challenge perceptual systems, including poor lighting condi- [10] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-
time.” in Robotics: Science and Systems, 2014.
tions, featureless corridors, and the presence of particulates [11] Z. J. Yew and G. H. Lee, “3dfeat-net: Weakly supervised local 3d
such as dust or fog. Despite traversing over 850m across features for point cloud registration,” in Proceedings of the European
three different levels in the abandoned subway, our system Conference on Computer Vision, 2018.
[12] E. Nelson, “B(erkeley) l(ocalization) a(nd) m(apping).” [Online].
reported only a 10cm end-to-end drift, largely owing to DLO’s Available: [Link]
robust keyframing scheme which adapted to large and small [13] M. Palieri, B. Morrell et al., “Locus: A multi-sensor lidar-centric
spaces. Our tests in the underground mine showed similar solution for high-precision odometry and 3d mapping in real-time,” IEEE
Robotics and Automation Letters, 2020.
promise: while this environment lacked any external lighting [14] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry
deep within the cave, DLO could still reliably track our package by tightly-coupled iterated kalman filter,” IEEE Robotics and
aerial vehicle across 348m of autonomous flight. These results Automation Letters, 2021.
[15] W. Xu, Y. Cai et al., “Fast-lio2: Fast direct lidar-inertial odometry,”
demonstrate the real-world reliability of our method. arXiv preprint arXiv:2107.06829, 2021.
[16] A. Agha, K. Otsu et al., “Nebula: Quest for robotic autonomy in chal-
IV. C ONCLUSION lenging environments; team costar at the darpa subterranean challenge,”
Journal of Field Robotics, 2021.
This work presented Direct LiDAR Odometry (DLO), a [17] K. Koide, M. Yokozuka et al., “Voxelized gicp for fast and accurate 3d
lightweight and accurate frontend localization solution with point cloud registration,” in IEEE International Conference on Robotics
and Automation, 2021.
minimal computational overhead for long-term traversals in [18] J. L. Blanco and P. K. Rai, “nanoflann: a C++ header-only fork of
extreme environments. A key innovation which distinguishes FLANN, a library for nearest neighbor (NN) with kd-trees,” https://
our work from others is how we efficiently derive a lo- [Link]/jlblancoc/nanoflann, 2014.
[19] W. Hess, D. Kohler et al., “Real-time loop closure in 2d lidar slam,” in
cal submap for global pose refinement using a database of IEEE International Conference on Robotics and Automation, 2016.
keyframe-point cloud pairs. This in turn permits a substantial [20] R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),”
number of solver data structures to be shared and reused in IEEE International Conference on Robotics and Automation, 2011.