39 #ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_
40 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
42 #include <flann/flann.hpp>
44 #include <pcl/kdtree/kdtree_flann.h>
45 #include <pcl/console/print.h>
48 template <
typename Po
intT,
typename Dist>
52 , 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>
64 , 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");
108 if (indices !=
nullptr)
110 convertCloudToArray (*input_, *indices_);
114 convertCloudToArray (*input_);
116 total_nr_points_ =
static_cast<uindex_t> (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);
147 std::vector<float> query (dim_);
148 point_representation_->vectorize (
static_cast<PointT> (point), query);
154 k_indices_mat, k_distances_mat,
158 if (!identity_mapping_)
160 for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
162 int& neighbor_index = k_indices[i];
163 neighbor_index = index_mapping_[neighbor_index];
171 template <
typename Po
intT,
typename Dist>
int
173 std::vector<float> &k_sqr_dists,
unsigned int max_nn)
const
175 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
177 std::vector<float> query (dim_);
178 point_representation_->vectorize (
static_cast<PointT> (point), query);
181 if (max_nn == 0 || max_nn > total_nr_points_)
182 max_nn = total_nr_points_;
184 std::vector<std::vector<int> > indices(1);
185 std::vector<std::vector<float> > dists(1);
187 ::flann::SearchParams params (param_radius_);
188 if (max_nn == total_nr_points_)
189 params.max_neighbors = -1;
191 params.max_neighbors = max_nn;
193 int neighbors_in_radius = flann_index_->radiusSearch (::
flann::Matrix<float> (&query[0], 1, dim_),
196 static_cast<float> (radius * radius),
199 k_indices = indices[0];
200 k_sqr_dists = dists[0];
203 if (!identity_mapping_)
205 for (
int i = 0; i < neighbors_in_radius; ++i)
207 int& neighbor_index = k_indices[i];
208 neighbor_index = index_mapping_[neighbor_index];
212 return (neighbors_in_radius);
216 template <
typename Po
intT,
typename Dist>
void
220 index_mapping_.clear ();
227 template <
typename Po
intT,
typename Dist>
void
237 const auto original_no_of_points = cloud.
size ();
239 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
240 float* cloud_ptr = cloud_.get ();
241 index_mapping_.reserve (original_no_of_points);
242 identity_mapping_ =
true;
244 for (std::size_t cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
247 if (!point_representation_->isValid (cloud[cloud_index]))
249 identity_mapping_ =
false;
253 index_mapping_.push_back (cloud_index);
255 point_representation_->vectorize (cloud[cloud_index], cloud_ptr);
261 template <
typename Po
intT,
typename Dist>
void
271 int original_no_of_points =
static_cast<int> (indices.size ());
273 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
274 float* cloud_ptr = cloud_.get ();
275 index_mapping_.reserve (original_no_of_points);
283 identity_mapping_ =
false;
285 for (
const auto &index : indices)
288 if (!point_representation_->isValid (cloud[index]))
292 index_mapping_.push_back (index);
294 point_representation_->vectorize (cloud[index], cloud_ptr);
299 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
301 #endif //#ifndef _PCL_KDTREE_KDTREE_IMPL_FLANN_H_