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);
}
