Next-Best-View planning for surface reconstruction of

large-scale 3D environments with multiple UAVs

Guillaume Hardouin1,2 , Julien Moras1 , Fabio Morbidi2 , Julien Marzat1 , El Mustapha Mouaddib2

Abstract— In this paper, we propose a novel cluster-based

Next-Best-View path planning algorithm to simultaneously
explore and inspect large-scale unknown environments with
multiple Unmanned Aerial Vehicles (UAVs). In the majority
of existing informative path-planning methods, a volumetric
criterion is used for the exploration of unknown areas, and
the presence of surfaces is only taken into account indirectly.
Unfortunately, this approach may lead to inaccurate 3D models,
with no guarantee of global surface coverage. To perform
accurate 3D reconstructions and minimize runtime, we extend
our previous online planner based on TSDF (Truncated Signed
Distance Function) mapping, to a fleet of UAVs. Sensor confi-
gurations to be visited are directly extracted from the map and
assigned greedily to the aerial vehicles, in order to maximize the
global utility at the fleet level. The performances of the proposed Fig. 1. Example outputs from our multi-UAV surface reconstruction
TSGA (TSP-Greedy Allocation) planner and of a nearest algorithm. [left] The Statue of Liberty reconstructed by 3 UAVs; [Right]
neighbor planner have been compared via realistic numerical Powerplant reconstructed by 5 UAVs.
experiments in two challenging environments (a power plant
and the Statue of Liberty) with up to five quadrotor UAVs
equipped with stereo cameras.
be alleviated by performing the requested task with a team
I. I NTRODUCTION of cooperating robots.
Autonomous robots are being increasingly used today for In this paper, we extend our previous work [1] on incre-
time-consuming and dangerous tasks usually performed by mental exploration and surface reconstruction of an unknown
human operators. For instance, aerial robots equipped with 3D environment, to a fleet of Unmanned Aerial Vehicles
different on-board sensors (RGB-D, stereo cameras, laser (UAVs). Differently from the classical surface frontier-based
range finders, etc.) hold great potential for modeling large- approaches [6], [7], we identify the incomplete areas directly
scale 3D structures. Recent applications include digital cul- from the online map and generate candidate sensor view-
tural heritage, exploration of confined and cluttered environ- points in the free space, to complete them. The proposed
ments, and structural inspection for preventive maintenance NBV planning method greedily assigns these configurations
[1]–[4]. Next-Best-View (NBV) planning [5], is a planning to the robots, according to the “utility” (i.e. information gain)
method in which a robot iteratively computes the best view- they locally provide to the fleet and known map (see Fig. 1).
point configurations to fully reconstruct the 3D environment, Optimal paths are computed by solving a variant of the
which can be (partially) known in advance or completely Travelling Salesman Problem (TSP) [8], and updated when
unknown. In the latter case, the robot is forced to dynami- unknown areas are completed. In summary, the original
cally discover the surrounding environment in the course of contributions of this paper are threefold:
the mission. The focus can be either on the exploration of • A multi-UAV NBV planner is designed to visit view-
the 3D volume (exploration problem) or on the consistency point configurations for surface reconstruction. It relies
and completeness of the reconstructed surface (inspection on a greedy allocation of configurations and on the
problem), for which different path-planning strategies have successive (approximate) resolution of the TSP.
been developed in the literature. When a single aerial robot • A single volumetric representation of surfaces, the Trun-
is used, the time necessary to cover large-scale environments cated Signed Distance Function (TSDF) [9], is used
may be prohibitively long, and incompatible with limited on- to cautiously navigate, identify incomplete areas, and
board power resources and flight autonomy (15-20 minutes evaluate the information gain and the quality of 3D
for a standard battery-powered quadrotor). This problem can reconstruction.
• The proposed method has been extensively validated in
1 DTIS, ONERA, Université Paris-Saclay, 91123 Palaiseau, France.
two realistic simulation environments.
Emails: [email protected]
2 MIS laboratory, Université de Picardie Jules Verne, 80000 Amiens, The remainder of this paper is organized as follows.
France. Emails: {fabio.morbidi, mouaddib}@u-picardie.fr In Sect. II, we review the existing literature on multi-
This work has been supported by ONERA DTIS and by the Hauts-de-
France region, through the research project ScanBot, “Robotic Scanners robot 3D reconstruction and NBV planning. Sect. III is
for Automatic 3D Modeling of Cultural Heritage” (2018-2021). devoted to the problem formulation, and in Sect. IV the
proposed method is described. The results of our numerical cost and utility for a multi-agent exploration mission, with
experiments, including a comparative study, are discussed in robots equipped with laser scanners in a 2D environment.
Sect. V. Finally, Sect. VI outlines the main contributions A cooperative frontier-based approach for a team of UAVs
of the paper, and presents some possible directions for flying in uncluttered (outdoor) areas, has been also recently
future research. proposed in [23]. It is one of the first works to consider a
3D space representation for multi-robot exploration, with a
II. R ELATED W ORK centralized Octomap built on a ground station, which is also
The ability to plan informative paths for online exploration used to coordinate the motion of the UAVs (RRT*-based
and modeling of 3D environments, is a fundamental skill that path planning). The method was evaluated using realistic
highly increases the level of autonomy of a robot. Numerous numerical experiments (ROS/Gazebo) with simulated stereo
strategies for 3D modeling of an unknown environment sensors. The approximation of the mutual information of
with a sensor-equipped robotic platform, are available in the range sensors [24] also led to the development of exploration
literature. In this paper, we focus on NBV methods which de- approaches based on probabilistic occupancy maps with
termine the best viewpoints to visit depending on the nature entropy reduction, such as decMCTS [25] or SGA [26].
of the robot’s mission. Surface inspection methods analyze Recently, in [27], [28], a finite-horizon decentralized planner
the reconstructed surface for viewpoint definition. The goal (DGSA) has been designed using sampling-based Monte
is to ensure that the reconstruction is accurate and complete. Carlo Tree Search (MCTS) [29]. The trajectories of the
Among them, frontier-based methods generate viewpoint robots are assigned by solving a submodular maximization
configurations on the frontier of the observed surface, which problem over matroid constraints with greedy assignment
satisfy some orientation, positioning, and sensing constraints. heuristics, for which polynomial-time algorithms and sub-
These configurations, visited by the sensor-equipped robot, optimality bounds can be established [30]. However, these
provide new information about the surface, with some over- mutual-information methods rely on occupancy mapping,
lapping to ensure continuity [5], [10], [11]. These methods and they do not exploit any surface representation. Therefore,
have been mostly applied for object reconstruction with robot it might be problematic, in practice, to assess the quality of
manipulators, under strong assumptions on the navigable free 3D reconstruction.
space. However, several works have extended NBV to mobile In this paper, we explicitly address the surface inspec-
robots by considering a volumetric representation [10], [12] tion problem by considering a NBV frontier-based planning
and more recently the TSDF [1], [13], [14]. On the other strategy for multiple UAVs in large-scale environments.
hand, volumetric exploration aims at exploring a predefined Viewpoint configurations are clustered according to their
volume containing the object of interest. It usually proceeds location in space to evaluate the interest of visiting specific
by building a map of a large unknown environment by regions and find a suitable path. We use the property of
using a 3D grid model (such as Octomap [15]) to identify submodular set functions to formalize the assignment prob-
known, unknown and occupied areas. The majority of recent lem [30], [31], and to allocate sensor configurations using
exploration methods rely on sampling-based planning [16]– a TSP-based greedy local search. Successive approximate
[18]. They expand a random tree (e.g. RRT/RRT* [19]) of resolutions allow to cover the unknown environment and
sampled sensor configurations in the free space, and select complete the reconstruction.
the NBV trajectory that guarantees the maximum coverage
of the volume. Such receding horizon methods are efficient, III. P ROBLEM FORMULATION
but the accuracy of the reconstructed surface is not explicitly This paper builds upon our previous work [1], and extends
taken into account. In order to guarantee fast exploration and it to a multi-robot setting. A fleet of N identical UAVs with
reconstruction accuracy, some recent methods have tried to 4 degrees of freedom (the 3D position [x, y, z]T ∈ R3
combine the two approaches. In [4], the authors subdivided and the yaw angle ψ ∈ S 1 ) is considered. Each UAV is
the reconstruction into two phases: in the first one, a coarse equipped with a forward-facing depth sensor with limited
model of the environment is obtained, while in the second field of view (FOV) and sensing range, extrinsically cali-
phase the reconstructed surface is refined. Other methods brated with respect to the body frame of the aerial robot.
try to cover the whole surface model by reasoning on the The UAVs should scan an unknown 3D environment (for
occupancy map [20], and the exploration path is refined instance, a building), characterized by its surface. A mapping
by accounting for surface incompleteness [3]. However, a algorithm is required in order to build a representation of
drawback of these strategies is that they rely on multiple the reconstructed surface as a collection of voxels, and to
volumetric and surface representations at the same time, and estimate unknown, occupied and empty space (for this, a
only a few methods deal directly with the surface inspection TSDF representation [9] was considered). A voxel is said
problem [1], [14]. to be scanned if it has been seen by a UAV. We assume
The aforementioned works only feature a single robot, and that the UAVs are equipped with an accurate localization
online incremental reconstruction with multiple cooperating system which allows to estimate their pose with respect
robots is an open problem in the literature [21]. The authors to a global reference frame, and that a robust lower-level
in [22] were the first to propose how to compute frontier trajectory-tracking algorithm is available (Model Predictive
cells, and to define the trade-off between their distance Control was used in our implementation [32]).
 The incompleteness of the surface model is defined as
follows:
Definition 1 (Incomplete surface element). We call In-
complete Surface Element (ISE), a voxel v lying on the
surface at a frontier, near both the unknown and empty space.
We denote by C the set of all ISEs.
Fig. 2. General flowchart of the reconstruction algorithm: the architecture
For a rigorous definition of unknown, occupied and empty of the planner is shown inside the shaded box.
space, the reader is referred to Sect. IV-B.
Definition 2 (Remaining incomplete surface). Let Q be
the set of all collision-free configurations q = [x, y, z, ψ]T depth measurements via an inverse-squared distance incre-
of a UAV, and let Qc ⊆ Q be the set of all configu- ment 1/zq2 (v) for the weight w, where zq (v) is the distance
rations q from which an ISE v ∈ C can be scanned. between voxel v and the current sensor pose q. With this
The remaining
S incomplete surface is then defined as choice, the sensing error is minimized [33], [34]. This model
Crem = v ∈ C v | Qc = ∅ . implicitly represents a surface, which corresponds to the zero
level set of the distance field: hence, the TSDF volume is a
The vector-valued function pij,k (s) : [0, 1] → R3 × S 1
volumetric representation of a surface. Depending on the
defines the path of UAV i from configuration j to config-
values of φ and w, one can determine whether a voxel is
uration k, where pij,k (0) = qij and pij,k (1) = qik , i ∈
unknown or known, i.e. occupied or empty. A voxel v is
{1, . . . , N }. We assume that pij,k (s) is collision-free and
considered known if w(v) ≥ Wth and unknown if w(v) <
feasible for UAV i (i.e. the kinematic/dynamic constraints
Wth , where the threshold Wth depends on the sensing range
of the aerial robot are satisfied along the path). We are now
of the depth sensor.
ready to state the problem studied in this paper.
Problem 1 (Inspection with a fleet of UAVs). Given a fleet B. ISE extractor and viewpoint generation
of N identical UAVs with initial configurations qi0 ∈ Q, Using the proposed surface model and following [13],
i ∈ {1, . . . , N }, find collision-free paths pi0,f (s) between a voxel v ∈ M is an ISE, i.e. v ∈ C, if the following
qi0 and the final configurations qif , which allow the aerial three conditions are fulfilled:
vehicles to scan the set Cins = C \ Crem of all ISEs. a) w(v) ≥ Wth ∧ φ(v) > 0, (empty)
IV. P ROPOSED APPROACH b) ∃ u ∈ Nv6 s.t. w(u) < Wth , (unknown)

In this paper, we consider a centralized architecture where c) ∃ o ∈ Nv18 s.t. w(o) ≥ Wth ∧ φ(o) ≤ 0, (occupied)
the mapping and planning information is shared among all where Nv6 and Nv18 denote the 6- and 18-connected voxel
the UAVs, and the communication is assumed to be perfect. neighborhoods of v, respectively. With reference to Defini-
The general flowchart of our algorithm is depicted in Fig. 2. tions 1 and 2, we introduce the notion of scanned element:
During the exploration, the poses and depth maps of each
UAV are sent to a ground station and the volumetric map is Definition 3 (Scanned element). A voxel v ∈ M which
incrementally built (Sect. IV-A). Then, the ISE extractor is satisfies w(u) ≥ Wth , ∀ u ∈ Nv6 , is called a scanned
used to identify sets of ISEs. Configurations which allow element.
to complete them are generated and clustered, depending The determination of a direction nv to observe the ISE v,
on their location in the 3D space (Sect. IV-B). The planner is based on the gradient of the weight function ∇w(x, y, z),
generates and continuously expands a directed graph which which can be computed as,
represents the travel utility between clusters in the free space. X c−v
In order to maximize a cumulative utility function for the nv = w0 (c) ,
robots, paths are extracted from the graph ensuring collision- 26
kc − vk
c ∈ Nv
free navigation and they are broadcast to the UAVs. As the
map is updated, new ISEs are uncovered and the path to where Nv26 is the 26-connected neighborhood of v and the
complete them is updated (Sect. IV-C). The 3D model is weight function w0 is
considered complete when no ISEs are left.
(
0 −Wth if voxel c is occupied,
w (c) =
A. Surface-based mapping Wth otherwise.
Surface-based reconstruction is performed over time to Note that nv is not a unit vector. A new sensor configuration
allow detection of incomplete areas. The depth maps sensed is generated along the direction nv at a distance δpose from
by the UAVs are integrated in a TSDF volumetric map M , voxel v (see Fig. 3). The sensor points towards v along
which consists of a voxel grid, where each voxel contains −nv and the value of δpose depends on the sensing range
a truncated signed distance value φ ∈ R and a weight of the depth camera. Two poses qj , qk ∈ Q generated from
w ≥ 0. The model is progressively built by integrating the the ISEs vj , vk ∈ C, respectively, may be very close, i.e.
 of cluster v. Then, the weight auv between cluster u and v
is the 6-tuple defined as,
auv = τkl , τlm , g(v), d(τkl ), d(τlm ), fuv ,

(2)
where
l i i
• τk denotes the path from qk ∈ u to ql ∈ v, i.e. the path
between cluster u and cluster v. We select qil among
all the configurations of v, so that τkl is the shortest
possible path,
m
Fig. 3. [left] Two-dimensional example of ISE v (filled green square). • τl denotes the shortest Hamiltonian path [36] including
Its 2D neighborhood is represented by a dashed green square. Unknown configurations of v, which starts at qil and ends at qim ,
voxels are black, occupied are gray, and empty voxels are white. The recon-
structed surface is depicted as a blue segment, and the sensor configuration • g(v) is the gain of cluster v, as defined in (1),
l
and its frustum as dark blue triangles; [right] Direction from the contour, • d(τk ) is the cost associated with the inter-cluster
nv , and corresponding viewpoint configuration at distance δpose from v path τkl , i.e. the length of τkl ,
(light blue triangle). m
• d(τl ) is the cost associated with the intra-cluster
path τlm , i.e. the length of τlm ,
• fuv is the utility function defined as,
dist(qj , qk ) <  for a small  > 0, and the viewing
fuv = g(v) exp −λtc d(τkl ) − λic d(τlm ) , (3)


directions nvj , nvk , almost parallel, i.e. |nvj · nvk | ' 1.
These configurations are then aggregated into a single view- where λtc and λic are positive penalty terms for the
point by averaging their positions and orientations to reduce inter-cluster and intra-cluster costs, respectively, which
the number of overall poses. can be used to promote the visit of clusters far apart or
Large-scale environments may result in a prohibitive large clusters. A similar utility function was originally
number of possible viewpoint configurations, which are proposed in [37].
impractical for planning purposes. To overcome this issue The weights on the directed graph G in (2), quantify the
and easily identify the most promising areas to scan, the potential benefit of choosing a path to pursue the 3D recon-
viewpoints are grouped into clusters uj , j ∈ {1, 2, . . . , Nc }, struction: the higher the value of the function fuv , the more
depending on their location in space. We denote by U = beneficial is the path. Since the formulation of the inspection
{u1 , u2 , . . . , uNc } the set of all clusters. A configuration problem depends on the number of robots, we will separately
ql belongs to a generic cluster u if ∃ qj ∈ u such that analyze the case of a single UAV and of multiple UAVs.
d(τlj ) < dν , where d(τlj ) denotes the length of the path 1) Single UAV: If we consider a single UAV as in [1],
τlj between ql and qj on a directed graph we will define we can formalize the inspection problem as a maximum
in Sect. IV-C, and dν is an upper bound on the distance. Asymmetric Travelling Salesman Problem (maxATSP), i.e. as
If no neighbors are found, dν is increased up to a maximum the problem of finding a maximum-utility Hamiltonian path
value dmax
ν . p on G. In what follows, we will denote by maxATSP(U ) the
Once the clusters have been defined, their level of infor- set function that takes the set of clusters U and outputs its
mativeness should be quantified. To evaluate a configuration, utility value p, from which path p is computed. In practice,
we use the ray-tracing method [35] from a frontier-based the ATSP is solved by first converting it into a symmetric
perspective, i.e. we count the number of ISEs that can be TSP (i.e. a standard TSP) and then using the well-known
seen. Let Cq be S the set of all ISEs seen from viewpoint Lin-Kernighan heuristic [38].
q and let Cu = q ∈ u Cq . The gain g(u) of cluster u is 2) Multiple UAVs: In the multi-robot case, the clusters
defined as, should be suitably allocated to the UAVs, and a cluster
|Cu |
g(u) = , (1) assignment problem should be solved. LetSU i be the set
|C| N
of clusters assigned to robot i, such that i = 1 U i = U .
where |Cu | denotes the cardinality of the set Cu . Then, the assignment problem can be stated as follows,
N
C. Next-Best-View planning nX
max maxATSP(U i ) | U i ∩ U ` = ∅,
The planner globally allocates clusters to the UAVs and U 1 , ..., U N ⊂ U
i=1
N
(4)
schedules their visit according to a given common TSDF [ i
o
map. To formalize this idea, let us introduce the weighted i 6= `, U = U ,
i=1
directed graph G = (U, E, {auv }(u, v) ∈ E ), where U is the PN
set of nodes (in our case, the clusters), E is the set of edges, where i=1 maxATSP(U i ) is a non-decreasing submodular
and {auv }(u, v) ∈ E is the collection of weights for the edges. set function [30], and the space of feasible paths has the
Each edge euv ∈ E is directed and links cluster u to v, with structure of a simple partition matroid. The submodular
u, v ∈ U . Let us assume that the initial configuration of maximisation problem (4) can be approximately solved using
UAV i belongs to one of the clusters of G, i.e. qi0 ∈ U . Let qik local greedy heuristics [31], such as the TSP-Greedy Alloca-
be a configuration of cluster u, and qil , qim two configurations tion (TSGA) procedure reported in Algorithm 1. Note that
 Algorithm 1: TSP-greedy allocation (TSGA)
Set U i = ∅ and pi = 0, ∀ i ∈ {1, . . . , N };
foreach cluster v ∈ U do
i ← arg max {maxATSP(U k ∪ v) − pk };
k ∈ {1,...,N }
Ui ← Ui ∪ v ;
pi ← maxATSP(U i );
piU i ← {pi , U i };
Broadcast the paths {p1U 1 , . . . , pN
U N } to the UAVs;

Fig. 4. Gazebo simulation environments: [left] Scenario 1, Powerplant;

[right] Scenario 2, the Statue of Liberty.
while provable worst-case bounds on the suboptimality can
be established [39], a detailed mathematical analysis is
beyond the scope of this paper and is not reported here.
reconstruction accuracy achieved by the fleet and by a single
The TSGA algorithm greedily assigns a cluster to a UAV,
UAV, using the strategy proposed in [1].
when it locally maximizes the overall utility of the fleet of
1) Experimental setup: An industrial plant benchmark,
N robots. Unlike classical insertion methods where a cluster
which is widely used in the volumetric exploration literature,
is added to a robot’s path [39], the maxATSP problem is
has been chosen (Scenario 1) [3]. In order to study the
solved for the extended cluster set U i ∪ v, which results
impact of the two penalty terms in the utility function (3),
in a more efficient path for UAV i. Once the clusters have
on the reconstruction accuracy/completeness, we also con-
been assigned and the corresponding paths p1U 1 , . . . , pNUN sidered a monumental statue (Scenario 2) [20], see Fig. 4.
computed, they are broadcast to each UAV. This strategy
The simulation parameters used in the two scenarios are
maximizes the individual utility of the UAVs over disjoint
reported in Table I. We used the RotorS simulator [40] to
sets, to maximize fleet-wise utility, and it is amenable to a
model quadrotor UAVs equipped with a stereo camera, in the
distributed implementation in which only local information
ROS-Gazebo1 environment. To represent depth map uncer-
is used (such as, local free space, ISEs, U i related to the
tainty, we considered a Gaussian noise model. The standard
local map of UAV i).
deviation associated with a pixel corresponds to the depth-
The maximization of the utility function over the long run,
value sensing error of the corresponding point located at a
prompts the UAVs to explore the areas which appear to be
distance z, i.e. σ(z) = |ef Bd | z 2 , where |ed | is the magnitude
the most valuable in terms of completeness and quality. This
of the disparity error, f the focal length in pixels, and B
might induce a UAV to visit a configuration that covers an
the baseline of the stereo camera in meters. Following [33],
area containing multiple ISEs and scan them all (cf. equ. (1)).
[41], the raw depth map was smoothed out by using a 3 × 3
It is then pointless to visit configurations associated to ISEs
kernel. The TSDF volume was generated with the algorithm
that have already been scanned. To avoid unnecessary visits,
proposed in [42], where the reconstruction is performed
the planner computes the remaining ISEs of paths since the
with MarchingCubes [43], and the weight increment has
last iteration, and possibly updates the plan. In practice,
been modified to be quadratic, as reported in Sect. IV-A.
a path is updated when 50% of the ISEs whose visit is
Collision-free UAV paths have been computed using the Lazy
currently scheduled, has been completed. This update rule
ensures a reactive visit of revealed ISEs as the map grows. 1 https://ros.org/ , http://gazebosim.org/
Note that Algorithm 1 may allocate paths of various
length: hence, the UAVs may conclude their tours at different
TABLE I
time instants. To reduce the idle time of the UAVs finishing
PARAMETERS USED IN THE NUMERICAL EXPERIMENTS .
first, we asynchronously assign new paths to them in order to
Parameter Scenario 1 Scenario 2
visit clusters which are not currently allocated to any robot
Voxel resolution rv [m] 0.3 0.15
(e.g. if UAV i has not completed its exploration round, the Threshold Wth 0.3 0.3
set of clusters assigned to the fleet becomes U \ U i ). The emax [m] 0.2598 0.1299
reconstruction procedure stops when no more ISEs are left. Camera range [m] [1.6, 8] [1, 5]
Camera FOV [deg.] (H, V) 90 × 60 90 × 60
V. N UMERICAL EXPERIMENTS ed [pixels] 0.1 0.1
f [pixels] 376 376
The proposed surface-driven method has been validated B [m] 0.11 0.11
via realistic numerical experiments. We compared the TSGA Collision radius [m] 1 1
planner with another multi-robot planner, the Nearest Neigh- UAV nominal speed [m/s] 0.5 0.5
bor (NNB) greedy algorithm, for a fleet of 3 and 5 UAVs. δpose [m] 4.7 3.6
In NBB, only one cluster is allocated to each robot by locally dν [m] 2.0 2.5
dmax
ν [m] 5 5
computing max v ∈ U fuv for the updated map. Replanning Penalty term λtc 0.3 0.17
is thus very fast compared to TSGA, but only one cluster Penalty term λic 0.03 0.15
at a time is set to be visited. We also evaluated the 3D
Fig. 5. [top] Scenario 1 and [bottom] Scenario 2: Reconstructed mesh and 3D exploration paths p10,f , p20,f , p30,f (green, red, blue) of 3 UAVs (the initial
locations are marked with magenta dots); [left] NNB planner; [middle] TSGA planner; [right] Signed distance error: the color coding shows the error in
meters with respect to the ground truth, computed with CloudCompare’s M3C2 algorithm.

PRM* planner from the Open Motion Planning Library [44]: 2) Metrics: The single-UAV planner proposed in [1] is
in this way, we can find the shortest path between two used as a baseline to evaluate our new multi-robot strategy,
configurations by taking the structure of the TSDF map and in terms of cumulated path length and completion time (to
the current location of other UAVs into account (the collision cover the entire 3D environments). This includes travel time
radius was set to 1 m). Lazy PRM* allows multi-query and sensing time (e.g. one depth map integration and map
path planning to all destination points, which is useful for update). The reconstructed 3D surface has been evaluated
reachable-path checking and distance evaluation, because of with CloudCompare4 using the M3C2 (Multiscale Model
the reduced computational complexity compared to RRT (the to Model Cloud Comparison) algorithm [45]. To quantify
average runtime is below 1 s). The UAVs track the generated how well the surface has been recovered, dense point clouds
paths using Model Predictive Control [32]: the reference were sampled on the reconstructed and ground truth (GT)
translational velocity was fixed at 0.5 m/s. In Scenario 1, meshes, and their deviation was measured by performing a
the popular Powerplant model2 was scaled to fit in a box of cloud-to-cloud comparison (see Fig. 5 [right]). For a fair
size 65×42×15 m3 (as a consequence, the five flues have the evaluation, all the invisible surfaces of the GT mesh were
same height, see Fig. 4). Because of its narrow passages, high pruned beforehand (e.g. the interior floor and walls), and
walls and roof, large flues and thin gantries, Powerplant is the analysis was restricted to the exterior surface mesh only.
challenging for both navigation and reconstruction (occlusion A point belonging to the GT point cloud was considered
problem). In Scenario 2, we considered a model of the to be covered by a corresponding one in the reconstructed
Statue of Liberty3 (20×20×60 m3 ), which contains multiple cloud, if their absolute distance was less than√ the length of
sharp edges and fine details. In both scenarios, the UAVs the half diagonal of a voxel, i.e. emax = rv 3/2, where rv
are initially located in the same area, around a ground is voxel’s resolution. The quality of the recovered surface is
station (magenta dots in Fig. 5) The results of our numerical evaluated in Table II, by reporting the average and standard
experiments are reported in Table II. The single-UAV planner deviation of the signed distance error with respect to the GT
with perfect and noisy depth measurements (denoted by [1] point cloud, and the root-mean-square error (RMSE).
and [1]∗ , respectively), is compared here with the TSGA 3) Penalty terms: The choice of the penalty terms λtc and
and NNB planners for 3 and 5 UAVs. To obtain statistically- λic appearing in the utility function (3), depends on the nature
significant values, 10 trials per scenario were carried out. of the 3D environment explored by the UAVs. Scenario 1
We ran all tests on a Dell Precision 7520 laptop with and 2 are, in this respect, two representative examples. In a
2.90 GHz Intel Core i7 processor, 16 GB RAM and Quadro wide, box-like environment as Scenario 1, the ISEs tend to
M2200 graphics card. appear in the proximity of occluded regions and sharp edges,
2 http://models.gazebosim.org/
3 https://free3D.com/ 4 https://danielgm.net/cc/
 TABLE II
R ESULTS OF THE NUMERICAL EXPERIMENTS ( STATISTICS OVER 10 TRIALS ).
Scenario 1 Scenario 2
Number of UAVs 1 3 5 1 3 5
Algorithm [1] [1]∗ NNB TSGA NNB TSGA [1] [1]∗ NNB TSGA NNB TSGA
Path length [m] 780 785 1038 790 1113 879 547 550 733 721 580 574.2
Completion time [min.] 32 330 1000 110 0900 100 2000 60 5100 60 2200 360 370 130 1000 100 1800 70 3000 60 4500
Time gain [%] w.r.t. [1]∗ − − 66.4 68.8 79.4 80.8 − − 64.4 72.2 79.7 81.8
Surface coverage [%] 91.5 90.4 90 91 90.5 90.6 92.3 91.2 91.1 91 90.9 91.1
M3C2 avg. error [cm] 0.14 −0.13 −0.15 −0.26 −0.11 −0.3 0.29 −0.02 −0.8 −0.03 0.06 −0.01
M3C2 std. error [cm] 5.85 7.51 7.52 7.54 7.5 7.52 3.41 3.67 3.61 3.69 3.65 3.66
RMSE [cm] 5.86 7.51 7.52 7.55 7.5 7.52 3.43 3.67 3.69 3.69 3.65 3.66

and large stretches of known surface may separate these in wide and large environments (Scenario 1), but the results
sites. To minimize the total distance traveled, inter-cluster are comparable in medium-size structures (Scenario 2).
utility should then take priority over intra-cluster utility, i.e. Indeed, the surface properties play an important role on
λtc  λic . On the other hand, the pedestal of the statue the reconstruction, and a planner favoring fast local updates
excluded, Scenario 2 mainly consists of round surfaces and tends to be more successful in small environments containing
the average distance between two clusters is much smaller close occluded areas, sharp edges and fine details, where
than in Scenario 1. Similar penalty terms should be then many ISEs can be revealed after a scan. On the other
selected this time (i.e. λtc ' λic , see Table I). hand, over a long horizon, a planner is more effective
4) Single UAV: The method in [1] and the algorithms at finding the shortest path in a large planar environment
described in [3], [20], exhibit similar completion times for where each viewpoint covers less ISEs. By comparing the
Scenario 1. The deviation is more pronounced with the completion times for Scenario 2, we can notice that TSGA
Statue of Liberty: in fact, the algorithm in [20] takes twice as is much faster than NNB, but that the UAVs need to travel
long to finish the exploration. However, the trajectory of the long distances before completing the same number of ISEs.
quadrotor UAV is longer (twice as much, in Scenario 1), and Nevertheless, NNB performs many more scans which are
more jagged with the planner in [1]. This is not surprising, close to each other, and long paths are computed to complete
since the viewpoint configurations have been generated for the reconstruction (see Fig. 5 [bottom left]).
accurate 3D reconstruction and not for navigation purposes Our method guarantees that all the regions that are acces-
as in [3]. Noisy depth measurements have a negligible effect sible to the UAVs are covered. Moreover, in keeping with
on the trajectory of the UAV and on the completion time, the recent qualitative analysis for single-robot exploration
but a degradation of the reconstruction and coverage quality in [14], it turns out to be competitive with the state-of-the-art
can be observed. approaches in terms of overall 3D reconstruction quality. In
5) Multi-UAV vs. single-UAV: The use of multiple UAVs fact, the quality of 3D reconstruction is resolution-dependent:
has a beneficial effect on the completion time. In particular, in fact, it is inversely proportional to the size of the TSDF
the TSGA planner significantly reduces it, as shown in voxels. A small resolution amounts to a large number of
Table II. In Scenario 1, the fleet of three (five) UAVs voxels to be integrated in the TSDF map, which is a resource-
performs the 3D reconstruction 68.8% (80.8%) faster than intensive process. Therefore, if multiple UAVs explore a
a single UAV. With Scenario 2 we had a similar outcome, large environment using on-board sensing and processing, a
the gain on the completion time being of 72.2% (81.8%). trade-off between reconstruction accuracy and computational
On the other hand, the cumulated path length of the fleet is efficiency should be found.
bigger than that of a single UAV in both scenarios, and the
VI. C ONCLUSIONS AND FUTURE WORK
number of quadrotors has little impact on the reconstruction
quality and completeness. In this paper, we have presented a new Next-Best-View
6) TSGA vs. NNB planner: Overall, the TSGA planner planning algorithm for online surface reconstruction of large-
is more effective than the NNB planner for multi-robot scale environments with a fleet of aerial robots. In particular,
navigation (see the comparative results reported in Table III). a novel cluster-based 3D reconstruction gain and cost-utility
TSGA outperforms NNB in terms of cumulated path length formulation, and a local TSP-greedy allocation planner have
been proposed. Realistic numerical experiments in ROS-
Gazebo, have successfully validated the proposed strategy
TABLE III in two challenging outdoor environments. A significant re-
C OMPARISON BETWEEN THE TSGA AND NBB PLANNER . duction of completion time has been observed with respect
to the single-UAV case and a baseline multi-robot path
Scenario 1 Scenario 2
Number of UAVs 3 5 3 5 planner (NNB).
Path length gain [%] 23.9 21 1.64 1
There are several promising directions for further research
Completion-time gain [%] 7.32 7.06 21.8 10 we would like to explore in the future. First of all, we plan to
test our method in the presence of localization uncertainty,
network communication delays and data dropout, and to [21] F. Amigoni and A. Gallo. A Multi-Objective Exploration Strategy
modify our centralized architecture (and notably our mapping for Mobile Robots. In Proc. IEEE Int. Conf. Robot. Automat., pages
3850–3855, 2005.
module) to make it amenable to a decentralized implemen- [22] W. Burgard, M. Moors, C. Stachniss, and F.E. Schneider. Coordinated
tation. We would also like to study the case of multiple het- multi-robot exploration. IEEE Trans. Robot., 21(3):376–386, 2005.
erogeneous robots (e.g. ground and aerial vehicles). Finally, [23] A. Mannucci, S. Nardi, and L. Pallottino. Autonomous 3D Exploration
of Large Areas: A Cooperative Frontier-Based Approach. In Proc. Int.
work is in progress to validate our approach on hardware Conf. Model. Simul. Auton. Systems, pages 18–39, 2017.
platforms in real-world environments. [24] B. Charrow, S. Liu, V. Kumar, and N. Michael. Information-theoretic
mapping using Cauchy-Schwarz Quadratic Mutual Information. In
R EFERENCES Proc. IEEE Int. Conf. Robot. Automat., pages 4791–4798, 2015.
[25] G. Best, O. Cliff, T. Patten, R. Mettu, and R. Fitch. Decentralised
[1] G. Hardouin, F. Morbidi, J. Moras, J. Marzat, and E. Mouaddib. Monte Carlo Tree Search for Active Perception. In Int. Work. Algor.
Surface-driven Next-Best-View planning for exploration of large-scale Found. Robot. (WAFR), 2016. Paper 50.
3D environments. In Proc. 21st IFAC World Congress, 2020, to appear. [26] N. Atanasov, J. Le Ny, K. Daniilidis, and G.J. Pappas. Decentralized
[2] W. Tabib, M. Corah, N. Michael, and R. Whittaker. Computationally active information acquisition: Theory and application to multi-robot
efficient information-theoretic exploration of pits and caves. In Proc. SLAM. In Proc. IEEE Int. Conf. Robot. Automat., pages 4775–4782,
IEEE/RSJ Int. Conf. Intel. Robots Syst., pages 3722–3727, 2016. 2015.
[3] S. Song and S. Jo. Surface-based Exploration for Autonomous 3D [27] M. Corah, C. O’Meadhra, K. Goel, and N. Michael. Communication-
Modeling. In Proc. IEEE Int. Conf. Robot. Automat., pages 4319– Efficient Planning and Mapping for Multi-Robot Exploration in Large
4326, 2018. Environments. IEEE Robot. Autonom. Lett., 4(2):1715–1721, 2019.
[4] A. Bircher, M. Kamel, K. Alexis, H. Oleynikova, and R. Siegwart. [28] M. Corah and N. Michael. Distributed matroid-constrained submod-
Receding horizon path planning for 3D exploration and surface ular maximization for multi-robot exploration: Theory and practice.
inspection. Auton. Robot., 42(2):291–306, 2018. Auton. Robot., 43(2):485–501, 2019.
[5] C. Connolly. The Determination of Next Best Views. In Proc. IEEE [29] G.M.J.-B. Chaslot. Monte-Carlo Tree Search. PhD thesis, Maastricht
Int. Conf. Robot. Automat., volume 2, pages 432–435, 1985. University, 2010.
[6] R. Pito. A Sensor-Based Solution to the “Next Best View” Problem. [30] M.L. Fisher, G.L. Nemhauser, and L.A. Wolsey. An Analysis of
In Proc. 13th Int. Conf. Pattern Recogn., volume 1, pages 941–945, Approximations for Maximizing Submodular Set Functions — II.
1996. Math. Program. Stud., 8:73–87, 1978.
[7] S. Kriegel, C. Rink, T. Bodenmüller, and M. Suppa. Efficient [31] G.L. Nemhauser, L.A. Wolsey, and M.A. Fisher. An Analysis of
next-best-scan planning for autonomous 3D surface reconstruction of Approximations for Maximizing Submodular Set Functions — I.
unknown objects. J. Real-Time Image Pr., 10(4):611–631, 2015. Math. Program., 14:265–294, 1978.
[8] A.P. Punnen. The Traveling Salesman Problem: Applications, For- [32] M. Kamel, T. Stastny, K. Alexis, and R. Siegwart. Model Predictive
mulations and Variations. In G. Gutin and A.P. Punnen, editors, The Control for Trajectory Tracking of Unmanned Aerial Vehicles Using
Traveling Salesman Problem and Its Variations, volume 12, pages 1– Robot Operating System. In A. Koubaa, editor, Robot Operating Sys-
28. Springer, 2007. tem (ROS) The Complete Reference, volume 2, pages 3–29. Springer,
[9] B. Curless and M. Levoy. A volumetric method for building complex 2017.
models from range images. In Proc. 23rd Annual Conf. Computer [33] C.V. Nguyen, S. Izadi, and D. Lovell. Modeling Kinect Sensor Noise
Graph. Interact. Tech., pages 303–312, 1996. for Improved 3D Reconstruction and Tracking. In 2nd IEEE Int. Conf.
[10] J.I. Vasquez-Gomez, L.E. Sucar, R. Murrieta-Cid, and E. Lopez- 3D Imaging, Model. Proc. Visual. & Transm., pages 524–530, 2012.
Damian. Volumetric Next-best-view Planning for 3D Object Recon- [34] H. Oleynikova, C. Lanegger, Z. Taylor, M. Pantic, A. Millane, R. Sieg-
struction with Positioning Error. Int. J. Adv. Robot. Syst., 11(10):159, wart, and J. Nieto. An open-source system for vision-based micro-
2014. aerial vehicle mapping, planning, and flight in cluttered environments.
[11] R. Border, J.D. Gammell, and P. Newman. Surface Edge Explorer J. Field Robot., 37(4):642–666, 2020.
(SEE): Planning Next Best Views Directly from 3D Observations. In [35] J.E. Bresenham. Algorithm for computer control of a digital plotter.
Proc. IEEE Int. Conf. Robot. Automat., pages 6116–6123, 2018. IBM Syst. J., 4(1):25–30, 1965.
[12] L. Yoder and S. Scherer. Autonomous Exploration for Infrastructure [36] C. Godsil and G. Royle. Algebraic Graph Theory. Springer, 2001.
Modeling with a Micro Aerial Vehicle. In D.S. Wettergreen and T.D. [37] H. González-Banos and J.-C. Latombe. Navigation strategies for
Barfoot, editors, Field and Service Robotics: Results of the 10th Int. exploring indoor environments. Int. J. Robot. Res., 21(10-11):829–
Conf., pages 427–440. Springer, 2016. 848, 2002.
[13] R. Monica and J. Aleotti. Contour-based next-best view planning [38] K. Helsgaun. An effective implementation of the Lin–Kernighan
from point cloud segmentation of unknown objects. Auton. Robot., traveling salesman heuristic. Eur. J. Oper. Rer., 126(1):106–130, 2000.
42(2):443–458, 2018. [39] S.T. Jawaid and S.L. Smith. Informative path planning as a maximum
[14] L.M. Schmid, M. Pantic, R. Khanna, L. Ott, R. Siegwart, and J. Nieto. traveling salesman problem with submodular rewards. Discrete Appl.
An Efficient Sampling-based Method for Online Informative Path Math., 186:112–127, 2015.
Planning in Unknown Environments. IEEE Robot. Autonom. Lett., [40] F. Furrer, M. Burri, M. Achtelik, and R. Siegwart. RotorS – A Modular
5(2):1500–1507, 2020. Gazebo MAV Simulator Framework. In A. Koubaa, editor, Robot
[15] A. Hornung, K.M. Wurm, M. Bennewitz, C. Stachniss, and W. Bur- Operating System (ROS): The Complete Reference, volume 1, pages
gard. OctoMap: An efficient probabilistic 3D mapping framework 595–625. Springer, 2016.
based on octrees. Auton. Robot., 34(3):189–206, 2013. [41] L. Keselman, J.I. Woodfill, A. Grunnet-Jepsen, and A. Bhowmik. Intel
[16] A. Bircher, M. Kamel, K. Alexis, H. Oleynikova, and R. Siegwart. RealSense Stereoscopic Depth Cameras. In Proc. IEEE Conf. Comp.
Receding Horizon “Next-Best-View” Planner for 3D Exploration. In Vis. Pattern Recogn. Workshops, pages 1–10, 2017.
Proc. IEEE Int. Conf. Robot. Automat., pages 1462–1468, 2016. [42] A. Zeng, S. Song, M. Nießner, M. Fisher, J. Xiao, and T. Funkhouser.
[17] C. Papachristos, M. Kamel, M. Popović, S. Khattak, A. Bircher, 3DMatch: Learning Local Geometric Descriptors from RGB-D Re-
H. Oleynikova, T. Dang, F. Mascarich, K. Alexis, and R. Siegwart. constructions. In Proc. IEEE Conf. Comp. Vis. Pattern Recogn., pages
Autonomous Exploration and Inspection Path Planning for Aerial 1802–1811, 2017.
Robots Using the Robot Operating System. In A. Koubaa, editor, [43] W.E. Lorensen and H.E. Cline. Marching cubes: A high resolution
Robot Operating System (ROS), pages 67–111. Springer, 2019. 3D surface construction algorithm. In Proc. ACM SIGGRAPH Comp.
[18] M. Selin, M. Tiger, D. Duberg, F. Heintz, and P. Jensfelt. Efficient Graph., volume 21, pages 163–169, 1987.
Autonomous Exploration Planning of Large-Scale 3-D Environments. [44] I.A. Şucan, M. Moll, and L.E. Kavraki. The Open Motion Planning
IEEE Robot. Autonom. Lett., 4(2):1699–1706, 2019. Library. IEEE Rob. Autom. Mag., 19(4):72–82, 2012.
[19] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal [45] D. Lague, N. Brodu, and J. Leroux. Accurate 3D comparison of
motion planning. Int. J. Robot. Res., 30(7):846–894, 2011. complex topography with terrestrial laser scanner: Application to the
[20] S. Song and S. Jo. Online Inspection Path Planning for Autonomous Rangitikei canyon (N-Z). ISPRS J. photogramm., 82:10–26, 2013.
3D Modeling using a Micro-Aerial Vehicle. In Proc. IEEE Int. Conf.
Robot. Automat., pages 6217–6224, 2017.