Efficient Dense Frontier Detection for 2D Graph SLAM Based on Occupancy Grid Submaps

TL;DR

This paper introduces an efficient frontier detection method for 2D graph SLAM using occupancy grid submaps, enhancing autonomous exploration by improving map building and localization robustness.

Contribution

It proposes a specialized frontier detection approach that is constrained to active submaps and robust to SLAM loop closures, advancing exploration efficiency.

Findings

Improved frontier detection efficiency in 2D SLAM

Robustness to SLAM loop closures demonstrated

Enhanced autonomous exploration capabilities

Abstract

In autonomous robot exploration, the frontier is the border in the world map between the explored space and unexplored space. The frontier plays an important role when deciding where in the environment the robots should go explore next. We examine a modular control system pipeline for autonomous exploration where a 2D graph SLAM algorithm based on occupancy grid submaps performs map building and localization. We provide an overview of the state of the art in frontier detection and the relevant SLAM concepts and propose a specialized frontier detection method which is efficiently constrained to active submaps, yet robust to SLAM loop closures.

Tables

Table 1. TABLE I: Experimental setup and results

Testing setup details

Intel i7 6800K @ 3600 MHz, Ubuntu 18.04, ROS Melodic

Cartographer offline node, exit before final optimization

No RViz visualization

Dataset: cartographer_paper_deutsches_museum.bag

Undisclosed IMU and multi-echo 2D lidar (scan rate: $\sim$36 Hz)

Duration: 1912 s  Covered area: $\sim$12 000 m2  Path length: 2272 m

Differences from default SLAM parameters provided by Google:

POSE_GRAPH.constraint_builder.sampling_ratio = 0.1

MAP_BUILDER.num_background_threads = 10

Wall clock SLAM processing time (not including final optimization)

without frontier detection: 360 s (5.3x realtime on average)

with asynchronous frontier detection: 434 s (4.4x realtime on average)

with synchronous frontier detection: 605 s (3.2x realtime on average)

Wall frequency of asynchronous frontier updates

Average: 78 Hz (13 ms)  Std. deviation: 22 ms

Average synchronous frontier update processing wall time

(605 s $\pm$ 4.5 s - 360 s) / 37816 scans = 6.5 ms per scan $\pm$ 0.12 ms

SLAM events

Total submap update events (inserted laser scans): 37816

Skipped submap update events (asynchronous only): 3900/37816

Optimization events handled during bag processing:

Asynchronous: 355 out of 420  Synchronous: 412 out of 420

Average wall time between optimization events:

Asynchronous: 1.22 s      Synchronous: 0.68 s

Dataset: Freiburg FR-079 uncorrected

SICK LMS 2D lidar (scan rate: 4.7 Hz)

Duration: 1061 s  Covered area: $\sim$90 m2   Path length: 396 m

Wall clock SLAM processing time (not including final optimization)

without frontier detection: 16.5 s (64x realtime on average)

with synchronous frontier detection: 20.3 s (52x realtime on average)

Average synchronous frontier update processing wall time

(20.3 s $\pm$ 0.3 s - 16.5 s) / 4198 scans = 0.92 ms per scan $\pm$ 0.08 ms

SLAM events

Total submap update events (inserted laser scans): 4198

Skipped submap update events: none (synchronous)

Optimization events handled during bag processing: 25 out of 27

Average wall time between optimization events: 0.82 s

Equations

$$\text{class}(p={\mathbf{S}}_{k,l}^{si}):=\{\begin{array}{ll}\text{unoccupied}& p<0.5\\ \text{occupied}& p>0.5\\ \text{unobserved}& p=0.5\end{array}$$

$$O(\log |S|+A(si)+P(L{F}_{si})\cdot |S_{\cap si}|+P(\bigcup _{sj\in S_{\cap si}}G{F}_{sj}))$$

$$O(|S|\cdot (\log |S|+P(GF))+P(FF))$$

Full text

I Introduction

A fully autonomous mobile robot, able to explore, navigate and perform actions in an unknown environment is one of the ultimate objectives of today’s mobile robotics research. To this end, we consider a single autonomous robot or an autonomous team of robots tasked with exploring the unknown environment. The autonomous exploration problem comprises collecting the data sensed from the environment, using the collected data to build a structured model of the environment, self-localizing in the environment model, high-level planning and scheduling of robot tasks (mission generation and assignment), and path planning and following. All of these need to be performed in real time.

In a common approach known as frontier exploration, the robot maintains information about the border which divides the explored and unexplored space in the environment – the frontier. Elements of the frontier represent places in the environment which the robot may approach and thereby increase the knowledge about the structure of the environment. With the information about the exploration frontier available, mission planning can be described in its simplest version as (boldly) go where no one has gone before.

Many components of the autonomous exploration problem are complex enough to be associated with their own field of robotics research, resulting in sophisticated methods and software modules being available for solving them. Namely, building the model of the environment (a map) and self-localization therein may be performed by a module implementing a Simultaneous Localization and Mapping (SLAM) algorithm. For this reason, we consider a control system implementing a modular exploration pipeline as depicted in Figure 1. Map building and localization are performed by a SLAM module. The map built by SLAM is processed in the frontier detection module, and finally the map, the localization pose and the detected frontier are used by the later stages of the pipeline (exploration task generation and scheduling, path planning and following) to guide the robot according to an exploration strategy.

The scope and the main contribution of this paper is the frontier detection part of the exploration pipeline – a new, efficient frontier detection approach, specialized for use with a 2D graph SLAM algorithm based on occupancy grid submaps [1]. The proposed approach is robust to loop closures and exploits the submap structure of the SLAM algorithm in order to quickly perform frontier updates. By providing high frequency incremental frontier updates which enable more responsive planning of exploration objectives, a real-time use case is facilitated on large and complex maps, e.g. the Deutsches Museum dataset [1]. All the while the proposed frontier detection algorithm delivers a result at least as good as a naive frontier edge-detection algorithm, i.e. performing edge detection on a completely assembled global map each time after SLAM updates the map by inserting scans or optimizing the pose graph to perform loop closures.

II Related Work

II-A Frontier Exploration as a Prevalent Exploration Method

Frontier exploration in the context of autonomous robotics was first introduced by Yamauchi in 1997 [2], paving the way for many others ([3, 4, 5]). Commonly, elements of the detected frontier are used as navigation goals during planning of exploration tasks. Building on this, there are more complex exploration strategies which attempt to coordinate entire robot teams ([6, 7]), or use frontiers as sinks in a potential field ([8]). Frontier detection is therefore a key elementary operation in frontier exploration, and it is important that it be performed as quickly as possible so that exploration can be more efficient [9].

II-B State of the Frontier Detection Art

A naive algorithm for frontier detection is to perform edge detection on the complete global map after each map update. However, this approach is not feasible for larger maps and real-time robot operation with such maps, as it presents a significant computational burden.

II-B1 Keidar and Kaminka’s seminal work on efficient frontier detection

Keidar and Kaminka [10] proposed in 2014 several approaches which attempt to perform frontier detection in an efficient manner. The first, Wavefront Frontier Detector (WFD), consists of running two consecutive breadth-first searches (BFS). The first BFS starts at the robot position and continues throughout the unoccupied space, until eventually a frontier point is found which belongs to a component of connected frontier points. From there, the rest of the connected component is found by a second BFS along the connected frontier points. While WFD avoids searching the unobserved space, it still searches all observed space in each iteration, which may degenerate into a full map search as exploration progresses.

The second approach to frontier detection proposed by Keidar and Kaminka, the Fast Frontier Detector (FFD), does not use the map built by SLAM, but rather constructs the contour of each laser scan using Bresenham’s line algorithm, and uses the constructed contour to detect the frontier and store it in a specialized data structure. Quin and Alempijević [9] note that FFD has to be executed after each scan, which results in many wasteful calculations if frontier updates are required only occasionally, and that Bresenham’s line algorithm can cut across unobserved space and miss some frontier cells. Our proposed approach does not require execution after each processed scan, supporting a use case where frontier updates are required only occasionally.

FFD is also notable for introducing the concept of active area – a bounding box positioned in the map around the robot position, circumscribing the last scan the map was updated with. The frontier update step is sped up by restricting it to the active area. Keidar and Kaminka also applied this concept to the WFD detector, yielding the incremental WFD (WFD-INC) algorithm, which requires non-trivial auxiliary data structures for frontier point maintenance. Our proposed algorithm has a similar concept of active submaps.

II-B2 Impact of loop closure in SLAM on frontier detection

Loop closure is an event when the SLAM algorithm recognizes that the robot has revisited the same place, and then makes a correction using this information which reduces the error caused by drift in localization along the whole loop. In order to return a correct result, frontier detection has to be able to efficiently cope with the map changes induced by loop closure corrections. These map changes are not confined to the active area – performing loop closure results in widespread changes all over the map. While an efficient frontier detection algorithm should avoid reassembling and iterating throughout the entire global map in every iteration, constraining the algorithm to only the active area makes it difficult to be robust to loop closures. WFD-INC addresses loop closure events by evicting the detected frontier and performing frontier detection from scratch using the new loop-corrected map.

To efficiently address loop closures, the frontier detection algorithm needs to get intimate to a certain degree with the implementation of the SLAM algorithm. For example, Keidar and Kaminka additionally proposed an implementation of WFD-INC for GMapping (a particle filter-based SLAM [11]) called incremental parallel WFD (WFD-IP). WFD-IP performs in parallel separate WFD-INC frontier detection for each particle (each particle having its own map). Like WFD-INC, our proposed method uses the internals of the SLAM algorithm in order to perform frontier detection faster while also being robust to loop closure.

Quin and Alempijević [9] introduce two frontier detection methods: naive active area (NaiveAA), which is the naive approach confined to the active area, and a version of WFD called Expanding WFD (EWFD) which steers the WFD breadth-first search into newly discovered unoccupied areas. EWFD assumes that the entropy for each cell can only decrease over time. This is not true in the general case for the complete global map when considering effects of loop closure – observed areas can get moved around in the global map during loop closure and leave unexplored space in their wake. However, the entropy decrease assumption is almost surely true for single submaps in submap-based SLAM, and we exploit this fact in our proposed approach.

II-B3 Other approaches

Senarathne and Wang [12] use an oriented bounding-box based inexact approach.

Umari [13] uses rapidly-exploring random trees (RRT) to perform sparse frontier detection by building a tree inside the unoccupied space in the map built by SLAM. When the algorithm crosses the frontier while trying to expand the random tree, a single frontier point is detected. However, using the implementation of the algorithm provided in [13] does require reassembling the global map in each iteration. Also, this algorithm is not robust to loop closure, since the built RRT tree does not follow the results of pose graph optimization. In our experiments, we have observed various problems with performance of RRT frontier detection in narrow corridors and with large maps, prompting us to devise the method for dense frontier detection proposed herein.

III Prerequisites

III-A Simultaneous Localization and Mapping

The term SLAM was coined by Leonard and Durrant-Whyte in 1991 [14]. As shown in the block diagram of the exploration pipeline in Figure 1, a SLAM algorithm uses sensor data to build a map and perform localization, which is further used in frontier detection, exploration task planning and execution. There is a wealthy trove of SLAM methods developed to this day, which can be roughly grouped into methods based on filtering and methods based on graph optimization. We will focus on graph SLAM, which represents poses and detected features as nodes in a graph, while the correspondences which impose constraints on the poses of the respective nodes are represented as graph edges. Various optimization methods may be used to minimize the residual error of all constraints, e.g. the Ceres solver [15].

Submaps are small local maps which are merged into a global map. One of earlier approaches to SLAM using submaps is [16], with further examples being [17] and [18].

III-B Cartographer

The proposed frontier detection method was designed for use with Cartographer, an open-source multi-robot multi-trajectory 2D and 3D graph SLAM based on occupancy grid submaps, developed by Google (Hess, Kohler, Rapp in 2016 [1]). Cartographer’s approach of optimizing the poses of all scans and submaps follows Sparse Pose Adjustment [19] and uses the Ceres solver [15] for optimizing the pose graph using the Levenberg–Marquardt algorithm (LMA).

Submaps in Cartographer are spatially and temporally compact occupancy grid maps, typically of resolution 0.05 m, made from a short, continuous series of rangefinder sensor measurements (laser scans) taken during traversal of a short section of the robot trajectory. It is desired that the size of the submaps be small enough such that the localization drift is not perceptible within a single submap.

Cartographer maintains a pair of active submaps that the laser scans are inserted into, according to a local pose obtained by performing scan matching against the older (more complete) submap from the active submap pair.

The occupancy probabilities of grid cells are initially unobserved i.e. unknown (exactly 0.5). When a predetermined fixed number of scans $n_{scans}$ is inserted into a submap, it is marked as finished, and a new submap is created to take its place in the active submap pair. Importantly, once a submap is finished, its occupancy grid is immutable from that point onward. Cell occupancy probabilities are clamped to the interval $[0.1,0.9]$ and are stored linearly mapped onto the space of unsigned 16-bit integers. Scan insertion into submaps is performed as Bayesian updates of the cell occupancy probabilities (see (3) in [1]). The cells corresponding to laser hit points are updated with “occupied” observations, while the intermediate points (obtained by casting rays from the laser rangefinder origin to hit points) are updated with “unoccupied” observations.

It is important to note that on a level of a single submap, the cell entropy can be assumed to be monotonically decreasing, i.e. the cells of an active submap cannot become unobserved once they are observed.

When Cartographer detects loop closures, pose constraints between the corresponding trajectory nodes and submaps are added as edges into the pose graph. Optimization is periodically invoked in order to find a new solution – a set of global submap and trajectory node poses. As discussed, for frontier detection, this implies that when pose graph optimization is performed, the submaps can and do get displaced and rotated (i.e. undergo rigid transformations), although their occupancy grids are immutable after they are marked as finished. Our proposed frontier detection approach takes advantage of these properties of Cartographer.

IV Frontier detection

IV-A Definitions

Rigid transformation $\mathbf{T}^{b}_{a}\in\mathbf{SE}(3)$ is the pose of the coordinate system $b$ relative to the coordinate system $a$. Transforming a point with coordinates expressed in $b$, $\mathbf{p}^{b}\in\mathbb{R}^{3}$, into the corresponding point $\mathbf{p}^{a}$ in $a$ is denoted as $\mathbf{p}^{a}=\mathbf{T}^{b}_{a}\;\mathbf{p}^{b}$.

The global map coordinate system is denoted with $g$. The solution of pose graph optimization are poses of graph nodes expressed with respect to $g$.

Submap is an occupancy grid with a local submap coordinate system, in which the submap occupancy grid cells are indexed with 2-integer tuples: $\mathbf{S}^{si}_{k,l}$ is the occupancy probability value of the cell $(k,l)$ in the submap $si$. A submap is finished after being updated with $n_{scans}$ consecutive laser scans. The set of active submaps contains the submaps which are not yet finished.

Global submap pose $\mathbf{T}_{g}^{si}$ is the global pose in $g$ of the origin of the local coordinate system of a submap $si$. Global submap poses are part of the optimized pose graph solution.

Occupancy classification – the cell occupancy probability values are not used directly in frontier detection. The probability values are first classified according to the following thresholding rule:

[TABLE]

Observed cells are occupancy grid cells that are not unobserved.

Local frontier point is the center of an unobserved occupancy grid cell which is adjacent to a unoccupied cell in the same submap. Local frontier of a submap is the set of its local frontier points. The red points in the first two pictures in Figure 2 are examples of local frontiers.

Stabbing query refers to looking up the corresponding cell in a given submap for a given global point. More precisely, for a submap $si$ and a global point $\mathbf{p}^{g}$, to find the occupancy grid cell $(k,l)$ in $si$ whose center coordinates in the local coordinate system of the submap $si$ are closest to $(\mathbf{T}_{g}^{si})^{-1}\mathbf{p}^{g}$. Further, performing a stabbing query test means checking if the corresponding cell $\mathbf{S}^{si}_{k,l}$ is an unobserved cell, in which case the test passes.

Global frontier point is the center of an unobserved global occupancy grid map cell adjacent to a unoccupied global map cell. A valid global frontier point must pass the stabbing query test against all submaps. Global frontier is the set of all valid global frontier points at a given time. The red points in the third picture in Figure 2 are an example of a global frontier.

Perimeter of the global or local frontier is the number of frontier points in the respective set.

It may be noted that the frontier points could alternatively have been defined as centers of unoccupied cells adjacent to unobserved cells. We have chosen to define frontier points as centers of unobserved cells as above in order to simplify the stabbing query test.

IV-B The frontier detection algorithm

There are two kinds of events which occur during SLAM execution which are of interest for frontier detection: a submap update event, where a scan is inserted into the active submaps; and a pose graph optimization event which also occurs periodically, but less often.

Algorithm 1 describes handling of submap update events, while Algorithm 3 describes handling of pose graph optimization events.

IV-B1 Handling of submap updates

For each laser scan processed in SLAM, both submaps in the active pair are updated, making submap updates the most frequently occurring type of event. Because a submap update can only affect the frontier in the area covered by the active submaps, the frontier detection algorithm can be constrained to this area in order to maximize efficiency.

The first step in handling a submap update of an active submap is performing occupancy grid classification and dense local frontier detection on the new version of the submap occupancy grid (Algorithm 1, algorithms 1–1). In other words, on the local submap level, we have opted to perform a naive edge detection approach instead of e.g. one of the more elaborate approaches described in section II.

The reason for using a naive approach for local frontier detection is twofold. First, because the submaps are bounded in size (controlled by the fixed parameter $n_{scans}$), and the number of active submaps is constant (2 active submaps per active robot in a multi-robot use case), the time complexity of local frontier detection is not affected by the size of the global map or the size of the dataset. Second, probability thresholding, classification (Algorithm 1) and edge detection (algorithms 1–1) can be vectorized, allowing for high performance on modern CPUs. Our implementation relies on Eigen [20], where we use Eigen’s matrix block algebra and Hadamard products to vectorize thresholding, classification and Boolean logic for edge detection.

Computing and storing the set of local frontier points for an active submap produces a set of candidates for the global frontier. Every local frontier point is transformed into the global map coordinate system $g$ according to the current global pose of the corresponding submap (Algorithm 2, Algorithm 2) and the stabbing query test is performed against the intersecting submaps (Algorithm 2, Algorithm 2). In case the test passes against all submaps, the transformed frontier point is indeed a global frontier point and is added to the global frontier set (Algorithm 2, Algorithm 2). If the test fails, the submap against which the test failed is recorded as a hint (Algorithm 2, Algorithm 2) so that future re-testing (in Algorithm 3) can be performed faster.

In the next step of handling submap updates, we exploit several properties of graph SLAM based on occupancy grid submaps:

The occupancy grids of finished submaps are immutable. Therefore, it is not necessary to re-detect local frontiers for already finished submaps. 2. 2.

The algorithm for handling submap updates may assume that no graph optimization has occurred since the last submap update event, so all existing global frontiers of finished submaps are valid (except for the situation described below). 3. 3.

The cell occupancy probabilities of active submaps have decreasing entropy, which means that only previously unobserved cells can become observed, and not vice-versa. In other words, updates to active submaps can invalidate (cover up) the previously valid global frontiers of intersecting submaps.

In Algorithm 1, algorithms 1–1, it is tested if the new versions of active submaps cover up existing valid global frontiers of the intersecting finished submaps. This is done by performing stabbing query tests of the global frontier points against the active submaps. If the stabbing query test fails, the newly covered up global frontier points are removed from the set of global frontier points (Algorithm 1), thus preserving the invariant that all global frontier points at a time are valid. The global frontiers of finished submaps whose global frontier points got removed are also marked as updated (Algorithm 1) for incremental publishing of frontier updates (Algorithm 1).

Bounding boxes of finished submaps are stored inside a spatial index tree data structure which enables fast queries of submaps which intersect with a given bounding box (used for looking up finished submaps intersecting with an active submap in Algorithm 1, Algorithm 1). When a submap is marked as finished, its bounding box is inserted into the tree structure (algorithms 1–1). Our implementation uses the Boost implementation of R-trees [21] for storing global axis-aligned bounding boxes of finished submaps.

It can also be noted that not all submap update events have to be handled in order to guarantee a correct result – any non-final submap update can be skipped. A robotic exploration system which does not require real-time frontier updates after every scan, but rather occasionally, can invoke the algorithm for handling submap updates only when a submap is finished. This can significantly reduce the computational effort of keeping the frontier up to date, up to a factor of $n_{scans}$.

IV-B2 Handling of graph optimization events

When graph SLAM performs optimization, a new solution is produced for poses of all graph nodes. For frontier detection, this means that the submap poses have changed. This invalidates the global bounding boxes of submaps and the entire global frontier, all of which has to be recomputed in Algorithm 3. However, advantage is taken of the fact that the local frontiers have already been computed for all submaps, so all that needs to be done is to re-transform the local frontier points to the global coordinate system $g$ and re-test them.

The global bounding boxes of submaps are recomputed in algorithms 3–3, while the rest of Algorithm 3 recomputes the global frontier similarly to Algorithm 1 using Algorithm 2: each local frontier point is re-transformed according to the corresponding new global submap pose and re-tested. If the stabbing query test is passed, the frontier point is inserted into the new global frontier set.

During re-testing of a frontier point against all submaps in the set of intersecting submaps, it is advisable to first try testing against the submap stored in the failing submap hint, if it exists (comment in Algorithm 2, Algorithm 2). This speeds up rejection of points which fail the test by first testing against the same submap which caused the local frontier point to fail the test earlier.

Recomputing the entire global frontier in Algorithm 3 is not a computationally lightweight operation. However, it is more efficient than a non-submap aware approach that would require iterating through the reassembled global map with time complexity proportional to the area of the global map. Recomputing the global frontier in Algorithm 3 has the time complexity proportional to the perimeter of local frontiers.

To summarize, Algorithm 1 assumed that all global frontier points before a submap update event were valid and up-to-date. Graph optimization violates this assumption by displacing the submaps according to submap poses of the new pose graph solution. Algorithm 3 restores this invariant by efficiently recomputing the global frontier.

V Algorithm analysis

V-A Soundness and completeness

Let us first note that the process of merging submaps into a global map according to a pose graph solution cannot result in an unobserved global map cell if any of the corresponding submap cells are observed. In other words, if a global map cell is unobserved, the corresponding cells in all submaps are also unobserved. Also, let $LF$ and $GF$ denote the sets of local frontier points of all submaps (transformed to $g$, but untested) and all valid global frontier points, respectively.

For discussing completeness, we will consider a valid global frontier point in $GF$, i.e. the center of an unobserved global map cell adjacent to a unoccupied global map cell. Due to the map merging process, all submap cells corresponding to the global frontier cell have to be unobserved as well. Next, the adjacent unoccupied global map cell is also unoccupied in at least one submap, whose merging caused the unoccupied global map cell to be marked as such. In that submap, the unobserved cell next to the unoccupied cell is a local frontier and is thus in $LF$. Therefore, each valid global frontier point corresponds to a local frontier point in at least one submap (i.e. $GF\subseteq LF$), and an algorithm which computes $GF$ by taking $LF$ as input is thus complete, providing it does not incorrectly discard any valid global frontier points in the process.

The rule for discarding local frontier points when they fail the stabbing query test can only result in observed cells being correctly discarded from the global frontier, and therefore the property of completeness is preserved. Also, the naive edge detection algorithm used for computing the local frontiers is trivially valid.

For soundness, we will consider a global frontier point returned by the proposed algorithm, and suppose that it is not valid. This could be either because the returned global frontier point is an observed global map cell, or because the returned global frontier point is not adjacent to a unoccupied global map cell. The first case is not possible because the frontier point would have failed the stabbing query test against the submap that contains an observed cell which resulted in marking the corresponding global map cell as observed.

The second case is possible but unlikely – for example, if there were a plurality of submaps with corresponding adjacent cells being occupied rather than unoccupied, so the merging process results in adjacent global map cells being marked as occupied instead of unoccupied, and the global frontier point is adjacent only to occupied global map cells. This is unlikely because the chance of the unobserved cell not getting covered up by observed cells (which in turn will make it fail the stabbing query test) is negligible. To exclude this case and to make the algorithm completely sound, the stabbing query test could be modified to also look at the adjacent cells in submaps as well, instead of just checking single cells. This would not increase the theoretical time complexity, but would make the implementation unnecessarily more elaborate and slow.

V-B Computational complexity

V-B1 Handling of submap updates

In Algorithm 1, Algorithm 1, the complexity of querying the R-tree for submaps intersecting with the updated submap $si$ is $O(\log|S|+|S_{\cap si}|)$, where $S$ is the set of all submaps, and $S_{\cap si}$ is the set of submaps intersecting with the submap $si$. This also includes the complexity of inserting a finished submap bounding box into the R-tree.

In algorithms 1–1, occupancy grid classification and naive local frontier detection run in $O(A(si))$, where $A(si)$ is the area of submap $si$, i.e. the number of cells in the submap.

The number of local frontier points in $si$, i.e. the perimeter of its local frontier $LF_{si}$ will be denoted as $P(LF_{si})$, while the global frontier of the submap $si$ and its perimeter will be denoted as $GF_{si}$ and $P(GF_{si})$, respectively.

For each detected local frontier point in the updated submap, the complexity of performing the stabbing query test against the intersecting submaps (Algorithm 2, Algorithm 2) is $O(|S_{\cap si}|)$, yielding the total complexity of this step $O(P(LF_{si})\cdot|S_{\cap si}|)$.

Invalidating global frontiers of the intersecting submaps (algorithms 1–1) runs linearly with respect to their perimeter, yielding complexity $O(P(\bigcup_{sj\in S_{\cap si}}GF_{sj}))$.

The total time complexity of handling an update of submap $si$ thus equals (simplified assuming $P(LF_{si})\geq 1$):

[TABLE]

V-B2 Handling of pose graph optimization events

The first step in Algorithm 3 is to rebuilt the R-tree (algorithms 3–3), which runs in $O(|S|\log|S|)$. This complexity also includes looking up the intersecting submaps for each submap (Algorithm 3).

Next, every local frontier point is transformed to the global coordinate system $g$ and the stabbing query test is performed against the intersecting submaps (Algorithm 2, algorithms 2–2). A pessimistic bound would entail testing every local frontier point against all other submaps, i.e. a time complexity of $O(|S|\cdot P(LF))$. The pessimism of this bound can be reduced by assuming that the points which fail the stabbing query test will do so on the first test, performed against the submap stored in the failing submap hint. If the perimeter of points which fail the test, equal to $P(LF)-P(GF)$, is denoted as $P(FF)$, this assumption yields the time complexity of $O(|S|\cdot P(GF)+P(FF))$.

The total time complexity of handling a pose graph optimization event thus depends on the number of submaps and the local and global frontier perimeters:

[TABLE]

We have managed to avoid having the two-dimensional map area in the time complexity of the global operation of handling pose graph optimization by taking advantage of submaps and their immutability.

VI Experimental results

Our implementation of the proposed frontier detection algorithm (available on Github111https://github.com/larics/cartographer_frontier_detection) has been developed as an extension of Cartographer. We have chosen to focus our experimental validation on running offline SLAM processing with frontier detection on a single-robot, but otherwise quite demanding publicly available dataset: the introductory example for Cartographer, the Google Deutsches Museum bag [1], captured with a human-carried sensor backpack.

We have also performed an evaluation with the Freiburg FR-079 dataset222ROS bag and SLAM parameters are available in the Github repository in order to roughly compare with [9], who tested most of the algorithms described in section II in a simulated FR-079 environment.

Demo videos of frontier detection for both evaluated datasets are available333https://goo.gl/62zEUy, along with an additional demo of a multi-robot scenario where the dataset was recorded with two teleoperated Pioneer mobile robots driven side-by-side.444Dataset and SLAM parameters also available in the Github repository

We have opted to use the Google Cartographer offline node, which processes a ROS bag dataset as fast as the CPU can handle (around 4-5x realtime on Deutsches Museum), enabling us to demonstrate frontier update frequencies far greater than the scan frequency of a single robot’s LIDAR.

In order to minimize the impact on SLAM performance, the frontier detection algorithm may execute asynchronously, in a separate thread. The frontier detection algorithm tries to process all submap updates, while it can adaptively skip non-final submap updates in case the processing speed falls behind SLAM. In the asynchronous case on Deutsches Museum, the wall clock frequency of incremental frontier updates has been measured as a benchmark of frontier detection performance.

Running frontier detection synchronously (in the main SLAM thread) makes it easier to measure the exact time spent performing frontier detection, which can be measured as the difference of wall clock durations of processing a dataset with synchronous frontier detection and with no frontier detection at all.

The results are given in Table I. We believe that the Deutsches Museum result is an achievement with respect to the state of the art. To compare, the best performing-algorithm in [9], EWFD, achieved 290 ms and 190 ms per frontier update iteration in two simulated FR-097 environment trajectories, with significantly lower numbers of processed scans (252 scans for the 290 ms Freiburg 1). We have achieved sub-millisecond (0.92 ms) wall-time per frontier update, while handling a fairly larger number of scans (4198) and 25 pose graph optimizations on average. We do have to point out that our experiments have been performed on newer hardware, and that we have not performed frontier grouping, but rather return the detected frontier as an unstructured set of points.

VI-A Additional notes on implementation of the frontier detection algorithm

An optimization that we have implemented is performing additional stabbing query tests against a small number of temporally close submaps in the local frontier edge detection condition in Algorithm 1, Algorithm 1 (for example, against 4 previous submaps). This has the result of permanently “baking in” a negative test result against these submaps, since these points are permanently erased from the local frontiers.

A benefit of discarding points early from local frontiers is speeding up recomputation of the global frontier in Algorithm 3, because there will be fewer candidates in the local frontier which have to be re-transformed and re-tested. Also, it is expected that the localization drift between sequential submaps is small, and that pose graph optimization will not produce a significant relative displacement between sequential submaps. Using the unoptimized poses may actually be preferential for processing sequential submaps, because it could prevent detecting a false frontier resulting from slight misalignment of sequential submaps introduced by pose graph optimization when closing loops.

We have employed a few cosmetic improvements which result in (subjectively) aesthetically better frontiers. The first is a change to (1) where cells with a very uncertain “unoccupied” probability (i.e. $\mathbf{S}^{si}_{k,l}\in[0.5-\varepsilon,0.5]$, where $\varepsilon\colonequals 0.04$ is an arbitrarily chosen small value) are treated as “unobserved” cells. This prevents detecting a false frontier around single false long-distance laser readings, which cause insertion of a false ray of “unoccupied” cells into the submap. This will also have the beneficial effect of driving the robot exploration system to get a “better look” at areas with uncertain unoccupied cells, since they are considered unexplored.

The second change we have made is simple smoothing of local frontiers by adjusting the definition of a local frontier to be the center of an unobserved cell with $\geq$ 2 unoccupied adjacent cells and $\geq$ 2 unobserved adjacent cells, where adjacent cells are cells in the Moore 8-neighbourhood. With no other changes to the rest of the algorithm, these changes result in a smoother global frontier, since the smoothed local frontier points are candidates for the global frontier.

VII Conclusion and future work

We have described, implemented and tested an efficient frontier detection algorithm that is specialized for 2D multi-robot graph SLAM based on occupancy grid submaps. Our algorithm is efficiently constrained to the area of active submaps, yet robust to loop closure by efficiently recomputing the global frontier after pose graph optimization is performed without the time complexity being a function of map area.

Future work: Using the proposed frontier detection algorithm in an actual closed exploration-system loop (Figure 1) is a work in progress. Also, some cited state of the art (e.g. [10]) performs grouping of continuous frontier points into segments, which is useful for selecting navigation objectives of exploration tasks. Another kind of post-processing of the detected frontier which might be of interest is reachability analysis, ie. detecting frontier points such as points behind glass, closed doors or behind walls (e.g. from false laser readings). Nonetheless, a performant algorithm for frontier detection is the basis for any such further improvements.