LAMP: Large-Scale Autonomous Mapping and Positioning for Exploration of Perceptually-Degraded Subterranean Environments

03/03/2020 ∙ by Kamak Ebadi, et al. ∙ 0

Simultaneous Localization and Mapping (SLAM) in large-scale, unknown, and complex subterranean environments is a challenging problem. Sensors must operate in off-nominal conditions; uneven and slippery terrains make wheel odometry inaccurate, while long corridors without salient features make exteroceptive sensing ambiguous and prone to drift; finally, spurious loop closures that are frequent in environments with repetitive appearance, such as tunnels and mines, could result in a significant distortion of the entire map. These challenges are in stark contrast with the need to build highly-accurate 3D maps to support a wide variety of applications, ranging from disaster response to the exploration of underground extraterrestrial worlds. This paper reports on the implementation and testing of a lidar-based multi-robot SLAM system developed in the context of the DARPA Subterranean Challenge. We present a system architecture to enhance subterranean operation, including an accurate lidar-based front-end, and a flexible and robust back-end that automatically rejects outlying loop closures. We present an extensive evaluation in large-scale, challenging subterranean environments, including the results obtained in the Tunnel Circuit of the DARPA Subterranean Challenge. Finally, we discuss potential improvements, limitations of the state of the art, and future research directions.



There are no comments yet.


page 1

page 2

page 4

page 5

page 6

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

Over the past two decades, there has been a growing demand for autonomous unmanned exploration of diverse subterranean environments, from tunnels and urban underground environments to complex cave networks. This surge of interest is motivated by terrestrial applications such as search and rescue, disaster response, and infrastructure monitoring, as well as the unparalleled scientific opportunities offered by exploration of other worlds. Lava tubes, subterranean conduits found on the Moon and Mars, are of particular interest: sheltered from cosmic radiation and temperature fluctuations, they provide ideal conditions for the development for microbial life, as well as potential sites for habitats in future human space missions [1].

Localization and mapping are key capabilities for autonomous underground operation. In typical applications, there is no prior map of the environment, and GPS cannot be relied on to localize the robots. Indeed, the ultimate goal of many autonomous exploration applications is to obtain an accurate 3D map of the environment. Unfortunately, most SLAM systems have inadequate performance when deployed in perceptually-degraded subterranean environments: sensors must operate in off-nominal conditions (poor illumination or lack-thereof, dust, water puddles and non-Lambertian surfaces) which render visual-SLAM approaches unreliable [2]; uneven and slippery terrains make wheel odometry inaccurate, while long, featureless corridors make lidar-based mapping prone to drift; finally, perceptual aliasing, the presence of many similar-looking corridors and intersections, induces spurious loop closures that can degrade the mapping results.

Fig. 1: Two autonomous ground rovers (Husky A200) explore the network of underground tunnels at the Arch mines.

Related work. We review the literature on SLAM systems for subterranean and perceptually degraded environments, and refer the reader to [3] for a broader survey on SLAM. Related work investigates different sensing modalities, including vision [4], visual-inertial [5, 6], and thermal-inertial [7]. However, solely relying on vision for underground localization and mapping can be challenging as cameras are directional and sensitive to illumination changes and environmental conditions. Lidar sensors provide a horizontal field of view and high sampling rate, and do not rely on external light sources. Therefore, 3D lidar-SLAM has been a popular solution to map complex unstructured environments, from early work [8] to more recent systems [9, 10, 11, 12, 13, 20]. Thrun et al. [14] propose an underground mine mapping algorithm that relies on 2D scan matching and a global alignment step. Tardioli et al. [15, 16] propose a system architecture for single and multi-robot exploration in underground tunnels. Zlot et al. [17] present a lidar-based SLAM method for mapping of a 17 km underground copper and gold mine; to detect loop closures, they use a surfel representation and search for matches against previous trajectory segments. Leingartner et al. [18] investigate the performance of off-the-shelf sensors and state-of-the-art mapping algorithms in mapping a 1.5 km long motorway tunnel in an urban search & rescue scenario and conclude that the investigated sensing and mapping techniques are not yet robust enough to deal with these perceptually degraded environments. Jacobson et al. [19] present a semi-supervised method that relies on manual selection of topological landmarks inside a 300 m long tunnel to perform localization using a low cost camera sensor.

Contribution. This paper reports on the implementation and testing of a multi-robot SLAM system developed in the context of the Tunnel Circuit of the DARPA Subterranean Challenge (the first of the three subdomains) [22], which aims to explore new approaches to rapidly map, navigate, search, and exploit large-scale complex underground environments. The contribution of the paper is threefold. First, we describe the multi-robot system architecture and the choice and implementation of the algorithmic modules. We comment on the improvements we made to existing algorithms to enhance subterranean operation, including (i) an accurate lidar front-end based on scan-to-scan and scan-to-submap matching, (ii) a versatile SLAM back-end that can incorporate data from multiple sensors and inputs from a human operator, and (iii) an Incremental Consistent Measurement (ICM) Set Maximization approach based on [21] to reject outlying loop closures. The second key contribution is an extensive evaluation in challenging underground mines across the United States, including experiments conducted in the Bruceton Research mine in August 2019, as part of the Tunnel Circuit of the DARPA Subterranean Challenge, where the system described in this paper has afforded the CoSTAR team [23] the second place. Finally, we discuss the lessons learned, including potential improvements, limitations of the state of the art, and future research directions.

Ii LAMP: Large-scale Autonomous
Mapping and Positioning

This section introduces our Large-scale Autonomous Mapping and Positioning

(LAMP) system. Using data from 3D lidar scanners of one or more robots, LAMP estimates the trajectories of each robot as well as a point cloud map of the full environment. Moreover, it uses an RGB-D camera to detect and localize known objects in the environment; we call these objects

artifacts (following [22]) to distinguish them from natural landmarks commonly used in landmark-based SLAM. While the system is designed to work with multiple robots (all connected to a central base station), it naturally falls back to a single-robot SLAM system when the base station is out of communication range.

Ii-a Overview: Single and Multi-robot Architecture

Fig. 2 shows LAMP’s multi-robot architecture. The local front-end includes a lidar front-end(Section II-B, module shown in blue in Fig. 2) that computes odometry estimates and detects loop closures, and a vision front-end (Section II-C, module shown in green in Fig. 2) for artifact detection and localization. The local back-end uses pose graph optimization (PGO) [3] to estimate the trajectory of the robot (Section II-D

) along with the location of artifacts, and also includes an outlier rejection mechanism, based on ICM (Section 

II-E). For robot , the result of the local front-end and back-end is an estimate of the robot trajectory (modeled as a sparse set of poses) and artifact positions, as well as a downsampled point cloud attached to each robot pose; we denote with and the pose graph and the pose estimates for robot , and we denote with the collections of local point clouds for robot .

Fig. 2: Centralized multi-robot SLAM architecture.

When within communication range, each robot sends its pose estimates , pose graph , and point clouds to a base station, which fuses the estimates from all robots, and displays the results to a human operator. The base station receives the tuple from each robot , searches for potential loop closures between the maps (inter-robot loop closure detection module), discards outliers (ICM module), and solves a second PGO problem, this time involving all robot trajectories and all observed artifacts. We discuss the key modules from Fig. 2 in the following sections.

Ii-B Local Lidar Front-end

The lidar front-end is in charge of using the raw lidar data to obtain relative pose measurements between consecutive robot poses (odometry) and non-consecutive poses (loop closures). The front-end builds upon an existing open-source implementation 

[24] and proceeds along the same lines as modern lidar-SLAM systems such as LOAM [9].

Ii-B1 Lidar Odometry

A typical 3D lidar, such as the Velodyne Puck LITE [25], generates up to 10,000 points per scan and produces 10 scans per second. In order to increase efficiency and filter out noise, we apply a 3D voxel grid filter to the raw point cloud, using the implementation available in the Point Cloud Library (PCL) [26]. We then use the Generalized Iterative Closest Point (GICP) algorithm [27] to perform scan-to-scan matching, which estimates the relative motion between the point cloud collected at the current time and the point cloud collected at time . We initialize the GICP algorithm with the identity pose. Since compact lidars, such as the Velodyne Puck, have relatively narrow vertical field of view () and low vertical angular resolution (, 16 scanning channels), the scan-to-scan estimates are not very accurate. Therefore, as done in other lidar-SLAM pipelines [9], we also use a scan-to-submap matching step, where the latest scan is matched against a local submap. Note that both matching modules produce an odometry estimate, but scan-to-submap matching typically produces more accurate estimates, as we show in the experimental section. The odometric estimate from the scan-to-submap matching is then passed to the back-end.

Ii-B2 Loop Closure Detection

In order to find loop closures, we compare the current scan at time with scans in within a given radius (10 m in our tests), see Fig. 3. The current scan is then registered against each of the nearby scans using GICP to obtain a transformation. In order to find loop closures, GICP is initialized with zero translation and a rotation from the current pose estimates (from odometry). Zero translation was found to give more reliable results in narrow tunnel environments and avoid local minima in GICP, leveraging the topology of the environment that limits the differences in possible positions. After executing GICP, we check the fitness score [26] and reject loop closures with score larger than a threshold (5.0 in our experiments). The loop closures that pass the fitness test are then passed to the local back-end. Since loop closure detection involves executing GICP for many pairs of scans, we execute it on a parallel thread, and we only execute it at a subset of scans (the key-scans, see Section II-D).

Ii-C Vision Front-end and Artifact Positioning

The goal of the vision front-end is to detect objects of interest and estimate their relative position with respect to the robot. For instance, in the context of the DARPA Subterranean Challenge, the objects of interest are either artifacts (known objects of interest), or fiducial markers at known locations (only present at the entrance of the tunnel, for calibration purposes). We use YOLO [29] to detect the artifacts and the AprilTags library [30, 31, 32] to detect the fiducial markers. YOLO produces a 2D bounding box around a detected artifact; we then estimate the position of the artifact by using the bearing corresponding to the center of the bounding box and the range obtained from the depth channel of the RGB-D camera.

Ii-D Single and Multi-robot Pose Graph Back-end

We use a versatile SLAM back-end based on pose graph optimization (PGO) [3] and implemented in GTSAM [33]. The same PGO implementation is executed both locally at each robot and at the base station, as discussed below.

Ii-D1 Local (Single-Robot) PGO

The local back-end at robot receives the odometry measurements from the front-end and periodically instantiates a new key-scan: a key-scan is created after a minimum odometric displacement ( rotation or translation) and the corresponding pose and odometry edge are added to the local pose graph . When the back-end receives loop closure measurements from the Loop Closure Detection Module, we first check their quality using the ICM module described in Section II-E, and reject outliers. Then, a loop closure edge is added to . Finally, when the vision front-end produces an artifact (or fiducial) detection, a new variable is instantiated in the graph (the unknown artifact position) and an edge is added between the artifact and the corresponding observation pose. In the case of a second observation of an artifact, an additional edge is added between the observation pose and the existing artifact node. To compute the trajectory estimate, we optimize the pose graph using the Gauss-Newton implementation in GTSAM.

Ii-D2 Base station (Multi-robot) PGO

The base station receives the single pose graphs from each robot within communication range and merges them together in a single pose graph . We assume that the robots share a common reference frame (in practice, the fiducial markers can be used to establish a common frame). Before optimizing the multi-robot graph , we search for inter-robot loop closures; since all robots share a common reference frame, this can be done using the same approach described in Section II-B2.

Human-in-the-loop Pose Graph Editing. The versatility of our PGO also enables a more fluid interaction with the human operator. In some cases the operator can identify (by inspecting the map) a loop closure that has not been detected by the lidar front-end. In our system, the operator can use rviz [34] and the Robot Operating System (ROS) [35] to interact with the map and attempt a loop closure between a pair of poses: this is done by calling a ROS service with the IDs of the potential loop closure poses, which results in the addition of an edge in the pose graph. This functionality can be used during operation or as a post-processing step.

Ii-E Incremental Consistent Measurement Set Maximization

Before executing PGO, we check the quality of the lidar loop closures and remove outliers. This is done by the Incremental Consistency Maximization modules in Fig. 2. The ICM modules are largely based on the Pairwise Consistent Measurement (PCM) Set Maximization algorithm of Mangelson et al. [21], which we modify to enable online (single and multi-robot) operation. ICM includes the following steps.

Fig. 3: Local pose graph : poses, edges, and cycles.

Odometry Check. While ICM has been designed for the multi-robot case, we adapt it to the single-robot case. In the single-robot case, each loop closure can be tested both in terms of pairwise consistency against other loop closures as in [21], and against the odometry edges. As in PCM, the basic observation is that, in the absence of noise, relative pose measurements should compose to the identity along cycles in the graph [36]. Cycles are defined as complete loops in the graph, as depicted in Fig. 3. Assume that the front-end produces a putative loop closure measurement of the relative pose between poses and . Then, the measurements along the cycle formed by the loop closure and the odometry edges (in red in Fig. 3) must compose to the identity:


where is the relative pose obtained by chaining the odometry edges from pose to pose . Eq. (1) represents the pose error accumulated along the loop. If is an outlier, it will cause the error to be large. Denoting with the number of edges along the loop (7 in the example of Fig. 3), we reject as outliers loop closures inducing average errors larger than a threshold (in our tests, we limit the rotation error to 0.05 rad and the translation error to 0.1 m).

Pairwise Consistency Check. Using a check similar to the one described in the previous section, we check pairwise consistency between pairs of loop closures. Assume that the front-end produces a putative loop closure measurement . In order to check if is consistent with a previous loop closure , we compute the loop error as:


and we build an adjacency matrix keeping track of which pair of loop closures are pairwise consistent, as in [21]. Note that we use the simplified check (2) instead of the probabilistic check proposed in [21] since we do not have access to precise covariances for the odometry and loop closures.

Incremental Consistency Maximization. So far we have rejected gross outliers using the odometry check and have built an adjacency matrix that keeps track of which pair of loop closures is consistent. In our implementation, the matrix is not built from scratch, but we only add a row and column when a new loop closure is detected and passes the odometry check. Then, we follow [21] and find a set of mutually consistent loop closures by computing the maximum clique on the adjacency matrix. Loop closures in the maximum clique are considered inliers and added to the graph.

Type Location
Arch Pocahontas Mine 1100 m Coal Mine Beckley, WV
Beckley Exhibition Mine 1000 m Coal Mine Beckley, WV
Bruceton Safety Research (SR) Mine 1400 m Coal Mine Pittsburgh, PA
Bruceton Experimental (EX) Mine 700 m Coal Mine Pittsburgh, PA
Highland Mine 1400 m Coal Mine Logan, WV
Eagle Mine 500 m Gold Mine Julian, CA
TABLE I: Underground tunnels and mines explored in our field experiments. The distance explored, width and height are averaged and rounded values from our experiments.
Fig. 4: Partial ground truth map of the Bruceton Safety Research Mine, obtained by optimizing the best robot trajectory with ground truth position of fiducials, artifacts, and total station measurements during the tunnel circuit of the DARPA Subterranean Challenge.

Iii Experiments

Iii-a Test Environments and Setup

We perform experiments in six underground tunnels and mines, listed in Table I. These environments are extremely challenging both for perception and mobility. In the following we provide a short description of our system setup.

Robots: The robots used in our tests are Husky-A200 series ground vehicles, equipped with VLP-16 Puck Lite lidar, an Intel RealSense D435 RGB-D camera, and an Intel NUC 7i7DNBE ( GHz, 32 GB RAM) processor. The robots rely on communication radios (visible on the back of the robot in Fig. 1) which create a mesh network to provide communication between robots and the base station.

Base Station: The base station is an Intel Hades Canyon NUC8i7HVKVA ( GHz, 32 GB RAM). The base station is placed outside the mine to receive incoming telemetry data as well as pose graphs, and artifact localization results from all the robots deployed in the tunnel.

Ground Truth: Obtaining ground truth trajectory and map estimates in large-scale underground environments is a challenging task. After the tests in the Bruceton mines, DARPA provided ground truth (surveyed) locations of the artifacts. To obtain a proxy for the ground truth trajectories of our robots, we enforced the ground truth locations of the artifacts in the pose graph of each robot, and used the resulting optimized trajectory as ground truth as presented in Fig. 4; when available, we also added sporadic measurements of the robot poses obtained using a total station (TS) [37]. Since ground truth artifact locations are only available for the Bruceton mines, we report quantitative metrics for these mines, and visualize mapping results for the others.

Iii-B Results and Evaluation

Fig. 5: Left: odometric trajectories from scan-to-scan and scan-to-submap matching versus ground truth for (a) Bruceton Safety Research (SR) mine and (b) Experimental (EX) mine. Right: drift statistics for (c) SR and (d) EX.

Iii-B1 Odometry Evaluation

In this section, we show that the scan-to-submap matching (i) drastically improves the odometry accuracy when compared to the scan-to-scan matching, and (ii) in nominal conditions, it has a drift of of the distance traveled. Fig. 5 shows results in the Bruceton mines. Fig. 5(a)-(b) report the trajectory estimates for the Safety Research and the Experimental mine, respectively. The scan-to-scan matching quickly drifts, as it poorly estimates the pitch of the robot, due to the low vertical resolution of the Puck. The scan-to-submap matching has significantly better performance. Fig. 5(c)-(d) provides a quantitative evaluation of robot drift in terms of Relative Pose Error (RPE) using evo [47]. In the Safety Research mine, the scan-to-submap odometry has a drift smaller than of the distance travel. On the Experimental mine, the drift is larger due to the presence of featureless corridors that cause GICP to underestimate the motion of the robot along the direction of the tunnel (see discussion in Section IV).

Fig. 6: Map estimates with and without loop closure (LC) detection for (a) Eagle mine, (b) Bruceton Experimental Research mine, (c) Beckley Exhibition Coal Mine.
Fig. 7: A section of Bruceton Safety Research mine. Left: PGO without ICM; the map is visibly distorted. Right: PGO with ICM; map has no distortion since outlier loop closures are rejected.

Iii-B2 Loop Closure Evaluation

Fig. 6 reports a selected set of maps obtained with and without loop closure detection (Section II-B) in three underground mines. Fig. 6(a)-(b) show that loop closure detection avoids the appearance of spurious corridors in the map (resulting from the odometry drift) in (a) Eagle Mine and (b) Bruceton Experimental Mine. Fig.6(c) shows the map obtained in the Beckley Exhibition Coal mine. This is particularly interesting, due to the presence of a very long loop. In this case, using the standard (odometric) initialization in GCIP does not allow closing the loop; the loop closure poses highlighted in red in Fig.6(c) are too far, and GICP does not converge to a reasonable estimate. However, the approach in Section II-B is still able to correctly detect and enforce the loop closure.

Iii-B3 ICM Evaluation

Spurious loop closures are frequent in environments with repetitive appearance, such as tunnels and mines. Including outliers in the PGO typically results in a significant distortion of the entire map, hence their rejection is crucial to obtain accurate maps. Fig. 7 shows the impact of ICM on the mapping results (more results will also presented in the multi-robot subsection). The map on the left is obtained without ICM and is distorted due to inaccurate loop closures. The map on the right is obtained by enabling ICM: poor loop closures are removed by ICM and the resulting final map shows no visible distortion. We evaluate the quality of loop closures by measuring the end-to-end drift where the robot is started and stopped at the same location in the tunnel. The end-to-end error in the map obtained with ICM is 0.2 m which is significantly less than the 2.8 m end-to-end drift in the map without ICM.

We show the performance and accuracy of LAMP in the Bruceton Safety Research and Experimental mines in Fig. 8. The boxplots show the artifact localization error with and without loop closures (LC) and outlier rejection (ICM). In addition, the localization performance achieved in the tunnel circuit of the DARPA challenge is shown in green; the latter shows a reduced error due to the inclusion of total station measurements of the robot pose in the pose graph.

Fig. 8: Artifact localization accuracy using LAMP, with and without loop closures (LC) and ICM, and with total station (TS) measurements in the Bruceton Safety Research and Experimental mines during the Tunnel Circuit of the DARPA Subterranean Challenge.

Iii-B4 Multi-robot Mapping Evaluation

To demonstrate our multi-robot architecture, we show the mapping results obtained at the base station. For this experiment, we set the ICM translation and rotation thresholds to 0.05 m and 0.005 rad. In order to reject outlying inter-robot loop closures the GICP is initialized with robot odometry and a loop closure is confirmed if the computed fitness score is greater than a set threshold (0.18). Fig. 9 shows the fused map from two robots (a) without loop closures, (b) with loop closures but no ICM, and (c) with ICM. We observe that without loop closures, not only the drift in the single robot trajectory estimates becomes apparent, but also errors in the initial calibration (based on the fiducial markers) lead to a significant divergence between the trajectories of the two robots. If we enable loop closure detection, but we do not use ICM, a few incorrect loop closures visibly distort the map. With loop closures and ICM, the final map obtained is much improved. Fig. 9(c), however, shows that ICM is not able to detect and reject an incorrect loop closure made at the first turn in the tunnel.

Iv Discussion

In this paper we presented LAMP, a centralized multi-robot SLAM system, developed in the context of the DARPA Subterranean Challenge, which provides a robust estimate of the trajectories of multiple robots and constructs a point cloud map using 3D lidar data. The system also includes a vision front-end to detect and localize objects of interest.

While LAMP was instrumental for the CoSTAR team [23] to achieve second place in the tunnel circuit of the DARPA Subterranean Challenge, it is merely the result of the first phase of the competition, and, as such, it is a work in progress. We believe there are several aspects that can be improved: some of these aspects can be addressed using more advanced techniques in the literature, while others expose fundamental open research problems. We review the main outstanding technical challenges and future work below.

Fig. 9: Multi-robot mapping experiment with two robots at the Bruceton Safety Research mine. (a) Base station map without performing inter-robot loop closure; (b) base station map after inter-robot loop closure detection is enabled (no ICM); (c) base station map after inter-robot loop closure detection and ICM are enabled.

Lidar Odometry: Our choice of the lidar front-end odometry worked well in nominal conditions; however, as shown in Fig. 5(b),(d) it might underestimate the robot motion in long symmetric corridors without detectable geometric features. In these scenarios, the scan matching problem become intrinsically ambiguous and GICP is easily trapped in local minima. This degeneracy is a well-known problem in lidar-SLAM and can be mitigated by fusing information from other sensors (e.g., wheel odometry, vision [4, 7], IMU [20, 38]), or adopting more recent methods that attempt to explicitly model the degeneracy [45].

Loop Closure Detection: While using the odometry initialization in GICP leads to missing many loop closure opportunities, the initialization proposed in this paper has the advantage of computing a large number of putative loop closures. Indeed, we are able to capitalize on loop closure opportunities despite a large amount of odometric drift. The downside is that our approach also produces a large amount of outliers. A natural evolution of the proposed system is to use feature-based approaches for robust global scan matching that do not require an initial guess [39]. Another practical solution might be to leverage other sensing modalities, e.g., use vision to detect loop closures [4]. In addition, computation of precise covariances for loop closures and odometry is an avenue for future work.

Outlier Rejection: While we discussed ICM as a pragmatic solution to reject outlier loop closures (based on [21]), we believe that more research is needed to obtain truly robust SLAM solutions. ICM currently requires extensive parameter tuning and it often fails to strike a good balance between rejecting outliers and keeping inliers. This is expected, due to the approximate nature of the pairwise consistency checks. Recent approaches [46] attempt to simultaneously assess the consistency of all loop closures accepted as inliers, but they currently do not scale well to large problems. The development of a robust global solver for PGO would increase the reliability of SLAM and relieve the user from time-consuming and problem-specific parameter tuning.

Multi-robot Mapping: LAMP is currently a centralized SLAM system. While a centralized system works well for a small team of robots, we observed communication bottlenecks when performing tests with more than four robots. The literature offers fully distributed PGO solutions to reduce the communication burden [40, 41, 42, 43, 44]; however, these are not robust to outliers, which is a key requirement in our context. For underground exploration, it is also crucial to exchange a map representation and possibly provide an interpretable visualization to the user. Point clouds are relatively cumbersome models and exchanging them (even after downsampling) quickly becomes impractical. Therefore, it is crucial to use map compression techniques (e.g., [17, 48]) to reduce the communication bandwidth. In general, it would be desirable to have an automatic approach to adjust the type and amount of exchanged data depending on the available bandwidth. More compact and uncertainty-aware representations [49] that can enable accurate risk prediction for motion planning are another direction for future research.


  • [1] J. Haruyama, T. Morota, S. Kobayashi, S. Sawai, P. G. Lucey, M. Shirao, and M. N. Nishino, “Lunar Holes and Lava Tubes as Resources for Lunar Science and Exploration,” Moon, pp. 139–163, 2012.
  • [2] D. Tardioli, L. Riazuelo, D. Sicignano, C. Rizzo, F. Lera, J. L. Villarroel, and L. Montano. “Ground robotics in tunnels: Keys and lessons learned after 10 years of research and experiments.” Journal of Field Robotics 36, no. 6, pp. 1074-1101, 2019.
  • [3] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard. “Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age.” IEEE Transactions on robotics 32, no. 6, pp. 1309-1332, 2016.
  • [4] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos. “ORB-SLAM: a versatile and accurate monocular SLAM system.” IEEE transactions on robotics 31, no. 5, pp. 1147-1163, 2015.
  • [5] M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust visual inertial odometry using a direct ekf-based approach,” in Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on. IEEE, 2015, pp. 298–304.
  • [6] S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based visual–inertial odometry using nonlinear optimization,” The International Journal of Robotics Research, vol. 34, no. 3, pp. 314–334, 2015.
  • [7] S. Khattak, F. Mascarich, T. Dang, C. Papachristos, and K. Alexis. “Robust Thermal-Inertial Localization for Aerial Robots: A Case for Direct Methods.” In 2019 International Conference on Unmanned Aircraft Systems (ICUAS), pp. 1061-1068. IEEE, 2019.
  • [8] A. Nuchter, H. Surmann, K. Lingemann, J. Hertzberg, and S. Thrun. “6D SLAM with an application in autonomous mine mapping.” In IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA’04. 2004, vol. 2, pp. 1998-2003. IEEE, 2004.
  • [9] J. Zhang, and S. Singh. “LOAM: Lidar Odometry and Mapping in Real-time.” In Robotics: Science and Systems, vol. 2, p. 9. 2014.
  • [10] W. Hess, D. Kohler, H. Rapp, and D. Andor. “Real-time loop closure in 2D LIDAR SLAM.” In 2016 IEEE International Conference on Robotics and Automation (ICRA), pp. 1271-1278. IEEE, 2016.
  • [11] T. Shan, and B. Englot. “LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain.” In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758-4765. IEEE, 2018.
  • [12] X. Ji, L. Zuo, C. Zhang, and Y. Liu. “LLOAM: LiDAR Odometry and Mapping with Loop-closure Detection Based Correction.” In 2019 IEEE International Conference on Mechatronics and Automation (ICMA), pp. 2475-2480. IEEE, 2019.
  • [13] R. Dubé, A. Cramariuc, D. Dugas, J. Nieto, R. Siegwart, and C. Cadena. “SegMap: 3D segment mapping using data-driven descriptors.” arXiv preprint arXiv:1804.09557 (2018).
  • [14] S. Thrun, D. Hahnel, D. Ferguson, M. Montemerlo, R. Triebel, W. Burgard, C. Baker, Z. Omohundro, S. Thayer, and W. Whittaker. “A system for volumetric robotic mapping of abandoned mines.” In 2003 IEEE International Conference on Robotics and Automation (Cat. No. 03CH37422), vol. 3, pp. 4270-4275. IEEE, 2003.
  • [15] D. Tardioli, D. Sicignano, L. Riazuelo, J. L. Villarroel, and L. Montano. “Robot teams for exploration in underground environments.” In Workshop ROBOT11: Robótica Experimental, pp. 205-212. 2012.
  • [16] D. Tardioli, L. Riazuelo, T. Seco, J. Espelosín, J. Lalana, J. L. Villarroel, and L. Montano. “A robotized dumper for debris removal in tunnels under construction.” In Iberian Robotics conference, pp. 126-139. Springer, Cham, 2017.
  • [17] R. Zlot and M. Bosse, “Efficient large-scale 3d mobile mapping and surface reconstruction of an underground mine.” In Field and service robotics, pp. 479-493. Springer, Berlin, Heidelberg, 2014.
  • [18] M. Leingartner, J. Maurer, A. Ferrein, and G. Steinbauer. “Evaluation of sensors and mapping approaches for disasters in tunnels.” Journal of field robotics 33, no. 8, pp. 1037-1057, 2016.
  • [19] A. Jacobson, F. Zeng, D. Smith, N. Boswell, T. Peynot, and M. Milford. “Semi-supervised slam: Leveraging low-cost sensors on underground autonomous vehicles for position tracking.” In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3970-3977. IEEE, 2018.
  • [20] C. L. Gentil, T. Vidal-Calleja, and S. Huang. “IN2LAAMA: INertial Lidar Localisation Autocalibration And MApping.” arXiv preprint arXiv:1905.09517 (2019).
  • [21] J. G. Mangelson, D. Dominic, R. M. Eustice, and R. Vasudevan. “Pairwise consistent measurement set maximization for robust multi-robot map merging.” In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 2916-2923. IEEE, 2018.
  • [22] ”DARPA Subteranean (SubT) Challenge.” [Online]. Available:
  • [23] ”NASA/JPL Team CoSTAR website.” [Online]. Available:
  • [24] ”Berkeley Localization and Mapping.” [Online]. Available:
  • [25] ”Velodyne Puck LITE lidar.” [Online]. Available:
  • [26] R. B. Rusu, and S. Cousins. “3d is here: Point cloud library (pcl).” In 2011 IEEE international conference on robotics and automation, pp. 1-4. IEEE, 2011.
  • [27] A. Segal, D. Haehnel, and S. Thrun. “Generalized-icp.” In Robotics: science and systems, vol. 2, no. 4, p. 435. 2009.
  • [28] T. Tomic, K. Schmid, P. Lutz, A. Domel, M. Kassecker, E. Mair, I. L. Grixa, F. Ruess, M. Suppa, and D. Burschka, “Toward a fully autonomous uav: Research platform for indoor and outdoor urban search and rescue,” IEEE robotics & automation magazine, vol. 19, no. 3, pp. 46–56, 2012.
  • [29] J. Redmon, and A. Farhadi. “Yolov3: An incremental improvement.” arXiv preprint arXiv:1804.02767 (2018).
  • [30] D. Malyuta, C. Brommer, D. Hentzen, T. Stastny, R. Siegwart, and R. Brockers, “Long-duration fully autonomous operation of rotorcraft unmanned aerial systems for remote-sensing data acquisition,” Journal of Field Robotics, p. arXiv:1908.06381, Aug. 2019.
  • [31] C. Brommer, D. Malyuta, D. Hentzen, and R. Brockers, “Long-duration autonomy for small rotorcraft UAS including recharging,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, IEEE, p. arXiv:1810.05683, oct 2018.
  • [32] J. Wang and E. Olson, “AprilTag 2: Efficient and robust fiducial detection,” in Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)”, October 2016.
  • [33] F. Dellaert, “Factor graphs and GTSAM: A hands-on introduction,” Georgia Institute of Technology, Atlanta, GA, USA, Tech. Rep. GT-RIMCP&R-2012-002, Sep. 2012.
  • [34] ”3D ROS visualization tool.” [Online]. Available:
  • [35] ”Robot Operating System.” [Online]. Available:
  • [36] L. Carlone, R. Aragues, J. A. Castellanos, and B. Bona. “A fast and accurate approximation for planar pose graph optimization.” Intl. J. of Robotics Research, 33(7):965–987, 2014.
  • [37] ”Nikon DTM322 Total Station.” [Online]. Available:
  • [38] R. Opromolla, G. Fasano, G. Rufino, M. Grassi, and A. Savvaris. “LIDAR-inertial integration for UAV localization and mapping in complex environments.” In 2016 International Conference on Unmanned Aircraft Systems (ICUAS), pp. 649-656. IEEE, 2016.
  • [39] H. Yang and L. Carlone,“A Polynomial-time Solution for Robust Registration with Extreme Outlier Rates”,Robotics: Science and Systems (RSS), 2019.
  • [40] P. Y. Lajoie, B. Ramtoula, Y. Chang, L. Carlone, and G. Beltrame. “DOOR-SLAM: Distributed, online, and outlier resilient SLAM for robotic teams.” IEEE Robotics and Automation Letters (2020).
  • [41] S. Choudhary, L. Carlone, C. Nieto, J. Rogers, H. I. Christensen, and F. Dellaert. “Distributed mapping with privacy and communication constraints: Lightweight algorithms and object-based models.” The International Journal of Robotics Research 36, no. 12, pp. 1286-1311, 2017.
  • [42] A. Cunningham, V. Indelman, and F. Dellaert. “DDF-SAM 2.0: Consistent distributed smoothing and mapping.” In 2013 IEEE international conference on robotics and automation, pp. 5220-5227. IEEE, 2013.
  • [43] M. J. Schuster, K. Schmid, C. Brand, and M. Beetz. “Distributed stereo vision‐based 6D localization and mapping for multi‐robot teams.” Journal of Field Robotics 36, no. 2 (2019): 305-332.
  • [44] T. A. Vidal-Calleja, C. Berger, J. Solà, and S. Lacroix. ”Large scale multiple robot visual mapping with heterogeneous landmarks in semi-structured terrain.” Robotics and Autonomous Systems 59, no. 9 (2011): 654-674.
  • [45] A. Hinduja, B-J. Ho, M. Kaess. “Degeneracy-Aware Factors with Applications to Underwater SLAM.” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019 (preprint).
  • [46] P. Lajoie, S. Hu, G. Beltrame, and L. Carlone. “Modeling Perceptual Aliasing in SLAM via Discrete-Continuous Graphical Models.” IEEE Robotics and Automation Letters (RA-L), 2019.
  • [47] M. Grupp, “evo: Python package for the evaluation of odometry and slam.”, 2017.
  • [48]

    C. O’Meadhra, W. Tabib, N. Michael. “Variable Resolution Occupancy Mapping Using Gaussian Mixture Models.” IEEE Robotics and Automation Letters (RA-L), 4(2):2015-2022, 2019.

  • [49] A. Agha-Mohammadi, E. Heiden, K. Hausman, and G. Sukhatme. “Confidence-rich grid mapping.” The International Journal of Robotics Research (2017): 0278364919839762.