Weird phenomenon on `Delete_Point_Boxes` function
LimHyungTae opened this issue · 4 comments
Dear Admin:
First, thank you for sharing your invaluable codes.
But, I'd like to use your incremental Tree algorithm to achieve radius search; unfortunately, I think the code doesn't support the radius search function.
Is there any particular issue not implementing radius search?
Thanks in advance!
So, I use Delete_Point_Boxes to substitute radius search, yet it doesn't work.
The example snippet is as follows:
#include "ikd_Tree.h"
#include <stdio.h>
#include <stdlib.h>
#include <random>
#include <algorithm>
#include "pcl/point_types.h"
#include "pcl/point_cloud.h"
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
using PointType = pcl::PointXYZI;
using PointVector = std::vector<PointType, Eigen::aligned_allocator<PointType>>;
#define Box_Length 50.0
#define Box_Num 1
PointVector point_cloud;
PointVector cloud_increment;
PointVector cloud_decrement;
PointVector cloud_deleted;
PointVector search_result;
PointVector raw_cmp_result;
PointVector DeletePoints;
PointVector removed_points;
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(vector<BoxPointType>& boxes, const PointType& center_pt, vector<float> box_lengths){
boxes.clear();
BoxPointType boxpoint;
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;
boxes.emplace_back(boxpoint);
}
/*
Generate target point for nearest search on the incremental k-d tree
*/
int main(int argc, char **argv) {
srand((unsigned) time(NULL));
// KD_TREE<PointType> ikd_Tree(0.3,0.6,0.2);
KD_TREE<PointType>::Ptr kdtree_ptr(new KD_TREE<PointType>(0.3, 0.6, 0.2));
KD_TREE<PointType> &ikd_Tree = *kdtree_ptr;
vector<BoxPointType> Delete_Boxes;
vector<BoxPointType> Add_Boxes;
vector<float> PointDist;
// Initialize k-d tree
pcl::PointCloud<PointType>::Ptr src(new pcl::PointCloud<PointType>);
string HOME = std::getenv("HOME");
string filename = HOME + "/catkin_ws/src/ikd_tree_ros/sample/global_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);
}
point_cloud = (*src).points;
auto t1 = chrono::high_resolution_clock::now();
ikd_Tree.Build(point_cloud);
auto t2 = chrono::high_resolution_clock::now();
auto duration = chrono::duration_cast<chrono::microseconds>(t2 - t1).count();
printf("Building tree takes: %0.3f ms\n", float(duration) / 1e3);
cout << "# of valid pts: " << ikd_Tree.validnum() << endl;
PointType center_pt;
center_pt.x = 100.0;
center_pt.y = 0.0;
center_pt.z = 0.0;
generate_box(Delete_Boxes, center_pt, {50.0, 50.0, 50.0});
t1 = chrono::high_resolution_clock::now();
int num_deleted = ikd_Tree.Delete_Point_Boxes(Delete_Boxes);
t2 = chrono::high_resolution_clock::now();
duration = chrono::duration_cast<chrono::microseconds>(t2 - t1).count();
printf("Radius delete takes: %0.3f ms\n", float(duration) / 1e3);
removed_points.clear();
ikd_Tree.acquire_removed_points(removed_points);
cout << "\033[1;32m # of deleted points - " << num_deleted << "\033[0m" << endl;
cout << "\033[1;32m # of removed points - " << removed_points.size() << "\033[0m" << endl;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr vox_colored(new pcl::PointCloud<pcl::PointXYZRGB>);
// Below lines are for visualization
cout<<point_cloud.size()<<endl;
cout<<point_cloud.size()<<endl;
cout<<removed_points.size()<<endl;
colorize(point_cloud, *src_colored, {255, 0, 0});
colorize(removed_points, *vox_colored, {0, 255, 0});
pcl::visualization::PCLVisualizer viewer("Raw");
viewer.addPointCloud<pcl::PointXYZRGB>(src_colored, "src_red");
viewer.addPointCloud<pcl::PointXYZRGB>(vox_colored, "vox_green");
auto & bpt = Delete_Boxes[0];
Eigen::Vector3f t((bpt.vertex_min[0] + bpt.vertex_max[0]) / 2,
(bpt.vertex_min[1] + bpt.vertex_max[1]) / 2,
(bpt.vertex_min[2] + bpt.vertex_max[2]) / 2);
Eigen::Quaternionf q(1.0, 0.0 ,0.0 ,0.0);
// viewer.addCube(t, q, Box_Length, Box_Length, Box_Length, "cube");
while (!viewer.wasStopped()) {// } && !viewer2.wasStopped()) {
viewer.spinOnce();
}
return 0;
}I'd like to extract the region highlighted by the yellow box (Please see PointType center_pt; and the function generate_box(Delete_Boxes, center_pt, {50.0, 50.0, 50.0});. The below grid size is 50.0m)
But there are a few deleted points and # of deleted points and # of removed points are different...:(
# of deleted points - 863317
# of removed points - 149328
Could you explain why this phenomenon occurs?
Furthermore, I found that ikd tree successfully has the complement of the box region as follows:
Codes to get complement of point cloud
pcl::PointCloud<PointType>::Ptr dst(new pcl::PointCloud <PointType>);
ikd_Tree.flatten(ikd_Tree.Root_Node, ikd_Tree.PCL_Storage, NOT_RECORD);
dst->points = ikd_Tree.PCL_Storage;Result (Visualization of dst)
How can I get the deleted points successfully? It would be a great help for me and I'll upload some visualized examples of ikd-tree and send pull requests for letting other people use this super-wonderful method broadly!
SORRY for my late reply.
For the radius search, it can be easily implemented on the ikd-Tree in a very similar way to the function Search_by_range. However, since the ikd-Tree was designed for FAST-LIO2 which did not require radius search, only range knn search is provided. I will update a new version with radius search next week to make the ikd-Tree more applicable.
For your test result, the box delete simply attaches 'deleted' labels on the nodes, after which those deleted points are removed during a rebuilding process. In other words, the delete operation and the remove operation are not performed at the same time. This is why you saw different number of deleted points (from delete operation) and removed points (from acquire_removed_points function`). Please refer to our preprint paper for further details.
If you want to obtain the deleted points, you should use Search_by_range function before you use box delete. This is also what I have done in the downsampling process.
Should you have any further questions, please mention me in your issues as it will automatically send me an email to notice.
Dear @Ecstasy-EC , you're 100% right!! I read the paper and I've known the async. operation of ikd-tree, but I think I didn't understand what eventually means lol. As you mentioned, using Search_by_range() function works as I wished as follows!!!
To achieve this, I wrapped up the private function Search_by_range() by a new public function called Get_Points_in_Box() in your code format.
template <typename PointType>
int KD_TREE<PointType>::Get_Points_in_Box(const BoxPointType &Box_of_Point, PointVector &Storage)
{
Storage.clear();
Search_by_range(Root_Node, Box_of_Point, Storage);
}It is super fantastic! I can't wait for the next week, i.e. updated version of ikd-tree. 😍 Thank you!