Point Cloud Library (PCL)  1.10.0-dev
octree.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  */
38 
39 #pragma once
40 
41 #include <pcl/search/search.h>
42 #include <pcl/octree/octree_search.h>
43 
44 namespace pcl
45 {
46  namespace search
47  {
48  /** \brief @b search::Octree is a wrapper class which implements nearest neighbor search operations based on the
49  * pcl::octree::Octree structure.
50  *
51  * The octree pointcloud class needs to be initialized with its voxel
52  * resolution. Its bounding box is automatically adjusted according to the
53  * pointcloud dimension or it can be predefined. Note: The tree depth
54  * equates to the resolution and the bounding box dimensions of the
55  * octree.
56  *
57  * \note typename: PointT: type of point used in pointcloud
58  * \note typename: LeafT: leaf node class (usuallt templated with integer indices values)
59  * \note typename: OctreeT: octree implementation ()
60  *
61  * \author Julius Kammerl
62  * \ingroup search
63  */
64  template<typename PointT,
65  typename LeafTWrap = pcl::octree::OctreeContainerPointIndices,
66  typename BranchTWrap = pcl::octree::OctreeContainerEmpty,
68  class Octree: public Search<PointT>
69  {
70  public:
71  // public typedefs
74 
75  using typename Search<PointT>::IndicesPtr;
76  using typename Search<PointT>::IndicesConstPtr;
77 
79  using PointCloudPtr = typename PointCloud::Ptr;
81 
82  // Boost shared pointers
86 
90 
91  /** \brief Octree constructor.
92  * \param[in] resolution octree resolution at lowest octree level
93  */
94  Octree (const double resolution)
95  : Search<PointT> ("Octree")
96  , tree_ (new pcl::octree::OctreePointCloudSearch<PointT, LeafTWrap, BranchTWrap> (resolution))
97  {
98  }
99 
100  /** \brief Empty Destructor. */
101 
103  {
104  }
105 
106  /** \brief Provide a pointer to the input dataset.
107  * \param[in] cloud the const boost shared pointer to a PointCloud message
108  */
109  inline void
111  {
112  tree_->deleteTree ();
113  tree_->setInputCloud (cloud);
114  tree_->addPointsFromInputCloud ();
115  input_ = cloud;
116  }
117 
118  /** \brief Provide a pointer to the input dataset.
119  * \param[in] cloud the const boost shared pointer to a PointCloud message
120  * \param[in] indices the point indices subset that is to be used from \a cloud
121  */
122  inline void
123  setInputCloud (const PointCloudConstPtr &cloud, const IndicesConstPtr& indices) override
124  {
125  tree_->deleteTree ();
126  tree_->setInputCloud (cloud, indices);
127  tree_->addPointsFromInputCloud ();
128  input_ = cloud;
129  indices_ = indices;
130  }
131 
132  /** \brief Search for the k-nearest neighbors for the given query point.
133  * \param[in] cloud the point cloud data
134  * \param[in] index the index in \a cloud representing the query point
135  * \param[in] k the number of neighbors to search for
136  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
137  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
138  * a priori!)
139  * \return number of neighbors found
140  */
141  inline int
142  nearestKSearch (const PointCloud &cloud, int index, int k, std::vector<int> &k_indices,
143  std::vector<float> &k_sqr_distances) const override
144  {
145  return (tree_->nearestKSearch (cloud, index, k, k_indices, k_sqr_distances));
146  }
147 
148  /** \brief Search for the k-nearest neighbors for the given query point.
149  * \param[in] point the given query point
150  * \param[in] k the number of neighbors to search for
151  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
152  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
153  * a priori!)
154  * \return number of neighbors found
155  */
156  inline int
157  nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices,
158  std::vector<float> &k_sqr_distances) const override
159  {
160  return (tree_->nearestKSearch (point, k, k_indices, k_sqr_distances));
161  }
162 
163  /** \brief Search for the k-nearest neighbors for the given query point (zero-copy).
164  *
165  * \param[in] index the index representing the query point in the
166  * dataset given by \a setInputCloud if indices were given in
167  * setInputCloud, index will be the position in the indices vector
168  * \param[in] k the number of neighbors to search for
169  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
170  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points (must be resized to \a k
171  * a priori!)
172  * \return number of neighbors found
173  */
174  inline int
175  nearestKSearch (int index, int k, std::vector<int> &k_indices, std::vector<float> &k_sqr_distances) const override
176  {
177  return (tree_->nearestKSearch (index, k, k_indices, k_sqr_distances));
178  }
179 
180  /** \brief search for all neighbors of query point that are within a given radius.
181  * \param cloud the point cloud data
182  * \param index the index in \a cloud representing the query point
183  * \param radius the radius of the sphere bounding all of p_q's neighbors
184  * \param k_indices the resultant indices of the neighboring points
185  * \param k_sqr_distances the resultant squared distances to the neighboring points
186  * \param max_nn if given, bounds the maximum returned neighbors to this value
187  * \return number of neighbors found in radius
188  */
189  inline int
190  radiusSearch (const PointCloud &cloud,
191  int index,
192  double radius,
193  std::vector<int> &k_indices,
194  std::vector<float> &k_sqr_distances,
195  unsigned int max_nn = 0) const override
196  {
197  tree_->radiusSearch (cloud, index, radius, k_indices, k_sqr_distances, max_nn);
198  if (sorted_results_)
199  this->sortResults (k_indices, k_sqr_distances);
200  return (static_cast<int> (k_indices.size ()));
201  }
202 
203  /** \brief search for all neighbors of query point that are within a given radius.
204  * \param p_q the given query point
205  * \param radius the radius of the sphere bounding all of p_q's neighbors
206  * \param k_indices the resultant indices of the neighboring points
207  * \param k_sqr_distances the resultant squared distances to the neighboring points
208  * \param max_nn if given, bounds the maximum returned neighbors to this value
209  * \return number of neighbors found in radius
210  */
211  inline int
212  radiusSearch (const PointT &p_q,
213  double radius,
214  std::vector<int> &k_indices,
215  std::vector<float> &k_sqr_distances,
216  unsigned int max_nn = 0) const override
217  {
218  tree_->radiusSearch (p_q, radius, k_indices, k_sqr_distances, max_nn);
219  if (sorted_results_)
220  this->sortResults (k_indices, k_sqr_distances);
221  return (static_cast<int> (k_indices.size ()));
222  }
223 
224  /** \brief search for all neighbors of query point that are within a given radius.
225  * \param index index representing the query point in the dataset given by \a setInputCloud.
226  * If indices were given in setInputCloud, index will be the position in the indices vector
227  * \param radius radius of the sphere bounding all of p_q's neighbors
228  * \param k_indices the resultant indices of the neighboring points
229  * \param k_sqr_distances the resultant squared distances to the neighboring points
230  * \param max_nn if given, bounds the maximum returned neighbors to this value
231  * \return number of neighbors found in radius
232  */
233  inline int
234  radiusSearch (int index, double radius, std::vector<int> &k_indices,
235  std::vector<float> &k_sqr_distances, unsigned int max_nn = 0) const override
236  {
237  tree_->radiusSearch (index, radius, k_indices, k_sqr_distances, max_nn);
238  if (sorted_results_)
239  this->sortResults (k_indices, k_sqr_distances);
240  return (static_cast<int> (k_indices.size ()));
241  }
242 
243 
244  /** \brief Search for approximate nearest neighbor at the query point.
245  * \param[in] cloud the point cloud data
246  * \param[in] query_index the index in \a cloud representing the query point
247  * \param[out] result_index the resultant index of the neighbor point
248  * \param[out] sqr_distance the resultant squared distance to the neighboring point
249  * \return number of neighbors found
250  */
251  inline void
252  approxNearestSearch (const PointCloudConstPtr &cloud, int query_index, int &result_index,
253  float &sqr_distance)
254  {
255  return (tree_->approxNearestSearch (cloud->points[query_index], result_index, sqr_distance));
256  }
257 
258  /** \brief Search for approximate nearest neighbor at the query point.
259  * \param[in] p_q the given query point
260  * \param[out] result_index the resultant index of the neighbor point
261  * \param[out] sqr_distance the resultant squared distance to the neighboring point
262  */
263  inline void
264  approxNearestSearch (const PointT &p_q, int &result_index, float &sqr_distance)
265  {
266  return (tree_->approxNearestSearch (p_q, result_index, sqr_distance));
267  }
268 
269  /** \brief Search for approximate nearest neighbor at the query point.
270  * \param query_index index representing the query point in the dataset given by \a setInputCloud.
271  * If indices were given in setInputCloud, index will be the position in the indices vector.
272  * \param result_index the resultant index of the neighbor point
273  * \param sqr_distance the resultant squared distance to the neighboring point
274  * \return number of neighbors found
275  */
276  inline void
277  approxNearestSearch (int query_index, int &result_index, float &sqr_distance)
278  {
279  return (tree_->approxNearestSearch (query_index, result_index, sqr_distance));
280  }
281 
282  };
283  }
284 }
285 
286 #ifdef PCL_NO_PRECOMPILE
287 #include <pcl/octree/impl/octree_search.hpp>
288 #else
289 #define PCL_INSTANTIATE_Octree(T) template class PCL_EXPORTS pcl::search::Octree<T>;
290 #endif
shared_ptr< const OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > ConstPtr
Definition: octree_search.h:69
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:415
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: octree.h:80
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
Octree(const double resolution)
Octree constructor.
Definition: octree.h:94
shared_ptr< std::vector< int > > IndicesPtr
Definition: search.h:83
PointCloudConstPtr input_
Definition: search.h:402
OctreePointCloudSearchPtr tree_
Definition: octree.h:85
shared_ptr< const std::vector< int > > IndicesConstPtr
Definition: search.h:84
int nearestKSearch(const PointT &point, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
Definition: octree.h:157
void sortResults(std::vector< int > &indices, std::vector< float > &distances) const
Definition: search.hpp:188
int radiusSearch(int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
search for all neighbors of query point that are within a given radius.
Definition: octree.h:234
shared_ptr< const pcl::search::Octree< PointInT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase< pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > > > ConstPtr
Definition: octree.h:73
typename pcl::octree::OctreePointCloudSearch< PointInT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::ConstPtr OctreePointCloudSearchConstPtr
Definition: octree.h:84
void approxNearestSearch(const PointT &p_q, int &result_index, float &sqr_distance)
Search for approximate nearest neighbor at the query point.
Definition: octree.h:264
IndicesConstPtr indices_
Definition: search.h:403
bool sorted_results_
Definition: search.h:404
Octree container class that does store a vector of point indices.
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices) override
Provide a pointer to the input dataset.
Definition: octree.h:123
shared_ptr< pcl::search::Octree< PointInT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty, pcl::octree::OctreeBase< pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty > > > Ptr
Definition: octree.h:72
int radiusSearch(const PointCloud &cloud, int index, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
search for all neighbors of query point that are within a given radius.
Definition: octree.h:190
int radiusSearch(const PointT &p_q, double radius, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances, unsigned int max_nn=0) const override
search for all neighbors of query point that are within a given radius.
Definition: octree.h:212
shared_ptr< OctreePointCloudSearch< PointT, LeafContainerT, BranchContainerT > > Ptr
Definition: octree_search.h:68
typename PointCloud::Ptr PointCloudPtr
Definition: octree.h:79
void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: octree.h:110
search::Octree is a wrapper class which implements nearest neighbor search operations based on the pc...
Definition: octree.h:68
~Octree()
Empty Destructor.
Definition: octree.h:102
void approxNearestSearch(const PointCloudConstPtr &cloud, int query_index, int &result_index, float &sqr_distance)
Search for approximate nearest neighbor at the query point.
Definition: octree.h:252
int nearestKSearch(int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point (zero-copy).
Definition: octree.h:175
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:416
int nearestKSearch(const PointCloud &cloud, int index, int k, std::vector< int > &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
Definition: octree.h:142
typename pcl::octree::OctreePointCloudSearch< PointInT, pcl::octree::OctreeContainerPointIndices, pcl::octree::OctreeContainerEmpty >::Ptr OctreePointCloudSearchPtr
Definition: octree.h:83
A point structure representing Euclidean xyz coordinates, and the RGB color.
void approxNearestSearch(int query_index, int &result_index, float &sqr_distance)
Search for approximate nearest neighbor at the query point.
Definition: octree.h:277
Octree container class that does not store any information.
boost::shared_ptr< T > shared_ptr
Alias for boost::shared_ptr.
Definition: pcl_macros.h:90
Generic search class.
Definition: search.h:73