arXiv:2102.10808v1 [cs.RO] 22 Feb 2021 fMcaia niern,Hn ogUiest,Hn Kong Hong University, Kong Hong China. Engineering, SAR., Mechanical of tree. the in new ones the existing when than promising smaller particularly much is is data tree Suc k-d computation. much dynamic save i a and operations tree, entire loca redundant the The re-building eliminate data. effectively acquired would newly t delete) the update and with insert locally (i.e., tree updating existing be would simply design tree or k-d 12]. [5]–[7] [11, points frequency new low the trees a on k-d only at result, re-built typicall updated a is usually existing As scratch to time-consuming. are from data and tree new inefficient is entire of very the data frame re-building the a by incorporating that ones applicati case, using robotic fact this actual scratch the In in from with sequentially built acquired contradicts is usually This tree the points. where all “static”, is [15] [9]–[14]. in collisi as obstacle such the fast is point-cloud, for in search on planning points check correspondences motion Nearest its in a [3]–[8]. important to scan) match also previous scan to the LiDAR crucial (or new map is mapping, a search and in points odometry point nearest LiDAR in tree-based applic example, robotic k-d various For in essentia [2]. required an tions [1] widely neighbors, is nearest data that of operation point search fast multi-dimensional enables which organizes that structure fternigtm nasai - tree. k-d static 4 a only in consumes time ikd-Tree running tests, an the and all of datasets odometry In LiDAR-inertial randomized application. in both complexity mapping data on point time tested LiDAR is real-world complete experi ikd-Tree On efficiency. a the high the level, prove level, to presented is theory practanalysis and On theory su over both in the experiments. and ikd-Tree maximize the engineered validate to carefully We efficiency. computing sea is parallel ikd-Tree point multi-thread nearest The ports efficient stages. enables later partiall which and in d tree, structure and paralle the tree re-insert, balances In the insert, monitors applications. actively (i.e., ikd-Tree robotic operations tha in incremental down-sampling the useful seve and operations practically supports box-wise are ikd-Tree tree as k-d leading the such only, static operations, features existing points than coming point-wise time new Besides computation with lower tree much increm k-d to ikd-Tree a The updates partition. tally space dynamic for ikd-Tree, fuzhang ofi h eunildt custo aue oenatural more a nature, acquisition data sequential the fit To applications robotic in structure tree k-d Common-used data efficient an is Tree) (K-D Tree K-Dimensional The Abstract .Ci .X n .Zagaewt h Department the with are Zhang F. and Xu W. Cai, Y. } k-re nIceetlKDTe o ooi Applications Robotic for Tree K-D Incremental An ikd-Tree: hspprpooe nefiin aastructure, data efficient an proposes paper This — @hku.hk { yixicai .I I. NTRODUCTION } @connect.hku.hk, iiCi e uadF Zhang Fu and Xu Wei Cai, Yixi { xuweii, elete), ment re- y ons. to l ical rch en- ral on all he % p- a- s. n y d h t l l plctos h k-rei pnsucda Github at sourced open is mapping ikd-Tree LiDAR-inertial The in random applications. point-cloud both real-world on and verified as data of substantially complexity reduced time is The ikd-Tree re-balancing. time in and both complete operations including threads a updates, mental tree parallel provides all for also two analysis paper complexity ikd- in This re-building update, necessary. tree tree re- when the partial real-time box separates by efficient (i.e., Tree re-balanced preserve points automatically To of is box building. tree a The or incremental point-wise) wise). supports delete (i.e., and It point re-insertion, single resolution. insertion, desired including operations the into down-sample simultaneously while them only points k new a with updates tree incrementally and builds which ikd-Tree, called enable to efficient applications. sufficiently robotic be tr real-time should the re-building re-balance points The to of required 3) efficiency is re-building deteriorates Hence which queries. point massive operations down- and after space point-cloud unbalanced insertion or grows as easily such as It not operations 2) such sampling; space support also operations should but points delete It efficient 1) merely challenges: several applications pc fe eblnigwiers aoiyte osntc not does tree blue majority re-balancing, rest tree while triangl re-ba re-balancing and be (red after insertion to points space points re-b need branches) after new and (i.e., tree and update space k-d dots) tree the denote k-d (black cubes incremental tree blue of k-d Illustration existing an 1. Fig. lxt r rsne nSection in presented are describe plexity is ikd-Tree Section of design in The work. related introduces 1 refo - view. 3-D a from tree lutae h nrmna pae n eblnigo th on re-balancing and updates incremental the illustrates nti ae,w rps yai - restructure tree k-d dynamic a propose we paper, this In robotic for suitable brings tree k-d dynamic a However, h eann ae sognzda olw:Section follows: as organized is paper remaining The 1 Git: https://github.com/hku-mars/ikd-Tree.git III hoeia nlsso ieadsaecom- space and time of analysis Theoretical . IV xeiet r shown are Experiments . ue eoethe denote cubes hange. acd b:the (b): lanced. lnig (a): alancing. s oinsert, to es) 1 Fig. . fa of cre- ee. -d II d e s - in Section V, followed by conclusions in Section VI.

II. RELATED WORK III. IKD-TREE DESIGN AND IMPLEMENTATION In this section, we describe how to design, build, and A k-d tree can be viewed as a binary search tree and update an incremental k-d tree in ikd-Tree to allow incre- inherits the same incremental operations (i.e., insert, re- mental operations (e.g., insertion, re-insertion, and delete) insert, and delete), such as those in AVL trees [16], treaps and dynamic re-balancing. [17] and splay trees [18]. In these techniques, re-balancing of a binary search tree after many points operations can A. Data Structure be easily achieved by tree node rotations. However, this The attributes of a tree node in ikd-Tree is presented in straightforward tree rotation only works for one-dimensional Data Structure 1. Line 2-4 are the common attributes for a data. For k-d trees with higher data dimension, it requires standard k-d tree. The attributes leftson and rightson are much more complicated tree reorganization. pointers to its left and right son node, respectively. The point Strategies specifically designed for fast re-balancing k- information (e.g., point coordinate, intensity) are stored in d trees fall into two categories: hardware-based accelera- point. Since a point corresponds a single node on a k-d tree, tion and specially designed structure enabling dynamic re- we will use points and nodes interchangeably. The division balancing. Hardware-based algorithms exploits the comput- axis is recorded in axis. Line 5-7 are the new attributes ing hardware to (re-) balance a kd-tree by building a new one. designed for incremental updates detailed in Section III-C. It has been thoroughly investigated to solve the ray tracing problem in dynamic scenes. In 3D graphic applications, Data Structure 1: Tree node structure algorithms on single-core CPU [19, 20] and multi-core CPU 1 Struct TreeNode: [21] are firstly proposed to speed up the k-d tree construction. // Common Attributes in Standard Zhou et al. proposed a real-time construction algorithm on K-D trees GPU [22]. These algorithms rely heavily on the computing 2 PointType ; resource which is usually limited on onboard computers. point 3 TreeNode * leftson, rightson; For the second category, Bentley et al. proposed a general 4 int axis; binary transformation method for converting a static k-d // New Attributes in ikd-Tree tree to a dynamic one [23]. The dynamic k-d tree supports 5 int treesize, invalidnum; only insertion but not delete, which leads to a growing tree 6 bool deleted, treedeleted, pushdown; size hence increased time for nearest search. Galperin et al. 7 float range[k][2]; [24] proposes a scapegoat k-d tree that can dynamically re- 8 end balance the tree by re-building unbalanced sub-trees, which is much more efficient than a complete re-building of the entire tree. Bkd tree [25] is a dynamic data structure extended B. Building An Incremental K-D Tree from a K-D-B tree [26] which focus on external memory Building an incremental k-d tree is similar to building a adaptions. A set of static k-d trees are built in the Bkd static k-d tree except maintaining extra information for incre- tree where the trees are re-balanced by re-building partial mental updates. The entire algorithm is shown in Algorithm set of the trees at regular intervals. The well-known point 1: given a point array V , the points are firstly sorted by the cloud library (PCL) [15] uses the fast library for approximate division axis with maximal covariance (Line 4-5). Then the nearest neighbors (FLANN) search [27]. Point insertion and median point is saved to point of a new tree node T (Line 6- delete are supported in FLANN but the trees are re-balanced 7). Points below and above the median are passed to the left via inefficient complete tree re-building after a predetermined and right son nodes of T , respectively, for recursive building amount of points insertion or delete [28]. (Line 9-10). The LazyLabelInit and Pullup in Line Our ikd-Tree is an efficient and complete data structure 11-12 update all attributes necessary for incremental updates enabling incremental operations (i.e., insert, re-insert, and (see Data Structure 1, Line 5-7) detailed in Section III-C. delete) and dynamic re-balancing of k-d trees. Compared to the dynamic k-d tree in [23], our implementation supports points delete. Besides the point-wise operations presented C. Incremental Updates in [16]–[18] and [24], our ikd-Tree further supports the The incremental updates refer to incremental operations incremental operations of a box of points (i.e., box-wise followed by a dynamic re-balancing detailed in Section III-D. operations) and simultaneous points down-sampling. The The incremental operations include insertion, delete and re- dynamic tree re-balancing strategy of ikd-Tree follows the insertion of points to/from the k-d tree. Specifically, the concept of scapegoat trees in [24], which only re-builds those insertion operation appends a new point (i.e., a node) to unbalanced sub-trees. The ikd-Tree is particularly suitable for the k-d tree. In the delete operation, we use a lazy delete robotic applications, such as real-time LiDAR mapping and strategy. That is, the points are not removed from the tree motion planning, where data are sampled sequentially and immediately but only labeled as “deleted” by setting the fast incremental update is necessary. attribute deleted to true (see Data Structure 1, Line 6). Algorithm 1: Build a balanced k-d tree Algorithm 2: Box-wise Updates

Input: V,N ⊲ Point Array and Point Number Input: CO ⊲ Operation box Output: RootNode ⊲ K-D Tree Node T ⊲ K-D Tree Node 1 RootNode = Build(V, 0,N − 1); SW ⊲ Switch of Parallelly Re-building 2 Function Build(V,l,r) 1 Function BoxwiseUpdate(T, CO, SW ) 3 mid ←⌊(l + r)/2⌋; 2 Pushdown(T ); 4 Axis ← Axis with Maximal Covariance; 3 CT ← T.range; 5 V ← sort(V,axis); 4 if CT ∩ CO = ∅ then return; 6 Node T ; 5 if CT j CO then 7 T.point ← V [mid]; 6 UpdateLazyLabel(); 8 T.axis ← Axis; 7 T.pushdown = true; 9 T.leftson ← Build(V,l,mid − 1); 8 return; 10 T.rightson ← Build(V,mid +1, r); 9 else 11 LazyLabelInit(T ); 10 P ← T.point; 12 Pullup(T ); 11 if P ⊂ CO then Modify T.deleted; 13 return T ; 12 BoxwiseUpdate(T.leftson,CO, SW ); 14 End Function 13 BoxwiseUpdate(T.rightson, CO, SW ); 14 end 15 Pullup(T ); If all nodes on the sub-tree rooted at T have been deleted, 16 if ViolateCriterion(T ) then the attribute treedeleted of T is set to true. Therefore the 17 if T.treesize < Nmax or Not SW then attributes deleted and treedeleted are called lazy labels. If 18 Rebuild(T ) points labeled as “deleted” but not removed are later inserted 19 else to the tree, it is referred to as “re-insertion” and is efficiently 20 ThreadSpawn(ParallelRebuild, T ) achieved by simply setting the deleted attribute back to false. 21 end Otherwise, points labeled as “deleted” will be removed from 22 end the tree during re-building process(see Section III-D). 23 End Function Our incremental updates support two types: point-wise updates and box-wise updates. The point-wise updates insert, delete, or re-insert a single point on the tree while the which is similar to the scapegoat k-d tree [24]. For point-wise box-wise updates insert, delete or re-insert all points in a insertion, the algorithm searches down from the root node given box aligned with the data coordinate axis. Box-wise recursively and compare the coordinate on division axis of updates may require to delete or re-insert an entire sub-tree the new point with the points stored on the tree nodes until rooted at T . In this case, recursively updating the lazy labels a leaf node is found to append a new tree node. For delete deleted and treedeleted for all offspring nodes of T are or re-insertion of a point P , the algorithm finds the tree still inefficient. To address this issue, we use a further lazy node storing the point P and modifies the attribute deleted. strategy to update the lazy labels of the offspring nodes. Further details can be found in our Github repository1. The lazy label for lazy labels deleted and treedeleted is 3) Box-wise Updates: The box-wise insertion is imple- pushdown (see Data Structure 1, Line 6). The three labels mented by inserting the new points one by one into the incre- deleted, treedeleted, and pushdown are all initialized as mental k-d tree. Other box-wise updates (box-wise delete and false in LazyLabelInit (see Algorithm 1, Line 11). re-insertion) are implemented utilizing the range information 1) Pushdown and Pullup: Two supporting functions, in attribute range, which forms a box CT , and the lazy labels Pushdown and Pullup, are designed to update attributes on the tree nodes. The pseudo code is shown in Algorithm on a tree node T . The Pushdown function copies the labels 2. Given the box of points CO to be updated on (sub-) tree deleted, treedeleted, and pushdown of T to its sons (but rooted at T , the algorithm first passes down its lazy labels to not further offsprings) when the attribute pushdown is true. its sons for further passing-down if visited (Line 2). Then, The Pullup function summarizes the information of the it searches the k-d tree from its root node recursively and sub-tree rooted at T to the following attributes of node T : checks whether the range CT on the (sub-)tree rooted at treesize (see Data Structure 1, Line 5) saving the number the current node T has an intersection with the box CO. of all nodes on the sub-tree, invalidnum saving the number If there is no intersection, the recursion returns directly of nodes labelled as “deleted” on the sub-tree, and range without updating the tree (Line 4). If the box CT is fully (see Data Structure 1, Line 7) summarising the range of all contained in the box CO, the box-wise delete set attributes points on the sub-tree along coordinate axis, where k is the deleted and treedeleted to true while the box-wise re- points dimension. insertion set them to false by function UpdateLazyLabel 2) Point-wise Updates: The point-wise updates on the (Line 6). The pushdown attribute is set to true indicating incremental k-d tree are implemented in a recursive way that the latest incremental updates have not been applied TABLE I Algorithm 3: Downsample COMPARISON OF SUPPORTED INCREMENTAL UPDATES Input: L ⊲ Length of Downsample Cube Static Dynamic Scapegoat P ⊲ New Point ikd-Tree K-D Tree K-D Tree K-D Tree 1 CD ← FindCube(L, P ) Insert ✗ ✓ ✓ ✓ 2 Pcenter ← Center(CD); Point- Delete ✗ ✗ ✓ ✓ 3 V ← BoxwiseSearch(RootNode, CD); wise Re-insert ✗ ✗ ✗ ✓ 4 V.push(P ); Insert ✗ ✓ ✓ ✓ 5 FindNearest ; Box- Pnearest ← (V, Pcenter) Delete ✗ ✗ ✗ ✓ wise 6 BoxwiseDelete(RootNode, CD) Re-insert ✗ ✗ ✗ ✓ 7 PointwiseInsert(RootNode, Pnearest); Downsample ✗ ✗ ✗ ✓

D. Re-balancing Our ikd-Tree actively monitors the balance property of the incremental k-d tree and dynamically re-balance it by partial re-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-d tree is rooted at T . The sub-tree is α-balanced if and only if it satisfies the following condition: (a) (b) S(T.leftson) < αbalS(T ) − 1 Fig. 2. Point Cloud Downsample. (a): the point cloud before down- (1) sampling. (b): the point cloud after down-sampling S(T.rightson) < αbalS(T ) − 1

where αbal ∈ (0.5, 1) and S(T ) is the treesize attribute of the node T . to the offspring nodes of T . For the condition that CT The α-deleted criterion of the sub-tree rooted at T is intersects but not contained in CO, the current point P is firstly deleted from or re-inserted to the tree if it is contained I(T ) < αdelS(T ) (2) in CO (Line 11), after which the algorithm looks into the son nodes recursively (Line 12-13) and updates all attributes where αdel ∈ (0, 1) and I(T ) denotes the number of invalid of the current node T (Line 15). Line 16-22 re-balance the nodes on the sub-tree (i.e., the attributes invalidnum of tree if certain criterion is violated (Line 16) by re-building node T ). the tree in the same (Line 18) or a separate (Line 20) If a sub-tree of the incremental k-d tree meets both thread. The function ViolateCriterion, Rebuild and criterion, the sub-tree is balanced. The entire tree is balanced ParrallelRebuild are detailed in Section III-D. if all sub-trees are balanced. Violation of either criterion will trigger a re-building process to re-balance that (sub-) tree: the 4) Downsample: Our ikd-Tree further supports down- α-balanced criterion maintains the maximum height of the sampling as detailed in Algorithm 3. For the given point (sub-) tree. It can be easily proved that the maximum height P and down-sampling resolution L, the algorithm partitions of an α-balanced tree is log1/αbal (n) where n is the tree size; the space evenly into cubes of length L, then the box CD that the α-deleted criterion ensures invalid nodes (i.e., labeled as contains point P is found (Line 1). The algorithm only keeps “deleted”) on the (sub-) trees are removed to reduce tree the point that is nearest to the center Pcenter of CD (Line size. Reducing height and size of the k-d tree allows highly 2). This is achieved by firstly searching all points contained efficient incremental operations and queries in future. The in CD on the k-d tree and stores them in a point array function ViolateCriterion in Algorithm 2, Line 16 V together with the new point P (Line 3-4). The nearest returns true if either criterion is violated. point Pnearest is obtained by comparing the distances of 2) Re-build: Assuming re-building is triggered on a sub- each point in V to the center Pcenter (Line 5). Then existing tree T (see Fig. 3), the sub-tree is firstly flattened into a points in CD are deleted (Line 6), after which the nearest point storage array V . The tree nodes labeled as “deleted” point Pnearest is inserted to the k-d tree (Line 7). The are discarded during flattening. A new perfectly balanced k-d implementation of box-wise search is similar to the box- tree is then built with all points in V by Algorithm 1. wise delete and re-insertion (see Algorithm 2). An example 3) Parallel Re-build: An evident degradation of real-time of downsample is shown in Fig. 2. ability is observed when re-building a large sub-tree on In summary, Table I shows the comparison of supported the incremental k-d tree. To preserve high real-time ability, incremental updates on the static k-d tree [1], the dynamic we design a double-thread re-building method: the main k-d tree [23], the scapegoat k-d tree [24] and our ikd-Tree. thread only re-builds sub-trees whose size is smaller than Algorithm 4: Parallelly Rebuild for Re-balancing Input: T ⊲ Root node of T for re-building 1 Function ParallelRebuild(T ) 2 LockUpdates(T ); 3 V ← Flatten(T); 4 Unlock(T ); ′ 5 T ← Build(V,0,size(V)-1); Fig. 3. Re-build an unbalanced sub-tree 6 foreach op in OperationLogger do ′ 7 IncrementalUpdates(T ,op,false) a predetermined value Nmax and the second thread re-builds 8 end the rest. The key problem is how to avoid information lose 9 T ← T ; and memory conflicts between the main thread and the temp 10 LockAll(T ); second thread. ′ 11 T ← T ; The re-building algorithm on the second thread is shown 12 Unlock(T ); in Algorithm 4. Denote the sub-tree to re-build in the second 13 Free(T ); thread as T and its root node as T . The second thread temp 14 End Function will lock all incremental updates (i.e., points insert, re-insert, and delete) but not queries on this sub-tree (Line 2). Then the second thread copies all valid points contained in the re-building. sub-tree T into a point array V (i.e. flatten) while leaving 1) Incremental Operations: The time complexity of point- the original sub-tree unchanged for possible queries during wise operations is given as the re-building process (Line 3). After the flattening, the Lemma 1 (Point-wise Operations): An incremental k-d sub-tree is unlocked for the main thread to take further tree can handle a point-wise incremental operation with time requests of incremental updates (Line 4). These requests will complexity of O(log n) where n is the tree size. be suspended and recorded in a queue named as operation Proof: The maximum height of an incremental k-d logger. Once the second thread completes building a new ′ tree can be easily proved to be log (n) from Eq. (1) balanced k-d tree T from the point array V (Line 5), the 1/αbal while that of a static k-d tree is log n. Hence the lemma recorded update requests will be performed on the balanced 2 ′ is directly obtained from [1] where the time complexity of sub-tree T by function IncrementalUpdates (Line 6- point insertion and delete on a k-d tree was proved to be 8) where the parallel re-building option is set to false (as it O(log n). The point-wise re-insertion modifies the attribute is already in the second thread). After all pending requests deleted on a tree node thus the time complexity is the same are processed, the algorithm locks the node T from both as point-wise delete. incremental updates and queries and replace it with the new ′ The time complexity of box-wise operations on an incre- one T (Line 9-12). Finally, the algorithm frees the memory mental k-d tree is: of the original sub-tree (Line 13). Note that LockUpdates does not block queries, which can be conducted parallelly Lemma 2 (Box-wise Operations): An incremental 3-d tree in the main thread. In contrast, LockAll blocks all access handles box-wise insertion of m points in CD with time including queries, but it finishes very quickly (i.e., only one complexity of O(m log n). Furthermore, suppose points on instruction), allowing timely queries in the main thread. The the 3-d tree are in space Sx ×Sy ×Sz and CD = Lx ×Ly × function LockUpdates and LockAll are implemented by Lz. The box-wise delete and re-insertion can be handled with mutual exclusion (mutex). time complexity of O(H(n)), where O(log n) if∆ > α( 2 )(*) E. K-Nearest Neighbor Search  min 3 1−a−b−c 1 The nearest search on the incremental k-d tree is an O(n ) if∆max 6 1 − α( 3 )(**)  1 − −  α( 3 ) ∆min ∆med accurate nearest search [2] instead of an approximate one as O(H(n)) = O(n ) if (*) and (**) fail and [27]. The function Pushdown is applied before searching 1 2 ∆med < α( 3 ) − α( 3 )  2 − the sub-tree rooted at node T to pass down its lazy labels. We  α( 3 ) ∆min otherwise. O(n ) use the attribute range to speed up the search process thus  (3) hard real-time ability is preserved. Due to the space limit, Sx Sy Sz where a = logn , b = logn and c = logn with the details of k-nearest search algorithm is not presented in Lx Ly Lz a,b,c > 0. ∆min, ∆med and ∆max are the minimal, median this paper. Interested readers can refer to the related codes and maximal value among a, b and c. α(u) is the flajolet- in our open source library. puech function with u ∈ [0, 1], where particular value is provided: α( 1 )=0.7162 and α( 2 )=0.3949 IV. COMPLEXITY ANALYSIS 3 3 Proof: The box-wise insertion is implemented by point- A. Time Complexity wise insertion thus the time complexity can be directly The time complexity of ikd-Tree breaks into the time for obtained from Lemma 1. An asymptotic time complexity incremental operations (insertion, re-insertion and delete) and for range search on a k-d tree is provided in [29]. The TABLE II Incremental Updates THE PARAMETERS SETUP OF IKD-TREE IN EXPERIMENTS ikd-Tree 40 Static K-D Tree Randomized Data LiDAR Inertial-Odometry Mapping (a) 0.6 Nmax 1500 1500 20 0.3 αbal 0.6 0.6 750 800 αdel 0.5 0.5 Time/ms Run CD - 0.2m × 0.2m × 0.2 0 100 200 300 400 500 600 700 800 900 1000 Nearest Search box-wise delete and re-insertion can be regarded as a range ikd-Tree search except that lazy labels are attached to the tree nodes. 2 Static K-D Tree Therefore, the conclusion of range search can be applied to (b) 1

the box-wise delete and re-insertion on the incremental k-d Run Time/ms Run tree. 0 100 200 300 400 500 600 700 800 900 1000 The down-sampling method on an incremental k-d tree Total Time Consumption 10 5 is composed of box-wise search and delete followed by 60 2 ikd-Tree the point insertion. By applying Lemma 1 and Lemma 2, 1.5 40 Static K-D Tree Total Point Number the time complexity of downsample is + . 1.6 O(log n) O(H(n)) (c) 1 1.3 Generally, the downsample hypercube CD is very small 20

750 800 0.5 Run Time/ms Run

comparing with the entire space. Therefore, the normalized Number Point 0 0 range ∆x, ∆y and ∆z are small and the value of ∆min 100 200 300 400 500 600 700 800 900 1000 satisfies the condition (*) for time complexity of O(log n). Operation Counter Hence, the time complexity of down-sampling is O(log n). Fig. 4. The time performance comparison between an ikd-Tree and a static 2) Re-build: Time complexity for re-building breaks into k-d tree. two types: single-thread re-building and parallel two-thread re-building. In the former case, the re-building is performed experiment generates 5,000 points randomly in a 10m × by the main thread in a recursive way, each level takes the 10m×10m space (i.e., the workspace) to initialize the incre- time of sorting (i.e., O(n)) and the total time over log n levels mental k-d tree. Then 1,000 test operations are conducted on is O(n log n) [1] when the the dimension k is low (e.g., 3 the k-d tree. In each test operation, 200 new points randomly in most robotic applications). For parallel re-building, the sampled in the workspace are inserted (point-wise) to the kd- time consumed in the main thread is only flattening (which tree. Then another 200 points are randomly sampled in the suspends the main thread from further incremental updates, workspace and searched on (but not inserted to) the k-d tree Algorithm 4, Line 2-4) but not building (which is performed for 5 nearest points of each. For every 50 test operations, in parallel by the second thread, Algorithm 4, Line 5) or tree 4 cubes are sampled in the workspace with side length of update (which takes constant time O(1), Algorithm 4, Line 1.5m and points contained in these 4 cubes are deleted (box- 10-12), leading to a time complexity of O(n). In summary, wise) from the k-d tree. For every 100 test operations, 2,000 the time complexity of re-building an incremental k-d tree is new points are sampled in the workspace and inserted (point- O(n) for two-thread parallel re-building and O(n log n) for wise) to the k-d tree. We compare the ikd-Tree with the static single-thread re-building. k-d tree used in point cloud library [15] where at each test 3) Nearest Search: For robotic applications, the points operation the k-d tree is entirely re-built. The experiments are dimension is usually very small. Hence the time complexity performed on a PC with Intel i7-10700 CPU at 2.90GHz and of k-nearest search on the incremental k-d tree can be simply only 2 threads running. The parameters of the incremental approximated as O(log n) because the maximum height of k-d tree are summarized in Table II where no down-sampling the incremental k-d tree is maintained no larger than log 1 n. is used to allow a fair comparison. Also the maximal point α number allowed to store on the leaf node of a static k-d tree B. Space Complexity is set to 1 while the original setting in point cloud library is As shown Section III, each node on the incremental k- 15. d tree records point information, tree size, invalid point The results of the first experiment are shown in Fig. 4, number and point distribution of the tree. Extra flags such where the point number increases from 5,000 to approximate as lazy labels are maintained on each node for box-wise 200,000. In this process, the time for incremental updates operations. For an incremental k-d tree with n nodes, the (including both incremental operations and re-building) on space complexity is O(n) though the space constant is a the ikd-Tree remains stably around 1.6 ms while that for few times larger than a static k-d tree. the static k-d tree grows linearly with number of the points (see Fig. 4(a)). The high peaks in the time consumption V. APPLICATION EXPERIMENTS are resulted from the large-scale point-wise insertion (and A. Randomized Data Experiments associated re-balancing) and the low peaks are resulted from The efficiency of our ikd-Tree is fully investigated by two box-wise delete (and associated re-balancing). As shown in experiments on randomized incremental data sets. The first Fig. 4(b), the time performance of the k-nearest search on the (a) Sparse Increments (b) Compact Increments Time Performance on FAST-LIO 20 ikd-Tree Static K-D Tree

(a) 10 Run Time/ms Run 0 300 310 320 330 340 350 360 370 380 390 400 410 (c) Time Performance on Incremental Updates time/s 10 2 Sparse 1 10 Compact 10 0 (b) 10 5 10 -1 ikd-Tree

10 -2 Static K-D Tree Run Time/ms Run Run Time/ms Run 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 10 -3 Nearest Search Incremental Updates Total Time Point Number 10 5 Re-balancing Criterion Fig. 5. Fig. (a) and (b) illustrate new points (orange triangles) and points 0.8 already on the k-d tree (blue dots). Fig. (c) shows the time for incremental updates of sparse and compact data on k-d trees of different size. 0.6 (c) 0.4 bal ikd-Tree is slightly slower than an static k-d tree, possibly 0.2 del due to the highly optimized implementation of the PCL 0 300 310 320 330 340 350 360 370 380 390 400 410 library. Despite of the slightly lower efficiency in query, the time/s overall time consumption of ikd-Tree outperforms the static k-d tree by one order of magnitude (See Fig. 4(c)). Fig. 6. Fig. (a) shows the average running time of fusing one new lidar scan in FAST LIO using the ikd-Tree and a static k-d tree. Fig. (b) shows The second experiment investigates the time performance time for nearest search, incremental updates, and total time in fusing one of incremental updates for new points of different distribu- lidar scan. Fig. (c) shows the balance property after re-building on main tion. In the experiment, we sample two sets of 4,000 new thread. points in a 10m × 10m × 10m space (i.e., the workspace): one is evenly distributed (i.e., sparse data, see Fig. 5 (a)) and the other concentrated in a 2.5m × 2.5m × 2.5m space (i.e., compact data, see Fig. 5 (b)). The sparse and compact data are inserted to an existing incremental k-d tree of different size but all sampled in the workspace. Fig. 5(c) shows the running time of sparse and compact point-wise insertion on k-d trees of different size. As expected, the incremental updates for compact data are slower than the sparse one Fig. 7. Mapping Result of the Main Building, University of Hong Kong. because re-building are more likely to be triggered when The green line is the path of the lidar computed by FAST LIO. inserting a large amount of points into a small sub-tree of a k-d tree.

B. LiDAR Inertial-Odometry and Mapping scene using a Livox Avia LiDAR3 [30] with 70°FoV and a high frame rate of 100 Hz. All the algorithm is running We test our developed ikd-Tree in an actual robotic on the DJI Manifold 2-C4 with a 1.8 GHz quad-core Intel application: lidar-inertial odometry (and mapping) presented i7-8550U CPU and 8 GB RAM. in [3]–[8]. In this application, k-d tree-based nearest points The time of fusing a new lidar scan in FAST LIO is search is crucial to match a point in a new LiDAR scan to its shown in Fig. 6(a). The time is the averaged time of the correspondences in the map (or the previous scan). Since the most recent 100 scans. It is seen that the ikd-Tree achieves map is dynamically growing by matching and merging new a nearly constant time performance around 1.6 ms, which scans, the k-d tree has to be re-built every time a new scan fundamentally enables a mapping rate up to 100Hz (against is merged. Existing methods [3]–[8] commonly used static 10Hz presented in the original work [8] using static k-d kd-tree from PCL and rebuilds the entire tree based on all tree). On the other hand, the time with the static k-d tree is points in the map (or a submap). This leads to a significant overall increasing linearly, and decreases occasionally due to computation costs severely limiting the map update rate (e.g., the small overlap between the lidar current FoV and the map. from 1Hz [5]–[7] to 10Hz [8]). The resultant processing time with the static k-d tree exceeds In this experiment, we replace the static k-d tree (build, up- 10 ms from 366s on, which is more than the collection time date, and query) by our ikd-Tree, which enables incremental of one lidar scan. update of the map by updating the new points only. We test The time for fusing a new lidar scan consists of many ikd-Tree on the lidar-inertial mapping package FAST LIO in operations, such as point registration, state estimation, and [8]2. The experiment is conducted on a real-world outdoor 3https://www.livoxtech.com/de/avia 2https://github.com/hku-mars/FAST_LIO 4https://www.dji.com/manifold-2/specs kd-tree related operations (including queries and update). [10] F. Gao and S. Shen, “Online quadrotor trajectory generation and The time breakdown of kd-tree related operations is shown autonomous navigation on point clouds,” in 2016 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR). IEEE, in Fig. 6(b). The average time of incremental updates for 2016, pp. 139–146. ikd-Tree is 0.23 ms which is only 4% of that using an static [11] B. T. Lopez and J. P. How, “Aggressive 3-d collision avoidance for k-d tree (5.71 ms). The average time of nearest search using high-speed navigation.” in ICRA, 2017, pp. 5759–5765. [12] P. R. Florence, J. Carter, J. Ware, and R. Tedrake, “Nanomap: the ikd-Tree and an static k-d tree are at the same level. Fast, uncertainty-aware proximity queries with lazy search over local Furthermore, Fig. 6(c) investigates the balancing property 3d data,” in 2018 IEEE International Conference on Robotics and of the incremental kd-tree by examining the two criterions Automation (ICRA). IEEE, 2018, pp. 7631–7638. [13] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online αbal and αdel, which are defined as: trajectory generation and autonomous navigation for quadrotors in cluttered environments,” Journal of Field Robotics, vol. 36, no. 4, max S(T.leftson),S(T.rightson) pp. 710–733, 2019. n o [14] J. Ji, Z. Wang, Y. Wang, C. Xu, and F. Gao, “Mapless-planner: A αbal(T )= S(T ) − 1 (4) robust and fast planning framework for aggressive autonomous flight I(T ) without map fusion,” arXiv preprint arXiv:2011.03975, 2020. αdel(T )= [15] R. B. Rusu and S. Cousins, “3d is here: Point cloud library (pcl),” S(T ) in 2011 IEEE international conference on robotics and automation. IEEE, 2011, pp. 1–4. As expected, the two criterion are maintained below the pre- [16] R. Bayer, “Symmetric binary b-trees: Data structure and maintenance scribed thresholds (see Table II) due to re-building, indicating algorithms,” Acta informatica, vol. 1, no. 4, pp. 290–306, 1972. [17] C. R. Aragon and R. Seidel, “Randomized search trees,” in FOCS, that the kd-tree is well-balanced through the incremental up- vol. 30, 1989, pp. 540–545. dates. The peaks over the thresholds is resulted from parallely [18] D. D. Sleator and R. E. Tarjan, “Self-adjusting binary search trees,” re-building, which drop quickly when the re-building is done. 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 with Finally, Fig. 7 shows the 100Hz mapping results. an adaptive error-bounded heuristic,” in 2006 IEEE Symposium on Interactive Ray Tracing. IEEE, 2006, pp. 81–88. VI. CONCLUSION [20] S. Popov, J. Gunther, H.-P. Seidel, and P. Slusallek, “Experiences with streaming construction of sah kd-trees,” in 2006 IEEE Symposium on This paper proposed an efficient data structure, ikd-Tree, to Interactive Ray Tracing. IEEE, 2006, pp. 89–94. [21] M. Shevtsov, A. Soupikov, and A. Kapustin, “Highly parallel fast incrementally update a k-d tree in robotic applications. The kd-tree construction for interactive ray tracing of dynamic scenes,” ikd-Tree supports incremental operations in robotics while Computer Graphics Forum, vol. 26, no. 3, pp. 395–404, 2007. maintaining balanced by partial re-building. We provided a [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, complete analysis of time and space complexity to prove the no. 5, pp. 1–11, 2008. high efficiency of the proposed dynamic structure. The ikd- [23] J. L. Bentley and J. B. Saxe, “Decomposable searching problems i: Tree was tested on a randomized experiment and an outdoor Static-to-dynamic transformation,” J. algorithms, vol. 1, no. 4, pp. LiDAR odometry and mapping experiment. In all tests, the 301–358, 1980. [24] I. Galperin and R. L. Rivest, “Scapegoat trees.” in SODA, vol. 93, proposed data structure achieves two orders of magnitude 1993, pp. 165–174. higher efficiency. [25] O. Procopiuc, P. K. Agarwal, L. Arge, and J. S. Vitter, “Bkd-tree: A dynamic scalable kd-tree,” in International Symposium on Spatial and Temporal Databases. Springer, 2003, pp. 46–65. REFERENCES [26] J. T. Robinson, “The kdb-tree: a search structure for large multidimen- sional dynamic indexes,” in Proceedings of the 1981 ACM SIGMOD [1] J. L. Bentley, “Multidimensional binary search trees used for asso- international conference on Management of data, 1981, pp. 10–18. ciative searching,” Communications of the ACM, vol. 18, no. 9, pp. [27] M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with 509–517, 1975. automatic algorithm configuration.” VISAPP (1), vol. 2, no. 331-340, [2] J. H. Friedman, J. L. Bentley, and R. A. Finkel, An algorithm for p. 2, 2009. finding best matches in logarithmic time. Department of Computer [28] M. Muja and D. Lowe, “Flann-fast library for approximate nearest Science, Stanford University, 1975. neighbors user manual,” Computer Science Department, University of [3] A. Nuchter, K. Lingemann, and J. Hertzberg, “Cached kd tree search British Columbia, Vancouver, BC, Canada, 2009. for icp algorithms,” in Sixth International Conference on 3-D Digital [29] P. Chanzy, L. Devroye, and C. Zamora-Cura, “Analysis of range search Imaging and Modeling (3DIM 2007). IEEE, 2007, pp. 419–426. for random kd trees,” Acta informatica, vol. 37, no. 4-5, pp. 355–383, [4] A. Segal, D. Haehnel, and S. Thrun, “Generalized-icp.” in Robotics: 2001. science and systems, vol. 2, no. 4. Seattle, WA, 2009, p. 435. [30] Z. Liu, F. Zhang, and X. Hong, “Low-cost retina-like robotic [5] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real- lidars based on incommensurable scanning,” arXiv preprint time.” in Robotics: Science and Systems, vol. 2, no. 9, 2014. arXiv:2006.11034, 2020. [6] T. Shan and B. Englot, “Lego-loam: Lightweight and ground- optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4758–4765. [7] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3126–3131. [8] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” arXiv preprint arXiv:2010.08196, 2020. [9] J. Ichnowski and R. Alterovitz, “Fast nearest neighbor search in se (3) for sampling-based motion planning,” in Algorithmic Foundations of Robotics XI. Springer, 2015, pp. 197–214.