Efficient Nearest-Neighbor Search for Dynamical
Systems with Nonholonomic Constraints
Valerio Varricchio, Brian Paden, Dmitry Yershov, and Emilio Frazzoli

arXiv:1709.07610v1 [cs.CG] 22 Sep 2017

Massachusetts Institute of Technology
Abstract. Nearest-neighbor search dominates the asymptotic complexity of sampling-based motion planning algorithms and is often addressed
with k-d tree data structures. While it is generally believed that the expected complexity of nearest-neighbor queries is O(log(N )) in the size of
the tree, this paper reveals that when a classic k-d tree approach is used
with sub-Riemannian metrics, the expected query complexity is in fact
Θ(N p log(N )) for a number p ∈ [0, 1) determined by the degree of nonholonomy of the system. These metrics arise naturally in nonholonomic
mechanical systems, including classic wheeled robot models. To address
this negative result, we propose novel k-d tree build and query strategies
tailored to sub-Riemannian metrics and demonstrate significant improvements in the running time of nearest-neighbor search queries.

1

Introduction

Sampling-Based algorithms such as Probabilistic Roadmaps (PRM) [9], Rapidly
exploring Random Trees (RRT) [11] and their asymptotically optimal variants
(PRM∗ , RRT∗ ) [7] are widely used in motion planning. These algorithms build a
random graph of motions between points on the robot’s configuration manifold.
During the graph expansion, nearest-neighbor search is used to limit the
computation to regions of the graph close to the new configurations and it is
shown to dominate the asymptotic complexity of randomized planners. The notion of closeness appropriate for motion planning is induced by the length of the
shortest paths between configurations, or in general, by the minimum cost of
controlling a system between states.
As compared to exhaustive linear search, efficient algorithms with reduced
complexity have been studied in computational geometry and their use in motion
planning has been highlighted as a factor of dramatic performance improvement
[12]. Among a variety of approaches, k-d trees [1] are ideal due to their remarkable efficiency in low-dimensional spaces, typical of motion planning problems.
Classic k-d trees are shown to have logarithmic average case complexity for
distance functions strongly equivalent to L-p metrics [4]. While this requirement
is reasonable for many applications, it does not apply to distances induced by
the shortest paths of nonholonomic systems. Therefore, identifying nearest neighbors in the sense of a generic control cost remains an important open problem
in sampling-based motion planning. In both literature and practical implementations, when searching for neighbors, randomized planners resort to distance
functions that only approximate the true control cost. Arguably, the most common choices are Euclidean distance or quadratic forms [12]. This ad-hoc approach
1

2. GEOMETRY OF NONHOLONOMIC SYSTEMS
can significantly slow down the convergence rate of sampling-based algorithms
if an inappropriate metric is selected.
A number of heuristics have been proposed to resolve this issue, such as
the reachability and utility guided RRTs [17,3] which bias the tree expansion
towards promising regions of the configuration space. Other approaches use the
cost of linear quadratic regulators [5] and learning techniques [15,2]. In specific
examples, these heuristics can significantly reduce the negative effects of finding
nearest neighbors according to a metric inconsistent with the minimum cost path
between configurations. However, the underlying inconsistency is not directly
addressed. In contrast, a strong motivation to address it comes from recent
research [8], which shows that a major speedup of sampling-based kynodinamic
planners can be achieved by considering nonholonomy at the stage of nearestneighbor search.
Specialized k-d tree algorithms have been proposed to account for nonstandard topologies of some configuration manifolds [10,19,6].
However, no effort is known towards generalizing such algorithms to differential constraints.
In this work, we investigate the use of k-d trees for exact nearest-neighbor
search in the presence of differential constraints. The main contributions can be
summarized as follows: (i) we derive the expected complexity of nearest-neighbor
queries with k-d trees built according to classic techniques and reveal that it is
super-logarithmic (ii) we propose novel k-d tree build and query procedures tailored to sub-Riemannian metrics (iii) we provide numerical trials which verify
our theoretical analysis and demonstrate the improvement afforded by the proposed algorithms as compared with popular open source software libraries, such
as FLANN [14] and OMPL [18].
In Section 2, we review background material on sub-Riemannian geometries
and show connections with nonholonomic systems, providing asymptotic bounds
to their reachable sets. Based on these bounds, in Section 3 we propose a query
procedure specialized for nonholonomic systems, after a brief review of the k-d
tree algorithm. In Section 4, we study the expected complexity of m-nearestneighbor queries on a classic k-d tree with sub-Riemannian metrics. Inspired
by this analysis, in Section 5 we propose a novel incremental build procedure.
Finally, in Section 6 we show positive experimental results for a nonholonomic
mobile robot, which confirm our theoretical predictions and the effectiveness of
the proposed algorithms.

2

Geometry of Nonholonomic Systems

Nonholonomic constraints are frequently encountered in robotics and describe
mechanical systems whose local mobility is, in some sense, limited. Basic concepts from differential geometry, reviewed below, are used to clarify these limitations and discuss them quantitatively.
2

2. GEOMETRY OF NONHOLONOMIC SYSTEMS
2.1

Elements of Differential Geometry

A subset M of Rn is a smooth k-dimensional manifold if for all p ∈ M there
exists a neighborhood V of M such that V ∩ M is diffeomorphic to an open
subset of Rk . A vector v ∈ Rn is said to be tangent to M at point p ∈ M if
there exists a smooth curve γ : [0, 1] → M, such that γ̇(0) = v and γ(0) = p.
The tangent space of M at p, denoted Tp M, is the subspace of vectors tangent
to M at p. A map Y : M → Rn is a vector field on M if for each p ∈ M,
Y (p) ∈ Tp M. A smooth euclidean vector bundle of rank r over M is defined as
a set E ⊂ M × Rl such that the set Ep : v ∈ Rl , (p, v) ∈ E is an r-dimensional
vector space for all p ∈ M. Ep is called the fiber of bundle E at point p. Any set
of h linearly independent smooth vector fields Y1 , . . . Yh such that for all p ∈ M,
span(Y1 (p), ..., Yh (p)) = Ep is called a basis of E and Yi are called generator
vector fields of E. The tangent bundle of M is defined as the vector bundle
T M whose fiber at each point p is the tangent space at that point, Tp M. A
distribution H on a manifold is a subbundle of the tangent bundle, i.e., a vector
bundle such that its fiber Hp at all points is a vector subspace of Tp M.
Connection with nonholonomic systems. Consider a nonholonomic system described by the differential constraint:
ẋ(t) = f (x(t), u(t)),

(1)

where the configuration x(t) and control u(t) belong to the smooth manifolds
X and U respectively. Each ui ∈ U defines a vector field gi (z) = f (z, ui ) on
X . Therefore, for a fixed configuration z, f (z, u) has values in a vector space
Hz := Span({gi (z)}). In other words, the dynamics described by equation (1)
define a distribution H on the configuration manifold.
Throughout the paper, we will make use of the Reeds-Shepp vehicle [16] as
an illustrative example.
Example (Configuration manifold of a Reeds-Shepp vehicle). The configuration
manifold for this vehicle model is X = SE(2) with coordinates x = (x1 , x2 , x3 ).
The mobility of the system is given by ẋ1 = u1 cos(x3 ), ẋ2 = u1 sin(x3 ), ẋ3 =
u1 u2 , where u1 , u2 ∈ [−1, 1]. The inputs u1 = (1, 0) and u2 = (1, 1) define the
vector fields g1 (x) = (cos(x3 ), sin(x3 ), 0) and g2 (x) = (cos(x3 ), sin(x3 ), 1). At
each z ∈ SE(2) the fiber of the Reeds-Shepp distribution HRS is Span{g1 (z),
g2 (z)}. Let fˆ(x) = (cos(x3 ), sin(x3 ), 0), ˆl(x) = (− sin(x3 ), cos(x3 ), 0) and θ̂(x) =
(0, 0, 1). These vector fields indicate the body frame of the vehicle, i.e., its front fˆ,
lateral ˆl and rotation θ̂ axes. The Reeds-Shepp distribution is then equivalently
defined by the fibers HzRS = Span{fˆ(z), θ̂(z)}.
2.2

Distances in a Sub-Riemannian Geometry

A sub-Riemannian geometry G on a manifold M is a tuple G = (M, H, h·, ·iH ),
where H is a distribution on M whose fibers at all points p are equipped with
the inner product h·, ·iH : Hp × Hp → R. The distribution H is referred to as the
3

2. GEOMETRY OF NONHOLONOMIC SYSTEMS
horizontal distribution. A smooth curve γ : [0, 1] → M is said to be horizontal
if γ̇(t) ∈ Hγ(t) for all t ∈ [0, 1].
R1p
The length of smooth curves is defined `G (γ) := 0 hγ̇(t), γ̇(t)iH dt. If
Γab denotes the set of horizontal curves between a, b ∈ M, then dG (a, b) :=
inf γ∈Γab `G (γ) is a sub-Riemannian metric defined by the geometry. The ball
centered at p of radius r with respect to the metric dG is denoted B(p, r).
In control theory, the attainable set A(x0 , t) is the subset of X such that
for each p ∈ A(x0 , t) there exists a u : [0, τ ] → U for which the solution to (1)
through x(0) = x0 satisfies x(τ ) = p for some τ ≤ t. Solutions to (1) are simply
horizontal curves, so the attainable set A(x0 , t) is equivalent to the ball B(x0 , t)
defined by the sub-Riemannian metric. This equivalence holds for systems with
time-reversal symmetry, under the controllability condition stated in Theorem 1.
Example (Sub-Riemannian geometry of a Reeds-Shepp vehicle). The geometry
associated with the Reeds-Shepp vehicle is G RS = (SE(2), HRS , h·, ·iRS ), with
the standard inner product hv, wiRS = v T w. Horizontal curves for this geometry
are feasible paths satisfying the differential constraints. Geodesics correspond
to minimum-time paths between two configurations and are known in closed
form [16].

2.3

Iterated Lie Brackets and the Ball-box Theorem

A system is said to be controllable if any pair of configurations can be connected
by a feasible (horizontal) path. Determining controllability of a nonholonomic
system is nontrivial. For example, the Reeds-Shepp vehicle cannot move directly
in the lateral direction, but intuition suggests that an appropriate sequence of
motions can result in a lateral displacement (e.g., in parallel parking). Chow’s
Theorem and the Ball-box Theorem, reviewed below, are fundamental tools related to the controllability and the reachable sets of nonholonomic systems.
The Lie derivative of a vector field Y at p ∈ M in the direction v ∈ Tp M is
d
defined as dY (p)v = dt
Y (γ(t)) t=0 , where γ is a smooth curve starting in p =
γ(0) with velocity v = γ̇(0). Given two vector fields Y1 , Y2 on M, the Lie bracket
[Y1 , Y2 ] is a vector field on M defined as [Y1 , Y2 ](p) = dY2 (p)Y1 (p)−dY1 (p)Y2 (p).
From the horizontal distribution H, one can construct a sequence of distributions by iterating the Lie brackets of the generating vector fields, Y1 , Y2 . . . Yh ,
with h ≤ k. Recursively, this sequence is defined as: H1 = H, Hi+1 = Hi ∪
[H, Hi ], where [H, Hi ] denotes the distribution given by the Lie brackets of each
generating vector field of H with those of Hi . Note that the Lie bracket of two vector fields can be linearly independent from the original fields, hence Hi ⊆ Hi+1 .
The Lie hull [13], denoted Lie(H), is the limit of the sequence Hi as i → ∞. A
distribution H is said to be bracket-generating if Lie(H) = T M.
Theorem 1. (Chow’s Theorem [13, p. 44] ). If Lie(H) = T M on a connected manifold M, then any a, b ∈ M can be joined by a horizontal curve.
4

2. GEOMETRY OF NONHOLONOMIC SYSTEMS
Example (Lie hull of a Reeds-Shepp vehicle). Consider the Lie hull of HRS generated by {fˆ(x), θ̂(x)}. For every x ∈ SE(2), these vectors span a two-dimensional
subspace of Tx (SE(2)). The first order Lie bracket is given by




d ˆ
d
[fˆ, θ̂](x) =
f (x) θ̂(x) −
θ̂(x) fˆ(x) = (− sin(x3 ), cos(x3 ), 0) . (2)
dx
dx
This coincides with the body frame lateral axis ˆl(x) of the vehicle. Therefore,
the second order distribution Hx2 = Span{fˆ(x), θ̂(x), [fˆ, θ̂](x)} spans the tangent bundle of SE(2), and thus the Lie hull is obtained in the second step. By
Theorem 1, there is a feasible motion connecting any two configurations which
is consistent with one’s intuition about the wheeled robot.
Above, we have shown that from a basis Y1 . . . Yh of a bracket-generating
distribution H, one can define a basis y1 , . . . yk of T M. Specifically yi = Yi
for i ≤ h, while the remaining d − h fields are obtained with Lie brackets.
The vector fields {yi } are called privileged directions [13]. Define the weight wi
of the privileged direction yi as the smallest order of Lie brackets required to
generate yi from the original basis. More formally, wi is such that yi ∈
/ Hwi −1
wi
and yi ∈ H for all i. The weighted box at p of size , weights w ∈ Nk>0 and
multipliers µ ∈ Rk>0 is defined as:
Boxw,µ (p, ) : {y ∈ Rn : |hy − p, yi (p)iRn | < µi wi for all i ∈ [1, k]}.

(3)

Theorem 2 (The ball-box Theorem). Let H be a distribution on a manifold
M satisfying the assumptions of Chow’s Theorem. Then, there exist constants
0 ∈ R>0 and c, C ∈ Rk>0 such that for all  < 0 and for all p ∈ M:
w,c
(p, ) ⊂ B(p, ) ⊂ Boxw,C (p, ) := Boxw
Boxw
o (p, ),
i (p, ) := Box

(4)

w
where Boxw
i and Boxo are referred to as the inner and outer bounding boxes for
the sub-Riemannian ball B, respectively.

Example (Ball-Box Theorem visualized for a Reeds-Shepp vehicle). From the Lie
hull construction in the previous example, we know that fˆ(x), θ̂(x) ∈ H1 so the
corresponding weights are wfˆ = wθ̂ = 1. Conversely, ˆl(x) first appears in H2 , so
its weight is wl̂ = 2. Theorem 2 states the existence of inner and outer bounding
boxes for reachable sets as t → 0 and predicts the infinitesimal order of each
side of these boxes, as shown in figure 1. Higher order Lie brackets correspond
to sides that approach zero at a faster asymptotic rate. The longitudinal and
angular sides — along fˆ and θ̂ — scale with Θ(t), while the lateral one — along
ˆl — with Θ(t2 ). Therefore, both boxes become increasingly elongated along fˆ
and θ̂ and flattened along ˆl as t  0. Intuitively, this geometric feature of boxes
reflects the well known fact that a small lateral displacement of a car requires
more time than an equivalent longitudinal one.
5

3. THE K-D TREE DATA STRUCTURE
2 Cl̂ t2

2 Cfˆ t
2 cfˆ t

1

2 Cθ̂ t

0.6

-0.2

-0.2

fˆ(q )
q

0

-0.5

-1

0

0.2

0.5

t = 0.7

-1

-1

0

q

-0.4

2

1

0

-0.5

-0.6

θ̂(q )

Boxw
i ( q , t)

B (q , t)

2

0.5

0.2

l̂ (q )

Boxw
o (q , t)

2 cl̂ t2

0
0.4

1

0.5

0

-0.5

t=1

0
-2

-2
-1

0

1

2

1

0

-1

-2

t=2

-2

-2

0

0
2

2

t=3

Fig. 1. Reachable sets and bounding boxes for a Reeds-Shepp vehicle around a configuration q and for different values of t. The lengths of the box sides are highlighted.
For both the inner and outer boxes, as t → 0, the sides along the front fˆ and heading
θ̂ axes are linear in t, while the side along the lateral axis ˆ
l is quadratic in t.

For the Reeds-Shepp vehicle, the sides of both boxes can be computed explicitly with geometric considerations on special manoeuvers that maximize
or minimize the displacement along eachp
axis. In particular, we get the values
Cfˆ = Cθ̂ = cθ̂ = 1, Cl̂ = 1/2, cfˆ = 3/2 − 1, cl̂ = 1/8.

3

The k-d tree Data Structure

A k-d tree T is a binary tree organizing a finite subset X ⊂ M, called a database,
with its elements xi ∈ X called data points. We would like to find the m points in
X closest to a given query point q on the manifold. Each point xi ∈ X is put in
relation with a normal vector ni ∈ Rn . Together, the pair vi = (xi , ni ) defines a
vertex of the binary tree. The set of vertices is denoted with V. A vertex defines
a partition of Rn into two halfspaces, referred to as the positive and negative
halfspace, and respectively described algebraically:
h+ (xi , ni ) := {x ∈ Rn : hni , xiRn > hni , xi iRn } ,
h− (xi , ni ) := {x ∈ Rn : hni , xiRn ≤ hni , xi iRn } .

(5)

An edge is an ordered pair of vertices e = (vi , vj ). A binary tree is defined as
T := (V, E − , E + ), with V set of vertices, E − set of left edges and E + set of right
edges. Given one edge e = (vi , vj ), vertex vj is referred to as the left child of vi if
e ∈ E − , or the right child if e ∈ E + . Let parent(vi ∈ V) = vj ∈ V s.t. (vi , vj ) ∈
E − ∪ E + . By convention, parent(vi ) = ∅ if such vj does not exist and vi is called
the root of T , denoted vi = root(T ). Let child(vi ∈ V, s ∈ {−, +}) = vj ∈ V
s.t. (vi , vj ) ∈ E s , or otherwise ∅ if such vj does not exist.
The fundamental property of k-d trees is that left children belong to the negative halfspace defined by their parents and right children to the positive halfspace.
Recursively, a vertex belongs to the parent halfspaces of all its ancestors. As a
result, a k-d tree defines a partition of M into non-overlapping polyhedra, called
buckets [4], that cover the entire manifold. In the sequel, we let BT denote the
set of buckets for a given k-d tree T .
6

3. THE K-D TREE DATA STRUCTURE
Buckets are associated with the leaves of T and with parents of only-child
leaves (i.e., leaves without a sibling). For any given point q ∈ M, we denote with
bq the unique bucket containing q.
3.1

m-nearest-neighbor query algorithm

The computational efficiency afforded by the algorithm comes from the following
observation: let q be the query point and suppose that among the distance evaluations computed thus far, the m closest points to q are within a distance dm
away. Now consider a vertex (x, n) and suppose the query point q is contained in
h+ (x, n) and B(q, dm ) ⊂ h+ (x, n). The data points represented by the sibling of
(x, n) and all of its descendants are contained in h− (r, c) and are therefore at a
distance greater than dm . Thus, the corresponding subtree can be omitted from
the search. The following primitives will be used to define the query procedure
presented in Algorithm 1.
Side of hyperplane. A procedure sideOf(x, p, n) → {−, +}, with x, p ∈ M,
n ∈ Rn . Returns + iff x ∈ h+ (p, n) and − otherwise. For convenience, define
opposite(+) = − and vice-versa.
Queue. For an m-nearest-neighbor query, the algorithm maintains a Bounded
Priority Queue, Q of size m to collect the results. The queue can be thought of
as a sequence Q = [q1 , q2 , . . . qm ], where each element qi is defined as a distancevertex pair qi : (di , vi ), di ∈ R≥0 , vi ∈ V. The property d1 ≤ d2 ≤ · · · ≤ dm is
an invariant of the data structure. When an element (dnew , nnew ) is inserted in
the queue, if dnew < dm , then qm is discarded and the indices of the remaining
elements are rearranged to maintain the order.
Ball-Hyperplane Intersection. A procedure that determines whether a ball
and a hyperplane intersect. Precisely, ballHyperplane(x, R, p, n), with x ∈ M
and R ≥ 0, returns true if B(x, R) ∩ h(p, n) 6= ∅. Note that it does not need to
return false otherwise.
In the classic analysis of k-d trees [4, eq. 14] the distance is assumed to be a
sum of component-wise terms, called coordinate distance functions. Namely:
P

k
d(x, y) = F
(6)
i=1 di (xi , yi ) .
When this holds, e.g., for L-p metrics, the ball-hyperplane intersection procedure
reduces to the direct comparison of two numbers.
However, this property does not hold for other notions of distance and in
general sub-Riemannian balls can have nontrivial shapes. For these cases, the
procedure can be implemented by checking that Vo (x, R) ∩ h(p, n) 6= ∅, where
Vo is an “outer set” with a convenient geometry, i.e., a set such that Vo (x, R) ⊇
B(x, R) for all x ∈ M, R > 0 and such that intersections with hyperplanes are
easy to verify. Clearly, vol(Vo (x, R))/vol(B(x, R)) ≥ 1 and it is desirable that
this ratio stays as small as possible.
A crucial consequence of Theorem 2 is that the choice Vo (x, R) = Boxw
o (x, R)
offers optimal asymptotical behavior, since the volume of the outer set scales with
7

3. THE K-D TREE DATA STRUCTURE
the smallest possible order, namely: vol(Boxw
o (x, R)) ∈ Θ[vol(B(x, R))]. This fact
is fundamental to devise efficient query algorithms for nonholonomic systems.
Example (Ball-hyperplane intersection for a Reeds-Shepp vehicle). The reachable sets shown in figure 1 have a nontrivial geometry. Let us consider two
possible implementations of the ball-hyperplane intersection procedure:
1. Euclidean bound (EB). Define the set C(p, R) = {x ∈ SE(2) |(x1 − p1 )2 +
(x2 − p2 )2 ≤ R2 , |x3 − p3 | ≤ 2R}, i.e., a cylinder in the configuration space
with axis along θ̂. Since the Euclidean distance ka − bk is a lower bound
to dRS (a, b), then B(x, R) ⊂ C(x, R) and ballHyperplane can be correctly
implemented by checking C(x, R) ∩ h(p, n) 6= ∅
2. Outer Box Bound (BB). In this case, the procedure checks Boxw
o (x, R) ∩
h(p, n) 6= ∅. A simple implementation is to test whether all vertices of the
box (8 in this case) are on the same halfspace defined by h(p, n).
While Euclidean bounds (EB) are a simple choice, the Outer Box Bound (BB)
method approximates the sub-Riemannian ball tightly. In fact, as R → 0,
4
vol(C(p, R)) ∈ Θ(R3 ), while vol(Boxw
o (p, R)) ∈ Θ(R ) and therefore the volume of the cylinder tends to be infinitely larger than the volume of the outer
box. As a result, method (BB) yields an asymptotically unbounded speedup in
the algorithm compared to (EB), as confirmed by our experimental results in
Section 6. In addition, any other implementation of the procedure will at most
provide a constant factor improvement with respect to method (BB).
Algorithm 1: k-d tree query
1
2
3
4
5
6
7
8
9

define query (q ∈ M, T = (V, E − , E + )):
Q ←− {q1:m = (∅, ∞)} ;
// initialize queue
define querySubtree (q ∈ M, vi = (xi , ni ) ∈ V):
if vi = ∅ then return;
// caller reached leaf
s ←− sideOf (q, xi , ni );
querySubtree (q, child(vi , s)) ;
// descend to child containing q
Q ←− Q ∪ {(dist(q, xi ), vi )} ;
// add vertex to the queue
if ballHyperplane (q, dk , xi , ni ) then
// check for intersections
querySubtree (q, child(vi , opposite(s))) ;
// visit sibling

11

querySubtree (q, root (T )) ;
return Q;

3.2

k-d tree build algorithms

10

// start recursion from root

The performance of nearest-neighbor queries is heavily dependant on how the
k-d tree is constructed. In the sequel, we describe two popular approaches to
construct k-d trees: batch (or static) and incremental (or dynamic).
In the batch algorithm, all data points are processed at once and statistics
of the database determine the vertices of the tree at each depth. This algorithm
guarantees a balanced tree, but the insertion of new points is not possible without
8

3. THE K-D TREE DATA STRUCTURE
re-building the tree. Conversely, in the incremental version, the tree is updated
on the fly as points are inserted, however, the tree balance guarantee is lost.
Both algorithms find applications in motion planning: the batch version can
be used to efficiently build roadmaps for off-line, multiple-query techniques such
as PRMs, while the incremental is suitable for anytime algorithms such as RRTs.
Batch k-d tree build algorithm
The following primitive procedures will be used to describe the batch construction of k-d trees, defined in Algorithm 2. The range of a set S along direction dˆ
ˆ − inf x∈S hx, di.
ˆ
is defined as rng dˆ(S) = supx∈S hx, di
Maximum range. Let maxRange : 2X → Rn . Given a subset D ∈ 2X of the
data points, maxRange determines a direction l along which the range of data
is maximal. We consider the formulation in [4], where l is chosen among the
cardinal directions {êi }i∈[0,...k−1] , so that maxRange(D) = arg maxêi [rng êi (D)].
Median element. Let median : 2X × Rn → D. Given a subset D ∈ 2X of the
data points and a direction l, median returns the median element of D along
direction l.
Algorithm 2: Batch k-d tree build
1
2
3
4
5
6
7
8
9
10
11

define build (X):
V←−∅; E − ←−∅; E + ←−∅ ;
// Initialize
define buildSubtree (D ∈ 2X , vi = (xi , ni ) ∈ V, s ∈ {−, +}):
if D = ∅ then return;
// caller reached
nnew ←− maxRange (D); xnew ←− median (D, nnew ) ;
vnew ←− (xnew , nnew ) ; V ←− V ∪ vnew ;
if s 6= ∅ then E s ←− E s ∪ (vi , vnew ) ;
L ←− D ∩ h− (xnew , nnew ); R ←− D\(L ∪ xnew );
buildSubtree (L, vnew , -); buildSubtree (R, vnew , +);

tree
leaf

buildSubtree (X, ∅, ∅);
return T = (V, E − , E + );

Incremental k-d tree build algorithm
A key operation to build a k-d tree incrementally is to pick splitting hyperplanes
on the fly as new data points are inserted. This is achieved with a splitting
sequence, which we define as:
Splitting sequence. A map ZM : N≥0 × M → Rn that, given an integer d ≥ 0
and a point p ∈ M, returns a normal vector n associated with them.
When dealing with k-dimensional data, one usually chooses a basis of cardinal
axes {êi }i∈[0...k−1] . The normal of the hyperplane associated with a vertex v is
typically picked by cycling through the cardinal axes based on the depth of v in
the binary tree. Let “a mod b” denote the remainder of the division of integer a
by integer b. Then, the classic splitting (CS) sequence is defined as:
classic
ZM
(d, x) = ê[d mod k] .

9

(7)

4. COMPLEXITY ANALYSIS
Algorithm 3: Incremental k-d tree build.
Insert in k-d tree T = (V, E + , E − ), start recursion with insert(xnew , root(T )).
1
2
3
4
5
6
7
8
9
10

4

define insert (xnew ∈ M, vi = (xi , ni ) ∈ V):
if V = ∅ then
nnew ←− ZM (0, xnew ); V←−(xnew , nnew ) ;
return

// if tree is empty...
// ...add root

side ←− sideOf (xnew , xi , ni ) ; c ←− child (vi , side) ;
if c = ∅ then
nnew ←− ZM (depth(vi ) + 1, xnew ) ; vnew ←− (xnew , nnew );
V ←− V ∪ vnew ; E side ←− E side ∪ (vi , vnew ) ;
return
insert (xnew , c) ;

Complexity Analysis

In this section, we discuss the expected asymptotic complexity of m-nearestneighbor queries in a k-d tree built according to Algorithm 2. The complexity of
the nearest-neighbor query procedure (Algorithm 1) is measured in terms of the
number nv of vertices examined. Let B(q, dm ) be the ball containing the m nearest neighbors to q. For the soundness of the algorithm, all the vertices contained
in this ball must be examined, and therefore, all the buckets overlapping with
it. As we recall from Section 3, buckets are associated with leaves, and therefore
the algorithm will visit as many leaves as the number of such buckets, formally:
nl ∈ Θ(|β|), where β := {b ∈ BT | b ∩ B(q, dm ) 6= ∅} and nl denotes the number
of leaves visited by the algorithm.
If the asymptotic order of visited leaves is known, proofs rely on the following
fact: since the batch algorithm guarantees a balanced tree, descending into nl
leaves from the root requires visiting nv ∈ Θ(nl log N ) vertices, where N is the
cardinality of the database X. Thus the expected query complexity is given by:
E(nv ) ∈ Θ[E(nl ) log N ].

(8)

Lemma 1 reviews the seminal result, originally from [4], that the expected
asymptotic complexity for a Minkowski (i.e., L-p) distance is logarithmic. Our
main theoretical contribution is the negative result described in Theorem 3,
where we reveal that the expected complexity for sub-Riemannian metrics is
in fact super-logarithmic. In the sequel, assume that data points are randomly
sampled from M with probability distribution p(x).
Lemma 1 ([4]). If the distance function used for the nearest-neighbor query is
induced by a p-norm, then the expected complexity of the nearest-neighbor search
on a k-d tree built with Algorithm 2 is O(log(N )) as N → ∞.
For the detailed proof, we suggest reading the original work by Friedman et
al. [4], while here we report key facts used in our next result. In [4, pg. 214], it
10

4. COMPLEXITY ANALYSIS
is shown that:
E[vol(B(q, dm ))] ≈

m
1
,
(N + 1) p(q)

(9)

with no assumptions on the metric and the probability distribution p(x).
In a batch-built k-d tree, the expected asymptotic shape of the bucket bq
containing the query point q is assumed hyper-cubical. From equation (9) with
1
. Thus, hyper-cubes in a neighborhood of q have
m = 1, E(vol(bq )) ≈ (N1+1) p(q)
sides of expected length E(lq ) = [(N + 1)p(q)]−1/k . Using these facts, it is then
shown that the expected number of visited leaves is asymptotically constant
with the size of the database, E(nl ) ∈ Θ(1) and therefore the average query
complexity is logarithmic, as per equation (8).

Theorem 3. Let the distance function used for the nearest-neighbor query be
a sub-Riemannian metric on a smooth connected manifold M with a bracketgenerating horizontal distribution of dimension h and weights {wi }. Then the
expected complexity of the nearest-neighbor query on a k-d tree built with Algorithm 2 is Θ(N p log(N )) as N → ∞, where the expression for p is:

X 1
X
wi
p=
−
, with W :=
wi .
(10)
k W
i
wi ≤W/k

Proof. For N large enough, Theorem 2 ensures the existence of inner and outer
bounding boxes to the sub-Riemannian ball B(q, dm ).
From equations (4) and (9), we get:
E(dm )W ·

k
Y
i=1

ci ≤

k
Y
1
1
≤ E(dm )W ·
Ci ,
N + 1 p(q)
i=1

(11)

P
where W := i wi . Therefore, the expected distance to the m-th nearest neighbor scales asymptotically as E(dm ) ∈ Θ(N −1/W
p ) for N → ∞. Recall that a
bucket bq has sides of expected length lq = k 1/ [(N + 1)p(q)] or equivalently,
lq ∈ Θ(N −1/k ). We are now interested in the asymptotic order of the number
of buckets overlapping with a weighted box of size dm . Let nh (d, lq ) the number
of hypercubes intersected by a segment
of Rk
l √ of lengthm d embedded in a grid
√
−1
with side lq . It can be shown that ( k) (d/lq ) ≤ nh (d, lq ) ≤ k + kdd/lq e.
Then, asymptotically, nh (d, lq ) ∈ Θ(dd/lq e) as (d, lq ) → 0. Since a box has k
orthogonal sides, each with expected length E(dm )wi , the expected number of
visited buckets, is:
!
!
k 
k l
m
Y
Y
wi
1
E(dm )wi
.
(12)
E(nl ) ∈ Θ
=Θ
N k− W
l
q
i=1
i=1

In the latter product, only the factors with exponent > 0 contribute to the
complexity of the algorithm, while the other terms tend to 1, as N → ∞. In
other words, the query complexity is determined only by low order Lie brackets
11

5. THE LIE SPLITTING STRATEGY
up until wi ≤ W/k. In fact, along the direction of higher order Lie brackets,
reachable sets shrink to zero faster than the side of a k-d tree bucket. Following
up from equation (12), we get:



X 1
Y
w
1
wi
i
E(nl ) ∈ Θ 
N k − W  = Θ (N p ) , p =
−
.
(13)
k W
wi ≤W/k

wi ≤W/k

From equation (8) it follows that the expected complexity of the query algorithm
is given by E(nv ) ∈ Θ(N p log N ) as N → ∞.
t
u
Remark. For holonomic systems, the query complexity is logarithmic, in accordance with Lemma 1. In fact, for holonomic systems, w1 = w2 = · · · = wk = 1,
then W = k and p = 0 from equation (10).
Remark. The query complexity is always between logarithmic and linear. Since
W ≥ k by definition, for all i such that wi ≤ W/k, one can state 0 < wi /W ≤
1
i
1/k ≤ 1. It follows that 0 ≤ k1 − w
W < k . Then p can be bounded with:

X 1
X 1
wi
1
<
≤ k · = 1 ⇒ p ∈ [0, 1).
0≤p=
−
(14)
k W
k
k
wi ≤W/k

wi ≤W/k

Example. For the Reeds-Shepp car, k = dim[SE(2)] = 3, wfˆ = wθ̂ = 1, wl̂ = 2
and W = wfˆ + wθ̂ + wl̂ = 4. Only wfˆ and wθ̂ satisfy wi ≤ W/k, therefore,
according
to 
Theorem 3, the expected query complexity of a batch kd-tree is
√
6
Θ
N log N . We confirm this prediction with experiments in section 6.

5

The Lie splitting strategy

The basic working principle of k-d trees is the ability to discard subtrees without
loss of information during a query. For each visited node v = (x, n), the algorithm checks whether the current biggest ball in the queue, B(q, dm ) intersects
h(x, n). If such an intersection exists, the algorithm visits both children of v,
otherwise one child is discarded and so is the entire subtree rooted in it. To
reduce complexity, it is thus desirable that during query, a k-d tree presents as
few ball-hyperplane intersections as possible.
In the classic splitting strategy, hyperplanes are chosen cycling through a
globally defined set of cardinal axes. However, we have shown that nonholonomic
systems have a set of locally-defined privileged axes and that the reachable sets
have different infinitesimal orders along each of them. When the metric comes
from a nonholonomic system, the hyperplanes in a classic k-d tree are not aligned
with reachable sets and the buckets have different asymptotic properties than
reachable sets. This can make intersections frequent, as shown in figure 2(a).
Ideally, to minimize the number of ball-hyperplane intersections, the buckets
in a k-d tree should approximate the bounding boxes for the reachable sets of the
12

5. THE LIE SPLITTING STRATEGY

θ
y

y

x

(a) Classic k-d tree construction

θ
x

(b) Lie k-d tree splitting strategy

Fig. 2. Qualitative comparison of a classic k-d tree with its counterpart built with the
proposed Lie splitting strategy. For nonholonomic systems, reachable sets (red, blue,
green) are elongated along configuration-dependent privileged directions. The smaller
the sets, the more pronounced their aspect ratios. The Lie splitting strategy adapts the
hyperplanes locally to the balls and decreases the expected number of ball-hyperplane
intersections, thus reducing the expected asymptotic query complexity.

dynamical system, as depicted in figure 2(b). To achieve this, we propose a novel
splitting rule, named the Lie splitting strategy, which exploits the differential
geometric properties of a system and the asymptotic scaling of its reachable
sets. The Lie splitting strategy is based on the following two principles:
1. The splitting normal associated with each data point xi is along one of the
privileged axes in that point, i.e., ZM (d, xi ) ∈ {y1 (xi ), y2 (xi ) . . . yk (xi )}.
2. The buckets of the k-d tree, in expectation, scale asymptotically according to
the weighted box along all privileged axes as t → 0. Formally, for all q ∈ M
and for all pairs of privileged axes yi (q), yj (q),
h
iwj
 h
iwi 
E rng yi (q) (bq )
∈ Θ E rng yj (q) (bq )
.
(15)
Requirement 2 prescribes the asymptotic behavior of the sequence ZM as d →
∞, which can be formalized as follows: let ni (d) = |{n < d : ZM (n, x) = yi (x)}|,
i.e., the total number of splits in the sequence ZM along axis yi before index d.
As d h→ ∞, the expected
bucket size along yi after ni (d) splits has asymptotic
i
order E rng yi (q) (bq ) ∈ Θ[e−ni (d) ]. Then, in terms of the number of splits,
equation (15) yields:

ni (d) · wj ∼ nj (d) · wi for all i, j ∈ [1, . . . k] as d → ∞.

(16)

Simply put, each privileged direction should be picked as a splitting normal with a frequency proportional to its weight. Note that this is only relevant
asymptotically, i.e., as d → ∞.
13

6. EXPERIMENTAL RESULTS
Example. For the Reeds-Shepp vehicle, a valid Lie splitting sequence is:

 ˆl(x), if d ≡ 0 or d ≡ 1 (mod 4)
Lie
ZRS (d, x) := fˆ(x), if d ≡ 2 (mod 4)

θ̂(x), if d ≡ 3 (mod 4)

(17)

It is easy to verify that this satisfies equation (16). In fact, lateral splits occur
in the sequence twice as often as longitudinal and lateral splits.

6

Experimental results

In this section, we validate our results and compare the performance of our
algorithms with different methods from widely used open-source libraries. Query
performances are averaged over 1000 randomly drawn query points. The same
insertion and query sequences are used across all the analyzed algorithms and
generated from a uniform distribution.
Experiment 1. In this experiment, we confirm the theoretical contributions presented in Section 4. In figure 3(a) we show the average number of leaves visited
when querying a batch k-d tree with Euclidean (blue) and Reeds-Shepp metrics (solid red). While the blue curve settles to a constant value, in accordance
with Lemma 1,√the red curve exhibits exponential growth. When normalizing
this curve by 6 N (dashed red), we observe a constant asymptotic behavior,
consistent with the rate determined by Theorem 3. For comparison, we report
the average time for Euclidean queries on an incremental k-d tree (black), which
tends to visit more vertices than its batch, balanced counterpart (blue).
Experiment 2. In this experiment (figures 3(b-d)), we plot the total number of
distance evaluations and the running times observed with different combinations
of build and query algorithms. Figure 3(d) reveals that the proposed outer Box
Bound (BB) ball-hyperplane intersection method (yellow, purple, green) reduces
the query time significantly as compared to Euclidean bounds (EB) (blue, red),
in accordance with our predictions in Section 3.1.
Additionally, Lie splitting (green) further improves query time, as compared
with the classic splitting (yellow). The corresponding incremental Lie k-d tree
also outperforms a classic batch-built one (purple), guaranteed to be balanced.
More important speedups emerge when comparing the proposed k-d trees
with different techniques, such as Hierarchical Clustering from FLANN and Geometric Near-neighbor Access Tree (GNAT) from OMPL. Interestingly, off-theshelf implementations of k-d trees offered by FLANN and other tools are unusable with nonholonomic metrics altogether, since they are limited to distances
of the form of equation (6). Therefore a comparison is not possible.
All the tested k-d trees visibly outperform Hierarchical Clustering (cyan)
both in build time and query time. In contrast, GNAT offers competitive query
times. However, its insertion is ∼ 100× slower than incremental k-d trees, since a
significant number of distances are evaluated in the build phase, while k-d trees
14

7. CONCLUSION
Batch - dE
Incremental, Z classic - dE
Batch - dRS , BB
Batch - dRS , BB
√
normalized by 6 N

10 2

Incremental, Z classic

- dRS , EB
Batch - dRS , EB
Incremental, Z classic - dRS , BB

10 8

Batch - dRS , BB

Lie
Incremental, ZRS
- dRS , BB

10 7

FLANN Hierarchical clustering
OMPL GNAT

10 6

10 1

10 5

N: number of samples

N: number of samples
10 1

10 2

10 3

10 4

10 5

102

10 6

103

(a) Average leaves visited (query)

10

104

105

106

107

108

(b) Total distance evaluations

8

10 4

10

10

10

6

4

10 3

2

N: number of samples

N: number of samples
10

2

10

3

10

4

10

5

10

6

10

7

10

8

10 2

(c) Build time (µs)

10 3

10 4

10 5

10 6

10 7

10 8

(d) Average query time (µs)

Fig. 3. (a) Experiment 1 : Average number of leaves visited during Euclidean (blue) and
Reeds-Shepp (red, solid) queries of a batch k-d tree. (b-d) Experiment 2 : Performance
of different algorithms. In the legends, dE and dRS indicate Euclidean and ReedsShepp queries, EB and BB indicate Euclidean Bound and outer Box Bound for ballLie
indicate the splitting sequence.
hyperplane intersections, Z classic and ZRS

only evaluate distances during query. This is reflected in a noticeably higher
asymptotic rate of distance evaluations, revealed in figure 3(b).

7

Conclusion

Motivated by applications in sampling-based motion planning, we investigated
k-d trees for efficient nearest-neighbor search with distances defined by the length
of paths of controllable nonholonomic systems. We have shown that for subRiemannian metrics, the query complexity of a classic batch-built k-d tree is
Θ(N p log N ), where p ∈ [0, 1) depends on the properties of the system. In addition, we have proposed improved build and query algorithms for k-d trees that
account for differential constraints. The proposed methods proved superior over
classic ones in numerical experiments carried out with a Reeds-Shepp vehicle.
Future work will analyze whether logarithmic complexity is achieved for nonholonomic systems. In addition, the proposed algorithms are exact and rely
on explicit distance evaluations. Since distances cannot be generally computed
in closed form, we are interested in investigating approximate nearest-neighbor
search algorithms with provable correctness bounds that do not require explicit
distance computations.
15

7. CONCLUSION

References
1. J. L. Bentley. Multidimensional binary search trees used for associative searching.
Communications of the ACM, 18(9):509–517, 1975.
2. M. Bharatheesha, W. Caarls, W. J. Wolfslag, and M. Wisse. Distance metric
approximation for state-space RRTs using supervised learning. In 2014 IEEE/RSJ
International Conference on Intelligent Robots and Systems, pages 252–257, 2014.
3. B. Burns and O. Brock. Single-query motion planning with utility-guided random trees. In Proceedings 2007 IEEE International Conference on Robotics and
Automation, pages 3307–3312, 2007.
4. J. H. Friedman, J. L. Bentley, and R. A. Finkel. An algorithm for finding best
matches in logarithmic expected time. ACM Transactions on Mathematical Software (TOMS), 3(3):209–226, 1977.
5. E. Glassman and R. Tedrake. A quadratic regulator-based heuristic for rapidly exploring state space. In Robotics and Automation (ICRA), 2010 IEEE International
Conference on, pages 5021–5028, 2010.
6. J. Ichnowski and R. Alterovitz. Fast nearest neighbor search in SE(3) for samplingbased motion planning. In Algorithmic Foundations of Robotics XI, pages 197–214.
Springer, 2015.
7. S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846–894, 2011.
8. S. Karaman and E. Frazzoli. Sampling-based optimal motion planning for nonholonomic dynamical systems. In Robotics and Automation (ICRA), 2013 IEEE
International Conference on, pages 5041–5047, 2013.
9. L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars. Probabilistic
roadmaps for path planning in high-dimensional configuration spaces. IEEE transactions on Robotics and Automation, 12(4):566–580, 1996.
10. J. J. Kuffner. Effective sampling and distance metrics for 3D rigid body path
planning. In Robotics and Automation, 2004. Proceedings. ICRA’04. 2004 IEEE
International Conference on, volume 4, pages 3993–3998, 2004.
11. S. M. LaValle. Rapidly-exploring random trees: A new tool for path planning.
Citeseer, 1998.
12. S. M. LaValle and J. J. Kuffner. Randomized kinodynamic planning. The International Journal of Robotics Research, 20(5):378–400, 2001.
13. R. Montgomery. A tour of subriemannian geometries, their geodesics and applications. Number 91. American Mathematical Soc., 2006.
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), pages 331–340. INSTICC Press, 2009.
15. L. Palmieri and K. Arras. Distance metric learning for RRT-based motion planning
for wheeled mobile robots. In 2014 IROS Machine Learning in Planning and
Control of Robot Motion Workshop, 2014.
16. J. Reeds and L. Shepp. Optimal paths for a car that goes both forwards and
backwards. Pacific journal of mathematics, 145(2):367–393, 1990.
17. A. Shkolnik, M. Walter, and R. Tedrake. Reachability-guided sampling for planning
under differential constraints. In Robotics and Automation, 2009. ICRA’09. IEEE
International Conference on, pages 2859–2865, 2009.
18. I. A. Şucan, M. Moll, and L. E. Kavraki. The Open Motion Planning Library.
IEEE Robotics & Automation Magazine, (4):72–82, December.
19. A. Yershova and S. M. LaValle. Improving motion-planning algorithms by efficient
nearest-neighbor searching. IEEE Transactions on Robotics, 23(1):151–157, 2007.

16

