34 #ifndef _C_POLYGON_REPRESENTATION_CPP_
35 #define _C_POLYGON_REPRESENTATION_CPP_
49 ROS_INFO(
"__________________________________");
50 ROS_INFO(
"Starting a new polygon class named %s",
polygon_name);
61 raw.
cloud = pcl::PointCloud<PointT>::Ptr(
new pcl::PointCloud<PointT>);
62 additional.
cloud = pcl::PointCloud<PointT>::Ptr(
new pcl::PointCloud<PointT>);
63 projected.
cloud = pcl::PointCloud<PointT>::Ptr(
new pcl::PointCloud<PointT>);
64 convex_hull.
cloud = pcl::PointCloud<PointT>::Ptr(
new pcl::PointCloud<PointT>);
65 coefficients = pcl::ModelCoefficients::Ptr(
new pcl::ModelCoefficients);
66 indices = pcl::PointIndices::Ptr(
new pcl::PointIndices);
70 pcl::PointCloud<PointT> *input_cloud,
71 pcl::PointCloud<pcl::Normal> *input_normals,
72 double DistanceThreshold,
73 double NormalDistanceWeight,
76 int num_original_pts = (int)input_cloud->size();
79 pcl::SACSegmentationFromNormals<PointT, pcl::Normal> segmentation;
82 segmentation.setOptimizeCoefficients(
true);
83 segmentation.setModelType(pcl::SACMODEL_NORMAL_PLANE);
84 segmentation.setMethodType (pcl::SAC_RANSAC);
85 segmentation.setMaxIterations (MaxIterations);
86 segmentation.setNormalDistanceWeight (NormalDistanceWeight);
87 segmentation.setDistanceThreshold(DistanceThreshold);
90 segmentation.setInputCloud(input_cloud->makeShared());
91 segmentation.setInputNormals(input_normals->makeShared());
94 if (
indices->indices.size () == 0)
96 ROS_ERROR(
"Could not estimate a planar model for the given dataset.");
return;
107 ROS_INFO(
"COMPUTING A PLANE CANDIDATE: candidate with %d points from an input pc with %d points",(
int)
raw.
cloud->size(), num_original_pts);
112 pcl::PointCloud<PointT> *input_cloud,
113 double DistanceThreshold,
118 pcl::SACSegmentation<PointT> segmentation;
120 segmentation.setOptimizeCoefficients(
true);
121 segmentation.setModelType(pcl::SACMODEL_PLANE);
122 segmentation.setMethodType (pcl::SAC_RANSAC);
123 segmentation.setMaxIterations (MaxIterations);
124 segmentation.setDistanceThreshold(DistanceThreshold);
126 ROS_INFO(
"Indices before %d ...",(
int)
raw.
cloud->size());
129 segmentation.setInputCloud(input_cloud->makeShared());
132 if (
indices->indices.size () == 0)
134 ROS_ERROR(
"Could not estimate a planar model for the given dataset.");
return;
void allocate_space(void)
t_polygon_point_cloud additional
pcl::PointCloud< PointT >::Ptr cloud
void compute_plane_candidate_from_pc(pcl::PointCloud< PointT > *input_cloud, pcl::PointCloud< pcl::Normal > *input_normals, double DistanceThreshold=0.5, double NormalDistanceWeight=0.5, int MaxIterations=1000)
int indices_extraction(pcl::PointIndices::Ptr ind, pcl::PointCloud< PointT > *input_cloud, pcl::PointCloud< PointT > *remove_cloud, pcl::PointCloud< PointT >::Ptr copy_cloud, pcl::PointCloud< pcl::Normal > *input_normals, pcl::PointCloud< pcl::Normal > *remove_normals)
pcl::PointIndices::Ptr indices
header for polygon representation. Defines how a polygon is stored in memory
~c_polygon_representation()
Destructor. free space of Ptr objecs.
t_polygon_point_cloud projected
ros::NodeHandle * rosnode
pcl::ModelCoefficients::Ptr coefficients
char advertising_name[1024]
t_polygon_point_cloud raw
t_polygon_point_cloud convex_hull
c_polygon_representation(const char *name, ros::NodeHandle *node)
Constructor. Allocate space for Ptr objecs.
void segment_plane_from_point_cloud(pcl::PointCloud< PointT > *input_cloud, double DistanceThreshold, int MaxIterations)
void set_names(const char *s)
set names