40 #ifndef PCL_SEARCH_IMPL_FLANN_SEARCH_H_
41 #define PCL_SEARCH_IMPL_FLANN_SEARCH_H_
43 #include <pcl/search/flann_search.h>
44 #include <pcl/kdtree/flann.h>
47 template <
typename Po
intT,
typename FlannDistance>
51 return (
IndexPtr (
new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
55 template <
typename Po
intT,
typename FlannDistance>
59 return (
IndexPtr (
new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
63 template <
typename Po
intT,
typename FlannDistance>
67 return (
IndexPtr (
new flann::KDTreeIndex<FlannDistance> (*data, flann::KDTreeIndexParams (trees_))));
71 template <
typename Po
intT,
typename FlannDistance>
80 template <
typename Po
intT,
typename FlannDistance>
83 if (input_copied_for_flann_)
84 delete [] input_flann_->ptr();
88 template <
typename Po
intT,
typename FlannDistance>
void
93 convertInputToFlannMatrix ();
94 index_ = creator_->createIndex (input_flann_);
95 index_->buildIndex ();
99 template <
typename Po
intT,
typename FlannDistance>
int
102 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
103 bool can_cast = point_representation_->isTrivial ();
108 data =
new float [point_representation_->getNumberOfDimensions ()];
109 point_representation_->vectorize (point,data);
112 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)): data;
115 flann::SearchParams p;
117 p.sorted = sorted_results_;
119 if (indices.size() !=
static_cast<unsigned int> (k))
120 indices.resize (k,-1);
121 if (dists.size() !=
static_cast<unsigned int> (k))
125 int result = index_->knnSearch (m,i,d,k, p);
129 if (!identity_mapping_)
131 for (
size_t i = 0; i < static_cast<unsigned int> (k); ++i)
133 int& neighbor_index = indices[i];
134 neighbor_index = index_mapping_[neighbor_index];
141 template <
typename Po
intT,
typename FlannDistance>
void
143 const PointCloud& cloud,
const std::vector<int>& indices,
int k, std::vector< std::vector<int> >& k_indices,
144 std::vector< std::vector<float> >& k_sqr_distances)
const
146 if (indices.empty ())
148 k_indices.resize (cloud.
size ());
149 k_sqr_distances.resize (cloud.
size ());
153 for (
size_t i = 0; i < cloud.
size(); i++)
155 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
159 bool can_cast = point_representation_->isTrivial ();
165 data =
new float[dim_*cloud.
size ()];
166 for (
size_t i = 0; i < cloud.
size (); ++i)
168 float* out = data+i*dim_;
169 point_representation_->vectorize (cloud[i],out);
175 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])): data;
178 flann::SearchParams p;
179 p.sorted = sorted_results_;
182 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
188 k_indices.resize (indices.size ());
189 k_sqr_distances.resize (indices.size ());
193 for (
size_t i = 0; i < indices.size(); i++)
195 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
199 float* data=
new float [dim_*indices.size ()];
200 for (
size_t i = 0; i < indices.size (); ++i)
202 float* out = data+i*dim_;
203 point_representation_->vectorize (cloud[indices[i]],out);
205 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
207 flann::SearchParams p;
208 p.sorted = sorted_results_;
211 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
215 if (!identity_mapping_)
217 for (
size_t j = 0; j < k_indices.size (); ++j)
219 for (
size_t i = 0; i < static_cast<unsigned int> (k); ++i)
221 int& neighbor_index = k_indices[j][i];
222 neighbor_index = index_mapping_[neighbor_index];
229 template <
typename Po
intT,
typename FlannDistance>
int
231 std::vector<int> &indices, std::vector<float> &distances,
232 unsigned int max_nn)
const
234 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
235 bool can_cast = point_representation_->isTrivial ();
240 data =
new float [point_representation_->getNumberOfDimensions ()];
241 point_representation_->vectorize (point,data);
244 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)) : data;
247 flann::SearchParams p;
248 p.sorted = sorted_results_;
250 p.max_neighbors = max_nn > 0 ? max_nn : -1;
252 std::vector<std::vector<int> > i (1);
253 std::vector<std::vector<float> > d (1);
254 int result = index_->radiusSearch (m,i,d,
static_cast<float> (radius * radius), p);
260 if (!identity_mapping_)
262 for (
size_t i = 0; i < indices.size (); ++i)
264 int& neighbor_index = indices [i];
265 neighbor_index = index_mapping_ [neighbor_index];
272 template <
typename Po
intT,
typename FlannDistance>
void
274 const PointCloud& cloud,
const std::vector<int>& indices,
double radius, std::vector< std::vector<int> >& k_indices,
275 std::vector< std::vector<float> >& k_sqr_distances,
unsigned int max_nn)
const
277 if (indices.empty ())
279 k_indices.resize (cloud.
size ());
280 k_sqr_distances.resize (cloud.
size ());
284 for (
size_t i = 0; i < cloud.
size(); i++)
286 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
290 bool can_cast = point_representation_->isTrivial ();
295 data =
new float[dim_*cloud.
size ()];
296 for (
size_t i = 0; i < cloud.
size (); ++i)
298 float* out = data+i*dim_;
299 point_representation_->vectorize (cloud[i],out);
303 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])) : data;
306 flann::SearchParams p;
307 p.sorted = sorted_results_;
311 p.max_neighbors = max_nn != 0 ? max_nn : -1;
312 index_->radiusSearch (m,k_indices,k_sqr_distances,
static_cast<float> (radius * radius), p);
318 k_indices.resize (indices.size ());
319 k_sqr_distances.resize (indices.size ());
323 for (
size_t i = 0; i < indices.size(); i++)
325 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
329 float* data =
new float [dim_ * indices.size ()];
330 for (
size_t i = 0; i < indices.size (); ++i)
332 float* out = data+i*dim_;
333 point_representation_->vectorize (cloud[indices[i]], out);
337 flann::SearchParams p;
338 p.sorted = sorted_results_;
342 p.max_neighbors = max_nn != 0 ? max_nn : -1;
343 index_->radiusSearch (m, k_indices, k_sqr_distances,
static_cast<float> (radius * radius), p);
347 if (!identity_mapping_)
349 for (
size_t j = 0; j < k_indices.size (); ++j )
351 for (
size_t i = 0; i < k_indices[j].size (); ++i)
353 int& neighbor_index = k_indices[j][i];
354 neighbor_index = index_mapping_[neighbor_index];
361 template <
typename Po
intT,
typename FlannDistance>
void
364 size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
366 if (input_copied_for_flann_)
367 delete input_flann_->ptr();
368 input_copied_for_flann_ =
true;
369 index_mapping_.clear();
370 identity_mapping_ =
true;
376 if (!indices_ || indices_->empty ())
379 if (input_->is_dense && point_representation_->isTrivial ())
382 input_flann_ =
MatrixPtr (
new flann::Matrix<float> (
const_cast<float*
>(
reinterpret_cast<const float*
>(&(*input_) [0])), original_no_of_points, point_representation_->getNumberOfDimensions (),sizeof (
PointT)));
383 input_copied_for_flann_ =
false;
387 input_flann_ =
MatrixPtr (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
388 float* cloud_ptr = input_flann_->ptr();
389 for (
size_t i = 0; i < original_no_of_points; ++i)
391 const PointT& point = (*input_)[i];
393 if (!point_representation_->isValid (point))
395 identity_mapping_ =
false;
399 index_mapping_.push_back (
static_cast<int> (i));
401 point_representation_->vectorize (point, cloud_ptr);
409 input_flann_ =
MatrixPtr (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
410 float* cloud_ptr = input_flann_->ptr();
411 for (
size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
413 int cloud_index = (*indices_)[indices_index];
414 const PointT& point = (*input_)[cloud_index];
416 if (!point_representation_->isValid (point))
418 identity_mapping_ =
false;
422 index_mapping_.push_back (
static_cast<int> (indices_index));
424 point_representation_->vectorize (point, cloud_ptr);
428 if (input_copied_for_flann_)
429 input_flann_->rows = index_mapping_.size ();
432 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;