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>
48 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr
51 return (IndexPtr (
new flann::KDTreeSingleIndex<FlannDistance> (*data,flann::KDTreeSingleIndexParams (max_leaf_size_))));
55 template <
typename Po
intT,
typename FlannDistance>
56 typename pcl::search::FlannSearch<PointT, FlannDistance>::IndexPtr
59 return (IndexPtr (
new flann::KMeansIndex<FlannDistance> (*data,flann::KMeansIndexParams ())));
63 template <
typename Po
intT,
typename FlannDistance>
72 template <
typename Po
intT,
typename FlannDistance>
75 if (input_copied_for_flann_)
76 delete [] input_flann_->ptr();
80 template <
typename Po
intT,
typename FlannDistance>
void
85 convertInputToFlannMatrix ();
86 index_ = creator_->createIndex (input_flann_);
87 index_->buildIndex ();
91 template <
typename Po
intT,
typename FlannDistance>
int
94 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
95 bool can_cast = point_representation_->isTrivial ();
100 data =
new float [point_representation_->getNumberOfDimensions ()];
101 point_representation_->vectorize (point,data);
104 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)): data;
107 flann::SearchParams p(-1);
109 p.sorted = sorted_results_;
110 if (indices.size() !=
static_cast<unsigned int> (k))
111 indices.resize (k,-1);
112 if (dists.size() !=
static_cast<unsigned int> (k))
116 int result = index_->knnSearch (m,i,d,k, p);
120 if (!identity_mapping_)
122 for (
size_t i = 0; i < static_cast<unsigned int> (k); ++i)
124 int& neighbor_index = indices[i];
125 neighbor_index = index_mapping_[neighbor_index];
132 template <
typename Po
intT,
typename FlannDistance>
void
134 const PointCloud& cloud,
const std::vector<int>& indices,
int k, std::vector< std::vector<int> >& k_indices,
135 std::vector< std::vector<float> >& k_sqr_distances)
const
137 if (indices.empty ())
139 k_indices.resize (cloud.
size ());
140 k_sqr_distances.resize (cloud.
size ());
144 for (
size_t i = 0; i < cloud.
size(); i++)
146 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
150 bool can_cast = point_representation_->isTrivial ();
156 data =
new float[dim_*cloud.
size ()];
157 for (
size_t i = 0; i < cloud.
size (); ++i)
159 float* out = data+i*dim_;
160 point_representation_->vectorize (cloud[i],out);
166 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])): data;
169 flann::SearchParams p;
170 p.sorted = sorted_results_;
172 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
178 k_indices.resize (indices.size ());
179 k_sqr_distances.resize (indices.size ());
183 for (
size_t i = 0; i < indices.size(); i++)
185 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
189 float* data=
new float [dim_*indices.size ()];
190 for (
size_t i = 0; i < indices.size (); ++i)
192 float* out = data+i*dim_;
193 point_representation_->vectorize (cloud[indices[i]],out);
195 const flann::Matrix<float> m (data ,indices.size (), point_representation_->getNumberOfDimensions ());
197 flann::SearchParams p;
198 p.sorted = sorted_results_;
200 index_->knnSearch (m,k_indices,k_sqr_distances,k, p);
204 if (!identity_mapping_)
206 for (
size_t j = 0; j < k_indices.size (); ++j)
208 for (
size_t i = 0; i < static_cast<unsigned int> (k); ++i)
210 int& neighbor_index = k_indices[j][i];
211 neighbor_index = index_mapping_[neighbor_index];
218 template <
typename Po
intT,
typename FlannDistance>
int
220 std::vector<int> &indices, std::vector<float> &distances,
221 unsigned int max_nn)
const
223 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
224 bool can_cast = point_representation_->isTrivial ();
229 data =
new float [point_representation_->getNumberOfDimensions ()];
230 point_representation_->vectorize (point,data);
233 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&point)) : data;
236 flann::SearchParams p;
237 p.sorted = sorted_results_;
239 p.max_neighbors = max_nn > 0 ? max_nn : -1;
240 std::vector<std::vector<int> > i (1);
241 std::vector<std::vector<float> > d (1);
242 int result = index_->radiusSearch (m,i,d,static_cast<float> (radius * radius), p);
248 if (!identity_mapping_)
250 for (
size_t i = 0; i < indices.size (); ++i)
252 int& neighbor_index = indices [i];
253 neighbor_index = index_mapping_ [neighbor_index];
260 template <
typename Po
intT,
typename FlannDistance>
void
262 const PointCloud& cloud,
const std::vector<int>& indices,
double radius, std::vector< std::vector<int> >& k_indices,
263 std::vector< std::vector<float> >& k_sqr_distances,
unsigned int max_nn)
const
265 if (indices.empty ())
267 k_indices.resize (cloud.
size ());
268 k_sqr_distances.resize (cloud.
size ());
272 for (
size_t i = 0; i < cloud.
size(); i++)
274 assert (point_representation_->isValid (cloud[i]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
278 bool can_cast = point_representation_->isTrivial ();
283 data =
new float[dim_*cloud.
size ()];
284 for (
size_t i = 0; i < cloud.
size (); ++i)
286 float* out = data+i*dim_;
287 point_representation_->vectorize (cloud[i],out);
291 float* cdata = can_cast ?
const_cast<float*
> (
reinterpret_cast<const float*
> (&cloud[0])) : data;
294 flann::SearchParams p;
295 p.sorted = sorted_results_;
298 p.max_neighbors = max_nn != 0 ? max_nn : -1;
299 index_->radiusSearch (m,k_indices,k_sqr_distances,static_cast<float> (radius * radius), p);
305 k_indices.resize (indices.size ());
306 k_sqr_distances.resize (indices.size ());
310 for (
size_t i = 0; i < indices.size(); i++)
312 assert (point_representation_->isValid (cloud [indices[i]]) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
316 float* data =
new float [dim_ * indices.size ()];
317 for (
size_t i = 0; i < indices.size (); ++i)
319 float* out = data+i*dim_;
320 point_representation_->vectorize (cloud[indices[i]], out);
324 flann::SearchParams p;
325 p.sorted = sorted_results_;
328 p.max_neighbors = max_nn != 0 ? max_nn : -1;
329 index_->radiusSearch (m, k_indices, k_sqr_distances, static_cast<float> (radius * radius), p);
333 if (!identity_mapping_)
335 for (
size_t j = 0; j < k_indices.size (); ++j )
337 for (
size_t i = 0; i < k_indices[j].size (); ++i)
339 int& neighbor_index = k_indices[j][i];
340 neighbor_index = index_mapping_[neighbor_index];
347 template <
typename Po
intT,
typename FlannDistance>
void
350 size_t original_no_of_points = indices_ && !indices_->empty () ? indices_->size () : input_->size ();
352 if (input_copied_for_flann_)
353 delete input_flann_->ptr();
354 input_copied_for_flann_ =
true;
355 index_mapping_.clear();
356 identity_mapping_ =
true;
362 if (!indices_ || indices_->empty ())
365 if (input_->is_dense && point_representation_->isTrivial ())
368 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)));
369 input_copied_for_flann_ =
false;
373 input_flann_ = MatrixPtr (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
374 float* cloud_ptr = input_flann_->ptr();
375 for (
size_t i = 0; i < original_no_of_points; ++i)
377 const PointT& point = (*input_)[i];
379 if (!point_representation_->isValid (point))
381 identity_mapping_ =
false;
385 index_mapping_.push_back (static_cast<int> (i));
387 point_representation_->vectorize (point, cloud_ptr);
395 input_flann_ = MatrixPtr (
new flann::Matrix<float> (
new float[original_no_of_points*point_representation_->getNumberOfDimensions ()], original_no_of_points, point_representation_->getNumberOfDimensions ()));
396 float* cloud_ptr = input_flann_->ptr();
397 for (
size_t indices_index = 0; indices_index < original_no_of_points; ++indices_index)
399 int cloud_index = (*indices_)[indices_index];
400 const PointT& point = (*input_)[cloud_index];
402 if (!point_representation_->isValid (point))
404 identity_mapping_ =
false;
408 index_mapping_.push_back (static_cast<int> (indices_index));
410 point_representation_->vectorize (point, cloud_ptr);
414 if (input_copied_for_flann_)
415 input_flann_->rows = index_mapping_.size ();
418 #define PCL_INSTANTIATE_FlannSearch(T) template class PCL_EXPORTS pcl::search::FlannSearch<T>;