39 #ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_
40 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
43 #include <pcl/kdtree/kdtree_flann.h>
44 #include <pcl/kdtree/flann.h>
45 #include <pcl/console/print.h>
48 template <
typename Po
intT,
typename Dist>
51 , flann_index_ (), cloud_ (NULL)
52 , index_mapping_ (), identity_mapping_ (false)
53 , dim_ (0), total_nr_points_ (0)
54 , param_k_ (::flann::SearchParams (-1 , epsilon_))
55 , param_radius_ (::flann::SearchParams (-1, epsilon_, sorted))
60 template <
typename Po
intT,
typename Dist>
63 , flann_index_ (), cloud_ (NULL)
64 , index_mapping_ (), identity_mapping_ (false)
65 , dim_ (0), total_nr_points_ (0)
66 , param_k_ (::flann::SearchParams (-1 , epsilon_))
67 , param_radius_ (::flann::SearchParams (-1, epsilon_, false))
73 template <
typename Po
intT,
typename Dist>
void
77 param_k_ = ::flann::SearchParams (-1 , epsilon_);
78 param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
82 template <
typename Po
intT,
typename Dist>
void
86 param_k_ = ::flann::SearchParams (-1, epsilon_);
87 param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
91 template <
typename Po
intT,
typename Dist>
void
97 dim_ = point_representation_->getNumberOfDimensions ();
105 PCL_ERROR (
"[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
110 convertCloudToArray (*input_, *indices_);
114 convertCloudToArray (*input_);
116 total_nr_points_ =
static_cast<int> (index_mapping_.size ());
117 if (total_nr_points_ == 0)
119 PCL_ERROR (
"[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
124 index_mapping_.size (),
126 ::flann::KDTreeSingleIndexParams (15)));
127 flann_index_->buildIndex ();
131 template <
typename Po
intT,
typename Dist>
int
133 std::vector<int> &k_indices,
134 std::vector<float> &k_distances)
const
136 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
138 if (k > total_nr_points_)
139 k = total_nr_points_;
141 k_indices.resize (k);
142 k_distances.resize (k);
144 std::vector<float> query (dim_);
145 point_representation_->vectorize (static_cast<PointT> (point), query);
151 k_indices_mat, k_distances_mat,
155 if (!identity_mapping_)
157 for (
size_t i = 0; i < static_cast<size_t> (k); ++i)
159 int& neighbor_index = k_indices[i];
160 neighbor_index = index_mapping_[neighbor_index];
168 template <
typename Po
intT,
typename Dist>
int
170 std::vector<float> &k_sqr_dists,
unsigned int max_nn)
const
172 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
174 std::vector<float> query (dim_);
175 point_representation_->vectorize (static_cast<PointT> (point), query);
178 if (max_nn == 0 || max_nn > static_cast<unsigned int> (total_nr_points_))
179 max_nn = total_nr_points_;
181 std::vector<std::vector<int> > indices(1);
182 std::vector<std::vector<float> > dists(1);
184 ::flann::SearchParams params (param_radius_);
185 if (max_nn == static_cast<unsigned int>(total_nr_points_))
186 params.max_neighbors = -1;
188 params.max_neighbors = max_nn;
190 int neighbors_in_radius = flann_index_->radiusSearch (::
flann::Matrix<float> (&query[0], 1, dim_),
193 static_cast<float> (radius * radius),
196 k_indices = indices[0];
197 k_sqr_dists = dists[0];
200 if (!identity_mapping_)
202 for (
int i = 0; i < neighbors_in_radius; ++i)
204 int& neighbor_index = k_indices[i];
205 neighbor_index = index_mapping_[neighbor_index];
209 return (neighbors_in_radius);
213 template <
typename Po
intT,
typename Dist>
void
222 index_mapping_.clear ();
229 template <
typename Po
intT,
typename Dist>
void
233 if (cloud.
points.empty ())
239 int original_no_of_points =
static_cast<int> (cloud.
points.size ());
241 cloud_ =
static_cast<float*
> (malloc (original_no_of_points * dim_ *
sizeof (
float)));
242 float* cloud_ptr = cloud_;
243 index_mapping_.reserve (original_no_of_points);
244 identity_mapping_ =
true;
246 for (
int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
249 if (!point_representation_->isValid (cloud.
points[cloud_index]))
251 identity_mapping_ =
false;
255 index_mapping_.push_back (cloud_index);
257 point_representation_->vectorize (cloud.
points[cloud_index], cloud_ptr);
263 template <
typename Po
intT,
typename Dist>
void
267 if (cloud.
points.empty ())
273 int original_no_of_points =
static_cast<int> (indices.size ());
275 cloud_ =
static_cast<float*
> (malloc (original_no_of_points * dim_ *
sizeof (
float)));
276 float* cloud_ptr = cloud_;
277 index_mapping_.reserve (original_no_of_points);
285 identity_mapping_ =
false;
287 for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt)
290 if (!point_representation_->isValid (cloud.
points[*iIt]))
294 index_mapping_.push_back (*iIt);
296 point_representation_->vectorize (cloud.
points[*iIt], cloud_ptr);
301 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
303 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr())
Provide a pointer to the input dataset.
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const
Search for all the nearest neighbors of the query point in a given radius.
KdTreeFLANN(bool sorted=true)
Default Constructor for KdTreeFLANN.
void setSortedResults(bool sorted)
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const
Search for k-nearest neighbors for the given query point.
boost::shared_ptr< const std::vector< int > > IndicesConstPtr
void setEpsilon(float eps)
Set the search epsilon precision (error bound) for nearest neighbors searches.
KdTree represents the base spatial locator class for kd-tree implementations.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
boost::shared_ptr< const PointCloud > PointCloudConstPtr
A point structure representing Euclidean xyz coordinates, and the RGB color.