39 #ifndef PCL_KDTREE_KDTREE_IMPL_FLANN_H_
40 #define PCL_KDTREE_KDTREE_IMPL_FLANN_H_
44 #include <flann/flann.hpp>
46 #include <pcl/kdtree/kdtree_flann.h>
47 #include <pcl/console/print.h>
50 template <
typename Po
intT,
typename Dist>
54 , identity_mapping_ (false)
55 , dim_ (0), total_nr_points_ (0)
56 , param_k_ (::
flann::SearchParams (-1 , epsilon_))
57 , param_radius_ (::
flann::SearchParams (-1, epsilon_, sorted))
62 template <
typename Po
intT,
typename Dist>
66 , identity_mapping_ (false)
67 , dim_ (0), total_nr_points_ (0)
68 , param_k_ (::
flann::SearchParams (-1 , epsilon_))
69 , param_radius_ (::
flann::SearchParams (-1, epsilon_, false))
75 template <
typename Po
intT,
typename Dist>
void
79 param_k_ = ::flann::SearchParams (-1 , epsilon_);
80 param_radius_ = ::flann::SearchParams (-1 , epsilon_, sorted_);
84 template <
typename Po
intT,
typename Dist>
void
88 param_k_ = ::flann::SearchParams (-1, epsilon_);
89 param_radius_ = ::flann::SearchParams (-1, epsilon_, sorted_);
93 template <
typename Po
intT,
typename Dist>
void
99 dim_ = point_representation_->getNumberOfDimensions ();
107 PCL_ERROR (
"[pcl::KdTreeFLANN::setInputCloud] Invalid input!\n");
110 if (indices !=
nullptr)
112 convertCloudToArray (*input_, *indices_);
116 convertCloudToArray (*input_);
118 total_nr_points_ =
static_cast<int> (index_mapping_.size ());
119 if (total_nr_points_ == 0)
121 PCL_ERROR (
"[pcl::KdTreeFLANN::setInputCloud] Cannot create a KDTree with an empty input cloud!\n");
126 index_mapping_.size (),
128 ::flann::KDTreeSingleIndexParams (15)));
129 flann_index_->buildIndex ();
133 template <
typename Po
intT,
typename Dist>
int
135 std::vector<int> &k_indices,
136 std::vector<float> &k_distances)
const
138 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to nearestKSearch!");
140 if (k > total_nr_points_)
141 k = total_nr_points_;
143 k_indices.resize (k);
144 k_distances.resize (k);
146 std::vector<float> query (dim_);
147 point_representation_->vectorize (
static_cast<PointT> (point), query);
153 k_indices_mat, k_distances_mat,
157 if (!identity_mapping_)
159 for (std::size_t i = 0; i < static_cast<std::size_t> (k); ++i)
161 int& neighbor_index = k_indices[i];
162 neighbor_index = index_mapping_[neighbor_index];
170 template <
typename Po
intT,
typename Dist>
int
172 std::vector<float> &k_sqr_dists,
unsigned int max_nn)
const
174 assert (point_representation_->isValid (point) &&
"Invalid (NaN, Inf) point coordinates given to radiusSearch!");
176 std::vector<float> query (dim_);
177 point_representation_->vectorize (
static_cast<PointT> (point), query);
180 if (max_nn == 0 || max_nn >
static_cast<unsigned int> (total_nr_points_))
181 max_nn = total_nr_points_;
183 std::vector<std::vector<int> > indices(1);
184 std::vector<std::vector<float> > dists(1);
186 ::flann::SearchParams params (param_radius_);
187 if (max_nn ==
static_cast<unsigned int>(total_nr_points_))
188 params.max_neighbors = -1;
190 params.max_neighbors = max_nn;
192 int neighbors_in_radius = flann_index_->radiusSearch (::
flann::Matrix<float> (&query[0], 1, dim_),
195 static_cast<float> (radius * radius),
198 k_indices = indices[0];
199 k_sqr_dists = dists[0];
202 if (!identity_mapping_)
204 for (
int i = 0; i < neighbors_in_radius; ++i)
206 int& neighbor_index = k_indices[i];
207 neighbor_index = index_mapping_[neighbor_index];
211 return (neighbors_in_radius);
215 template <
typename Po
intT,
typename Dist>
void
219 index_mapping_.clear ();
226 template <
typename Po
intT,
typename Dist>
void
230 if (cloud.
points.empty ())
236 int original_no_of_points =
static_cast<int> (cloud.
points.size ());
238 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
239 float* cloud_ptr = cloud_.get ();
240 index_mapping_.reserve (original_no_of_points);
241 identity_mapping_ =
true;
243 for (
int cloud_index = 0; cloud_index < original_no_of_points; ++cloud_index)
246 if (!point_representation_->isValid (cloud.
points[cloud_index]))
248 identity_mapping_ =
false;
252 index_mapping_.push_back (cloud_index);
254 point_representation_->vectorize (cloud.
points[cloud_index], cloud_ptr);
260 template <
typename Po
intT,
typename Dist>
void
264 if (cloud.
points.empty ())
270 int original_no_of_points =
static_cast<int> (indices.size ());
272 cloud_.reset (
new float[original_no_of_points * dim_], std::default_delete<
float[]> ());
273 float* cloud_ptr = cloud_.get ();
274 index_mapping_.reserve (original_no_of_points);
282 identity_mapping_ =
false;
284 for (
const int &index : indices)
287 if (!point_representation_->isValid (cloud.
points[index]))
291 index_mapping_.push_back (index);
293 point_representation_->vectorize (cloud.
points[index], cloud_ptr);
298 #define PCL_INSTANTIATE_KdTreeFLANN(T) template class PCL_EXPORTS pcl::KdTreeFLANN<T>;
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
int radiusSearch(const PointT &point, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
Search for all the nearest neighbors of the query point in a given radius.
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
void setSortedResults(bool sorted)
KdTreeFLANN(bool sorted=true)
Default Constructor for KdTreeFLANN.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
KdTree represents the base spatial locator class for kd-tree implementations.
typename PointCloud::ConstPtr PointCloudConstPtr
shared_ptr< const std::vector< int > > IndicesConstPtr
PointCloud represents the base class in PCL for storing collections of 3D points.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
A point structure representing Euclidean xyz coordinates, and the RGB color.