SLAM Toolbox: SLAM for the dynamic world

Developments in the field of mobile robotics and autonomous driving have resulted in the use of robots and vehicles in retail stores, hospitals, warehouses, on the roads, and on sidewalks. These deployed areas are both dynamic and frequently massive in scale. The average size of a Walmart store is over 16,000 m (Walmart, 2020) and a single square block in Chicago is over 21,000 m (Heramb, 2007). Retail and warehouse spaces can change drastically throughout the year and the state of roadways can be changing by the hour. Much work has been made to address changing environments in robot perception (Macenski, Tsai, et al., 2020), but less has been built in open-source to represent maps of dynamic spaces.

For fully autonomous deployed systems to operate in these large and changing environments, they require tools that can be used to accurately map an area specified for their operation, update it over time, and scale to handle mapping of some of the largest indoor and outdoor spaces imaginable. The field of Simultaneous Localization and Mapping (SLAM) aims to solve this problem using a variety of sensor modalities, including: laser scanners, radars, cameras, encoders, gps and IMUs. The most commonly used perception sensor used for localization and mapping in industrial environments is the laser scanner (Chong et al., 2015). SLAM methods using laser scanners are generally considered the most robust in the SLAM field and can provide accurate positioning in the presence of dynamic obstacles and changing environments (Cole & Newman, 2006).
Previously existing open-source laser scanner SLAM algorithms available to users in the popular Robot Operating System (ROS) include GMapping, Karto, Cartographer, and Hector. However, few of these can build accurate maps of large spaces on the scale of the average Walmart store. Even fewer can do so in real-time using the mobile processor typically found in mobile robot systems today. The only package that could accomplish the above was Cartographer. However, it was abandoned by Google and it is no longer maintained.
We propose a new fully open-source ROS package, SLAM Toolbox, to solve this problem. SLAM Toolbox builds on the legacy of Open Karto (Konolige et al., 2010), the open-source library from SRI International, providing not only accurate mapping algorithms, but a variety of other tools and improvements. SLAM Toolbox provides multiple modes of mapping depending on need, synchronous and asynchronous, utilities such as kinematic map merging, a localization mode, multi-session mapping, improved graph optimization, substantially reduced compute time, and prototype lifelong and distributed mapping applications.

This package, slam_toolbox is open-source under an
LGPLv2.1 at https://github.com/ SteveMacenski/slam_toolbox.git and is available in every current ROS distribution. It was also selected as the new default SLAM vendor in ROS 2, the second generation of robot operating systems, replacing GMapping. SLAM Toolbox was integrated into the new ROS 2 Navigation2 project, providing real-time positioning in dynamic environments for autonomous navigation (Macenski, Martín, et al., 2020). It has been shown to map spaces as large as 24,000 m 2 , or 250,000 f t 2 , in real-time by non-expert technicians. An example map can be seen in Figure 1.

Statement of Need
This software package provides an approach for multi-session mapping and localization. It also utilizes modern graph-optimization techniques and provides first of its kind pose-graph manipulation tools and demo applications. Below we discuss related work and how slam_to olbox is meeting a currently unmet need in the community.
GMapping is one of the most commonly used SLAM libraries, presented in 2007. It uses a particle filter approach to SLAM for the purpose of building grid maps from 2D lidar data. However, GMapping is not well suited for large spaces and fails to accurately close loops at an industrial scale. Additionally, filter-based approaches cannot be easily reinitialized across multiple sessions.
HectorSLAM relies on lidar scan matching and 3D navigation filter based on EKF state estimation. This method focuses on real-time robot pose estimation and generates 2D map with high update rate. Unlike other mentioned methods, Hector does not use odometry data, which can cause inaccurate pose and map updates when lidar scans arrive at a lower rate, or when mapping large or featureless spaces. HectorSLAM however does not provide loop closure capabilities, making it unsuitable for reliable mapping of large spaces or when using laser scanners with low update rates.
KartoSLAM and Cartographer are both graph-based algorithms that store a graph of robot poses and features. Graph-based algorithms have to maintain only the pose-graph, which usually makes it efficient in handling resources, especially while building maps of a large scale. KartoSLAM uses Sparse Bundle Adjustment for loop-closure graph optimization. Cartographer consists of front-end, which is in charge of scan matching and building trajectory and submaps, and back-end that does the loop closure procedure. The graph solver used in Cartographer is Google Ceres (Agarwal et al., n.d.). Cartographer provides pure localization mode, when user has a satisfactory map for usage. It also provides data serialization for storing processed submaps only. However, Cartographer has stopped maintenance and support from Google and has been largely abandoned. Further, it fails to build suitable maps for annotation and localization with other localization software packages on robotic platforms without exceptional odometry. The software's unusual complexity makes it challenging to modify or resolve seemingly simple issues, making it not suitable for many applications.

Features
SLAM Toolbox is able to map spaces effectively using mobile Intel CPUs commonly found on robots well in excess of 100,000 f t 2 . It can be done easily using untrained technicans typically hired to deploy robot solutions or remotely using monitoring systems. Some applications have been created to automatically map a space using SLAM Toolbox as well as paired with exploration planners.
It can also serialize a current mapping session and deserialize it at a later time to continue refining or expanding an existing map. This serialization saves the complete raw data and pose-graph rather than submaps, as in Cartographer, allowing a variety of novel tools to be developed and more accurate multi-session mapping. These utilities include manual posegraph manipulation, whereas a user can manually manipulate the pose-graph nodes and data to rotate a map or assist in a challenging loop closure, shown in Figure 2. It also includes kinematic map merging, the process of merging multiple serialized maps into a composite map. A 3D visualizer plugin was also created to assist in utilization of these tools and the core SLAM library capabilities. Many additional tools and utilities could be developed using this representation as well. It provides 3 major operation modes and executables: synchronous mapping, asynchronous mapping, and pure localization. Synchronous mapping provides the ability to map and localize in a space keeping a buffer of measurements to add to the SLAM problem. This can be advantageous when the quality of the map is of particular importance or when doing offline processing. By contrast, the asynchronous mode will only process new measurements when the last measurement is completed and the new update criteria are met. As a result, this will never lag behind real-time when running complex loop closures. However, the map may not include all valid measurements if processing the last one takes too long. This mode is advantageous when the quality of real-time localization is of particular importance. Both of these modes can be used for multi-session SLAM, the process of reloading a prior session and continuing to refine the pose-graph. Figure 3 shows a map of a large office building created by partially mapping in one session and completing the map in another session. This map has multiple loop closures between the two datasets and was later used with the pure-localization mode to navigate autonomously.
The pure localization mode cannot be used to persist changes in the environment. Instead, it uses a rolling buffer of measurements in the current session and matches them against the original session(s) measurements and pose-graph. The current session's measurements will be added to the pose-graph with new constraints and nodes in the graph. This allows changes in the environment to be embraced to increase localization quality based on new features or moved objects. Over time, the measurements in the rolling buffer will "expire" and be removed from the pose-graph and localization problem, reverting the pose-graph to its original state for that region. The authors refer to this process as elastic pose-graph deformation. An interesting side effect is that the pure-localization mode can be used for effective lidar odometry when paired with no prior mapping session data. It will simply match against its local buffer and keep only a recent view of the environment, allowing lidar odometry to scale to infinite sized spaces. Finally, many updates were made to the OpenKarto SLAM libary. The measurement matching methods were restructured for a 10x speed-up enabling multi-threaded processing. The provided Sparse Bundle Adjustment optimization interface was replaced with Google Ceres, providing faster and more flexible optimization settings. Additionally, the optimizer interface was turned into a run-time dynamically loaded plugin interface to allow future developers to use the latest and greatest in optimization technologies without modifying the original code. Serialization and deserializion support was enabled to allow for saving and reloading mapping sessions. Finally, new processing modes and K-D tree search were developed to process measurements to enable localization and multi-session mapping. Various smaller improvements and optimizations were also made but excluded for brevity.

Robots Using slam_toolbox
SLAM Toolbox has been integrated, tested, and deployed on a number of robot platforms across the world by both industry and researchers. It is also the default SLAM vendor for ROS 2.
A few known examples where SLAM Toolbox has been used or is being used are: