deleted points can still be found in kdtree.
DingFeng9313 opened this issue · 10 comments
I run ikd-tree with a running sliding window of point cloud. pointclouds are queued in time_stamp sequential. each point are added to ikdtree and then delete later. But as I was checking kdtree points with these code:
if(0)
//If you need to see map point, change to "if(1)"
{
PointVector ().swap(ikdtree.PCL_Storage);
ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
featsFromMap->clear();
featsFromMap->points = ikdtree.PCL_Storage;
}
then publish these point clouds.
I found there are always points not deleted, i.e. they remain in the kdtree even though deleted.
Another question, what if i add_Points with downsample_flag=True. Then delete original point later. Will ikdtree delete this cuboid? what behavior will ikdtree generate?
I play around with ikdtree-demo. And found the same problem.
My setting is:
#define Point_Num 100
#define New_Point_Num 6
#define Delete_Point_Num 6
#define Nearest_Num 5
#define Test_Time 1000
If I understand it right, after 100 test time, the remaining valid num which I checked with ikd_Tree.validnum() will still be 100.
But it wasn't. No matter I use
ikd_Tree.Add_Points(cloud_increment, false);
or
ikd_Tree.Add_Points(cloud_increment, true);
Is this a bug? Or I have misunderstood ikdtree's usage?
To clarify, I use the same point cloud for Add_Points and Delete_Points, i.e.
ikd_Tree.Add_Points(cloud_increment, false);
ikd_Tree.Delete_Points(cloud_increment);
So that all point deleted must have been added before.
I play around with ikdtree-demo. And found the same problem.
My setting is:#define Point_Num 100 #define New_Point_Num 6 #define Delete_Point_Num 6 #define Nearest_Num 5 #define Test_Time 1000If I understand it right, after 100 test time, the remaining valid num which I checked with
ikd_Tree.validnum()will still be 100.
But it wasn't. No matter I use
ikd_Tree.Add_Points(cloud_increment, false);
or
ikd_Tree.Add_Points(cloud_increment, true);Is this a bug? Or I have misunderstood ikdtree's usage?
Hi!
I have run the demo using your parameter settings. The validnum is always 100 for all 1000 test times. I wonder if you have switched off the Delete_Box_Switch and Add_Box_Switch when you tested on the demo.
I run ikd-tree with a running sliding window of point cloud. pointclouds are queued in time_stamp sequential. each point are added to ikdtree and then delete later. But as I was checking kdtree points with these code:
if(0)
//If you need to see map point, change to "if(1)"
{
PointVector ().swap(ikdtree.PCL_Storage);
ikdtree.flatten(ikdtree.Root_Node, ikdtree.PCL_Storage, NOT_RECORD);
featsFromMap->clear();
featsFromMap->points = ikdtree.PCL_Storage;
}
then publish these point clouds.
I found there are always points not deleted, i.e. they remain in the kdtree even though deleted.Another question, what if i add_Points with downsample_flag=True. Then delete original point later. Will ikdtree delete this cuboid? what behavior will ikdtree generate?
Looks weird. This should not happen. How did you delete those points?
The downsample_flag=true will keep one point in a downsample box which is closest to the center of the downsample box. That means those points far from the center of the downsample box will be deleted by the ikd-Tree. You might refer to our paper for more details about the downsampling.
My bad, I leave box-wise operation remains, so it is the box-wise operation that modifies valid num. After I comment all box-wise operation, It acts as expected.
Actually in my application, I have no box-wise operation but valid_num still not as expected. I guess it must be some bug in my code, I will check on that.
Sorry for the trouble, and thank you for your wonderful work.
After checking with my specific points, I found there are still error, here is my modified demo code.
#define Test_Time 7
int main(int argc, char** argv){
srand((unsigned) time(NULL));
printf("Testing ...\n");
int counter = 0;
bool flag = true;
vector<BoxPointType> Delete_Boxes;
vector<BoxPointType> Add_Boxes;
vector<float> 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.set_downsample_param(0.5);
// ikd_Tree.Build(point_cloud);
auto t2 = chrono::high_resolution_clock::now();
auto build_duration = chrono::duration_cast<chrono::microseconds>(t2-t1).count();
std::deque<PointVector> points_seq;
std::vector<PointVector> test_points_vec;
test_points_vec.emplace_back(PointVector({{-54.708, -2.412, 0.500}, {-54.768, -2.412, 0.500}, {-54.828, -2.412, 0.500}, {-55.185, -2.812, 0.500}, {-55.239, -2.812, 0.500}}));
test_points_vec.emplace_back(PointVector({{-54.708, -2.412, 0.500}, {-54.768, -2.412, 0.500}, {-54.828, -2.412, 0.500}, {-55.185, -2.812, 0.500}, {-55.239, -2.812, 0.500}}));
test_points_vec.emplace_back(PointVector({{-55.339, -2.712, 0.500}, {-55.395, -2.712, 0.500}, {-55.451, -2.712, 0.500}, {-55.580, -2.812, 0.500}, {-55.634, -2.812, 0.500}}));
test_points_vec.emplace_back(PointVector({{-55.696, -2.737, 0.500}, {-55.752, -2.737, 0.500}, {-55.808, -2.737, 0.500}, {-55.937, -2.837, 0.500}, {-55.991, -2.837, 0.500}}));
test_points_vec.emplace_back(PointVector({{-55.682, -2.112, 0.500}, {-55.745, -2.112, 0.500}, {-55.807, -2.112, 0.500}, {-55.869, -2.112, 0.500}, {-55.930, -2.112, 0.500}}));
test_points_vec.emplace_back(PointVector({{-55.916, -2.090, 0.500}, {-55.979, -2.090, 0.500}, {-56.042, -2.090, 0.500}, {-56.103, -2.090, 0.500}, {-56.164, -2.090, 0.500}}));
test_points_vec.emplace_back(PointVector({{-56.248, -2.112, 0.500}, {-56.157, -1.912, 0.500}, {-56.222, -1.912, 0.500}, {-56.285, -1.912, 0.500}, {-56.348, -1.912, 0.500}}));
int add_index = 0;
int remove_index = 0;
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();
int add_point_size = 0;
if(ikd_Tree.Root_Node == nullptr){
ikd_Tree.Build(test_points_vec[add_index++]);
}else {
add_point_size = ikd_Tree.Add_Points(test_points_vec[add_index++], false);
}
t2 = chrono::high_resolution_clock::now();
auto add_duration = chrono::duration_cast<chrono::microseconds>(t2-t1).count();
auto total_duration = add_duration;
points_seq.emplace_front(cloud_increment);
int kdtreePointsFromMapNum = ikd_Tree.validnum();
int kdtree_size_st = ikd_Tree.size();
printf("Add point size %d, time cost is %0.3f ms, add point size %d valid num %d kdtree size %d \n", cloud_increment.size(), float(add_duration)/1e3, add_point_size, kdtreePointsFromMapNum, kdtree_size_st);
// Point-wise Delete
generate_decrement_point_cloud(Delete_Point_Num);
t1 = chrono::high_resolution_clock::now();
if(add_index - remove_index > 3){
ikd_Tree.Delete_Points(test_points_vec[remove_index++]);
}
kdtreePointsFromMapNum = ikd_Tree.validnum();
kdtree_size_st = ikd_Tree.size();
t2 = chrono::high_resolution_clock::now();
auto delete_duration = chrono::duration_cast<chrono::microseconds>(t2-t1).count();
printf("remove point size %d, time cost is %0.3f ms, valid num %d kdtree size %d \n", cloud_increment.size(), float(delete_duration)/1e3, kdtreePointsFromMapNum, kdtree_size_st);
total_duration += delete_duration;
// Box-wise Delete
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;
}
I defined Test_Time to 7 as there are only 7 PointVector.
I maintained a list of PointVectors and add 5 points at a time, and remove the front 5 points once ikdtree volume reaches 3.
My expectation is that valid num will always be 20 after add_points operation and 15 after remove_points operation, but here is the result:
Test 1:
Add point size 200, time cost is 0.005 ms, add point size 0 valid num 5 kdtree size 5
remove point size 200, time cost is 0.000 ms, valid num 5 kdtree size 5
Test 2:
Add point size 200, time cost is 0.002 ms, add point size 0 valid num 10 kdtree size 10
remove point size 200, time cost is 0.000 ms, valid num 10 kdtree size 10
Test 3:
Add point size 200, time cost is 0.011 ms, add point size 0 valid num 15 kdtree size 15
remove point size 200, time cost is 0.000 ms, valid num 15 kdtree size 15
Test 4:
Add point size 200, time cost is 0.013 ms, add point size 0 valid num 20 kdtree size 20
remove point size 200, time cost is 0.001 ms, valid num 15 kdtree size 20
Test 5:
Add point size 200, time cost is 0.019 ms, add point size 0 valid num 20 kdtree size 20
remove point size 200, time cost is 0.001 ms, valid num 15 kdtree size 20
Test 6:
Add point size 200, time cost is 0.018 ms, add point size 0 valid num 20 kdtree size 20
remove point size 200, time cost is 0.001 ms, valid num 16 kdtree size 20
Test 7:
Add point size 200, time cost is 0.017 ms, add point size 0 valid num 21 kdtree size 21
remove point size 200, time cost is 0.001 ms, valid num 18 kdtree size 21
Finished 7 times test
I find that with random generated points, this rarely happen. However with my points, this problem almost happens immediately.
I obtain my points from a laser scan which is a common sensor for robots.
@DingFeng9313
The problem you met is resulted from the precision as your data points have same coordinates for at least one axis. As we have applied the downsample mechanism on our tree, we ignore the cases that data points have close value in any of the axis. The tree fails to find the correct point to delete under such condition.
The problem could be solve but I do not have bandwith for that these days. You might try to avoid such a problem by some small modification on your sensors or data processing.
Get it.
But I see you applied the same ikdtree in your FAST-LIO program. As your robot or quadrotor remains still, the pointclouds collected from sensor will always be at the same position, modify the position of observation point would produce error perceptions. Looks like you use BoxOperation to delete all points, thus avoid such problems. I am about to use BoxOperation to delete all points around robot after a while or the validnum() is too large, then add new observations.
Thanks for you answer
Get it.
But I see you applied the same ikdtree in your FAST-LIO program. As your robot or quadrotor remains still, the pointclouds collected from sensor will always be at the same position, modify the position of observation point would produce error perceptions. Looks like you use BoxOperation to delete all points, thus avoid such problems. I am about to use BoxOperation to delete all points around robot after a while or the validnum() is too large, then add new observations.
Thanks for you answer
Well, the easiest way to avoid this in the FAST-LIO is to switch on the downsample function and set a small resolution. You might refer to our paper about the working principle of the on-tree downsampling, which always saves the point closest to the center of a downsample box.