Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
kdtree_flann.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id: kdtree_flann.h 36261 2011-02-26 01:34:42Z mariusm $
38 *
39 */
40
41#pragma once
42
43#include <pcl/kdtree/kdtree.h>
44#include <flann/util/params.h>
45
46#include <memory>
47
48// Forward declarations
49namespace flann
50{
51 template <typename T> struct L2_Simple;
52 template <typename T> class Index;
53}
54
55namespace pcl
56{
57namespace detail {
58// Helper struct to create a compatible Matrix and copy data back when needed
59// Replace using if constexpr in C++17
60template <typename IndexT>
61struct compat_with_flann : std::false_type {};
62
63template <>
64struct compat_with_flann<std::size_t> : std::true_type {};
65
66template <typename IndexT>
67using CompatWithFlann = std::enable_if_t<compat_with_flann<IndexT>::value, bool>;
68template <typename IndexT>
69using NotCompatWithFlann = std::enable_if_t<!compat_with_flann<IndexT>::value, bool>;
70} // namespace detail
71
72/**
73 * @brief Comaptibility template function to allow use of various types of indices with
74 * FLANN
75 * @details Template is used for all params to not constrain any FLANN side capability
76 * @param[in|out] index A index searcher, of type ::flann::Index<Dist> or similar, where
77 * Dist is a template for computing distance between 2 points
78 * @param[in] query A ::flann::Matrix<float> or compatible matrix representation of the
79 * query point
80 * @param[out] indices Indices found in radius
81 * @param[out] dists Computed distance matrix
82 * @param[in] radius Threshold for consideration
83 * @param[in] params Any parameters to pass to the radius_search call
84 */
85template <class FlannIndex,
86 class Query,
87 class Indices,
88 class Distances,
89 class SearchParams>
90int
91radius_search(const FlannIndex& index,
92 const Query& query,
93 Indices& indices,
94 Distances& dists,
95 float radius,
96 const SearchParams& params);
97
98/**
99 * @brief Comaptibility template function to allow use of various types of indices with
100 * FLANN
101 * @details Template is used for all params to not constrain any FLANN side capability
102 * @param[in|out] index A index searcher, of type ::flann::Index<Dist> or similar, where
103 * Dist is a template for computing distance between 2 points
104 * @param[in] query A ::flann::Matrix<float> or compatible matrix representation of the
105 * query point
106 * @param[out] indices Neighboring k indices found
107 * @param[out] dists Computed distance matrix
108 * @param[in] radius Threshold for consideration
109 * @param[in] params Any parameters to pass to the radius_search call
110 */
111template <class FlannIndex,
112 class Query,
113 class Indices,
114 class Distances,
115 class SearchParams>
116int
117knn_search(const FlannIndex& index,
118 const Query& query,
119 Indices& indices,
120 Distances& dists,
121 unsigned int k,
122 const SearchParams& params);
123
124/** \brief KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
125 * The class is making use of the FLANN (Fast Library for Approximate Nearest Neighbor)
126 * project by Marius Muja and David Lowe.
127 *
128 * \author Radu B. Rusu, Marius Muja
129 * \ingroup kdtree
130 */
131template <typename PointT, typename Dist = ::flann::L2_Simple<float>>
132class KdTreeFLANN : public pcl::KdTree<PointT> {
133public:
134 using KdTree<PointT>::input_;
137 using KdTree<PointT>::sorted_;
141
144
145 using IndicesPtr = shared_ptr<Indices>;
146 using IndicesConstPtr = shared_ptr<const Indices>;
147
149
150 // Boost shared pointers
151 using Ptr = shared_ptr<KdTreeFLANN<PointT, Dist>>;
152 using ConstPtr = shared_ptr<const KdTreeFLANN<PointT, Dist>>;
153
154 /** \brief Default Constructor for KdTreeFLANN.
155 * \param[in] sorted set to true if the application that the tree will be used for
156 * requires sorted nearest neighbor indices (default). False otherwise.
157 *
158 * By setting sorted to false, the \ref radiusSearch operations will be faster.
159 */
160 KdTreeFLANN(bool sorted = true);
161
162 /** \brief Copy constructor
163 * \param[in] k the tree to copy into this
164 */
166
167 /** \brief Copy operator
168 * \param[in] k the tree to copy into this
169 */
172 {
174 flann_index_ = k.flann_index_;
175 cloud_ = k.cloud_;
176 index_mapping_ = k.index_mapping_;
177 identity_mapping_ = k.identity_mapping_;
178 dim_ = k.dim_;
179 total_nr_points_ = k.total_nr_points_;
180 param_k_ = k.param_k_;
181 param_radius_ = k.param_radius_;
182 return (*this);
183 }
184
185 /** \brief Set the search epsilon precision (error bound) for nearest neighbors
186 * searches. \param[in] eps precision (error bound) for nearest neighbors searches
187 */
188 void
189 setEpsilon(float eps) override;
190
191 void
192 setSortedResults(bool sorted);
193
194 inline Ptr
196 {
197 return Ptr(new KdTreeFLANN<PointT, Dist>(*this));
198 }
199
200 /** \brief Destructor for KdTreeFLANN.
201 * Deletes all allocated data arrays and destroys the kd-tree structures.
202 */
203 ~KdTreeFLANN() { cleanup(); }
204
205 /** \brief Provide a pointer to the input dataset.
206 * \param[in] cloud the const boost shared pointer to a PointCloud message
207 * \param[in] indices the point indices subset that is to be used from \a cloud - if
208 * NULL the whole cloud is used
209 */
210 void
211 setInputCloud(const PointCloudConstPtr& cloud,
212 const IndicesConstPtr& indices = IndicesConstPtr()) override;
213
214 /** \brief Search for k-nearest neighbors for the given query point.
215 *
216 * \attention This method does not do any bounds checking for the input index
217 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
218 *
219 * \param[in] point a given \a valid (i.e., finite) query point
220 * \param[in] k the number of neighbors to search for
221 * \param[out] k_indices the resultant indices of the neighboring points (must be
222 * resized to \a k a priori!) \param[out] k_sqr_distances the resultant squared
223 * distances to the neighboring points (must be resized to \a k a priori!) \return
224 * number of neighbors found
225 *
226 * \exception asserts in debug mode if the index is not between 0 and the maximum
227 * number of points
228 */
229 int
230 nearestKSearch(const PointT& point,
231 unsigned int k,
232 Indices& k_indices,
233 std::vector<float>& k_sqr_distances) const override;
234
235 /** \brief Search for all the nearest neighbors of the query point in a given radius.
236 *
237 * \attention This method does not do any bounds checking for the input index
238 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
239 *
240 * \param[in] point a given \a valid (i.e., finite) query point
241 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
242 * \param[out] k_indices the resultant indices of the neighboring points
243 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
244 * points \param[in] max_nn if given, bounds the maximum returned neighbors to this
245 * value. If \a max_nn is set to 0 or to a number higher than the number of points in
246 * the input cloud, all neighbors in \a radius will be returned. \return number of
247 * neighbors found in radius
248 *
249 * \exception asserts in debug mode if the index is not between 0 and the maximum
250 * number of points
251 */
252 int
253 radiusSearch(const PointT& point,
254 double radius,
255 Indices& k_indices,
256 std::vector<float>& k_sqr_distances,
257 unsigned int max_nn = 0) const override;
258
259private:
260 /** \brief Internal cleanup method. */
261 void
262 cleanup();
263
264 /** \brief Converts a PointCloud to the internal FLANN point array representation.
265 * Returns the number of points. \param cloud the PointCloud
266 */
267 void
268 convertCloudToArray(const PointCloud& cloud);
269
270 /** \brief Converts a PointCloud with a given set of indices to the internal FLANN
271 * point array representation. Returns the number of points. \param[in] cloud the
272 * PointCloud data \param[in] indices the point cloud indices
273 */
274 void
275 convertCloudToArray(const PointCloud& cloud, const Indices& indices);
276
277private:
278 /** \brief Class getName method. */
279 std::string
280 getName() const override
281 {
282 return ("KdTreeFLANN");
283 }
284
285 /** \brief A FLANN index object. */
286 std::shared_ptr<FLANNIndex> flann_index_;
287
288 /** \brief Internal pointer to data. TODO: replace with std::shared_ptr<float[]> with
289 * C++17*/
290 std::shared_ptr<float> cloud_;
291
292 /** \brief mapping between internal and external indices. */
293 std::vector<int> index_mapping_;
294
295 /** \brief whether the mapping between internal and external indices is identity */
296 bool identity_mapping_;
297
298 /** \brief Tree dimensionality (i.e. the number of dimensions per point). */
299 int dim_;
300
301 /** \brief The total size of the data (either equal to the number of points in the
302 * input cloud or to the number of indices - if passed). */
303 uindex_t total_nr_points_;
304
305 /** \brief The KdTree search parameters for K-nearest neighbors. */
306 ::flann::SearchParams param_k_;
307
308 /** \brief The KdTree search parameters for radius search. */
309 ::flann::SearchParams param_radius_;
310 };
311}
312
313#ifdef PCL_NO_PRECOMPILE
314#include <pcl/kdtree/impl/kdtree_flann.hpp>
315#endif
KdTreeFLANN is a generic type of 3D spatial locator using kD-tree structures.
void setEpsilon(float eps) override
Set the search epsilon precision (error bound) for nearest neighbors searches.
shared_ptr< const KdTreeFLANN< PointT, Dist > > ConstPtr
KdTreeFLANN< PointT, Dist > & operator=(const KdTreeFLANN< PointT, Dist > &k)
Copy operator.
shared_ptr< const Indices > IndicesConstPtr
typename KdTree< PointT >::PointCloud PointCloud
shared_ptr< KdTreeFLANN< PointT, Dist > > Ptr
int nearestKSearch(const PointT &point, unsigned int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for k-nearest neighbors for the given query point.
int radiusSearch(const PointT &point, double radius, Indices &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.
typename KdTree< PointT >::PointCloudConstPtr PointCloudConstPtr
shared_ptr< Indices > IndicesPtr
void setSortedResults(bool sorted)
~KdTreeFLANN()
Destructor for KdTreeFLANN.
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.
Definition kdtree.h:55
PointRepresentationConstPtr point_representation_
For converting different point structures into k-dimensional vectors for nearest-neighbor search.
Definition kdtree.h:351
typename PointCloud::ConstPtr PointCloudConstPtr
Definition kdtree.h:62
PointCloudConstPtr input_
The input point cloud dataset containing the points we need to use.
Definition kdtree.h:336
float epsilon_
Epsilon precision (error bound) for nearest neighbors searches.
Definition kdtree.h:342
bool sorted_
Return the radius search neighbours sorted.
Definition kdtree.h:348
IndicesConstPtr indices_
A pointer to the vector of point indices to use.
Definition kdtree.h:339
std::enable_if_t< compat_with_flann< IndexT >::value, bool > CompatWithFlann
std::enable_if_t<!compat_with_flann< IndexT >::value, bool > NotCompatWithFlann
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59
int radius_search(const FlannIndex &index, const Query &query, Indices &indices, Distances &dists, float radius, const SearchParams &params)
Comaptibility template function to allow use of various types of indices with FLANN.
int knn_search(const FlannIndex &index, const Query &query, Indices &indices, Distances &dists, unsigned int k, const SearchParams &params)
Comaptibility template function to allow use of various types of indices with FLANN.
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
A point structure representing Euclidean xyz coordinates, and the RGB color.