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_EXTRACT_POLYGONAL_PRISM_DATA_H_
00039 #define PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_
00040
00041 #include <pcl/pcl_base.h>
00042 #include <pcl/features/normal_3d.h>
00043 #include <pcl/sample_consensus/sac_model_plane.h>
00044
00045 namespace pcl
00046 {
00053 template <typename PointT> bool isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon);
00054
00062 template <typename PointT> bool isXYPointIn2DXYPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon);
00063
00067
00074 template <typename PointT>
00075 class ExtractPolygonalPrismData : public PCLBase<PointT>
00076 {
00077 using PCLBase<PointT>::input_;
00078 using PCLBase<PointT>::indices_;
00079 using PCLBase<PointT>::initCompute;
00080 using PCLBase<PointT>::deinitCompute;
00081
00082 public:
00083 typedef pcl::PointCloud<PointT> PointCloud;
00084 typedef typename PointCloud::Ptr PointCloudPtr;
00085 typedef typename PointCloud::ConstPtr PointCloudConstPtr;
00086
00087 typedef PointIndices::Ptr PointIndicesPtr;
00088 typedef PointIndices::ConstPtr PointIndicesConstPtr;
00089
00091 ExtractPolygonalPrismData () : min_pts_hull_ (3), height_limit_min_ (0), height_limit_max_ (FLT_MAX),
00092 vpx_ (0), vpy_ (0), vpz_ (0)
00093 {};
00094
00098 inline void setInputPlanarHull (const PointCloudConstPtr &hull) { planar_hull_ = hull; }
00099
00101 inline PointCloudConstPtr getInputPlanarHull () { return (planar_hull_); }
00102
00107 inline void
00108 setHeightLimits (double height_min, double height_max)
00109 {
00110 height_limit_min_ = height_min;
00111 height_limit_max_ = height_max;
00112 }
00113
00115 inline void
00116 getHeightLimits (double &height_min, double &height_max)
00117 {
00118 height_min = height_limit_min_;
00119 height_max = height_limit_max_;
00120 }
00121
00127 inline void
00128 setViewPoint (float vpx, float vpy, float vpz)
00129 {
00130 vpx_ = vpx;
00131 vpy_ = vpy;
00132 vpz_ = vpz;
00133 }
00134
00136 inline void
00137 getViewPoint (float &vpx, float &vpy, float &vpz)
00138 {
00139 vpx = vpx_;
00140 vpy = vpy_;
00141 vpz = vpz_;
00142 }
00143
00147 void segment (PointIndices &output);
00148
00149 protected:
00151 PointCloudConstPtr planar_hull_;
00152
00154 int min_pts_hull_;
00155
00157 double height_limit_min_;
00158
00160 double height_limit_max_;
00161
00163 float vpx_, vpy_, vpz_;
00164
00166 virtual std::string getClassName () const { return ("ExtractPolygonalPrismData"); }
00167 };
00168 }
00169
00170 #endif //#ifndef PCL_EXTRACT_POLYGONAL_PRISM_DATA_H_