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