diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index b8a0efa..0000000 --- a/.gitmodules +++ /dev/null @@ -1,4 +0,0 @@ -[submodule "include/ikd-Tree"] - path = include/ikd-Tree - url = https://github.com/hku-mars/ikd-Tree.git - branch = fast_lio diff --git a/include/ikd-Tree b/include/ikd-Tree deleted file mode 160000 index c0e36a1..0000000 --- a/include/ikd-Tree +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c0e36a16b6e4d557d3783b16911207f6398dd478 diff --git a/include/ikd-Tree/.gitignore b/include/ikd-Tree/.gitignore new file mode 100644 index 0000000..30062c2 --- /dev/null +++ b/include/ikd-Tree/.gitignore @@ -0,0 +1,2 @@ +build/ +/.vscode \ No newline at end of file diff --git a/include/ikd-Tree/CMakeLists.txt b/include/ikd-Tree/CMakeLists.txt new file mode 100644 index 0000000..629324d --- /dev/null +++ b/include/ikd-Tree/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(ikd_tree_demo) + +add_compile_options(-std=c++14) +set(CMAKE_CXX_FLAGS "-std=c++14 -pthread -O3") + +find_package(PCL 1.8 REQUIRED) + +include_directories( + ${PCL_INCLUDE_DIRS} + ikd-Tree +) + +add_executable(ikd_tree_demo examples/ikd_Tree_demo.cpp ikd-Tree/ikd_Tree.cpp) +target_link_libraries(ikd_tree_demo ${PCL_LIBRARIES}) + +add_executable(ikd_tree_async_demo examples/ikd_Tree_Async_demo.cpp ikd-Tree/ikd_Tree.cpp) +target_link_libraries(ikd_tree_async_demo ${PCL_LIBRARIES}) + +add_executable(ikd_tree_Search_demo examples/ikd_Tree_Search_demo.cpp ikd-Tree/ikd_Tree.cpp) +target_link_libraries(ikd_tree_Search_demo ${PCL_LIBRARIES}) \ No newline at end of file diff --git a/include/ikd-Tree/LICENSE b/include/ikd-Tree/LICENSE new file mode 100644 index 0000000..d159169 --- /dev/null +++ b/include/ikd-Tree/LICENSE @@ -0,0 +1,339 @@ + GNU GENERAL PUBLIC LICENSE + Version 2, June 1991 + + Copyright (C) 1989, 1991 Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +License is intended to guarantee your freedom to share and change free +software--to make sure the software is free for all its users. This +General Public License applies to most of the Free Software +Foundation's software and to any other program whose authors commit to +using it. (Some other Free Software Foundation software is covered by +the GNU Lesser General Public License instead.) You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +this service if you wish), that you receive source code or can get it +if you want it, that you can change the software or use pieces of it +in new free programs; and that you know you can do these things. + + To protect your rights, we need to make restrictions that forbid +anyone to deny you these rights or to ask you to surrender the rights. +These restrictions translate to certain responsibilities for you if you +distribute copies of the software, or if you modify it. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must give the recipients all the rights that +you have. You must make sure that they, too, receive or can get the +source code. And you must show them these terms so they know their +rights. + + We protect your rights with two steps: (1) copyright the software, and +(2) offer you this license which gives you legal permission to copy, +distribute and/or modify the software. + + Also, for each author's protection and ours, we want to make certain +that everyone understands that there is no warranty for this free +software. If the software is modified by someone else and passed on, we +want its recipients to know that what they have is not the original, so +that any problems introduced by others will not reflect on the original +authors' reputations. + + Finally, any free program is threatened constantly by software +patents. We wish to avoid the danger that redistributors of a free +program will individually obtain patent licenses, in effect making the +program proprietary. To prevent this, we have made it clear that any +patent must be licensed for everyone's free use or not licensed at all. + + The precise terms and conditions for copying, distribution and +modification follow. + + GNU GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License applies to any program or other work which contains +a notice placed by the copyright holder saying it may be distributed +under the terms of this General Public License. The "Program", below, +refers to any such program or work, and a "work based on the Program" +means either the Program or any derivative work under copyright law: +that is to say, a work containing the Program or a portion of it, +either verbatim or with modifications and/or translated into another +language. (Hereinafter, translation is included without limitation in +the term "modification".) Each licensee is addressed as "you". + +Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running the Program is not restricted, and the output from the Program +is covered only if its contents constitute a work based on the +Program (independent of having been made by running the Program). +Whether that is true depends on what the Program does. + + 1. You may copy and distribute verbatim copies of the Program's +source code as you receive it, in any medium, provided that you +conspicuously and appropriately publish on each copy an appropriate +copyright notice and disclaimer of warranty; keep intact all the +notices that refer to this License and to the absence of any warranty; +and give any other recipients of the Program a copy of this License +along with the Program. + +You may charge a fee for the physical act of transferring a copy, and +you may at your option offer warranty protection in exchange for a fee. + + 2. You may modify your copy or copies of the Program or any portion +of it, thus forming a work based on the Program, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) You must cause the modified files to carry prominent notices + stating that you changed the files and the date of any change. + + b) You must cause any work that you distribute or publish, that in + whole or in part contains or is derived from the Program or any + part thereof, to be licensed as a whole at no charge to all third + parties under the terms of this License. + + c) If the modified program normally reads commands interactively + when run, you must cause it, when started running for such + interactive use in the most ordinary way, to print or display an + announcement including an appropriate copyright notice and a + notice that there is no warranty (or else, saying that you provide + a warranty) and that users may redistribute the program under + these conditions, and telling the user how to view a copy of this + License. (Exception: if the Program itself is interactive but + does not normally print such an announcement, your work based on + the Program is not required to print an announcement.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Program, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Program, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Program. + +In addition, mere aggregation of another work not based on the Program +with the Program (or with a work based on the Program) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may copy and distribute the Program (or a work based on it, +under Section 2) in object code or executable form under the terms of +Sections 1 and 2 above provided that you also do one of the following: + + a) Accompany it with the complete corresponding machine-readable + source code, which must be distributed under the terms of Sections + 1 and 2 above on a medium customarily used for software interchange; or, + + b) Accompany it with a written offer, valid for at least three + years, to give any third party, for a charge no more than your + cost of physically performing source distribution, a complete + machine-readable copy of the corresponding source code, to be + distributed under the terms of Sections 1 and 2 above on a medium + customarily used for software interchange; or, + + c) Accompany it with the information you received as to the offer + to distribute corresponding source code. (This alternative is + allowed only for noncommercial distribution and only if you + received the program in object code or executable form with such + an offer, in accord with Subsection b above.) + +The source code for a work means the preferred form of the work for +making modifications to it. For an executable work, complete source +code means all the source code for all modules it contains, plus any +associated interface definition files, plus the scripts used to +control compilation and installation of the executable. However, as a +special exception, the source code distributed need not include +anything that is normally distributed (in either source or binary +form) with the major components (compiler, kernel, and so on) of the +operating system on which the executable runs, unless that component +itself accompanies the executable. + +If distribution of executable or object code is made by offering +access to copy from a designated place, then offering equivalent +access to copy the source code from the same place counts as +distribution of the source code, even though third parties are not +compelled to copy the source along with the object code. + + 4. You may not copy, modify, sublicense, or distribute the Program +except as expressly provided under this License. Any attempt +otherwise to copy, modify, sublicense or distribute the Program is +void, and will automatically terminate your rights under this License. +However, parties who have received copies, or rights, from you under +this License will not have their licenses terminated so long as such +parties remain in full compliance. + + 5. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Program or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Program (or any work based on the +Program), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Program or works based on it. + + 6. Each time you redistribute the Program (or any work based on the +Program), the recipient automatically receives a license from the +original licensor to copy, distribute or modify the Program subject to +these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties to +this License. + + 7. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Program at all. For example, if a patent +license would not permit royalty-free redistribution of the Program by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Program. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system, which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 8. If the distribution and/or use of the Program is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Program under this License +may add an explicit geographical distribution limitation excluding +those countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 9. The Free Software Foundation may publish revised and/or new versions +of the General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + +Each version is given a distinguishing version number. If the Program +specifies a version number of this License which applies to it and "any +later version", you have the option of following the terms and conditions +either of that version or of any later version published by the Free +Software Foundation. If the Program does not specify a version number of +this License, you may choose any version ever published by the Free Software +Foundation. + + 10. If you wish to incorporate parts of the Program into other free +programs whose distribution conditions are different, write to the author +to ask for permission. For software which is copyrighted by the Free +Software Foundation, write to the Free Software Foundation; we sometimes +make exceptions for this. Our decision will be guided by the two goals +of preserving the free status of all derivatives of our free software and +of promoting the sharing and reuse of software generally. + + NO WARRANTY + + 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY +FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN +OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES +PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED +OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS +TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE +PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, +REPAIR OR CORRECTION. + + 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR +REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, +INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING +OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED +TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY +YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER +PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE +POSSIBILITY OF SUCH DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License along + with this program; if not, write to the Free Software Foundation, Inc., + 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. + +Also add information on how to contact you by electronic and paper mail. + +If the program is interactive, make it output a short notice like this +when it starts in an interactive mode: + + Gnomovision version 69, Copyright (C) year name of author + Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, the commands you use may +be called something other than `show w' and `show c'; they could even be +mouse-clicks or menu items--whatever suits your program. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the program, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the program + `Gnomovision' (which makes passes at compilers) written by James Hacker. + + , 1 April 1989 + Ty Coon, President of Vice + +This General Public License does not permit incorporating your program into +proprietary programs. If your program is a subroutine library, you may +consider it more useful to permit linking proprietary applications with the +library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. diff --git a/include/ikd-Tree/README.md b/include/ikd-Tree/README.md new file mode 100644 index 0000000..4d74115 --- /dev/null +++ b/include/ikd-Tree/README.md @@ -0,0 +1,111 @@ +# 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. + +## What does ikd-Tree support? + +- Build a balanced k-d tree - `Build()` + +- Dynamically insert points to or delete points from the k-d tree - `Add_Points() / Delete_Points()` + +- Delete points inside given axis-aligned bounding boxes - `Delete_Point_Boxes()` + +- K Nearest Neighbor Search with range limitation - `Nearest_Search()` + +- Acquire points inside a given axis-aligned bounding box on the k-d tree - `Box_Search()` + +- Acquire points inside a ball with given radius on the k-d tree - `Radius_Search()` + +## User Manual + +- Browse the [User Manual](https://github.com/hku-mars/ikd-Tree/blob/main/documents/UserManual.pdf) for using our ikd-Tree. + +## Developers + +- [Yixi CAI 蔡逸熙](https://github.com/Ecstasy-EC): Data structure design and implementation + +- [Wei XU 徐威](https://github.com/XW-HKU): Incorporation into [LiDAR-inertial odometry package FAST_LIO2 (TRO, 2022)](https://github.com/hku-mars/FAST_LIO) + + +## Related paper + +- [ikd-Tree: An Incremental K-D Tree for robotic applications](https://arxiv.org/abs/2102.10808) + +- [FAST-LIO2: Fast Direct LiDAR-Inertial Odometry](https://ieeexplore.ieee.org/abstract/document/9697912) + +If you are using any code of this repo in your research, please cite at least one of the articles as following: +- **ikd-Tree** +``` +@article{cai2021ikd, + title={ikd-Tree: An Incremental KD Tree for Robotic Applications}, + author={Cai, Yixi and Xu, Wei and Zhang, Fu}, + journal={arXiv preprint arXiv:2102.10808}, + year={2021} +} +``` +- **FAST-LIO2** +``` +@article{xu2022fast, + title={Fast-lio2: Fast direct lidar-inertial odometry}, + author={Xu, Wei and Cai, Yixi and He, Dongjiao and Lin, Jiarong and Zhang, Fu}, + journal={IEEE Transactions on Robotics}, + year={2022}, + publisher={IEEE} +} +``` + +## Build & Run demo +### 1. How to build this project +```bash +cd ~/catkin_ws/src +git clone git@github.com:hku-mars/ikd-Tree.git +cd ikd-Tree/build +cmake .. +make -j 9 +``` +### 2. Run our examples + +**Note: To run Example 2 & 3, please download the PCD file ([HKU_demo_pointcloud](https://drive.google.com/file/d/1tMYiBIFn-fcjisaoIrmIKA09NICGG9KJ/view?usp=sharing)) into`${Your own directory}/ikd-Tree/materials`** + +```bash +cd ${Your own directory}/ikd-Tree/build +# Example 1. Check the speed of ikd-Tree +./ikd_tree_demo +# Example 2. Searching-points-by-box examples +./ikd_Tree_Search_demo +# Example 3. An aysnc. exmaple for readers' better understanding of the principle of ikd-Tree +./ikd_tree_async_demo +``` + +**Example 2: ikd_tree_Search_demo** + +Box Search Result | Radius Search Result +:-------------------------:|:-------------------------: +![](materials/imgs/Box_Search.png) | ![](materials/imgs/Radius_Search.png) + +Points returned from the two search methods are shown in red. + +**Example 3: ikd_tree_Async_demo** + +Original Map: + + + + + +Box Delete Results: + +Points removed from ikd-Tree(red) | Map after box delete +:-------------------------:|:-------------------------: +![](materials/imgs/removed.png) | ![](materials/imgs/remain.png) + +This example is to demonstrate the asynchronous phenomenon in ikd-Tree. The points are deleted by attaching 'deleted' on the tree nodes (map shown in the ) instead of being removed from the ikd-Tree immediately. They are removed from the tree when rebuilding process is performed. Please refer to our paper for more details about delete and rebuilding. + + +## Acknowledgments +- Thanks [Marcus Davi](https://github.com/Marcus-Davi) for helps in templating the ikd-Tree for more general applications. + +- Thanks [Hyungtae Lim 임형태](https://github.com/LimHyungTae) for providing application examples on point clouds. + +## License + +The source code of ikd-Tree is released under [GPLv2](http://www.gnu.org/licenses/old-licenses/gpl-2.0.html) license. For commercial use, please contact Mr. Yixi CAI () or Dr. Fu ZHANG (). diff --git a/include/ikd-Tree/documents/FAST-LIO2_Fast_Direct_LiDAR-Inertial_Odometry.pdf b/include/ikd-Tree/documents/FAST-LIO2_Fast_Direct_LiDAR-Inertial_Odometry.pdf new file mode 100644 index 0000000..e5048c9 Binary files /dev/null and b/include/ikd-Tree/documents/FAST-LIO2_Fast_Direct_LiDAR-Inertial_Odometry.pdf differ diff --git a/include/ikd-Tree/documents/UserManual.pdf b/include/ikd-Tree/documents/UserManual.pdf new file mode 100644 index 0000000..2e32a85 Binary files /dev/null and b/include/ikd-Tree/documents/UserManual.pdf differ diff --git a/include/ikd-Tree/documents/ikd-Tree - An incremental k-d tree for robotic applications.pdf b/include/ikd-Tree/documents/ikd-Tree - An incremental k-d tree for robotic applications.pdf new file mode 100644 index 0000000..93e50e9 Binary files /dev/null and b/include/ikd-Tree/documents/ikd-Tree - An incremental k-d tree for robotic applications.pdf differ diff --git a/include/ikd-Tree/examples/ikd_Tree_Async_demo.cpp b/include/ikd-Tree/examples/ikd_Tree_Async_demo.cpp new file mode 100644 index 0000000..e36db3b --- /dev/null +++ b/include/ikd-Tree/examples/ikd_Tree_Async_demo.cpp @@ -0,0 +1,134 @@ +/* + Description: An example to explain the asynchronous deletion on ikd-Tree + Author: Hyungtae Lim +*/ + + +#include "ikd_Tree.h" +#include +#include +#include +#include +#include "pcl/point_types.h" +#include "pcl/common/common.h" +#include "pcl/point_cloud.h" +#include +#include + +using PointType = pcl::PointXYZ; +using PointVector = KD_TREE::PointVector; + +void colorize( const PointVector &pc, pcl::PointCloud &pc_colored, const std::vector &color) { + int N = pc.size(); + + pc_colored.clear(); + pcl::PointXYZRGB pt_tmp; + + for (int i = 0; i < N; ++i) { + const auto &pt = pc[i]; + pt_tmp.x = pt.x; + pt_tmp.y = pt.y; + pt_tmp.z = pt.z; + pt_tmp.r = color[0]; + pt_tmp.g = color[1]; + pt_tmp.b = color[2]; + pc_colored.points.emplace_back(pt_tmp); + } +} + +void generate_box(BoxPointType &boxpoint, const PointType ¢er_pt, vector box_lengths) { + float &x_dist = box_lengths[0]; + float &y_dist = box_lengths[1]; + float &z_dist = box_lengths[2]; + + boxpoint.vertex_min[0] = center_pt.x - x_dist; + boxpoint.vertex_max[0] = center_pt.x + x_dist; + boxpoint.vertex_min[1] = center_pt.y - y_dist; + boxpoint.vertex_max[1] = center_pt.y + y_dist; + boxpoint.vertex_min[2] = center_pt.z - z_dist; + boxpoint.vertex_max[2] = center_pt.z + z_dist; +} + +int main(int argc, char **argv) { + /*** 1. Initialize k-d tree */ + KD_TREE::Ptr kdtree_ptr(new KD_TREE(0.3, 0.6, 0.2)); + KD_TREE &ikd_Tree = *kdtree_ptr; + + /*** 2. Load point cloud data */ + pcl::PointCloud::Ptr src(new pcl::PointCloud); + string filename = "../materials/hku_demo_pointcloud.pcd"; + if (pcl::io::loadPCDFile(filename, *src) == -1) //* load the file + { + PCL_ERROR ("Couldn't read file test_pcd.pcd \n"); + return (-1); + } + printf("Original: %d points are loaded\n", static_cast(src->points.size())); + + /*** 3. Build ikd-Tree */ + auto start = chrono::high_resolution_clock::now(); + ikd_Tree.Build((*src).points); + auto end = chrono::high_resolution_clock::now(); + auto duration = chrono::duration_cast(end - start).count(); + printf("Building tree takes: %0.3f ms\n", float(duration) / 1e3); + printf("# of valid points: %d \n", ikd_Tree.validnum()); + + /*** 4. Set a box region and delete the corresponding region */ + PointType center_pt; + center_pt.x = 5.0; + center_pt.y = -5.0; + center_pt.z = 10.0; + BoxPointType boxpoint; + generate_box(boxpoint, center_pt, {10.0, 10.0, 20.0}); + + start = chrono::high_resolution_clock::now(); + vector boxes = {boxpoint}; + int num_deleted = ikd_Tree.Delete_Point_Boxes(boxes); + end = chrono::high_resolution_clock::now(); + duration = chrono::duration_cast(end - start).count(); + printf("Removal by box takes: %0.3f ms\n", float(duration) / 1e3); + + /*** NOTE. Check the removed points + * In ikd-tree, the delete operation and the remove operation are not performed at the same time!! + * Please refer the issue 14: + * https://github.com/hku-mars/ikd-Tree/issues/14 + * It usually occurs when the target region is large!!!! + * (i.e. the # of removed point are quite large) + * */ + PointVector Removed_Points; + ikd_Tree.acquire_removed_points(Removed_Points); + printf("# of deleted points: %d\n", num_deleted); + printf("# of removed points: %d\n", static_cast(Removed_Points.size())); + + /*** 5. Check remaining points in ikd-Tree */ + pcl::PointCloud::Ptr Remaining_Points(new pcl::PointCloud); + ikd_Tree.flatten(ikd_Tree.Root_Node, ikd_Tree.PCL_Storage, NOT_RECORD); + Remaining_Points->points = ikd_Tree.PCL_Storage; + printf("Finally, %d Points remain\n", static_cast(Remaining_Points->points.size())); + + /*** Below codes are just for visualization */ + pcl::PointCloud::Ptr removed_colored(new pcl::PointCloud); + pcl::PointCloud::Ptr remaining_colored(new pcl::PointCloud); + + pcl::visualization::PointCloudColorHandlerGenericField src_color(src, "x"); + pcl::visualization::PointCloudColorHandlerGenericField remaining_color(Remaining_Points, "x"); + + colorize(Removed_Points, *removed_colored, {255, 0, 0}); + + pcl::visualization::PCLVisualizer viewer0("Points removed from ikd-Tree"); + viewer0.addPointCloud(src,src_color, "src"); + viewer0.addPointCloud(removed_colored, "removed"); + viewer0.setCameraPosition(-5, 30, 175, 0, 0, 0, 0.2, -1.0, 0.2); + viewer0.setSize(1600, 900); + + pcl::visualization::PCLVisualizer viewer1("Map after Delete"); + viewer1.addPointCloud(Remaining_Points,remaining_color, "remain"); + viewer1.setCameraPosition(-5, 30, 175, 0, 0, 0, 0.2, -1.0, 0.2); + viewer1.setSize(1600, 900); + + while (!viewer0.wasStopped() && !viewer1.wasStopped()) {// } && !viewer2.wasStopped()) { + viewer0.spinOnce(); + viewer1.spinOnce(); + } + + return 0; +} \ No newline at end of file diff --git a/include/ikd-Tree/examples/ikd_Tree_Search_demo.cpp b/include/ikd-Tree/examples/ikd_Tree_Search_demo.cpp new file mode 100644 index 0000000..f9f41e0 --- /dev/null +++ b/include/ikd-Tree/examples/ikd_Tree_Search_demo.cpp @@ -0,0 +1,137 @@ +/* + Description: An example to introduce box search and radius search using ikd-Tree + Author: Hyungtae Lim, Yixi Cai +*/ +#include "ikd_Tree.h" +#include +#include +#include +#include +#include "pcl/point_types.h" +#include "pcl/common/common.h" +#include "pcl/point_cloud.h" +#include +#include + + +using PointType = pcl::PointXYZ; +using PointVector = KD_TREE::PointVector; +template class KD_TREE; + +void colorize( const PointVector &pc, pcl::PointCloud &pc_colored, const std::vector &color) { + int N = pc.size(); + + pc_colored.clear(); + pcl::PointXYZRGB pt_tmp; + + for (int i = 0; i < N; ++i) { + const auto &pt = pc[i]; + pt_tmp.x = pt.x; + pt_tmp.y = pt.y; + pt_tmp.z = pt.z; + pt_tmp.r = color[0]; + pt_tmp.g = color[1]; + pt_tmp.b = color[2]; + pc_colored.points.emplace_back(pt_tmp); + } +} + +void generate_box(BoxPointType &boxpoint, const PointType ¢er_pt, vector box_lengths) { + float &x_dist = box_lengths[0]; + float &y_dist = box_lengths[1]; + float &z_dist = box_lengths[2]; + + boxpoint.vertex_min[0] = center_pt.x - x_dist; + boxpoint.vertex_max[0] = center_pt.x + x_dist; + boxpoint.vertex_min[1] = center_pt.y - y_dist; + boxpoint.vertex_max[1] = center_pt.y + y_dist; + boxpoint.vertex_min[2] = center_pt.z - z_dist; + boxpoint.vertex_max[2] = center_pt.z + z_dist; +} + +float test_dist(PointType a, PointType b) +{ + float dist = 0.0f; + dist = (a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z); + return dist; +} + +int main(int argc, char **argv) { + /*** 1. Initialize k-d tree */ + KD_TREE::Ptr kdtree_ptr(new KD_TREE(0.3, 0.6, 0.2)); + KD_TREE &ikd_Tree = *kdtree_ptr; + + /*** 2. Load point cloud data */ + pcl::PointCloud::Ptr src(new pcl::PointCloud); + string filename = "../materials/hku_demo_pointcloud.pcd"; + if (pcl::io::loadPCDFile(filename, *src) == -1) //* load the file + { + PCL_ERROR ("Couldn't read file test_pcd.pcd \n"); + return (-1); + } + printf("Original: %d points are loaded\n", static_cast(src->points.size())); + + /*** 3. Build ikd-Tree */ + auto start = chrono::high_resolution_clock::now(); + ikd_Tree.Build((*src).points); + auto end = chrono::high_resolution_clock::now(); + auto duration = chrono::duration_cast(end - start).count(); + printf("Building tree takes: %0.3f ms\n", float(duration) / 1e3); + printf("# of valid points: %d \n", ikd_Tree.validnum()); + + /*** 4. Set a box region and search using box search */ + PointType center_pt; + center_pt.x = 10.0; + center_pt.y = 0.0; + center_pt.z = 0.0; + BoxPointType boxpoint; + generate_box(boxpoint, center_pt, {5.00, 5.00, 50.0}); + + start = chrono::high_resolution_clock::now(); + PointVector Searched_Points; + ikd_Tree.Box_Search(boxpoint, Searched_Points); + end = chrono::high_resolution_clock::now(); + duration = chrono::duration_cast(end - start).count(); + printf("Search Points by box takes: %0.3f ms with %d points\n", float(duration) / 1e3, static_cast(Searched_Points.size())); + + /*** 5. Set a ball region and search using radius search */ + PointType ball_center_pt; + ball_center_pt.x = 10.0; + ball_center_pt.y = -5.0; + ball_center_pt.z = 5.0; + float radius = 7.5; + start = chrono::high_resolution_clock::now(); + PointVector Searched_Points_radius; + ikd_Tree.Radius_Search(ball_center_pt, radius, Searched_Points_radius); + end = chrono::high_resolution_clock::now(); + duration = chrono::duration_cast(end - start).count(); + printf("Search Points by radius takes: %0.3f ms with %d points\n", float(duration) / 1e3, int(Searched_Points_radius.size())); + + /*** Below codes are just for visualization */ + pcl::PointCloud::Ptr src_colored(new pcl::PointCloud); + pcl::PointCloud::Ptr searched_colored(new pcl::PointCloud); + pcl::PointCloud::Ptr searched_radius_colored(new pcl::PointCloud); + + pcl::visualization::PointCloudColorHandlerGenericField src_color(src, "x"); + colorize(Searched_Points, *searched_colored, {255, 0, 0}); + colorize(Searched_Points_radius, *searched_radius_colored, {255, 0, 0}); + + pcl::visualization::PCLVisualizer viewer0("Box Search"); + viewer0.addPointCloud(src,src_color, "src"); + viewer0.addPointCloud(searched_colored, "searched"); + viewer0.setCameraPosition(-5, 30, 175, 0, 0, 0, 0.2, -1.0, 0.2); + viewer0.setSize(1600, 900); + + pcl::visualization::PCLVisualizer viewer1("Radius Search"); + viewer1.addPointCloud(src,src_color, "src"); + viewer1.addPointCloud(searched_radius_colored, "radius"); + viewer1.setCameraPosition(-5, 30, 175, 0, 0, 0, 0.2, -1.0, 0.2); + viewer1.setSize(1600, 900); + + while (!viewer0.wasStopped() && !viewer1.wasStopped()){ + viewer0.spinOnce(); + viewer1.spinOnce(); + } + + return 0; +} \ No newline at end of file diff --git a/include/ikd-Tree/examples/ikd_Tree_demo.cpp b/include/ikd-Tree/examples/ikd_Tree_demo.cpp new file mode 100644 index 0000000..de2aaa0 --- /dev/null +++ b/include/ikd-Tree/examples/ikd_Tree_demo.cpp @@ -0,0 +1,286 @@ +/* + Description: An example for using ikd-Tree + Author: Yixi Cai + Email: yixicai@connect.hku.hk +*/ + +#include +#include +#include +#include +#include + +using PointType = ikdTree_PointType; +using PointVector = KD_TREE::PointVector; + +#define X_MAX 5.0 +#define X_MIN -5.0 +#define Y_MAX 5.0 +#define Y_MIN -5.0 +#define Z_MAX 5.0 +#define Z_MIN -5.0 + +#define Point_Num 20000 +#define New_Point_Num 200 +#define Delete_Point_Num 100 +#define Nearest_Num 5 +#define Test_Time 1000 +#define Search_Counter 200 +#define Box_Length 1.5 +#define Box_Num 4 +#define Delete_Box_Switch true +#define Add_Box_Switch true + +PointVector point_cloud; +PointVector cloud_increment; +PointVector cloud_decrement; +PointVector cloud_deleted; +PointVector search_result; +PointVector raw_cmp_result; +PointVector DeletePoints; +PointVector removed_points; + +KD_TREE ikd_Tree(0.3,0.6,0.2); + +float rand_float(float x_min, float x_max){ + float rand_ratio = rand()/(float)RAND_MAX; + return (x_min + rand_ratio * (x_max - x_min)); +} + + +/* + Generate the points to initialize an incremental k-d tree +*/ + +void generate_initial_point_cloud(int num){ + PointVector ().swap(point_cloud); + PointType new_point; + for (int i=0;i & Add_Boxes, float box_length, int box_num){ + vector ().swap(Add_Boxes); + float d = box_length/2; + float x_p, y_p, z_p; + BoxPointType boxpoint; + for (int k=0;k < box_num; k++){ + x_p = rand_float(X_MIN, X_MAX); + y_p = rand_float(Y_MIN, Y_MAX); + z_p = rand_float(Z_MIN, Z_MAX); + boxpoint.vertex_min[0] = x_p - d; + boxpoint.vertex_max[0] = x_p + d; + boxpoint.vertex_min[1] = y_p - d; + boxpoint.vertex_max[1] = y_p + d; + boxpoint.vertex_min[2] = z_p - d; + boxpoint.vertex_max[2] = z_p + d; + Add_Boxes.push_back(boxpoint); + int n = cloud_deleted.size(); + int counter = 0; + while (counter < n){ + PointType tmp = cloud_deleted[cloud_deleted.size()-1]; + cloud_deleted.pop_back(); + + if (tmp.x +EPSS < boxpoint.vertex_min[0] || tmp.x - EPSS > boxpoint.vertex_max[0] || tmp.y + EPSS < boxpoint.vertex_min[1] || tmp.y - EPSS > boxpoint.vertex_max[1] || tmp.z + EPSS < boxpoint.vertex_min[2] || tmp.z - EPSS > boxpoint.vertex_max[2]){ + cloud_deleted.insert(cloud_deleted.begin(),tmp); + } else { + point_cloud.push_back(tmp); + } + counter += 1; + } + } +} + +/* + Generate random boxes for box-wise delete on the incremental k-d tree +*/ + +void generate_box_decrement(vector & Delete_Boxes, float box_length, int box_num){ + vector ().swap(Delete_Boxes); + float d = box_length/2; + float x_p, y_p, z_p; + BoxPointType boxpoint; + for (int k=0;k < box_num; k++){ + x_p = rand_float(X_MIN, X_MAX); + y_p = rand_float(Y_MIN, Y_MAX); + z_p = rand_float(Z_MIN, Z_MAX); + boxpoint.vertex_min[0] = x_p - d; + boxpoint.vertex_max[0] = x_p + d; + boxpoint.vertex_min[1] = y_p - d; + boxpoint.vertex_max[1] = y_p + d; + boxpoint.vertex_min[2] = z_p - d; + boxpoint.vertex_max[2] = z_p + d; + Delete_Boxes.push_back(boxpoint); + int n = point_cloud.size(); + int counter = 0; + while (counter < n){ + PointType tmp = point_cloud[point_cloud.size()-1]; + point_cloud.pop_back(); + if (tmp.x +EPSS < boxpoint.vertex_min[0] || tmp.x - EPSS > boxpoint.vertex_max[0] || tmp.y + EPSS < boxpoint.vertex_min[1] || tmp.y - EPSS > boxpoint.vertex_max[1] || tmp.z + EPSS < boxpoint.vertex_min[2] || tmp.z - EPSS > boxpoint.vertex_max[2]){ + point_cloud.insert(point_cloud.begin(),tmp); + } else { + cloud_deleted.push_back(tmp); + } + counter += 1; + } + } +} + + +/* + Generate target point for nearest search on the incremental k-d tree +*/ + +PointType generate_target_point(){ + PointType point; + point.x = rand_float(X_MIN, X_MAX);; + point.y = rand_float(Y_MIN, Y_MAX); + point.z = rand_float(Z_MIN, Z_MAX); + return point; +} + +int main(int argc, char** argv){ + srand((unsigned) time(NULL)); + printf("Testing ...\n"); + int counter = 0; + bool flag = true; + vector Delete_Boxes; + vector Add_Boxes; + vector PointDist; + float average_total_time = 0.0; + float box_delete_time = 0.0; + float box_add_time = 0.0; + float add_time = 0.0; + float delete_time = 0.0; + float search_time = 0.0; + int box_delete_counter = 0; + int box_add_counter = 0; + PointType target; + // Initialize k-d tree + generate_initial_point_cloud(Point_Num); + auto t1 = chrono::high_resolution_clock::now(); + ikd_Tree.Build(point_cloud); + auto t2 = chrono::high_resolution_clock::now(); + auto build_duration = chrono::duration_cast(t2-t1).count(); + while (counter < Test_Time){ + printf("Test %d:\n",counter+1); + // Point-wise Insertion + generate_increment_point_cloud(New_Point_Num); + t1 = chrono::high_resolution_clock::now(); + ikd_Tree.Add_Points(cloud_increment, false); + t2 = chrono::high_resolution_clock::now(); + auto add_duration = chrono::duration_cast(t2-t1).count(); + auto total_duration = add_duration; + printf("Add point time cost is %0.3f ms\n",float(add_duration)/1e3); + // Point-wise Delete + generate_decrement_point_cloud(Delete_Point_Num); + t1 = chrono::high_resolution_clock::now(); + ikd_Tree.Delete_Points(cloud_decrement); + t2 = chrono::high_resolution_clock::now(); + auto delete_duration = chrono::duration_cast(t2-t1).count(); + total_duration += delete_duration; + printf("Delete point time cost is %0.3f ms\n",float(delete_duration)/1e3); + // Box-wise Delete + auto box_delete_duration = chrono::duration_cast(t2-t2).count(); + if (Delete_Box_Switch && (counter+1) % 500 == 0){ + printf("Waiting to generate 4 cuboids for box-wise delete test...\n"); + generate_box_decrement(Delete_Boxes, Box_Length, Box_Num); + t1 = chrono::high_resolution_clock::now(); + ikd_Tree.Delete_Point_Boxes(Delete_Boxes); + t2 = chrono::high_resolution_clock::now(); + box_delete_counter ++; + box_delete_duration += chrono::duration_cast(t2-t1).count(); + printf("Delete box points time cost is %0.3f ms\n",float(box_delete_duration)/1e3); + } + total_duration += box_delete_duration; + // Box-wise Re-insertion + auto box_add_duration = chrono::duration_cast(t2-t2).count(); + if (Add_Box_Switch && (counter+1) % 100 == 0){ + generate_box_increment(Add_Boxes, Box_Length, Box_Num); + t1 = chrono::high_resolution_clock::now(); + ikd_Tree.Add_Point_Boxes(Add_Boxes); + t2 = chrono::high_resolution_clock::now(); + box_add_counter ++; + box_add_duration += chrono::duration_cast(t2-t1).count(); + printf("Add box points time cost is %0.3f ms\n",float(box_add_duration)/1e3); + } + total_duration += box_add_duration; + // Nearest Search + auto search_duration = chrono::duration_cast(t2-t2).count(); + for (int k=0;k(t2-t1).count(); + } + printf("Search nearest point time cost is %0.3f ms\n",float(search_duration)/1e3); + total_duration += search_duration; + printf("Total time is %0.3f ms\n",total_duration/1e3); + printf("Tree size is: %d\n\n", ikd_Tree.size()); + // If necessary, the removed points can be collected. + PointVector ().swap(removed_points); + ikd_Tree.acquire_removed_points(removed_points); + // Calculate total running time + average_total_time += float(total_duration)/1e3; + box_delete_time += float(box_delete_duration)/1e3; + box_add_time += float(box_add_duration)/1e3; + add_time += float(add_duration)/1e3; + delete_time += float(delete_duration)/1e3; + search_time += float(search_duration)/1e3; + counter += 1; + } + + printf("Finished %d times test\n",counter); + printf("Average Time:\n"); + printf("Total Time is: %0.3fms\n",average_total_time/1e3); + printf("Point-wise Insertion (%d points): %0.3fms\n",New_Point_Num,add_time/counter); + printf("Point-wise Delete (%d points): %0.3fms\n", Delete_Point_Num,delete_time/counter); + printf("Box-wse Delete (%d boxes): %0.3fms\n",Box_Num,box_delete_time/box_delete_counter); + printf("Box-wse Re-insertion (%d boxes): %0.3fms\n",Box_Num,box_add_time/box_add_counter); + printf("Nearest Search (%d points): %0.3fms\n", Search_Counter,search_time/counter); + return 0; +} \ No newline at end of file diff --git a/include/ikd-Tree/ikd_Tree.cpp b/include/ikd-Tree/ikd_Tree.cpp new file mode 100644 index 0000000..9a7c032 --- /dev/null +++ b/include/ikd-Tree/ikd_Tree.cpp @@ -0,0 +1,1449 @@ +#include "ikd_Tree.h" + +/* +Description: ikd-Tree: an incremental k-d tree for robotic applications +Author: Yixi Cai +email: yixicai@connect.hku.hk +*/ + +template +KD_TREE::KD_TREE(float delete_param, float balance_param, float box_length) { + delete_criterion_param = delete_param; + balance_criterion_param = balance_param; + downsample_size = box_length; + Rebuild_Logger.clear(); + termination_flag = false; + start_thread(); +} + +template +KD_TREE::~KD_TREE() +{ + stop_thread(); + Delete_Storage_Disabled = true; + delete_tree_nodes(&Root_Node); + PointVector ().swap(PCL_Storage); + Rebuild_Logger.clear(); +} + +template +void KD_TREE::Set_delete_criterion_param(float delete_param){ + delete_criterion_param = delete_param; +} + +template +void KD_TREE::Set_balance_criterion_param(float balance_param){ + balance_criterion_param = balance_param; +} + +template +void KD_TREE::set_downsample_param(float downsample_param){ + downsample_size = downsample_param; +} + +template +void KD_TREE::InitializeKDTree(float delete_param, float balance_param, float box_length){ + Set_delete_criterion_param(delete_param); + Set_balance_criterion_param(balance_param); + set_downsample_param(box_length); +} + +template +void KD_TREE::InitTreeNode(KD_TREE_NODE * root){ + root->point.x = 0.0f; + root->point.y = 0.0f; + root->point.z = 0.0f; + root->node_range_x[0] = 0.0f; + root->node_range_x[1] = 0.0f; + root->node_range_y[0] = 0.0f; + root->node_range_y[1] = 0.0f; + root->node_range_z[0] = 0.0f; + root->node_range_z[1] = 0.0f; + root->division_axis = 0; + root->father_ptr = nullptr; + root->left_son_ptr = nullptr; + root->right_son_ptr = nullptr; + root->TreeSize = 0; + root->invalid_point_num = 0; + root->down_del_num = 0; + root->point_deleted = false; + root->tree_deleted = false; + root->need_push_down_to_left = false; + root->need_push_down_to_right = false; + root->point_downsample_deleted = false; + root->working_flag = false; + pthread_mutex_init(&(root->push_down_mutex_lock),NULL); +} + +template +int KD_TREE::size(){ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + if (Root_Node != nullptr) { + return Root_Node->TreeSize; + } else { + return 0; + } + } else { + if (!pthread_mutex_trylock(&working_flag_mutex)){ + s = Root_Node->TreeSize; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } else { + return Treesize_tmp; + } + } +} + +template +BoxPointType KD_TREE::tree_range(){ + BoxPointType range; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + if (Root_Node != nullptr) { + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + } else { + memset(&range, 0, sizeof(range)); + } + } else { + if (!pthread_mutex_trylock(&working_flag_mutex)){ + range.vertex_min[0] = Root_Node->node_range_x[0]; + range.vertex_min[1] = Root_Node->node_range_y[0]; + range.vertex_min[2] = Root_Node->node_range_z[0]; + range.vertex_max[0] = Root_Node->node_range_x[1]; + range.vertex_max[1] = Root_Node->node_range_y[1]; + range.vertex_max[2] = Root_Node->node_range_z[1]; + pthread_mutex_unlock(&working_flag_mutex); + } else { + memset(&range, 0, sizeof(range)); + } + } + return range; +} + +template +int KD_TREE::validnum(){ + int s = 0; + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + if (Root_Node != nullptr) + return (Root_Node->TreeSize - Root_Node->invalid_point_num); + else + return 0; + } else { + if (!pthread_mutex_trylock(&working_flag_mutex)){ + s = Root_Node->TreeSize-Root_Node->invalid_point_num; + pthread_mutex_unlock(&working_flag_mutex); + return s; + } else { + return -1; + } + } +} + +template +void KD_TREE::root_alpha(float &alpha_bal, float &alpha_del){ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + return; + } else { + if (!pthread_mutex_trylock(&working_flag_mutex)){ + alpha_bal = Root_Node->alpha_bal; + alpha_del = Root_Node->alpha_del; + pthread_mutex_unlock(&working_flag_mutex); + return; + } else { + alpha_bal = alpha_bal_tmp; + alpha_del = alpha_del_tmp; + return; + } + } +} + +template +void KD_TREE::start_thread(){ + pthread_mutex_init(&termination_flag_mutex_lock, NULL); + pthread_mutex_init(&rebuild_ptr_mutex_lock, NULL); + pthread_mutex_init(&rebuild_logger_mutex_lock, NULL); + pthread_mutex_init(&points_deleted_rebuild_mutex_lock, NULL); + pthread_mutex_init(&working_flag_mutex, NULL); + pthread_mutex_init(&search_flag_mutex, NULL); + pthread_create(&rebuild_thread, NULL, multi_thread_ptr, (void*) this); + printf("Multi thread started \n"); +} + +template +void KD_TREE::stop_thread(){ + pthread_mutex_lock(&termination_flag_mutex_lock); + termination_flag = true; + pthread_mutex_unlock(&termination_flag_mutex_lock); + if (rebuild_thread) pthread_join(rebuild_thread, NULL); + pthread_mutex_destroy(&termination_flag_mutex_lock); + pthread_mutex_destroy(&rebuild_logger_mutex_lock); + pthread_mutex_destroy(&rebuild_ptr_mutex_lock); + pthread_mutex_destroy(&points_deleted_rebuild_mutex_lock); + pthread_mutex_destroy(&working_flag_mutex); + pthread_mutex_destroy(&search_flag_mutex); +} + +template +void * KD_TREE::multi_thread_ptr(void * arg){ + KD_TREE * handle = (KD_TREE*) arg; + handle->multi_thread_rebuild(); + return nullptr; +} + +template +void KD_TREE::multi_thread_rebuild(){ + bool terminated = false; + KD_TREE_NODE * father_ptr, ** new_node_ptr; + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + while (!terminated){ + pthread_mutex_lock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&working_flag_mutex); + if (Rebuild_Ptr != nullptr ){ + /* Traverse and copy */ + if (!Rebuild_Logger.empty()){ + printf("\n\n\n\n\n\n\n\n\n\n\n ERROR!!! \n\n\n\n\n\n\n\n\n"); + } + rebuild_flag = true; + if (*Rebuild_Ptr == Root_Node) { + Treesize_tmp = Root_Node->TreeSize; + Validnum_tmp = Root_Node->TreeSize - Root_Node->invalid_point_num; + alpha_bal_tmp = Root_Node->alpha_bal; + alpha_del_tmp = Root_Node->alpha_del; + } + KD_TREE_NODE * old_root_node = (*Rebuild_Ptr); + father_ptr = (*Rebuild_Ptr)->father_ptr; + PointVector ().swap(Rebuild_PCL_Storage); + // Lock Search + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0){ + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + // Lock deleted points cache + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + flatten(*Rebuild_Ptr, Rebuild_PCL_Storage, MULTI_THREAD_REC); + // Unlock deleted points cache + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + // Unlock Search + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + pthread_mutex_unlock(&working_flag_mutex); + /* Rebuild and update missed operations*/ + Operation_Logger_Type Operation; + KD_TREE_NODE * new_root_node = nullptr; + if (int(Rebuild_PCL_Storage.size()) > 0){ + BuildTree(&new_root_node, 0, Rebuild_PCL_Storage.size()-1, Rebuild_PCL_Storage); + // Rebuild has been done. Updates the blocked operations into the new tree + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + int tmp_counter = 0; + while (!Rebuild_Logger.empty()){ + Operation = Rebuild_Logger.front(); + max_queue_size = max(max_queue_size, Rebuild_Logger.size()); + Rebuild_Logger.pop(); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + pthread_mutex_unlock(&working_flag_mutex); + run_operation(&new_root_node, Operation); + tmp_counter ++; + if (tmp_counter % 10 == 0) usleep(1); + pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + /* Replace to original tree*/ + // pthread_mutex_lock(&working_flag_mutex); + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter != 0){ + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter = -1; + pthread_mutex_unlock(&search_flag_mutex); + if (father_ptr->left_son_ptr == *Rebuild_Ptr) { + father_ptr->left_son_ptr = new_root_node; + } else if (father_ptr->right_son_ptr == *Rebuild_Ptr){ + father_ptr->right_son_ptr = new_root_node; + } else { + throw "Error: Father ptr incompatible with current node\n"; + } + if (new_root_node != nullptr) new_root_node->father_ptr = father_ptr; + (*Rebuild_Ptr) = new_root_node; + int valid_old = old_root_node->TreeSize-old_root_node->invalid_point_num; + int valid_new = new_root_node->TreeSize-new_root_node->invalid_point_num; + if (father_ptr == STATIC_ROOT_NODE) Root_Node = STATIC_ROOT_NODE->left_son_ptr; + KD_TREE_NODE * update_root = *Rebuild_Ptr; + while (update_root != nullptr && update_root != Root_Node){ + update_root = update_root->father_ptr; + if (update_root->working_flag) break; + if (update_root == update_root->father_ptr->left_son_ptr && update_root->father_ptr->need_push_down_to_left) break; + if (update_root == update_root->father_ptr->right_son_ptr && update_root->father_ptr->need_push_down_to_right) break; + Update(update_root); + } + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter = 0; + pthread_mutex_unlock(&search_flag_mutex); + Rebuild_Ptr = nullptr; + pthread_mutex_unlock(&working_flag_mutex); + rebuild_flag = false; + /* Delete discarded tree nodes */ + delete_tree_nodes(&old_root_node); + } else { + pthread_mutex_unlock(&working_flag_mutex); + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + pthread_mutex_lock(&termination_flag_mutex_lock); + terminated = termination_flag; + pthread_mutex_unlock(&termination_flag_mutex_lock); + usleep(100); + } + printf("Rebuild thread terminated normally\n"); +} + +template +void KD_TREE::run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation){ + switch (operation.op) + { + case ADD_POINT: + Add_by_point(root, operation.point, false, (*root)->division_axis); + break; + case ADD_BOX: + Add_by_range(root, operation.boxpoint, false); + break; + case DELETE_POINT: + Delete_by_point(root, operation.point, false); + break; + case DELETE_BOX: + Delete_by_range(root, operation.boxpoint, false, false); + break; + case DOWNSAMPLE_DELETE: + Delete_by_range(root, operation.boxpoint, false, true); + break; + case PUSH_DOWN: + (*root)->tree_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->point_downsample_deleted |= operation.tree_downsample_deleted; + (*root)->tree_deleted = operation.tree_deleted || (*root)->tree_downsample_deleted; + (*root)->point_deleted = (*root)->tree_deleted || (*root)->point_downsample_deleted; + if (operation.tree_downsample_deleted) (*root)->down_del_num = (*root)->TreeSize; + if (operation.tree_deleted) (*root)->invalid_point_num = (*root)->TreeSize; + else (*root)->invalid_point_num = (*root)->down_del_num; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + break; + default: + break; + } +} + +template +void KD_TREE::Build(PointVector point_cloud){ + if (Root_Node != nullptr){ + delete_tree_nodes(&Root_Node); + } + if (point_cloud.size() == 0) return; + STATIC_ROOT_NODE = new KD_TREE_NODE; + InitTreeNode(STATIC_ROOT_NODE); + BuildTree(&STATIC_ROOT_NODE->left_son_ptr, 0, point_cloud.size()-1, point_cloud); + Update(STATIC_ROOT_NODE); + STATIC_ROOT_NODE->TreeSize = 0; + Root_Node = STATIC_ROOT_NODE->left_son_ptr; +} + +template +void KD_TREE::Nearest_Search(PointType point, int k_nearest, PointVector& Nearest_Points, vector & Point_Distance, double max_dist){ + MANUAL_HEAP q(2*k_nearest); + q.clear(); + vector ().swap(Point_Distance); + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + Search(Root_Node, k_nearest, point, q, max_dist); + } else { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(Root_Node, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + int k_found = min(k_nearest,int(q.size())); + PointVector ().swap(Nearest_Points); + vector ().swap(Point_Distance); + for (int i=0;i < k_found;i++){ + Nearest_Points.insert(Nearest_Points.begin(), q.top().point); + Point_Distance.insert(Point_Distance.begin(), q.top().dist); + q.pop(); + } + return; +} + +template +void KD_TREE::Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage) +{ + Storage.clear(); + Search_by_range(Root_Node, Box_of_Point, Storage); +} + +template +void KD_TREE::Radius_Search(PointType point, const float radius, PointVector &Storage) +{ + Storage.clear(); + Search_by_radius(Root_Node, point, radius, Storage); +} + +template +int KD_TREE::Add_Points(PointVector & PointToAdd, bool downsample_on){ + int NewPointSize = PointToAdd.size(); + int tree_size = size(); + BoxPointType Box_of_Point; + PointType downsample_result, mid_point; + bool downsample_switch = downsample_on && DOWNSAMPLE_SWITCH; + float min_dist, tmp_dist; + int tmp_counter = 0; + for (int i=0; i 1 || same_point(PointToAdd[i], downsample_result)){ + if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, true, true); + Add_by_point(&Root_Node, downsample_result, true, Root_Node->division_axis); + tmp_counter ++; + } + } else { + if (Downsample_Storage.size() > 1 || same_point(PointToAdd[i], downsample_result)){ + Operation_Logger_Type operation_delete, operation; + operation_delete.boxpoint = Box_of_Point; + operation_delete.op = DOWNSAMPLE_DELETE; + operation.point = downsample_result; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + if (Downsample_Storage.size() > 0) Delete_by_range(&Root_Node, Box_of_Point, false , true); + Add_by_point(&Root_Node, downsample_result, false, Root_Node->division_axis); + tmp_counter ++; + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + if (Downsample_Storage.size() > 0) Rebuild_Logger.push(operation_delete); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + }; + } + } else { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + Add_by_point(&Root_Node, PointToAdd[i], true, Root_Node->division_axis); + } else { + Operation_Logger_Type operation; + operation.point = PointToAdd[i]; + operation.op = ADD_POINT; + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&Root_Node, PointToAdd[i], false, Root_Node->division_axis); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + } + return tmp_counter; +} + +template +void KD_TREE::Add_Point_Boxes(vector & BoxPoints){ + for (int i=0;i < BoxPoints.size();i++){ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + Add_by_range(&Root_Node ,BoxPoints[i], true); + } else { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = ADD_BOX; + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&Root_Node ,BoxPoints[i], false); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Delete_Points(PointVector & PointToDel){ + for (int i=0;i +int KD_TREE::Delete_Point_Boxes(vector & BoxPoints){ + int tmp_counter = 0; + for (int i=0;i < BoxPoints.size();i++){ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != Root_Node){ + tmp_counter += Delete_by_range(&Root_Node ,BoxPoints[i], true, false); + } else { + Operation_Logger_Type operation; + operation.boxpoint = BoxPoints[i]; + operation.op = DELETE_BOX; + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&Root_Node ,BoxPoints[i], false, false); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + return tmp_counter; +} + +template +void KD_TREE::acquire_removed_points(PointVector & removed_points){ + pthread_mutex_lock(&points_deleted_rebuild_mutex_lock); + for (int i = 0; i < Points_deleted.size();i++){ + removed_points.push_back(Points_deleted[i]); + } + for (int i = 0; i < Multithread_Points_deleted.size();i++){ + removed_points.push_back(Multithread_Points_deleted[i]); + } + Points_deleted.clear(); + Multithread_Points_deleted.clear(); + pthread_mutex_unlock(&points_deleted_rebuild_mutex_lock); + return; +} + +template +void KD_TREE::BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage){ + if (l>r) return; + *root = new KD_TREE_NODE; + InitTreeNode(*root); + int mid = (l+r)>>1; + int div_axis = 0; + int i; + // Find the best division Axis + float min_value[3] = {INFINITY, INFINITY, INFINITY}; + float max_value[3] = {-INFINITY, -INFINITY, -INFINITY}; + float dim_range[3] = {0,0,0}; + for (i=l;i<=r;i++){ + min_value[0] = min(min_value[0], Storage[i].x); + min_value[1] = min(min_value[1], Storage[i].y); + min_value[2] = min(min_value[2], Storage[i].z); + max_value[0] = max(max_value[0], Storage[i].x); + max_value[1] = max(max_value[1], Storage[i].y); + max_value[2] = max(max_value[2], Storage[i].z); + } + // Select the longest dimension as division axis + for (i=0;i<3;i++) dim_range[i] = max_value[i] - min_value[i]; + for (i=1;i<3;i++) if (dim_range[i] > dim_range[div_axis]) div_axis = i; + // Divide by the division axis and recursively build. + + (*root)->division_axis = div_axis; + switch (div_axis) + { + case 0: + nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x); + break; + case 1: + nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_y); + break; + case 2: + nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_z); + break; + default: + nth_element(begin(Storage)+l, begin(Storage)+mid, begin(Storage)+r+1, point_cmp_x); + break; + } + (*root)->point = Storage[mid]; + KD_TREE_NODE * left_son = nullptr, * right_son = nullptr; + BuildTree(&left_son, l, mid-1, Storage); + BuildTree(&right_son, mid+1, r, Storage); + (*root)->left_son_ptr = left_son; + (*root)->right_son_ptr = right_son; + Update((*root)); + return; +} + +template +void KD_TREE::Rebuild(KD_TREE_NODE ** root){ + KD_TREE_NODE * father_ptr; + if ((*root)->TreeSize >= Multi_Thread_Rebuild_Point_Num) { + if (!pthread_mutex_trylock(&rebuild_ptr_mutex_lock)){ + if (Rebuild_Ptr == nullptr || ((*root)->TreeSize > (*Rebuild_Ptr)->TreeSize)) { + Rebuild_Ptr = root; + } + pthread_mutex_unlock(&rebuild_ptr_mutex_lock); + } + } else { + father_ptr = (*root)->father_ptr; + int size_rec = (*root)->TreeSize; + PCL_Storage.clear(); + flatten(*root, PCL_Storage, DELETE_POINTS_REC); + delete_tree_nodes(root); + BuildTree(root, 0, PCL_Storage.size()-1, PCL_Storage); + if (*root != nullptr) (*root)->father_ptr = father_ptr; + if (*root == Root_Node) STATIC_ROOT_NODE->left_son_ptr = *root; + } + return; +} + +template +int KD_TREE::Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample){ + if ((*root) == nullptr || (*root)->tree_deleted) return 0; + (*root)->working_flag = true; + Push_Down(*root); + int tmp_counter = 0; + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return 0; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return 0; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return 0; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1] > (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]){ + (*root)->tree_deleted = true; + (*root)->point_deleted = true; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + tmp_counter = (*root)->TreeSize - (*root)->invalid_point_num; + (*root)->invalid_point_num = (*root)->TreeSize; + if (is_downsample){ + (*root)->tree_downsample_deleted = true; + (*root)->point_downsample_deleted = true; + (*root)->down_del_num = (*root)->TreeSize; + } + return tmp_counter; + } + if (!(*root)->point_deleted && boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z){ + (*root)->point_deleted = true; + tmp_counter += 1; + if (is_downsample) (*root)->point_downsample_deleted = true; + } + Operation_Logger_Type delete_box_log; + struct timespec Timeout; + if (is_downsample) delete_box_log.op = DOWNSAMPLE_DELETE; + else delete_box_log.op = DELETE_BOX; + delete_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild, is_downsample); + } else { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->left_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild, is_downsample); + } else { + pthread_mutex_lock(&working_flag_mutex); + tmp_counter += Delete_by_range(&((*root)->right_son_ptr), boxpoint, false, is_downsample); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) Rebuild(root); + if ((*root) != nullptr) (*root)->working_flag = false; + return tmp_counter; +} + +template +void KD_TREE::Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild){ + if ((*root) == nullptr || (*root)->tree_deleted) return; + (*root)->working_flag = true; + Push_Down(*root); + if (same_point((*root)->point, point) && !(*root)->point_deleted) { + (*root)->point_deleted = true; + (*root)->invalid_point_num += 1; + if ((*root)->invalid_point_num == (*root)->TreeSize) (*root)->tree_deleted = true; + return; + } + Operation_Logger_Type delete_log; + struct timespec Timeout; + delete_log.op = DELETE_POINT; + delete_log.point = point; + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)){ + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ + Delete_by_point(&(*root)->left_son_ptr, point, allow_rebuild); + } else { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->left_son_ptr, point,false); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } else { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ + Delete_by_point(&(*root)->right_son_ptr, point, allow_rebuild); + } else { + pthread_mutex_lock(&working_flag_mutex); + Delete_by_point(&(*root)->right_son_ptr, point, false); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(delete_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) Rebuild(root); + if ((*root) != nullptr) (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild){ + if ((*root) == nullptr) return; + (*root)->working_flag = true; + Push_Down(*root); + if (boxpoint.vertex_max[0] <= (*root)->node_range_x[0] || boxpoint.vertex_min[0] > (*root)->node_range_x[1]) return; + if (boxpoint.vertex_max[1] <= (*root)->node_range_y[0] || boxpoint.vertex_min[1] > (*root)->node_range_y[1]) return; + if (boxpoint.vertex_max[2] <= (*root)->node_range_z[0] || boxpoint.vertex_min[2] > (*root)->node_range_z[1]) return; + if (boxpoint.vertex_min[0] <= (*root)->node_range_x[0] && boxpoint.vertex_max[0] > (*root)->node_range_x[1] && boxpoint.vertex_min[1] <= (*root)->node_range_y[0] && boxpoint.vertex_max[1]> (*root)->node_range_y[1] && boxpoint.vertex_min[2] <= (*root)->node_range_z[0] && boxpoint.vertex_max[2] > (*root)->node_range_z[1]){ + (*root)->tree_deleted = false || (*root)->tree_downsample_deleted; + (*root)->point_deleted = false || (*root)->point_downsample_deleted; + (*root)->need_push_down_to_left = true; + (*root)->need_push_down_to_right = true; + (*root)->invalid_point_num = (*root)->down_del_num; + return; + } + if (boxpoint.vertex_min[0] <= (*root)->point.x && boxpoint.vertex_max[0] > (*root)->point.x && boxpoint.vertex_min[1] <= (*root)->point.y && boxpoint.vertex_max[1] > (*root)->point.y && boxpoint.vertex_min[2] <= (*root)->point.z && boxpoint.vertex_max[2] > (*root)->point.z){ + (*root)->point_deleted = (*root)->point_downsample_deleted; + } + Operation_Logger_Type add_box_log; + struct timespec Timeout; + add_box_log.op = ADD_BOX; + add_box_log.boxpoint = boxpoint; + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ + Add_by_range(&((*root)->left_son_ptr), boxpoint, allow_rebuild); + } else { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->left_son_ptr), boxpoint, false); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ + Add_by_range(&((*root)->right_son_ptr), boxpoint, allow_rebuild); + } else { + pthread_mutex_lock(&working_flag_mutex); + Add_by_range(&((*root)->right_son_ptr), boxpoint, false); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_box_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) Rebuild(root); + if ((*root) != nullptr) (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis){ + if (*root == nullptr){ + *root = new KD_TREE_NODE; + InitTreeNode(*root); + (*root)->point = point; + (*root)->division_axis = (father_axis + 1) % 3; + Update(*root); + return; + } + (*root)->working_flag = true; + Operation_Logger_Type add_log; + struct timespec Timeout; + add_log.op = ADD_POINT; + add_log.point = point; + Push_Down(*root); + if (((*root)->division_axis == 0 && point.x < (*root)->point.x) || ((*root)->division_axis == 1 && point.y < (*root)->point.y) || ((*root)->division_axis == 2 && point.z < (*root)->point.z)){ + if ((Rebuild_Ptr == nullptr) || (*root)->left_son_ptr != *Rebuild_Ptr){ + Add_by_point(&(*root)->left_son_ptr, point, allow_rebuild, (*root)->division_axis); + } else { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->left_son_ptr, point, false,(*root)->division_axis); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } else { + if ((Rebuild_Ptr == nullptr) || (*root)->right_son_ptr != *Rebuild_Ptr){ + Add_by_point(&(*root)->right_son_ptr, point, allow_rebuild,(*root)->division_axis); + } else { + pthread_mutex_lock(&working_flag_mutex); + Add_by_point(&(*root)->right_son_ptr, point, false,(*root)->division_axis); + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(add_log); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + pthread_mutex_unlock(&working_flag_mutex); + } + } + Update(*root); + if (Rebuild_Ptr != nullptr && *Rebuild_Ptr == *root && (*root)->TreeSize < Multi_Thread_Rebuild_Point_Num) Rebuild_Ptr = nullptr; + bool need_rebuild = allow_rebuild & Criterion_Check((*root)); + if (need_rebuild) Rebuild(root); + if ((*root) != nullptr) (*root)->working_flag = false; + return; +} + +template +void KD_TREE::Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist){ + if (root == nullptr || root->tree_deleted) return; + double cur_dist = calc_box_dist(root, point); + double max_dist_sqr = max_dist * max_dist; + if (cur_dist > max_dist_sqr) return; + int retval; + if (root->need_push_down_to_left || root->need_push_down_to_right) { + retval = pthread_mutex_trylock(&(root->push_down_mutex_lock)); + if (retval == 0){ + Push_Down(root); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } else { + pthread_mutex_lock(&(root->push_down_mutex_lock)); + pthread_mutex_unlock(&(root->push_down_mutex_lock)); + } + } + if (!root->point_deleted){ + float dist = calc_dist(point, root->point); + if (dist <= max_dist_sqr && (q.size() < k_nearest || dist < q.top().dist)){ + if (q.size() >= k_nearest) q.pop(); + PointType_CMP current_point{root->point, dist}; + q.push(current_point); + } + } + int cur_search_counter; + float dist_left_node = calc_box_dist(root->left_son_ptr, point); + float dist_right_node = calc_box_dist(root->right_son_ptr, point); + if (q.size()< k_nearest || dist_left_node < q.top().dist && dist_right_node < q.top().dist){ + if (dist_left_node <= dist_right_node) { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } else { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_right_node < q.top().dist) { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } else { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } else { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } else { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + if (q.size() < k_nearest || dist_left_node < q.top().dist) { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } else { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + } else { + if (dist_left_node < q.top().dist) { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + } else { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->left_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + if (dist_right_node < q.top().dist) { + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + } else { + pthread_mutex_lock(&search_flag_mutex); + while (search_mutex_counter == -1) + { + pthread_mutex_unlock(&search_flag_mutex); + usleep(1); + pthread_mutex_lock(&search_flag_mutex); + } + search_mutex_counter += 1; + pthread_mutex_unlock(&search_flag_mutex); + Search(root->right_son_ptr, k_nearest, point, q, max_dist); + pthread_mutex_lock(&search_flag_mutex); + search_mutex_counter -= 1; + pthread_mutex_unlock(&search_flag_mutex); + } + } + } + return; +} + +template +void KD_TREE::Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector & Storage){ + if (root == nullptr) return; + Push_Down(root); + if (boxpoint.vertex_max[0] <= root->node_range_x[0] || boxpoint.vertex_min[0] > root->node_range_x[1]) return; + if (boxpoint.vertex_max[1] <= root->node_range_y[0] || boxpoint.vertex_min[1] > root->node_range_y[1]) return; + if (boxpoint.vertex_max[2] <= root->node_range_z[0] || boxpoint.vertex_min[2] > root->node_range_z[1]) return; + if (boxpoint.vertex_min[0] <= root->node_range_x[0] && boxpoint.vertex_max[0] > root->node_range_x[1] && boxpoint.vertex_min[1] <= root->node_range_y[0] && boxpoint.vertex_max[1] > root->node_range_y[1] && boxpoint.vertex_min[2] <= root->node_range_z[0] && boxpoint.vertex_max[2] > root->node_range_z[1]){ + flatten(root, Storage, NOT_RECORD); + return; + } + if (boxpoint.vertex_min[0] <= root->point.x && boxpoint.vertex_max[0] > root->point.x && boxpoint.vertex_min[1] <= root->point.y && boxpoint.vertex_max[1] > root->point.y && boxpoint.vertex_min[2] <= root->point.z && boxpoint.vertex_max[2] > root->point.z){ + if (!root->point_deleted) Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr){ + Search_by_range(root->left_son_ptr, boxpoint, Storage); + } else { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->left_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr){ + Search_by_range(root->right_son_ptr, boxpoint, Storage); + } else { + pthread_mutex_lock(&search_flag_mutex); + Search_by_range(root->right_son_ptr, boxpoint, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +void KD_TREE::Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage) +{ + if (root == nullptr) + return; + Push_Down(root); + PointType range_center; + range_center.x = (root->node_range_x[0] + root->node_range_x[1]) * 0.5; + range_center.y = (root->node_range_y[0] + root->node_range_y[1]) * 0.5; + range_center.z = (root->node_range_z[0] + root->node_range_z[1]) * 0.5; + float dist = sqrt(calc_dist(range_center, point)); + if (dist > radius + sqrt(root->radius_sq)) return; + if (dist <= radius - sqrt(root->radius_sq)) + { + flatten(root, Storage, NOT_RECORD); + return; + } + if (!root->point_deleted && calc_dist(root->point, point) <= radius * radius){ + Storage.push_back(root->point); + } + if ((Rebuild_Ptr == nullptr) || root->left_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->left_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->left_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + if ((Rebuild_Ptr == nullptr) || root->right_son_ptr != *Rebuild_Ptr) + { + Search_by_radius(root->right_son_ptr, point, radius, Storage); + } + else + { + pthread_mutex_lock(&search_flag_mutex); + Search_by_radius(root->right_son_ptr, point, radius, Storage); + pthread_mutex_unlock(&search_flag_mutex); + } + return; +} + +template +bool KD_TREE::Criterion_Check(KD_TREE_NODE * root){ + if (root->TreeSize <= Minimal_Unbalanced_Tree_Size){ + return false; + } + float balance_evaluation = 0.0f; + float delete_evaluation = 0.0f; + KD_TREE_NODE * son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) son_ptr = root->right_son_ptr; + delete_evaluation = float(root->invalid_point_num)/ root->TreeSize; + balance_evaluation = float(son_ptr->TreeSize) / (root->TreeSize-1); + if (delete_evaluation > delete_criterion_param){ + return true; + } + if (balance_evaluation > balance_criterion_param || balance_evaluation < 1-balance_criterion_param){ + return true; + } + return false; +} + +template +void KD_TREE::Push_Down(KD_TREE_NODE *root){ + if (root == nullptr) return; + Operation_Logger_Type operation; + operation.op = PUSH_DOWN; + operation.tree_deleted = root->tree_deleted; + operation.tree_downsample_deleted = root->tree_downsample_deleted; + if (root->need_push_down_to_left && root->left_son_ptr != nullptr){ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->left_son_ptr){ + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_left = false; + } else { + pthread_mutex_lock(&working_flag_mutex); + root->left_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->left_son_ptr->tree_deleted = root->tree_deleted || root->left_son_ptr->tree_downsample_deleted; + root->left_son_ptr->point_deleted = root->left_son_ptr->tree_deleted || root->left_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) root->left_son_ptr->down_del_num = root->left_son_ptr->TreeSize; + if (root->tree_deleted) root->left_son_ptr->invalid_point_num = root->left_son_ptr->TreeSize; + else root->left_son_ptr->invalid_point_num = root->left_son_ptr->down_del_num; + root->left_son_ptr->need_push_down_to_left = true; + root->left_son_ptr->need_push_down_to_right = true; + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_left = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + if (root->need_push_down_to_right && root->right_son_ptr != nullptr){ + if (Rebuild_Ptr == nullptr || *Rebuild_Ptr != root->right_son_ptr){ + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + root->need_push_down_to_right = false; + } else { + pthread_mutex_lock(&working_flag_mutex); + root->right_son_ptr->tree_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->point_downsample_deleted |= root->tree_downsample_deleted; + root->right_son_ptr->tree_deleted = root->tree_deleted || root->right_son_ptr->tree_downsample_deleted; + root->right_son_ptr->point_deleted = root->right_son_ptr->tree_deleted || root->right_son_ptr->point_downsample_deleted; + if (root->tree_downsample_deleted) root->right_son_ptr->down_del_num = root->right_son_ptr->TreeSize; + if (root->tree_deleted) root->right_son_ptr->invalid_point_num = root->right_son_ptr->TreeSize; + else root->right_son_ptr->invalid_point_num = root->right_son_ptr->down_del_num; + root->right_son_ptr->need_push_down_to_left = true; + root->right_son_ptr->need_push_down_to_right = true; + if (rebuild_flag){ + pthread_mutex_lock(&rebuild_logger_mutex_lock); + Rebuild_Logger.push(operation); + pthread_mutex_unlock(&rebuild_logger_mutex_lock); + } + root->need_push_down_to_right = false; + pthread_mutex_unlock(&working_flag_mutex); + } + } + return; +} + +template +void KD_TREE::Update(KD_TREE_NODE * root){ + KD_TREE_NODE * left_son_ptr = root->left_son_ptr; + KD_TREE_NODE * right_son_ptr = root->right_son_ptr; + float tmp_range_x[2] = {INFINITY, -INFINITY}; + float tmp_range_y[2] = {INFINITY, -INFINITY}; + float tmp_range_z[2] = {INFINITY, -INFINITY}; + // Update Tree Size + if (left_son_ptr != nullptr && right_son_ptr != nullptr){ + root->TreeSize = left_son_ptr->TreeSize + right_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + right_son_ptr->invalid_point_num + (root->point_deleted? 1:0); + root->down_del_num = left_son_ptr->down_del_num + right_son_ptr->down_del_num + (root->point_downsample_deleted? 1:0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !right_son_ptr->tree_deleted && !root->point_deleted)){ + tmp_range_x[0] = min(min(left_son_ptr->node_range_x[0],right_son_ptr->node_range_x[0]),root->point.x); + tmp_range_x[1] = max(max(left_son_ptr->node_range_x[1],right_son_ptr->node_range_x[1]),root->point.x); + tmp_range_y[0] = min(min(left_son_ptr->node_range_y[0],right_son_ptr->node_range_y[0]),root->point.y); + tmp_range_y[1] = max(max(left_son_ptr->node_range_y[1],right_son_ptr->node_range_y[1]),root->point.y); + tmp_range_z[0] = min(min(left_son_ptr->node_range_z[0],right_son_ptr->node_range_z[0]),root->point.z); + tmp_range_z[1] = max(max(left_son_ptr->node_range_z[1],right_son_ptr->node_range_z[1]),root->point.z); + } else { + if (!left_son_ptr->tree_deleted){ + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!right_son_ptr->tree_deleted){ + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted){ + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } else if (left_son_ptr != nullptr){ + root->TreeSize = left_son_ptr->TreeSize + 1; + root->invalid_point_num = left_son_ptr->invalid_point_num + (root->point_deleted?1:0); + root->down_del_num = left_son_ptr->down_del_num + (root->point_downsample_deleted?1:0); + root->tree_downsample_deleted = left_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = left_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!left_son_ptr->tree_deleted && !root->point_deleted)){ + tmp_range_x[0] = min(left_son_ptr->node_range_x[0],root->point.x); + tmp_range_x[1] = max(left_son_ptr->node_range_x[1],root->point.x); + tmp_range_y[0] = min(left_son_ptr->node_range_y[0],root->point.y); + tmp_range_y[1] = max(left_son_ptr->node_range_y[1],root->point.y); + tmp_range_z[0] = min(left_son_ptr->node_range_z[0],root->point.z); + tmp_range_z[1] = max(left_son_ptr->node_range_z[1],root->point.z); + } else { + if (!left_son_ptr->tree_deleted){ + tmp_range_x[0] = min(tmp_range_x[0], left_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], left_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], left_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], left_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], left_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], left_son_ptr->node_range_z[1]); + } + if (!root->point_deleted){ + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + + } else if (right_son_ptr != nullptr){ + root->TreeSize = right_son_ptr->TreeSize + 1; + root->invalid_point_num = right_son_ptr->invalid_point_num + (root->point_deleted? 1:0); + root->down_del_num = right_son_ptr->down_del_num + (root->point_downsample_deleted? 1:0); + root->tree_downsample_deleted = right_son_ptr->tree_downsample_deleted & root->point_downsample_deleted; + root->tree_deleted = right_son_ptr->tree_deleted && root->point_deleted; + if (root->tree_deleted || (!right_son_ptr->tree_deleted && !root->point_deleted)){ + tmp_range_x[0] = min(right_son_ptr->node_range_x[0],root->point.x); + tmp_range_x[1] = max(right_son_ptr->node_range_x[1],root->point.x); + tmp_range_y[0] = min(right_son_ptr->node_range_y[0],root->point.y); + tmp_range_y[1] = max(right_son_ptr->node_range_y[1],root->point.y); + tmp_range_z[0] = min(right_son_ptr->node_range_z[0],root->point.z); + tmp_range_z[1] = max(right_son_ptr->node_range_z[1],root->point.z); + } else { + if (!right_son_ptr->tree_deleted){ + tmp_range_x[0] = min(tmp_range_x[0], right_son_ptr->node_range_x[0]); + tmp_range_x[1] = max(tmp_range_x[1], right_son_ptr->node_range_x[1]); + tmp_range_y[0] = min(tmp_range_y[0], right_son_ptr->node_range_y[0]); + tmp_range_y[1] = max(tmp_range_y[1], right_son_ptr->node_range_y[1]); + tmp_range_z[0] = min(tmp_range_z[0], right_son_ptr->node_range_z[0]); + tmp_range_z[1] = max(tmp_range_z[1], right_son_ptr->node_range_z[1]); + } + if (!root->point_deleted){ + tmp_range_x[0] = min(tmp_range_x[0], root->point.x); + tmp_range_x[1] = max(tmp_range_x[1], root->point.x); + tmp_range_y[0] = min(tmp_range_y[0], root->point.y); + tmp_range_y[1] = max(tmp_range_y[1], root->point.y); + tmp_range_z[0] = min(tmp_range_z[0], root->point.z); + tmp_range_z[1] = max(tmp_range_z[1], root->point.z); + } + } + } else { + root->TreeSize = 1; + root->invalid_point_num = (root->point_deleted? 1:0); + root->down_del_num = (root->point_downsample_deleted? 1:0); + root->tree_downsample_deleted = root->point_downsample_deleted; + root->tree_deleted = root->point_deleted; + tmp_range_x[0] = root->point.x; + tmp_range_x[1] = root->point.x; + tmp_range_y[0] = root->point.y; + tmp_range_y[1] = root->point.y; + tmp_range_z[0] = root->point.z; + tmp_range_z[1] = root->point.z; + } + memcpy(root->node_range_x,tmp_range_x,sizeof(tmp_range_x)); + memcpy(root->node_range_y,tmp_range_y,sizeof(tmp_range_y)); + memcpy(root->node_range_z,tmp_range_z,sizeof(tmp_range_z)); + float x_L = (root->node_range_x[1] - root->node_range_x[0]) * 0.5; + float y_L = (root->node_range_y[1] - root->node_range_y[0]) * 0.5; + float z_L = (root->node_range_z[1] - root->node_range_z[0]) * 0.5; + root->radius_sq = x_L*x_L + y_L * y_L + z_L * z_L; + if (left_son_ptr != nullptr) left_son_ptr -> father_ptr = root; + if (right_son_ptr != nullptr) right_son_ptr -> father_ptr = root; + if (root == Root_Node && root->TreeSize > 3){ + KD_TREE_NODE * son_ptr = root->left_son_ptr; + if (son_ptr == nullptr) son_ptr = root->right_son_ptr; + float tmp_bal = float(son_ptr->TreeSize) / (root->TreeSize-1); + root->alpha_del = float(root->invalid_point_num)/ root->TreeSize; + root->alpha_bal = (tmp_bal>=0.5-EPSS)?tmp_bal:1-tmp_bal; + } + return; +} + +template +void KD_TREE::flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type){ + if (root == nullptr) return; + Push_Down(root); + if (!root->point_deleted) { + Storage.push_back(root->point); + } + flatten(root->left_son_ptr, Storage, storage_type); + flatten(root->right_son_ptr, Storage, storage_type); + switch (storage_type) + { + case NOT_RECORD: + break; + case DELETE_POINTS_REC: + if (root->point_deleted && !root->point_downsample_deleted) { + Points_deleted.push_back(root->point); + } + break; + case MULTI_THREAD_REC: + if (root->point_deleted && !root->point_downsample_deleted) { + Multithread_Points_deleted.push_back(root->point); + } + break; + default: + break; + } + return; +} + +template +void KD_TREE::delete_tree_nodes(KD_TREE_NODE ** root){ + if (*root == nullptr) return; + Push_Down(*root); + delete_tree_nodes(&(*root)->left_son_ptr); + delete_tree_nodes(&(*root)->right_son_ptr); + + pthread_mutex_destroy( &(*root)->push_down_mutex_lock); + delete *root; + *root = nullptr; + + return; +} + +template +bool KD_TREE::same_point(PointType a, PointType b){ + return (fabs(a.x-b.x) < EPSS && fabs(a.y-b.y) < EPSS && fabs(a.z-b.z) < EPSS ); +} + +template +float KD_TREE::calc_dist(PointType a, PointType b){ + float dist = 0.0f; + dist = (a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y) + (a.z-b.z)*(a.z-b.z); + return dist; +} + +template +float KD_TREE::calc_box_dist(KD_TREE_NODE * node, PointType point){ + if (node == nullptr) return INFINITY; + float min_dist = 0.0; + if (point.x < node->node_range_x[0]) min_dist += (point.x - node->node_range_x[0])*(point.x - node->node_range_x[0]); + if (point.x > node->node_range_x[1]) min_dist += (point.x - node->node_range_x[1])*(point.x - node->node_range_x[1]); + if (point.y < node->node_range_y[0]) min_dist += (point.y - node->node_range_y[0])*(point.y - node->node_range_y[0]); + if (point.y > node->node_range_y[1]) min_dist += (point.y - node->node_range_y[1])*(point.y - node->node_range_y[1]); + if (point.z < node->node_range_z[0]) min_dist += (point.z - node->node_range_z[0])*(point.z - node->node_range_z[0]); + if (point.z > node->node_range_z[1]) min_dist += (point.z - node->node_range_z[1])*(point.z - node->node_range_z[1]); + return min_dist; +} + +template bool KD_TREE::point_cmp_x(PointType a, PointType b) { return a.x < b.x;} +template bool KD_TREE::point_cmp_y(PointType a, PointType b) { return a.y < b.y;} +template bool KD_TREE::point_cmp_z(PointType a, PointType b) { return a.z < b.z;} + +// manual queue +template +void MANUAL_Q::clear(){ + head = 0; + tail = 0; + counter = 0; + is_empty = true; + return; +} + +template +void MANUAL_Q::pop(){ + if (counter == 0) return; + head ++; + head %= Q_LEN; + counter --; + if (counter == 0) is_empty = true; + return; +} + +template +T MANUAL_Q::front(){ + return q[head]; +} + +template +T MANUAL_Q::back(){ + return q[tail]; +} + +template +void MANUAL_Q::push(T op){ + q[tail] = op; + counter ++; + if (is_empty) is_empty = false; + tail ++; + tail %= Q_LEN; +} + +template +bool MANUAL_Q::empty(){ + return is_empty; +} + +template +int MANUAL_Q::size(){ + return counter; +} + +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; +template class KD_TREE; \ No newline at end of file diff --git a/include/ikd-Tree/ikd_Tree.h b/include/ikd-Tree/ikd_Tree.h new file mode 100644 index 0000000..ed7eb77 --- /dev/null +++ b/include/ikd-Tree/ikd_Tree.h @@ -0,0 +1,251 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define EPSS 1e-6 +#define Minimal_Unbalanced_Tree_Size 10 +#define Multi_Thread_Rebuild_Point_Num 1500 +#define DOWNSAMPLE_SWITCH true +#define ForceRebuildPercentage 0.2 +#define Q_LEN 1000000 + +using namespace std; + +struct ikdTree_PointType +{ + float x,y,z; + ikdTree_PointType (float px = 0.0f, float py = 0.0f, float pz = 0.0f){ + x = px; + y = py; + z = pz; + } +}; + +struct BoxPointType{ + float vertex_min[3]; + float vertex_max[3]; +}; + +enum operation_set {ADD_POINT, DELETE_POINT, DELETE_BOX, ADD_BOX, DOWNSAMPLE_DELETE, PUSH_DOWN}; + +enum delete_point_storage_set {NOT_RECORD, DELETE_POINTS_REC, MULTI_THREAD_REC}; + +template +class MANUAL_Q{ + private: + int head = 0,tail = 0, counter = 0; + T q[Q_LEN]; + bool is_empty; + public: + void pop(); + T front(); + T back(); + void clear(); + void push(T op); + bool empty(); + int size(); +}; + + + +template +class KD_TREE{ +public: + using PointVector = vector>; + using Ptr = shared_ptr>; + struct KD_TREE_NODE{ + PointType point; + uint8_t division_axis; + int TreeSize = 1; + int invalid_point_num = 0; + int down_del_num = 0; + bool point_deleted = false; + bool tree_deleted = false; + bool point_downsample_deleted = false; + bool tree_downsample_deleted = false; + bool need_push_down_to_left = false; + bool need_push_down_to_right = false; + bool working_flag = false; + float radius_sq; + pthread_mutex_t push_down_mutex_lock; + float node_range_x[2], node_range_y[2], node_range_z[2]; + KD_TREE_NODE *left_son_ptr = nullptr; + KD_TREE_NODE *right_son_ptr = nullptr; + KD_TREE_NODE *father_ptr = nullptr; + // For paper data record + float alpha_del; + float alpha_bal; + }; + + struct Operation_Logger_Type{ + PointType point; + BoxPointType boxpoint; + bool tree_deleted, tree_downsample_deleted; + operation_set op; + }; + + struct PointType_CMP{ + PointType point; + float dist = 0.0; + PointType_CMP (PointType p = PointType(), float d = INFINITY){ + this->point = p; + this->dist = d; + }; + bool operator < (const PointType_CMP &a)const{ + if (fabs(dist - a.dist) < 1e-10) return point.x < a.point.x; + else return dist < a.dist; + } + }; + + class MANUAL_HEAP{ + public: + MANUAL_HEAP(int max_capacity = 100){ + cap = max_capacity; + heap = new PointType_CMP[max_capacity]; + heap_size = 0; + } + + ~MANUAL_HEAP(){ delete[] heap;} + + void pop(){ + if (heap_size == 0) return; + heap[0] = heap[heap_size-1]; + heap_size--; + MoveDown(0); + return; + } + + PointType_CMP top(){ return heap[0];} + + void push(PointType_CMP point){ + if (heap_size >= cap) return; + heap[heap_size] = point; + FloatUp(heap_size); + heap_size++; + return; + } + + int size(){ return heap_size;} + + void clear(){ heap_size = 0;} + private: + int heap_size = 0; + int cap = 0; + PointType_CMP * heap; + void MoveDown(int heap_index){ + int l = heap_index * 2 + 1; + PointType_CMP tmp = heap[heap_index]; + while (l < heap_size){ + if (l + 1 < heap_size && heap[l] < heap[l+1]) l++; + if (tmp < heap[l]){ + heap[heap_index] = heap[l]; + heap_index = l; + l = heap_index * 2 + 1; + } else break; + } + heap[heap_index] = tmp; + return; + } + + void FloatUp(int heap_index){ + int ancestor = (heap_index-1)/2; + PointType_CMP tmp = heap[heap_index]; + while (heap_index > 0){ + if (heap[ancestor] < tmp){ + heap[heap_index] = heap[ancestor]; + heap_index = ancestor; + ancestor = (heap_index-1)/2; + } else break; + } + heap[heap_index] = tmp; + return; + } + + }; + +private: + // Multi-thread Tree Rebuild + bool termination_flag = false; + bool rebuild_flag = false; + pthread_t rebuild_thread; + pthread_mutex_t termination_flag_mutex_lock, rebuild_ptr_mutex_lock, working_flag_mutex, search_flag_mutex; + pthread_mutex_t rebuild_logger_mutex_lock, points_deleted_rebuild_mutex_lock; + // queue Rebuild_Logger; + MANUAL_Q Rebuild_Logger; + PointVector Rebuild_PCL_Storage; + KD_TREE_NODE ** Rebuild_Ptr = nullptr; + int search_mutex_counter = 0; + static void * multi_thread_ptr(void *arg); + void multi_thread_rebuild(); + void start_thread(); + void stop_thread(); + void run_operation(KD_TREE_NODE ** root, Operation_Logger_Type operation); + // KD Tree Functions and augmented variables + int Treesize_tmp = 0, Validnum_tmp = 0; + float alpha_bal_tmp = 0.5, alpha_del_tmp = 0.0; + float delete_criterion_param = 0.5f; + float balance_criterion_param = 0.7f; + float downsample_size = 0.2f; + bool Delete_Storage_Disabled = false; + KD_TREE_NODE * STATIC_ROOT_NODE = nullptr; + PointVector Points_deleted; + PointVector Downsample_Storage; + PointVector Multithread_Points_deleted; + void InitTreeNode(KD_TREE_NODE * root); + void Test_Lock_States(KD_TREE_NODE *root); + void BuildTree(KD_TREE_NODE ** root, int l, int r, PointVector & Storage); + void Rebuild(KD_TREE_NODE ** root); + int Delete_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild, bool is_downsample); + void Delete_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild); + void Add_by_point(KD_TREE_NODE ** root, PointType point, bool allow_rebuild, int father_axis); + void Add_by_range(KD_TREE_NODE ** root, BoxPointType boxpoint, bool allow_rebuild); + void Search(KD_TREE_NODE * root, int k_nearest, PointType point, MANUAL_HEAP &q, double max_dist);//priority_queue + void Search_by_range(KD_TREE_NODE *root, BoxPointType boxpoint, PointVector &Storage); + void Search_by_radius(KD_TREE_NODE *root, PointType point, float radius, PointVector &Storage); + bool Criterion_Check(KD_TREE_NODE * root); + void Push_Down(KD_TREE_NODE * root); + void Update(KD_TREE_NODE * root); + void delete_tree_nodes(KD_TREE_NODE ** root); + void downsample(KD_TREE_NODE ** root); + bool same_point(PointType a, PointType b); + float calc_dist(PointType a, PointType b); + float calc_box_dist(KD_TREE_NODE * node, PointType point); + static bool point_cmp_x(PointType a, PointType b); + static bool point_cmp_y(PointType a, PointType b); + static bool point_cmp_z(PointType a, PointType b); + +public: + KD_TREE(float delete_param = 0.5, float balance_param = 0.6 , float box_length = 0.2); + ~KD_TREE(); + void Set_delete_criterion_param(float delete_param); + void Set_balance_criterion_param(float balance_param); + void set_downsample_param(float box_length); + void InitializeKDTree(float delete_param = 0.5, float balance_param = 0.7, float box_length = 0.2); + int size(); + int validnum(); + void root_alpha(float &alpha_bal, float &alpha_del); + void Build(PointVector point_cloud); + void Nearest_Search(PointType point, int k_nearest, PointVector &Nearest_Points, vector & Point_Distance, double max_dist = INFINITY); + void Box_Search(const BoxPointType &Box_of_Point, PointVector &Storage); + void Radius_Search(PointType point, const float radius, PointVector &Storage); + int Add_Points(PointVector & PointToAdd, bool downsample_on); + void Add_Point_Boxes(vector & BoxPoints); + void Delete_Points(PointVector & PointToDel); + int Delete_Point_Boxes(vector & BoxPoints); + void flatten(KD_TREE_NODE * root, PointVector &Storage, delete_point_storage_set storage_type); + void acquire_removed_points(PointVector & removed_points); + BoxPointType tree_range(); + PointVector PCL_Storage; + KD_TREE_NODE * Root_Node = nullptr; + int max_queue_size = 0; +}; + + diff --git a/include/ikd-Tree/materials/imgs/Box_Search.png b/include/ikd-Tree/materials/imgs/Box_Search.png new file mode 100644 index 0000000..34f730d Binary files /dev/null and b/include/ikd-Tree/materials/imgs/Box_Search.png differ diff --git a/include/ikd-Tree/materials/imgs/HKU_campus.png b/include/ikd-Tree/materials/imgs/HKU_campus.png new file mode 100644 index 0000000..4b3a6f5 Binary files /dev/null and b/include/ikd-Tree/materials/imgs/HKU_campus.png differ diff --git a/include/ikd-Tree/materials/imgs/Radius_Search.png b/include/ikd-Tree/materials/imgs/Radius_Search.png new file mode 100644 index 0000000..888e911 Binary files /dev/null and b/include/ikd-Tree/materials/imgs/Radius_Search.png differ diff --git a/include/ikd-Tree/materials/imgs/remain.png b/include/ikd-Tree/materials/imgs/remain.png new file mode 100644 index 0000000..2743b37 Binary files /dev/null and b/include/ikd-Tree/materials/imgs/remain.png differ diff --git a/include/ikd-Tree/materials/imgs/removed.png b/include/ikd-Tree/materials/imgs/removed.png new file mode 100644 index 0000000..bcf0a94 Binary files /dev/null and b/include/ikd-Tree/materials/imgs/removed.png differ