ikd-Tree: An Incremental K-D Tree for Robotic Applications
aa r X i v : . [ c s . R O ] F e b ikd-Tree: An Incremental K-D Tree for Robotic Applications Yixi Cai, Wei Xu and Fu Zhang
Abstract — This paper proposes an efficient data structure,ikd-Tree, for dynamic space partition. The ikd-Tree incremen-tally updates a k-d tree with new coming points only, leadingto much lower computation time than existing static k-d trees.Besides point-wise operations, the ikd-Tree supports severalfeatures such as box-wise operations and down-sampling thatare practically useful in robotic applications. In parallel tothe incremental operations (i.e., insert, re-insert, and delete),ikd-Tree actively monitors the tree structure and partially re-balances the tree, which enables efficient nearest point searchin later stages. The ikd-Tree is carefully engineered and sup-ports multi-thread parallel computing to maximize the overallefficiency. We validate the ikd-Tree in both theory and practicalexperiments. On theory level, a complete time complexityanalysis is presented to prove the high efficiency. On experimentlevel, the ikd-Tree is tested on both randomized datasets andreal-world LiDAR point data in LiDAR-inertial odometry andmapping application. In all tests, ikd-Tree consumes only 4%of the running time in a static k-d tree.
I. I
NTRODUCTION
The K-Dimensional Tree (K-D Tree) is an efficient datastructure that organizes multi-dimensional point data [1]which enables fast search of nearest neighbors, an essentialoperation that is widely required in various robotic applica-tions [2]. For example, in LiDAR odometry and mapping,k-d tree-based nearest points search is crucial to match apoint in a new LiDAR scan to its correspondences in themap (or the previous scan) [3]–[8]. Nearest points search isalso important in motion planning for fast obstacle collisioncheck on point-cloud, such as in [9]–[14].Common-used k-d tree structure in robotic applications[15] is “static”, where the tree is built from scratch usingall points. This contradicts with the fact that the data isusually acquired sequentially in actual robotic applications.In this case, incorporating a frame of new data to existingones by re-building the entire tree from scratch is typicallyvery inefficient and time-consuming. As a result, k-d treesare usually updated at a low frequency [5]–[7] or simplyre-built only on the new points [11, 12].To fit the sequential data acquisition nature, a more naturalk-d tree design would be updating (i.e., insert and delete) theexisting tree locally with the newly acquired data. The localupdate would effectively eliminate redundant operations inre-building the entire tree, and save much computation. Sucha dynamic k-d tree is particularly promising when the newdata is much smaller than existing ones in the tree.
Y. Cai, W. Xu and F. Zhang are with the Departmentof Mechanical Engineering, Hong Kong University, Hong KongSAR., China. { yixicai } @connect.hku.hk, { xuweii,fuzhang } @hku.hk Fig. 1. Illustration of incremental k-d tree update and re-balancing. (a):an existing k-d tree (black dots) and new points (red triangles) to insert,blue cubes denote the space (i.e., branches) need to be re-balanced. (b): thek-d tree after points insertion and tree re-balancing, blue cubes denote thespace after re-balancing while rest majority tree does not change.
However, a dynamic k-d tree brings suitable for roboticapplications several challenges: 1) It should support notmerely efficient points operations such as insertion anddelete but also space operations such as point-cloud down-sampling; 2) It easily grows unbalanced after massive pointsor space operations which deteriorates efficiency of pointsqueries. Hence re-building is required to re-balance the tree.3) The re-building should be sufficiently efficient to enablereal-time robotic applications.In this paper, we propose a dynamic k-d tree structurecalled ikd-Tree, which builds and incrementally updates a k-dtree with new points only while simultaneously down-samplethem into the desired resolution. It supports incrementaloperations including insertion, re-insertion, and delete of asingle point (i.e., point-wise) or a box of points (i.e., box-wise). The tree is automatically re-balanced by partial re-building. To preserve efficient real-time tree update, ikd-Tree separates the tree re-building in two parallel threadswhen necessary. This paper also provides a complete timecomplexity analysis for all tree updates, including both incre-mental operations and re-balancing. The time complexity ofikd-Tree is reduced substantially as verified on both randomdata and real-world point-cloud in LiDAR-inertial mappingapplications. The ikd-Tree is open sourced at Github . Fig.1 illustrates the incremental updates and re-balancing on thetree from a 3-D view.The remaining paper is organized as follows: Section IIintroduces related work. The design of ikd-Tree is describedin Section III. Theoretical analysis of time and space com-plexity are presented in Section IV. Experiments are shown Git: https://github.com/hku-mars/ikd-Tree.git n Section V, followed by conclusions in Section VI.II. R
ELATED WORK
A k-d tree can be viewed as a binary search tree andinherits the same incremental operations (i.e., insert, re-insert, and delete), such as those in AVL trees [16], treaps[17] and splay trees [18]. In these techniques, re-balancingof a binary search tree after many points operations canbe easily achieved by tree node rotations. However, thisstraightforward tree rotation only works for one-dimensionaldata. For k-d trees with higher data dimension, it requiresmuch more complicated tree reorganization.Strategies specifically designed for fast re-balancing k-d trees fall into two categories: hardware-based accelera-tion and specially designed structure enabling dynamic re-balancing. Hardware-based algorithms exploits the comput-ing hardware to (re-) balance a kd-tree by building a new one.It has been thoroughly investigated to solve the ray tracingproblem in dynamic scenes. In 3D graphic applications,algorithms on single-core CPU [19, 20] and multi-core CPU[21] are firstly proposed to speed up the k-d tree construction.Zhou et al. proposed a real-time construction algorithm onGPU [22]. These algorithms rely heavily on the computingresource which is usually limited on onboard computers.For the second category, Bentley et al. proposed a generalbinary transformation method for converting a static k-dtree to a dynamic one [23]. The dynamic k-d tree supportsonly insertion but not delete, which leads to a growing treesize hence increased time for nearest search. Galperin et al. [24] proposes a scapegoat k-d tree that can dynamically re-balance the tree by re-building unbalanced sub-trees, whichis much more efficient than a complete re-building of theentire tree. Bkd tree [25] is a dynamic data structure extendedfrom a K-D-B tree [26] which focus on external memoryadaptions. A set of static k-d trees are built in the Bkdtree where the trees are re-balanced by re-building partialset of the trees at regular intervals. The well-known pointcloud library (PCL) [15] uses the fast library for approximatenearest neighbors (FLANN) search [27]. Point insertion anddelete are supported in FLANN but the trees are re-balancedvia inefficient complete tree re-building after a predeterminedamount of points insertion or delete [28].Our ikd-Tree is an efficient and complete data structureenabling incremental operations (i.e., insert, re-insert, anddelete) and dynamic re-balancing of k-d trees. Compared tothe dynamic k-d tree in [23], our implementation supportspoints delete. Besides the point-wise operations presentedin [16]–[18] and [24], our ikd-Tree further supports theincremental operations of a box of points (i.e., box-wiseoperations) and simultaneous points down-sampling. Thedynamic tree re-balancing strategy of ikd-Tree follows theconcept of scapegoat trees in [24], which only re-builds thoseunbalanced sub-trees. The ikd-Tree is particularly suitable forrobotic applications, such as real-time LiDAR mapping andmotion planning, where data are sampled sequentially andfast incremental update is necessary. III.
IKD -T REE D ESIGN AND I MPLEMENTATION
In this section, we describe how to design, build, andupdate an incremental k-d tree in ikd-Tree to allow incre-mental operations (e.g., insertion, re-insertion, and delete)and dynamic re-balancing.
A. Data Structure
The attributes of a tree node in ikd-Tree is presented in
Data Structure 1 . Line 2-4 are the common attributes for astandard k-d tree. The attributes lef tson and rightson arepointers to its left and right son node, respectively. The pointinformation (e.g., point coordinate, intensity) are stored in point . Since a point corresponds a single node on a k-d tree,we will use points and nodes interchangeably. The divisionaxis is recorded in axis . Line 5-7 are the new attributesdesigned for incremental updates detailed in Section III-C.
Data Structure 1 : Tree node structure Struct
TreeNode : // Common Attributes in StandardK-D trees PointType point ; TreeNode * lef tson , rightson ; int axis ; // New Attributes in ikd-Tree int treesize , invalidnum ; bool deleted , treedeleted , pushdown ; float range [ k ][2] ; end B. Building An Incremental K-D Tree
Building an incremental k-d tree is similar to building astatic k-d tree except maintaining extra information for incre-mental updates. The entire algorithm is shown in
Algorithm1 : given a point array V , the points are firstly sorted by thedivision axis with maximal covariance (Line 4-5). Then themedian point is saved to point of a new tree node T (Line 6-7). Points below and above the median are passed to the leftand right son nodes of T , respectively, for recursive building(Line 9-10). The LazyLabelInit and
Pullup in Line11-12 update all attributes necessary for incremental updates(see
Data Structure 1 , Line 5-7) detailed in Section III-C.
C. Incremental Updates
The incremental updates refer to incremental operationsfollowed by a dynamic re-balancing detailed in Section III-D.The incremental operations include insertion, delete and re-insertion of points to/from the k-d tree. Specifically, theinsertion operation appends a new point (i.e., a node) tothe k-d tree. In the delete operation, we use a lazy deletestrategy. That is, the points are not removed from the treeimmediately but only labeled as “deleted” by setting theattribute deleted to true (see
Data Structure 1 , Line 6). lgorithm 1:
Build a balanced k-d tree
Input:
V, N ⊲
Point Array and Point Number
Output:
RootN ode ⊲
K-D Tree Node RootN ode = Build( V, , N − ) ; Function
Build(
V, l, r ) mid ← ⌊ ( l + r ) / ⌋ ; Axis ← Axis with Maximal Covariance; V ← sort ( V, axis ) ; Node T ; T.point ← V [ mid ] ; T.axis ← Axis ; T.lef tson ← Build(
V, l, mid − ) ; T.rightson ← Build(
V, mid + 1 , r ) ; LazyLabelInit ( T ); Pullup( T ) ; return T ; End Function
If all nodes on the sub-tree rooted at T have been deleted,the attribute treedeleted of T is set to true. Therefore theattributes deleted and treedeleted are called lazy labels. Ifpoints labeled as “deleted” but not removed are later insertedto the tree, it is referred to as “re-insertion” and is efficientlyachieved by simply setting the deleted attribute back to false.Otherwise, points labeled as “deleted” will be removed fromthe tree during re-building process(see Section III-D).Our incremental updates support two types: point-wiseupdates and box-wise updates. The point-wise updates insert,delete, or re-insert a single point on the tree while thebox-wise updates insert, delete or re-insert all points in agiven box aligned with the data coordinate axis. Box-wiseupdates may require to delete or re-insert an entire sub-treerooted at T . In this case, recursively updating the lazy labels deleted and treedeleted for all offspring nodes of T arestill inefficient. To address this issue, we use a further lazystrategy to update the lazy labels of the offspring nodes.The lazy label for lazy labels deleted and treedeleted is pushdown (see Data Structure 1 , Line 6). The three labels deleted , treedeleted , and pushdown are all initialized asfalse in LazyLabelInit (see
Algorithm 1 , Line 11).
1) Pushdown and Pullup:
Two supporting functions,
Pushdown and
Pullup , are designed to update attributeson a tree node T . The Pushdown function copies the labels deleted , treedeleted , and pushdown of T to its sons (butnot further offsprings) when the attribute pushdown is true.The Pullup function summarizes the information of thesub-tree rooted at T to the following attributes of node T : treesize (see Data Structure 1 , Line 5) saving the numberof all nodes on the sub-tree, invalidnum saving the numberof nodes labelled as “deleted” on the sub-tree, and range (see
Data Structure 1 , Line 7) summarising the range of allpoints on the sub-tree along coordinate axis , where k is thepoints dimension.
2) Point-wise Updates:
The point-wise updates on theincremental k-d tree are implemented in a recursive way
Algorithm 2:
Box-wise Updates
Input: C O ⊲ Operation box
T ⊲
K-D Tree Node
SW ⊲
Switch of Parallelly Re-building Function
BoxwiseUpdate(
T, C O , SW ) Pushdown( T ) ; C T ← T.range ; if C T ∩ C O = ∅ then return ; if C T j C O then UpdateLazyLabel() ; T.pushdown = true; return ; else P ← T.point ; if P ⊂ C O then Modify
T.deleted ; BoxwiseUpdate(
T.lef tson, C O , SW ) ; BoxwiseUpdate(
T.rightson, C O , SW ) ; end Pullup( T ) ; if ViolateCriterion( T ) then if T.treesize < N max or Not SW then Rebuild( T ) else ThreadSpawn(ParallelRebuild , T ) end end End Function which is similar to the scapegoat k-d tree [24]. For point-wiseinsertion, the algorithm searches down from the root noderecursively and compare the coordinate on division axis ofthe new point with the points stored on the tree nodes untila leaf node is found to append a new tree node. For deleteor re-insertion of a point P , the algorithm finds the treenode storing the point P and modifies the attribute deleted .Further details can be found in our Github repository .
3) Box-wise Updates:
The box-wise insertion is imple-mented by inserting the new points one by one into the incre-mental k-d tree. Other box-wise updates (box-wise delete andre-insertion) are implemented utilizing the range informationin attribute range , which forms a box C T , and the lazy labelson the tree nodes. The pseudo code is shown in Algorithm2 . Given the box of points C O to be updated on (sub-) treerooted at T , the algorithm first passes down its lazy labels toits sons for further passing-down if visited (Line 2). Then,it searches the k-d tree from its root node recursively andchecks whether the range C T on the (sub-)tree rooted atthe current node T has an intersection with the box C O .If there is no intersection, the recursion returns directlywithout updating the tree (Line 4). If the box C T is fullycontained in the box C O , the box-wise delete set attributes deleted and treedeleted to true while the box-wise re-insertion set them to false by function UpdateLazyLabel (Line 6). The pushdown attribute is set to true indicatingthat the latest incremental updates have not been applied lgorithm 3:
Downsample
Input:
L ⊲
Length of Downsample Cube
P ⊲
New Point C D ← FindCube(
L, P ) P center ← Center( C D ) ; V ← BoxwiseSearch(
RootN ode, C D ) ; V.push ( P ) ; P nearest ← FindNearest ( V, P center ) ; BoxwiseDelete(
RootN ode, C D ) PointwiseInsert(
RootN ode, P nearest ) ; (a) (b) Fig. 2. Point Cloud Downsample. (a): the point cloud before down-sampling. (b): the point cloud after down-sampling to the offspring nodes of T . For the condition that C T intersects but not contained in C O , the current point P isfirstly deleted from or re-inserted to the tree if it is containedin C O (Line 11), after which the algorithm looks into theson nodes recursively (Line 12-13) and updates all attributesof the current node T (Line 15). Line 16-22 re-balance thetree if certain criterion is violated (Line 16) by re-buildingthe tree in the same (Line 18) or a separate (Line 20)thread. The function ViolateCriterion , Rebuild and
ParrallelRebuild are detailed in Section III-D.
4) Downsample:
Our ikd-Tree further supports down-sampling as detailed in
Algorithm 3 . For the given point P and down-sampling resolution L , the algorithm partitionsthe space evenly into cubes of length L , then the box C D thatcontains point P is found (Line 1). The algorithm only keepsthe point that is nearest to the center P center of C D (Line2). This is achieved by firstly searching all points containedin C D on the k-d tree and stores them in a point array V together with the new point P (Line 3-4). The nearestpoint P nearest is obtained by comparing the distances ofeach point in V to the center P center (Line 5). Then existingpoints in C D are deleted (Line 6), after which the nearestpoint P nearest is inserted to the k-d tree (Line 7). Theimplementation of box-wise search is similar to the box-wise delete and re-insertion (see Algorithm 2 ). An exampleof downsample is shown in Fig. 2.In summary, Table I shows the comparison of supportedincremental updates on the static k-d tree [1], the dynamick-d tree [23], the scapegoat k-d tree [24] and our ikd-Tree.
TABLE IC
OMPARISON OF S UPPORTED I NCREMENTAL U PDATES
StaticK-D Tree DynamicK-D Tree ScapegoatK-D Tree ikd-TreePoint-wise Insert ✗ ✓ ✓ ✓
Delete ✗ ✗ ✓ ✓
Re-insert ✗ ✗ ✗ ✓
Box-wise Insert ✗ ✓ ✓ ✓
Delete ✗ ✗ ✗ ✓
Re-insert ✗ ✗ ✗ ✓
Downsample ✗ ✗ ✗ ✓
D. Re-balancing
Our ikd-Tree actively monitors the balance property of theincremental k-d tree and dynamically re-balance it by partialre-building.
1) Balancing Criterion:
The balancing criterion is com-posed of two sub-criterions: α -balanced criterion and α -deleted criterion. Suppose a sub-tree of the incremental k-dtree is rooted at T . The sub-tree is α -balanced if and only ifit satisfies the following condition: S ( T.lef tson ) < α bal (cid:16) S ( T ) − (cid:17) S ( T.rightson ) < α bal (cid:16) S ( T ) − (cid:17) (1)where α bal ∈ (0 . , and S ( T ) is the treesize attributeof the node T .The α -deleted criterion of the sub-tree rooted at T is I ( T ) < α del S ( T ) (2)where α del ∈ (0 , and I ( T ) denotes the number of invalidnodes on the sub-tree (i.e., the attributes invalidnum ofnode T ).If a sub-tree of the incremental k-d tree meets bothcriterion, the sub-tree is balanced. The entire tree is balancedif all sub-trees are balanced. Violation of either criterion willtrigger a re-building process to re-balance that (sub-) tree: the α -balanced criterion maintains the maximum height of the(sub-) tree. It can be easily proved that the maximum heightof an α -balanced tree is log /α bal ( n ) where n is the tree size;the α -deleted criterion ensures invalid nodes (i.e., labeled as“deleted”) on the (sub-) trees are removed to reduce treesize. Reducing height and size of the k-d tree allows highlyefficient incremental operations and queries in future. Thefunction ViolateCriterion in Algorithm 2, Line 16 returns true if either criterion is violated.
2) Re-build:
Assuming re-building is triggered on a sub-tree T (see Fig. 3), the sub-tree is firstly flattened into apoint storage array V . The tree nodes labeled as “deleted”are discarded during flattening. A new perfectly balanced k-dtree is then built with all points in V by Algorithm 1 .
3) Parallel Re-build:
An evident degradation of real-timeability is observed when re-building a large sub-tree onthe incremental k-d tree. To preserve high real-time ability,we design a double-thread re-building method: the mainthread only re-builds sub-trees whose size is smaller than ig. 3. Re-build an unbalanced sub-tree a predetermined value N max and the second thread re-buildsthe rest. The key problem is how to avoid information loseand memory conflicts between the main thread and thesecond thread.The re-building algorithm on the second thread is shownin Algorithm 4 . Denote the sub-tree to re-build in the secondthread as T and its root node as T . The second threadwill lock all incremental updates (i.e., points insert, re-insert,and delete) but not queries on this sub-tree (Line 2). Thenthe second thread copies all valid points contained in thesub-tree T into a point array V (i.e. flatten) while leavingthe original sub-tree unchanged for possible queries duringthe re-building process (Line 3). After the flattening, thesub-tree is unlocked for the main thread to take furtherrequests of incremental updates (Line 4). These requests willbe suspended and recorded in a queue named as operationlogger. Once the second thread completes building a newbalanced k-d tree T ′ from the point array V (Line 5), therecorded update requests will be performed on the balancedsub-tree T ′ by function IncrementalUpdates (Line 6-8) where the parallel re-building option is set to false (as itis already in the second thread). After all pending requestsare processed, the algorithm locks the node T from bothincremental updates and queries and replace it with the newone T ′ (Line 9-12). Finally, the algorithm frees the memoryof the original sub-tree (Line 13). Note that LockUpdates does not block queries, which can be conducted parallellyin the main thread. In contrast,
LockAll blocks all accessincluding queries, but it finishes very quickly (i.e., only oneinstruction), allowing timely queries in the main thread. Thefunction
LockUpdates and
LockAll are implemented bymutual exclusion (mutex).
E. K-Nearest Neighbor Search
The nearest search on the incremental k-d tree is anaccurate nearest search [2] instead of an approximate one as[27]. The function
Pushdown is applied before searchingthe sub-tree rooted at node T to pass down its lazy labels. Weuse the attribute range to speed up the search process thushard real-time ability is preserved. Due to the space limit,the details of k-nearest search algorithm is not presented inthis paper. Interested readers can refer to the related codesin our open source library.IV. C OMPLEXITY A NALYSIS
A. Time Complexity
The time complexity of ikd-Tree breaks into the time forincremental operations (insertion, re-insertion and delete) and
Algorithm 4:
Parallelly Rebuild for Re-balancing
Input:
T ⊲
Root node of T for re-building Function
ParallelRebuild( T ) LockUpdates( T ) ; V ← Flatten( T ) ; Unlock( T ) ; T ′ ← Build(
V,0,size(V)-1 ) ; foreach op in OperationLogger do IncrementalUpdates( T ′ , op, f alse ) end T temp ← T ; LockAll( T ) ; T ← T ′ ; Unlock( T ) ; Free( T temp ) ; End Function re-building.
1) Incremental Operations:
The time complexity of point-wise operations is given as
Lemma 1 (Point-wise Operations): An incremental k-dtree can handle a point-wise incremental operation with timecomplexity of O (log n ) where n is the tree size.Proof: The maximum height of an incremental k-dtree can be easily proved to be log /α bal ( n ) from Eq. (1)while that of a static k-d tree is log n . Hence the lemmais directly obtained from [1] where the time complexity ofpoint insertion and delete on a k-d tree was proved to be O (log n ) . The point-wise re-insertion modifies the attribute deleted on a tree node thus the time complexity is the sameas point-wise delete.The time complexity of box-wise operations on an incre-mental k-d tree is: Lemma 2 (Box-wise Operations): An incremental 3-d treehandles box-wise insertion of m points in C D with timecomplexity of O ( m log n ) . Furthermore, suppose points onthe 3-d tree are in space S x × S y × S z and C D = L x × L y × L z . The box-wise delete and re-insertion can be handled withtime complexity of O ( H ( n )) , where O ( H ( n )) = O (log n ) if ∆ min > α ( ) (*) O ( n − a − b − c ) if ∆ max − α ( ) (**) O ( n α ( ) − ∆ min − ∆ med ) if (*) and (**) fail and ∆ med < α ( ) − α ( ) O ( n α ( ) − ∆ min ) otherwise. (3) where a = log n S x L x , b = log n S y L y and c = log n S z L z with a, b, c > . ∆ min , ∆ med and ∆ max are the minimal, medianand maximal value among a , b and c . α ( u ) is the flajolet-puech function with u ∈ [0 , , where particular value isprovided: α ( ) = 0 . and α ( ) = 0 . Proof:
The box-wise insertion is implemented by point-wise insertion thus the time complexity can be directlyobtained from Lemma 1. An asymptotic time complexityfor range search on a k-d tree is provided in [29]. The
ABLE IIT HE P ARAMETERS S ETUP OF IKD -T REE IN E XPERIMENTS
Randomized Data LiDAR Inertial-Odometry Mapping N max α bal α del C D - . m × . m × . box-wise delete and re-insertion can be regarded as a rangesearch except that lazy labels are attached to the tree nodes.Therefore, the conclusion of range search can be applied tothe box-wise delete and re-insertion on the incremental k-dtree.The down-sampling method on an incremental k-d treeis composed of box-wise search and delete followed bythe point insertion. By applying Lemma 1 and Lemma 2,the time complexity of downsample is O (log n ) + O ( H ( n )) .Generally, the downsample hypercube C D is very smallcomparing with the entire space. Therefore, the normalizedrange ∆ x , ∆ y and ∆ z are small and the value of ∆ min satisfies the condition (*) for time complexity of O (log n ) .Hence, the time complexity of down-sampling is O (log n ) .
2) Re-build:
Time complexity for re-building breaks intotwo types: single-thread re-building and parallel two-threadre-building. In the former case, the re-building is performedby the main thread in a recursive way, each level takes thetime of sorting (i.e., O ( n ) ) and the total time over log n levelsis O ( n log n ) [1] when the the dimension k is low (e.g., 3in most robotic applications). For parallel re-building, thetime consumed in the main thread is only flattening (whichsuspends the main thread from further incremental updates, Algorithm 4 , Line 2-4) but not building (which is performedin parallel by the second thread,
Algorithm 4 , Line 5) or treeupdate (which takes constant time O (1) , Algorithm 4 , Line10-12), leading to a time complexity of O ( n ) . In summary,the time complexity of re-building an incremental k-d tree is O ( n ) for two-thread parallel re-building and O ( n log n ) forsingle-thread re-building.
3) Nearest Search:
For robotic applications, the pointsdimension is usually very small. Hence the time complexityof k-nearest search on the incremental k-d tree can be simplyapproximated as O (log n ) because the maximum height ofthe incremental k-d tree is maintained no larger than log α n . B. Space Complexity
As shown Section III, each node on the incremental k-d tree records point information, tree size, invalid pointnumber and point distribution of the tree. Extra flags suchas lazy labels are maintained on each node for box-wiseoperations. For an incremental k-d tree with n nodes, thespace complexity is O ( n ) though the space constant is afew times larger than a static k-d tree.V. A PPLICATION E XPERIMENTS
A. Randomized Data Experiments
The efficiency of our ikd-Tree is fully investigated by twoexperiments on randomized incremental data sets. The first
100 200 300 400 500 600 700 800 900 100002040 R un T i m e / m s Incremental Updates ikd-TreeStatic K-D Tree
100 200 300 400 500 600 700 800 900 1000012 R un T i m e / m s Nearest Search ikd-TreeStatic K-D Tree
100 200 300 400 500 600 700 800 900 1000
Operation Counter R un T i m e / m s P o i n t N u m b e r Total Time Consumption ikd-TreeStatic K-D TreeTotal Point Number
750 8000.30.6750 8001.31.6 (a)(b)(c)
Fig. 4. The time performance comparison between an ikd-Tree and a statick-d tree. experiment generates 5,000 points randomly in a m × m × m space (i.e., the workspace) to initialize the incre-mental k-d tree. Then 1,000 test operations are conducted onthe k-d tree. In each test operation, 200 new points randomlysampled in the workspace are inserted (point-wise) to the kd-tree. Then another 200 points are randomly sampled in theworkspace and searched on (but not inserted to) the k-d treefor 5 nearest points of each. For every 50 test operations,4 cubes are sampled in the workspace with side length of . m and points contained in these 4 cubes are deleted (box-wise) from the k-d tree. For every 100 test operations, 2,000new points are sampled in the workspace and inserted (point-wise) to the k-d tree. We compare the ikd-Tree with the statick-d tree used in point cloud library [15] where at each testoperation the k-d tree is entirely re-built. The experiments areperformed on a PC with Intel i7-10700 CPU at 2.90GHz andonly 2 threads running. The parameters of the incrementalk-d tree are summarized in Table II where no down-samplingis used to allow a fair comparison. Also the maximal pointnumber allowed to store on the leaf node of a static k-d treeis set to 1 while the original setting in point cloud library is15.The results of the first experiment are shown in Fig. 4,where the point number increases from 5,000 to approximate200,000. In this process, the time for incremental updates(including both incremental operations and re-building) onthe ikd-Tree remains stably around 1.6 ms while that forthe static k-d tree grows linearly with number of the points(see Fig. 4(a)). The high peaks in the time consumptionare resulted from the large-scale point-wise insertion (andassociated re-balancing) and the low peaks are resulted frombox-wise delete (and associated re-balancing). As shown inFig. 4(b), the time performance of the k-nearest search on the parse Increments Compact Increments Point Number R un T i m e / m s Time Performance on Incremental Updates
SparseCompact (a) (b)(c)
Fig. 5. Fig. (a) and (b) illustrate new points (orange triangles) and pointsalready on the k-d tree (blue dots). Fig. (c) shows the time for incrementalupdates of sparse and compact data on k-d trees of different size. ikd-Tree is slightly slower than an static k-d tree, possiblydue to the highly optimized implementation of the PCLlibrary. Despite of the slightly lower efficiency in query, theoverall time consumption of ikd-Tree outperforms the statick-d tree by one order of magnitude (See Fig. 4(c)).The second experiment investigates the time performanceof incremental updates for new points of different distribu-tion. In the experiment, we sample two sets of 4,000 newpoints in a m × m × m space (i.e., the workspace):one is evenly distributed (i.e., sparse data, see Fig. 5 (a)) andthe other concentrated in a . m × . m × . m space (i.e.,compact data, see Fig. 5 (b)). The sparse and compact dataare inserted to an existing incremental k-d tree of differentsize but all sampled in the workspace. Fig. 5(c) shows therunning time of sparse and compact point-wise insertionon k-d trees of different size. As expected, the incrementalupdates for compact data are slower than the sparse onebecause re-building are more likely to be triggered wheninserting a large amount of points into a small sub-tree of ak-d tree. B. LiDAR Inertial-Odometry and Mapping
We test our developed ikd-Tree in an actual roboticapplication: lidar-inertial odometry (and mapping) presentedin [3]–[8]. In this application, k-d tree-based nearest pointssearch is crucial to match a point in a new LiDAR scan to itscorrespondences in the map (or the previous scan). Since themap is dynamically growing by matching and merging newscans, the k-d tree has to be re-built every time a new scanis merged. Existing methods [3]–[8] commonly used statickd-tree from PCL and rebuilds the entire tree based on allpoints in the map (or a submap). This leads to a significantcomputation costs severely limiting the map update rate (e.g.,from Hz [5]–[7] to Hz [8]).In this experiment, we replace the static k-d tree (build, up-date, and query) by our ikd-Tree, which enables incrementalupdate of the map by updating the new points only. We testikd-Tree on the lidar-inertial mapping package FAST LIO in[8] . The experiment is conducted on a real-world outdoor https://github.com/hku-mars/FAST_LIO
300 310 320 330 340 350 360 370 380 390 400 410 time/s R un T i m e / m s Time Performance on FAST-LIO ikd-TreeStatic K-D Tree
Nearest Search Incremental Updates Total Time -3 -2 -1 R un T i m e / m s ikd-TreeStatic K-D Tree
300 310 320 330 340 350 360 370 380 390 400 410 time/s
Re-balancing Criterion baldel (a)(b)(c)
Fig. 6. Fig. (a) shows the average running time of fusing one new lidarscan in FAST LIO using the ikd-Tree and a static k-d tree. Fig. (b) showstime for nearest search, incremental updates, and total time in fusing onelidar scan. Fig. (c) shows the balance property after re-building on mainthread.Fig. 7. Mapping Result of the Main Building, University of Hong Kong.The green line is the path of the lidar computed by FAST LIO. scene using a Livox Avia LiDAR [30] with 70°FoV anda high frame rate of 100 Hz . All the algorithm is runningon the DJI Manifold 2-C with a 1.8 GHz quad-core Inteli7-8550U CPU and 8 GB RAM.The time of fusing a new lidar scan in FAST LIO isshown in Fig. 6(a). The time is the averaged time of themost recent 100 scans. It is seen that the ikd-Tree achievesa nearly constant time performance around 1.6 ms , whichfundamentally enables a mapping rate up to Hz (against Hz presented in the original work [8] using static k-dtree). On the other hand, the time with the static k-d tree isoverall increasing linearly, and decreases occasionally due tothe small overlap between the lidar current FoV and the map.The resultant processing time with the static k-d tree exceeds10 ms from 366 s on, which is more than the collection timeof one lidar scan.The time for fusing a new lidar scan consists of manyoperations, such as point registration, state estimation, and d-tree related operations (including queries and update).The time breakdown of kd-tree related operations is shownin Fig. 6(b). The average time of incremental updates forikd-Tree is 0.23 ms which is only 4% of that using an statick-d tree (5.71 ms ). The average time of nearest search usingthe ikd-Tree and an static k-d tree are at the same level.Furthermore, Fig. 6(c) investigates the balancing propertyof the incremental kd-tree by examining the two criterions α bal and α del , which are defined as: α bal ( T ) = max n S ( T.lef tson ) , S ( T.rightson ) o S ( T ) − α del ( T ) = I ( T ) S ( T ) (4)As expected, the two criterion are maintained below the pre-scribed thresholds (see Table II) due to re-building, indicatingthat the kd-tree is well-balanced through the incremental up-dates. The peaks over the thresholds is resulted from parallelyre-building, which drop quickly when the re-building is done.Finally, Fig. 7 shows the Hz mapping results.VI. C ONCLUSION
This paper proposed an efficient data structure, ikd-Tree, toincrementally update a k-d tree in robotic applications. Theikd-Tree supports incremental operations in robotics whilemaintaining balanced by partial re-building. We provided acomplete analysis of time and space complexity to prove thehigh efficiency of the proposed dynamic structure. The ikd-Tree was tested on a randomized experiment and an outdoorLiDAR odometry and mapping experiment. In all tests, theproposed data structure achieves two orders of magnitudehigher efficiency. R
EFERENCES[1] J. L. Bentley, “Multidimensional binary search trees used for asso-ciative searching,”
Communications of the ACM , vol. 18, no. 9, pp.509–517, 1975.[2] J. H. Friedman, J. L. Bentley, and R. A. Finkel,
An algorithm forfinding best matches in logarithmic time . Department of ComputerScience, Stanford University, 1975.[3] A. Nuchter, K. Lingemann, and J. Hertzberg, “Cached kd tree searchfor icp algorithms,” in
Sixth International Conference on 3-D DigitalImaging and Modeling (3DIM 2007) . IEEE, 2007, pp. 419–426.[4] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in
Robotics:science and systems , vol. 2, no. 4. Seattle, WA, 2009, p. 435.[5] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in
Robotics: Science and Systems , vol. 2, no. 9, 2014.[6] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in . IEEE, 2018, pp. 4758–4765.[7] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidarodometry and mapping package for lidars of small fov,” in . IEEE,2020, pp. 3126–3131.[8] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometrypackage by tightly-coupled iterated kalman filter,” arXiv preprintarXiv:2010.08196 , 2020.[9] J. Ichnowski and R. Alterovitz, “Fast nearest neighbor search in se (3)for sampling-based motion planning,” in
Algorithmic Foundations ofRobotics XI . Springer, 2015, pp. 197–214. [10] F. Gao and S. Shen, “Online quadrotor trajectory generation andautonomous navigation on point clouds,” in . IEEE,2016, pp. 139–146.[11] B. T. Lopez and J. P. How, “Aggressive 3-d collision avoidance forhigh-speed navigation.” in
ICRA , 2017, pp. 5759–5765.[12] P. R. Florence, J. Carter, J. Ware, and R. Tedrake, “Nanomap:Fast, uncertainty-aware proximity queries with lazy search over local3d data,” in . IEEE, 2018, pp. 7631–7638.[13] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Onlinetrajectory generation and autonomous navigation for quadrotors incluttered environments,”
Journal of Field Robotics , vol. 36, no. 4,pp. 710–733, 2019.[14] J. Ji, Z. Wang, Y. Wang, C. Xu, and F. Gao, “Mapless-planner: Arobust and fast planning framework for aggressive autonomous flightwithout map fusion,” arXiv preprint arXiv:2011.03975 , 2020.[15] R. B. Rusu and S. Cousins, “3d is here: Point cloud library (pcl),”in .IEEE, 2011, pp. 1–4.[16] R. Bayer, “Symmetric binary b-trees: Data structure and maintenancealgorithms,”
Acta informatica , vol. 1, no. 4, pp. 290–306, 1972.[17] C. R. Aragon and R. Seidel, “Randomized search trees,” in
FOCS ,vol. 30, 1989, pp. 540–545.[18] D. D. Sleator and R. E. Tarjan, “Self-adjusting binary search trees,”
Journal of the ACM (JACM) , vol. 32, no. 3, pp. 652–686, 1985.[19] W. Hunt, W. R. Mark, and G. Stoll, “Fast kd-tree construction withan adaptive error-bounded heuristic,” in . IEEE, 2006, pp. 81–88.[20] S. Popov, J. Gunther, H.-P. Seidel, and P. Slusallek, “Experiences withstreaming construction of sah kd-trees,” in . IEEE, 2006, pp. 89–94.[21] M. Shevtsov, A. Soupikov, and A. Kapustin, “Highly parallel fastkd-tree construction for interactive ray tracing of dynamic scenes,”
Computer Graphics Forum , vol. 26, no. 3, pp. 395–404, 2007.[22] K. Zhou, Q. Hou, R. Wang, and B. Guo, “Real-time kd-tree construc-tion on graphics hardware,”
ACM Transactions on Graphics , vol. 27,no. 5, pp. 1–11, 2008.[23] J. L. Bentley and J. B. Saxe, “Decomposable searching problems i:Static-to-dynamic transformation,”
J. algorithms , vol. 1, no. 4, pp.301–358, 1980.[24] I. Galperin and R. L. Rivest, “Scapegoat trees.” in
SODA , vol. 93,1993, pp. 165–174.[25] O. Procopiuc, P. K. Agarwal, L. Arge, and J. S. Vitter, “Bkd-tree: Adynamic scalable kd-tree,” in
International Symposium on Spatial andTemporal Databases . Springer, 2003, pp. 46–65.[26] J. T. Robinson, “The kdb-tree: a search structure for large multidimen-sional dynamic indexes,” in
Proceedings of the 1981 ACM SIGMODinternational conference on Management of data , 1981, pp. 10–18.[27] M. Muja and D. G. Lowe, “Fast approximate nearest neighbors withautomatic algorithm configuration.”
VISAPP (1) , vol. 2, no. 331-340,p. 2, 2009.[28] M. Muja and D. Lowe, “Flann-fast library for approximate nearestneighbors user manual,”
Computer Science Department, University ofBritish Columbia, Vancouver, BC, Canada , 2009.[29] P. Chanzy, L. Devroye, and C. Zamora-Cura, “Analysis of range searchfor random kd trees,”
Acta informatica , vol. 37, no. 4-5, pp. 355–383,2001.[30] Z. Liu, F. Zhang, and X. Hong, “Low-cost retina-like roboticlidars based on incommensurable scanning,” arXiv preprintarXiv:2006.11034arXiv preprintarXiv:2006.11034