int KdTreeFLANN<PointT>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices, std::vector<float> &k_squared_distances, int max_nn) const { static flann::Matrix<int> indices_empty; static flann::Matrix<float> dists_empty; if (!point_representation_->isValid (point)) return 0; std::vector<float> tmp(dim_); point_representation_->vectorize ((PointT)point, tmp); radius *= radius; // flann uses squared radius size_t size; if (indices_ == NULL) // no indices set, use full size of point cloud: size = input_->points.size (); else size = indices_->size (); int neighbors_in_radius = 0; if (k_indices.size () == size && k_squared_distances.size () == size) // preallocated vectors { flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size()); flann::Matrix<float> k_distances_mat (&k_squared_distances[0], 1, k_squared_distances.size()); neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_), k_indices_mat, k_distances_mat, radius, flann::SearchParams (-1, epsilon_, sorted_)); } else // need to do search twice, first to find how many neighbors and allocate the vectors { neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_), indices_empty, dists_empty, radius, flann::SearchParams (-1, epsilon_, sorted_)); k_indices.resize (neighbors_in_radius); k_squared_distances.resize (neighbors_in_radius); if (neighbors_in_radius == 0) { return (0); } flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size()); flann::Matrix<float> k_distances_mat (&k_squared_distances[0], 1, k_squared_distances.size()); flann_index_->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim_), k_indices_mat, k_distances_mat, radius, flann::SearchParams (-1, epsilon_, sorted_)); } // Do mapping to original point cloud for (int i = 0; i < neighbors_in_radius; ++i) { int& neighbor_index = k_indices[i]; neighbor_index = index_mapping_[neighbor_index]; } return (neighbors_in_radius); }
template <typename PointT, typename Dist> int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const { assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); if (k > total_nr_points_) k = total_nr_points_; k_indices.resize (k); k_distances.resize (k); std::vector<float> query (dim_); point_representation_->vectorize (static_cast<PointT> (point), query); ::flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k); ::flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k); // Wrap the k_indices and k_distances vectors (no data copy) flann_index_->knnSearch (::flann::Matrix<float> (&query[0], 1, dim_), k_indices_mat, k_distances_mat, k, param_k_); // Do mapping to original point cloud if (!identity_mapping_) { for (size_t i = 0; i < static_cast<size_t> (k); ++i) { int& neighbor_index = k_indices[i]; neighbor_index = index_mapping_[neighbor_index]; } } return (k); }
template <typename PointT, typename Dist> int pcl::KdTreeFLANN<PointT, Dist>::nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const { assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to nearestKSearch!"); if (k > total_nr_points_) { PCL_ERROR ("[pcl::KdTreeFLANN::nearestKSearch] An invalid number of nearest neighbors was requested! (k = %d out of %d total points).\n", k, total_nr_points_); k = total_nr_points_; } k_indices.resize (k); k_distances.resize (k); std::vector<float> query (dim_); point_representation_->vectorize ((PointT)point, query); flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k); flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k); // Wrap the k_indices and k_distances vectors (no data copy) flann_index_->knnSearch (flann::Matrix<float> (&query[0], 1, dim_), k_indices_mat, k_distances_mat, k, param_k_); // Do mapping to original point cloud if (!identity_mapping_) { for (size_t i = 0; i < (size_t)k; ++i) { int& neighbor_index = k_indices[i]; neighbor_index = index_mapping_[neighbor_index]; } } return (k); }
template <typename PointT> int KdTreeFLANN<PointT>::nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) { if (!point_representation_->isValid (point)) return (0); std::vector<float> tmp (dim_); point_representation_->vectorize ((PointT)point, tmp); flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k); flann::Matrix<float> k_distances_mat (&k_distances[0], 1, k); flann_index_->knnSearch (flann::Matrix<float>(&tmp[0], 1, dim_), k_indices_mat, k_distances_mat, k, flann::SearchParams (-1 ,epsilon_)); // Do mapping to original point cloud for (size_t i = 0; i < k_indices.size (); ++i) { int& neighbor_index = k_indices[i]; neighbor_index = index_mapping_[neighbor_index]; } return (k); }
std::vector<boost::uint32_t> Index::query(double const& x, double const& y, double const& z, double distance, boost::uint32_t k) { std::vector<boost::uint32_t> output; #ifdef PDAL_HAVE_FLANN std::vector<float> distances; distances.resize(k); std::vector<int> indices; indices.resize(k); std::vector<float> query(m_stage.getDimensions()); query[0] = x; query[1] = y; if (m_stage.getDimensions() > 2) query[2] = z; bool logOutput = m_stage.log()->getLevel() > logDEBUG4; m_stage.log()->floatPrecision(8); if (logOutput) m_stage.log()->get(logDEBUG2) << "Searching for x: " << x << " y: " << y << " z: " << z << " with distance threshold " << distance << std::endl; flann::Matrix<int> k_indices_mat (&indices[0], 1, k); flann::Matrix<float> k_distances_mat (&distances[0], 1, k); m_index->knnSearch (flann::Matrix<float> (&query[0], 1, m_stage.getDimensions()), k_indices_mat, k_distances_mat, k, flann::SearchParams(128)); for(unsigned i=0; i < k; ++i) { // if distance is 0, just return the nearest one, otherwise filter by distance if (Utils::compare_distance<float>((float)distance, 0)) { if (logOutput) m_stage.log()->get(logDEBUG4) << "Query found: " << "index: " << indices[i] << " distance: " << distances[i] <<std::endl; output.push_back(indices[i]); } else { if (::sqrt(distances[i]) < distance) { if (logOutput) m_stage.log()->get(logDEBUG4) << "Query found: " << "index: " << indices[i] << " distance: " << distances[i] <<std::endl; output.push_back(indices[i]); } } } #else boost::ignore_unused_variable_warning(x); boost::ignore_unused_variable_warning(y); boost::ignore_unused_variable_warning(z); boost::ignore_unused_variable_warning(distance); boost::ignore_unused_variable_warning(k); #endif return output; }
int main (int argc, char *argv[]) { std::string infilename = argv[1]; //load the cloud // sensor_msgs::PointCloud2::Ptr sensor_d (new sensor_msgs::PointCloud2); pcl::PointCloud<PointD>::Ptr distances (new pcl::PointCloud<PointD>); pcl::io::loadPCDFile(infilename.c_str(), *distances); pcl::KdTreeFLANN<PointD> kdtree; kdtree.setInputCloud (distances); PointD searchPoint; searchPoint.distance = 1; //now try a search std::vector<int> pointIdxRadiusSearch; std::vector<float> pointRadiusSquaredDistance; float radius = 10; std::cout << "Neighbors within radius search at (" << searchPoint.distance << ") with radius=" << radius << std::endl; kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance); pcl::console::print_info("We got %i neighbors\n", pointIdxRadiusSearch.size()); for (int i =0; i< 10; ++i) { std::cout << pointIdxRadiusSearch[i] << "\t " << sqrt(pointRadiusSquaredDistance[i]) << std::endl; } //TRY WITH JUST FLANN IF YOU CAN! //write distances to a vector long int n_points = distances->points.size(); long int dim = 1; //dimensions of dataset float v_dist[n_points]; for (int i=0; i < n_points; ++i) { v_dist[i] =distances->points[i].distance; } //initialize the flann index float epsilon = 0.0; //error in retrieving points typedef flann::L2_Simple<float> dist; typedef flann::Index<dist> FLANNIndex; FLANNIndex* flann_index = new FLANNIndex (flann::Matrix<float> (v_dist, n_points, dim), flann::KDTreeSingleIndexParams (15)); flann_index->buildIndex(); float pos = searchPoint.distance; //radius was yet declared radius *= radius; //flann need squared radius static flann::Matrix<int> indices_empty; static flann::Matrix<float> dists_empty; int neighbors_in_radius = 0; bool sorted = true; flann::SearchParams param_radius = flann::SearchParams (-1 ,epsilon, sorted); std::vector<float> tmp (dim); tmp[0] = pos; //first search for allocate dimensions neighbors_in_radius = flann_index->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim), indices_empty, dists_empty, radius, param_radius); std::vector<int> k_indices; std::vector<float> k_squared_distances; k_indices.resize (neighbors_in_radius); k_squared_distances.resize (neighbors_in_radius); std::cout << "with this method:" << neighbors_in_radius << std::endl; //now perform actual search //declare flann::Matrix of right size // flann::Matrix<int> k_indices_mat (new int[k_indices.size()*1], 1, k_indices.size ()); // flann::Matrix<float> k_distances_mat (new float[k_indices.size()*1], 1, k_indices.size ()); flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size ()); flann::Matrix<float> k_distances_mat (&k_squared_distances[0], 1, k_squared_distances.size ()); //and do search int new_number = flann_index->radiusSearch (flann::Matrix<float>(&tmp[0], 1, dim), k_indices_mat, k_distances_mat, radius, param_radius); for (int i =0; i< 10; ++i) { std::cout << k_indices[i] << "\t " << sqrt(k_squared_distances[i]) << std::endl; } std::cout << new_number << std::endl; //NOW RECREATE USING FLANN return 1; }
template <typename PointT, typename Dist> int pcl::KdTreeFLANN<PointT, Dist>::radiusSearch (const PointT &point, double radius, std::vector<int> &k_indices, std::vector<float> &k_sqr_dists, unsigned int max_nn) const { assert (point_representation_->isValid (point) && "Invalid (NaN, Inf) point coordinates given to radiusSearch!"); std::vector<float> query (dim_); point_representation_->vectorize ((PointT)point, query); int neighbors_in_radius = 0; // If the user pre-allocated the arrays, we are only going to if (k_indices.size () == (size_t)total_nr_points_ && k_sqr_dists.size () == (size_t)total_nr_points_) { flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size ()); flann::Matrix<float> k_distances_mat (&k_sqr_dists[0], 1, k_sqr_dists.size ()); neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float> (&query[0], 1, dim_), k_indices_mat, k_distances_mat, (float) (radius * radius), param_radius_); } else { // Has max_nn been set properly? if (max_nn == 0 || max_nn > (unsigned int)total_nr_points_) max_nn = total_nr_points_; static flann::Matrix<int> indices_empty; static flann::Matrix<float> dists_empty; neighbors_in_radius = flann_index_->radiusSearch (flann::Matrix<float> (&query[0], 1, dim_), indices_empty, dists_empty, (float) (radius * radius), param_radius_); neighbors_in_radius = std::min ((unsigned int)neighbors_in_radius, max_nn); k_indices.resize (neighbors_in_radius); k_sqr_dists.resize (neighbors_in_radius); if(neighbors_in_radius != 0) { flann::Matrix<int> k_indices_mat (&k_indices[0], 1, k_indices.size ()); flann::Matrix<float> k_distances_mat (&k_sqr_dists[0], 1, k_sqr_dists.size ()); flann_index_->radiusSearch (flann::Matrix<float> (&query[0], 1, dim_), k_indices_mat, k_distances_mat, (float) (radius * radius), param_radius_); } } // Do mapping to original point cloud if (!identity_mapping_) { for (int i = 0; i < neighbors_in_radius; ++i) { int& neighbor_index = k_indices[i]; neighbor_index = index_mapping_[neighbor_index]; } } return (neighbors_in_radius); }