Overview obstacle maps for obstacle‐aware navigation of autonomous drones

Abstract Achieving the autonomous deployment of aerial robots in unknown outdoor environments using only onboard computation is a challenging task. In this study, we have developed a solution to demonstrate the feasibility of autonomously deploying drones in unknown outdoor environments, with the main capability of providing an obstacle map of the area of interest in a short period of time. We focus on use cases where no obstacle maps are available beforehand, for instance, in search and rescue scenarios, and on increasing the autonomy of drones in such situations. Our vision‐based mapping approach consists of two separate steps. First, the drone performs an overview flight at a safe altitude acquiring overlapping nadir images, while creating a high‐quality sparse map of the environment by using a state‐of‐the‐art photogrammetry method. Second, this map is georeferenced, densified by fitting a mesh model and converted into an Octomap obstacle map, which can be continuously updated while performing a task of interest near the ground or in the vicinity of objects. The generation of the overview obstacle map is performed in almost real time on the onboard computer of the drone, a map of size 100m×75m is created in ≈2.75min, therefore, with enough time remaining for the drone to execute other tasks inside the area of interest during the same flight. We evaluate quantitatively the accuracy of the acquired map and the characteristics of the planned trajectories. We further demonstrate experimentally the safe navigation of the drone in an area mapped with our proposed approach.

capabilities, setting up a fully automated flight in environments from which the operator has limited information, for instance, for search and rescue and disaster relief operations, is not feasible.

Early works on this topic motivated by the International Aerial
Robotics Competition (IARC) Missions 3 and 4 (AUVSI Association, 2018) running, respectively, on years 1998-2000 and 2001-2008 showed promising results using unmanned helicopters and computer vision. In Mission 3, the aerial robot had to detect and avoid obstacles, identify survivors, and recognize drum containers. The winning team from the Technical University of Berlin (Kondak & Remuß, 2007;Musial, Brandenburg, & Hommel, 2000) was able to perform the target identification and localization tasks; however, their helicopter did not fly near the debris, but rather flew high over the area (Greer, McKerrow, & Abrantes, 2002). In Mission 4, the aerial robot had to identify a particular building and deploy a rover to accomplish a task inside it. A team of the Georgia Institute of Technology won this challenge (Johnson, Mooney, & Christophersen, 2013;Rooz et al., 2009) by completing the entire mission. Working in topics that relate to our presented work, the same team also developed a helicopter system able to fly over an area and acquire an accurate three-dimensional (3D) reconstruction using a pan-tilt-mounted laser range finder (LADAR or LIDAR) and explored the 3D obstacle avoidance problem in simulation (Geyer & Johnson, 2006). A comparison of the 3D reconstructions obtained by performing an overview flight and acquiring and processing data from either a LIDAR or a camera is discussed on the work by Leberl et al. (2010). The IARC Mission 5 (2009) proposed the challenge of autonomously exploring an indoor area with tight spaces while searching for a target object on a wall. Mission 5 was fully accomplished using a quadrotor drone and a low-weight LIDAR and a stereo camera system as main sensors by a team from the Massachusetts Institute of Technology (MIT; Bachrach, Prentice, He, & Roy, 2011). The same authors demonstrated similar capabilities using only LIDAR and a smaller drone platform in their work (Bachrach, He, & Roy, 2009).
As these early works show, the fast deployment of autonomous drones in unknown outdoor environments is since several years an ongoing research problem. In this study, some of the main challenges related to this topic are tackled, namely the acquisition of a good-quality obstacle map and the calculation of trajectories that allow fast navigation in the area of interest. Our presented approach uses only onboard computation power, and as a result, the drone does not need to transfer data to a ground-station via a wireless communication link.
Our vision-based mapping approach consists of two separate steps.
First, the drone performs an overview flight at a safe altitude acquiring overlapping downward-looking images, while creating a high-quality map of the environment by using a state-of-the-art photogrammetry method, the online Structure from Motion (SfM) pipeline (Hoppe et al., 2012;Rumpler et al., 2016). Second, this map is georeferenced and converted into an Octomap, see Figure 1, which is used as an initial overview obstacle map that can be updated during the rest of the flight while performing a task of interest near the ground. The generation of the overview obstacle map is performed in a few minutes on the drone onboard computer, and thus, with enough time remaining for the drone ( Figure 2) to execute other tasks inside the area of interest during the same flight.
Our trajectory planning approach is designed to provide smooth trajectories away from obstacles. We have tested our navigation and trajectory planning algorithms, experimentally utilizing an obstacle map obtained using our mapping method. The aim of the experiment is to demonstrate the feasibility of autonomously deploying drones in unknown outdoor environments.
F I G U R E 1 Obstacle map of an outdoor environment of size 105 m × 75 m generated from 56 images, with 12 m high buildings and up to 14 m high trees, generated using our method explained in Section 3.1. The obstacle map is displayed color-coded according to the height and has a minimum voxel resolution of 1 m. On the left, "Google Earth ©2015," an overview image of the area is shown, where the target 50 m × 50 m region of interest is located around the parking lot and a red contour denotes the effectively mapped area. The onboard processing time for the creation of this obstacle map was ≈2.75 min. Man-made obstacles, such as buildings and cars, and big trees are quite well reconstructed and included in the obstacle map. However, small trees are often not correctly mapped and need to be sensed later on during lower altitude flight. For this purpose, our drone, see Figure 2, is equipped with several stereo-heads that can acquire point-clouds of trees along with unmapped and dynamic obstacles during flight, which can be used to update the map [Color figure can be viewed at wileyonlinelibrary.com] PESTANA ET AL.

| 735
The concept of overview obstacle maps and the presented solution for drone deployment were inspired by the objectives of the 2016 DJI Developer Challenge. 1 This competition consisted of a search and rescue mission, in which the drone needed to explore a designated area searching for survivors. Our team, the Graz Griffins, took part in this challenge and was among the few participants that qualified to participate in the finals, where we demonstrated our solution at work.
The outline of this study is the following. The related work and our contributions are discussed in Section 2. The algorithms utilized for the realization of this study are described in Sections 3 and 4: the mapping approach in Sections 3.1 and 3.2, the navigation control in Section 4.1, and our trajectory planning solution in Section 4.2. The experiments are described and discussed in Section 5 with: the evaluation of the trajectory planner in Section 5.2, a qualitative and quantitative evaluation of the capabilities of our overview obstacle maps in Section 5.3, and an experimental flight showcasing performance of our system in a map acquired using our proposed approach in Section 5.4. Sections 6 and 7 contain the conclusions and a discussion about possibilities for future work.

| STATE OF THE ART
Generating maps of areas of medium size in real-time onboard a drone or leveraging offboard computing resources is a challenging task, and much research has been dedicated to it with varying degrees of success.
A real-time loosely coupled visual-inertial odometry (VIO) framework, by Weiss, Achtelik, Lynen, Chli, and Siegwart (2012), was developed based on a modified version of parallel tracking and mapping (PTAM) (Klein & Murray, 2009) improved for onboard execution and on a computationally fast estimation algorithm used as a fall-back method fusing the inertial measurement unit (IMU) readings with optical flow, thus only requiring a minimal amount of feature correspondences in consecutive frames. Using this, efficient version of PTAM (Weiss, Achtelik, Kneip, Scaramuzza, & Siegwart, 2011) showed an effective terrain exploration technique for micro-aerial vehicles (MAVs) that generate, in real time in a ground-station, a textured 3D mesh by means of a Delaunay triangulation (Labatut, Pons, & Keriven, 2007), which supports the drone operator in understanding the MAV's environment.
Several research works have focused on the creation of maps that can be later reused by the drone to localize in real time during an autonomous flight. Surber, Teixeira, and Chli (2017) use the VIO algorithm open keyframe-based visual-inertial SLAM (OKVIS) (Leutenegger et al., 2013 to acquire a map of an area during a manual flight, and later reuse this map to reduce the UAV's dependency on global positioning system (GPS) and evaluated their system against ground-truth position data acquired with a Leica Total Station. Recently, researchers from the ETH Zürich have released a visual-inertial mapping framework to process and produce multisession maps (T. Schneider et al., 2018), which uses robust visual inertial odometry (ROVIO) (Bloesch, Omari, Hutter, & Siegwart, 2015) as the VIO front-end, and has been used to achieve autonomous drone flight (Burri, Oleynikova, Achtelik, & Siegwart, 2015). In Burri et al. (2015), the full bundle adjustment (BA) result and the obstacle map are generated after a manual flight and are later used in autonomous flights achieving precise indoor localization, navigation, and obstacle avoidance. The known state-of-the-art visual SLAM frameworks ORB-simultaneous localization and mapping (SLAM) (Mur-Artal, Montiel, & Tardós, 2015) and ORB-SLAM2 (Mur-Artal & Tardós, 2017) also provide the capability of reusing a map acquired during a previous session or experiment. In Qiu, Liu, and Shen (2017), the authors propose the usage of mesh models obtained using well-accepted off-line SfM algorithms (Triggs, McLauchlan, Hartley, & Fitzgibbon, 1999) to substitute the usage of GPS. To achieve the vision-based localization against the model, the authors propose an edge alignment scheme for the current image against a virtual image extracted from the model that is used as a reference or keyframe. A visual odometry framework (Schenk & Fraundorfer, 2017) using a similar algorithm for RGB-D sensors provides a better evaluation on the approach and produces estimates with a drift accumulation on par with state-of-the-art visual odometry (VO) methods.
Achieving dense mapping onboard a drone is very challenging due to the limited computational capabilities of their onboard computers.
Several methods have been proposed that are too computation intensive and require powerful graphics processing units (GPUs), but the achieved levels of detail, and thus, the high quality of their dense maps would be extremely desirable for navigating drones. Examples of such dense-reconstruction algorithms are the following: KinectFusion (Newcombe, Izadi, et al., 2011), ElasticFusion (Whelan, Leutenegger, Salas-Moreno, Glocker, & Davison, 2015, MonoFusion (Pradeep et al., 2013), and dense tracking and mapping (DTAM; Newcombe, Lovegrove, & Davison, 2011), or the similar approach by Stühmer, Gumhold, and Cremers (2010). There is, thus, interest in the community in developing algorithms with better computational efficiency and accuracy trade-off that could be executed onboard drones. Heng, Lee, Fraundorfer, and Pollefeys (2011) propose a F I G U R E 2 Hardware setup: DJI M100 drone, autopilot, GPS, gimbal camera, DJI Guidance system with five stereo camera heads, and a Nvidia Jetson TK1 onboard computer. GPS: global positioning system; GPU: graphics processing unit [Color figure can be viewed at wileyonlinelibrary.com] method to generate a real-time dense-reconstruction offboard while guiding the drone by means of onboard VO. A framework to assist a surveyor while acquiring an SfM data set was proposed by Hoppe et al. (2012), an offboard calculated color-coded mesh model is displayed in real time for the purpose of providing the surveyor with feedback about the local quality of the reconstruction. Wendel, Maurer, Graber, Pock, and Bischof (2012) utilize the drone onboard PTAM-based calculated poses in an offboard server to produce a life dense 3D reconstruction that is displayed in real time on a tablet. The dense monocular 3D reconstruction algorithm regularized monocular depth estimation (REMODE)  measures depth against a reference view and performs uncertaintydependent point-cloud smoothing achieving real-time execution using CUDA (https://en.wikipedia.org/wiki/CUDA) by combining an algorithm to generate dense point-clouds using patch-level or perpixel Bayesian depth estimation using a parametric model (Vogiatzis & Hernández, 2011) and the fast state-of-the-art visual odometry method semidirect visual odometry (SVO; . REMODE has since been utilized on data acquired with drones to generate dense depth maps in real time for various research projects: creating medium-sized maps in an offboard ground-station by streaming the acquired data (Faessler et al., 2016), the creation of dense maps onboard (Forster, Faessler, Fontana, Werlberger, & Scaramuzza, 2015) by restricting their size to a relatively small 2.5D fixed-size grid-map around the robot (Fankhauser, Bloesch, Gehring, Hutter, & Siegwart, 2014), and the feasibility of sharing the 2.5D map acquired by the drone to guide a ground robot. Regarding the latter and still using REMODE, a mobile robot plans and executes trajectories in rough terrain in a small area mapped by a drone (Delmerico, Mueggler, Nitsch, & Scaramuzza, 2017), by training a terrain classifier on-the-fly (Delmerico, Giusti, Mueggler, Gambardella, & Scaramuzza, 2016), and a legged robot and the drone achieve localization on the same global coordinate frame in Käslin et al. (2016). In Lynen et al. (2015), an efficient indexed nearest-neighbor search to achieve image-based relocalization on a prebuilt map is proposed, where the map is obtained using standard SfM techniques with its scale recovered using IMU data and the recursive nonlinear filtering approach OKVIS (Agarwal et al., 2012;Leutenegger et al., 2013Leutenegger et al., , 2015, and a VIO method for local pose tracking inspired on the multi-state constraint kalman filter (MSCKF) (Mourikis et al., 2009) is used. Using OKVIS (Leutenegger et al., 2013 as VIO front-end, the research work (Oleynikova, Burri, Lynen, & Siegwart, 2015) also proposes a method to localize a drone and a ground robot on the same map by means of a previously acquired reference map. Recently, a drone dense-reconstruction (Karrer, Kamel, Siegwart, & Chli, 2016) data set has been released, which focuses on small working areas and producing precise 3D dense models for the purpose of object manipulation, in which ground-truth position data acquired with a Leica Total Station are available.
The system developed by J. Schneider et al. (2016) creates a relatively dense georeferenced point-cloud of very high accuracy while localizing the drone in real time at 100 Hz using only onboard computation on a 3.6 GHz Intel CPU (Santa Clara, CA) with 4 cores.
Another possibility to create dense reconstructions is using VO semidense methods, which extract the depth of high-gradient regions of the scene, such as large-scale direct LSD-SLAM (Engel, Schöps, & Cremers, 2014), Direct Sparse Odometry (DSO; Engel, Koltun, & Cremers, 2018), or semidense mapping (SDM; . However, these methods are not well suited for this purpose because their depth estimates are not filtered or optimized for dense mapping. The direct tracking and mapping method dense piecewiseplanar tracking and mapping (DPPTAM; Concha & Civera, 2015) achieve good results by including piecewise-planar surfaces in the model, but the computation requirement is too high for direct onboard execution. The method by Teixeira and Chli (2016) is extremely fast but the produced mesh results include strong interpolations causing error in sharp-edges, such as corners. A later method from the same authors ) uses large-scale direct monocular SLAM (LSD-SLAM), super-pixels, and filtering that eliminates most depth outlier estimates, and it achieves very competitive runtimes on an Intel-i7 4700MQ/Intel-i7 5557U/Intel NUC processor (Intel Corporation (Intel), Santa Clara, CA) that can be mounted onboard drones.
The trajectory planner presented in this study was designed using methods from the state of the art to deliver long and smooth trajectories on our overview obstacle maps and it is presented as a component of the developed system. The reader is here directed to work in the field of fast trajectory replanning that would allow the drone to explore unknown cluttered environments while flying near the obstacles. These types of planners are able to regenerate an obstacle-free smooth trajectory at a high rate, for instance, the following recent works make computation efficiency improvements by using operations in an OcTree data structure (Chen, Liu, & Shen, 2016), a local multiresolution discretization (Nieuwenhuisen & Behnke, 2016), and local replanners (Oleynikova et al., 2016;Usenko, vonStumberg, Pangercic, & Cremers, 2017).
In this study, we propose a method to create an overview obstacle map of a desired outdoor area onboard the drone. The success of our approach is a direct consequence of utilizing a survey flight trajectory that provides an image data set of a large area with high and approximately constant image overlap resulting in a very well constrained BA problem. This choice reduces the size of the optimization problem, for which the associated Hessian matrix has a known structure (Triggs et al., 1999), and keeps it at an onboard computationally manageable size. The resulting sparse 3D model is meshed to generate an obstacle map by using a Delaunay triangulation (Labatut et al., 2007). Although our approach comes at the cost of only mapping the obstacles that are well represented by the sparse SfM model, consisting of 3D points and lines, our densification operation is computationally very efficient. When compared with related work, our approach presents several novelties.
In comparison to previous work that re-utilizes a map acquired on an earlier session for navigation (e.g., Burri et al., 2015;Qiu et al., 2017;T. Schneider et al., 2018), our solution allows the acquisition of a moderately sized obstacle map onboard the drone, which we demonstrate for maps of size 100 m × 75 m that are created in ≈ 2.75 min, allowing the drone PESTANA ET AL.

| 737
to perform a near-ground navigation task on the same flight. Because the creation of our sparse 3D model is incremental, the mapping operation can be stopped at any time resulting in a smaller mapped area and a shorter map creation time. Similarly, in comparison to the discussed onboard dense-reconstruction methods, our maps cover much bigger areas than the onboard solutions from the related work (e.g., Forster et al., 2015;. Accuracy evaluation of our obstacle map is performed to provide a basis for the comparison of our obstacle map to that of other methods. Here, it is noted that in some works (e.g., J. Schneider et al., 2016), the accuracy is evaluated based on the distance of the mapped points to the ground-truth point-cloud rather than the other way around, which is not as informative for the purpose of using the 3D reconstruction as an obstacle map.
Similarly to Weiss et al. (2011), we densify the sparse model representation into an obstacle map by fitting a mesh model by using a Delaunay triangulation (Labatut et al., 2007), which is computationally very efficient. In comparison to the work by Weiss et al. (2011), we use In a nutshell, the main contributions of this study are as follows: • We propose the concept of overview obstacle map generation for the fast deployment of drones in unknown outdoor environments.
A short survey flight provides the data for the vision-based incremental generation of the obstacle map onboard and leaves enough time to directly exploit the created map for near-ground navigation.
• An accurate** evaluation of the generated obstacle map is presented in Section 5.3.
• We demonstrate the potential of our solution by performing autonomous obstacle-free navigation on a map acquired using our proposed method.

| OVERVIEW OBSTACLE MAP GENERATION
The calculation of the overview map is performed using an SfM algorithm. This choice allows us to use a minimal set of sensors commonly available on drones: a GPS sensor, used for scaling and georeferenciation, and a standard camera. In comparison to visualinertial approaches, we forgo the intersensor calibration of the camera with respect to the IMU sensor and the time synchronization of the data from both sensors, which could be achieved, for instance, using the Kalibr open-source library from Furgale, Rehder, and Siegwart (2013).
Additionally, SfM algorithms do not require a high frame rate and work well with still images, so that they can be used without a navigation computer vision camera. These characteristics allow the use of our solution in a broad range of commercially available drones.

| Online SfM
The multirotor autonomously performs an overview flight at a safe high altitude over the region of interest. This region is defined by its GPS corner points. The drone takes off and ascends to a safe height, approaches the region of interest, and plans and executes a regular survey flight trajectory according to a desired image overlap setting.
The set of acquired overlapping nadir images is used to generate a sparse 3D map enhanced with line features on-the-fly. Simultaneously, a 3D mesh representing the surface model is fitted to the sparse model and at the end of the survey flight, it is rasterized to obtain the overview obstacle map.
The utilized online SfM pipeline, developed at our institute, 2 was first proposed by Hoppe et al. (2012), and it is very runtime efficient.
The task of the online SfM is to reconstruct the scene 3D points and simultaneously calculate the camera poses against the calculated sparse point-cloud. The pipeline is based on a precalibrated camera model that we obtained with the method of Daftry, Maurer, Wendel, and Bischof (2013) and utilizes scale invariant feature transform (SIFT) features (Lowe, 2004;Wu, 2007) to be able to handle imagery with large baselines. The sparse model is initialized from the first two images, for which a valid relative pose estimate can be computed by using the robust version of the five-point pose estimation algorithm (Nistér, 2004). Afterward, the absolute pose estimation method by Kneip, Scaramuzza, and Siegwart (2011) is used to align the incoming images to the current sparse 3D reconstruction in real time. Meanwhile, iterative bundle adjustment (Triggs et al., 1999) is performed in a parallel thread to prevent the scene drift likely to be caused by the incremental map building procedure. Using an incremental and real-time version of Line3D + + (Hofer, Maurer, & Bischof, 2017), a set of 3D lines is calculated from the aligned images. The sparse 3D model, thus, consists of a point-cloud, a set of line segments and the camera pose. The calculated set of 3D lines is sampled so as to add points and information for the calculation of the surface model. In addition, the lines enhance the interpretability of the visualization of the sparse 3D map, especially for man-made structures and line-rich regions.
Following the method (Rumpler et al., 2014, the GPS measurements and the camera poses of the sparse model are aligned by means of a robust random sample consensus (RANSAC)-based leastsquares minimization of the distance between both sets of locations. This effectively provides the correct scale, rotation, and relative translation for the sparse 3D map. A better georeferenciation could be obtained by placing georeferenced fiducial markers on site; however, in this study, we forgo the use of markers so as to maintain our approach suitable for search and rescue scenarios.

| Obstacle map representation
We use the Octomap (Hornung et al., 2013) obstacle map representation for this purpose, which is an OcTree-based volumetric map representation (Figure 1, right). Its implementation is opensource 3 and it is integrated to be easily used with the Robot Operating System (ROS) 4 . We selected it for various reasons: It is memory and runtime efficient and achieves real-time execution onboard and it can represent general-shaped obstacles. In our approach, the obstacle map is obtained from a single mesh model, for which Octomap is a good fit. In contrast, methods based on creating the surface model using Truncated Signed Distance Fields (TSDFs), such as Voxblox (Oleynikova, Taylor, Fehr, Siegwart, & Nieto, 2017), are better suited to be used with depth sensors, such as RGB-D and stereo cameras.
To accelerate the calculation of obstacle-free trajectories, we use a precalculated distance map that provides the clearance of any point in free space to its closest obstacle. An efficient implementation of such an algorithm for Octomap was developed by Lau, Sprunk, and Burgard (2013) and released open-source as a library named "DynamicEDT3D: A library for Incrementally Updatable Euclidean distance transforms in 3D' 5 . Its main advantages are featuring constant access-time, because its internal data structure storing the distance map is an array, and being capable of time-efficient incremental updates.
For the specific case of our drone, see Figure 2, the pointclouds provided by our stereo-heads are of low resolution (320 × 240 pixels). For this reason, during flight, we are able to apply fast incremental updates to both, the obstacle map and the distance map, by using their native Application Programming Interfaces (API). We have tested the runtime of this operation on data sets and for an Octomap with a minimum voxel resolution of 1 m, the updates can be applied onboard in real time, at a frequency higher than 1 Hz.

| NAVIGATION USING THE OVERVIEW OBSTACLE MAP
The navigation controller design and tuning is explained in Section 4.1. To achieve safe near-ground navigation in cluttered environments, we implemented a trajectory planner (see Section 4.2) that generates trajectories at a configurable clearance distance from obstacles.
F I G U R E 3 Successive steps undertaken to generate an obstacle map of size × 105 m 75 m, see explanation in Section 3.1: autonomous overview flight at a safe altitude over the × 50 m 50 m area of interest, generation of the sparse model using our incremental SfM pipeline, generating a dense surface model from the sparse one, conversion to Octomap by direct sampling of the surface model and successive updates during flight at low altitude by processing the stereo-head data streams. The Octomaps are displayed color-coded according to the height and have a minimum voxel resolution of 1 m [Color figure can be viewed at wileyonlinelibrary.com] 4.1 | Navigation control

| System identification
The flight behavior of our drone was characterized by performing speed command step response identification tests. Based on our experimental data and understanding of the system, the dynamical behavior from velocity command to the velocity output is assumed to be described by a transfer function where s is the Laplace variable and ( ) P s maps the Laplace transform , the speed command. The parameters of our model are: V , the static gain; T d , a pure delay; and T , the time constant.
A rough controller parameter tuning was calculated based on the resulting model and later experimentally improved.

| Feedforward control
The mathematical model for the dynamics from velocity command to real velocity can be used to determine a feedforward control action.
This action takes knowledge about the future development of the desired velocity into account and would thus, in the absence of errors, lead to the drone following the desired trajectory exactly. Because all axes are considered separately, the following section restricts itself to the x-axis; all other axes can be handled in the same way.
Assuming that the desired trajectory ( ) x t d is given by a smooth mathematical function. Then, the value of the desired position x d and , d , and so forth are known at each time instant t. For our dynamics model of the drone, the transfer function (1), the feedforward control command is that is, we need to look "into the future" by T d seconds and have knowledge about the desired acceleration and velocity. In the absence of modeling errors and flight disturbances, this command would lead to the drone following the desired trajectory exactly, that

| Feedback control
The feedforward control law alone does not guarantee that the drone will actually follow the trajectory in a real setting, even if the initial position matches the beginning of the trajectory exactly. We utilize a feedback loop controller, similar to the PID controller architecture, for the three linear coordinates and the yaw heading and utilizing both position and speed measurement feedback and references. The utilized measurement feedbacks are the position, ( ) x t , and velocity, ( ) x t , from the internal GPS + IMU fusion provided by the autopilot board. An example speed command, v c , for the autopilot over one of the coordinate axes iṡ( where K p and K d are the controller tuning parameters and is the reference trajectory.

| Overall control
In our experiments, the measurement feedback utilized by the controller are the position, ( ) x t , and velocity,˙( ) x t , from the GPS + IMU fusion provided by the autopilot board.
The reference smooth trajectory is calculated in two steps. First, a trajectory specified through waypoints, and accompanying speed and acceleration plans are obtained using the speed planner explained in Section 4.2. And, second, a third-order spline is fitted to the set of waypoints, times of passage, speeds, and accelerations.
The resulting spline is the reference smooth trajectory for the controller, specified in Equation (4)

| Trajectory planning
The purpose of our trajectory planner is to allow fast and safe navigation along long trajectories in cluttered environments. The calculated path should, therefore, be smooth and keep clear of obstacles. Whenever a new goal position is received, a new path is delivered to the controller. Similar to the approach proposed by Richter, Bry, and Roy (2013), we use the differential flatness of the quadrotor dynamics (Mellinger & Kumar, 2011) to plan a smooth trajectory in 3D position coordinates without directly considering the system dynamics, and perform the following calculations separately: obstacle-free path planning and subsequent generation of a smooth, continuous, and differentiable trajectory path.
Our method proceeds as follows ( Figure 4): 1. Calculate a path using a state-of-the-art trajectory planner that minimizes a cost function, which penalizes proximity to obstacles, unnecessary changes in altitude and length.
2. Limiting the increase on the path cost, the raw output path from the planning algorithm is consecutively shortened into a smooth trajectory.
3. Taking into account the path curvature and parameters that fix the maximum values for the velocity and acceleration, feasible time-of-passage over the waypoints, speed, and acceleration plans are calculated.
4. The resulting path, speed, acceleration, and timing information are used to fit a spline that is then used as trajectory reference and for the calculation of feedforward control commands by the navigation controller, see Section 4.1.
In the rest of this section, the intermediary steps of our trajectory planning approach are explained.

| Obstacle-free path planning
The search for an obstacle-free path is performed in 3D space without considering the attitude of the drone, which is set later by the acceleration plan based on the differential flatness property of the quadrotor dynamics (Mellinger & Kumar, 2011). The trajectory queries are from the current pose estimate, as a starting point, to the goal position.
A cost function is evaluated over candidate paths, see Equations (5)-(10), and is utilized to guide the search of the optimal path using a state-of-the-art trajectory planning method.
We have tested our approach using two different state-of-the-art Calculating the distance from multiple points of the trajectory to obstacles represents a computation bottleneck for any trajectory planning method. For this reason, we utilize the distance map library "DynamicEDT3D" proposed by Lau et al. (2013) in our implementation to accelerate the retrieval of the obstacle clearance, which is defined as the distance from a point to its closest obstacle, and for collision checking related calculations.
, and K l are tuning constants that set the relative strength of each cost contribution, d max is the maximum clearance distance at which the distance map is saturated, is the value of the clearance provided by the distance map at is the z component of the unitary vector along the path at x. In this study, the X and Y world axes are horizontal, and the Z-axis is vertical and pointing upward. To prevent the planning algorithm from providing trajectories that traverse through obstacles, the clearance term, ( ) c x clearance , introduces a cost of infinity when inside obstacles, The cost tuning constants are selected to achieve the following behavior. The trajectory is preferred to, in this order: not be unnecessarily near obstacles, not have unnecessary changes in altitude, and be as short as possible. The resulting trajectory is, rather than the global optimum, the feasible path of minimum cost, as defined by Equations (5)-(10), among those explored by the trajectory planning routine during a preset amount of time.

| Trajectory shortening and smoothing
The resulting raw path from the prior step usually presents sharp angles at many waypoints. Therefore, in this step, the path is modified by performing a sequence of obstacle-aware shortening and smoothing operations. The blind application of these operations would result in a path, which would pass too near obstacles, corresponding to a numerically high cost for the smoothed path in comparison to the raw path.
F I G U R E 4 Synthetic outdoor Octomap environment of size 50 m × 50 m × 48 m. A 43.61-m long planned trajectory obtained with our approach is shown. The path output of a state-of-the-art planning algorithm (red) is consecutively shortened (greenblue-orange-white), resulting in the smoothed trajectory shown in white, which is further used to calculate a speed plan (semitransparent lines) and fit a spline that is used by the controller as trajectory reference. From the initial planning through all consecutive shortening operations, the trajectory optimizes an obstacle-clearance metric, resulting in trajectories that provide a safe distance to nearby obstacles [Color figure can be viewed at wileyonlinelibrary.com] PESTANA ET AL.

| 741
To avoid this, some increase in the path cost is allowed, but it is constrained to a fraction of the raw path cost. In this manner, the above-mentioned sought qualities of the raw path are kept in the smoothed path. The shortening and the smoothing are the result of applying subsequent suboperations iteratively, for which a cost increase can be calculated individually. Therefore, the performed path simplifications are all cost-aware, that is, a shortening or smoothing suboperation is only accepted when its corresponding cost increase, measured by means of Equations (5)-(10), is below a threshold.
The performed suboperations are the following (Figure 4): 1. Reduce the number of vertices that are present in the current path: Interim waypoints are removed if the trajectory is still collision-free and until the total cost does not increase more than 10%.
2. Collapse waypoints that are too near each other, an overall allowed cost increase of 10%.
3. Shortcut the path, an allowed cost increase of 10%: Not only waypoints are considered for the path length reduction, but also inner points of the path segments.

4.
Smooth the path using the B-spline algorithm with an allowed cost increase of 15%: New waypoints are sampled making the path rounder around sharp corners.

5.
New waypoints are sampled in the current trajectory segments, and others are reduced to achieve segment lengths inside an acceptable predefined range, an allowed cost increase of 10%.
The resulting smooth path sets the final position coordinates for all the waypoints. In the next step, only the dynamic information of the trajectory, for example, speed and times of passage, is calculated.

| Speed, acceleration, and time-of-passage planning
We are interested in being able to explicitly set maximum speed and acceleration constraints. Our approach achieves this by taking inspiration in the work of Hoffmann, Waslander, and Tomlin (2008) and making improvements to it, so as to produce a smoother speed plan, for example, with more continuous acceleration derivatives.
The main configuration parameters of the algorithm consist of the maximum upward, downward, and horizontal velocities and    (11) and (12), by using the Gauss-Newton least-squares minimization (Triggs et al., 1999), note that 1 and N are the indexes of the initial and end velocities: where 2 and 2 4 .
In these equations, i. a i at, , along-track acceleration at waypoint i, ii. T i at, , derivative of the along-track acceleration at waypoint i, The similarity of the smoothing cost function to the one minimized by a smoothing spline is shown in Equation (11), see related chapter of the book (James et al., 2013). The expression is first developed using the intermediary variables a i at, and T i at, resulting in Equation (12), and for the optimization, it is further developed to depend only on the speed plan using Equations (13) and (14). The data term, the old velocity plan v old ,  (11) and (12).
at, 1 at, Using this notation, the increment to the speed plan, Δv c , at each iteration is calculated from the following set of equations where J is the Jacobian matrix of Δz. where v new max is defined for every component as the minimum  This fact allows the calculation of a speed plan with an acceleration derivative bounded by a specific value. In practice, to obtain efficient computation times, the smoothing spline optimization is repeated a fixed number of times.
, , plan old plan

| Geometric speed planner
In Section 4.2, we have described a planning approach, which divides the trajectory generation in separate three subproblems: obstacle-free path planning, trajectory shortening, and smoothing; and speed, acceleration, and time-of-passage planning. The presented speed planning approach can also be used when the drone flies in obstacle-free areas over paths specified by

| EXPERIMENTAL RESULTS
We have performed three types of experiments for the evaluation of our solution, the combination of the mapping procedure, see Sections 3.1 and 3.2, and our navigation approach, see

| Experimental platform
The drone shown in Figure 2 was used in our experiments. It is equipped with a powerful onboard computer and several sensors. Its main equipment is: • DJI M100 drone (SZ DJI Technology Co., Ltd. (DJI), Shenzhen, Guangdong, China), which, with a TB47D battery and equipped as described, features a takeoff weight of 3.5 kg and achieves a flight time of approximately 12 min.
• Nvidia Jetson TK1 (DJI Manifold, Nvidia Corporation (Nvidia), Santa Clara, CA) onboard computer that features a quad-core processor, 2 GB of RAM and a CUDA-enabled Tegra chip.
• DJI Guidance visual-sensing system with five stereo-heads simultaneously provides images from all five directions and point-clouds from two directions at 320 × 240 pixels at 10 Hz.

| Evaluation of the trajectory planning approach
The evaluation of the trajectory planner is performed in a synthetic obstacle map ( Figure 5) that represents an industrial environment of size × × 50 50 48m, which is distributed as part of the GitHub repository of the RotorS simulation framework (Burri, 2015;Furrer, Burri, Achtelik, & Siegwart, 2016). Using this environment, we showcase the capabilities of our trajectory planner to generate smooth trajectories around and away from obstacles.
The configuration of the trajectory planner in this section is as follows. The PRM* and the RRT* planners are configured with the default parameters from the OMPL library, except for the maximum distance limit of a new vertex to the current tree in the RRT* algorithm, which is deactivated. The obstacle-free trajectory calculation was configured with a maximum distance d max of 6 m for the distance map and a maximum planning time of 1 s. This particular choice for the maximum planning time is based on the fact that the overview obstacle map contains abundant free space above ground. In most cases, the F I G U R E 9 Example trajectories from the evaluation of the planner described in Section 5.2, color coding of the trajectories as explained in Figure 4 [Color figure can be viewed at wileyonlinelibrary.com] T A B L E 1 Trajectory and speed planning performance benchmark using the RRT* for the obstacle-free path calculation

| Comparison of speed and acceleration plans with and without velocity smoothing
In this section, we compare our speed planning method, see Section 4.2.3, to the one proposed by Hoffmann et al. (2008), from which our method was inspired. The result of running both methods in two trajectories is shown in Figures 6 and 7.
The following information is shown in these figures: (top) visualization of the planned trajectory with the Octomap displayed color-coded for altitude from low (pink) to high (blue, green, and red).
The ( F I G U R E 1 3 Detail of smaller elements that are well reconstructed and included into the georeferenced overview obstacle map: four cars from scene1; a pylon with solar panels and a structure from scene2; and the hut, debris, and a car from scene3 [Color figure can be viewed at wileyonlinelibrary.com] F I G U R E 1 4 Quantitative accuracy evaluation of our mapping method against dense point-clouds obtained using Pix4D. The top image corresponds to scene1, the middle to scene2, and the bottom to scene3. The reconstruction error is calculated as the distance between each dense point and the onboard calculated mesh model. The dense point-clouds are color-coded according to the reconstruction error, where the color-to-distance correspondence** is shown in the histograms. The reconstruction error distribution is shown graphically by the histograms in the figure and numerically in Table 5 [Color figure can be viewed at wileyonlinelibrary.com]

| Performance benchmarking of the trajectory planner
The capabilities of our trajectory planning approach are evaluated by testing it on nine different queries repeatedly on the synthetic obstacle map, as shown in Figure 9. Each query corresponds to a given initial and target point. This evaluation was executed directly on the drone's onboard computer, a Nvidia Jetson TK1 development board. In order for the repeated evaluation not to be dependent on the internal state of the planner, which is relevant when using the PRM* algorithm, the planner is reinitialized after querying the planner for the nine trajectories. This process was repeated 100 times using the PRM* and the RRT* algorithms, resulting on the performance statistics shown in Tables 1-4.
The calculated performance parameters for the trajectories and speed plans are shown in Tables 1 and 2, and for the execution times in Tables 3 and 4. Each row of the tables shows the following performance statistics for the corresponding trajectory query: regarding the path length, the direct distance from initial to target point and the average trajectory length with its corresponding σ 3 uncertainty; regarding the path clearance, the mean of the minimum clearance, the minimum overall clearance for this query, and the resulting mean path clearance with its corresponding σ 3 uncertainty; regarding the planned velocity, the mean traversal velocity with its corresponding σ 3 uncertainty, the mean maximum velocity, and the maximum overall velocity for this query; regarding the traversal time, its mean for each query and the relative increase of the traversal time required by the speed plan smoothing, which effectively compares our speed plan with that resulting from the approach by Hoffmann et al. (2008); and regarding the acceleration derivative, the mean of its maximum value and its overall maximum value for each query are shown. The execution times for each main subpart of our trajectory planning approach are shown in Tables 3 and 4, where the speed plan calculation time for the algorithm by Hoffmann et al. (2008) is also shown, along with the resulting average number of waypoints for each query.
The results, see Tables 1 and 2, show that for queries outside buildings, with enough free space for the path, both obstacle-free planning algorithms, PRM* and RRT*, provide similar performance and result also in comparable metrics for the speed plan. The biggest differences are to be expected in trajectory queries that require navigation through narrow spaces, such as 6, 7, and 8, see  Note. This data corresponds to the histograms from Figure 14. The reconstruction error is calculated as the distance between the dense points from the Pix4D reconstructions, displayed in Figure

| 753
Overall, the planner always provided a feasible trajectory for all the executed queries and, except for the cases explained, the paths showcase a very good clearance and are reasonably distant from obstacles.
Regarding the execution times on the drone's onboard computer, see Tables 3 and 4 as discussed in Section 5.2.1, in speed plans that are unfeasible for high velocities. We favor, therefore, the use of our approach, because overall, these execution times are good for our target application in this research study.

| Evaluation of the overview obstacle map
In this section, we evaluate quantitatively the quality of the overview obstacle maps obtained using our method, described in Section 3.1, when executed during flight onboard the drone. To acquire these maps, the drone flew autonomously a regular survey flight trajectory. In the context of the qualities of the three reconstructions, the following elements are usually well represented in the map: • Some example elements are shown zoomed in Figure 13.
• Scene1- Figure 10: big buildings, cars, man-made objects with visually distinguishable lines, road edges, and, to a certain extent, grass and part of the trees.
• Scene2- Figure 11: a building, man-made structures with visually distinguishable lines, road edges, and grass.
• Scene3- Figure 12: woodland area, human-made debris, cars, and, to a certain extent, grass, and part of the trees. We assess the accuracy of our onboard mesh models against a dense point-cloud reconstruction that was obtained using the photogrammetry software Pix4D 6 . To perform the accuracy assessment calculation, the point-cloud was first registered to the mesh model using the Iterative Closes Point (ICP) algorithm, second cropped along the borders, because the area of interest is in the middle of the model, and third the distance of each dense point to the mesh was calculated, which is used as estimate of the reconstruction error. In this calculation, the error was saturated to 2 m so that its distribution, shown in the histograms, can be better appreciated. The registration of the dense point-cloud to the mesh model is performed to extract the slight differences in the georeferenciation results, which would otherwise affect our accuracy assessment. The reconstruction error between these high-quality point-clouds and their corresponding onboard calculated mesh models is shown graphically in Figure 14 and numerically in • Vegetation, branches, and foliage. For example, in particular trees.
• Dark and untextured areas, for example, asphalt or façades in shadows, are not reconstructed properly when using pointbased SfM. The subsequent Delaunay triangulation (Labatut et al., 2007), therefore, tends to close these nonreconstructed areas with big triangles, for example, see the building near the border in scene2.
For the purpose of navigation, our current solution to this issue, discussed in Section 3.2, is to update the Octomap obstacle map onthe-fly by fusing the depth maps from the stereo-heads.

| Evaluation of autonomous navigation
For the experimental evaluation of our navigation architecture, 1.47 m s 2 , respectively. Otherwise, the planner is configured as described in Section 5.2.
We utilize the drone equipped as explained in Section 5. A side benefit of georeferencing the obstacle maps is that we are able to perform geofencing and to set no-fly areas specifying only their GPS corner points, so that the trajectory planner regards them as obstacles. This feature was evaluated by marking a parking area as a no-fly zone, see Figure 16.
Our navigation results are summarized by the performance parameters shown in Table 6. The drone navigated safely in the

| CONCLUSIONS
In this study, we proposed a vision-based method for a drone to generate onboard on-the-fly an overview obstacle map that is immediately available for navigation tasks. Flying at the overview altitude the drone is able on its own to map a GPS-defined region of interest in a short period of time. In this process, the reconstruction of man-made objects and infrastructure is enhanced by exploiting 3D lines. The actual size and level of detail of the acquired map depends on the utilized camera, lens, and flight altitude. Based on the  Our experiments demonstrate the capabilities for an autonomous drone to acquire the obstacle map of a moderately sized area using a vision-based method and, by using the acquired map, to immediately perform near-ground obstacle-free navigation in that area. The overview obstacle map is up-to-date and through it we can generate paths away from obstacles. In contrast to pure reactive obstacle avoidance or to trajectory planning with an outdated potentially invalid map, the upto-date overview map should enable an overall increased mission execution efficiency. The reason for this is that the availability of the overview map during an autonomous mission allows the robot to focus on direct task objectives rather than on exploration. Although onboard mapping approaches will continue to be improved, our proposed visionbased approach represents a step forward in the fast deployment of autonomous drones in unknown outdoor environments.

| FUTURE WORK
There are still regions of the scene, which are challenging to reconstruct using point-based SfM. A possible approach to deal with them is to densify the point-cloud, for example, by using patch-based multi-view stereo (PMVS) (Furukawa & Ponce, 2010), SURE (Rothermel, Wenzel, Fritsch, & Haala, 2012), PlaneSweepLib (PSL; (Häne, Heng, Lee, Sizov, & Pollefeys 2014), or the work by Shekhovtsov, Reinbacher, Graber, and Pock (2016). Often times, the usage of these algorithms requires more computation power than available on current onboard computers of drones, which motivates the development of faster algorithms for the map densification.
Another approach is the utilization of view-planning methods (Roberts et al., 2017;Schmid et al., 2012), for instance, the approach (Mostegel, Rumpler, Fraundorfer, & Bischof, 2016) results in a set of images adapted to the challenging elements present in the scene, for example, by providing short baseline images for vegetation.
In our approach, the drone is localized in the overview obstacle map by using GPS + IMU fusion. This poses a problem for collision avoidance because a typical GPS + IMU state estimate can drift a few meters, particularly for longer flight times and during navigation close to the ground and among buildings. The localization precision against the overview map can be further improved by additionally fusing visionbased localization methods. However, feature-based matching between images acquired at a large and a close distance is challenging and thus provides options for future research. An enhanced relocalization capability would also provide improvements for collaborative mapping, map reuse, and map sharing between different robots and devices.
Next to our method, maps are also generated using SLAM approaches that combine a VO or VIO front-end with a bundle adjustment back-end (e.g., Forster, Lynen, Kneip, & Scaramuzza, 2013Schmuck & Chli, 2017;T. Schneider et al., 2018). In the context of overview obstacle maps, a detailed benchmark comparison of the mapping accuracy and processing time requirements of our SfM-based method against those algorithms would be of interest.
The experimental focus on this study was performed to support the main contribution in the mapping task. The further benchmarking of the trajectory planner in experiments and simulation, for instance, based on the benchmarks (Mettler, Kong, Goerzen, & Whalley, 2010;Nous, Meertens, Wagter, & de Croon, 2016), is left as future work.

ACKNOWLEDGMENTS
This study has been supported by the Austrian Science Fund (FWF)