Download Accelerating Heuristic Computation in Motion Planning with Experience Graphs and more Study notes Algorithms and Programming in PDF only on Docsity!
Speeding up heuristic computation
in planning with Experience Graphs
Mike Phillips^1 and Maxim Likhachev^1
Abstract— Experience Graphs have been shown to accelerate motion planning using parts of previous paths in an A* framework. Experience Graphs work by computing a new heuristic for weighted A* search on top of the domain’s original heuristic and the edges in an Experience Graph. The new heuristic biases the search toward relevant prior experience and uses the original heuristic for guidance otherwise. In previous work, Experience Graphs were always built on top of domain heuristics which were computed by dynamic programming (a lower dimensional version of the original planning problem). When the original heuristic is computed this way the Experience Graph heuristic can be computed very efficiently. However, there are many commonly used heuristics in planning that are not computed in this fashion, such as euclidean distance. While the Experience Graph heuristic can be computed using these heuristics, it is not efficient, and in many cases the heuristic computation takes much of the planning time. In this work, we present a more efficient way to use these heuristics for motion planning problems by making use of popular nearest neighbor algorithms. Experimentally, we show an average 8 times reduction in heuristic computation time, resulting in overall planning time being reduced by 66%. with no change in the expanded states or resulting path.
I. INTRODUCTION Many of the tasks we would like robots to perform are highly repetitive, such as transporting objects from one place to another in a warehouse or clearing the table and putting dirty dishes in the sink. Many of these tasks also require motion planning in order to guarantee each motion is completed in a collision free and efficient manner. With tasks being relatively similar each time, it is inefficient to generate each motion from scratch. Instead, we would prefer for our planners to make use of prior experience to improve performance. Planning with Experience Graphs (E-Graphs) was in- troduced in [1]. This method allows weighted A* based planners to incorporate previously generated paths and user demonstrations into the planning process. It does so while maintaining guarantees on resolution completeness (if a solution exists from the start to the goal within the user chosen discretization, the planner will find it) and bounded suboptimality (the planner’s solutions will be within a user- chosen factor of the cost of an optimal solution). In motion planning, A* based methods search from the starting con- figuration toward the goal state (the reverse is also possible) by repeatedly choosing a state that has been discovered and expanding it (applying a set of motions to it and adding the resulting new states to the set of discovered). These methods
*This research was sponsored by the NSF grant IIS-1409549. (^1) Robotics Institute, Carnegie Mellon University, Pittsburgh, PA
use a heuristic function which estimates the remaining cost- to-go from any state to the goal, in order to focus search effort on states that are likely to lead to the goal quicker. Experience Graphs construct a special heuristic function for the A* planner by combining the original heuristic with previous paths. The result is a heuristic which is goal directed but biased toward getting there via reusing previous paths when relevant. The E-Graph heuristic can build on top of almost any reasonable user chosen heuristic, but is particularly efficient to compute when combined with a heuristic computed using dynamic programming. These tend to be heuristics computed over lower dimensional versions of the original problem. For instance, in a robot navigation problem the configuration space might be position and heading (x, y, θ) and a heuris- tic might be the cost-to-go (ignoring θ) for each 2D cell computed via a single 2D search from the goal. Since the E- Graph heuristic is also computed by dynamic programming (Section III-B), it turns out that its computation can be folded into these dynamic programming-based heuristics with almost no additional overhead. In previous publications on Experience Graphs, these types of heuristics were always used. However, many commonly used heuristics in robotics are not computed this way, such as euclidean and manhattan distance or heuristics based on the dubins car model. For regular A* searches (without E-Graphs) these types of heuristics are often efficient to use, in many cases having O(1) computation time for a pair of states. However, the naive implementation of the E-Graph heuristic, on top of these type of heuristics is linear in the size of the E- Graph when evaluating each state’s heuristic that the search encounters. This consumes much of the planning time and for large E-Graphs can be prohibitive. In this paper, we speed up the E-Graph heuristic com- putation for general heuristics with no change in planner behavior, i.e. the planner expands the exact same states in the same order and produces the exact same paths. We make use of offline precomputation time and the availability of efficient nearest neighbor methods, which have also played a role in speeding up popular sampling-based motion planners (e.g. RRT, PRM). We experimentally evaluate these improvements in a chal- lenging full body motion planning domain for the PR2 robot. We use the Euclidean distance heuristic which previously could be less efficient when combined with E-Graphs. We observe an average speedup of 8 times over the previous heuristic computation method resulting in overall planning
times taking 66% as long.
II. RELATED WORK As mentioned earlier, Experience Graph are used to plan paths faster by reusing previously planned paths [1] or demonstrations [2]. There are a number of other approaches that use previous paths to speedup motion planning [3], [4], [5], [6]. However, the improvements being made in this paper are specific to the Experience Graph heuristic computation. A large part of our improvements will make use of exact nearest neighbor methods which build a data structure that allows them to avoid looking at all members of the dataset during queries. The KD-tree (k-dimensional tree) assumes the data is a set of k-dimensional points and recursively splits the data in half using axis-aligned hyperplanes in the vector space [7]. The VP-tree (vantage point tree), makes fewer assumptions. It works by selecting a pivot from within the dataset and then choosing a “median radius” around it such that half of the datapoints are inside and half are outside according to the distance metric. It then recurses on the two sets [8]. Here the data is not required to be in a vector space like the KD-tree, instead there are only contraints on the distance metric itself. The main constraint is that it satisfies the triangle inequality. A similar method is the GH-tree (generalized hyperplane tree) which works by choosing two pivots from the dataset and splitting the data in half according to how close they are to both points (roughly, for each datapoint, is it closer to the first pivot or the second) and then recursing [9]. The GH-tree makes the same assumptions on the distance metric as the VP-tree. LSH (locality-sensitive hashing) is a method which uses a hash function to group nearby datapoints into the same bin with high probability [10]. Identifying nearest neighbors quickly is a crucial com- ponent to sampling-based motion planners such as RRTs (rapidly-exploring random tree) [11] and PRMs (probabilistic roadmap) [12]. These methods repeatedly choose random samples in configuration space and then attempt to connect them to one or more of the nearest neighbors in this randomly generated tree or graph. In [13] KD-trees are applied to such planners to improve performance.
III. BACKGROUND
A. Definitions
E-Graphs assume the motion planning problem is repre- sented as a graph where a start and goal state are provided (sstart, sgoal) and the desired output is a path (sequence of edges) that connect the start to the goal.
- G(V G, EG) is a graph modeling the original motion planning problem, where V G^ is the set of vertices and EG^ is the set of edges connecting pairs of vertices in V G.
- GE^ (V E^ , EE^ ) is the E-Graph (GE^ ⊆ G). It is typically a colleciton of previously planned paths or demonstations.
- c(u, v) is the cost of the edge from vertex u to vertex v - cE^ (u, v) is the cost of the edge from vertex u to vertex v in graph GE^ and is always equal to c(u, v) when it exists. If edge (u, v) does not exist in EE^ , cE^ (u, v) = ∞. The algorithm is based on heuristic search and therefore uses a heuristic function hG(u, v) estimating the cost from u to v (u, v ∈ V G). E-Graphs assume hG(u, v) is admissible and consistent for any pair of states u, v ∈ V G. An admis- sible heuristic never overestimates the minimum cost from any state to any other state. A consistent heuristic hG(u, v) is one that satisfies the triangle inequality, hG(u, v) ≤ c(u, s) + hG(s, v) and hG(u, u) = 0, ∀u, v, s ∈ V G^ and ∀(u, s) ∈ EG.
B. Experience Graphs This section provides a brief description of how E-Graphs work. For more details see the prior work where Experience Graphs were introduced [1]. An Experience Graph GE^ is a collection of previously planned paths or demonstrations (experiences). Planning with Experience Graphs uses weighted A* (A* with heuristics inflated by ε > 1 ) to search the original graph G (which represents the planning problem) but tries to reuse paths in GE^ in order to minimize the exploration of the original graph G (which is significantly larger than GE^ ). This is done by modifying the heuristic computation of weighted A* to drive the search toward paths in GE^ , that appear to lead towards the goal. Essentially, this comes down to computing a new heuristic distance from the state in question to the goal where traveling off of GE^ is penalized but traveling on edges of GE^ is not. The new heuristic hE^ is defined in terms of the original heuristic hG^ and edges in GE^ for all states s 0 in the original graph.
hE^ (s 0 ) = min π
N∑ − 2
i=
min{εE^ hG^ (si, si+1) , cE^ (si, si+1)}
(1) where π is called the heuristic path 〈s 0... sN − 1 〉 and sN − 1 = sgoal and εE^ is a scalar ≥ 1. As shown in [1], the heuristic is εE^ -consistent and therefore guarantees that the solution cost will be no worse than εE^ times the cost of the optimal solution when running A* search to find a path. More generally, planning with Experience Graphs using weighted A* search inflates the entire hE^ heuristic by ε. Consequently, the cost of the solution is bounded by ε·εE^ times the optimal solution cost. Equation 1 is computed by finding the shortest heuristic path (Dijkstra’s algorithm) from s 0 to the goal in a simplified version of the planning problem where there are two kinds of edges. The first set (the first term in Eqn. Equation 1). are edges that represent the same connectivity as hG^ in the original planning problem but their cost is inflated by εE^. So if the original heuristic is euclidean distance then there are edges between all pairs of states. However, if the original heuristic is some lower dimensional search then we use the edge set from that. The second set of edges (the second term in Eqn. Equation 1) are from GE^ with their costs cE
Algorithm 1 Naive method 1: procedure ONSTARTUP() 2: procedure AFTERGOAL() 3: Run Dijkstra’s Algorithm on G′^ with source sgoal 4: HE^ (s) = the cost of s in the Dijkstra search 5: procedure GETHEURISTIC(s) 6: hE^ (s) = mins′∈V ′
( εE^ hG(s, s′) + HE^ (s′)
)
we can reduce the time spent at the start of each planning episode. When the planner initializes, we assume we already know the E-Graph and the parameter εE^. If this is not true, than the following procedure will have to be repeated whenever either changes. On planner initialization (ONSTARTUP in Algorithm 2), we will be computing the all-pairs shortest paths on the graph G′′^ which is the same as G′^ but without the goal vertex (since it is not known yet). To do this we create an adjacency matrix with the two types of edges that are used in G′. We then run the Floyd-Warshall algorithm to compute d(u, v)∀u, v ∈ V E^ , i.e. the shortest path from any u to v in G′′. This runs in O(|V E^ |^3 ), which is large, but since it just happens once on planner initialization, it does not affect planning times. In fact, this could be computed once offline and the d(u, v) values could be written to file for future use. When a planning episode starts (and we are given the goal), we need to compute HE^ (s) but we will make use of the precomputed d(u, v) values to do this more efficiently than the Dijkstra search from the naive method. Instead, each HE^ (s) is computed by finding the E-Graph node the goal connects to first along its heuristic path to s. More formally,
HE^ (s) = mins′∈V E
εE^ hG(sgoal, s′) + d(s′, s)
, ∀s ∈ V E (3) The computation of all HE^ values now takes O(|V E^ |^2 ) instead of the naive O(|V E^ |^2 log|V E^ |).
- Dijkstra with an unsorted array: An alternative to the above precomputation approach is to run Dijkstra’s algorithm using a different data structure. While Dijkstra’s is often run with a binary heap, resulting in the O(|V E^ |^2 log|V E^ |) runtime. For graphs with high connectivity (ours is fully con- nected) it is actually faster to use an unsorted array instead. It results in a runtime of O(|V |^2 ). This will make AFTERGOAL take the same asymptotic runtime as the method we just posed, without having to run the all-pairs shortest path precomputation (ONSTARTUP can go back to being empty). However, in practice, we still found the version that uses the precomputations to run slightly faster, and therefore, present those results in our experiments.
C. Making GETHEURISTIC sub-linear
Each time the naive method evaluates the heuristic for a state, it looks at every node in the E-Graph to find the one that minimizes Eqn. 2, which takes O(|V E^ |) time. We will accelerate this process by using popular nearest neighbor methods. The methods we will be considering are ones which can return exact nearest neighbors. In general,
Algorithm 2 Improved method 1: procedure ONSTARTUP() 2: Build adjacency matrix for E-Graph vertices 3: Run Floyd-Warshall to compute d(u, v)∀u, v ∈ V E 4: procedure AFTERGOAL() 5: HE^ (sgoal) = 0 6: HE^ (s) = mins′∈V E ( εE^ hG(sgoal, s′) + d(s′, s) ) , ∀s ∈ V E 7: Build nearest neighbor data stucture N N 8: procedure GETHEURISTIC(s) 9: v = getN earestN eighbor(N N, s) 10: return εE^ hG(s, v) + HE^ (v)
these methods require the distance function to be a metric F and therefore must satisfy three constraints ∀u, v, w.
- F(u, u) = 0
- F(u, v) = F(v, u)
- F(u, v) ≤ F(v, w) + F(w, u) For a state s′^ in the E-Graph we compute the heuristic upfront when getting the goal, as described in the previous section. These states are then put into the nearest neighbor data structure N N as the following vector.
u ˜ =
[
˜u 1 ˜u 2
]
[
s′ HE^ (s′)
]
During planning, when we call GETHEURISTIC on a state s we need to find a state s′^ such that it minimizes Eqn. 2. To do this we represent s as vector
˜v =
[
˜v 1 ˜v 2
]
[
s 0
]
and do a look up in N N according to the following distance metric:
F(˜u, ˜v) = εE^ hG(˜u 1 , ˜v 1 ) + |˜u 2 − v˜ 2 | (4) This metric satisfies all the required conditions since the two terms both respect the triangle inequality (recall hG^ is consistent). Now that we have a metric, we can employ a wide variety of exact nearest neighbor methods such as VP-tree, GH-tree, and KD-tree. All of these work by recursively splitting the dataset in half based on a pivot element or other criteria. These methods can achieve exact nearest neighbor lookups in logarithmic time under certain contitions regarding the distribution of the data. Though in general they perform faster than linear search mostly on large datasets. At the end of the AFTERGOAL function of Algorithm 2, the nearest neighbor data structure N N is built using the metric F. Then in the GETHEURISTIC function, we simply query N N for the nearest neighbor to s and return the distance.
D. Using optimized KD-trees Some optimized implementations of KD-trees require the distance metric to be of the form:
dist(x, y) = f 1 (x 1 , y 2 ) + f 2 (x 2 , y 2 ) +... + fn(xn, yn) (5)
where x 1 ,... , xn and y 1 ,... , yn are all scalars.
FLANN (fast library for approximate nearest neighbors) is a popular nearest neighbor library that does this [14]. Euclidean distance is supported under this form by squar- ing it and therefore effectively removing the square root (which does not affect the order of nearest neighbors). However, our metric F does not fit the form if hG^ is euclidean distance since we then have:
dist(x, y) =
(x 1 − y 1 )^2 + (x 2 − y 2 )^2... + |xHE − yHE | (6) and the first term couples x 1 , x 2 ,.. .. To support these optimized libraries, we develop an addi- tional method for handling these constraints. We assume that the original heuristic hG^ can be written in the form of Eqn. 5. Then we will build the KD-tree using only hG^ (note this means the KD-tree can be built in ONSTARTUP in Algorithm 3). Then in GETHEURISTIC the nearest neighbor (according to hG) is found using the KD- tree and in a post-processing step, we make a linear pass through the E-Graph nodes to find the one that minimizes hE^. The trick is that we will use the knowledge that we have about the minimal elements according to hG^ to terminate the linear pass early. In practice, the linear pass only looks at a small fraction of the E-Graph nodes before deciding we have the optimal one. More specifically, in function GETHEURISTIC of Algo- rithm 3, we will be maintaining four variables
- hEbest: is the best (lowest) value of hE^ (s) computed so far (from E-Graph vertices inspected)
- HlowE : a lower bound on HE^ among E-Graph vertices we have not yet inspected
- hGlow: a lower bound on hG(s, s′) among E-Graph vertices s′^ we have not yet inspected
- HmaxE : a stopping condition, when HlowE ≥ HmaxE it is no longer possible for any remaing E-Graph nodes to reduce hEbest The first thing in GetHeuristic is to evaluate hE^ (s) using sgoal and initialize hEbest to this value. This also initializes HlowE to 0, the HE^ value of sgoal. We then find the K nearest neighbors in the KD-tree (recall the KD-tree only uses hG) and evaluate hE^ (s) using each, and updating hEbest as needed. The E-Graph nodes returned from the KD-tree are sorted, so nn 1 has the lowest hG^ distance to s of all E-Graph nodes. We now set hGlow using nnK knowing that all remaining E- Graph nodes we inspect must have an hG^ distance to s that is at least as large as the one from nnK. Therefore, it is beneficial to set K > 1 in order get a better lower bound (though not as large as |V E^ | since that would defeat the point). We then set the stopping condition HEmax = hEbest − hGlow, which basically says that for an uninspected E-Graph state b to provide a lower hEbest, HE^ (b) must be lower by at least hGlow to make up for the fact that we know hG(s, b) is at least that large. We prove the correctness of this stopping condition shortly.
Algorithm 3 Using optimized KD-trees 1: procedure ONSTARTUP() 2: Build adjacency matrix for E-Graph vertices 3: Run Floyd-Warshall to compute d(u, v)∀u, v ∈ V E 4: Build KDtree of E-Graph vertices with metric hG 5: procedure AFTERGOAL() 6: HE^ (sgoal) = 0 7: HE^ (s) = mins′∈V E
( εE^ hG(sgoal, s′) + d(s′, s)
) , ∀s ∈ V E 8: Create S a list of EGraph nodes sorted by HE 9: procedure GETHEURISTIC(s) 10: HlowE = 0 11: hEbest = εE^ hG(s, sgoal) 12: nn 1... nnK = getN N (KDtree, K, s) 13: hEbest = min
( hEbest, mini=1...K
( εE^ hG(s, nni) + HE^ (nni)
)) 14: hGlow = εE^ hG(s, nnK ) 15: HmaxE = hEbest − hGlow 16: for i = 1, 2 ,... , |S| do 17: hEtemp = εE^ hG(s, S(i)) + HE^ (S(i)) 18: if hEtemp < hEbest then 19: hEbest = hEtemp 20: HmaxE = hEbest − hGlow 21: HlowE = HE^ (S(i)) 22: if HElow ≥ HmaxE then 23: break 24: if hEbest ≤ εKD (hGlow + HlowE ) then 25: break 26: return hEbest
At this point we start iterating through S which contains all E-Graph nodes sorted by their HE^ values in increasing order (computed in AFTERGOAL). At each iteration we see if we can update hEbest using the next E-Graph node. If so, we also update HmaxE according to its definition, which will lower the stopping condition, making it possible to terminate ever earlier. Then we update the HlowE based on the current E-Graph vertex since we know we are iterating through HE^ values in increasing order, every E-Graph vertex remaining to be inspected has an HE^ at least as large as the vertex we just inspected. We then see if it is possible to terminate early. We have an optimal termination condition HlowE ≥ HmaxE and a bounded suboptimal condition hEbest < εKD (hGlow + HlowE ). For εKD > 1 we can often terminate earlier knowing that we are within a factor of εKD times the optimal hEbest. We will now prove that the two termination conditions are optimal and bounded suboptimal, respectively. Theorem 1 (Optimal termination): When the main loop of GETHEURISTIC(S) terminates due to HlowE ≥ HmaxE , hEbest is minimal. Proof: Assume for sake of contradiction that after terminating the loop using the optimal condition, there is some E-Graph node b∗^ which would have led to hE best∗ lower than the hEbest found using b, the node that provided the current best value. Since we terminated,
HlowE ≥ HmaxE
Since we iterate through the E-Graph nodes in increasing order by HE^ , HE^ (b∗) ≥ HlowE. So,
HE^ (b∗) ≥ HmaxE
follow Algorithm 3). The E-Graph parameters used were ε = 2 and εE^ = 10 providing a suboptimality bound of 20 (the theoretical bound is higher by a factor of εKD for KD-tree methods using the bounded suboptimal termination criteria). While the theoretical bound is quite large, in practice the solutions costs are much lower than this. While computing true optimal paths in such a high-dimensional space (for the purpose of comparison) is very difficult, we estimate the found solutions have costs no worse than 2 or 3 times the optimal cost.
TABLE I PLANNING TIMES USING DIFFERENT HEURISTIC COMPUTATION METHODS
mean median planning mean planning median % time time hE^ time time hE^ time on hE naive 1.09 ± .72 0.39 ± .25 0.19 0.09 47% vp 0.73 ± .50 0.05 ± .03 0.115 0.01 12% gh 0.78 ± .53 0.08 ± .05 0.13 0.03 23% kd(1) 0.84 ± .55 0.15 ± .10 0.14 0.055 31% kd(2) 0.77 ± .54 0.06 ± .03 0.125 0.015 16% kd(3) 0.73 ± .50 0.05 ± .03 0.115 0.01 13%
In Table I we present the results of our experiments. All methods succeeded on 94 of the 100 trials given a 30 second time limit. The rows show the different methods. For the KD- tree, the number afterward indicates the suboptimality bound used (εKD ). Naive, VP-tree, GH-tree, and KD-tree(1) are all optimal and therefore result in the planner expanding the exact same states (mean number of expands was 492) and producing the same path. The KD-tree using suboptimality bounds of 2 and 3, resulted in negligible difference in the number of expansions (less than 1% change; mean number of expands was 491). The table presents the mean and median of total planning time (columns 2 and 4) as well as the mean and median of the planning time that went toward computing heuristics (columns 3 and 5). We also report the average percentage of planning time that went toward computing heuristics (column 6). The confidence intervals shown with the means are at the 95% confidence level. We can see that the VP-tree performs the best in all measures among the optimal methods. Though if the KD-tree method is given a suboptimality bound of 3, it performs just as well. Overall we can see that the amount of planning time that goes toward heuristic computation drops from 47% using the naive method down to 12% when using the VP-tree. Heuristic computation time was reduced by a factor of 8 and on average planning times dropped by 33%. It is important to note that these improvements to heuristic computation time and planning times come at no cost (other than a slightly more complicated implementation and larger memory footprint). The exact same states are expanded, and the same paths are found, just in less time. The methods that perform precomputation (all but naive) took an additional 1.8s for the planner to initialize. This is a one time cost when the planner is launched (all of
the 100 trials could then be run). While this time is not particularly large, it could become worse if the E-Graph were much bigger. Additionally, any time the E-Graph changes (e.g. a new path is added or the environment changes), the precomputations would need to be re-run. If the precompu- tations would have to be run often, we suggest not using the precomputations and instead running the Dijkstra search in AFTERGOAL with an unsorted array (Section IV-B.1). We found that this makes all methods slower by 0.02s but it avoids the need for precomputation. VI. CONCLUSIONS In this work we presented a more efficient way to compute general heuristics for E-Graphs, especially for those which are not computed using dynamic programming. Planning with Experience Graphs can now be done more efficiently with heuristics like euclidean distance, which are ubiquitous in robot motion planning. Our improvements make use of nearest neighbor methods which have also been used to speed up sampling-based motion planners. Future work is to experiment with other heuristics like the Dubins car model.
REFERENCES [1] M. Phillips, B. J. Cohen, S. Chitta, and M. Likhachev, “E-graphs: Bootstrapping planning with experience graphs,” in Robotics: Science and Systems, 2012. [2] M. Phillips, V. Hwang, S. Chitta, and M. Likhachev, “Learning to plan for constrained manipulation from demonstrations,” in Robotics: Science and Systems, 2013. [3] J. Bruce and M. Veloso, “Real-time randomized path planning for robot navigation,” in IEEE/RSJ International Conference on Intelligent Robots and Systems, 2002. [4] N. Jetchev and M. Toussaint, “Trajectory prediction: Learning to map situations to robot trajectories,” in IEEE International Conference on Robotics and Automation, 2010. [5] X. Jiang and M. Kallmann, “Learning humanoid reaching tasks in dy- namic environments,” in IEEE International Conference on Intelligent Robots and Systems, 2007. [6] D. Berenson, P. Abbeel, and K. Goldberg, “A robot path planning framework that learns from experience,” in ICRA, 2012. [7] J. H. Friedman, J. L. Bentley, and R. A. Finkel, “An algorithm for finding best matches in logarithmic expected time,” ACM Trans. Math. Softw., vol. 3, no. 3, pp. 209–226, Sept. 1977. [8] P. N. Yianilos, “Data structures and algorithms for nearest neighbor search in general metric spaces,” in Proceedings of the Fourth Annual ACM-SIAM Symposium on Discrete Algorithms, ser. SODA ’93, 1993, pp. 311–321. [9] J. K. Uhlmann, “Satisfying general proximity / similarity queries with metric trees,” Information Processing Letters, vol. 40, no. 4, pp. 175
- 179, 1991. [10] A. Gionis, P. Indyk, and R. Motwani, “Similarity search in high dimensions via hashing,” in Proceedings of the 25th International Conference on Very Large Data Bases, ser. VLDB ’99, 1999, pp. 518–529. [11] J. J. K. Jr. and S. M. LaValle, “Rrt-connect: An efficient approach to single-query path planning,” in ICRA, 2000. [12] L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Prob- abilistic roadmaps for path planning in high-dimensional configuration spaces,” IEEE Transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996. [13] A. Atramentov and S. LaValle, “Efficient nearest neighbor searching for motion planning,” in Robotics and Automation, 2002. Proceedings. ICRA ’02. IEEE International Conference on, vol. 1, 2002, pp. 632– 637 vol.1. [14] M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration,” in International Conference on Computer Vision Theory and Application VISSAPP’09). INSTICC Press, 2009, pp. 331–340.
