34 #ifndef _POLYGON_INTERSECTION_CPP_
35 #define _POLYGON_INTERSECTION_CPP_
44 pcl::PointCloud<pcl::PointXYZ> pc;
55 pcl::PointCloud<pcl::PointXYZ> pc_local;
58 transform_global_to_local(pc, &pc_local);
60 for (
size_t i=0; i< pc_local.points.size(); i++)
62 if (!isnan(pc_local.at(i).x))
71 if (simplify_polygon(&polygon))
78 ROS_ERROR(
"Could not add polygon. It is not simple and could not simplify");
87 CGALPwh_list_2::const_iterator it;
90 CGAL::intersection (
polygons[0],
polygons[1], std::back_inserter(polygon_out));
92 for (
size_t k=2; k<
polygons.size(); k++)
97 it = polygon_out.begin();
99 CGAL::intersection (*it,
polygons[k], std::back_inserter(polygon_out));
103 for (it = polygon_out.begin(); it != polygon_out.end(); ++it)
110 ROS_ERROR(
"Problem in polygon interception. Output was %d polygons",count);
115 it = polygon_out.begin();
117 if (!(*it).is_unbounded())
120 pcl::PointCloud<pcl::PointXYZ> pc_out_local;
122 CGALPolygon_2::Vertex_const_iterator vit;
123 for (vit = (*it).outer_boundary().vertices_begin(); vit != (*it).outer_boundary().vertices_end(); ++vit)
129 pc_out_local.push_back(pt);
133 transform_local_to_global(&pc_out_local, pc_out);
142 std::vector<CGALPolygon_with_holes_2> polygon_out_list;
144 printf(
"polygon union: i will do union of %d polygons\n", (
int)
polygons.size());
145 for (
size_t i=0; i<
polygons.size(); i++)
147 printf(
"polygon %d has %d vertices\n", (
int)i, (
int)
polygons[i].size());
155 polygon_out_list.push_back(polygon_out);
157 for (
size_t i=1; i<
polygons.size(); i++)
159 printf(
"Computing for plg %d\n", (
int)i);
160 bool was_merged=
false;
161 for (
size_t j=0; j<polygon_out_list.size(); j++)
164 if (CGAL::join(
polygons[i], polygon_out_list[j], polygon_out_list[j]))
166 printf(
"plg %d was merged to list %d\n",(
int)i,(
int)j);
172 printf(
"plg %d cannot be merged with plg list %d\n",(
int)i,(
int)j);
177 if (was_merged==
false)
181 polygon_out_list.push_back(polygon_out);
182 printf(
"Adding plg %d to list. Now has %d size\n",(
int)i, (
int)polygon_out_list.size());
190 std::vector<CGALPolygon_with_holes_2> polygon_out_list1;
192 printf(
"Will try to compact list\n");
195 polygon_out_list1.push_back(polygon_out_list[0]);
197 for (
size_t i=1; i<polygon_out_list.size(); i++)
199 printf(
"Computing for plg %d\n", (
int)i);
200 bool was_merged=
false;
201 for (
size_t j=0; j<polygon_out_list1.size(); j++)
204 if (CGAL::join(polygon_out_list[i], polygon_out_list1[j], polygon_out_list1[j]))
206 printf(
"plg %d was merged to list %d\n",(
int)i,(
int)j);
212 printf(
"plg %d cannot be merged with plg list %d\n",(
int)i,(
int)j);
217 if (was_merged==
false)
219 polygon_out_list1.push_back(polygon_out_list[i]);
220 printf(
"Adding plg %d to list. Now has %d size\n",(
int)i, (
int)polygon_out_list1.size());
227 for (
size_t i=0; i< polygon_out_list1.size(); i++)
231 pcl::PointCloud<pcl::PointXYZ> pc_out_local;
232 pcl::PointCloud<pcl::PointXYZ> pc_out;
234 CGALPolygon_2::Vertex_const_iterator vit;
236 for (vit = polygon_out_list1[i].outer_boundary().vertices_begin(); vit != polygon_out_list1[i].outer_boundary().vertices_end(); ++vit)
242 pc_out_local.push_back(pt);
246 transform_local_to_global(&pc_out_local, &pc_out);
247 pc_list->push_back(pc_out);
253 ROS_ERROR(
"No polygons given. No union can be performed");
CGAL::Polygon_2< CGALKernel > CGALPolygon_2
CGALKernel::Point_2 CGALPoint_2
int clear_all_polygons(void)
int add_polygon_to_list(pcl::PointCloud< pcl::PointXYZ >::Ptr p_pc)
CGAL::Polygon_with_holes_2< CGALKernel > CGALPolygon_with_holes_2
this is the header for the main code that performs geometric polygonal primitives extraction ...
std::vector< CGALPolygon_2 > polygons
std::list< CGALPolygon_with_holes_2 > CGALPwh_list_2
int compute_polygon_union(std::vector< pcl::PointCloud< pcl::PointXYZ > > *pc_list)
int compute_polygon_intersection(pcl::PointCloud< pcl::PointXYZ > *pc_out)