2020 IEEE International Conference on Robotics and Automation (ICRA)

31 May - 31 August, 2020. Paris, France

Online LiDAR-SLAM for Legged Robots

with Robust Registration and Deep-Learned Loop Closure
Milad Ramezani, Georgi Tinchev, Egor Iuganov and Maurice Fallon

Abstract— In this paper, we present a 3D factor-graph

LiDAR-SLAM system which incorporates a state-of-the-art
deeply learned feature-based loop closure detector to enable
a legged robot to localize and map in industrial environments.
Point clouds are accumulated using an inertial-kinematic state
estimator before being aligned using ICP registration. To close
loops we use a loop proposal mechanism which matches individ-
ual segments between clouds. We trained a descriptor offline to
match these segments. The efficiency of our method comes from
carefully designing the network architecture to minimize the
number of parameters such that this deep learning method can
be deployed in real-time using only the CPU of a legged robot,
a major contribution of this work. The set of odometry and
loop closure factors are updated using pose graph optimization.
Finally we present an efficient risk alignment prediction method
which verifies the reliability of the registrations. Experimental
results at an industrial facility demonstrated the robustness and
flexibility of our system, including autonomous following paths 5 m
derived from the SLAM map.

I. I NTRODUCTION
Robotic mapping and localization have been heavily stud-
ied over the last two decades and provide the perceptual basis
for many different tasks such as motion planning, control and
manipulation. A vast body of research has been carried out to
allow a robot to determine where it is located in an unknown
environment, to navigate and to accomplish tasks robustly
online. Despite substantial progress, enabling an autonomous
mobile robot to operate robustly for long time periods in Fig. 1: Bird’s-eye view of a map constructed by the ANYmal
exploring an unlit, windowless, industrial facility with our proposed
complex environments, is still an active research area. 3D SLAM system. The approach can rapidly detect and verify loop-
Visual SLAM has shown substantial progress [1], [2], closures (red lines) so as to construct an accurate map.
with much work focusing on overcoming the challenge of and joint information in a recursive approach. Our LiDAR
changing lighting variations [3]. Instead, in this work we odometry has drift below 1.5% of distance traveled.
focus on LiDAR as our primary sensor modality. Laser mea- The contributions of our paper are summarized as follows:
surements are actively illuminated and precisely sense the 1) An online 3D LiDAR-SLAM system for legged robots
environment at long ranges which is attractive for accurate based on ICP registration.
motion estimation and mapping. Hence, we focus on the 2) A verification metric which quantifies the reliability of
LiDAR-SLAM specifically for legged robots. point cloud registration.
The core odometry of our SLAM system is based on 3) A demonstration of learned loop-closure detection de-
Iterative Closest Point (ICP), a well-known method for 3D signed to be deployable on a mobile CPU during run
shape registration [13], to align clouds from a 3D LiDAR time.
sensor (Velodyne). Our approach builds upon our previous 4) A real-time demonstration of the algorithm on the
work of Autotuned ICP (AICP), originally proposed in [4], ANYmal quadruped robot.
which analyzes the content of incoming clouds to robustify
The remainder of this paper is structured as follows: Sec. II
registration. Initialization of AICP is provided by the robot’s
presents related works. Sec. III details different components
state estimator [5], which estimates the robot pose by fus-
of our SLAM system. Sec. IV presents evaluation studies
ing the inertial measurements with the robot’s kinematics
before a conclusion and future works are drawn in Sec. V.
The authors are with the Oxford Robotics Insti- II. R ELATED W ORKS
tute (ORI), University of Oxford, UK. {milad,
gtinchev, mfallon}@robots.ox.ac.uk, This section provides a literature review of perception in
[email protected] walking robots and LiDAR-SLAM systems in general.

978-1-7281-7395-5/20/$31.00 ©2020 IEEE 4158

Authorized licensed use limited to: SUNY AT STONY BROOK. Downloaded on October 03,2020 at 13:24:40 UTC from IEEE Xplore. Restrictions apply.
 LiDAR Point
A. Perception systems on walking robots
Simultaneous Localization and Mapping (SLAM) is a
LiDAR
key capability for walking robots and consequently their
IMU
autonomy. Example systems include, the mono visual SLAM
system of [6] which ran on the HRP-2 humanoid robot and RF Joint3
is based on an Extended Kalman Filter (EKF) framework. Base RF Joint2
In contrast, [7] leveraged a particle filter to estimate the
posterior of their SLAM method, again on HRP-2. Oriolo et
RF Joint1
al. [8] demonstrated visual odometry on the Nano bipedal
robot by tightly coupling visual information with the robot’s
Map
kinematics and inertial sensing within an EKF. However,
Contact
these approaches acquired only a sparse map of the environ-
ment on the fly, substantially limiting the robot’s perception. Fig. 2: Axis conventions of various frames used in the LiDAR-
Works such as [9] and [10] leveraged the frame-to-model SLAM system and their relationship with respect to the base frame.
As an example, we show only the Right Front (RF) leg.
visual tracking of KinectFusion [11] or ElasticFusion [12],
which use a coarse-to-fine ICP algorithm. Nevertheless, create a globally consistent pose graph using iSAM2 [20]
vision-based SLAM techniques struggle in varying illumi- (as part of the GTSAM library).
nation conditions. While robustness to illumination change Behley et al. in [21] proposed a surfel-based LiDAR-
has been explored, for example [14] and [3], it remains a SLAM system by leveraging the projective data association
major challenge to any visual navigation system. between the current scan and a rendered model view from
Compared to bipedal robots, quadrupedal robots have the surfel map.
better versatility and resilience when navigating challenging Dubé et al. in [22] developed an online cooperative
terrain. Thus, they are more suited to long-term tasks such LiDAR-SLAM system. Using two and three robots, each
as inspection of industrial sites. In an extreme case, a robot equipped with 3D LiDAR, an incremental sparse pose-graph
might need to jump over a terrain hurdle. Park et al., in [15], is populated by successive place recognition constraints.
used a Hokuyo laser range finder to detect 40 cm hurdles, The place recognition constraints are identified utilizing the
which then allowed the MIT Cheetah-2’s controller to jump SegMatch algorithm [23], which represents LiDAR clouds as
over the obstacles, though the LiDAR measurements were a set of compact yet discriminative features. The descriptors
not used for localization. were used in a matching process to determine loop closures.
Nobili et al. [16] presented a state estimator for the
III. A PPROACH
Hydraulic Quadruped robot (HyQ) which was based on a
modular inertial-driven EKF. The robot’s base link velocity Our goal is to provide the ANYmal with a drift-free
and position was propagated using IMU measurements with localization estimate over a very long mission, as well as to
corrections from modules including leg odometry, visual enable the robot to accurately map its surroundings. Fig. 2
odometry and LiDAR odometry. The system is prone to drift and Fig. 3 elucidate the different components of our system
because the navigation system did not include a mechanism from a hardware and a software perspective.
to detect loop-closures.
A. Kinematic-Inertial Odometry
B. LiDAR-SLAM Systems We use the default state estimator for the robot to estimate
incremental motion using proprioceptive sensing. This is the
An effective LiDAR-based approach is LiDAR Odome- Two-State Implicit Filter (TSIF) [24]. The position of the
try and Mapping (LOAM) [17]. LOAM extracts edge and contact feet in the inertial frame {I}, obtained from forward
surface point features in a LiDAR cloud by evaluating the kinematics, is treated as a temporal measurement to estimate
roughness of the local surface. The features are reprojected to the robot’s pose in the fixed odometry frame, {O}:
the start of the next scan based on a motion model, with point  O
RB p O

correspondences found within this next scan. Finally, the 3D O B
TB = , (1)
motion is estimated recursively by minimizing the overall 0 1
distances between point correspondences. LOAM achieves where TO B ∈ SE(3) is the transformation from the base
high accuracy with a low cost of computation. frame {B} to the odometry frame {O}.
Similar to LOAM, Deschaud in [18] developed a LiDAR This estimate of the base frame drifts over time as it does
odometry, IMLS-LiDAR, with similar accuracy by leverag- not use any exteroceptive sensors and the position of the
ing scan-to-model matching. quadruped and its rotation around z-axis are not observable.
Shan et al. in [19] extended LOAM with Lightweight and In the following, we define our LiDAR-SLAM system for
Ground-Optimized LiDAR Odometry and Mapping (LeGO- estimation of the robot’s pose with respect to the map frame
LOAM) which added latitudinal and longitudinal parameters {M} which is our goal. It is worth noting that the TSIF
separately in a two-phase optimization. In addition, LeGO- framework estimates the covariance of the state [24] which
LOAM detected loop-closures using an ICP algorithm to we employ during our geometric loop-closure detection.

4159

Authorized licensed use limited to: SUNY AT STONY BROOK. Downloaded on October 03,2020 at 13:24:40 UTC from IEEE Xplore. Restrictions apply.
 Joint
Encoders State Estimator (400 Hz) Each individual loop closure becomes a factor and is added
(400 Hz)
Kinematic - Inertial Odometry to the factor graph. The loop-closure factor, in this work, is a
Inertial
Measurements
(400 Hz)
factor whose end is the current pose of the robot and whose
Initialisation start is one of the reference clouds, stored in the history of the
1 Hz ~1 Hz Sparse robot’s excursion. The nominated reference cloud must meet
AICP
Factor Graph two criteria of nearest neighbourhood and sufficient overlap
LiDAR Localisation
Scans Accumulation
(10 Hz)
and with the current reference cloud. An accepted loop-closure
Mapping
1 Hz
Loop Closure factor, φj (XM , XN ), is defined as:
φj (XM , XN ) = (TM
−1
M −1
BM TBN )
eM
(∆j,aicp TBM )
−1 e M
TBN , (4)
Fig. 3: Block diagram of the LiDAR-SLAM system. M M
where Te B and T e B are the robot’s poses in the map frame
B. LiDAR-SLAM System M N
{M} corrected by AICP with respect to the reference cloud
Our LiDAR-SLAM system is a pose graph SLAM system (M − 1) and the reference cloud (N − 1), respectively. The
built upon our ICP registration approach called Autotuned- ∆j,aicp is the AICP correction between the current reference
ICP (AICP) [4]. AICP automatically adjusts the outlier filter cloud M and the nominated reference cloud N .
of ICP by computing an overlap parameter, Ω ∈ [0, 1] since Once all the factors, including odometry and loop-closure
the assumption of a constant overlap, which is conventional factors, have been added to the factor graph, we optimize
in the standard outlier filters, violates the registration in real the graph so that we find the Maximum A Posteriori (MAP)
scenarios. estimate for the robot poses corresponding to the reference
Given the initial estimated pose from the kinematic-inertial clouds. To carry out this inference over the variables Xi ,
odometry, we obtain a reference cloud to which we align where i is the number of the robot’s pose in the factor graph,
each consecutive reading1 point cloud. the product of all the factors, must be maximized:
In this manner the successive reading clouds are precisely Y Y
X M AP = argmax φi (Xi−1 , Xi ) φj (XM , XN ). (5)
aligned with the reference clouds with greatly reduced drift. X i j
The robot’s pose, corresponding to each reading cloud is Assuming that factors follow a Gaussian distribution and
obtained as follows: all measurements are only corrupted with white noise, i.e.
TM O
B = ∆aicp TB , (2) noise with normal distribution and zero mean, the optimiza-
where TM is the robot’s pose in the map frame {M} and tion problem in Eq. (5) is equivalent to minimizing a sum
B
∆aicp is the alignment transformation calculated by AICP. of nonlinear least squares:
X
Calculating the corrected poses, corresponding to the point X M AP = argmin ||yi (Xi , Xi−1 ) − mi ||2Σi
X
clouds, we compute the relative transformation between the X
i
(6)
successive reference clouds to create the odometry factors of + ||yj (XM , XN ) − mj ||2Σj ,
the factor graph which we introduce in Eq. (3). j
The odometry factor, φi (Xi−1 , Xi ), is defined as: where m, y and Σ denote the measurements, their mathe-
−1
−1
M −1 e M e M matical model and the covariance matrices, respectively.
φi (Xi−1 , Xi ) = (TM
Bi−1 TBi ) TBi−1 TBi , (3)
As noted, the MAP estimate is only reliable when the
M M
where T eB
i−1
and Te B are the AICP estimated poses of the
i
residuals in Eq. (6) follow the normal distribution. However,
robot for the node Xi−1 and Xi , respectively, and TM Bi−1 and
ICP is susceptible to failure in the absence of geomet-
TM are the noise-free transformations. ric features, e.g. in corridors or door entries, which can
Bi
A prior factor, φ0 (X0 ), which is taken from the pose have a detrimental effect when optimizing the pose graph.
estimate of the kinematic-inertial odometry, is initially added In Sec. III-D, we propose a fast verification technique for
to the factor graph to set an origin and a heading for the robot point cloud registration to detect possible failure of the AICP
within the map frame {M}. registration.
To correct for odometric drift, loop-closure factors are
added to the factor graph once the robot revisits an area of the C. Loop Proposal Methods
environment. We implemented two approaches for proposing This section introduces our learned loop-closure proposal
loop-closures: a) geometric proposal based on the distance and geometric loop-closure detection.
between the current pose and poses already in the factor
1) Deeply-learned Loop Closure Proposal: We use the
graph which is useful for smaller environments, and b) a
method of Tinchev et al. [26], which is based on matching
learning approach for global loop-closure proposal, detailed
individual segments in pairs of point clouds using a deeply-
in Sec III-C.1, which scales to large environments. Each
learned feature descriptor. Its specific design uses a shallow
proposal provides an initial guess, which is refined with ICP.
network such that it does not require a GPU during run-
1 Borrowing the notation from [25], the reference and reading clouds
time inference on the robot. We present a summary of the
correspond to the robot’s poses at the start and end of each edge in the method called Efficient Segment Matching (ESM), but refer
factor graph and AICP registers the latter to the former. the reader to [26].

4160

Authorized licensed use limited to: SUNY AT STONY BROOK. Downloaded on October 03,2020 at 13:24:40 UTC from IEEE Xplore. Restrictions apply.
 Algorithm 1: Improved Risk Alignment Prediction. state estimator (TSIF) to define a dynamic search window
1 input: point clouds CS , CT ; estimated poses XS , XT around the current pose of the robot. Then the previous
2 output: alignment risk ρ = f (Ω, α) robot’s poses, which reside within the search window, are
3 begin examined based on two criteria: nearest neighbourhood and
4 Segment CS and CT into a set of planes: PSi and PT j , verification of cloud registration (described in Sec. III-D).
5 Compute centroid of each plane: KSi , KT j ,
6 Transform query keypoints KT j into the space of CS ,
Finally, the geometric loop closure is computed between the
7 Search the nearest neighbour of each query plane PT j , current cloud and the cloud corresponding to the nominated
8 for plane PT do pose using AICP.
9 Find match PS amongst candidates in the k-d tree,
10 Compute the matching score Ωp , D. Fast Verification of Point Cloud Registration
11 if Ωp is max then This section details a verification approach for ICP point
12 Determine the normal of plane PT , cloud registration to determine if two point clouds can be
13 Push back the normal into the matrix N , safely registered. We improve upon our previously proposed
14 end
15 end alignability metric in [28] with a much faster method.
16 Compute alignability α = λmax / λmin ; Method from [28]: First, the point clouds are segmented
17 Learn ρ = f (Ω, α); into a set of planar surfaces. Second, a matrix N ∈ RM ×3
18 Return ρ; is computed, where each row corresponds to the normal of
19 end the planes ordered by overlap. M is the number of matching
First, a neural network is trained offline using individual planes in the overlap region between the two clouds. Finally,
LiDAR observations (segments). By leveraging odometry in the alignability metric, α is defined as the ratio between the
the process, we can match segment instances without manual smallest and largest eigenvalues of N .
The matching score, Ωp , is computed as the overlap
intervention. The input to the network is a batch of triplets
between two planes, PT j and PSi where i ∈ NS and
- anchor, positive and negative segments. The anchor and
j ∈ NT . NS and NT are the number of planes in the
positive samples are the same object from two successive
input clouds. In addition, in order to find the highest pos-
Velodyne scans, while the negative segment is a segment
sible overlap, the algorithm iterates over all possible planes
chosen ≈ 20 m apart. The method then performs a series of
from two point clouds. This results in overall complexity
X-conv operators directly on raw point cloud data, based
O(NS NT (NPS NPT )), where NPS and NPT are the average
on PointCNN [27], followed by three fully connected layers,
number of points in planes of the two clouds.
where the last layer is used as the descriptor for the segments.
Proposed Improvement: To reduce the pointwise compu-
During our trials, when the SLAM system receives a new
tation, we first compute the centroids of each plane KSi
reference cloud, it is preprocessed and then segmented into
and KT j and align them from point cloud CT to CS given
a collection of point cloud clusters. For each segment in
the computed transformation. We then store the centroids in
the reference cloud, a descriptor vector is computed with
a k-d tree and for each query plane PT ∈ CT we find the
an efficient TensorFlow C++ implementation by performing
K nearest neighbours. We compute the overlap for the K
a forward pass using the weights from the already trained
nearest neighbours, and use the one with the highest overlap.
model. This allows a batch of segments to be preprocessed
This results in O(NT K(NPS NPT )), where K << Ns .
simultaneously with zero-meaning and normalized variance
In practice, we found that K = 1 is sufficient for our
and then forward passed through the trained model. We use a
experiments. Furthermore, we only store the centroids in a
three dimensional tensor as input to the network - the length
k-d tree, reducing the space complexity.
is the number of segments in the current point cloud, the
We discuss the performance of this algorithm, as well
width represents a fixed-length down-sampled vector of all
as its computational complexity in the experiment section
the points in an individual segment, and the height contains
of this paper. Pseudo code of the algorithm is available in
the x, y and z values. Due to the efficiency of the method,
Algorithm 1.
we need not split the tensor into mini-batches, allowing us
to process the full reference cloud in a single forward pass. IV. E XPERIMENTAL E VALUATION
Once the descriptors for the reference cloud are computed, We employ a state-of-the-art quadruped, ANYmal (version
they are compared to the map of previous reference clouds. B) [29], as our experimental platform. Fig. 4 shows a view
ESM uses an l2 distance in feature space to detect matching of the ANYmal robot. The robot weighs about 33 kg without
segments and a robust estimator to retrieve a 6DoF pose. Sensor Model Frequency Specifications
This produces a transformation of the current reference Xsens Bias Repeatability: < 0.5◦ /s; 5 mg
IMU 400
cloud with respect to the previous reference clouds. The MTi-100
Bias Stability: 10◦ /h; 40 µg
transformation is then used in AICP to add a loop-closure Resolution in Azimuth: < 0.4◦
as a constraint to the graph-based optimization. Finally, LiDAR Velodyne Resolution in Zenith: 2.0◦
10
Unit VLP-16 Range < 100 m
ESM’s map representation is updated, when the optimization Accuracy: ± 3 cm
concludes. Encoder ANYdrive 400 Resolution < 0.025◦
Torque ANYdrive 400 Resolution < 0.1 N m
2) Geometric Loop-Closure Detection: To geometrically
detect loop-closures, we use the covariance of the legged TABLE I: Specifications of the sensors installed on the ANYmal.
4161

Authorized licensed use limited to: SUNY AT STONY BROOK. Downloaded on October 03,2020 at 13:24:40 UTC from IEEE Xplore. Restrictions apply.
 30 m

Fig. 5: Demonstration of the learning-based loop closure in the

Fig. 4: Our experiments were carried out with the ANYmal, in the outdoor experiment. See Sec. IV-B for more details.
Oil Rig training site. To determine ground truth robot poses, we Fig. 6 (Right) shows that our approach highly correlates
used a Leica TS16 laser tracker to track a 360◦ prism on the robot. with the result from the original approach. Finally, we refer
any external perception modules and can carry a maximum the reader to the original work that provided a thorough com-
payload of 10 kg and trot at a maximum speed of 1.0 m/s. parison of alignment risk against Inverse Condition Number
As shown in Fig. 2, each leg contains 3 actuated joints (ICN) [30] and Degeneracy parameter [31] in ill-conditioned
which altogether gives 18 degrees of freedom, 12 actuated scenarios. The former mathematically assessed the condition
joints and the 6 DoF robot base, which the robot uses to of the optimization, whereas the latter determines the degen-
dynamically navigate challenging terrain. Tab. I summarizes erate dimension of the optimization.
the specifications of the robot’s sensors used in this paper.
The proposed LiDAR-SLAM system is evaluated using the B. Evaluation of Learned Loop Closure Detector
datasets collected by our ANYmal quadruped robot. In the next experiment we explore the performance of
We first analyze the verification method. Second, we the different loop closure methods using a dataset collected
investigate the learned loop-closure detection in terms of outdoors at the Fire Service College, Moreton-in-Marsh, UK.
speed and reliability (Sec. IV-B). We then demonstrate the 1) Robustness to viewpoint variation: Fig. 5 shows a
performance or our SLAM system on two large-scale exper- preview of our SLAM system with the learned loop closure
iments, one indoor and one outdoor (Sec. IV-C) including method. We selected just two point clouds to create the map,
an online demonstration where the map is used for route with their positions indicated by the blue arrows in Fig. 5.
following (Sec. IV-D). A demonstration video can be found The robot executed two runs in the environment, which
at: https://ori.ox.ac.uk/lidar-slam. comprised 249 point clouds. We deliberately chose to tra-
verse an offset path (red) the second time so as to determine
A. Verification Performance how robust our algorithm is to translation and viewpoint
We focus on the alignability metric α of the alignment variation. In total 50 loop closures were detected (green
risk prediction since it is our primary contribution. We arrows) around the two map point clouds. Interestingly, the
computed α between consecutive point clouds of our out- approach not only detected loop closures from both trajec-
door experiment, which we discuss later in Sec. IV-C. As tories, with translational offsets up to 6.5 m, but also with
seen in Fig. 6 (Left), the alignability filter based on a orientation variation up to 180◦ - something not achievable
k-d tree is substantially faster than the original filter. As by standard visual localization - a primary motivation for
noted in Sec. III-D, our approach is less dependent on the using LiDAR. Across the 50 loop closures an average of 5.24
point cloud size, due to only using the plane centroids. segments were recognised per point cloud. The computed
Whereas, the original alignability filter fully depends on all transformation had an Root Mean Square Error (RMSE) of
the points of the segments, resulting in higher computation 0.08 ± 0.02 m from the ground truth alignment. This was
time. Having tested our alignability filter for the datasets achieved in approximately 486 ms per query point cloud.
taken from Velodyne VLP-16, the average computation time 2) Computation Time: Fig. 8 shows a graph of the
is less than 0.5 seconds (almost 15x improvement) which is computation time. The computation time for the geometric
suitable for real-time operation. loop closure method depends on the number of traversals
Alignability Filter
10 1 Alignability Filter Based on k-d Tree
mean ~ 5.15 sec
Time (sec)

10 0

mean ~ 0.33 sec

10 -1
20 40 60 80 100 120 140 160 180
Registration number

Fig. 6: Comparison of our proposed verification method with the original in terms of computation time (left) and performance (right).

4162

Authorized licensed use limited to: SUNY AT STONY BROOK. Downloaded on October 03,2020 at 13:24:40 UTC from IEEE Xplore. Restrictions apply.
 2

-2

-4

-6

Y (m)
-8

-10

-12
Ground truth
-14 AICP
SLAM without Verification
-16 SLAM with Verification
5 10 15 20 25 30 35 40
X (m)

Fig. 7: Result of SLAM system with verification enabled (left). Estimated trajectories of algorithm variants versus ground truth (right).
around the same area. The geometric loop closures iterated Translational Heading Relative Pose
Method
Error (RMSE) (m) Error (RMSE) (deg) Error (m)
over the nearest N clouds, based on a radius; the covariance SLAM with Verification 0.06 N/A 0.090
SLAM without Verification 0.23 1.6840 0.640
and distance travelled caused it to slow down. Similarly, the AICP (LiDAR odometry) 0.62 3.1950 1.310
verification method needs to iterate over large proportion of TSIF (legged odometry) 5.40 36.799 13.64

the point clouds in the same area, affecting the real time TABLE II: Comparison of the localization accuracy for the
operation. different approaches.
Instead, the learning loop closure proposal scales better Tab. II reports SLAM results with and without verification
with map size. It compares low dimensional feature descrip- compared to the AICP LiDAR odometry and the TSIF legged
tor vectors, which is much faster than the thousands of data state estimator. As the Leica TS16 does not provide rotational
points in Euclidean space. estimates, we took the best performing method - SLAM with
C. Indoor and Outdoor Experiments verification - and compared the rest of the trajectories to it
To evaluate the complete SLAM system, the robot walked with the ATE metric. Based on this experiment, the drift of
indoor and outdoor along trajectories with the length of SLAM with verification is less than 0.07%, satisfying many
about 100 m and 250 m, respectively. Each experiment lasted location-based tasks of the robot.
about 45 minutes. Fig. 1 (Bottom) and Fig. 4 illustrate D. Experiments on the ANYmal
the test locations: industrial buildings. To evaluate mapping
In a final experiment, we tested the SLAM system online
accuracy, we compared our SLAM system, AICP, and the
on the ANYmal. After building a map with several loops
baseline legged odometry (TSIF) using ground truth. As
(while teleoperated), we queried a path back to the operator
shown in Fig. 4, we used a Leica TS16 to automatically
station. Using the Dijkstra’s algorithm [33], the shortest
track a 360◦ prism rigidly mounted on top of the robot. This
path was created using the factor graph. As each edge
way, we could record the robot’s position with millimeter
has previously been traverse, following the return trajectory
accuracy at about 7 Hz (when in line of sight).
returned the robot to the starting location. The supplementary
For evaluation metrics, we use Relative Pose Error (RPE)
video demonstrates the experiment.
and Absolute Trajectory Error (ATE) [32]. RPE determines
the local accuracy of the trajectory over time. ATE is the V. C ONCLUSION AND F UTURE W ORK
RMSE of the Euclidean distance between the estimated This paper presented an accurate and robust LiDAR-
trajectory and the ground truth. SLAM system on a resource constrained legged robot using a
As seen in Fig. 7 (right), our SLAM system with verifi- factor graph-based optimization. We introduced an improved
cation is almost completely consistent with the ground truth. registration verification algorithm capable of running in real
The verification algorithm approved 27 true-positive loop- time. In addition, we leveraged a state-of-the-art learned loop
closure factors (indicated in red in Fig. 7 (Left)) which were closure detector which is sufficiently efficient to run online
added to the factor graph. Without this verification 38 loop- and had significant viewpoint robustness. We examined our
closures were created, some in error, resulting in an inferior system in indoor and outdoor industrial environments with a
map. Fig. 1 (Top) also evidences the global consistency of final demonstration showing online operation of the system
the map. However, there is no ground truth available for our on our robot. In future work, we will speed up our ICP
indoor experiment for evaluation of the trajectory. registration to increase the update frequency of our SLAM
10
Geometric Loop Closure with verification system and examine our system in more varied scenarios.
Geometric Loop Closure without verification
8
Learned Loop Closure
VI. ACKNOWLEDGEMENT
Time (sec)

6
We would like to thank our colleagues in ORI, in particular
4
Simona Nobili, for their help in this work. This research was
2
supported by the Innovate UK-funded ORCA Robotics Hub
0
50 100 150 200 (EP/R026173/1) and the EU H2020 Project MEMMO. M.
Registration number Fallon is supported by a Royal Society University Research
Fig. 8: Computation times of the considered loop closure methods. Fellowship.

4163

Authorized licensed use limited to: SUNY AT STONY BROOK. Downloaded on October 03,2020 at 13:24:40 UTC from IEEE Xplore. Restrictions apply.
 R EFERENCES [29] M. Hutter, C. Gehring, D. Jud, A. Lauber, C. D. Bellicoso, V. Tsounis,
J. Hwangbo, K. Bodie, P. Fankhauser, and M. Bloesch, “Anymal - a
[1] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: a highly mobile and dynamic quadrupedal robot,” in IROS, 2016.
versatile and accurate monocular SLAM system,” TRO, 2015. [30] E. Cheney and D. Kincaid, Numerical mathematics and computing.
[2] J. Engel, T. Schöps, and D. Cremers, “LSD-SLAM: Large-scale direct Cengage Learning, 2012.
monocular SLAM,” in ECCV, 2014. [31] J. Zhang, M. Kaess, and S. Singh, “On degeneracy of optimization-
[3] H. Porav, W. Maddern, and P. Newman, “Adversarial training for based state estimation problems,” in ICRA, 2016.
adverse conditions: Robust metric localisation using appearance trans- [32] J. Sturm, N. Engelhard, F. Endres, W. Burgard, and D. Cremers, “A
fer,” in ICRA, 2018. benchmark for the evaluation of RGB-D SLAM systems,” in IROS,
[4] S. Nobili, R. Scona, M. Caravagna, and M. Fallon, “Overlap-based 2012.
ICP tuning for robust localization of a humanoid robot,” in ICRA, [33] E. Dijkstra, “A note on two problems in connexion with graphs,”
2017. Numerische mathematik, 1959.
[5] M. Bloesch, M. Hutter, M. A. Hoepflinger, S. Leutenegger, C. Gehring,
C. D. Remy, and R. Siegwart, “State estimation for legged robots-
consistent fusion of leg kinematics and IMU,” in RSS, 2013.
[6] O. Stasse, A. Davison, R. Sellaouti, and K. Yokoi, “Real-time 3D
slam for humanoid robot considering pattern generator information,”
in IROS, 2006.
[7] N. Kwak, O. Stasse, T. Foissotte, and K. Yokoi, “3D grid and particle
based slam for a humanoid robot,” in Humanoids, 2009.
[8] G. Oriolo, A. Paolillo, L. Rosa, and M. Vendittelli, “Vision-based
odometric localization for humanoids using a kinematic EKF,” in
Humanoids, 2012.
[9] R. Wagner, U. Frese, and B. Bäuml, “Graph SLAM with signed
distance function maps on a humanoid robot,” in IROS, 2014.
[10] R. Scona, S. Nobili, Y. R. Petillot, and M. Fallon, “Direct visual SLAM
fusing proprioception for a humanoid robot,” in IROS, 2017.
[11] R. Newcombe, S. Izadi, O. Hilliges, D. Molyneaux, D. Kim, A. Davi-
son, P. Kohli, J. Shotton, S. Hodges, and A. Fitzgibbon, “KinectFusion:
Real-time dense surface mapping and tracking.” in ISMAR, 2011.
[12] T. Whelan, S. Leutenegger, R. Salas-Moreno, B. Glocker, and A. Davi-
son, “ElasticFusion: Dense SLAM without a pose graph.” RSS, 2015.
[13] P. J. Besl and N. D. McKay, “Method for registration of 3-d shapes,”
in Sensor fusion IV: control paradigms and data structures, vol. 1611.
International Society for Optics and Photonics, 1992, pp. 586–606.
[14] M. Milford and G. Wyeth, “SeqSLAM: Visual route-based navigation
for sunny summer days and stormy winter nights,” in ICRA, 2012.
[15] H.-W. Park, P. Wensing, and S. Kim, “Online planning for autonomous
running jumps over obstacles in high-speed quadrupeds,” in RSS, 2015.
[16] S. Nobili, M. Camurri, V. Barasuol, M. Focchi, D. Caldwell, C. Sem-
ini, and M. Fallon, “Heterogeneous sensor fusion for accurate state
estimation of dynamic legged robots,” in RSS, 2017.
[17] J. Zhang and S. Singh, “LOAM: Lidar Odometry and Mapping in
real-time.” in RSS, 2014.
[18] J.-E. Deschaud, “Imls-slam: scan-to-model matching based on 3d
data,” in 2018 IEEE International Conference on Robotics and Au-
tomation (ICRA). IEEE, 2018, pp. 2480–2485.
[19] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-
optimized LiDAR odometry and mapping on variable terrain,” in IROS,
2018.
[20] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Del-
laert, “iSAM2: Incremental smoothing and mapping using the bayes
tree,” IJRR, 2012.
[21] J. Behley and C. Stachniss, “Efficient surfel-based slam using 3d laser
range data in urban environments.” in Robotics: Science and Systems,
2018.
[22] R. Dubé, A. Gawel, H. Sommer, J. Nieto, R. Siegwart, and C. Cadena,
“An online multi-robot SLAM system for 3D LiDARS,” in IROS,
2017.
[23] R. Dubé, D. Dugas, E. Stumm, J. Nieto, R. Siegwart, and C. Cadena,
“Segmatch: Segment based place recognition in 3D point clouds,” in
ICRA, 2017.
[24] M. Bloesch, M. Burri, H. Sommer, R. Siegwart, and M. Hutter, “The
two-state implicit filter recursive estimation for mobile robots,” RAL,
2017.
[25] F. Pomerleau, F. Colas, R. Siegwart, and S. Magnenat, “Comparing
icp variants on real-world data sets,” Autonomous Robots, 2013.
[26] G. Tinchev, A. Penate-Sanchez, and M. Fallon, “Learning to see
the wood for the trees: Deep laser localization in urban and natural
environments on a CPU,” RAL, 2019.
[27] Y. Li, R. Bu, M. Sun, W. Wu, X. Di, and B. Chen, “PointCNN:
Convolution on X-transformed points,” in NIPS, 2018.
[28] S. Nobili, G. Tinchev, and M. Fallon, “Predicting alignment risk to
prevent localization failure,” in ICRA, 2018.

4164

Authorized licensed use limited to: SUNY AT STONY BROOK. Downloaded on October 03,2020 at 13:24:40 UTC from IEEE Xplore. Restrictions apply.