Professional Documents
Culture Documents
1558-1748 © 2021 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See https://www.ieee.org/publications/rights/index.html for more information.
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
CHEN et al.: NDT-LOAM: REAL-TIME LIDAR ODOMETRY AND MAPPING WITH WEIGHTED NDT AND LFA 3661
correspondences of an intermediate representation from raw a direct method. In this approach, loop closure detection and
sensor measurements, and then estimate sensor motion by graph optimization can correct the accumulated error and
minimizing the re-projection error. The precomputation step of improve the mapping accuracy. But these calculations are too
correspondences is typically solved by extracting and matching time-consuming for limited CPU to correct location error in
a sparse/dense set of key points (or feature points) and the real-time.
parametric representations of other geometric primitives, such LOAM developed by Carnegie Mellon University (CMU)
as line or curve segments. Direct methods skip this precom- have been ranked high on the KITTI odometry benchmark
putation step and directly employ the sensor measurements for several years. It divides the SLAM problem into two
(photometric measurements of cameras or geometric quantities modules. One module performs odometry at a high frequency
of Lidars), jointly estimating motion and correspondences. but with low accuracy to estimate velocity of the laser scanner.
Compared with camera, Lidar can acquire huge-scale and Although not necessary, if an IMU is available, it can provide
high-accuracy measurements without being affected by the a motion prior and mitigate for gross, high-frequency motion.
lighting conditions [21]. Lidar has become a vitally important A second module runs at a frequency of an order of magnitude
sensor in SLAM applications recently. In this work we focus lower for fine matching and registration of the point cloud.
on Lidar Odometry, we present a real-time Lidar odometry Specifically, the correspondences are previously found and
(NDT-LOAM) using an improved weighted normal distribu- determined in both modules, which extract feature points
tions transform (NDT) and a keyframe matching strategy. The located on sharp edges and planar surfaces, and match the fea-
cells in NDT are treated with different weights based on their ture points to edge line segments and planar surface patches,
ranges and surface features in the object function of NDT. respectively. Obviously, LOAM belongs to the feature-based
A simple heuristic is further employed to select the ktyyframe. methods. Combination of the two modules allows the map
After compared with state-of-the-art method Lidar Odometry creation in real-time manner. However, to reach the possible
And Mapping (LOAM) [15], the proposed method shows a maximum accuracy on the KITTI odometry benchmark, the
satisfactory performance on both efficiency and accuracy. Lidar mapping runs actually at the same frequency as the Lidar
odometry and processes each individual scan, running at 1 HZ,
II. R ELATED W ORK 10% of the sensor’s real-time speed. Moreover, researchers
Lidar has obvious advantages in environment sensing, and have implemented some LOAM variants, including LeGO-
it has attracted wide attention in SLAM. Researchers has LOAM [18] with ground-optimized process, LIO-SAM [19]
published extensive works on Lidar SLAM including improv- with tightly-coupled Lidar inertial odometry, Loam_livox [18]
ing its efficiency, reducing the accumulated errors and map with a solid-state Lidar Livox, R-LOAM [27] with point-to-
optimization. Before the popularity of Graph-based SLAM, mesh features, F-LOAM1 , A-LOAM2 etc.
Bayesian filter techniques is a method of choice for model Recently, IMLS-SLAM [13] is an another low-drift SLAM
acquisition in the Lidar SLAM community [22]. Bayesian- algorithm based only on 3D Lidar data. It relies on a scan-
based SLAM treats the SLAM problem as a pose state to-model matching framework. This direct method first uses
estimation problem at a mobile measurement platform, such as a specific sampling strategy based on the Lidar scans, but
Hector SLAM [23], Gmapping [24], etc. While continuously does not build an explicit correspondence relationship. It then
merging new observation data, it uses Extended Kalman Filter defines the model as the previous localized Lidar sweeps and
(EKF), Particle filter (PF) or Information Filter (IF) to enhance use the Implicit Moving Least Squares (IMLS) surface repre-
and update the pose state. Graph-based SLAM is also named sentation. This approach needs 1.3s to finish each individual
full SLAM [25], and representative open source algorithms are scan and cannot run in real time.
Karto SLAM [24], cartographer [14], etc. It constructs a graph In the concept “Simultaneous Localization and Mapping”,
whose nodes correspond to the platform poses at different the word “Simultaneous” specifically reveals the importance
points in time and whose edges represent constraints between of the real-time processing for the odometry, the frontend
the poses obtained from observations, and then the map can of SLAM, while pursing the high localization accuracy. The
be computed by finding the spatial configuration of the nodes above methods gain a higher odometry accuracy at the expense
that is mostly consistent with the measurements modeled by of more time consumption than the real-time processing. They
the edges [26]. cannot solve the contradiction between processing efficiency
For the recent advances of 3D Lidar SLAM with six and localization accuracy. The NDT-LOAM in this paper aims
degrees of freedom (6-DOF), we present three state-of- to deal with the problems of accuracy and efficiency. It utilizes
the-art approaches: Cartographer, LOAM, and IMLS-SLAM. an improved weighted NDT and a keyframe matching strategy,
Google’s Cartographer [14] combines scan-to-submap match- and achieve a low-drift, real-time and state-of-the-art SLAM
ing with loop closure detection and graph optimization. Local solution based on 3D Lidar data.
submap trajectories are created using an algorithm called
correlative scan matching (CSM). In the back-end, all scans III. M ETHODOLOGY
are matched to nearby submaps using pixel-accurate CSM A complete SLAM generally includes two parts: front-
to create loop closure constraints. The constraint graph of end odometry and back-end optimization. The paper focuses
submap and scan poses is periodically optimized in the on the front-end, and presents an efficient Lidar odometry
backend. Because CSM is based on the probability grid (NDT-LOAM). The system architecture is presented in
without the pre-computed correspondence, Cartographer is Fig. 1, and it divides the SLAM front-end problem into
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
3662 IEEE SENSORS JOURNAL, VOL. 22, NO. 4, FEBRUARY 15, 2022
B. Weighted NDT
A. Classical NDT
In Classical NDT, all cells having enough points are equally
NDT was firstly proposed by Biber and Straßer in 2003 [28] weighted to build the registration objective function. In fact,
as a method for 2D scan registration, and then was described the cells with different ranges and different dimensionality
in detail for 3D point cloud by Martin Magnusson [29]. features have different effects on calculating the transformation
NDT maps and transforms a point cloud to a piecewise parameters [15].
continuous function consisting of a set of local probability Firstly, we discuss the relationship between the long mea-
density functions (PDFs) of Gaussian normal distributions, surement range and the weight. For a long measurement range
each of which describes the surface shape in an occupied space comparing one nearby the sensor center, the same rotation
cell (a square in the 2D case, or a cube in the 3D). error can cause more significant distortions in the point cloud,
The Gaussian PDF in each cell can be interpreted as a which means that the longer the distance, the stronger the
generative process for the surface point p within a cell. constraint. Therefore, we introduce a range weight wr
In other words, it is originally assumed that the location
of p is drawn from this distribution. The mean μ and Wrk = |x k | . (7)
covariance of the distribution are computed as
where k is the index of the point.
Secondly, the NDT can also be described as a method for
1
n
μ= Pi (1) compactly representing a surface. The transformation maps a
n point cloud to a smooth surface representation, described as a
i=1
set of local PDFs. Each PDF with the mean μ and covariance
1
n
= (Pi − μ) (Pi − μ)T (2) describes the shape and dimensionality feature of a local
n−1 surface in a cell.
i=1
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
CHEN et al.: NDT-LOAM: REAL-TIME LIDAR ODOMETRY AND MAPPING WITH WEIGHTED NDT AND LFA 3663
The covariance in each cell is a symmetric positive The entries of the Hessian matrix H are
definite matrix, and it can be conducted an eigenvalue decom- δ2 F
position for the eigenvectors and eigenvalues. The eigenvalues Hi j =
δξi δξ j
are positive and ordered so that λ1 ≥ λ2 ≥ λ3 > 0 Various
geometrical parameters can be derived from the eigenvalues n
d2 T
= Wk d1 d2 · exp − Pk k−1 Pk
and eigenvectors of the covariance matrix . Several indica- 2
k=1
tors have already been proposed in [30], and the following
δ Pk δ P
· (−d2 (Pk k−1 )(Pk k−1 k )
T T
indicators have been selected to express the linear, planar, and
spherical indices within a cell [31]: δξi δξ j
δ 2 Pk δ P T δ P
∀ j ∈ [1, 3], σ j = λ j + Pk k−1
T
+ k k−1 k ) (14)
σ1 − σ2 δξi δξ j δξ j δξi
a1D =
σ1 The parameter ξ can be solved through nonlinear iterations
σ2 − σ3 of the Newton method with line search [32] by maximizing
a2D =
σ1 the object function (8),
σ3
a3D = (8) ξ ← ξ − α H −1 g (15)
σ1
and, the dimensionality index (1D, 2D or 3D) of a cell is where α is a factor determined by the line search method.
defined by:
d ∗ = arg max[ad D ] (9) C. Keyframe Strategy
d∈[1,3]
Data accumulation and temporal inference are important
The cells with different dimensionality indices have differ- steps to mitigate the drift of the odometry. Especially for a
ent impacts on NDT [13]. The more similar the local surface methodology with sparse point clouds as presented here, the
is to the 2D plane, the more accurate the local PDF reflects the matching with several frames make it less susceptible to errors,
true physical surface in point cloud. Therefore, the cells with compared with matching among consecutive frame. Therefore,
higher planar index or 2D feature contribute more to NDT we adopt a keyframe matching strategy for the Lidar odometry.
registration, which deserves a higher weight. Because of the If the current scan frame St in timestamp t is matched to the
sparse of the point cloud, it is possible that just one scan line last scan frame St −1 in timestamp t-1 by NDT registration,
is located in several cells. These cells with linear behavior or the matching strategy of the consecutive frame is demoted
1D feature cannot present the physical surface accurately and as Scan2Scan. While the current scan frame St is matched
should be distributed less weight. The weight ratio of the 1D, to the recent keyframe in timestamp t-n (n ≥1), we denote
2D and 3D feature cells is obtained from experience, and the the keyframe matching strategy as Scan2Key. It’s important
results achieve higher accuracy when the weights are set as to note that the Scan2Key strategy is just utilized in direct
follows: odometry rather than LFA, as shown in Fig. 1. In addition,
⎧
⎪ ∗
⎨ 1.25(d = 2) we do not discuss the backend of SLAM in this paper, and
W D = 1.0(d ∗ = 3) (10) the keyframes are independent of the loop closure and global
⎪
⎩ optimization. For the keyframe selection, we employ a simple
0.75(d ∗ = 1)
heuristic: if the translation relative to the last keyframe attains
Therefore, the object function of weighted NDT is to a distance threshold εdis , or the rotation relative to the
n last keyframe attains to a threshold εdeg , or the time interval
F(ξ ) = Wk · f (T (ξ, Pk )) from the last one is more than a time threshold εt , the frame
k=1 is inserted as a keyframe. In the practical experiment, the
n
d2 T distance threshold is usually tougher than the other two and it
= −Wk d1 exp − T (ξ, Pk ) − μ Pk is the first rule of the keyframe selection.
2
k=1
−1
× T (ξ, Pk ) − μ Pk (11) D. Local Feature Adjustment (LFA)
Wk = Wrk · W Dk (12) The LFA module matches features in the current scan
frame St to a surrounding point cloud map Q t −1 to further
For brevity, let Pk ≡ T (ξ, Pk )−μ Pk . In other words, Pk
is the refine the pose transformation. LFA consists of two steps:
different between the value of the point Pk after transformed feature corresponding and pose adjustment. We adopt the Lidar
by the current pose parameters and the center of the cell which mapping module of LOAM to implement LFA, and the reader
the point belongs to. The entries of the gradient vector g is can obtain the detailed description from [15].
δF In the feature corresponding, the algorithm extracts point
gi =
δξi features from the raw scan points. Point features can be
n either corner points or surface points. The above odometry
T −1 δ P d2 T −1
= Wk d1 d2 · Pk k exp − Pk k Pk (13) estimation yields a 6-DoF pose estimate, which is used as an
δξi 2 initial pose estimate for the mapping module. Using initial
k=1
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
3664 IEEE SENSORS JOURNAL, VOL. 22, NO. 4, FEBRUARY 15, 2022
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
CHEN et al.: NDT-LOAM: REAL-TIME LIDAR ODOMETRY AND MAPPING WITH WEIGHTED NDT AND LFA 3665
TABLE I
P OSITION E RROR C OMPARISON OF I NITIAL P OSE
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
3666 IEEE SENSORS JOURNAL, VOL. 22, NO. 4, FEBRUARY 15, 2022
Fig. 4. The initial pose/ odometry trajectory comparison between ALOAM and NDT-LOAM.
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
CHEN et al.: NDT-LOAM: REAL-TIME LIDAR ODOMETRY AND MAPPING WITH WEIGHTED NDT AND LFA 3667
Fig. 5. Point cloud map from NDT-LOAM and pose trajectories from LeGO-LOAM/NDT-LOAM on #00, #05 and #09 KITTI sequences.
from LeGO-LOAM. The DNT-LOAM trace is closer to the Fig. 6 shows the average angle error ratio and average
GT of the motion than LeGO-LOAM, showing a relatively distance error ratio of the 11 sequences for NDT-LOAM
high level of accuracy, the resultant three-dimensional point algorithm. It can be seen that the vehicle speeds of the
cloud map is intuitive and accurate, and the movement is 11 sequences are all between 15km/h and 85km/h. The average
consistent. Tab. III lists several metrics to evaluate the perfor- angle error ratio decreases as the length of the distance interval
mance of NDT-LOAM and LeGO-LOAM. Our NDT-LOAM increases. It shows a certain wave with the change of the speed
outperforms LeGO-LOAM by almost 50%. interval mobility, and the average angle error of each interval
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
3668 IEEE SENSORS JOURNAL, VOL. 22, NO. 4, FEBRUARY 15, 2022
Fig. 7. Pose trajectories from NDT-LOAM/ LeGO-LOAM and point cloud map from NDT-LOAM and on Kylin backpack data.
is between [0.0025 degrees/m, 0.0045 degrees/m]. Average The average position errors of NDT-LOAM and ALOAM/
position error ratio remains stable as the length of the distance FLOAM/LOAM are shown in Tab. II. Both ALOAM and
interval increases and the average position error of each inter- NDT-LOAM in this article run at a rate of 10 Hz
val is between [0.8%, 0.95%] (the total average position error in real-time level. The results of LOAM are from the orig-
is 0.899%), indicating that the overall positioning accuracy is inal paper, the author sacrifices efficiency for accuracy, and
relatively stable. The average position error ratio presents a the corresponding speed is 1 Hz. The results of FLOAM
certain degree of volatility, and the average position error of are from the paper [36]. From the position error data in
each interval is between [0.6%, 1.4%]. Tab. II, we can see the position accuracy of NDT-LOAM on
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
CHEN et al.: NDT-LOAM: REAL-TIME LIDAR ODOMETRY AND MAPPING WITH WEIGHTED NDT AND LFA 3669
TABLE V
R UNTIME OF M ODULES FOR P ROCESSING O NE S CAN (ms)
sequence #00, #03, #04, #05, #06, #07, #09 is slightly higher More qualitative results are shown in Fig. 7. Blue lines in
within 0.8% position errors. While the positioning accuracy of Fig. 7(a) and 7(c) are the trajectory of LeGO-LOAM, and the
#01, #02, #08, #10 is poor, and the position error is higher than green lines are our NDT-LOAM. Our method has lower tra-
1.0%. #01 is the highway environment, with few reference jectory error than LeGO-LOAM. Fig. 7(b) and 7(d) show the
targets on both sides of the road, and vehicle movement point cloud map from NDT-LOAM, while Fig. 7(e) and 7(f)
speed is high (the average speed of #01 is 20m/s, and the show the collection scenes and corresponding point clouds of
value of other data is about 10m/s), which heavily affects ascending and descending stairs of #K2. These results indicate
the positioning accuracy. The acquisition platform sensors of that our algorithm has high accuracy and strong robustness.
#00, #01, #02 have poor external calibration accuracy, and
this is a key factor that results in big error on sequence #01,
#02. The real motion value of #08 has an obvious error in
the starting position, reaching to 5 m, which influences the E. Evaluation of Runtime Performance
accuracy assessment of #08. Through the observation on the We evaluate the time consumption of our method and the
continuous playback of #10 camera pictures, we find that there baseline LeGO-LOAM. It is concluded that LeGO-LOAM out-
is a section of data captured on rugged roads where have a performs LOAM in real-time performance [18], so we ignore
large vibration at the level, which may be which may be an the comparison with LOAM in this part, readers can reference
important factor affecting the error. Under the same real-time to the paper [18] for more details. As shown in Tab. V,
efficiency conditions, the position accuracy of NDT-LOAM is LeGO-LOAM consists of segmentation, extraction, odometry
much higher than ALOAM and FLOAM. The position error of and mapping modules, and our NDT-LOAM includes DLO,
the 7 sequence data #03-#09 of NDT-LOAM is slightly lower LFA-extraction and LFA-Mapping. Mapping is the most time-
than LOAM, and the position error of the 4 sequences #01, consuming module. Since our initial pose is close to the
#02, #10 is slightly higher than LOAM. The overall position optimized one, it doesn’t cost much time to find the refined
of the NDT-LOAM and the original LOAM is basically at pose and can run in real-time. In some cases (such as #4,
the same level. The evo tool is a popular Python package for #6, and #9), mapping module takes smaller time than that
the evaluation of odometry and SLAM. We also use the tool of LeGO-LOAM. Overall, we can see that the NDT-LOAM
evaluation the absolute pose error of NDT-LOAM and LeGO- algorithm achieved a high level of accuracy and resolved the
LOAM on #00, #05 and #09 KITTI sequences, and Tab. III conflict between accuracy and efficiency.
lists the result. Our NDT-LOAM outperforms LeGO-LOAM
by almost 50%.
D. Kylin Backpack Experiment V. C ONCLUSION
With our Kylin backpack, we collected two sequences #K1 In the concept “Simultaneous Localization and Mapping”,
and #K2 to assess the proposed method. The #K1 data is from the high localization accuracy and the real-time processing
indoor space, while #K2 data includes indoor and outdoor are equally important. Some previous methods gain a higher
scenes. Due to the lack of high-precision GPS location as the odometry accuracy at the expense of more time consumption
pose ground truth, we consciously walk out of a relatively than the real-time processing. In this paper we propose an effi-
regular area at the beginning and end of the path, and use cient Lidar odometry method using an improved wNDT and
the position offset of the beginning and the end as the pose a keyframe matching strategy. In the experiments, we tested
trajectory error of the NDT-LOAM/ LeGO-LOAM method. NDT-LOAM on the KITTI odometry dataset and compare it
Some important threshold parameters used in the NDT-LOAM with the state-of-the-art algorithm LOAM. The results illus-
experiment were set as follows: the distance threshold and trate that NDT-LOAM is an efficient odometry method with
angle threshold of the key frame selection were 2m and 10◦, low drift.
respectively. Since the current method does not involve loop closure
Quantitative results of the NDT-LOAM/ LeGO-LOAM and graph optimization in the backend of SLAM, our future
experiment are shown in Tab. IV. The trajectory errors of work is to improve the approach to fix motion estimation drift
our method are 0.2 m and 0.25 m for #K1 and #K2 by closing the loop. Also, we will develop the multi-sensor
respectively, while LeGO-LOAM has almost 8 m trajectory (Visual/Lidar/IMU) integration technology for further reducing
error for sequence #K1 and 2.6 m error for sequence #K2. the motion estimation drift.
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
3670 IEEE SENSORS JOURNAL, VOL. 22, NO. 4, FEBRUARY 15, 2022
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
CHEN et al.: NDT-LOAM: REAL-TIME LIDAR ODOMETRY AND MAPPING WITH WEIGHTED NDT AND LFA 3671
Baoding Zhou received the Ph.D. degree in pho- Zhenzhong Xiao was born in Shandong, China,
togrammetry and remote sensing from Wuhan in 1980. He received the Ph.D. degree from Xi’an
University, Wuhan, China, in 2015. He is cur- Jiaotong University, Xi’an, China. He is currently
rently an Assistant Professor with the College of one of the founders and the Chief Technology
Civil and Transportation Engineering, Shenzhen Officer of Orbbec Inc. His research interests
University, China. His research interests include include 3D camera research and development,
indoor localization, indoor mapping, intelligent 3D perception, artificial intelligence, and com-
sensing, and intelligent transportation systems. puter vision.
Authorized licensed use limited to: SHENZHEN UNIVERSITY. Downloaded on May 17,2022 at 06:52:07 UTC from IEEE Xplore. Restrictions apply.
View publication stats