34 #ifndef _C_POLYGON_REPRESENTATION_H_
35 #define _C_POLYGON_REPRESENTATION_H_
43 #include <sensor_msgs/PointCloud2.h>
47 #include <pcl/ros/conversions.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/point_types.h>
59 #include "config_util.h"
61 #include "lcmtypes_velodyne_t.h"
62 #include "lcmtypes_pose_t.h"
63 #include "small_linalg.h"
66 #include <visualization_msgs/Marker.h>
71 #include <pcl/ModelCoefficients.h>
72 #include <pcl/io/pcd_io.h>
73 #include <pcl/point_types.h>
74 #include <pcl/sample_consensus/method_types.h>
75 #include <pcl/sample_consensus/model_types.h>
76 #include <pcl/segmentation/sac_segmentation.h>
77 #include <pcl/features/normal_3d.h>
78 #include <pcl/ModelCoefficients.h>
79 #include <pcl/io/pcd_io.h>
80 #include <pcl/point_types.h>
81 #include <pcl/features/normal_3d.h>
82 #include <pcl/filters/extract_indices.h>
83 #include <pcl/filters/passthrough.h>
84 #include <pcl/sample_consensus/method_types.h>
85 #include <pcl/sample_consensus/model_types.h>
86 #include <pcl/segmentation/sac_segmentation.h>
87 #include <pcl/filters/project_inliers.h>
89 #include <pcl/filters/extract_indices.h>
90 #include <pcl/surface/convex_hull.h>
91 #include <pcl/ModelCoefficients.h>
92 #include <pcl/point_types.h>
93 #include <pcl/io/pcd_io.h>
94 #include <pcl/features/normal_3d.h>
95 #include <pcl/filters/extract_indices.h>
96 #include <pcl/filters/voxel_grid.h>
97 #include <pcl/kdtree/kdtree.h>
98 #include <pcl/sample_consensus/method_types.h>
99 #include <pcl/sample_consensus/model_types.h>
100 #include <pcl/segmentation/sac_segmentation.h>
101 #include <pcl/segmentation/extract_clusters.h>
103 #include <pcl/segmentation/extract_polygonal_prism_data.h>
106 #include "pcl_ros/segmentation/extract_polygonal_prism_data.h"
109 #define _NUM_POLYGONS_ 5
110 #define _NUM_CLUSTERS_ 5
141 ppc->
cloud->header.frame_id =
"/sensor_frame";
142 ppc->
cloud->height = 1;
143 ppc->
cloud->is_dense=0;
155 pcl::PointCloud<PointT> *input_cloud,
156 pcl::PointCloud<PointT> *remove_cloud,
157 pcl::PointCloud<PointT>::Ptr copy_cloud,
158 pcl::PointCloud<pcl::Normal> *input_normals,
159 pcl::PointCloud<pcl::Normal> *remove_normals)
161 if (input_cloud!=NULL)
163 pcl::ExtractIndices<PointT> extract;
164 extract.setInputCloud(input_cloud->makeShared());
165 extract.setIndices(ind);
170 extract.setNegative(
false);
171 extract.filter(*copy_cloud);
175 if(remove_cloud!=NULL)
177 extract.setNegative(
true);
178 extract.filter(*remove_cloud);
182 if (input_normals!=NULL)
184 pcl::ExtractIndices<pcl::Normal> extract_normals;
185 extract_normals.setInputCloud(input_normals->makeShared());
186 extract_normals.setIndices(ind);
188 if (remove_normals!=NULL)
190 extract_normals.setNegative(
true);
191 extract_normals.filter(*remove_normals);
201 pcl::PointCloud<pcl::Normal> *input_normals,
202 double DistanceThreshold = 0.5,
203 double NormalDistanceWeight = 0.5,
204 int MaxIterations = 1000
215 pcl::ProjectInliers<pcl::PointXYZ> projection;
217 projection.setModelType(pcl::SACMODEL_NORMAL_PLANE);
218 projection.setInputCloud(
raw.
cloud->makeShared());
227 pcl::ConvexHull<pcl::PointXYZ> chull;
234 pcl::PointCloud<PointT> *input_cloud,
235 double DistanceThreshold,
242 pcl::ExtractPolygonalPrismData<PointT> epp;
243 pcl::PointCloud<PointT>::ConstPtr input_cloud_constptr;
244 input_cloud_constptr.reset (
new pcl::PointCloud<PointT> (*input_cloud));
245 pcl::PointCloud<PointT>::ConstPtr convex_hull_constptr;
246 convex_hull_constptr.reset (
new pcl::PointCloud<PointT> (*
convex_hull.
cloud));
247 pcl::PointIndices::Ptr ind = pcl::PointIndices::Ptr(
new pcl::PointIndices);
248 pcl::ExtractIndices<PointT> extract;
251 ROS_INFO(
"Extracting Convex Hull inliers");
252 ROS_INFO(
"Input cloud has %d points",(
int)input_cloud->size());
256 epp.setInputCloud(input_cloud_constptr);
257 epp.setInputPlanarHull(convex_hull_constptr);
258 epp.setHeightLimits(-0.2, 0.2);
259 epp.setViewPoint(0,0,0);
297 pcl::SACSegmentation<PointT> segmentation;
299 segmentation.setOptimizeCoefficients(
true);
300 segmentation.setModelType(pcl::SACMODEL_PLANE);
301 segmentation.setMethodType (pcl::SAC_RANSAC);
302 segmentation.setMaxIterations (100);
303 segmentation.setDistanceThreshold(333);
305 ROS_INFO(
"Indices before %d ...",(
int)
raw.
cloud->size());
308 segmentation.setInputCloud(input_cloud->makeShared());
311 if (
indices->indices.size () == 0)
313 ROS_ERROR(
"Could not estimate a planar model for the given dataset.");
return;
317 ROS_INFO(
"Removed %d points from input_cloud. Now has %d points",(
int)
additional.
cloud->size(),(int)input_cloud->size());
320 ROS_INFO(
"Extracting Convex Hull inliers done ...");
void compute_convex_hull(void)
void allocate_space(void)
void publish_projected_cloud(void)
t_polygon_point_cloud additional
sensor_msgs::PointCloud2 cloud_msg
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)
void publish_additional_cloud(void)
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
void publish_convex_hull_cloud(void)
void publish_raw_cloud(void)
void project_cloud_to_plane(pcl::PointCloud< PointT >::Ptr input_cloud)
~c_polygon_representation()
Destructor. free space of Ptr objecs.
t_polygon_point_cloud projected
void project_raw_cloud_to_plane(void)
ros::NodeHandle * rosnode
pcl::ModelCoefficients::Ptr coefficients
struct tag_t_polygon_point_cloud t_polygon_point_cloud
char advertising_name[1024]
t_polygon_point_cloud raw
void compute_convex_hull_inliers(pcl::PointCloud< PointT > *input_cloud, pcl::PointCloud< pcl::Normal > *input_normals)
t_polygon_point_cloud convex_hull
void publish_polygon_point_cloud(t_polygon_point_cloud *ppc)
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