dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * shot_local_estimator.h
   3  *
   4  *  Created on: Mar 24, 2012
   5  *      Author: aitor
   6  */
   7
   8 #pragma once
   9
  10 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
  11 #include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
  12 #include <pcl/features/fpfh.h>
  13
  14 namespace pcl {
  15 namespace rec_3d_framework {
  16
  17 template <typename PointInT, typename FeatureT>
  18 class FPFHLocalEstimation : public LocalEstimator<PointInT, FeatureT> {
  19
  20   using PointInTPtr = typename pcl::PointCloud<PointInT>::Ptr;
  21   using FeatureTPtr = typename pcl::PointCloud<FeatureT>::Ptr;
  22   using KeypointCloud = pcl::PointCloud<pcl::PointXYZ>;
  23
  24   using LocalEstimator<PointInT, FeatureT>::support_radius_;
  25   using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
  26   using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
  27
  28 public:
  29   bool
  30   estimate(PointInTPtr& in,
  31            PointInTPtr& processed,
  32            PointInTPtr& keypoints,
  33            FeatureTPtr& signatures) override
  34   {
  35
  36     if (!normal_estimator_) {
  37       PCL_ERROR("FPFHLocalEstimation :: This feature needs normals... "
  38                 "please provide a normal estimator\n");
  39       return false;
  40     }
  41
  42     if (keypoint_extractor_.empty()) {
  43       PCL_ERROR("FPFHLocalEstimation :: This feature needs a keypoint extractor... "
  44                 "please provide one\n");
  45       return false;
  46     }
  47
  48     // compute normals
  49     pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  50     normal_estimator_->estimate(in, processed, normals);
  51
  52     this->computeKeypoints(processed, keypoints, normals);
  53     std::cout << " " << normals->size() << " " << processed->size() << std::endl;
  54
  55     if (keypoints->points.empty()) {
  56       PCL_WARN("FPFHLocalEstimation :: No keypoints were found\n");
  57       return false;
  58     }
  59
  60     assert(processed->size() == normals->size());
  61
  62     // compute signatures
  63     using FPFHEstimator =
  64         pcl::FPFHEstimation<PointInT, pcl::Normal, pcl::FPFHSignature33>;
  65     typename pcl::search::KdTree<PointInT>::Ptr tree(new pcl::search::KdTree<PointInT>);
  66
  67     pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs(
  68         new pcl::PointCloud<pcl::FPFHSignature33>);
  69     FPFHEstimator fpfh_estimate;
  70     fpfh_estimate.setSearchMethod(tree);
  71     fpfh_estimate.setInputCloud(keypoints);
  72     fpfh_estimate.setSearchSurface(processed);
  73     fpfh_estimate.setInputNormals(normals);
  74     fpfh_estimate.setRadiusSearch(support_radius_);
  75     fpfh_estimate.compute(*fpfhs);
  76
  77     signatures->resize(fpfhs->size());
  78     signatures->width = fpfhs->size();
  79     signatures->height = 1;
  80
  81     int size_feat = 33;
  82     for (std::size_t k = 0; k < fpfhs->size(); k++)
  83       for (int i = 0; i < size_feat; i++)
  84         (*signatures)[k].histogram[i] = (*fpfhs)[k].histogram[i];
  85
  86     return true;
  87   }
  88 };
  89
  90 } // namespace rec_3d_framework
  91 } // namespace pcl