34 #ifndef _polygon_boolean_operations_CPP_
35 #define _polygon_boolean_operations_CPP_
45 pcl::PointCloud<pcl::PointXYZ> pc;
55 pcl::PointCloud<pcl::PointXYZ> pc_local;
60 for (
size_t i=0; i< pc_local.points.size(); i++)
62 if (!isnan(pc_local.at(i).x))
85 ROS_ERROR(
"Could not insert polygon. Input polygon is not simple and could not simplify");
103 ROS_ERROR(
"Could not join polygon. Input polygon is not simple and could not simplify");
116 S.intersection(polygon);
121 ROS_ERROR(
"Could not intersect polygon. Input polygon is not simple and could not simplify");
136 std::list<PBO_Polygon_with_holes_2> res;
137 std::list<PBO_Polygon_with_holes_2>::const_iterator it;
139 std::cout <<
"The result contains " <<
S.number_of_polygons_with_holes() <<
" components:" << std::endl;
141 S.polygons_with_holes (std::back_inserter (res));
143 for (it = res.begin(); it != res.end(); ++it)
145 pcl::PointCloud<pcl::PointXYZ> pc_out_local;
147 PS_CGALPolygon_2::Vertex_const_iterator vit;
148 for (vit = (*it).outer_boundary().vertices_begin(); vit != (*it).outer_boundary().vertices_end(); ++vit)
151 pt.x = CGAL::to_double((*vit).x());
152 pt.y = CGAL::to_double((*vit).y());
154 pc_out_local.push_back(pt);
163 if (pc_out_local.points.size() > pc_out->points.size())
179 std::list<PBO_Polygon_with_holes_2> res;
180 std::list<PBO_Polygon_with_holes_2>::const_iterator it;
182 pc_out_vector->erase(pc_out_vector->begin(), pc_out_vector->end());
184 std::cout <<
"The result contains " <<
S.number_of_polygons_with_holes() <<
" components:" << std::endl;
186 S.polygons_with_holes (std::back_inserter (res));
189 for (it = res.begin(); it != res.end(); ++it)
192 pcl::PointCloud<pcl::PointXYZ> pc_out_local;
194 ROS_INFO(
"polygon %d is simple %d",k,(*it).outer_boundary().is_simple());
197 PS_CGALPolygon_2::Vertex_const_iterator vit;
198 for (vit = (*it).outer_boundary().vertices_begin(); vit != (*it).outer_boundary().vertices_end(); ++vit)
201 pt.x = CGAL::to_double((*vit).x());
202 pt.y = CGAL::to_double((*vit).y());
204 pc_out_local.push_back(pt);
208 pcl::PointCloud<pcl::PointXYZ> pc_out;
211 pc_out_vector->push_back(pc_out);
218 std::list<PBO_Polygon_with_holes_2> res;
219 std::list<PBO_Polygon_with_holes_2>::const_iterator it;
221 std::cout <<
"The result contains " <<
S.number_of_polygons_with_holes() <<
" components:" << std::endl;
223 S.polygons_with_holes (std::back_inserter (res));
225 for (it = res.begin(); it != res.end(); ++it)
227 pcl::PointCloud<pcl::PointXYZ> pc_out_local;
229 PS_CGALPolygon_2::Vertex_const_iterator vit;
230 for (vit = (*it).outer_boundary().vertices_begin(); vit != (*it).outer_boundary().vertices_end(); ++vit)
233 pt.x = CGAL::to_double((*vit).x());
234 pt.y = CGAL::to_double((*vit).y());
236 pc_out_local.push_back(pt);
253 std::list<PBO_Polygon_with_holes_2> res;
254 std::list<PBO_Polygon_with_holes_2>::const_iterator it;
256 std::cout <<
"The result contains " <<
S.number_of_polygons_with_holes() <<
" components:" << std::endl;
258 S.polygons_with_holes (std::back_inserter (res));
260 for (it = res.begin(); it != res.end(); ++it)
int get_all_pcls(std::vector< pcl::PointCloud< pcl::PointXYZ > > *pc_out_vector)
class_polygon_boolean_operations(void)
int insert(pcl::PointCloud< pcl::PointXYZ >::Ptr p_pc)
PS_CGALKernel::Point_2 PS_CGALPoint_2
PBO_Polygon_2 from_pcl_to_cgalpolygon(pcl::PointCloud< pcl::PointXYZ > *pc)
int get_largest_pcl(pcl::PointCloud< pcl::PointXYZ > *pc_out)
int simplify_polygon(PS_CGALPolygon_2 *p)
int join(pcl::PointCloud< pcl::PointXYZ > *pc)
int intersection(pcl::PointCloud< pcl::PointXYZ > *pc)
CGAL::Polygon_2< PS_CGALKernel > PS_CGALPolygon_2
~class_polygon_boolean_operations(void)
int get_first_pcl(pcl::PointCloud< pcl::PointXYZ > *pc_out)
CGAL::Polygon_2< PBO_Kernel > PBO_Polygon_2
void print_polygon_with_holes(const CGAL::Polygon_with_holes_2< Kernel, Container > &pwh)
Defines the class bo_boolean operations *.
void print_polygon(const CGAL::Polygon_2< Kernel, Container > &P)