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

.gitignore

@@ -0,0 +1,4 @@
+.vscode/
+build/
+cmake-build-release/
+cmake-build-debug/

CMakeLists.txt

@@ -4,9 +4,18 @@ project(ikd_tree_demo)
 
 find_package(PCL 1.8 REQUIRED)
 
-include_directories(${PCL_INCLUDE_DIRS})
+
+include_directories(
+    ${PCL_INCLUDE_DIRS}
+    ikd_Tree)
 link_directories(${PCL_LIBRARIES_DIRS})
 add_definitions(${PCL_DEFINITIONS})
 
-add_executable(ikd_tree_demo ikd_Tree_demo.cpp ikd_Tree.cpp)
-target_link_libraries(ikd_tree_demo ${PCL_LIBRARIES})
+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(search_points_by_box examples/ikd_Tree_Search_Points_by_box.cpp ikd_Tree/ikd_Tree.cpp)
+target_link_libraries(search_points_by_box ${PCL_LIBRARIES})

README.md

@@ -31,3 +31,36 @@ More details please refer to our paper and video~
   - Add a new parameter `max_dist` to support ranged search to achieve faster nearest search in robotic applications.
   - Fix minor bugs to improve the overall performance. 
 
+
+## Build & Run demo
+### 1. How to build this project
+```
+cd ~/catkin_ws/src
+git clone git@github.com:hku-mars/ikd-Tree.git
+cd ikd-Tree/build
+cmake ..
+make -j 9
+cd ../materials
+wget https://urserver.kaist.ac.kr/publicdata/ikd-tree/large_scale_map.pcd
+```
+### 2. Run our examples
+
+```
+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
+./search_points_by_box
+# Example 3. An aysnc. exmaple for readers' better understanding of the principle of ikd-Tree
+./ikd_tree_async_demo
+```
+
+Then, you can show the visualized examples as follows!
+
+Input (HKUST's Red Bird Sundial) |  Output of Example 2
+:-------------------------:|:-------------------------:
+![](materials/imgs/sundial.png) |  ![](materials/imgs/search_points_example.png)
+
+Input (A large scale map) |  Output of Example 3
+:-------------------------:|:-------------------------:
+![](materials/imgs/large_map.png) |  ![](materials/imgs/ikd_async.png)

examples/ikd_Tree_Async_demo.cpp

@@ -0,0 +1,129 @@
+#include "ikd_Tree.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <algorithm>
+#include "pcl/point_types.h"
+#include "pcl/common/common.h"
+#include "pcl/point_cloud.h"
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+using PointType = pcl::PointXYZ;
+using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
+
+void colorize( const PointVector &pc, pcl::PointCloud<pcl::PointXYZRGB> &pc_colored, const std::vector<int> &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 &center_pt, vector<float> 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<PointType>::Ptr kdtree_ptr(new KD_TREE<PointType>(0.3, 0.6, 0.2));
+    KD_TREE<PointType>      &ikd_Tree        = *kdtree_ptr;
+
+    /*** 2. Load point cloud data */
+    pcl::PointCloud<PointType>::Ptr src(new pcl::PointCloud<PointType>);
+    string filename = "../materials/large_scale_map.pcd";
+    if (pcl::io::loadPCDFile<PointType>(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<int>(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<chrono::microseconds>(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 = 200.0;
+    center_pt.y = 0.0;
+    center_pt.z = 0.0;
+    BoxPointType boxpoint;
+    generate_box(boxpoint, center_pt, {50.0, 50.0, 100.0});
+
+    start = chrono::high_resolution_clock::now();
+    vector<BoxPointType> boxes = {boxpoint};
+    int num_deleted = ikd_Tree.Delete_Point_Boxes(boxes);
+    end  = chrono::high_resolution_clock::now();
+    duration = chrono::duration_cast<chrono::microseconds>(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<int>(Removed_Points.size()));
+
+    /*** 5. Check remaining points in ikd-Tree */
+    pcl::PointCloud<PointType>::Ptr Remaining_Points(new pcl::PointCloud<PointType>);
+    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<int>(Remaining_Points->points.size()));
+
+    /*** Below codes are just for visualization */
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr removed_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr remaining_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+    colorize(src->points, *src_colored, {255, 0, 0});
+    colorize(Removed_Points, *removed_colored, {0, 255, 0});
+    colorize(Remaining_Points->points, *remaining_colored, {0, 0, 255});
+
+    pcl::visualization::PCLVisualizer viewer0("output");
+    viewer0.addPointCloud<pcl::PointXYZRGB>(src_colored, "src");
+    viewer0.addPointCloud<pcl::PointXYZRGB>(removed_colored, "removed");
+    viewer0.setCameraPosition(200, 200, 800, 0, 0, 0);
+    viewer0.setSize(800, 600);
+
+    pcl::visualization::PCLVisualizer viewer1("remaining");
+    viewer1.addPointCloud<pcl::PointXYZRGB>(remaining_colored, "remaining");
+    viewer1.setCameraPosition(200, 200, 800, 0, 0, 0);
+    viewer1.setPosition(800, 0);
+    viewer1.setSize(800, 600);
+
+    while (!viewer0.wasStopped() && !viewer1.wasStopped()) {// } && !viewer2.wasStopped()) {
+        viewer0.spinOnce();
+        viewer1.spinOnce();
+    }
+
+    return 0;
+}

examples/ikd_Tree_Search_Points_by_box.cpp

@@ -0,0 +1,117 @@
+#include "ikd_Tree.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <random>
+#include <algorithm>
+#include "pcl/point_types.h"
+#include "pcl/common/common.h"
+#include "pcl/point_cloud.h"
+#include <pcl/io/pcd_io.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+using PointType = pcl::PointXYZ;
+using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
+
+void colorize( const PointVector &pc, pcl::PointCloud<pcl::PointXYZRGB> &pc_colored, const std::vector<int> &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 &center_pt, vector<float> 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<PointType>::Ptr kdtree_ptr(new KD_TREE<PointType>(0.3, 0.6, 0.2));
+    KD_TREE<PointType>      &ikd_Tree        = *kdtree_ptr;
+
+    /*** 2. Load point cloud data */
+    pcl::PointCloud<PointType>::Ptr src(new pcl::PointCloud<PointType>);
+    string filename = "../materials/sundial.pcd";
+    if (pcl::io::loadPCDFile<PointType>(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<int>(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<chrono::microseconds>(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 the corresponding region */
+    PointType center_pt;
+    center_pt.x = 0.0;
+    center_pt.y = 0.0;
+    center_pt.z = 0.0;
+    BoxPointType boxpoint;
+    generate_box(boxpoint, center_pt, {5.0, 5.0, 100.0});
+
+    start = chrono::high_resolution_clock::now();
+    PointVector Searched_Points;
+    ikd_Tree.Search_Points_by_box(boxpoint, Searched_Points);
+    end  = chrono::high_resolution_clock::now();
+    duration = chrono::duration_cast<chrono::microseconds>(end - start).count();
+    printf("Search Points by box takes: %0.3f ms\n", float(duration) / 1e3);
+
+    /*** 5. Check remaining points in ikd-Tree */
+    pcl::PointCloud<PointType>::Ptr Remaining_Points(new pcl::PointCloud<PointType>);
+    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<int>(Remaining_Points->points.size()));
+
+    /*** Below codes are just for visualization */
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr searched_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
+    pcl::PointCloud<pcl::PointXYZRGB>::Ptr remaining_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+    colorize(src->points, *src_colored, {255, 0, 0});
+    colorize(Searched_Points, *searched_colored, {0, 255, 0});
+    colorize(Remaining_Points->points, *remaining_colored, {0, 0, 255});
+
+    pcl::visualization::PCLVisualizer viewer0("output");
+    viewer0.addPointCloud<pcl::PointXYZRGB>(src_colored, "src");
+    viewer0.addPointCloud<pcl::PointXYZRGB>(searched_colored, "searched");
+    viewer0.setCameraPosition(-120, 120, 120, 0, 0, 45);
+    viewer0.setSize(800, 600);
+
+    pcl::visualization::PCLVisualizer viewer1("remaining");
+    viewer1.addPointCloud<pcl::PointXYZRGB>(remaining_colored, "remaining");
+    viewer1.setCameraPosition(-120, 120, 120, 0, 0, 45);
+    viewer1.setPosition(800, 0);
+    viewer1.setSize(800, 600);
+
+    while (!viewer0.wasStopped() && !viewer1.wasStopped()) {// } && !viewer2.wasStopped()) {
+        viewer0.spinOnce();
+        viewer1.spinOnce();
+    }
+
+    return 0;
+}

ikd_Tree.cpp → ikd_Tree/ikd_Tree.cpp

@@ -459,6 +459,13 @@ void KD_TREE<PointType>::Nearest_Search(PointType point, int k_nearest, PointVec
     return;
 }
 
+template <typename PointType>
+int KD_TREE<PointType>::Search_Points_by_box(const BoxPointType &Box_of_Point, PointVector &Storage)
+{
+    Storage.clear();
+    Search_by_range(Root_Node, Box_of_Point, Storage);
+}
+
 template <typename PointType>
 int KD_TREE<PointType>::Add_Points(PointVector &PointToAdd, bool downsample_on)
 {

ikd_Tree.h → ikd_Tree/ikd_Tree.h

@@ -324,6 +324,7 @@ class KD_TREE
     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<float> &Point_Distance, double max_dist = INFINITY);
+    int Search_Points_by_box(const BoxPointType &Box_of_Point, PointVector &Storage);
     int Add_Points(PointVector &PointToAdd, bool downsample_on);
     void Add_Point_Boxes(vector<BoxPointType> &BoxPoints);
     void Delete_Points(PointVector &PointToDel);