36 #ifndef _polygon_primitive_2dhulls_CPP_
37 #define _polygon_primitive_2dhulls_CPP_
42 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
43 #include <CGAL/convex_hull_2.h>
44 #include<CGAL/Polygon_2.h>
59 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_projected = pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>);
65 pcl::ConvexHull<pcl::PointXYZ> chull;
66 chull.setInputCloud(pc_projected);
67 chull.reconstruct(*pcout);
68 pcout->width = pcin->width;
69 pcout->height = pcin->height;
70 pcout->header.frame_id = pcin->header.frame_id;
92 void c_polygon_primitive::compute_convex_hull_cgal(
const pcl::PointCloud<pcl::PointXYZ>::Ptr pcin,
const pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud<pcl::PointXYZ>::Ptr pcout, tf::Transform *tr,
double &area,
double &solidity,
bool flg)
98 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_projected = pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>);
99 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_local = pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>);
100 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_ch_local = pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>);
113 pcl_ros::transformPointCloud(*pc_projected, *pc_local,
data.
frames.
current.transform.inverse());
130 std::vector<CGAL::Exact_predicates_inexact_constructions_kernel::Point_2> pts;
133 for (
int i=0; i< (int) pc_local->size(); i++)
139 std::vector<CGAL::Exact_predicates_inexact_constructions_kernel::Point_2> pts_ch;
147 CGAL::convex_hull_2( pts.begin(), pts.end(),std::back_inserter(pts_ch));
151 for (
int t = 0; t<(int)pts_ch.size(); t++ )
153 pc_ch_local->points.push_back(pcl::PointXYZ(pts_ch[t].x(), pts_ch[t].y(),0.0));
163 pcl_ros::transformPointCloud(*pc_ch_local, *pcout,
data.
frames.
current.transform);
165 pc_projected.reset();
180 CGAL::Polygon_2<CGAL::Exact_predicates_inexact_constructions_kernel> star;
184 for (
int i=0; i< (int) pcin->size(); i++)
191 return fabs(star.area());
204 pcl::PointCloud<pcl::PointXYZ>::Ptr pctmp (
new pcl::PointCloud<pcl::PointXYZ>);
205 pcl::PointCloud<pcl::PointXYZ>::Ptr voronoi_centers (
new pcl::PointCloud<pcl::PointXYZ>);
207 pcl::PointCloud<pcl::PointXYZ>::Ptr pc_projected = pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>);
213 std::vector< pcl::Vertices > V;
214 pcl::ConcaveHull<pcl::PointXYZ> chull;
215 chull.setInputCloud(pc_projected);
220 chull.reconstruct(*pctmp, V);
221 pctmp->width = pcin->width;
222 pctmp->header.frame_id = pcin->header.frame_id;
233 pcout->points.erase(pcout->points.begin(), pcout->points.end());
237 for (std::vector<pcl::Vertices>::iterator vit=V.begin(); vit != V.end(); vit++)
240 for (std::vector<uint32_t>::iterator it=V[v].vertices.begin(); it != V[v].vertices.end(); it++)
243 p.x = pctmp->points[V[v].vertices[i]].x;
244 p.y = pctmp->points[V[v].vertices[i]].y;
245 p.z = pctmp->points[V[v].vertices[i]].z;
247 pcout->points.push_back(p);
254 pcout->header.frame_id = pctmp->header.frame_id;
255 pcout->width = pcout->points.size();
pcl::ModelCoefficients::Ptr current
void project_pc_to_plane(pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout)
Projects all the points in pcin to a plane (along the normal to that plane) defined by coeff and stor...
struct c_polygon_primitive::@9 pointclouds
A structure containing all the point clouds needed to represent the polygon primitive.
void compute_convex_hull_cgal(const pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout, tf::Transform *tr, double &area, double &solidity, bool flg=false)
struct t_polygon_primitive_data::@5 frames
the axis frames
struct t_polygon_primitive_data::@3::@7 convex
data from the polygon convex hull
void compute_concave_hull(pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout, const pcl::ModelCoefficients::Ptr coeff)
Computes the concave_hull of a given point cloud.
t_polygon_primitive_data data
struct t_polygon_primitive_data::@3 hulls
Information about the several hulls used.
A class c_polygon_primitive that contains information about a detected polygon primitive as well as t...
double compute_polygon_area(const polygon_3d< pcl::PointXYZ >::Ptr pcin)
Computes the area of a polygon.
void compute_convex_hull(const pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout)
Uses pcl library to compute the convex hull of a set of points in pcin, by projecting them first to t...