IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO.

4, OCTOBER 2019 3569

Efficient Dense Frontier Detection for 2-D Graph

SLAM Based on Occupancy Grid Submaps
Juraj Oršulić , Damjan Miklić , and Zdenko Kovačić

Abstract—In autonomous robot exploration, the frontier is the which the robot may approach and thereby increase the knowl-
border in the world map dividing the explored and unexplored edge about the structure of the environment. With the informa-
space. The frontier plays an important role when deciding where tion about the exploration frontier available, mission planning
in the environment the robots should go explore next. We consider a
modular control system pipeline for autonomous exploration where can be described in its simplest version as (boldly) go where no
a 2-D graph simultaneous localization and mapping (SLAM) algo- one has gone before.
rithm based on occupancy grid submaps performs map building Many components of the autonomous exploration problem are
and localization, and frontier detection is one of the key system complex enough to be associated with their own field of robotics
components. We provide an overview of the state of the art in research, resulting in sophisticated methods and software mod-
frontier detection and the relevant SLAM concepts and propose
a fast specialized frontier detection method which is efficiently ules being available for solving them. Namely, building the
constrained to active submaps, yet robust to graph SLAM loop model of the environment (a map) and self-localization therein
closures. may be performed by a module implementing a simultaneous
Index Terms—Visual-based navigation, computer vision for
localization and mapping (SLAM) algorithm. For this reason, we
other robotic applications, path planning for multiple mobile robots consider a control system implementing a modular exploration
or agents. pipeline as depicted in Fig. 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
I. INTRODUCTION map, the localization pose and the detected frontier are used
FULLY autonomous mobile robot, able to explore, nav- by the later stages of the pipeline (exploration task generation
A igate and perform actions in an unknown environment
is one of the ultimate objectives of today’s mobile robotics
and scheduling, path planning and following) to guide the robot
according to an exploration strategy.
research. To this end, we consider a single autonomous robot The scope and the main contribution of this letter is the frontier
or an autonomous team of robots tasked with exploring the detection part of the exploration pipeline–a new, efficient frontier
unknown environment. The autonomous exploration problem detection approach, specialized for use with a 2D graph SLAM
comprises collecting the data sensed from the environment, algorithm based on occupancy grid submaps [1]. The proposed
using the collected data to build a structured model of the en- approach is robust to loop closures and exploits the submap
vironment, self-localizing in the environment model, high-level structure of the SLAM algorithm in order to quickly perform
planning and scheduling of robot tasks (mission generation and frontier updates. By providing high frequency incremental fron-
assignment), and path planning and following. All of these need tier updates which enable more responsive planning of explo-
to be performed in real time. ration objectives, a real-time use case is facilitated on large and
In a common approach known as frontier exploration, the complex maps, e.g. the Deutsches Museum dataset [1]. All the
robot maintains information about the border which divides the while the proposed frontier detection algorithm delivers a result
explored and unexplored space in the environment–the frontier. at least as good as a naive frontier edge-detection algorithm, i.e.
Elements of the frontier represent places in the environment 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.
Manuscript received February 24, 2019; accepted June 24, 2019. Date of
publication July 12, 2019; date of current version July 24, 2019. This letter
was recommended for publication by Associate Editor T. Peynot and Editor
E. Marchand upon evaluation of the reviewers’ comments. (Corresponding
II. RELATED WORK
author: Juraj Oršulić.)
A. Frontier Exploration as a Prevalent Exploration Method
J. Oršulić and Z. Kovačić are with the Laboratory for Robotics and In-
telligent Control Systems, Faculty of Electrical Engineering and Computing, Frontier exploration in the context of autonomous robotics
University of Zagreb, 10000 Zagreb, Croatia (e-mail: [email protected];
[email protected]). was first introduced by Yamauchi in 1997 [2], paving the way for
D. Miklić is with the RoMb Technologies d.o.o., 10000 Zagreb, Croatia many others ([3]–[5]). Commonly, elements of the detected fron-
(e-mail: [email protected]). tier are used as navigation goals during planning of exploration
This letter has supplementary downloadable material available at http://
ieeexplore.ieee.org, provided by the authors. tasks. Building on this, there are more complex exploration
Digital Object Identifier 10.1109/LRA.2019.2928203 strategies which attempt to coordinate entire robot teams ([6],

2377-3766 © 2019 IEEE. Personal use is permitted, but republication/redistribution requires IEEE permission.
See http://www.ieee.org/publications_standards/publications/rights/index.html for more information.
3570 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

Fig. 1. Block diagram providing a high-level overview of the considered control system for performing autonomous exploration. The robot may be equipped with
a laser rangefinder, an inertial measurement unit, and wheel encoders for odometry. A Simultaneous Localization and Mapping module uses the sensed environment
data to build an environment map and to estimate the pose therein, while a frontier detection module keeps track of the exploration frontier. An exploration task
generation and scheduling module assigns exploration tasks to be performed according to an exploration strategy and forwards it to a path planning and following
module, which uses the map and localization from SLAM to steer the robot towards the goal defined by the assigned task. Notably, the frontier detector is tightly
coupled with the SLAM module, enabling efficient implementation of frontier detection.

[7]), or use frontiers as sinks in a potential field ([8]). Frontier the WFD detector, yielding the incremental WFD (WFD-INC)
detection is therefore a key elementary operation in frontier algorithm, which requires non-trivial auxiliary data structures
exploration, and it is important that it be performed as quickly for frontier point maintenance. Our proposed algorithm has a
as possible so that exploration can be more efficient [9]. similar concept of active submaps.
2) Impact of Loop Closure in SLAM on Frontier Detection:
Loop closure is an event when the SLAM algorithm recognizes
B. State of the Frontier Detection Art that the robot has revisited the same place, and then makes
A naive algorithm for frontier detection is to perform edge a correction using this information which reduces the error
detection on the complete global map after each map update. caused by drift in localization along the whole loop. In order
However, this approach is not feasible for larger maps and real- to return a correct result, frontier detection has to be able to
time robot operation with such maps, as it presents a significant efficiently cope with the map changes induced by loop closure
computational burden. corrections. These map changes are not confined to the active
1) Keidar and Kaminka’s Seminal Work on Efficient Frontier area–performing loop closure results in widespread changes all
Detection: Keidar and Kaminka [10] proposed in 2014 sev- over the map. While an efficient frontier detection algorithm
eral approaches which attempt to perform frontier detection should avoid reassembling and iterating throughout the entire
in an efficient manner. The first, Wavefront Frontier Detec- global map in every iteration, constraining the algorithm to only
tor (WFD), consists of running two consecutive breadth-first the active area makes it difficult to be robust to loop closures.
searches (BFS). The first BFS starts at the robot position and WFD-INC addresses loop closure events by evicting the detected
continues throughout the unoccupied space, until eventually frontier and performing frontier detection from scratch using the
a frontier point is found which belongs to a component of new loop-corrected map.
connected frontier points. From there, the rest of the connected To efficiently address loop closures, the frontier detection
component is found by a second BFS along the connected algorithm needs to get intimate to a certain degree with
frontier points. While WFD avoids searching the unobserved the implementation of the SLAM algorithm. For example,
space, it still searches all observed space in each iteration, which Keidar and Kaminka additionally proposed an implementation
may degenerate into a full map search as exploration progresses. of WFD-INC for GMapping (a particle filter-based SLAM [11])
The second approach to frontier detection proposed by Keidar called incremental parallel WFD (WFD-IP). WFD-IP performs
and Kaminka, the Fast Frontier Detector (FFD), does not use in parallel separate WFD-INC frontier detection for each par-
the map built by SLAM, but rather constructs the contour of ticle (each particle having its own map). Like WFD-INC, our
each laser scan using Bresenham’s line algorithm, and uses proposed method uses the internals of the SLAM algorithm in
the constructed contour to detect the frontier and store it in order to perform frontier detection faster while also being robust
a specialized data structure. Quin and Alempijević [9] note to loop closure.
that FFD has to be executed after each scan, which results in Quin and Alempijević [9] introduce two frontier detection
many wasteful calculations if frontier updates are required only methods: naive active area (NaiveAA), which is the naive ap-
occasionally, and that Bresenham’s line algorithm can cut across proach confined to the active area, and a version of WFD called
unobserved space and miss some frontier cells. Our proposed Expanding WFD (EWFD) which steers the WFD breadth-first
approach does not require execution after each processed scan, search into newly discovered unoccupied areas. EWFD assumes
supporting a use case where frontier updates are required only that the entropy for each cell can only decrease over time. This
occasionally. is not true in the general case for the complete global map
FFD is also notable for introducing the concept of active when considering effects of loop closure–observed areas can get
area–a bounding box positioned in the map around the robot moved around in the global map during loop closure and leave
position, circumscribing the last scan the map was updated unexplored space in their wake. However, the entropy decrease
with. The frontier update step is sped up by restricting it to assumption is almost surely true for single submaps in submap-
the active area. Keidar and Kaminka also applied this concept to based SLAM, and we exploit this fact in our proposed approach.
ORŠULIĆ et al.: EFFICIENT DENSE FRONTIER DETECTION FOR 2-D GRAPH SLAM BASED ON OCCUPANCY GRID SUBMAPS 3571

3) Other Approaches: Senarathne and Wang [12] use an The occupancy probabilities of grid cells are initially unob-
oriented bounding-box based inexact approach. served i.e. unknown (exactly 0.5). When a predetermined fixed
Umari [13] uses rapidly-exploring random trees (RRT) to number of scans nscans is inserted into a submap, it is marked
perform sparse frontier detection by building a tree inside the un- as finished, and a new submap is created to take its place in
occupied space in the map built by SLAM. When the algorithm the active submap pair. Importantly, once a submap is finished,
crosses the frontier while trying to expand the random tree, a sin- its occupancy grid is immutable from that point onward. Cell
gle frontier point is detected. However, using the implementation occupancy probabilities are clamped to the interval [0.1, 0.9]
of the algorithm provided in [13] does require reassembling the and are stored linearly mapped onto the space of unsigned
global map in each iteration. Also, this algorithm is not robust 16-bit integers. Scan insertion into submaps is performed as
to loop closure, since the built RRT tree does not follow the Bayesian updates of the cell occupancy probabilities (see (3)
results of pose graph optimization. In our experiments, we have in [1]). The cells corresponding to laser hit points are updated
observed various problems with performance of RRT frontier with “occupied” observations, while the intermediate points
detection in narrow corridors and with large maps, prompting (obtained by casting rays from the laser rangefinder origin to
us to devise the method for dense frontier detection proposed hit points) are updated with “unoccupied” observations.
herein. 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
III. PREREQUISITES
they are observed.
A. Simultaneous Localization and Mapping When Cartographer detects loop closures, pose constraints
The term SLAM was coined by Leonard and Durrant-Whyte between the corresponding trajectory nodes and submaps are
in 1991 [14]. As shown in the block diagram of the exploration added as edges into the pose graph. Optimization is periodically
pipeline in Fig. 1, a SLAM algorithm uses sensor data to build a invoked in order to find a new solution–a set of global submap
map and perform localization, which is further used in frontier and trajectory node poses. As discussed, for frontier detection,
detection, exploration task planning and execution. There is a this implies that when pose graph optimization is performed, the
wealthy trove of SLAM methods developed to this day, which submaps can and do get displaced and rotated (i.e. undergo rigid
can be roughly grouped into methods based on filtering and transformations), although their occupancy grids are immutable
methods based on graph optimization. We will focus on graph after they are marked as finished. Our proposed frontier detection
SLAM, which represents poses and detected features as nodes approach takes advantage of these properties of Cartographer.
in a graph, while the correspondences which impose constraints
on the poses of the respective nodes are represented as graph IV. FRONTIER DETECTION
edges. Various optimization methods may be used to minimize
the residual error of all constraints, e.g. the Ceres solver [15]. A. Definitions
Submaps are small local maps which are merged into a global Rigid transformation Tba ∈ SE(3) is the pose of the coordi-
map. One of earlier approaches to SLAM using submaps is [16], nate system b relative to the coordinate system a. Transforming
with further examples being [17] and [18]. a point with coordinates expressed in b, pb ∈ R3 , into the
corresponding point pa in a is denoted as pa = Tba pb .
B. Cartographer The global map coordinate system is denoted with g. The
solution of pose graph optimization are poses of graph nodes
The proposed frontier detection method was designed for use expressed with respect to g.
with Cartographer, an open-source multi-robot multi-trajectory Submap is an occupancy grid with a local submap coor-
2D and 3D graph SLAM based on occupancy grid submaps, dinate system, in which the submap occupancy grid cells are
developed by Google (Hess, Kohler, Rapp in 2016 [1]). Car- indexed with 2-integer tuples: Ssi k,l is the occupancy probability
tographer’s approach of optimizing the poses of all scans and value of the cell (k, l) in the submap si. A submap is finished af-
submaps follows Sparse Pose Adjustment [19] and uses the ter being updated with nscans consecutive laser scans. The set of
Ceres solver [15] for optimizing the pose graph using the active submaps contains the submaps which are not yet finished.
Levenberg–Marquardt algorithm (LMA). Global submap pose Tsi g is the global pose in g of the origin
Submaps in Cartographer are spatially and temporally com- of the local coordinate system of a submap si. Global submap
pact occupancy grid maps, typically of resolution 0.05 m, made poses are part of the optimized pose graph solution.
from a short, continuous series of rangefinder sensor measure- Occupancy classification–the cell occupancy probability
ments (laser scans) taken during traversal of a short section of values are not used directly in frontier detection. The probability
the robot trajectory. It is desired that the size of the submaps be values are first classified according to the following thresholding
small enough such that the localization drift is not perceptible rule:
within a single submap.
Cartographer maintains a pair of active submaps that the laser ⎧
⎪
⎨unoccupied p < 0.5
scans are inserted into, according to a local pose obtained by
performing scan matching against the older (more complete) class(p = Ssi
k,l ) := occupied p > 0.5 (1)
⎪
⎩
submap from the active submap pair. unobserved p = 0.5
3572 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

Fig. 2. Depiction of merging two adjacent submaps and their local frontiers into the global composite map and the global frontier. Colour legend: unobserved
grid cells are grey, unoccupied cells are white, occupied cells are black, while frontier points are red. Each submap has a local frontier which is based solely on
the occupancy grid of that submap. The points in the local frontier set are candidates for becoming part of the global frontier. The submaps are then transformed
into the global map coordinate system according to the current optimized solution of graph SLAM. Any local frontier point which after transforming ends up in
an observed area of any other submap (i.e. fails the stabbing query test against that submap) is discarded from the global frontier set. An extended visualization of
this figure is available in the video playlist linked in Section VI.

Observed cells are occupancy grid cells that are not

unobserved.
Local frontier point is the center of an unobserved occu-
pancy 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 Fig. 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 pg , 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 (Tsi −1 g
g ) p .
Further, performing a stabbing query test means checking if
the corresponding cell Ssik,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 Fig. 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.

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 periodi- updated, making submap updates the most frequently occurring
cally, but less often. type of event. Because a submap update can only affect the
Algorithm 1 describes handling of submap update events, frontier in the area covered by the active submaps, the frontier
while Algorithm 3 describes handling of pose graph optimiza- detection algorithm can be constrained to this area in order to
tion events. maximize efficiency.
1) Handling of Submap Updates: For each laser scan The first step in handling a submap update of an active submap
processed in SLAM, both submaps in the active pair are is performing occupancy grid classification and dense local
ORŠULIĆ et al.: EFFICIENT DENSE FRONTIER DETECTION FOR 2-D GRAPH SLAM BASED ON OCCUPANCY GRID SUBMAPS 3573

frontier detection on the new version of the submap occupancy

grid (Algorithm 1, lines 6–10). 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 nscans ), and the number (cover up) the previously valid global frontiers of inter-
of active submaps is constant (2 active submaps per active robot secting submaps.
in a multi-robot use case), the time complexity of local frontier In Algorithm 1, lines 11–13, it is tested if the new versions
detection is not affected by the size of the global map or the size of active submaps cover up existing valid global frontiers of
of the dataset. Second, probability thresholding, classification the intersecting finished submaps. This is done by performing
(line 6) and edge detection (lines 7–9) can be vectorized, allow- stabbing query tests of the global frontier points against the
ing for high performance on modern CPUs. Our implementation active submaps. If the stabbing query test fails, the newly covered
relies on Eigen [20], where we use Eigen’s matrix block algebra up global frontier points are removed from the set of global
and Hadamard products to vectorize thresholding, classification frontier points (line 14), thus preserving the invariant that all
and Boolean logic for edge detection. global frontier points at a time are valid. The global frontiers of
Computing and storing the set of local frontier points for finished submaps whose global frontier points got removed are
an active submap produces a set of candidates for the global also marked as updated (line 15) for incremental publishing of
frontier. Every local frontier point is transformed into the global frontier updates (line 20).
map coordinate system g according to the current global pose of Bounding boxes of finished submaps are stored inside a spatial
the corresponding submap (Algorithm 2, line 1) and the stabbing index tree data structure which enables fast queries of submaps
query test is performed against the intersecting submaps (Algo- which intersect with a given bounding box (used for looking
rithm 2, line 3). In case the test passes against all submaps, the up finished submaps intersecting with an active submap in
transformed frontier point is indeed a global frontier point and is Algorithm 1, line 5). When a submap is marked as finished,
added to the global frontier set (Algorithm 2, line 4). If the test its bounding box is inserted into the tree structure (lines 16–18).
fails, the submap against which the test failed is recorded as a hint Our implementation uses the Boost implementation of R-trees
(Algorithm 2, line 5) so that future re-testing (in Algorithm 3) [21] for storing global axis-aligned bounding boxes of finished
can be performed faster. submaps.
In the next step of handling submap updates, we exploit It can also be noted that not all submap update events have to
several properties of graph SLAM based on occupancy grid be handled in order to guarantee a correct result–any non-final
submaps: submap update can be skipped. A robotic exploration system
1) The occupancy grids of finished submaps are immutable. which does not require real-time frontier updates after every
Therefore, it is not necessary to re-detect local frontiers scan, but rather occasionally, can invoke the algorithm for han-
for already finished submaps. dling submap updates only when a submap is finished. This
2) The algorithm for handling submap updates may assume can significantly reduce the computational effort of keeping the
that no graph optimization has occurred since the last frontier up to date, up to a factor of nscans .
submap update event, so all existing global frontiers of 2) Handling of Graph Optimization Events: When graph
finished submaps are valid (except for the situation de- SLAM performs optimization, a new solution is produced for
scribed below). poses of all graph nodes. For frontier detection, this means that
3) The cell occupancy probabilities of active submaps have the submap poses have changed. This invalidates the global
decreasing entropy, which means that only previously un- bounding boxes of submaps and the entire global frontier, all of
observed cells can become observed, and not vice-versa. which has to be recomputed in Algorithm 3. However, advantage
In other words, updates to active submaps can invalidate is taken of the fact that the local frontiers have already been
3574 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

computed for all submaps, so all that needs to be done is to property of completeness is preserved. Also, the naive edge
re-transform the local frontier points to the global coordinate detection algorithm used for computing the local frontiers is
system g and re-test them. trivially valid.
The global bounding boxes of submaps are recomputed in For soundness, we will consider a global frontier point re-
lines 1–3, while the rest of Algorithm 3 recomputes the global turned by the proposed algorithm, and suppose that it is not
frontier similarly to Algorithm 1 using Algorithm 2: each local valid. This could be either because the returned global frontier
frontier point is re-transformed according to the corresponding point is an observed global map cell, or because the returned
new global submap pose and re-tested. If the stabbing query global frontier point is not adjacent to a unoccupied global map
test is passed, the frontier point is inserted into the new global cell. The first case is not possible because the frontier point
frontier set. would have failed the stabbing query test against the submap
During re-testing of a frontier point against all submaps in that contains an observed cell which resulted in marking the
the set of intersecting submaps, it is advisable to first try testing corresponding global map cell as observed.
against the submap stored in the failing submap hint, if it exists The second case is possible but unlikely–for example, if there
(comment in Algorithm 2, line 2). This speeds up rejection of were a plurality of submaps with corresponding adjacent cells
points which fail the test by first testing against the same submap being occupied rather than unoccupied, so the merging process
which caused the local frontier point to fail the test earlier. results in adjacent global map cells being marked as occupied
Recomputing the entire global frontier in Algorithm 3 is not instead of unoccupied, and the global frontier point is adjacent
a computationally lightweight operation. However, it is more only to occupied global map cells. This is unlikely because
efficient than a non-submap aware approach that would require the chance of the unobserved cell not getting covered up by
iterating through the reassembled global map with time com- observed cells (which in turn will make it fail the stabbing
plexity proportional to the area of the global map. Recomputing query test) is negligible. To exclude this case and to make the
the global frontier in Algorithm 3 has the time complexity algorithm completely sound, the stabbing query test could be
proportional to the perimeter of local frontiers. modified to also look at the adjacent cells in submaps as well,
To summarize, Algorithm 1 assumed that all global frontier instead of just checking single cells. This would not increase the
points before a submap update event were valid and up-to-date. theoretical time complexity, but would make the implementation
Graph optimization violates this assumption by displacing the unnecessarily more elaborate and slow.
submaps according to submap poses of the new pose graph
solution. Algorithm 3 restores this invariant by efficiently re-
B. Computational Complexity
computing the global frontier.
1) Handling of Submap Updates: In Algorithm 1, line 5, the
V. ALGORITHM ANALYSIS complexity of querying the R-tree for submaps intersecting with
the updated submap si is O(log |S| + |S∩si |), where S is the set
A. Soundness and Completeness of all submaps, and S∩si is the set of submaps intersecting with
Let us first note that the process of merging submaps into the submap si. This also includes the complexity of inserting a
a global map according to a pose graph solution cannot result finished submap bounding box into the R-tree.
in an unobserved global map cell if any of the corresponding In lines 6–10, occupancy grid classification and naive local
submap cells are observed. In other words, if a global map cell frontier detection run in O(A(si)), where A(si) is the area of
is unobserved, the corresponding cells in all submaps are also submap si, i.e. the number of cells in the submap.
unobserved. Also, let LF and GF denote the sets of local frontier The number of local frontier points in si, i.e. the perimeter
points of all submaps (transformed to g, but untested) and all of its local frontier LFsi will be denoted as P (LFsi ), while the
valid global frontier points, respectively. global frontier of the submap si and its perimeter will be denoted
For discussing completeness, we will consider a valid global as GFsi and P (GFsi ), respectively.
frontier point in GF , i.e. the center of an unobserved global For each detected local frontier point in the updated submap,
map cell adjacent to a unoccupied global map cell. Due to the complexity of performing the stabbing query test against the
the map merging process, all submap cells corresponding to intersecting submaps (Algorithm 2, line 3) is O(|S∩si |), yielding
the global frontier cell have to be unobserved as well. Next, the the total complexity of this step O(P (LFsi ) · |S∩si |).
adjacent unoccupied global map cell is also unoccupied in at Invalidating global frontiers of the intersecting submaps lines
least one submap, whose merging caused the unoccupied global (11–15) runs linearly
 with respect to their perimeter, yielding
map cell to be marked as such. In that submap, the unobserved complexity O(P ( sj∈S∩si GFsj )).
cell next to the unoccupied cell is a local frontier and is thus in The total time complexity of handling an update of submap
LF . Therefore, each valid global frontier point corresponds to si thus equals (simplified assuming P (LFsi ) ≥ 1):
a local frontier point in at least one submap (i.e. GF ⊆ LF ), ⎛ ⎛ ⎞⎞
and an algorithm which computes GF by taking LF as input
is thus complete, providing it does not incorrectly discard any O ⎝log |S| + A(si) + P (LFsi ) · |S∩si | + P ⎝ GFsj ⎠⎠
valid global frontier points in the process. sj∈S∩si
The rule for discarding local frontier points when they fail (2)
the stabbing query test can only result in observed cells being 2) Handling of Pose Graph Optimization Events: The first
correctly discarded from the global frontier, and therefore the step in Algorithm 3 is to rebuilt the R-tree (lines 1–3), which
ORŠULIĆ et al.: EFFICIENT DENSE FRONTIER DETECTION FOR 2-D GRAPH SLAM BASED ON OCCUPANCY GRID SUBMAPS 3575

runs in O(|S| log |S|). This complexity also includes looking TABLE I
EXPERIMENTAL SETUP AND RESULTS
up the intersecting submaps for each submap (line 6).
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, lines 1–3). A
pessimistic bound would entail testing every local frontier point
against all other submaps, i.e. a time complexity of O(|S| ·
P (LF )). The pessimism of this bound can be reduced by assum-
ing 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 (F F ), this assumption
yields the time complexity of O(|S| · P (GF ) + P (F F )).
The total time complexity of handling a pose graph optimiza-
tion event thus depends on the number of submaps and the local
and global frontier perimeters:
O(|S| · (log |S| + P (GF )) + P (F F )) (3)
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 algo-
rithm (available on Github)1 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 Cartog-
rapher, the Google Deutsches Museum bag [1], captured with a
human-carried sensor backpack.
We have also performed an evaluation with the Freiburg FR-
079 dataset2 in order to roughly compare with [9], who tested Running frontier detection synchronously (in the main SLAM
most of the algorithms described in Section II in a simulated thread) makes it easier to measure the exact time spent perform-
FR-079 environment. ing frontier detection, which can be measured as the difference
Demo videos of frontier detection for both evaluated datasets of wall clock durations of processing a dataset with synchronous
are available,3 along with an additional demo of a multi-robot frontier detection and with no frontier detection at all.
scenario where the dataset was recorded with two teleoperated The results are given in Table I. We believe that the Deutsches
Pioneer mobile robots driven side-by-side.4 Museum result is an achievement with respect to the state of the
We have opted to use the Google Cartographer offline node, art. To compare, the best performing-algorithm in [9], EWFD,
which processes a ROS bag dataset as fast as the CPU can handle achieved 290 ms and 190 ms per frontier update iteration in
(around 4-5x realtime on Deutsches Museum), enabling us to two simulated FR-097 environment trajectories, with signifi-
demonstrate frontier update frequencies far greater than the scan cantly lower numbers of processed scans (252 scans for the
frequency of a single robot’s LIDAR. 290 ms Freiburg 1). We have achieved sub-millisecond (0.92 ms)
In order to minimize the impact on SLAM performance, the wall-time per frontier update, while handling a fairly larger
frontier detection algorithm may execute asynchronously, in a number of scans (4198) and 25 pose graph optimizations on
separate thread. The frontier detection algorithm tries to pro- average. We do have to point out that our experiments have been
cess all submap updates, while it can adaptively skip non-final performed on newer hardware, and that we have not performed
submap updates in case the processing speed falls behind SLAM. frontier grouping, but rather return the detected frontier as an
In the asynchronous case on Deutsches Museum, the wall clock unstructured set of points.
frequency of incremental frontier updates has been measured as
a benchmark of frontier detection performance. A. Additional Notes on Implementation of the Frontier
Detection Algorithm
1 https://github.com/larics/cartographer_frontier_detection
2 ROS bag and SLAM parameters are available in the Github repository.
An optimization that we have implemented is performing
3 https://goo.gl/62zEUy. additional stabbing query tests against a small number of tempo-
4 Dataset and SLAM parameters also available in the Github repository. rally close submaps in the local frontier edge detection condition
3576 IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019

in Algorithm 1, line 8 (for example, against 4 previous submaps). algorithm for frontier detection is the basis for any such further
This has the result of permanently “baking in” a negative test improvements.
result against these submaps, since these points are permanently
erased from the local frontiers.
REFERENCES
A benefit of discarding points early from local frontiers is
speeding up recomputation of the global frontier in Algorithm 3, [1] W. Hess, D. Kohler, H. Rapp, and D. Andor, “Real-time loop closure
in 2d LIDAR SLAM,” in Proc. IEEE Int. Conf. Robot. Autom., 2016,
because there will be fewer candidates in the local frontier which pp. 1271–1278.
have to be re-transformed and re-tested. Also, it is expected [2] B. Yamauchi, “A frontier-based approach for autonomous exploration,”
that the localization drift between sequential submaps is small, in Proc. IEEE Int. Symp. Comput. Intell. Robot. Autom., 1997, pp. 146–
151.
and that pose graph optimization will not produce a significant [3] W. Burgard, M. Moors, D. Fox, R. Simmons, and S. Thrun, “Collaborative
relative displacement between sequential submaps. Using the multi-robot exploration,” in Proc. IEEE Int. Conf. Robot. Autom. Symposia
unoptimized poses may actually be preferential for process- Proc., Millennium Conf. (Cat. No. 00CH37065), 2000. [Online]. Available:
https://doi.org/10.1109/robot.2000.844100
ing sequential submaps, because it could prevent detecting a [4] L. Freda and G. Oriolo, “Frontier-based probabilistic strategies for sensor-
false frontier resulting from slight misalignment of sequential based exploration,” in Proc. IEEE Int. Conf. Robot. Autom., 2005. [Online].
submaps introduced by pose graph optimization when closing Available: https://doi.org/10.1109/robot.2005.1570713
[5] J. Wettach and K. Berns, “Dynamic frontier based exploration with a
loops. mobile indoor robot,” in Proc. ISR (41st Int. Symp. Robot.) ROBOTIK
We have employed a few cosmetic improvements which (6th German Conf. Robot.), Jun. 2010, pp. 1–8.
result in (subjectively) aesthetically better frontiers. The first [6] W. Burgard, M. Moors, C. Stachniss, and F. Schneider, “Coordinated multi-
robot exploration,” IEEE Trans. Robot., vol. 21, no. 3, pp. 376–386, Jun.
is a change to (1) where cells with a very uncertain “unoccu- 2005. [Online]. Available: https://doi.org/10.1109/tro.2004.839232
pied” probability (i.e. Ssi
k,l ∈ [0.5 − ε, 0.5], where ε := 0.04 is [7] J. Faigl and M. Kulich, “On determination of goal candidates in frontier-
an arbitrarily chosen small value) are treated as “unobserved” based multi-robot exploration,” in Proc. Eur. Conf. Mobile Robots, Sep.
2013. [Online]. Available: https://doi.org/10.1109/ecmr.2013.6698844
cells. This prevents detecting a false frontier around single false [8] R. Shade and P. Newman, “Choosing where to go: Complete 3d exploration
long-distance laser readings, which cause insertion of a false ray with stereo,” in Proc. IEEE Int. Conf. Robot. Autom., May 2011. [Online].
of “unoccupied” cells into the submap. This will also have the Available: https://doi.org/10.1109/icra.2011.5980121
[9] P. Quin, A. Alempijevic, G. Paul, and D. Liu, “Expanding wavefront
beneficial effect of driving the robot exploration system to get frontier detection: An approach for efficiently detecting frontier cells,”
a “better look” at areas with uncertain unoccupied cells, since in Proc. Australasian Conf. Robot. Autom., 2014, pp. 113–120.
they are considered unexplored. [10] M. Keidar and G. A. Kaminka, “Efficient frontier detection for robot
exploration,” Int. J. Robot. Res., vol. 33, no. 2, pp. 215–236, Oct. 2013.
The second change we have made is simple smoothing of local [Online]. Available: https://doi.org/10.1177/0278364913494911
frontiers by adjusting the definition of a local frontier to be the [11] G. Grisetti, C. Stachniss, and W. Burgard, “Improved techniques for grid
center of an unobserved cell with ≥ 2 unoccupied adjacent cells mapping with Rao-Blackwellized particle filters,” IEEE Trans. Robot.,
vol. 23, no. 1, pp. 34–46, Feb. 2007.
and ≥ 2 unobserved adjacent cells, where adjacent cells are cells [12] P. Senarathne, D. Wang, Z. Wang, and Q. Chen, “Efficient frontier detection
in the Moore 8-neighbourhood. With no other changes to the and management for robot exploration,” in Proc. IEEE 3rd Annu. Int. Conf.
rest of the algorithm, these changes result in a smoother global Cyber Technol. Autom., Control Intell. Syst., 2013, pp. 114–119.
[13] H. Umari and S. Mukhopadhyay, “Autonomous robotic exploration
frontier, since the smoothed local frontier points are candidates based on multiple rapidly-exploring randomized trees,” in Proc.
for the global frontier. IEEE/RSJ Int. Conf. Intell. Robots Syst., Sep. 2017. [Online]. Available:
https://doi.org/10.1109/iros.2017.8202319
[14] J. J. Leonard and H. F. Durrant-Whyte, “Simultaneous map building and
VII. CONCLUSION AND FUTURE WORK localization for an autonomous mobile robot,” in Proc. IEEE/RSJ Int.
Workshop Intell. Robots Syst., 1991, pp. 1442–1447.
We have described, implemented and tested an efficient fron- [15] S. Agarwal, K. Mierle, and others, Ceres Solver. [Online]. Available:
tier detection algorithm that is specialized for 2D multi-robot http://ceres-solver.org
graph SLAM based on occupancy grid submaps. Our algorithm [16] S. Williams, G. Dissanayake, and H. Durrant-Whyte, “An efficient ap-
proach to the simultaneous localisation and mapping problem,” in Proc.
is efficiently constrained to the area of active submaps, yet robust IEEE Int. Conf. Robot. Autom. (Cat. No. 02CH37292), 2002. [Online].
to loop closure by efficiently recomputing the global frontier Available: https://doi.org/10.1109/robot.2002.1013394
after pose graph optimization is performed without the time [17] J. Strom and E. Olson, “Occupancy grid rasterization in large environments
for teams of robots,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2011,
complexity being a function of map area. pp. 4271–4276.
Future work: Using the proposed frontier detection algorithm [18] M. Bosse and R. Zlot, “Map matching and data association for large-scale
in an actual closed exploration-system loop (Fig. 1) is a work in two-dimensional laser scan-based slam,” Int. J. Robot. Res., vol. 27, no. 6,
pp. 667–691, 2008.
progress. Also, some cited state of the art (e.g. [10]) performs [19] K. Konolige, G. Grisetti, R. Kümmerle, W. Burgard, B. Limketkai, and
grouping of continuous frontier points into segments, which is R. Vincent, “Efficient sparse pose adjustment for 2d mapping,” in Proc.
useful for selecting navigation objectives of exploration tasks. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2010, pp. 22–29.
[20] G. Guennebaud et al., Eigen v3. (2010). [Online]. Available: http://eigen.
Another kind of post-processing of the detected frontier which tuxfamily.org.
might be of interest is reachability analysis, ie. detecting frontier [21] Boost R-tree spatial index. [Online]. Available: https://www.boost.org
points such as points behind glass, closed doors or behind /doc/libs/1_68_0/libs/geometry/doc/html/geometry/spatial_indexes/intro
duction.html
walls (e.g. from false laser readings). Nonetheless, a performant