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 #ifndef PCL_RSD_H_
00039 #define PCL_RSD_H_
00040
00041 #include <pcl/features/feature.h>
00042
00043 namespace pcl
00044 {
00054 template <typename PointInT, typename PointNT, typename PointOutT> void
00055 computeRSD (const pcl::PointCloud<PointInT> &surface, const pcl::PointCloud<PointNT> &normals,
00056 const std::vector<int> &indices, double max_dist,
00057 int nr_subdiv, double plane_radius, PointOutT &radii);
00058
00060
00076 template <typename PointInT, typename PointNT, typename PointOutT>
00077 class RSDEstimation : 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_radius_;
00084 using Feature<PointInT, PointOutT>::search_parameter_;
00085 using Feature<PointInT, PointOutT>::surface_;
00086 using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
00087
00088 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
00089 typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn;
00090
00092 RSDEstimation () : nr_subdiv_ (5), plane_radius_ (0.2)
00093 {
00094 feature_name_ = "RadiusSurfaceDescriptor";
00095 };
00096
00100 inline void setNrSubdivisions (int nr_subdiv) { nr_subdiv_ = nr_subdiv; }
00101
00103 inline int getNrSubdivisions () { return (nr_subdiv_); }
00104
00110 inline void setPlaneRadius (double plane_radius) { plane_radius_ = plane_radius; }
00111
00113 inline double getPlaneRadius () { return (plane_radius_); }
00114
00116 inline void setKSearch (int) { ROS_ERROR ("[pcl::%s::computeFeature] RSD does not work with k nearest neighbor search. Use setRadiusSearch() instead!", getClassName ().c_str ()); }
00117
00118 protected:
00119
00125 void computeFeature (PointCloudOut &output);
00126
00127 private:
00128
00130
00131
00133 int nr_subdiv_;
00134
00136 double plane_radius_;
00137 };
00138 }
00139
00140 #endif //#ifndef PCL_RSD_H_
00141
00142