36 #ifndef _polygon_primitive_offsetpolygon_CPP_
37 #define _polygon_primitive_offsetpolygon_CPP_
41 #include<CGAL/basic.h>
42 #include<CGAL/Cartesian.h>
43 #include<CGAL/Polygon_2.h>
44 #include<CGAL/Exact_predicates_inexact_constructions_kernel.h>
45 #include<CGAL/Straight_skeleton_builder_2.h>
46 #include<CGAL/Polygon_offset_builder_2.h>
47 #include<CGAL/compute_outer_frame_margin.h>
48 #include <CGAL/Cartesian.h>
49 #include <CGAL/Polygon_2.h>
52 typedef CGAL::Exact_predicates_inexact_constructions_kernel
Kernel;
59 typedef CGAL::Straight_skeleton_2<Kernel>
Ss;
66 typedef CGAL::Straight_skeleton_builder_2<SsBuilderTraits,Ss>
SsBuilder;
69 typedef CGAL::Polygon_offset_builder_2<Ss,OffsetBuilderTraits,Contour>
OffsetBuilder;
85 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_local = pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>);
86 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_local_extended = pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>);
94 pcl_ros::transformPointCloud(*pcin, *pc_local,
data.
frames.
current.transform.inverse());
97 for (
int i=0; i< (int) pc_local->size(); i++)
99 if (pc_local->points[i].z>0.01 || pc_local->points[i].z<-0.01)
111 for (
int i=0; i< (int) pc_local->size(); i++)
113 mypol.push_back(
Point_2(pc_local->points[i].x, pc_local->points[i].y));
119 if(!mypol.is_simple())
127 double prev_x=-999999, prev_y=-999999;
131 for (Contour::Vertex_const_iterator j = mypol.vertices_begin(); j != mypol.vertices_end(); ++ j )
138 Contour::Vertex_const_iterator begin = mypol.vertices_begin();
139 Contour::Vertex_const_iterator end = --mypol.vertices_end();
140 if ( begin->x() == end->x() && begin->y() == end->y())
147 for (Contour::Vertex_const_iterator j = mypol.vertices_begin(); j != mypol.vertices_end(); ++ j )
151 if (prev_x==j->x() && prev_y==j->y())
179 for (Contour::Vertex_const_iterator j = mypol.vertices_begin(); j != mypol.vertices_end(); ++ j )
185 if (mypol.is_simple()) keep_trying=0;
192 if(!mypol.is_counterclockwise_oriented())
195 mypol.reverse_orientation();
201 std::vector<Point_2> star;
205 for (Contour::Vertex_const_iterator j = mypol.vertices_begin(); j != mypol.vertices_end(); ++ j )
208 pc_local->points[t].x = j->x();
209 pc_local->points[t].y = j->y();
210 pc_local->points[t].z = 0;
218 for (
int i=0; i< (int) pc_local->size(); i++)
220 star.push_back(
Point_2(pc_local->points[i].x, pc_local->points[i].y));
234 boost::optional<double> margin = CGAL::compute_outer_frame_margin(star.begin(),star.end(), offset);
242 CGAL::Bbox_2 bbox = CGAL::bbox_2(star.begin(),star.end());
245 double fxmin = bbox.xmin() - *margin ;
246 double fxmax = bbox.xmax() + *margin ;
247 double fymin = bbox.ymin() - *margin ;
248 double fymax = bbox.ymax() + *margin ;
264 ssb.enter_contour(frame,frame+4);
269 ssb.enter_contour(star.rbegin(),star.rend());
274 boost::shared_ptr<Ss> ss = ssb.construct_skeleton();
290 ob.construct_offset_contours(offset, std::back_inserter(offset_contours));
296 ContourSequence::iterator f = offset_contours.end();
297 double lLargestArea = 0.0 ;
299 for (ContourSequence::iterator i = offset_contours.begin(); i != offset_contours.end(); ++ i )
302 double lArea = CGAL_NTS abs( (*i)->area() ) ;
303 if ( lArea > lLargestArea )
306 lLargestArea = lArea ;
312 offset_contours.erase(f);
335 for (ContourSequence::const_iterator i = offset_contours.begin(); i != offset_contours.end(); ++ i )
339 pc_local_extended->points.resize((*i)->size());
340 pc_local_extended->height=1;
341 pc_local_extended->width=(*i)->size();
344 for (Contour::Vertex_const_iterator j = (*i)->vertices_begin(); j != (*i)->vertices_end(); ++ j )
347 pc_local_extended->points[t].x = j->x();
348 pc_local_extended->points[t].y = j->y();
349 pc_local_extended->points[t].z = 0;
362 pcl_ros::transformPointCloud(*pc_local_extended, *pcout,
data.
frames.
current.transform);
367 pc_local_extended.reset();
boost::shared_ptr< Contour > ContourPtr
Ss::Halfedge_handle Halfedge_handle
pcl::ModelCoefficients::Ptr current
CGAL::Straight_skeleton_2< Kernel > Ss
CGAL::Straight_skeleton_builder_2< SsBuilderTraits, Ss > SsBuilder
CGAL::Polygon_offset_builder_2< Ss, OffsetBuilderTraits, Contour > OffsetBuilder
CGAL::Polygon_offset_builder_traits_2< Kernel > OffsetBuilderTraits
Ss::Vertex_handle Vertex_handle
struct t_polygon_primitive_data::@5 frames
the axis frames
std::vector< ContourPtr > ContourSequence
CGAL::Polygon_2< Kernel > Contour
t_polygon_primitive_data data
A class c_polygon_primitive that contains information about a detected polygon primitive as well as t...
CGAL::Exact_predicates_inexact_constructions_kernel Kernel
void offset_polygon(double val, const pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout, tf::Transform *tr)
Offsets a given polygon. The offseting is produced along the XoY plane defined by the points in pcin...
Ss::Halfedge_iterator Halfedge_iterator
CGAL::Straight_skeleton_builder_traits_2< Kernel > SsBuilderTraits