00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef PCL_FEATURES_3DSC_H_
00041 #define PCL_FEATURES_3DSC_H_
00042
00043 #include <pcl/point_types.h>
00044 #include <pcl/features/feature.h>
00045 #include <boost/random.hpp>
00046
00047 namespace pcl
00048 {
00076 template <typename PointInT, typename PointNT, typename PointOutT = pcl::ShapeContext>
00077 class ShapeContext3DEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
00078 {
00079 public:
00080 using Feature<PointInT, PointOutT>::feature_name_;
00081 using Feature<PointInT, PointOutT>::getClassName;
00082 using Feature<PointInT, PointOutT>::indices_;
00083 using Feature<PointInT, PointOutT>::search_parameter_;
00084 using Feature<PointInT, PointOutT>::search_radius_;
00085 using Feature<PointInT, PointOutT>::surface_;
00086 using Feature<PointInT, PointOutT>::input_;
00087 using Feature<PointInT, PointOutT>::searchForNeighbors;
00088 using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00089
00090 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00091 typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn;
00092
00097 ShapeContext3DEstimation (bool random = false) :
00098 radii_interval_(0),
00099 theta_divisions_(0),
00100 phi_divisions_(0),
00101 volume_lut_(0),
00102 azimuth_bins_(12),
00103 elevation_bins_(11),
00104 radius_bins_(15),
00105 min_radius_(0.1),
00106 point_density_radius_(0.2),
00107 descriptor_length_ (),
00108 rng_alg_ (),
00109 rng_ (new boost::uniform_01<boost::mt19937> (rng_alg_))
00110 {
00111 feature_name_ = "ShapeContext3DEstimation";
00112 search_radius_ = 2.5;
00113
00114
00115 if (random)
00116 rng_->base ().seed (static_cast<unsigned> (std::time(0)));
00117 else
00118 rng_->base ().seed (12345u);
00119 }
00120
00121 virtual ~ShapeContext3DEstimation() {}
00122
00126 inline void
00127 setAzimuthBins (size_t bins) { azimuth_bins_ = bins; }
00128
00130 inline size_t
00131 getAzimuthBins () { return (azimuth_bins_); }
00132
00136 inline void
00137 setElevationBins (size_t bins) { elevation_bins_ = bins; }
00138
00140 inline size_t
00141 getElevationBins () { return (elevation_bins_); }
00142
00146 inline void
00147 setRadiusBins (size_t bins) { radius_bins_ = bins; }
00148
00150 inline size_t
00151 getRadiusBins () { return (radius_bins_); }
00152
00156 inline void
00157 setMinimalRadius (double radius) { min_radius_ = radius; }
00158
00160 inline double
00161 getMinimalRadius () { return (min_radius_); }
00162
00167 inline void
00168 setPointDensityRadius (double radius) { point_density_radius_ = radius; }
00169
00171 inline double
00172 getPointDensityRadius () { return (point_density_radius_); }
00173
00174 protected:
00176 bool
00177 initCompute ();
00178
00187 bool
00188 computePoint (size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc);
00189
00193 void
00194 computeFeature (PointCloudOut &output);
00195
00197 std::vector<float> radii_interval_;
00198
00200 std::vector<float> theta_divisions_;
00201
00203 std::vector<float> phi_divisions_;
00204
00206 std::vector<float> volume_lut_;
00207
00209 size_t azimuth_bins_;
00210
00212 size_t elevation_bins_;
00213
00215 size_t radius_bins_;
00216
00218 double min_radius_;
00219
00221 double point_density_radius_;
00222
00224 size_t descriptor_length_;
00225
00227 boost::mt19937 rng_alg_;
00228
00230 boost::shared_ptr<boost::uniform_01<boost::mt19937> > rng_;
00231
00237 void
00238 shiftAlongAzimuth (size_t block_size, std::vector<float>& desc);
00239
00241 inline double
00242 rnd ()
00243 {
00244 return ((*rng_) ());
00245 }
00246 private:
00250 void
00251 computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &) {}
00252 };
00253
00279 template <typename PointInT, typename PointNT>
00280 class ShapeContext3DEstimation<PointInT, PointNT, Eigen::MatrixXf> : public ShapeContext3DEstimation<PointInT, PointNT, pcl::SHOT>
00281 {
00282 public:
00283 using ShapeContext3DEstimation<PointInT, PointNT, pcl::SHOT>::feature_name_;
00284 using ShapeContext3DEstimation<PointInT, PointNT, pcl::SHOT>::indices_;
00285 using ShapeContext3DEstimation<PointInT, PointNT, pcl::SHOT>::descriptor_length_;
00286 using ShapeContext3DEstimation<PointInT, PointNT, pcl::SHOT>::normals_;
00287 using ShapeContext3DEstimation<PointInT, PointNT, pcl::SHOT>::input_;
00288 using ShapeContext3DEstimation<PointInT, PointNT, pcl::SHOT>::compute;
00289
00290 private:
00294 void
00295 computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output);
00296
00300 void
00301 compute (pcl::PointCloud<pcl::SHOT> &) {}
00302 };
00303 }
00304
00305 #endif //#ifndef PCL_3DSC_H_