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 namespace rec_3d_framework {
  17
  18 template <typename PointInT, typename FeatureT>
  19 class SHOTLocalEstimation : public LocalEstimator<PointInT, FeatureT> {
  20
  21   using PointInTPtr = typename pcl::PointCloud<PointInT>::Ptr;
  22   using FeatureTPtr = typename pcl::PointCloud<FeatureT>::Ptr;
  23
  24   using LocalEstimator<PointInT, FeatureT>::support_radius_;
  25   using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
  26   using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
  27   using LocalEstimator<PointInT, FeatureT>::adaptative_MLS_;
  28
  29 public:
  30   bool
  31   estimate(PointInTPtr& in,
  32            PointInTPtr& processed,
  33            PointInTPtr& keypoints,
  34            FeatureTPtr& signatures) override
  35   {
  36
  37     if (!normal_estimator_) {
  38       PCL_ERROR("SHOTLocalEstimation :: This feature needs normals... "
  39                 "please provide a normal estimator\n");
  40       return false;
  41     }
  42
  43     if (keypoint_extractor_.empty()) {
  44       PCL_ERROR("SHOTLocalEstimation :: This feature needs a keypoint extractor... "
  45                 "please provide one\n");
  46       return false;
  47     }
  48
  49     pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  50     pcl::MovingLeastSquares<PointInT, PointInT> mls;
  51     if (adaptative_MLS_) {
  52       typename search::KdTree<PointInT>::Ptr tree;
  53       Eigen::Vector4f centroid_cluster;
  54       pcl::compute3DCentroid(*in, centroid_cluster);
  55       float dist_to_sensor = centroid_cluster.norm();
  56       float sigma = dist_to_sensor * 0.01f;
  57       mls.setSearchMethod(tree);
  58       mls.setSearchRadius(sigma);
  59       mls.setUpsamplingMethod(mls.SAMPLE_LOCAL_PLANE);
  60       mls.setUpsamplingRadius(0.002);
  61       mls.setUpsamplingStepSize(0.001);
  62     }
  63
  64     normals.reset(new pcl::PointCloud<pcl::Normal>);
  65     {
  66       pcl::ScopeTime t("Compute normals");
  67       normal_estimator_->estimate(in, processed, normals);
  68     }
  69
  70     if (adaptative_MLS_) {
  71       mls.setInputCloud(processed);
  72
  73       PointInTPtr filtered(new pcl::PointCloud<PointInT>);
  74       mls.process(*filtered);
  75
  76       processed.reset(new pcl::PointCloud<PointInT>);
  77       normals.reset(new pcl::PointCloud<pcl::Normal>);
  78       {
  79         pcl::ScopeTime t("Compute normals after MLS");
  80         filtered->is_dense = false;
  81         normal_estimator_->estimate(filtered, processed, normals);
  82       }
  83     }
  84
  85     // compute keypoints
  86     this->computeKeypoints(processed, keypoints, normals);
  87     std::cout << " " << normals->size() << " " << processed->size() << std::endl;
  88
  89     if (keypoints->points.empty()) {
  90       PCL_WARN("SHOTLocalEstimation :: No keypoints were found\n");
  91       return false;
  92     }
  93
  94     std::cout << keypoints->size() << " " << normals->size() << " " << processed->size()
  95               << std::endl;
  96     // compute signatures
  97     using SHOTEstimator = pcl::SHOTEstimation<PointInT, pcl::Normal, pcl::SHOT352>;
  98     typename pcl::search::KdTree<PointInT>::Ptr tree(new pcl::search::KdTree<PointInT>);
  99
 100     pcl::PointCloud<pcl::SHOT352>::Ptr shots(new pcl::PointCloud<pcl::SHOT352>);
 101
 102     SHOTEstimator shot_estimate;
 103     shot_estimate.setSearchMethod(tree);
 104     shot_estimate.setInputCloud(keypoints);
 105     shot_estimate.setSearchSurface(processed);
 106     shot_estimate.setInputNormals(normals);
 107     shot_estimate.setRadiusSearch(support_radius_);
 108     shot_estimate.compute(*shots);
 109     signatures->resize(shots->size());
 110     signatures->width = shots->size();
 111     signatures->height = 1;
 112
 113     int size_feat = sizeof((*signatures)[0].histogram) / sizeof(float);
 114
 115     for (std::size_t k = 0; k < shots->size(); k++)
 116       for (int i = 0; i < size_feat; i++)
 117         (*signatures)[k].histogram[i] = (*shots)[k].descriptor[i];
 118
 119     return true;
 120   }
 121 };
 122
 123 } // namespace rec_3d_framework
 124 } // namespace pcl