fast_pfh.h
Go to the documentation of this file.
00001 
00059 /*
00060  * fast_pfh.h
00061  *
00062  *  Created on: Nov 14, 2011
00063  *      Author: goa-jh
00064  */
00065 
00066 #ifndef FAST_PFH_H_
00067 #define FAST_PFH_H_
00068 
00069 #include "../feature_container.h"
00070 #include <pcl/features/fpfh.h>
00071 #ifdef GICP_ENABLE
00072 #include <pcl/search/kdtree.h>
00073 #include <pcl/kdtree/kdtree_flann.h>
00074 #else
00075 #include <pcl/kdtree/kdtree.h>
00076 #endif
00077 
00078 
00079 namespace cob_3d_registration {
00080 
00081 template<typename Point>
00082 class Feature_FPFH : public FeatureContainerInterface_Euclidean<Point>
00083 {
00084   float radius_;
00085 
00086   pcl::PointCloud<pcl::FPFHSignature33> feature_in_, feature_out_;
00087 
00088 public:
00089   Feature_FPFH():
00090     radius_(0.1)
00091   {
00092   }
00093 
00094   void setFPFHRadius(float v) {radius_ = v;}
00095 
00096   virtual bool hidden_build() {
00097 
00098     ROS_INFO("calc fpfh for source");
00099     {
00100 #ifdef GICP_ENABLE
00101       boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree <Point>);
00102 #else
00103           #ifdef PCL_VERSION_COMPARE
00104                 boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree<Point>);
00105           #else
00106         boost::shared_ptr<pcl::KdTree<Point> > tree (new pcl::KdTreeFLANN<Point>);
00107           #endif
00108 #endif
00109       pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
00110 
00111       pcl::NormalEstimation<Point, pcl::Normal> ne;
00112       ne.setSearchMethod (tree);
00113       ne.setInputCloud (this->org_in_.makeShared());
00114       // Use all neighbors in a sphere of radius 3cm
00115       ne.setRadiusSearch (0.05);
00116 
00117       // Compute the features
00118       ne.compute (*normals);
00119 
00120       // Create the FPFH estimation class, and pass the input dataset+normals to it
00121       pcl::FPFHEstimation<Point, pcl::Normal, pcl::FPFHSignature33> fpfh;
00122       fpfh.setInputCloud (this->org_in_.makeShared());
00123       fpfh.setInputNormals (normals);
00124 
00125       fpfh.setSearchMethod (tree);
00126       fpfh.setRadiusSearch (radius_);
00127 
00128       // Compute the features
00129       fpfh.compute (feature_in_);
00130     }
00131 
00132     ROS_INFO("calc fpfh for target");
00133     {
00134 #ifdef PCL_VERSION_COMPARE
00135   #ifdef GICP_ENABLE
00136       boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree<Point>);
00137 #  else
00138       boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::search::KdTree<Point>);
00139   #endif
00140 #else
00141   #ifdef GICP_ENABLE
00142       boost::shared_ptr<pcl::search::KdTree<Point> > tree (new pcl::KdTreeFLANN<Point>);
00143   #else
00144       boost::shared_ptr<pcl::KdTree<Point> > tree (new pcl::KdTreeFLANN<Point>);
00145   #endif
00146 #endif
00147       pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal> ());
00148 
00149       pcl::NormalEstimation<Point, pcl::Normal> ne;
00150       ne.setSearchMethod (tree);
00151       ne.setInputCloud (this->org_out_.makeShared());
00152       // Use all neighbors in a sphere of radius 3cm
00153       ne.setRadiusSearch (0.1);
00154 
00155       // Compute the features
00156       ne.compute (*normals);
00157 
00158       // Create the FPFH estimation class, and pass the input dataset+normals to it
00159       pcl::FPFHEstimation<Point, pcl::Normal, pcl::FPFHSignature33> fpfh;
00160       fpfh.setInputCloud (this->org_out_.makeShared());
00161       fpfh.setInputNormals (normals);
00162 
00163       fpfh.setSearchMethod (tree);
00164       fpfh.setRadiusSearch (radius_);
00165 
00166       // Compute the features
00167       fpfh.compute (feature_out_);
00168     }
00169 
00170     return true;
00171   }
00172 
00173   virtual Eigen::VectorXf getFeatureOut(const int index) {
00174     Eigen::VectorXf b;
00175     b.resize(33);
00176     for(int i=0; i<33; i++) b(i) = feature_out_.points[index].histogram[i];
00177     return b;
00178   }
00179   virtual Eigen::VectorXf getFeatureIn(const int index) {
00180     Eigen::VectorXf b;
00181     b.resize(33);
00182     for(int i=0; i<33; i++) b(i) = feature_in_.points[index].histogram[i];
00183     return b;
00184   }
00185 
00186 };
00187 
00188 }
00189 
00190 #endif /* FAST_PFH_H_ */


cob_3d_registration
Author(s): Joshua Hampp
autogenerated on Wed Aug 26 2015 11:02:36