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/shot.h>
  13 #include <pcl/io/pcd_io.h>
  14
  15 namespace pcl
  16 {
  17   namespace rec_3d_framework
  18   {
  19     template<typename PointInT, typename FeatureT>
  20       class ColorSHOTLocalEstimation : public LocalEstimator<PointInT, FeatureT>
  21       {
  22
  23         using PointInTPtr = typename pcl::PointCloud<PointInT>::Ptr;
  24         using FeatureTPtr = typename pcl::PointCloud<FeatureT>::Ptr;
  25
  26         using LocalEstimator<PointInT, FeatureT>::support_radius_;
  27         using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
  28         using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
  29
  30       public:
  31         bool
  32         estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)
  33         {
  34
  35           if (!normal_estimator_)
  36           {
  37             PCL_ERROR("SHOTLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
  38             return false;
  39           }
  40
  41           if (keypoint_extractor_.size() == 0)
  42           {
  43             PCL_ERROR("SHOTLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
  44             return false;
  45           }
  46
  47           pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  48           normals.reset (new pcl::PointCloud<pcl::Normal>);
  49           {
  50             pcl::ScopeTime t ("Compute normals");
  51             normal_estimator_->estimate (in, processed, normals);
  52           }
  53
  54           //compute keypoints
  55           computeKeypoints(processed, keypoints, normals);
  56
  57           if (keypoints->points.size () == 0)
  58           {
  59             PCL_WARN("ColorSHOTLocalEstimation :: No keypoints were found\n");
  60             return false;
  61           }
  62
  63           //compute signatures
  64           using SHOTEstimator = pcl::SHOTColorEstimation<PointInT, pcl::Normal, pcl::SHOT1344>;
  65           typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
  66
  67           pcl::PointCloud<pcl::SHOT1344>::Ptr shots (new pcl::PointCloud<pcl::SHOT1344>);
  68           SHOTEstimator shot_estimate;
  69           shot_estimate.setSearchMethod (tree);
  70           shot_estimate.setInputNormals (normals);
  71           shot_estimate.setInputCloud (keypoints);
  72           shot_estimate.setSearchSurface(processed);
  73           shot_estimate.setRadiusSearch (support_radius_);
  74           shot_estimate.compute (*shots);
  75           signatures->resize (shots->points.size ());
  76           signatures->width = static_cast<int> (shots->points.size ());
  77           signatures->height = 1;
  78
  79           int size_feat = sizeof(signatures->points[0].histogram) / sizeof(float);
  80
  81           for (std::size_t k = 0; k < shots->points.size (); k++)
  82             for (int i = 0; i < size_feat; i++)
  83               signatures->points[k].histogram[i] = shots->points[k].descriptor[i];
  84
  85           return true;
  86
  87         }
  88
  89       };
  90   }
  91 }