diff --git a/segmentation/include/pcl/segmentation/impl/region_growing.hpp b/segmentation/include/pcl/segmentation/impl/region_growing.hpp index 0c74e466c81..a682fe85c0d 100644 --- a/segmentation/include/pcl/segmentation/impl/region_growing.hpp +++ b/segmentation/include/pcl/segmentation/impl/region_growing.hpp @@ -358,13 +358,27 @@ pcl::RegionGrowing::findPointNeighbours () std::vector distances; point_neighbours_.resize (input_->points.size (), neighbours); - - for (int i_point = 0; i_point < point_number; i_point++) + if (input_->is_dense) { - int point_index = (*indices_)[i_point]; - neighbours.clear (); - search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); - point_neighbours_[point_index].swap (neighbours); + for (int i_point = 0; i_point < point_number; i_point++) + { + int point_index = (*indices_)[i_point]; + neighbours.clear (); + search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); + point_neighbours_[point_index].swap (neighbours); + } + } + else + { + for (int i_point = 0; i_point < point_number; i_point++) + { + neighbours.clear (); + int point_index = (*indices_)[i_point]; + if (!pcl::isFinite (input_->points[point_index])) + continue; + search_->nearestKSearch (i_point, neighbour_number_, neighbours, distances); + point_neighbours_[point_index].swap (neighbours); + } } }