Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_SEARCH_BRUTE_FORCE_H_
00039 #define PCL_SEARCH_BRUTE_FORCE_H_
00040
00041 #include <pcl/search/search.h>
00042
00043 namespace pcl
00044 {
00045 namespace search
00046 {
00051 template<typename PointT>
00052 class BruteForce: public Search<PointT>
00053 {
00054 typedef typename Search<PointT>::PointCloud PointCloud;
00055 typedef typename Search<PointT>::PointCloudConstPtr PointCloudConstPtr;
00056
00057 typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
00058 typedef boost::shared_ptr<const std::vector<int> > IndicesConstPtr;
00059
00060 using pcl::search::Search<PointT>::input_;
00061 using pcl::search::Search<PointT>::indices_;
00062 using pcl::search::Search<PointT>::sorted_results_;
00063
00064 struct Entry
00065 {
00066 Entry (int idx, float dist) : index (idx), distance (dist) {}
00067
00068 Entry () : index (0), distance (0) {}
00069 unsigned index;
00070 float distance;
00071
00072 inline bool
00073 operator < (const Entry& other) const
00074 {
00075 return (distance < other.distance);
00076 }
00077
00078 inline bool
00079 operator > (const Entry& other) const
00080 {
00081 return (distance > other.distance);
00082 }
00083 };
00084
00085
00086 float getDistSqr (const PointT& point1, const PointT& point2) const;
00087 public:
00088 BruteForce (bool sorted_results = false)
00089 : Search<PointT> ("BruteForce", sorted_results)
00090 {
00091 }
00092
00094 virtual
00095 ~BruteForce ()
00096 {
00097 }
00098
00107 int
00108 nearestKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
00109
00120 int
00121 radiusSearch (const PointT& point, double radius,
00122 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00123 unsigned int max_nn = 0) const;
00124
00125 private:
00126 int
00127 denseKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
00128
00129 int
00130 sparseKSearch (const PointT &point, int k, std::vector<int> &k_indices, std::vector<float> &k_distances) const;
00131
00132 int
00133 denseRadiusSearch (const PointT& point, double radius,
00134 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00135 unsigned int max_nn = 0) const;
00136
00137 int
00138 sparseRadiusSearch (const PointT& point, double radius,
00139 std::vector<int> &k_indices, std::vector<float> &k_sqr_distances,
00140 unsigned int max_nn = 0) const;
00141 };
00142 }
00143 }
00144
00145 #endif // PCL_SEARCH_BRUTE_FORCE_H_