This repository provides implementation of an incremental k-d tree for robotic applications.

Overview

ikd-Tree

ikd-Tree is an incremental k-d tree designed for robotic applications. The ikd-Tree incrementally updates a k-d tree with new coming points only, leading to much lower computation time than existing static k-d trees. Besides point-wise operations, the ikd-Tree supports several features such as box-wise operations and down-sampling that are practically useful in robotic applications.

Developers

Yixi Cai 蔡逸熙: Data structure design and implementation

Wei Xu 徐威: Incorporation into LiDAR-inertial odometry package (FAST_LIO. It will be upgraded to FAST_LIO 2.0 in March, 2021.)

More details please refer to our paper and video~

Related paper

Related paper available on arxiv:

ikd-Tree: An Incremental K-D Tree for robotic applications

Related video: https://youtu.be/ueOunk03zxA

Issues
  • deleted points can still be found in kdtree.

    deleted points can still be found in kdtree.

    I run ikd-tree with a running sliding window of point cloud. pointclouds are queued in time_stamp sequential. each point are added to ikdtree and then delete later. But as I was checking kdtree points with these code: if(0) //If you need to see map point, change to "if(1)" { PointVector ().swap(ikdtree.PCL_Storage); ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD); featsFromMap->clear(); featsFromMap->points = ikdtree.PCL_Storage; } then publish these point clouds. I found there are always points not deleted, i.e. they remain in the kdtree even though deleted.

    Another question, what if i add_Points with downsample_flag=True. Then delete original point later. Will ikdtree delete this cuboid? what behavior will ikdtree generate?

    opened by DingFeng9313 10
  • error about Nearest_Search with multi thread

    error about Nearest_Search with multi thread

    Hi, thanks for your great works. ikdtree works well with one thread but not with multi thread. the output are " *** Error in `./ikdtreeLoclization': double free or corruption (fasttop): 0x00007fcd1c000b50 *** " and the test demo are:

        std::vector<PointType ,Eigen::aligned_allocator<PointType>> pointNear;
        std::vector<float> nearDis(nearNum);
        
        omp_set_num_threads(3);
        #pragma omp parallel for 
        for (int j = 0; j < lidarCurW->size() ; j++)
        {
                PointType pointJ=lidarCurW->points[j];
                ikdtree.Nearest_Search(pointJ,nearNum,pointNear,nearDis,5.0);
     }
    

    hope for your reply, thanks

    opened by HeXu1 5
  • Separate includes files and example files / Add additional, visualized examples / Elaborate README.md

    Separate includes files and example files / Add additional, visualized examples / Elaborate README.md

    Dear @Ecstasy-EC:

    Hello there Yixi, I'd like to do PR to your ikd-tree repository. Your fantastic method deserves to be nicer!

    [Features]

    • Separate includes files and example files
    • Add two examples (Also, I follow your code format...In fact I hate your combination format of the Snake case and Pascal case lol)
      • In particular, the public function Search_Points_by_box is added. Please refer to codes.
    • Update README
      • In particular, build commands are added
      • Visualized results are added

    As you're an HKUSTian, so I utilized livox-loam to get the Red Bird Sundial pcd file :smile:

    In addition, my large_scale_map.pcd is used for an example 3, i.e., the async example. I think this file can be substituted by any file. If you'd like to use this file, it would also be better to upload large_scale_map.pcd on your lab's official drive (e.g. Google drive). If then, please README.md appropriately!

    Cheers~

    opened by LimHyungTae 4
  • Weird phenomenon on `Delete_Point_Boxes` function

    Weird phenomenon on `Delete_Point_Boxes` function

    Dear Admin:

    First, thank you for sharing your invaluable codes.

    But, I'd like to use your incremental Tree algorithm to achieve radius search; unfortunately, I think the code doesn't support the radius search function.

    Is there any particular issue not implementing radius search?

    Thanks in advance!

    opened by LimHyungTae 4
  • transformed KDClass into templated class for easy integration with PC…

    transformed KDClass into templated class for easy integration with PC…

    transformed KDClass into templated class for easy integration with PCL and custom point types. All external struct and classes have been moved inside KDTree class for uniformization

    I've also changed the demo.cpp to conform with new usage. My next intention is to write a PCL wrapper to use as a PCL::Search object

    enhancement 
    opened by Marcus-Davi 4
  • Rebuild_Ptr initialization problem

    Rebuild_Ptr initialization problem

    Hi, Nice work! I found you didn't initialize the variable of "Rebuild_Ptr" in your code. When I run your demo, everything seems ok, but when I use your code into my own project with ROS it always leads to problem crash, I solved this problem after I set the initial value of "Rebuild_Ptr" to nullptr. I can't sure is this a bug or I just somehow make my program right. So Can you help me to see if this problem will cause the program to crash? Best wish.

    opened by HeadReaper-hc 4
  • can this kdtree delete points lightweight without reconstruct a new kdtree

    can this kdtree delete points lightweight without reconstruct a new kdtree

    hi Thanks for sharing your work, it is a great improvement that make kdtree cost less time when add new points, however, if I want to delete some points, can this framework handle without build a new kdtree?

    opened by xieqi1 3
  • 关于里程计里应用ikd-tree的一些问题

    关于里程计里应用ikd-tree的一些问题

    你好,非常感谢分享工作! 我有一些疑问,还请解惑: 1.一般在里程计的scan2map过程,是需要找到当前帧附近邻域内(比如50米)的历史的关键帧,也就是目前代码中的localmap(我认为),如果里程计一直是单向在跑,我认为ikd-tree一直增量的更新这个树,确实没问题,但是一旦调头了,那ikd-tree不是已经把历史的一些点给删除了吗,所以这里会不会存在问题呢?换句话说就是ikd-tree能否保留历史关键帧上的点呢? 2.在测试的时候我发现ikd-tree的size()一直在增大,是不是我忽略了什么细节? 3.对这个ikdtree非常感兴趣,可否加个微信什么的方便私底下询问,我的是16621710962。万分感谢了

    opened by yxallnyy 2
  • ‘acquire_removed_points()’ not collect the points deleted in the downsample process ?

    ‘acquire_removed_points()’ not collect the points deleted in the downsample process ?

    I did some tests and read the code. It seems that ikd-tree does not collect those points deleted in the downsample process. Can I get the missing points from "Downsample_Storage" ?

    (I really need those points, because I need to do object tracking with them.)

    PCL kdtree provides index to mark origin map points and all points are managered out of kdtree, but ikd-tree does not have index.

    Now I define an embedded index uint32_t value in "PointType" to solve this problem, but it's not perfect because it's growing and will overflow soon.

    opened by Woodpecker0 2
  • I wonder whether Nearest_Search support multi-thread operation ?

    I wonder whether Nearest_Search support multi-thread operation ?

    Thanks for your work, it's elegant and useful.

    I would like to use nearest search in a openmp section, eg:

    ` #pragma omp parallel for num_threads(4) for (int i = 0; i < laserCloudSurfLastDSNum; i++) { PointType pointOri, pointSel, coeff; std::vector pointSearchInd; std::vector pointSearchSqDis;

    pointOri = laserCloudSurfLastDS->points[i];
    pointAssociateToMap(&pointOri, &pointSel); 
    kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis); //replaced by ikd-tree
    

    } ` Is it possible to do multiple search at the same time ? It seems that a mutex is used within the Nearest_Search(), which means that parallel operations actually save no time. Am i right ?

    opened by Woodpecker0 2
  • max_dist in search function

    max_dist in search function

    Thanks for releasing great works.

    "ikd_Tree.cpp", 886 line. if (dist <= max_dist && (q.size() < k_nearest || dist < q.top().dist)) is it right?

    or if (dist <= max_dist*max_dist && (q.size() < k_nearest || dist < q.top().dist))

    opened by traum100 1
  • serious issue about nth_element behavior

    serious issue about nth_element behavior

    I am doing some rewrite of this ikdtree to adapt my situation. And I find you used nth_element while building a tree. According to my experiments there might be a bug.

    Suppose at some point division-axis is 0, which is x-axis, let's use 1-D data to explain:

    Suppose we have a vector [0,1,2,2,2,2,2,2,3,4] which is points' x-axis value after nth_element() function call, y-axis value is totally different, say [0,1,2,3,4,5,6,7,8,9]。nth_element() divide these points into different group but not determinately which point is on left or right node. point (2,2) might be on the left or right.

    Thus while searching, or delete or add by point. it not guaranteed to find this point.

    opened by DingFeng9313 1
  • Better C++ compatibility for branch fast_lio

    Better C++ compatibility for branch fast_lio

    https://github.com/hku-mars/ikd-Tree/issues/22

    Right now the header file is "using namespace std". This is considered bad practice, but easy to fix.

    Additionally <unistd.h> is used to put the current thread to sleep. This is not platform independent, but easily replaced by C++ std library functionality.

    opened by xaedes 1
  • Better C++ compatibility

    Better C++ compatibility

    Right now the header file is "using namespace std". This is considered bad practice, but easy to fix.

    Additionally <unistd.h> is used to put the current thread to sleep. This is not platform independent and can easily be replaced by C++ std library functionality.

    opened by xaedes 0
  • gpu acceleration and parallellism

    gpu acceleration and parallellism

    Hi, from what I can see in the code, you are using multiple threads, but all of the calculations are done by the CPU correct? Would it be possible to adjust the code to make use of the GPU using something like CUDA to be able to achieve even better performance?

    opened by Jaetriel 1
Owner
HKU-Mars-Lab
Mechatronics and Robotic Systems (MaRS) Laboratory
HKU-Mars-Lab
This is like Inverting Binary Tree, but instead of a Binary Tree it's a File Tree.

Invert File Tree in C++ This is like Inverting Binary Tree, but instead of the Binary Tree it's a File Tree. This is intended as a simple exercise to

Tsoding 11 Jun 18, 2021
Implementation of K-D tree in C++ programming language.

KD_Trees Implementation of K-D tree in C++ programming language Demonstration Image What's in this repository anyway? This is a C++(PL) implementation

Shreyash 4 Dec 17, 2021
Public repository for rolling release of main Vector robot code repository.

vector Public repository for rolling release of main Vector robot code repository. This rolling release will be worked to completion until all non-thi

Digital Dream Labs 54 Jun 5, 2022
This repository contains some data structures implementation in C programming language

This repository contains some data structures implementation in C programming language. I wrote the tutorial posts about these data structures on my personal blog site in Bengali language. If you know Bengali then visit my site

Hasan Abdullah 117 Jun 23, 2022
SQL grammar for tree sitter

tree-sitter-sql I want to do something fun at work since we have stuff like this in Go: const hoverDocumentQuery = ` -- source: enterprise/internal/co

TJ DeVries 20 Nov 16, 2021
Tree sitter grammar for Svelte

Tree-sitter-svelte Tree-sitter grammar for svelte Install npm i tree-sitter-svelte tree-sitter Usage To get started with exploring the grammar in a w

Himujjal Upadhyaya 39 Jun 17, 2022
Device Tree for Redmi K30 Ultra

Copyright (C) 2020 PixelExperience Plus Edition Device configuration for Redmi K30 Ultra ========================================= The Redmi K30 Ultra

Xayah 22 Jun 2, 2022
FleakOS Kernel Source Tree

FleakOS FleakOS Kernel Source Tree Dependencies sudo apt-get install -y xorriso sudo apt-get install -y gcc-multilib sudo apt-get install -y nasm sudo

FleakOS 29 Dec 10, 2021
A tree-sitter grammar for go.mod files

tree-sitter-go-mod tree-sitter grammar for go.mod files. Status The grammar is fairly small, and has been working well for highlighting for me. I expe

Camden Cheek 20 Jun 11, 2022
A tree-sitter grammar for HCL (HashiCorp Configuration Language), used by projects such as Terraform.

tree-sitter-hcl tree-sitter grammar for HCL (HashiCorp Configuration Language) files. HCL is the configuration format used by projects such as Terrafo

Mitchell Hashimoto 60 Jun 25, 2022
A tree-sitter grammar for protocol buffer files (proto3).

tree-sitter-proto tree-sitter grammar for protocol buffer files (proto3 only). Status The grammar should be complete. I'm still working on the highlig

Mitchell Hashimoto 39 Jun 26, 2022
Generic parse tree, configurable lexer, `lemon` parser generator, wrapped for C++17 and Python 3.

This project augments the Lemon parser generator with a high-level parse tree interface, grammar action DSL, and an integrated, configurable lexer allowing the creation of an entire standalone, object-oriented parser from a single input grammar file. The entire parser is written in native C/C++, and the parser interface is made comfortably available to both C++ and Python3 applications.

Aubrey R. Jones 11 Jun 16, 2022
Algo-Tree is a collection of Algorithms and data structures which are fundamentals to efficient code and good software design

Algo-Tree is a collection of Algorithms and data structures which are fundamentals to efficient code and good software design. Creating and designing excellent algorithms is required for being an exemplary programmer. It contains solutions in various languages such as C++, Python and Java.

DSC-Banasthali 53 Jun 12, 2022
Build a tree-sitter dynamic module

!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! I should clarify that this module is NOT a standalone tree-sitter module. It is supo

Yuan Fu 4 May 22, 2022
RavencoinLite Core integration/staging tree

RavencoinLite Core integration/staging tree https://ravencoinlite.org What is RavencoinLite? RavencoinLite is an experimental digital currency that en

null 24 Oct 12, 2021
tree-sitter parser and syntax highlighter for the Dwarf Fortress raw language

tree-sitter-dfraw A simple language parser and highlighter made with tree-sitter tokyonight nightfly Using with nvim-treesitter Please refer to the ad

null 2 Apr 1, 2022
HEEx grammer for Tree-sitter

Tree-sitter HEEx Tree-sitter grammar and parser for HEEx, the HTML-aware and component-friendly extension of EEx for Phoenix. For Surface support, see

Connor Lay (Clay) 24 Jun 20, 2022
A clone of Google C++ B-tree

This library is a C++ template library and, as such, there is no library to build and install. Copy the .h files and use them! See http://code.googl

Diego Caro 77 Feb 1, 2022