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 #ifndef PCL_KEYPOINT_H_
00037 #define PCL_KEYPOINT_H_
00038
00039
00040 #include "pcl/pcl_base.h"
00041 #include <boost/function.hpp>
00042 #include <boost/bind.hpp>
00043 #include "pcl/kdtree/kdtree.h"
00044
00045 namespace pcl
00046 {
00050
00053 template <typename PointInT, typename PointOutT>
00054 class Keypoint : public PCLBase<PointInT>
00055 {
00056 using PCLBase<PointInT>::initCompute;
00057 using PCLBase<PointInT>::deinitCompute;
00058
00059 public:
00060 using PCLBase<PointInT>::indices_;
00061 using PCLBase<PointInT>::input_;
00062
00063 typedef PCLBase<PointInT> BaseClass;
00064 typedef typename pcl::KdTree<PointInT> KdTree;
00065 typedef typename pcl::KdTree<PointInT>::Ptr KdTreePtr;
00066 typedef pcl::PointCloud<PointInT> PointCloudIn;
00067 typedef typename PointCloudIn::Ptr PointCloudInPtr;
00068 typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr;
00069 typedef pcl::PointCloud<PointOutT> PointCloudOut;
00070 typedef boost::function<int (int, double, std::vector<int> &, std::vector<float> &)> SearchMethod;
00071 typedef boost::function<int (const PointCloudIn &cloud, int index, double, std::vector<int> &, std::vector<float> &)> SearchMethodSurface;
00072
00073 public:
00075
00076 Keypoint () : BaseClass(), surface_ (), tree_ (), search_parameter_ (0), search_radius_ (0), k_ (0) {};
00077
00079
00082 inline void
00083 setSearchSurface (const PointCloudInConstPtr &cloud) { surface_ = cloud; }
00084
00086
00087 inline PointCloudInConstPtr getSearchSurface () { return (surface_); }
00088
00090
00093 inline void setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
00094
00096
00097 inline KdTreePtr getSearchMethod () { return (tree_); }
00098
00100
00101 inline double getSearchParameter () { return (search_parameter_); }
00102
00104
00107 inline void setKSearch (int k) { k_ = k; }
00108
00110
00111 inline int getKSearch () { return (k_); }
00112
00114
00118 inline void setRadiusSearch (double radius) { search_radius_ = radius; }
00119
00121
00122 inline double getRadiusSearch () { return (search_radius_); }
00123
00125
00129 inline void compute (PointCloudOut &output);
00130
00132
00140 inline int
00141 searchForNeighbors (int index, double parameter, std::vector<int> &indices, std::vector<float> &distances)
00142 {
00143 if (surface_ == input_)
00144 return (search_method_ (index, parameter, indices, distances));
00145 else
00146 return (search_method_surface_ (*input_, index, parameter, indices, distances));
00147 }
00148
00149 protected:
00151 std::string name_;
00152
00154 SearchMethod search_method_;
00155
00157 SearchMethodSurface search_method_surface_;
00158
00160 PointCloudInConstPtr surface_;
00161
00163 KdTreePtr tree_;
00164
00166 double search_parameter_;
00167
00169 double search_radius_;
00170
00172 int k_;
00173
00175
00176 inline const std::string& getClassName () const { return (name_); }
00177
00179
00180 virtual void detectKeypoints (PointCloudOut &output) = 0;
00181 };
00182 }
00183
00184 #include "pcl/keypoints/keypoint.hpp"
00185
00186 #endif //#ifndef PCL_KEYPOINT_H_