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_PFH_H_
00039 #define PCL_PFH_H_
00040
00041 #include <pcl/features/feature.h>
00042
00043 namespace pcl
00044 {
00058 bool
00059 computePairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1,
00060 const Eigen::Vector4f &p2, const Eigen::Vector4f &n2,
00061 float &f1, float &f2, float &f3, float &f4);
00062
00064
00086 template <typename PointInT, typename PointNT, typename PointOutT>
00087 class PFHEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
00088 {
00089 public:
00090 using Feature<PointInT, PointOutT>::feature_name_;
00091 using Feature<PointInT, PointOutT>::getClassName;
00092 using Feature<PointInT, PointOutT>::indices_;
00093 using Feature<PointInT, PointOutT>::k_;
00094 using Feature<PointInT, PointOutT>::search_parameter_;
00095 using Feature<PointInT, PointOutT>::surface_;
00096 using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00097
00098 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00099 typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn;
00100
00102 PFHEstimation () : nr_subdiv_ (5), d_pi_ (1.0 / (2.0 * M_PI))
00103 {
00104 feature_name_ = "PFHEstimation";
00105 };
00106
00120 bool
00121 computePairFeatures (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00122 int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4);
00123
00132 void
00133 computePointPFHSignature (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00134 const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram);
00135
00139 inline void
00140 setNrSubdivisions (int nr_subdiv) { nr_subdiv_ = nr_subdiv; }
00141
00145 inline void
00146 getNrSubdivisions (int &nr_subdiv) { nr_subdiv = nr_subdiv_; }
00147
00148 protected:
00149
00155 void
00156 computeFeature (PointCloudOut &output);
00157
00158 private:
00160 int nr_subdiv_;
00161
00163 Eigen::VectorXf pfh_histogram_;
00164
00166 Eigen::Vector4f pfh_tuple_;
00167
00169 int f_index_[3];
00170
00172 float d_pi_;
00173 };
00174 }
00175
00176 #endif //#ifndef PCL_PFH_H_
00177
00178