Point Cloud Library (PCL) 1.14.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] k Number of neighbors to search for
109 * @param[in] params Any parameters to pass to the knn_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() override
204 {
205 cleanup();
206 }
207
208 /** \brief Provide a pointer to the input dataset.
209 * \param[in] cloud the const boost shared pointer to a PointCloud message
210 * \param[in] indices the point indices subset that is to be used from \a cloud - if
211 * NULL the whole cloud is used
212 */
213 void
214 setInputCloud(const PointCloudConstPtr& cloud,
215 const IndicesConstPtr& indices = IndicesConstPtr()) override;
216
217 /** \brief Search for k-nearest neighbors for the given query point.
218 *
219 * \attention This method does not do any bounds checking for the input index
220 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
221 *
222 * \param[in] point a given \a valid (i.e., finite) query point
223 * \param[in] k the number of neighbors to search for
224 * \param[out] k_indices the resultant indices of the neighboring points (must be
225 * resized to \a k a priori!) \param[out] k_sqr_distances the resultant squared
226 * distances to the neighboring points (must be resized to \a k a priori!) \return
227 * number of neighbors found
228 *
229 * \exception asserts in debug mode if the index is not between 0 and the maximum
230 * number of points
231 */
232 int
233 nearestKSearch(const PointT& point,
234 unsigned int k,
235 Indices& k_indices,
236 std::vector<float>& k_sqr_distances) const override;
237
238 /** \brief Search for all the nearest neighbors of the query point in a given radius.
239 *
240 * \attention This method does not do any bounds checking for the input index
241 * (i.e., index >= cloud.size () || index < 0), and assumes valid (i.e., finite) data.
242 *
243 * \param[in] point a given \a valid (i.e., finite) query point
244 * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
245 * \param[out] k_indices the resultant indices of the neighboring points
246 * \param[out] k_sqr_distances the resultant squared distances to the neighboring
247 * points \param[in] max_nn if given, bounds the maximum returned neighbors to this
248 * value. If \a max_nn is set to 0 or to a number higher than the number of points in
249 * the input cloud, all neighbors in \a radius will be returned. \return number of
250 * neighbors found in radius
251 *
252 * \exception asserts in debug mode if the index is not between 0 and the maximum
253 * number of points
254 */
255 int
256 radiusSearch(const PointT& point,
257 double radius,
258 Indices& k_indices,
259 std::vector<float>& k_sqr_distances,
260 unsigned int max_nn = 0) const override;
261
262private:
263 /** \brief Internal cleanup method. */
264 void
265 cleanup();
266
267 /** \brief Converts a PointCloud to the internal FLANN point array representation.
268 * Returns the number of points. \param cloud the PointCloud
269 */
270 void
271 convertCloudToArray(const PointCloud& cloud);
272
273 /** \brief Converts a PointCloud with a given set of indices to the internal FLANN
274 * point array representation. Returns the number of points. \param[in] cloud the
275 * PointCloud data \param[in] indices the point cloud indices
276 */
277 void
278 convertCloudToArray(const PointCloud& cloud, const Indices& indices);
279
280private:
281 /** \brief Class getName method. */
282 std::string
283 getName() const override
284 {
285 return ("KdTreeFLANN");
286 }
287
288 /** \brief A FLANN index object. */
289 std::shared_ptr<FLANNIndex> flann_index_;
290
291 /** \brief Internal pointer to data. TODO: replace with std::shared_ptr<float[]> with
292 * C++17*/
293 std::shared_ptr<float> cloud_;
294
295 /** \brief mapping between internal and external indices. */
296 std::vector<int> index_mapping_;
297
298 /** \brief whether the mapping between internal and external indices is identity */
299 bool identity_mapping_{false};
300
301 /** \brief Tree dimensionality (i.e. the number of dimensions per point). */
302 int dim_{0};
303
304 /** \brief The total size of the data (either equal to the number of points in the
305 * input cloud or to the number of indices - if passed). */
306 uindex_t total_nr_points_{0};
307
308 /** \brief The KdTree search parameters for K-nearest neighbors. */
309 ::flann::SearchParams param_k_;
310
311 /** \brief The KdTree search parameters for radius search. */
312 ::flann::SearchParams param_radius_;
313 };
314}
315
316#ifdef PCL_NO_PRECOMPILE
317#include <pcl/kdtree/impl/kdtree_flann.hpp>
318#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.
~KdTreeFLANN() override
Destructor for KdTreeFLANN.
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)
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:56
PointRepresentationConstPtr point_representation_
For converting different point structures into k-dimensional vectors for nearest-neighbor search.
Definition kdtree.h:352
typename PointCloud::ConstPtr PointCloudConstPtr
Definition kdtree.h:63
PointCloudConstPtr input_
The input point cloud dataset containing the points we need to use.
Definition kdtree.h:337
float epsilon_
Epsilon precision (error bound) for nearest neighbors searches.
Definition kdtree.h:343
bool sorted_
Return the radius search neighbours sorted.
Definition kdtree.h:349
IndicesConstPtr indices_
A pointer to the vector of point indices to use.
Definition kdtree.h:340
PointCloud represents the base class in PCL for storing collections of 3D points.
std::enable_if_t< compat_with_flann< IndexT >::value, bool > CompatWithFlann
std::enable_if_t<!compat_with_flann< IndexT >::value, bool > NotCompatWithFlann
shared_ptr< const Indices > IndicesConstPtr
Definition pcl_base.h:59
detail::int_type_t< detail::index_type_size, false > uindex_t
Type used for an unsigned index in PCL.
Definition types.h:120
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
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.
A point structure representing Euclidean xyz coordinates, and the RGB color.