polygon_intersection.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
34 #ifndef _POLYGON_INTERSECTION_CPP_
35 #define _POLYGON_INTERSECTION_CPP_
36 
37 #include "polygon_intersection.h"
38 
39 int class_polygon_intersection::clear_all_polygons(void){polygons.erase(polygons.begin(), polygons.end());return 1;};
40 
41 
42 int class_polygon_intersection::add_polygon_to_list(const pcl::PointCloud<pcl::PointXYZ>::Ptr p_pc)
43 {
44  pcl::PointCloud<pcl::PointXYZ> pc;
45  pc=*p_pc;
46  return add_polygon_to_list(&pc);
47 }
48 
49 int class_polygon_intersection::add_polygon_to_list(pcl::PointCloud<pcl::PointXYZ>* pc)
50 {
51  //declare a polygon
52  CGALPolygon_2 polygon;
53 
54  //declare de point cloud in local coordinates
55  pcl::PointCloud<pcl::PointXYZ> pc_local;
56 
57  //transform the pc to local coordinates
58  transform_global_to_local(pc, &pc_local);
59 
60  for (size_t i=0; i< pc_local.points.size(); i++) //Build the CGAL polygon
61  {
62  if (!isnan(pc_local.at(i).x))
63  {
64  polygon.push_back(CGALPoint_2(
65  pc_local.at(i).x,
66  pc_local.at(i).y));
67  //printf("pc_local.at(%d).z = %3.2f\n", (int)i, pc_local.at(i).z);
68  }
69  }
70 
71  if (simplify_polygon(&polygon))
72  {
73  polygons.push_back(polygon); //Push back on the polygons vector
74  return 1;
75  }
76  else
77  {
78  ROS_ERROR("Could not add polygon. It is not simple and could not simplify");
79  return 0;
80  }
81 }
82 
83 int class_polygon_intersection::compute_polygon_intersection(pcl::PointCloud<pcl::PointXYZ>* pc_out)
84 {
85  CGALPwh_list_2 polygon_out;
86  //CGALPwh pol;
87  CGALPwh_list_2::const_iterator it;
88 
89  //CGAL::intersection (polygons[0], polygons[1], std::back_inserter(polygon_out));
90  CGAL::intersection (polygons[0], polygons[1], std::back_inserter(polygon_out));
91 
92  for (size_t k=2; k<polygons.size(); k++)
93  {
94 
97  it = polygon_out.begin();
98 
99  CGAL::intersection (*it, polygons[k], std::back_inserter(polygon_out));
100  }
101 
102  int count=0;
103  for (it = polygon_out.begin(); it != polygon_out.end(); ++it)
104  {
105  count++;
106  }
107 
108  if(count!=1)
109  {
110  ROS_ERROR("Problem in polygon interception. Output was %d polygons",count);
111  return 0;
112  }
113  else
114  {
115  it = polygon_out.begin();
116 
117  if (!(*it).is_unbounded())
118  {
119  //ROS_INFO("Polygon is bounded and has %d vertices", (*it).outer_boundary().size());
120  pcl::PointCloud<pcl::PointXYZ> pc_out_local;
121 
122  CGALPolygon_2::Vertex_const_iterator vit;
123  for (vit = (*it).outer_boundary().vertices_begin(); vit != (*it).outer_boundary().vertices_end(); ++vit)
124  {
125  pcl::PointXYZ pt;
126  pt.x = (*vit).x();
127  pt.y = (*vit).y();
128  pt.z = 0;
129  pc_out_local.push_back(pt);
130  }
131 
132  //transform the pc to global coordinates
133  transform_local_to_global(&pc_out_local, pc_out);
134  }
135 
136  }
137  return 1;}
138 
139 
140 int class_polygon_intersection::compute_polygon_union(std::vector<pcl::PointCloud<pcl::PointXYZ> > *pc_list)
141 {
142  std::vector<CGALPolygon_with_holes_2> polygon_out_list;
143 
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++)
146  {
147  printf("polygon %d has %d vertices\n", (int)i, (int)polygons[i].size());
148  }
149 
150  if (polygons.size()>0)
151  {
152  //insert the first polygon to the list
153  CGALPolygon_with_holes_2 polygon_out;
154  CGAL::join(polygons[0], polygons[0], polygon_out);
155  polygon_out_list.push_back(polygon_out);
156 
157  for (size_t i=1; i<polygons.size(); i++) //go though all polygons
158  {
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++) //go through the list
162  {
163 
164  if (CGAL::join(polygons[i], polygon_out_list[j], polygon_out_list[j])) //an union was possible
165  {
166  printf("plg %d was merged to list %d\n",(int)i,(int)j);
167  was_merged=true;
168  break;
169  }
170  else
171  {
172  printf("plg %d cannot be merged with plg list %d\n",(int)i,(int)j);
173  }
174 
175  }
176 
177  if (was_merged==false)
178  {
179  CGALPolygon_with_holes_2 polygon_out;
180  CGAL::join(polygons[i], polygons[i], polygon_out);
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());
183  }
184 
185  }
186 
188  //now try to compact the list
190  std::vector<CGALPolygon_with_holes_2> polygon_out_list1;
191 
192  printf("Will try to compact list\n");
193 
194  //insert the first polygon to the list
195  polygon_out_list1.push_back(polygon_out_list[0]);
196 
197  for (size_t i=1; i<polygon_out_list.size(); i++) //go though all polygon in polygonlist
198  {
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++) //go through the list
202  {
203 
204  if (CGAL::join(polygon_out_list[i], polygon_out_list1[j], polygon_out_list1[j])) //an union was possible
205  {
206  printf("plg %d was merged to list %d\n",(int)i,(int)j);
207  was_merged=true;
208  break;
209  }
210  else
211  {
212  printf("plg %d cannot be merged with plg list %d\n",(int)i,(int)j);
213  }
214 
215  }
216 
217  if (was_merged==false)
218  {
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());
221  }
222 
223  }
224 
225 
226  //printf("final list has %d polygons\n", (int)polygon_out_list.size());
227  for (size_t i=0; i< polygon_out_list1.size(); i++)
228  {
229  //printf("polygon_out_list %d has %d vertices and %d holes\n", (int)i, (int)polygon_out_list[i].outer_boundary().size(), (int)polygon_out_list[i].number_of_holes());
230 
231  pcl::PointCloud<pcl::PointXYZ> pc_out_local;
232  pcl::PointCloud<pcl::PointXYZ> pc_out;
233 
234  CGALPolygon_2::Vertex_const_iterator vit;
235 
236  for (vit = polygon_out_list1[i].outer_boundary().vertices_begin(); vit != polygon_out_list1[i].outer_boundary().vertices_end(); ++vit)
237  {
238  pcl::PointXYZ pt;
239  pt.x = (*vit).x();
240  pt.y = (*vit).y();
241  pt.z = 0;
242  pc_out_local.push_back(pt);
243  }
244 
246  transform_local_to_global(&pc_out_local, &pc_out);
247  pc_list->push_back(pc_out);
248  }
249 
250  }
251  else if (polygons.size()==0)
252  {
253  ROS_ERROR("No polygons given. No union can be performed");
254 
255  }
256 
257 
258  return 1;
259 }
260 
261 #endif
262 
CGAL::Polygon_2< CGALKernel > CGALPolygon_2
CGALKernel::Point_2 CGALPoint_2
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)


polygon_primitives_extraction
Author(s): Miguel Oliveira
autogenerated on Mon Mar 2 2015 01:32:42