Go to the documentation of this file.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 #ifndef _plane_segmentation_pcl_rgb_alg_h_
00026 #define _plane_segmentation_pcl_rgb_alg_h_
00027
00028 #include <iri_plane_segmentation_pcl_rgb/PlaneSegmentationPclRgbConfig.h>
00029 #include "mutex.h"
00030
00031
00032
00033 #include <pcl/point_cloud.h>
00034
00035
00036 #include <pcl/io/pcd_io.h>
00037 #include <pcl/ModelCoefficients.h>
00038 #include <pcl/segmentation/sac_segmentation.h>
00039 #include <pcl/sample_consensus/method_types.h>
00040 #include <pcl/filters/project_inliers.h>
00041 #include <pcl/filters/extract_indices.h>
00042
00043
00044 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00045 #include <pcl/surface/convex_hull.h>
00046
00047
00048 #include <pcl/segmentation/extract_clusters.h>
00049
00050
00051
00052 #include <pcl/filters/voxel_grid.h>
00053
00054
00055 #include <pcl/filters/passthrough.h>
00056
00057
00063 class PlaneSegmentationPclRgbAlgorithm
00064 {
00065 protected:
00072 CMutex alg_mutex_;
00073
00074
00075 private:
00076
00077 bool pointcloud_downsample;
00078 double pointcloud_downsample_size;
00079
00080
00081 bool choose_plane_by_distance;
00082 bool choose_nearest_plane;
00083
00084
00085 double plane_size_thresh;
00086 double plane_min_height;
00087 double plane_max_height;
00088 int plane_segm_iterations;
00089 double plane_segm_probability;
00090
00091
00092 bool plane_clustering;
00093 int plane_min_cluster_size;
00094 double plane_min_cluster_distance;
00095
00096
00097
00098
00099 public:
00106 typedef iri_plane_segmentation_pcl_rgb::PlaneSegmentationPclRgbConfig Config;
00107
00114 Config config_;
00115
00124 PlaneSegmentationPclRgbAlgorithm(void);
00125
00131 void lock(void) { alg_mutex_.enter(); };
00132
00138 void unlock(void) { alg_mutex_.exit(); };
00139
00147 bool try_enter(void) { return alg_mutex_.try_enter(); };
00148
00160 void config_update(Config& new_cfg, uint32_t level=0);
00161
00162
00163
00164
00171 ~PlaneSegmentationPclRgbAlgorithm(void);
00172
00173
00174
00175
00181 pcl::PointCloud<pcl::PointXYZRGB>::Ptr segmentBiggestPlane (pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb_orig,
00182 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_orig,
00183 pcl::ModelCoefficients::Ptr coefficients);
00184
00185
00192 void getBiggestPlaneInliersDownsampling(pcl::PointIndices::Ptr inliers,
00193 pcl::ModelCoefficients::Ptr coefficients,
00194 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
00195
00201 void getBiggestPlaneInliers(pcl::PointIndices::Ptr inliers,
00202 pcl::ModelCoefficients::Ptr coefficients,
00203 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud);
00204
00205 void getNearestBigPlaneInliers(pcl::PointIndices::Ptr inliers,
00206 pcl::ModelCoefficients::Ptr coefficients,
00207 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_orig);
00208
00214 pcl::PointCloud<pcl::PointXYZ>::Ptr getBiggestClusterPC (pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_orig);
00215
00222 void fixPlaneCoefficientsOrientation(pcl::ModelCoefficients::Ptr coefs);
00223
00224
00225
00226 inline double
00227 pointToPlaneDistance (const pcl::PointXYZ &p, const std::vector< float > &plane_coefficients)
00228 {
00229 return ( fabs (pointToPlaneDistanceSigned (p, plane_coefficients)) );
00230 }
00231
00232 inline double
00233 pointToPlaneDistanceSigned (const pcl::PointXYZ &p, const std::vector< float > &plane_coefficients)
00234 {
00235 return ( plane_coefficients[0] * p.x + plane_coefficients[1] * p.y + plane_coefficients[2] * p.z + plane_coefficients[3] );
00236 }
00237 };
00238
00239 #endif