Exemple #1
0
  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);
  }
Exemple #2
0
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);
}
Exemple #3
0
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);
}
Exemple #4
0
  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);
  }
Exemple #5
0
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;
}
Exemple #6
0
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;
}
Exemple #7
0
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);
}