Point Cloud Library (PCL)  1.7.1
brute_force.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  */
37 
38 #ifndef PCL_SEARCH_BRUTE_FORCE_H_
39 #define PCL_SEARCH_BRUTE_FORCE_H_
40 
41 #include <pcl/search/search.h>
42 
43 namespace pcl
44 {
45  namespace search
46  {
47  /** \brief Implementation of a simple brute force search algorithm.
48  * \author Suat Gedikli
49  * \ingroup search
50  */
51  template<typename PointT>
52  class BruteForce: public Search<PointT>
53  {
54  typedef typename Search<PointT>::PointCloud PointCloud;
55  typedef typename Search<PointT>::PointCloudConstPtr PointCloudConstPtr;
56 
57  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
58  typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
59 
63 
64  struct Entry
65  {
66  Entry (int idx, float dist) : index (idx), distance (dist) {}
67 
68  Entry () : index (0), distance (0) {}
69  unsigned index;
70  float distance;
71 
72  inline bool
73  operator < (const Entry& other) const
74  {
75  return (distance < other.distance);
76  }
77 
78  inline bool
79  operator > (const Entry& other) const
80  {
81  return (distance > other.distance);
82  }
83  };
84 
85  // replace by some metric functor
86  float getDistSqr (const PointT& point1, const PointT& point2) const;
87  public:
88  BruteForce (bool sorted_results = false)
89  : Search<PointT> ("BruteForce", sorted_results)
90  {
91  }
92 
93  /** \brief Destructor for KdTree. */
94  virtual
96  {
97  }
98 
99  /** \brief Search for the k-nearest neighbors for the given query point.
100  * \param[in] point the given query point
101  * \param[in] k the number of neighbors to search for
102  * \param[out] k_indices the resultant indices of the neighboring points (must be resized to \a k a priori!)
103  * \param[out] k_distances the resultant squared distances to the neighboring points (must be resized to \a k
104  * a priori!)
105  * \return number of neighbors found
106  */
107  int
108  nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
109 
110  /** \brief Search for all the nearest neighbors of the query point in a given radius.
111  * \param[in] point the given query point
112  * \param[in] radius the radius of the sphere bounding all of p_q's neighbors
113  * \param[out] k_indices the resultant indices of the neighboring points
114  * \param[out] k_sqr_distances the resultant squared distances to the neighboring points
115  * \param[in] max_nn if given, bounds the maximum returned neighbors to this value. If \a max_nn is set to
116  * 0 or to a number higher than the number of points in the input cloud, all neighbors in \a radius will be
117  * returned.
118  * \return number of neighbors found in radius
119  */
120  int
121  radiusSearch (const PointT& point, double radius,
122  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
123  unsigned int max_nn = 0) const;
124 
125  private:
126  int
127  denseKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
128 
129  int
130  sparseKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
131 
132  int
133  denseRadiusSearch (const PointT& point, double radius,
134  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
135  unsigned int max_nn = 0) const;
136 
137  int
138  sparseRadiusSearch (const PointT& point, double radius,
139  std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
140  unsigned int max_nn = 0) const;
141  };
142  }
143 }
144 
145 #ifdef PCL_NO_PRECOMPILE
146 #include <pcl/search/impl/brute_force.hpp>
147 #endif
148 
149 #endif // PCL_SEARCH_BRUTE_FORCE_H_