I Introduction
As the development of autonomy in aerial robots, Micro Aerial Vehicle (MAV) has been more and more involved in our daily life. Among all applications emerged in recent years, quadrotor teachandrepeat has shown significant potentials in aerial videography, industrial inspection, and humanrobot interaction. In this paper, we investigate and answer the problem of what is the best way to incorporate a human’s intention in autonomous and aggressive flight, and what is a flexible, robust and complete aerial teachandrepeat system.
There is a massive market for consumer drones nowadays. However, we observe that most of the operators of consumer drones are not professional pilots and would struggle in generating their ideal trajectory for a long time. In some scenarios, such as the drone racing or aerial filming, a beginnerlevel pilot is impossible to control the drone to finish the race safely or take an aerial video smoothly unless months of training. Also, there is considerable demand in applying drones to repetitive industrial inspections or searchandrescue missions, where human provides a preferable routine. In these situations, demonstrating a desirable trajectory and letting the drone to repeat it is a common wish. However, the taught trajectory generated by an unskilled pilot is usually incredibly hard or dynamically infeasible to repeat, especially in some cluttered environments. Moreover, most of the visionbased teachandrepeat applications [1], [2], [3], such as our previous work [1], are sensitive to changing environments. In [1], even the environment changes very slightly, the global map has to be rebuilt, and the teaching has to be redone.
Based on these observations, in this paper, instead of asking the drone to follow the humanpiloted trajectory exactly, we only require the human operator to provide a rough trajectory with an expected topological structure. Such a human’s teaching trajectory can be arbitrarily slow or jerky, but it captures the rough route the drone is expected to fly. Our system then autonomously converts this poor teaching trajectory to a topological equivalent and energytime efficient one with an expected aggressiveness. Moreover, during the repeating flight, our system locally observes environmental changes and replans slidingwindowed safe trajectories to avoid unmapped or moving obstacles. In this way, our system can deal with changing environments. Our proposed system extends the classical robotics teachandrepeat framework and is named as teachrepeatreplan. It is complete, flexible, and robust.
In our proposed system, the surrounding environment is reconstructed by onboard sensors. Then the user’s demonstrated trajectory is recorded by virtually controlling the drone in the map with a joystick or remote controller. Afterward, we find a flight corridor that preserves the topological structure of the teaching trajectory. The global planning is decoupled as spatial and temporal planning subproblems. Having the flight corridor, an energyoptimal spatial trajectory which is guaranteed to be safe, and a timeoptimal temporal trajectory which is guaranteed to be physically feasible, are iteratively generated. In repeating, while the quadrotor is tracking the global spatialtemporal trajectory, a computationally efficient local map [4] is fused onboard by stereo cameras. Based on local observations, our proposed system uses a slidingwindow fast replanning method [5] to avoid possible collisions. The replanning module utilizes gradient information to locally wrap the global trajectory to generate safe and kinodynamic feasible local plans against unmapped or moving obstacles.
The concept of generating optimal topologyequivalent trajectories for quadrotor teachandrepeat was first proposed in our previous research [1]. In [1], once the repeating trajectory is generated, the drone executes it without any other considerations. In that work [1], the environment must remain intact during the repeating, and the localization of the drone is assumed to be perfect. These requirements are certainly not guaranteed in practice, therefore, prevent the system from being applied widely. In this paper, we extend the framework of the classical teachandrepeat and propose several new contributions to make our system complete, robust, and flexible. Contributions of this paper are listed as:

We advance our flight corridor generation method. The flight corridor we use now provides much more optimization freedom compared to our previous work [1]. The improvement of the flight corridor facilitates the generation of more efficient and smooth global trajectories. Moreover, we propose methods to accelerate the corridor generation on both CPU and GPU.

We present a whole set of experiments and comparisons in various scenarios to validate our system.

We release all components in the system as opensource packages, which include local/global planning, perception, and localization, and onboard controller.
In what follows, we discuss related literature in Sect. II and introduce our system in Sect. III. Our methods for finding a flight corridor consisting of large convex polyhedrons, and spatialtemporal trajectory optimization are detailed in Sect. IV and Sect. V, respectively. The local replanning is introduced in Sect. VI. Experimental and benchmarked results are given in Sect. VII. The paper is concluded in Sect. VIII.
Ii Related Works
Robotics teachandrepeat: Many robotics teachandrepeat works, especially for mobile robots, have been published in recent years. Most of them focus on improving the accuracy or robustness in repeating/following the path by operators, which is fundamentally different from our motivation. A lidarbased teachandrepeat system is proposed in [6], where laser scans are used to localize the ground vehicle against its taught path driven by the user. Furgale et al. [7] [8] also develop a lidarbased ground robot, which is specially designed for repeating longterm motions in highly dynamic environments. This system equips a local motion planner which samples local trajectory to avoid dynamic elements during route following. A map maintenance module is used to identify moving objects and estimate their velocities. An iterative learning controller is proposed in [9], to reduce the tracking error during the repeating of the robot. This controller can compensate disturbances such as unmodelled terrains and environmental changes by learning a feedforward control policy. Visionbased teachingandrepeat systems are also proposed in several works, such as the visual localization used by the rover in [3]. In this work, the authors build a manifold map during the teaching and then use it for localization in the repeating, In [10], a multiexperience localization algorithm is proposed to address the issue of environmental changes. The ground robot is localized robustly against several past experiences. In [11] and [12], to further improve the accuracy and robustness in localization, illumination and terrain appearances are considered in their proposed visual navigation system used for teachandrepeat, Compared to ground teachandrepeat works, research on aerial teachandrepeat is few. In [2], a visionbased drone is used to inspect infrastructure repetitively. In the teaching phase, the desired trajectory is demonstrated by the operator, and some keyframes in the visual SLAM are recorded as checkpoints. While repeating, local trajectories are generated to connect those checkpoints by using the minimumsnap polynomials [13]. To function properly, in this work, the teaching trajectory itself must be smooth, and the environments must have no changes during the whole repeating. In contrast, our proposed method can convert an arbitrarily poor path to a safe and efficient trajectory with expected flying aggressiveness. Also, our system is flexible. Since it records the teaching path by virtually controlling the drone in simulation, a manually piloted teaching process is not necessary. Finally, our proposed system is robust to environmental changing and can even avoid moving obstacles.
Quadrotor trajectory planning: Trajectory optimization is essential in generating safe and executable repeating trajectories from poor teaching ones. The minimumsnap trajectory optimization is proposed by Mellinger [13], where piecewise polynomials are used to represent the quadrotor trajectory and are optimized by quadratic programs (QP). A method for solving a closedform solution of the minimum snap is proposed in [14]. In this work, a safe geometric path is firstly found to guide the generation of the trajectory. By adding intermediate waypoints to the path iteratively, a safe trajectory is finally generated after solving the minimumsnap problem several times. Our previous works [15] [16] [17] carve a flight corridor consisting of simple convex shapes (sphere, cube) in a complex environment. The flight corridor constructed by a series of axisaligned cubes or spheres can be extracted very fast on occupancy map or Kdtree. Then we use the flight corridor and physical limits to constrain a piecewise Bézier curve, to generate a guaranteed safe and kinodynamic feasible trajectory, Other works are proposed to find general convex polyhedrons for constraining the trajectory. In [18], a piecewise linear path is used to guide and initialize the polyhedron generation. In [19], by assuming all obstacles are convex, SDP and QP are iteratively solved to find the maximum polyhedron seeded at a random coordinate in 3D space. Gradient information in maps is also valuable for local trajectory optimization. In CHOMP [20], the trajectory optimization problem is formulated as a nonlinear optimization over the penalty of safety and smoothness. In [21], [22] and [23], gradientbased methods are combined with piecewise polynomials for local planning of quadrotors. In this paper, we also utilize gradientbased optimization for local replanning.
Time optimization or socalled time parametrization is used to optimize the time profile of a trajectory, given the physical limits of a robot. Methods can be divided as direct methods [24] and indirect methods[25]. Direct methods generate an optimal spatialtemporal trajectory directly in the configuration space. For indirect methods, a trajectory independent of time is firstly generated, the relationship between time and the trajectory is optimized by an additional optimization process. The method in [26] finds a mapping function between the time and the trajectory, which is done by recursively adding key points into the function, and squeeze out the infeasibility of the time profile. This method obtains an optimal local solution and is computationally expensive. [25] also proposes a mapping function, which maps time to a virtual parametrization of the trajectory. The mapping function is then optimized under a complicated nonlinear formulation. However, the global optimality is not guaranteed, and a feasible initial solution is necessary to bootstrap the optimization. Convex optimization [27] and numerical integration [28] are two typical methods of robotics time optimal path parametrization (TOPP) problem. Although numerical integration [28] [29] has shown superior performance in computing efficiency, convex optimization [27] has the advantage of adding regularization terms other than total time into its objective function. This specialty suits well for our application where the user defines the expected aggressiveness of the drone, and sometimes the drone may not be expected to fly as fast as possible. As for the efficiency, since we do temporal optimization offline before the repeating, computing time is not critical.
Iii System Overview
Iiia System Architecture
The overall software and hardware architecture of our quadrotor system are shown in Fig. 2 and 3. The global mapping, flight corridor generator, and global spatialtemporal planner are done on an offboard computer. Other online processings are running onboard on the drone during the flight. Before teaching, the global map is built by onboard sensors. During teaching, a flight corridor is generated by inflating the teaching trajectory. Then the spatial and temporal trajectories are optimized iteratively within the flight corridor under a coordinate descent scheme [30]. The local planner using gradientbased optimization is running onboard to avoid unexpected obstacles observed in the repeating flights. For trajectory tracking, we use a geometric controller [31]. And the attitude is stabilized by the autopilot.
IiiB Globally Consistent Localization and Mapping
We use VINS [32], a robust visualinertial odometry (VIO) framework, to localize the drone. Moreover, the loop closure detection and global pose graph optimization are used in our system, to globally correct the pose estimation. The global mapping is done by fusing depth measurements from the stereo cameras with the pose estimation. By using our previous research on deformable map [33], our global mapping module maintains a series of submaps with each attached to a keyframe. In this way, the map is attached to the pose graph and is therefore globally driftless. During the mapping, when a loop closure is detected, keyframes in the global pose graph are corrected, and all submaps are deformed accordingly. The global pose graph optimization is also activated during the repeating. When loop closure is detected, the pose of the drone is corrected accordingly to eliminate the drift.
IiiC Global SpatialTemporal Planning
For an extremely poor teaching trajectory, both the geometric shape and time profile of it is far from optimal and therefore useless, or even harmful for conducting optimization. However, the topological information of the teaching trajectory is essential since it reflects the human’s intention. To preserve the topological information, we group the free space around the teaching trajectory to form a flight corridor (Sect. IV). The corridor contains the teaching trajectory within it, shares the same topological structure, and provides large freedom for optimization. It’s hard to concurrently optimize a trajectory spatially and temporally in the flight corridor. However, generating a safe spatial trajectory given a fixed time allocation (Sect. VA) and optimizing the time profile of a fixed spatial trajectory (Sect. VB) are both conquerable. Therefore, we iteratively optimize the trajectory in the spacetime joint solution space by designing a coordinate descent [30] framework. An objective with weighting energy and time duration is defined for optimization. We firstly generate a spatial trajectory whose energy is minimized, then we use the temporal optimization to obtain the optimal time profile of it. The optimal time profile is used to parametrize a trajectory again for spatial optimization. The spatialtemporal optimizations are done iteratively until the total cost cannot be reduced any more.
IiiD Local Collision Avoidance
In practice, the accumulated drift of VIO is unavoidable, and the recall rate of loop closure is unstable. Although we have built a dense global map, when the drift is significant and not corrected by loop detection in time, the quadrotor may have collisions with obstacles. Moreover, the environment may change or contain moving obstacles. Our previous work [1] has to rebuild the map when changes happen and can not deal with dynamic obstacles. To resolve the above issues, we integrate our previous local map fusion module [4] into our system to detect collisions locally and serves the local trajectory optimization. Also, we propose a slidingwindow local replanning method based on our previous research on quadrotor local planning [5], to avoid collisions on the flight.
In the repeating phase, the drone controls its yaw angle to face its flying direction and build a local map by stereo cameras. We consistently check the local trajectory within a replanning time horizon. If collisions along the local trajectory are reported, replanning is triggered to wrap the trajectory out of obstacles by gradientbased optimization [5].
Iv Flight Corridor Generation
As stated in Sect. IIIC, the first step of our global planning is to build a flight corridor around the teaching trajectory for spatialtemporal trajectory optimization. In our previous work [1], the flight corridor is constructed by finding a series of axisaligned cubes, which may sacrifice much space, especially in a highly nonconvex environment, as is shown in Fig 4. A more illustrative comparison is shown in Fig. 5, where the convex polyhedron captures much more free space than the simple cube. Using simple axisaligned cubes significantly limit the solution space of trajectory optimization, which may result in a poor solution. What’s more, in situations where the free space is very limited, such as flying through a very narrow circle, a cubebased corridor [1] may even fail to cover all teaching trajectory and result in no solutions existing in the corridor. Therefore, to utilize the free space more sufficiently and adapt to even extremely cluttered maps, we propose a method to generate general, free, large convex polyhedrons.
Since the human’s teaching trajectory may be arbitrarily jerky, we cannot assume there is a piecewise linear path to initiate the polyhedron generation, as in [18]. Also, we make no requirements on the convexity of obstacles in the map as in [19]. Our method is based on convex set clustering, which is similar to [34], but is different and advanced at:

We make no assumption on the growing directions of convex clusters and generate completely collisionfree polyhedrons based on our dense occupancy map.

We introduce several careful engineering considerations which significantly speedup the clustering.

We fully utilize the parallel structure of this algorithm and accelerate it over an order of magnitude in GPUs.

We introduce a complete pipeline from building the convex polyhedron clusters to establishing constraints in trajectory optimization.
Iva Convex Cluster Inflation
The core algorithm for the construction of the flight corridor is to find the largest convex free polyhedron at a given coordinate. In this paper, we use an occupancy grid map to represent the environment. Each polyhedron in the flight corridor is the convex hull of a voxel set, which is convex and contains only free voxels. The voxel set is found by clustering as many free voxels as possible around an arbitrary seed voxel. In this paper, we name the voxel set as convex cluster, and the process of finding such a set as convex cluster inflation. Our method for finding such a convex cluster is based on the definition of the convex set:
Definition: A set
in a vector space over
is called a convex set if the line segment joining any pair of points of lies entirely in . [35].The pipeline for iteratively inflating such a cluster while preserving convexity is stated in Alg. 1. Our method operates on a 3D occupancy map where voxels are labeled as obstacle or free. Three voxel sets are maintained in the algorithm. stands for the targeting convex voxel cluster. is the set of voxels that are tentative to be added to in this iteration. And contains newly added voxels which preserve the convexity. The cluster inflation starts by adding the seed voxel to , and adding all neighboring voxels of to . In an iteration, each voxel in is checked whether it can preserve convexity using the function CHECK_CONVEXITY(, ). This function, as is shown in Alg. 2, casts rays from to each existing voxels in . According to the definition of the convex set, with is convex if and only if all rays are collisionfree. Based on this criteria, qualified voxels are considered as active voxels and are added into and . Neighboring voxels of all active voxels are traversed and collected by the function GET_NEIGHBORS() for next iteration. The inflation ends when becomes empty, which implies no additional voxels can be added into . Fig. 6 also illustrates the procedure of the convex cluster inflation.
Having a convex cluster consists of a number of voxels, we convert it to the algebraic representation of a polyhedron for following spatial trajectory optimization. Quick hull algorithm [36] is adopted here for quickly finding the convex hull of all clustered voxels. The convex hull is in vertex representation (Vrepresentation)
, and is then converted to its equivalent hyperplane representation (Hrepresentation) by using double description method
[37]. The Hrepresentation of a 3D polyhedron is a set of affine functions:(1)  
where is the normal vector of the 3D hyperplane and is a constant.
IvB CPU Acceleration
As is shown in Alg. 1, determining whether a voxel preserves the convexity needs to conduct raycasting to all existing voxels in the convex cluster. Iterating with all voxels and rays makes this algorithm impossible to run in realtime, especially when the occupancy grid map has a fine resolution. To make the polyhedron generated in realtime, we take careful engineering considerations on the implementations and propose some critical techniques to significantly increase the overall efficiency.
IvB1 Polyhedron Initialization
We initialize each convex cluster as an axisaligned cube using our previous method [15], which can be done very fast since only index query ((1)) operations are necessary. After inflating the cube to its maximum volume, as in Fig. 4, we switch to the convex clustering to further group convex free space around the cube.
The proposed polyhedron initialization may result in a final polyhedron different from the one which is clustered from scratch. This is because an axisaligned cube only inflates in directions while a convex cluster grows in all possible directions (26connections in a 3D grid map). However, this initialization process is reasonable. Our purpose is not making each polyhedron optimal but capturing as much as possible free space than a simple cube cannot. In practice, the initialization provides a fast discovery of nearby space which is easy to group, and does not prevent the following convex cluster inflation to refine the polyhedron and find sizeable free space. In Sect. VIIC1, we show that the initialization process significantly improves the computing efficiency with only a neglectable sacrifice on the volume of the final polyhedron.
IvB2 Early Termination
We label all voxels in the cluster as inner voxels which inside the convex cluster, and outer voxels which on the boundary of the convex cluster. When traversing a ray from a candidate voxel to a voxel in the convex cluster, we early terminate the ray casting when it arrives at a voxel labeled as inner.
Theorem 1: The early termination at inner voxels is sufficient for checking convexity.
Proof: According to the definition of convex set, a ray connecting an inner voxel to any other voxel in the convex cluster lies entirely in the convex cluster. Hence, the extension line of an inner voxel must lie inside the convex cluster and therefore it must pass the convexity check.
IvB3 Voxel Selection
To further reduce the number of voxels that need to be cast rays, given a candidate voxel, only outer voxels are used to check its convexity.
Theorem 2: Using outer voxels of a convex cluster is sufficient for checking convexity.
Proof: Obviously, the convex cluster is a closed set with outer voxels at its boundary. The candidate voxel is outside this set. Therefore, casting a ray from any inner voxel to the candidate voxel must pass one of the outer voxels. According to Theorem 1, checking convexity of this ray can terminate after the ray passes an outer voxels, which means for a candidate voxel, checking rays cast to outer voxels is sufficient.
By introducing above techniques, the proposed convex cluster inflation can work in real time for a mediate grid resolution () on CPUs. The efficacy of these techniques is numerically validated in Sect. VIIC1.
IvC GPU Acceleration
We propose a parallel computing scheme that significantly speeds up the inflation by one order of magnitude where a GPU is available. As is shown in Sec. IVA, when the convex cluster discovers a new neighboring voxel, sequentially traversing and checking all rays is naturally parallelizable. With the help of many core GPUs, we can cast rays and check collisions parallelly. Moreover, to fully utilize the massively parallel capability of a GPU, reduce the serialize operations, and minimize the data transferring between CPU and GPU, we examine all potential voxels of the cluster parallelly in one iteration. Instead of discovering a new voxel and checking its rays, we find all neighbors of the active set and check their rays all in parallel. The detailed procedure is presented in Alg. 3, where GET_NEIGHBORS() collects all neighbors of a set of voxels, and PARA_CHECK_CONVEXITY(, ) checks the convexity of all candidate voxels parallelly in GPUs. Note that in the serialized version of the proposed method, the voxel discovered earlier may prevent later ones from being clustered, as is illustrated in Fig. 6. However, in the parallel clustering, all voxels examined at the same time may add conflicting voxels to the cluster. Therefore, we introduce an additional variable, to record sequential information of voxels. As shown in Alg. 4, the kernel function is running on the GPU per block. It checks the ray cast from every candidate voxel in to a cluster voxel in and to each other candidate voxel which has a prior index. After that, the function CHECK_RESULTS() selects all qualified voxels and adds them into . Firstly, candidate voxels that have collisions with are directly excluded. Then, candidate voxels having collisions with other candidates that have already been added into are excluded. In this way, we finally get the same results as in the serialized version of the clustering. The efficacy of the parallel computing is shown in Sect. VIIC1.
IvD Corridor Generation and Loop Elimination
Since the trajectory provided by a user may behave arbitrarily jerky and contain local loops, we introduce a specially designed mechanism to elliminate unnecessary loops, i.e., repeatable polyhedrons. The exclusion of repeatable polyhedrons is essential since in following trajectory optimization (Sect. V), each polyhedron is assigned with one piece of the trajectory. Repeatable polyhedrons would result in an optimized trajectory loops as the user does, which is obviously not efficient. The pipeline of the corridor generation is shown in Alg. 5 and Fig. 7. At the beginning of the teaching, the flight corridor is initialized by finding the maximum polyhedron around the position of the drone. Then as the human pilots the drone to move, we keep checking the drone’s position. If it goes outside the last polyhedron ([1]), we further check whether the drone discovers new free space or not. If the drone is contained within the second last polyhedron ([2]), we can determine that the teaching trajectory has a loop, as shown in Fig. 7(c). Then, the last polyhedron in the corridor is regarded as repeatable and is therefore popped out from the corridor. Otherwise, as shown in Fig. 7(d), the drone is piloted to discover new space. Then a new polyhedron is inflated and added to the tail of the corridor. The corridor generation is terminated when the teaching finish. The final flight corridor shares the same topological structure with the teaching trajectory since no obstacles are included in the corridor. And it has no unnecessary loops.
V SpatialTemporal Global Trajecotry Optimization
Va Spatial Trajecotry Optimization
For the spatial optimization, we use the Bernstein basis to represent the trajectory as a piecewise Bézier curve, since it can be easily constrained in the flight corridor by enforcing constraints on control points. An order Bernstein basis is:
(2) 
where is the degree of the basis, is the binomial coefficient and is the variable parameterizing the trajectory. An piece piecewise Bézier curve is written as:
(3) 
For the piece of the curve, is the control point, and is the time duration. The spatial trajectory is generated in dimensions, and . is omitted in following derivation for brevity. In this equation, is scaled by since a standard Bézier curve is defined on .
Follow the formulation in minimumsnap [13], the squared jerk is minimized in this paper. Since the order derivative of a curve corresponds to the angular velocity, the minimization of jerks alleviates the rotation and therefore facilitates visual tracking. The objective of the piecewise curve is:
(4) 
which is in a quadratic form denoted as . Here is composited by all control points in x, y, z dimensions. is a semidefinite Hessian matrix.
For a Bézier curve, its higher order derivatives can be represented by linear combinations of corresponding lowerorder control points. For the and order derivatives of the piece of the curve in Eq. 3, we have:
(5)  
VA1 Boundary Constraints
The trajectory has the boundary constraints on the initial state () and the final state () of the quadrotor. Since a Bézier curve always passes the first and last control points, we enforce the boundary constraints by directly setting equality constraints on corresponding control points in each dimension:
(6)  
VA2 Continuity Constraints
For ensuring smoothness, the minimumjerk trajectory must be continuous for derivatives up to order at all connecting points on the piecewise trajectory. The continuity constraints are enforced by setting equality constraints between corresponding control points of two consecutive curves. For the and pieces of the curve, we can write the equation in each dimension as:
(7)  
VA3 Safety Constraints
The safety of the trajectory is guaranteed by enforcing each piece of the curve to be inside the corresponding polyhedron. Thanks to the convex hull property, an entire Bézier curve is confined within the convex hull formed by all its control points. Therefore we constrain control points using hyperplane functions obtained in Eq. IVA. For the control point of the piece of the trajectory in dimensions, constraints are:
(8)  
Constraints in Equs. VA1 and VA2 are affine equality constraints () and Eq. VA3 is in affine inequality formulation (). Finally, the spatial trajectory optimization problem is formulated as a QP as follows:
min  
s.t.  (9)  
Unlike our previous works on corridor constrained trajectory [15, 17], here the kinodynamic feasibility (velocity and acceleration) is not guaranteed by adding higherorder constraints into this program, but by temporal optimization (Sect. VB). For a resttorest trajectory, the program in Eq. VA3 is always mathematically feasible.
VB Temporal Trajectory Optimization
In spatial optimization, a corridorconstrained spatial trajectory is generated given a fixed time allocation. To optimize the trajectory temporally, we design a retiming function to map the original time variable to a variable . The relation between and is shown in Fig. 8. In this paper, the retiming function is named as the temporal trajectory, and finding the optimal is called the temporal optimization. For the Npiece spatial curve defined in Equ. 3, we write as a corresponding Npiece formulation:
(10) 
where are original time durations of the spatial curve , and are time durations after temporal optimization. Since physically time only increases, is a monotonically increasing function. Therefore we have . For clarity, in what follows, we use to denote taking derivatives with respect to , and for taking derivatives with respect to . By substituting with in
and taking derivatives with chain rule, we can write the velocity as:
and acceleration as:
(11) 
The velocity and acceleration are also piecewise functions.
VC MinimumTime Formulation
VC1 Objective
The total time of the temporal trajectory can be written as:
(12) 
considering . We can introduce a regularization term that penalizes the changing rate of , to tradeoff the minimization of time and control extremeness, or socalled motion aggressiveness, in our final temporal trajectory. The objective function is then written as:
(13) 
where is a weight of the aggressiveness. By setting a larger we can obtain more gentle motions in the temporal trajectory. If , the temporal optimization is solved for generating motions as fast as possible. The motions generated with a large can be viewed in our previous work [1].
VC2 Constraints
The continuities of are enforced by setting constraints between every two consecutive pieces of it. In each dimension , we have:
(17)  
(18) 
Then, to satisfy the initial and the final velocity and acceleration , we set boundary constraints:
(19)  
(20)  
(21)  
(22) 
Finally, kinodynamic feasibility constraints are set as:
(23)  
(24) 
where and are the physical limits of the drone.
VC3 SOCP Reformulation
The above optimization problem has convex objective and constraints and is, therefore, a convex program. To make it easily solvable, for each piece of the trajectory, is discretized to according to a given resolution . . Then, becomes piecewise constant at each discretization point. According to Equ. 15, is piecewise linear. In this way, and are modeled by a series of discrete variables and , where is evaluated at and is evaluated at .
By applying the above discretization, the objective in Eq. 16 is derived as:
(25) 
which is mathematically equivalent to the affine formulation:
(26) 
by introducing and
(27) 
as slack variables and additional constraints.
Eq. 28 can be formulated as a standard rotated quadratic cone:
(30) 
which is denoted as
(31) 
Also, Equ. 29 can be written as a standard (nonrotated) quadratic cone:
(32) 
and is denoted as
(33) 
Finally, a slack variable is introduced to transform the objective in Equ. 26 to an affine function:
(34) 
with a rotated quadratic cone:
(35) 
i.e.
(36) 
where contains in all pieces of the trajectory.
Also, the discretization is applied to and in constraints listed in Sect. VC2. Details are omitted for brevity. After that, we reformulate these constraints as affine equality and inequality functions. Besides, although we assume is piecewise constant, we bound the changing rate of considering the response time of the actuators of our quadrotor. We also write this changing rate constraint in an affine form:
(37) 
where (not jerk) is a predefined bound of the changing rate of acceleration. Since the difference of between and cannot be determined during the optimization, we only bound the changing rate of in domain.
The temporal optimization problem in Sect. VC is formulated as a standard Second Order Cone Program (SOCP):
min  
s.t.  
(38)  
Here and consist of all and . is the resolution of discretization of the problem. The effect of different and to the temporal trajectory and a more detailed derivation of the SOCP can be viewed in [38].
In our teachrepeatreplan system, since the global repeating trajectory always has static initial and final states, Equ. VC3 is always mathematically feasible regardless of the solution of spatial optimization. Because a feasible solution of the optimization program can always be found by infinitely enlarging the time. Combined with the fact that the spatial optimization also always has a solution (Sect. VA), once a flight corridor is given, a spatialtemporal trajectory must exist.
Vi Online Local Replanning
In our previous work [1], once the global planning finished, the drone would execute the trajectory without other considerations. This strategy is based on assumptions that 1) the map of the environment is perfectly built and remains intact; 2) globally consistent pose estimation is provided. We use a VIO system with loop closure to correct local pose drifts, and our dense map is globally deformed according to the global pose graph. However, the first assumption does not always hold, especially when new obstacles suddenly appear or the environment changes. As for the second assumption, our global pose estimation relies on the loop closure detection, which also does not guarantee an extremely high recall rate. In situations where there are significant pose drifts but no timely loop closure corrections, the drone may have collisions with obstacles, as in Fig. 9.
Via Local Replanning Framework
To address above issues fundamentally, we propose a local replanning framework which reactively wraps the global trajectory to avoid unmodeled obstacles. A sliding local map is maintained onboard, where obstacles are fused, and an ESDF (Euclidean Signed Distance Field) [39] is updated accordingly. Note that the dense global map is attached to the global pose graph but the local map introduced here is associated with the local VIO frame and sliding with the drone.
ViA1 ESDF Mapping
We adopt our previous work FIESTA [4], which is an advanced incremental ESDF [40] mapping framework, to build the local map for online replanning. FIESTA fuses the depth information into a voxelhashed occupancy map [41] and updates the distance value of voxels as few as possible using a breadthfirst search (BFS) framework. It is lightweight, efficient, and produces nearoptimal results. Details can be checked in [4]. The ESDF is necessary for the following gradientbased trajectory wrapping. An example of a local occupancy map and its corresponding ESDF map are shown in Fig. 10. Note, in our system the range of the local map is decided by the range of current depth observation.
ViA2 Sliding Window Replanning
Due to the limited onboard sensing range and computing resources, it is impossible and unnecessary to conduct global replanning. In this article, we maintain a temporal sliding window over the global trajectory and conduct local replanning within it. As is shown in Fig. 11, when obstacles are observed to block the trajectory in the sliding window, a replaned trajectory is generated to avoid obstacles, and rejoin the global trajectory afterward.
ViB GradientBased Bspline Optimization
ViB1 Bspline Trajectory Formulation
A Bspline is a piecewise polynomial function defined by a series of control points and knot vector . For a degree Bspline, we have . Following the matrix representation of the De Boor–Cox formula [42], the value of a Bspline can be evaluated as:
(39) 
here is a constant matrix depends only on . And , for .
ViB2 Bspline Initialization
We initialize the local trajectory optimization by reparameterizing the trajectory in the replanning horizon as a uniform Bspline. The reason we use uniform Bspline is that it has a simple mathematical formula that is easy to evaluate in the following optimization. For a uniform Bsplines, each knot span has identical value . The local trajectory is first discretized to a set of points according to a given . Then these points are fitted to a uniform Bspline by solving a minleast square problem.
Note that, a degree uniform Bspline is naturally order continuous between consecutive spans. Therefore, there is no need to enforce continuity constraints in the following optimization explicitly. Besides, for a degree Bspline trajectory defined by control points, the first and last control points are fixed due to the continuous requirement of the starting and ending states of the local trajectory.
ViB3 Elastic Band Optimization
The basic requirements of the replaned Bspline are threefolds: smoothness, safety, and dynamical feasibility. We define the smoothness cost using a jerkpenalized elastic band cost function[43, 44]:
(40) 
which can be viewed as a sum of the squared jerk of control points on the Bspline. Note here we use this formulation which is independent of the time parametrization of the trajectory instead of the traditional time integrated cost function [13]. Because the time duration in each span of the Bspline may be adjusted after the optimization (Sec. VIB4), Eq. VIB3 captures the geometric shape of the Bspline regardless of the time parametrization. Besides, Eq. VIB3 bypasses the costly evaluation of the integration and is, therefore, more numerically robust and computationally efficient in optimization.
The safety and dynamical feasibility requirements of the Bspline are enforced as soft constraints and added to the cost function. Also, the collision cost , dynamical feasibility cost , and are evaluated at only control points. The collision cost is formulated as the accumulated L2penalized closest distance to obstacles along the trajectory, which is written as
(41) 
where is the distance between to its closet obstacle and is recorded in the ESDF. is defined as
(42) 
where is the expected path clearance. and are applied to velocities and accelerations, which exceed the physical limits. The formulations of and are similar to Eq. 41 and are omitted here. The overall cost function is:
(43) 
where are weighting coefficients. can be minimized for a local optimal solution by general optimization methods such as GaussNewton or LevenbergMarquardt.
ViB4 Iterative Refinement
In the aboveunconstrained optimization problem, although collisions and dynamical infeasibilities are penalized, there is no hard guarantee on generating a strictly feasible solution. To improve the success rate in practice, we add a postprocess to refine the trajectory iteratively. In each iteration, we check collisions and feasibilities of all optimized control points. If collisions are detected, we increase the collision term by increasing and solve the optimization problem (Eq. 43) again.
Since we wrap the local trajectory to go around obstacles, the trajectory is always lengthened after the optimization. Consequently, using the original time parametrization will unavoidably result in a higher aggressiveness, which means the quadrotor tends to fly faster. Then its velocity and acceleration would easily exceed the predefined limits. Therefore, we adjust the time parameterization of the local trajectory to squeeze out dynamical infeasibilities. We slightly enlarge infeasible knots spans of the Bspline by the following heuristic.
(44) 
where is a constant slightly larger than . are infeasible velocity and acceleration and are maximum allowed acceleration and velocity of the drone. The time duration is iteratively enlarged until obtaining a feasible solution or exceeding the maximum iteration limit. If no feasible solution exists after the time adjustment, is increased, and the trajectory is optimized again.
Vii Results
Viia Implementation Details
The global planning method proposed in this paper is implemented with a QP solver OOQP^{2}^{2}2http://pages.cs.wisc.edu/~swright/ooqp/ and a SOCP solver Mosek^{3}^{3}3https://www.mosek.com. The local replanning depends on a nonlinear optimization solver NLopt^{4}^{4}4https://nlopt.readthedocs.io. The source code of all modules in our quadrotor system, including local/global localization, mapping, and planning, are released as rospackages^{5}^{5}5https://github.com/HKUSTAerialRobotics/TeachRepeatReplan for the reference of community. Readers of this paper can easily replicate all the presented results. The state estimation, pose graph optimization, local mapping, local replanning, and the controller is running onboard on a Manifold2C^{6}^{6}6https://store.dji.com/product/manifold2?vid=80932 minicomputer. Other modules are running on an offboard laptop which has a GTX 1080^{7}^{7}7https://www.nvidia.com/enus/geforce/20series/ graphics card.
Our global map is built to attach to a global pose graph. Both the map and the pose graph are saved for repeating. Before the repeating, the drone is handheld to close the loop of the current VIO frame with the saved global pose graph. The relative transformation of these two frames is used to project the control commands to the VIO frame. During the repeating, pose graph optimization is also activated to calculate the pose drift and compensate for the control command.
ViiB Simulated Flight Test
We first test our global and local planning methods in simulations. The simulated environments are randomly deployed with various types of obstacles and circles for drone racing, as shown in Fig. 12. The simulating tool we use is a lightweight simulator MockaFly^{8}^{8}8https://github.com/HKUSTAerialRobotics/mockasimulator, which contains quadrotor dynamics model, controller, and map generator. And the simulator’s is also released as an opensource package with this paper. In the simulation, a drone is controlled by a joystick to demonstrate the teaching trajectory. The simulated drone is equipped with a depth camera whose depth measurements are realtime rendered in GPU by backprojecting the drone’s surrounding obstacles. We randomly add noise on the depth measurements to mimic a real sensor. The replanning module is activated in the simulation and is triggered by the noise added on the depth. The teaching trajectory and the flight corridor is shown in Fig. 12(a). The global trajectory, local replanned trajectory, and depth measurement are shown in Fig. 12(b). More details about the simulation are presented in the attached video.
ViiC Benchmark Comparisons
ViiC1 Corridor Generation
We test the performance of the flight corridor generation methods (Sect. IV), to show the efficacy of the proposed techniques for CPU (Sect. IVB) and GPU (Sect. IVC) accelerations. For convenience, we denote the basic process for doing convex cluster inflation as CPU_raw; CPU_raw added cube initialization as CPU+; the one with cube initialization, vertex selection and early termination as CPU++; and the parallel version of the convex cluster inflation as GPU. We first compare the time consumed for finding the largest flight corridor with these methods, to validate the improvements of efficiency by using our proposed CPU and GPU acceleration techniques. Then, we compare the ratio of space capturing by methods with and without the polyhedron initialization, and by our previous method [1]. The motivation of the latter comparison is twofold:

It serves to show superior performance by replacing cubes with polyhedrons.

As discussed in Sect. IVB, the initialization process would result in different final clustering results compared to the pure convex cluster inflation. This comparison also validates that the initialization process only makes neglectable harm to free space capturing.
We generate 10 random maps, with 10 20 random teaching trajectories given in each map. The average length of teaching trajectories is . Results are given in Tabs. I and II.
Computing Time (s)  GPU  CPU++  CPU+  CPU_raw 

Res. =  0.031  0.111  0.162  0.359 
Res. =  0.055  0.310  0.503  1.309 
Res. =  0.169  1.423  2.803  9.583 
Res. =  0.942  13.940  30.747  141.659 
Res. =  3.660  71.862  157.181  927.131 
As shown in Tab. I, as the resolution of the map being finer, the computing time of the simple convex cluster inflation quickly becomes unacceptable huge. In CPU, with the help of polyhedron initialization, the computational efficiency is improved several times. Moreover, according to Tab. I, introducing the voxel selection and early termination can increase the speed more than one order of magnitude in a fine resolution. The efficacy of the GPU acceleration is even more significant. As shown in Tab. I, the GPU version improves the computing speed 30 times at a fine resolution (), and 10 times at a coarse resolution (). For a finer resolution, more candidate voxels are discovered in one iteration of Alg. 3, thus more computations are conducted parallelly to save time.
Space Ratio (%)  w/ Initialization  w/o Initialization  Previous [1] 

Res. =  99.22  100.00  82.28 
Res. =  99.56  100.00  82.92 
Res. =  98.93  100.00  81.82 
Res. =  97.06  100.00  82.78 
Res. =  97.14  100.00  83.03 
For the second comparison, we count the number of free voxels included in the flight corridor found by each method. At each resolution, we take the result of the method without initialization as 100% and compare others against it. Tab. II indicates two conclusions:

Using polyhedrons instead of axisaligned cubes can significantly increase the volume of the flight corridor.

Using initialization only slightly sacrifices the volume of the flight corridor. And the sacrifice is neglectable in a medium or coarse resolution ().
The first conclusion holds because a simple cube only discovers free space in directions and sacrifices much space in a highly nonconvex environment, as in Fig. 4. The second conclusion comes from the fact that in a highly nonconvex environment, a regular shaped polyhedron (a cube) does not prevent the following voxel clustering in its nearby space. It shows that the initialization plus the clustering refinement does not harm the volume of the final polyhedron, and is acceptable in practice, especially for a resolution not very fine.
ViiC2 Global Planning
Method  Length (m)  Time (s)  Energy 

Proposed Method  84.607  55.154  83.350 
Previous Method  86.723  57.736  89.883 
Gradientbased [45]  89.622  111.398  109.575 
Waypointbased [14]  97.045  94.895  204.267 
We compare the proposed global planning method against our previous work [1] and other representative optimizationbased trajectory generation methods, such as the waypointbased method [14] and the gradientbased method [45]. For the latter two benchmarked methods, there is no explicit way to capture the topological structure of the teaching trajectory. Therefore, we convert the teaching trajectory to a piecewise path by recursively finding a collisionfree straight line path along with it. Then we use this path to initialize the waypointbased [14] method and the gradientbased method [45]. Benchmarked methods are also integrated into the coordinate descent framework with temporal optimization. Some parameters dominate the performance of these benchmarked methods, especially for the gradientbased method [45] where the tradeoff between collision and smoothness is essential. For a fair comparison, parameters are tuned to achieve the best performances before the test. We randomly generate 10 simulated environments with dense obstacles, as in Sect. VIIB, and conduct 10 teachandrepeat trials in each map. A sample result of generated trajectories is shown in Fig. 14.
As shown in Tab. III, our proposed method outperforms in all length, time, and energy aspects. The waypointbased [14] method and the gradientbased [45] method both require a piecewise linear path as initialization. The waypointbased [14] method can only add intermediate waypoints on the initial path. Therefore, it is mostly dominated by its initialization and tends to output a solution with low quality. The gradientbased [45] method has no such restriction and can adjust the path automatically by utilizing gradient information. However, its optimization formulation is underlying nonconvex, since the collision cost is defined on a nonconvex ESDF. Therefore, the gradientbased [45] method always finds a locally optimal solution around its initial guess. Compared to these two methods, our method is initializationfree. Both the spatial and temporal optimization of our proposed method enjoys the convexity in its formulation. They are guaranteed to find the global energyoptimal and timeoptimal solutions in the flight corridor. Naturally, a smoother trajectory also tends to generate a faster time profile. So finally, under the same coordinate descent framework, our method always outperforms [45] and [14]. Compared to [1], the advanced corridor generation proposed in this paper (Sect. IV) can always capture more free space than using our previous axisaligned corridor. Naturally, it provides much more freedom for global planning and results in much better solutions.
ViiD Indoor Flight Test
ViiD1 Fast Flight in a Static Environment
Firstly, we conduct experiments in a cluttered drone racing scenario. This experiment validates the robustness of our proposed system, and also pushes the boundary of aggressive flight of quadrotors. Several different types of obstacles, including circles, arches, and tunnels, are deployed randomly to composite a complex environment, as shown in Fig. 15(a). The smallest circle only has a diameter of , which is very narrow compared to the tiptotip size of our drone. The maximum velocity and acceleration of the drone are set as and , respectively. And the parameter in Equ. 13 is set as 0, which means the quadrotor is expected to fly as fast as possible as long as it respects the kinodynamic limits. A dense global consistent map is prebuilt using the method stated in Sect. IIIB. During the teaching phase, the quadrotor is virtually piloted by a human to move amid obstacles. Then the quadrotor autonomously converts this teaching trajectory to a global repeating trajectory and starts to track it. Snapshots of the drone in the flight are shown in Fig. 16. The teaching trajectory and the convex safe flight corridor are visualized in Fig 17(a). And the global repeating trajectory is in Fig. 17(b).
ViiD2 Local Replanning Against Unknown Obstacles
Our system can deal with changing environments and moving obstacles. In this experiment, we test our system also in the drone racing site to validate our local replanning module. Several obstacles are moved or added to change the drone racing environment significantly, and some others are dynamically added during the repeating flight, as shown in Fig. 18 In this experiment, the maximum velocity and acceleration for the quadrotor are set as and . The local ESDF map is sliding with the drone using a ringbuffered updating mechanism [46]. The resolution of the local perception is . The size of the map is decided by points observed spreading in the current frame. The horizon and frequency of the local replanning are and , respectively. Replanning is triggered 8 times during the flight in this experiment, and local safe and dynamical feasible splines are generated on time accordingly. Local trajectories, local maps, and the overview of this experiment are shown in Fig. 19. We refer readers to the attached video for more details.
ViiE Outdoor Flight Test
Finally, we conduct quadrotor flight experiments with a much higher aggressiveness in two different outdoor scenes, as in Fig. 20, to show the robustness of our system in natural environments. Although these experiments are conducted outdoor, GPS or other external positioning devices are not used. The teachrepeatreplan pipeline is as the same as before indoor experiments VIID. The maximum allowed velocity and acceleration limits for these two trials are set as , and , , respectively. The drone’s desired and estimated positions and velocities in the second trial are given in Fig. 23, which shows acceptable tracking errors. Since the flight speed is significantly higher than indoor experiments, we set a smaller replanning horizon as . Results such as the global and local trajectory and the global map are visualized in Figs. 21 and 22. More clearly visualizations of outdoor experiments are given in the video.
Viii Conclusion
In this paper, we propose a framework, teachrepeatreplan for quadrotor aggressive flights in complex environments. The main idea of this work is to find the topological equivalent free space of the user’s teaching trajectory, use spatialtemporal trajectory optimization to obtain an energyefficient repeating trajectory, and incorporate online perception and replanning to ensure the safety against environmental changes and moving obstacles. The teaching process is conducted by virtually controlling the drone in simulation. The generated repeating trajectory captures users’ intention and respect an expected flight aggressiveness, which enables autonomous flights much more aggressive than human’s piloting in complex environments. The online replanning guarantees the safety of the flight and also respects the reference of the repeating trajectory.
To group large free space around the teaching trajectory, we propose a GPUaccelerated convex polyhedron clustering method to find a flight corridor. The optimal global trajectory generation problem is decoupled as spatial and temporal subproblems. Then these two subproblems are iteratively optimized under the coordinate descent framework. Moreover, we incorporate the local perception and local trajectory replanning modules into our framework to deal with environmental changes, dynamic obstacles, and localization drifts.
The proposed system is complete and robust. Users of our system do not have to pilot the drone carefully to give a teaching trajectory. Instead, an arbitrarily jerky/poor trajectory can be converted to an efficient and safe global trajectory. Moreover, when the environment changes or the global localization drifts, the local perception and replanning modules guarantee the safety of the drone while tracking the global trajectory. Our system is also flexible and easily replicable, as evidenced by various types of experiments presented in this paper, and a thirdparty application^{9}^{9}9Flight demonstration at the Electrical and Mechanical Services Department (EMSD), Hong Kong government. Video: https://youtu.be/Ut8WT0BURrM. We release all components of our system for the reference of the community.
References
 [1] F. Gao, L. Wang, K. Wang, W. Wu, B. Zhou, L. Han, and S. Shen, “Optimal trajectory generation for quadrotor teachandrepeat,” IEEE Robotics and Automation Letters (RAL), 2019.
 [2] M. Fehr, T. Schneider, M. Dymczyk, J. Sturm, and R. Siegwart, “Visualinertial teach and repeat for aerial inspection,” arXiv preprint arXiv:1803.09650, 2018.
 [3] P. Furgale and T. D. Barfoot, “Visual teach and repeat for longrange rover autonomy,” Journal of Field Robotics, vol. 27, no. 5, pp. 534–560, 2010.
 [4] L. Han, F. Gao, B. Zhou, and S. Shen, “Fiesta: Fast incremental euclidean distance fields for online motion planning of aerial robots,” arXiv preprint arXiv:1903.02144, 2019.
 [5] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” IEEE Robotics and Automation Letters (RAL), 2019.
 [6] C. Sprunk, G. D. Tipaldi, A. Cherubini, and W. Burgard, “Lidarbased teachandrepeat of mobile robot trajectories,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2013, pp. 3144–3149.
 [7] P. Furgale, P. Krüsi, F. Pomerleau, U. Schwesinger, F. Colas, and R. Siegwart, “There and back againdealing with highlydynamic scenes and longterm change during topological/metric route following,” in ICRA14 Workshop on Modelling, Estimation, Perception, and Control of All Terrain Mobile Robots, 2014.
 [8] P. Krüsi, B. Bücheler, F. Pomerleau, U. Schwesinger, R. Siegwart, and P. Furgale, “Lightinginvariant adaptive route following using iterative closest point matching,” Journal of Field Robotics, vol. 32, no. 4, pp. 534–564, 2015.
 [9] C. J. Ostafew, A. P. Schoellig, and T. D. Barfoot, “Visual teach and repeat, repeat: Iterative learning control to improve mobile robot path tracking in challenging outdoor environments,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2013, pp. 176–181.
 [10] M. Paton, K. MacTavish, M. Warren, and T. D. Barfoot, “Bridging the appearance gap: Multiexperience localization for longterm visual teach and repeat,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2016, pp. 1918–1925.
 [11] M. Paton, K. MacTavish, C. J. Ostafew, and T. D. Barfoot, “It’s not easy seeing green: Lightingresistant stereo visual teach & repeat using colorconstant images,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), 2015, pp. 1519–1526.
 [12] L.P. Berczi and T. D. Barfoot, “It’s like déjà vu all over again: Learning placedependent terrain assessment for visual teach and repeat,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2016, pp. 3973–3980.
 [13] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Shanghai, China, May 2011, pp. 2520–2525.
 [14] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in Proc. of the Intl. Sym. of Robot. Research (ISRR), Dec. 2013, pp. 649–666.
 [15] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Brisbane, Australia, May 2018.
 [16] F. Gao and S. Shen, “Online quadrotor trajectory generation and autonomous navigation on point clouds,” in Proc. of the IEEE Intl. Sym. on Safety, Security, and Rescue Robotics (SSRR), lausanne, switzerland, 2016, pp. 139–146.
 [17] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” Journal of Field Robotics, 2018.
 [18] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3d complex environments,” IEEE Robotics and Automation Letters (RAL), pp. 1688–1695, 2017.
 [19] R. Deits and R. Tedrake, “Computing large convex regions of obstaclefree space through semidefinite programming,” in Algorithmic Foundations of Robotics XI. Springer, 2015, vol. 107, pp. 109–124.
 [20] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), May 2009, pp. 489–494.
 [21] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuoustime trajectory optimization for online uav replanning,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Daejeon, Korea, Oct. 2016, pp. 5332–5339.
 [22] Y. Lin, F. Gao, T. Qin, W. Gao, T. Liu, W. Wu, and S. Shen, “Autonomous aerial navigation using monocular visualinertial fusion,” Journal of Field Robotics, vol. 35, no. 1, pp. 23–51, 2018.
 [23] H. Oleynikova, C. Lanegger, Z. Taylor, M. Pantic, M. Alexander, R. Siegwart, and J. Nieto, “An opensource system for visionbased microaerial vehicle mapping, planning, and flight in cluttered environments,” arXiv preprint arXiv:1812.03892, 2019.
 [24] H. M. Choset, S. Hutchinson, K. M. Lynch, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of robot motion: theory, algorithms, and implementation. MIT press, 2005.
 [25] M. Roberts and P. Hanrahan, “Generating dynamically feasible trajectories for quadrotor cameras,” ACM Transactions on Graphics (TOG), vol. 35, no. 4, p. 61, 2016.
 [26] J. Jamieson and J. Biggs, “Near minimumtime trajectories for quadrotor uavs in complex environments,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2016, pp. 1550–1555.
 [27] D. Verscheure, B. Demeulenaere, J. Swevers, J. De Schutter, and M. Diehl, “Timeoptimal path tracking for robots: A convex optimization approach,” IEEE Transactions on Automatic Control, vol. 54, no. 10, pp. 2318–2327, 2009.
 [28] Q.C. Pham, “A general, fast, and robust implementation of the timeoptimal path parameterization algorithm,” IEEE Transactions on Robotics, vol. 30, no. 6, pp. 1533–1540, 2014.
 [29] H. Pham and Q.C. Pham, “A new approach to timeoptimal path parameterization based on reachability analysis,” IEEE Transactions on Robotics, vol. 34, no. 3, pp. 645–659, 2018.
 [30] S. J. Wright, “Coordinate descent algorithms,” Mathematical Programming, vol. 151, no. 1, pp. 3–34, 2015.
 [31] T. Lee, M. Leoky, and N. H. McClamroch, “Geometric tracking control of a quadrotor uav on se (3),” in Proc. of the IEEE Control and Decision Conf. (CDC), Atlanta, GA, Dec. 2010, pp. 5420–5425.
 [32] T. Qin, P. Li, and S. Shen, “Vinsmono: A robust and versatile monocular visualinertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
 [33] K. Wang, F. Gao, and S. Shen, “Realtime scalable dense surfel mapping,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), 2019.
 [34] F. Blochliger, M. Fehr, M. Dymczyk, T. Schneider, and R. Siegwart, “Topomap: Topological mapping and navigation based on visual slam maps,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), 2018, pp. 1–9.
 [35] S. Lay, Convex sets and their applications. Courier Corporation, 2007.
 [36] C. B. Barber, D. P. Dobkin, D. P. Dobkin, and H. Huhdanpaa, “The quickhull algorithm for convex hulls,” ACM Transactions on Mathematical Software (TOMS), vol. 22, no. 4, pp. 469–483, 1996.
 [37] K. Fukuda and A. Prodon, “Double description method revisited,” in FrancoJapanese and FrancoChinese Conference on Combinatorics and Computer Science. Springer, 1995, pp. 91–111.
 [38] F. Gao, W. Wu, J. Pan, B. Zhou, and S. Shen, “Optimal time allocation for quadrotor trajectory generation,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Madrid, Spain, Oct 2018.
 [39] P. F. Felzenszwalb and D. P. Huttenlocher, “Distance transforms of sampled functions,” Theory of computing, vol. 8, no. 1, pp. 415–428, 2012.

[40]
T. Schouten and E. L. van den Broek, “Incremental distance transforms (idt),”
in
2010 20th International Conference on Pattern Recognition
. IEEE, 2010, pp. 237–240.  [41] M. Klingensmith, I. Dryanovski, S. Srinivasa, and J. Xiao, “Chisel: Real time large scale 3d reconstruction onboard a mobile device using spatially hashed signed distance fields.” in Proc. of Robot.: Sci. and Syst. (RSS), 2015.
 [42] C. de Boor, “Subroutine package for calculating with bsplines.” Los Alamos Scientific Lab., N. Mex., Tech. Rep., 1971.
 [43] S. Quinlan and O. Khatib, “Elastic bands: Connecting path planning and control,” 1993, pp. 802–807.
 [44] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimization approach to smooth trajectories for motion planning with carlike robots,” in Proc. of the IEEE Control and Decision Conf. (CDC), 2015, pp. 835–842.
 [45] F. Gao, Y. Lin, and S. Shen, “Gradientbased online safe trajectory generation for quadrotor flight in complex environments,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Sept 2017, pp. 3681–3688.
 [46] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Realtime trajectory replanning for mavs using uniform bsplines and a 3d circular buffer,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2017, pp. 215–222.