Non consistent Output of Neighbor Search

[wienans]

Hi thanks for the great work. I encountered a potential bug within the lib. I want to filter my point cloud based on how many Neighbors a point has. For this i firstly used the PCL kdTreeFLANN implementation (see if flann_lib_ == 0). In comparison i wanted to try your Implementation (see if flann_lib_==1). To bring first things out of the way, i think i am right with squaring the radius in your library in comparison to the pcl implementation?

if(flann_lib_ == 0){
    // init. kd search tree
    KdTreePtr kd_tree_(new pcl::KdTreeFLANN<pcl::PointXYZI>());
    kd_tree_->setInputCloud(input_cloud);
    // Go over all the points and check which doesn't have enough neighbors
    // perform filtering
    for (pcl::PointCloud<pcl::PointXYZI>::iterator it = input_cloud->begin();
        it != input_cloud->end(); ++it) {
      float x_i = it->x;
      float y_i = it->y;
      double intes = it->intensity;
      float range_i = sqrt(pow(x_i, 2) + pow(y_i, 2));
      float search_radius_dynamic =
          radius_multiplier_ * azimuth_angle_ * 3.14159265359 / 180 * range_i;
          
      if (search_radius_dynamic < min_search_radius_) {
        search_radius_dynamic = min_search_radius_;
      }

      std::vector<int> pointIdxRadiusSearch;
      std::vector<float> pointRadiusSquaredDistance;
      if (intensity_ && intes > th_intensity_){
        filtered_cloud.push_back(*it);
      }else{
        int neighbors =
          kd_tree_->radiusSearch(*it, search_radius_dynamic, pointIdxRadiusSearch,
                                pointRadiusSquaredDistance,min_neighbors_);
        if (neighbors >= min_neighbors_) { filtered_cloud.push_back(*it);}
      }
    }
  }
  else if(flann_lib_ == 1) {
    // init. kd search tree
    nanoflann::KdTreeFLANN<pcl::PointXYZI> kd_tree_flann_;
    kd_tree_flann_.setInputCloud(input_cloud);

    for (pcl::PointCloud<pcl::PointXYZI>::iterator it = input_cloud->begin();
        it != input_cloud->end(); ++it) {
      float x_i = it->x;
      float y_i = it->y;
      double intes = it->intensity;
      float range_i = sqrt(pow(x_i, 2) + pow(y_i, 2));
      float search_radius_dynamic =
          radius_multiplier_ * azimuth_angle_ * 3.14159265359 / 180 * range_i;
          
      if (search_radius_dynamic < min_search_radius_) {
        search_radius_dynamic = min_search_radius_;
      }

      std::vector<int> pointIdxRadiusSearch;
      std::vector<float> pointRadiusSquaredDistance;
      if (intensity_ && intes > th_intensity_){
        filtered_cloud.push_back(*it);
      }else{
        int neighbors =
          kd_tree_flann_.radiusSearch(*it, pow(search_radius_dynamic,2), pointIdxRadiusSearch,
                                pointRadiusSquaredDistance);
        if (neighbors >= min_neighbors_) { filtered_cloud.push_back(*it);}
      }
    }
  }

For both implementations i made a short video of a visualization of the output point cloud via Rviz. As you might noticed i have an intesity filter which overwrites the the radius Search. Thats why i colored the pointcloud based on the intensity. Yellow points are put through with the intensity filter and red points are the points which have not enough intensity and are filtered by the radius search. PCL Implementation shows a consistent point cloud output https://www.youtube.com/watch?v=a8EuP8oOWDg

With the NanoFlann implementation the red points are "flickering" in some point cloud outputs there are included in some not. And within this flicker it is also not consistent and there are some strange "sharp" edges which look as a dimension filter would be applied. The fuction returns 0 as a result for found Neighbors in the flicker cases. https://youtu.be/AOaBHOvWsnI

Actually i don't have an idea what could cause this behavior maybe you have an idea. First i saw this on the commit from April updated the code to the current commit and in both cases this happens.

Greets Sven

[jlblancoc]

I know this comes late.... but my intuition tells me that the flicker may come from the usage of the shared_ptr: nanoflann does not make a copy of the data as FLANN does, so if you are updating the data from a live sensor, and not keeping an internal reference to the shared_ptr within your custom kdTreeFLANN class, searches may be browsing already-deallocated memory. Try to debug with valgrind to verify this, or just keep a copy of the shared_ptr inside your nanoflann adaptor class.

BTW: I found a couple of attempts to integrate nanoflann within PCL but they were not merged... (!!)