27 #ifndef _points_from_volume_lib_H_
28 #define _points_from_volume_lib_H_
43 #include <sensor_msgs/PointCloud2.h>
44 #include <pcl_ros/transforms.h>
45 #include <pcl/conversions.h>
46 #include <pcl/point_cloud.h>
47 #include <pcl/point_types.h>
48 #include <pcl/segmentation/extract_polygonal_prism_data.h>
49 #include <pcl/filters/extract_indices.h>
50 #include <pcl/filters/project_inliers.h>
51 #include <pcl_conversions/pcl_conversions.h>
54 #define _CONVEX_LIB_DEBUG_ 0
66 flag_convexhull_set=
false;
71 #if _CONVEX_LIB_DEBUG_
72 printf(
"Destructor...\n");
86 convex_hull.header.frame_id = pc_in.header.frame_id;
87 #if _CONVEX_LIB_DEBUG_
88 cout<<
"pc_in frame id on set_convex_hul -> "<<pc_in.header.frame_id<<endl;
90 flag_convexhull_set=
true;
102 pcl::PointCloud<T> pc_in;
103 pcl::PCLPointCloud2 pcl_pc;
104 pcl_conversions::toPCL(*pcmsg_in, pcl_pc);
105 pcl::fromPCLPointCloud2(pcl_pc, pc_in);;
106 return set_convex_hull(pc_in);
131 if(!flag_convexhull_set)
133 ROS_ERROR(
"Flag not set to TRUE");
137 if(pc_in.header.frame_id!=convex_hull.header.frame_id)
139 #if _CONVEX_LIB_DEBUG_
140 cout<<
"convex_hull frame id "<<convex_hull.header.frame_id<<endl;
141 cout<<
"pc_in frame id on convexhull_function"<<pc_in.header.frame_id<<endl;
143 ROS_ERROR(
"Non-matching frame id's");
147 pcl::ExtractPolygonalPrismData<T> epp;
148 typename pcl::PointCloud<T>::ConstPtr input_cloud_constptr;
149 input_cloud_constptr.reset (
new pcl::PointCloud<T> (pc_in));
150 typename pcl::PointCloud<T>::ConstPtr convex_hull_constptr;
151 convex_hull_constptr.reset (
new pcl::PointCloud<T> (convex_hull));
153 pcl::ExtractIndices<T> extract;
154 pcl::PointIndices::Ptr indices;
156 indices = pcl::PointIndices::Ptr(
new pcl::PointIndices);
158 pc_in_volume.header.frame_id=pc_in.header.frame_id;
161 epp.setInputCloud(input_cloud_constptr);
162 epp.setInputPlanarHull(convex_hull_constptr);
163 epp.setHeightLimits(negative_offset,positive_offset);
164 epp.setViewPoint(0,0,0);
165 epp.segment(*indices);
167 pc_in_volume.points.clear();
168 pc_in_volume.height=1;
169 pc_in_volume.width=pc_in_volume.points.size();
170 if ((
int)indices->indices.size()!=0)
172 extract.setInputCloud(pc_in.makeShared());
173 extract.setIndices(indices);
174 extract.setNegative(flag_in_out);
175 extract.filter(pc_in_volume);
187 input_cloud_constptr.reset();
188 convex_hull_constptr.reset();
pcl::PointCloud< T > convex_hull
t_func_output convexhull_function(pcl::PointCloud< T > &pc_in, double positive_offset, double negative_offset, bool flag_in_out)
Extract points from the pcl that are inside the convexhull.
t_func_output set_convex_hull(pcl::PointCloud< T > &pc_in)
Set the convexhull.
pcl::PointCloud< T > get_pc_in_volume()
Points in the desired volume.
t_func_output set_convex_hull(const sensor_msgs::PointCloud2ConstPtr &pcmsg_in)
Set the convexhull.
pcl::PointCloud< T > pc_in_volume