Move pcl stuff to Reen module
This commit is contained in:
wmayer
159
src/Mod/ReverseEngineering/App/SurfaceTriangulation.cpp
Normal file
159
src/Mod/ReverseEngineering/App/SurfaceTriangulation.cpp
Normal file
@@ -0,0 +1,159 @@
|
||||
/***************************************************************************
|
||||
* 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/point_types.h>
|
||||
#include <pcl/features/normal_3d.h>
|
||||
#include <pcl/surface/mls.h>
|
||||
#include <pcl/surface/mls_omp.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>
|
||||
|
||||
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
|
||||
int nr_points = mesh.cloud.width * mesh.cloud.height;
|
||||
int point_size = mesh.cloud.data.size () / nr_points;
|
||||
// number of faces for header
|
||||
int 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 (int 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 == sensor_msgs::PointField::FLOAT32) && (
|
||||
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(int 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
|
||||
|
||||
Reference in New Issue
Block a user