170 lines
6.0 KiB
C++
170 lines
6.0 KiB
C++
/***************************************************************************
|
|
* Copyright (c) 2012 Werner Mayer <wmayer[at]users.sourceforge.net> *
|
|
* *
|
|
* This file is part of the FreeCAD CAx development system. *
|
|
* *
|
|
* This library is free software; you can redistribute it and/or *
|
|
* modify it under the terms of the GNU Library General Public *
|
|
* License as published by the Free Software Foundation; either *
|
|
* version 2 of the License, or (at your option) any later version. *
|
|
* *
|
|
* This library is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU Library General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU Library General Public *
|
|
* License along with this library; see the file COPYING.LIB. If not, *
|
|
* write to the Free Software Foundation, Inc., 59 Temple Place, *
|
|
* Suite 330, Boston, MA 02111-1307, USA *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
|
|
#include "PreCompiled.h"
|
|
|
|
#include "SurfaceTriangulation.h"
|
|
#include <Mod/Points/App/Points.h>
|
|
#include <Mod/Mesh/App/Mesh.h>
|
|
#include <Mod/Mesh/App/Core/Elements.h>
|
|
#include <Mod/Mesh/App/Core/MeshKernel.h>
|
|
|
|
// http://svn.pointclouds.org/pcl/tags/pcl-1.5.1/test/
|
|
#if defined(PCL_FOUND)
|
|
#include <pcl/pcl_config.h>
|
|
#include <pcl/point_types.h>
|
|
#include <pcl/features/normal_3d.h>
|
|
#include <pcl/surface/mls.h>
|
|
#include <pcl/point_traits.h>
|
|
#include <pcl/surface/gp3.h>
|
|
#include <pcl/surface/grid_projection.h>
|
|
//#include <pcl/surface/convex_hull.h>
|
|
//#include <pcl/surface/concave_hull.h>
|
|
#include <pcl/surface/organized_fast_mesh.h>
|
|
#include <pcl/surface/ear_clipping.h>
|
|
#include <pcl/common/common.h>
|
|
#include <boost/random.hpp>
|
|
|
|
#ifndef PCL_REVISION_VERSION
|
|
#define PCL_REVISION_VERSION 0
|
|
#endif
|
|
|
|
using namespace pcl;
|
|
using namespace pcl::io;
|
|
using namespace std;
|
|
using namespace Reen;
|
|
|
|
SurfaceTriangulation::SurfaceTriangulation(const Points::PointKernel& pts, Mesh::MeshObject& mesh)
|
|
: myPoints(pts), myMesh(mesh)
|
|
{
|
|
}
|
|
|
|
void SurfaceTriangulation::perform()
|
|
{
|
|
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
|
|
PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
|
|
search::KdTree<PointXYZ>::Ptr tree;
|
|
search::KdTree<PointNormal>::Ptr tree2;
|
|
|
|
for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
|
|
cloud->push_back(PointXYZ(it->x, it->y, it->z));
|
|
}
|
|
|
|
// Create search tree
|
|
tree.reset (new search::KdTree<PointXYZ> (false));
|
|
tree->setInputCloud (cloud);
|
|
|
|
// Normal estimation
|
|
NormalEstimation<PointXYZ, Normal> n;
|
|
PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
|
|
n.setInputCloud (cloud);
|
|
//n.setIndices (indices[B);
|
|
n.setSearchMethod (tree);
|
|
n.setKSearch (20);
|
|
n.compute (*normals);
|
|
|
|
// Concatenate XYZ and normal information
|
|
pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
|
|
|
|
// Create search tree
|
|
tree2.reset (new search::KdTree<PointNormal>);
|
|
tree2->setInputCloud (cloud_with_normals);
|
|
|
|
// Init objects
|
|
GreedyProjectionTriangulation<PointNormal> gp3;
|
|
|
|
// Set parameters
|
|
gp3.setInputCloud (cloud_with_normals);
|
|
gp3.setSearchMethod (tree2);
|
|
gp3.setSearchRadius (2.025);
|
|
gp3.setMu (2.5);
|
|
gp3.setMaximumNearestNeighbors (100);
|
|
gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
|
|
gp3.setMinimumAngle(M_PI/18); // 10 degrees
|
|
gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
|
|
gp3.setNormalConsistency(false);
|
|
|
|
// Reconstruct
|
|
PolygonMesh mesh;
|
|
gp3.reconstruct (mesh);
|
|
|
|
// number of points
|
|
size_t nr_points = mesh.cloud.width * mesh.cloud.height;
|
|
size_t point_size = mesh.cloud.data.size () / nr_points;
|
|
// number of faces for header
|
|
size_t nr_faces = mesh.polygons.size ();
|
|
|
|
MeshCore::MeshPointArray points;
|
|
points.reserve(nr_points);
|
|
MeshCore::MeshFacetArray facets;
|
|
facets.reserve(nr_faces);
|
|
|
|
// get vertices
|
|
MeshCore::MeshPoint vertex;
|
|
for (size_t i = 0; i < nr_points; ++i) {
|
|
int xyz = 0;
|
|
for (size_t d = 0; d < mesh.cloud.fields.size(); ++d) {
|
|
int c = 0;
|
|
// adding vertex
|
|
if ((mesh.cloud.fields[d].datatype ==
|
|
#if PCL_VERSION_COMPARE(>,1,6,0)
|
|
pcl::PCLPointField::FLOAT32) &&
|
|
#else
|
|
sensor_msgs::PointField::FLOAT32) &&
|
|
#endif
|
|
(mesh.cloud.fields[d].name == "x" ||
|
|
mesh.cloud.fields[d].name == "y" ||
|
|
mesh.cloud.fields[d].name == "z"))
|
|
{
|
|
float value;
|
|
memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
|
|
vertex[xyz] = value;
|
|
if (++xyz == 3) {
|
|
points.push_back(vertex);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
// get faces
|
|
MeshCore::MeshFacet face;
|
|
for (size_t i = 0; i < nr_faces; i++) {
|
|
face._aulPoints[0] = mesh.polygons[i].vertices[0];
|
|
face._aulPoints[1] = mesh.polygons[i].vertices[1];
|
|
face._aulPoints[2] = mesh.polygons[i].vertices[2];
|
|
facets.push_back(face);
|
|
}
|
|
|
|
MeshCore::MeshKernel kernel;
|
|
kernel.Adopt(points, facets, true);
|
|
myMesh.swap(kernel);
|
|
myMesh.harmonizeNormals();
|
|
|
|
// Additional vertex information
|
|
//std::vector<int> parts = gp3.getPartIDs();
|
|
//std::vector<int> states = gp3.getPointStates();
|
|
}
|
|
|
|
#endif // PCL_FOUND
|
|
|