bool TOMPCFilter<T>::convexHull(VecCloud &clouds_in, VecCloud &clouds_out, VecHull &hulls)
{
// std::cout << "Convex Hull" << std::endl;
for(int i=0 ; i<clouds_in.size(); i++){
pcl::PointCloud<T>& point_cloud = *clouds_in.at(i);
// assign points
std::vector<Point_3> points;
for(int j=0;j<point_cloud.size();j++){
Point_3 point(point_cloud.points[j].x, point_cloud.points[j].y, point_cloud.points[j].z);
points.push_back(point);
}
// define polyhedron to hold convex hull
Polyhedron_3 poly;
// compute convex hull of non-collinear points
CGAL::convex_hull_3(points.begin(), points.end(), poly);
hulls.push_back(poly);
CloudPtr cloud_convex (new pcl::PointCloud<T>);
Polyhedron_3::Vertex_iterator it;
for(it = poly.vertices_begin(); it != poly.vertices_end(); ++it){
T point;
point.x = it->point()[0];
point.y = it->point()[1];
point.z = it->point()[2];
cloud_convex->points.push_back(point);
}
clouds_out.push_back(cloud_convex);
}
return 1;
}
static Point_3 getCenter(const Polyhedron_3 &p)
{
Point_3 C(0., 0., 0.);
for (auto I = p.vertices_begin(), E = p.vertices_end(); I != E; ++I)
C = C + (I->point() - CGAL::Origin());
C = C * (1. / p.size_of_vertices());
return C;
}
void CGALConvexHull3D::run(const MatrixFr& points) {
std::list<Point_3> cgal_pts;
const size_t num_pts = points.rows();
const size_t dim = points.cols();
if (dim != 3) {
std::stringstream err_msg;
err_msg << "Invalid dim: " << dim << " Expect dim=3.";
throw RuntimeError(err_msg.str());
}
for (size_t i=0; i<num_pts; i++) {
const VectorF& p = points.row(i);
cgal_pts.push_back(Point_3(p[0], p[1], p[2]));
}
Polyhedron_3 hull;
CGAL::convex_hull_3(cgal_pts.begin(), cgal_pts.end(), hull);
assert(hull.is_closed());
assert(hull.is_pure_triangle());
const size_t num_vertices = hull.size_of_vertices();
const size_t num_faces = hull.size_of_facets();
m_vertices.resize(num_vertices, dim);
m_faces.resize(num_faces, 3);
size_t vertex_count=0;
for (auto itr=hull.vertices_begin(); itr!=hull.vertices_end(); itr++) {
const Point_3& p = itr->point();
m_vertices.coeffRef(vertex_count, 0) = p.x();
m_vertices.coeffRef(vertex_count, 1) = p.y();
m_vertices.coeffRef(vertex_count, 2) = p.z();
itr->id() = vertex_count;
vertex_count++;
}
size_t face_count=0;
for (auto f_itr=hull.facets_begin(); f_itr!=hull.facets_end(); f_itr++) {
size_t edge_count=0;
auto h_itr = f_itr->facet_begin();
do {
m_faces.coeffRef(face_count, edge_count) = h_itr->vertex()->id();
edge_count++;
h_itr++;
} while (h_itr != f_itr->facet_begin());
face_count++;
}
compute_index_map(points);
reorient_faces();
}
Polyhedra generate_polyhedra_from_points(const std::vector<std::array<double,3> >& vertices)
{
std::vector<Point_3> points(vertices.size());
for(int i=0;i<(int)vertices.size();i++)
points[i] = Point_3(vertices[i][0],vertices[i][1],vertices[i][2]);
Polyhedron_3 poly;
CGAL::convex_hull_3(points.begin(), points.end(), poly);
std::transform(poly.facets_begin(), poly.facets_end(), poly.planes_begin(), Plane_from_facet());
for(Halfedge_iterator half=poly.halfedges_begin();half!=poly.halfedges_end();++half)
{
Point_3 v1 = half->vertex()->point();
Point_3 v2 = half->next()->vertex()->point();
Point_3 v3 = half->next()->next()->vertex()->point();
Point_3 v4 = half->opposite()->vertex()->point();
Point_3 v5 = half->opposite()->next()->vertex()->point();
Point_3 v6 = half->opposite()->next()->next()->vertex()->point();
if(coplanar_handmade(v1,v2,v3,v4)&&coplanar_handmade(v1,v2,v3,v5)&&coplanar_handmade(v1,v2,v3,v6)&&
coplanar_handmade(v1,v2,v4,v5)&&coplanar_handmade(v1,v2,v4,v6)&&coplanar_handmade(v1,v2,v5,v6)&&
coplanar_handmade(v1,v3,v4,v5)&&coplanar_handmade(v1,v3,v4,v6)&&coplanar_handmade(v1,v4,v5,v6)&&
coplanar_handmade(v2,v3,v4,v5)&&coplanar_handmade(v2,v3,v4,v6)&&coplanar_handmade(v2,v3,v5,v6)&&
coplanar_handmade(v2,v4,v5,v6)&&coplanar_handmade(v1,v3,v5,v6)&&coplanar_handmade(v3,v4,v5,v6))
{
poly.join_facet(half);
half=poly.halfedges_begin();
}
}
int cur = 0;
for(auto it=poly.points_begin(); it!=poly.points_end(); it++, cur++)
points[cur] = *it;
cur = 0;
std::vector<std::vector<int> > faces;
for(Facet_iterator i=poly.facets_begin(); i!=poly.facets_end(); i++)
{
faces.push_back(std::vector<int>());
Halfedge_facet_circulator j = i->facet_begin();
do
{faces[cur].push_back(std::distance(poly.vertices_begin(), j->vertex()));} while (++j != i->facet_begin());
cur++;
}
return Polyhedra{points,faces};
}
/* FIXME: Copied from Polyhedron_io.cpp with slight modifications. */
static std::shared_ptr<Polyhedron> convertWithAssociation(Polyhedron_3 p,
const Point_3 &C, const std::vector<Plane_3> &initPlanes)
{
/* Check for non-emptiness. */
ASSERT(p.size_of_vertices());
ASSERT(p.size_of_facets());
int numVertices = p.size_of_vertices();
int numFacets = p.size_of_facets();
/* Allocate memory for arrays. */
Vector3d *vertices = new Vector3d[numVertices];
Facet *facets = new Facet[numFacets];
/* Transform vertexes. */
int iVertex = 0;
for (auto vertex = p.vertices_begin(); vertex != p.vertices_end(); ++vertex)
{
Point_3 point = C + vertex->point();
vertices[iVertex++] = Vector3d(point.x(), point.y(), point.z());
}
/*
* Transform facets.
* This algorithm is based on example kindly provided at CGAL online user
* manual. See example Polyhedron/polyhedron_prog_off.cpp
*/
int iFacet = 0;
auto plane = p.planes_begin();
auto facet = p.facets_begin();
/* Iterate through the std::lists of planes and facets. */
do
{
int id = p.indexPlanes_[iFacet];
facets[id].id = id;
/* Transform current plane. */
Plane_3 pi = centerizePlane(*plane,
Point_3(-C.x(), -C.y(), -C.z()),
signedDist(initPlanes[id], C));
facets[id].plane = Plane(Vector3d(pi.a(), pi.b(), pi.c()),
pi.d());
/*
* Iterate through the std::list of halfedges incident to the curent CGAL
* facet.
*/
auto halfedge = facet->facet_begin();
/* Facets in polyhedral surfaces are at least triangles. */
CGAL_assertion(CGAL::circulator_size(halfedge) >= 3);
facets[id].numVertices = CGAL::circulator_size(halfedge);
facets[id].indVertices =
new int[3 * facets[id].numVertices + 1];
/*
* TODO: It's too unsafe architecture if we do such things as setting
* the size of internal array outside the API functions. Moreover, it
* can cause us to write memory leaks.
* indFacets and numFacets should no be public members.
*/
int iFacetVertex = 0;
do
{
facets[id].indVertices[iFacetVertex++] =
std::distance(p.vertices_begin(), halfedge->vertex());
} while (++halfedge != facet->facet_begin());
/* Add cycling vertex to avoid assertion during printing. */
facets[id].indVertices[facets[id].numVertices] =
facets[id].indVertices[0];
ASSERT(facets[id].correctPlane());
/* Increment the ID of facet. */
++iFacet;
} while (++plane != p.planes_end() && ++facet != p.facets_end());
return std::make_shared<Polyhedron>(numVertices, numFacets, vertices,
facets);
}