From 4ca65678bd27ea252d122a3c676edc99d38f442e Mon Sep 17 00:00:00 2001
From: wmayer <wmayer@users.sourceforge.net>
Date: Sat, 26 May 2012 14:52:04 +0200
Subject: [PATCH] Playing with pcl library

---
 CMakeLists.txt                     |   4 +
 src/Mod/Points/App/AppPointsPy.cpp | 235 ++++++++++++++++++++++++++++-
 src/Mod/Points/App/CMakeLists.txt  |  10 ++
 3 files changed, 248 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index dda4867a2..a11097003 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -247,6 +247,10 @@ MARK_AS_ADVANCED(FORCE FREECAD_LIBPACK_CHECKFILE6X FREECAD_LIBPACK_CHECKFILE7X)
     #find_package(Eigen2)
     find_package(Eigen3)
 
+# -------------------------------- pcl ----------------------------------
+
+    find_package(PCL COMPONENTS common kdtree features surface)
+
 # -------------------------------- ODE ----------------------------------
 
     find_package(ODE)
diff --git a/src/Mod/Points/App/AppPointsPy.cpp b/src/Mod/Points/App/AppPointsPy.cpp
index 70e8677d8..a3ad68ab5 100644
--- a/src/Mod/Points/App/AppPointsPy.cpp
+++ b/src/Mod/Points/App/AppPointsPy.cpp
@@ -24,7 +24,7 @@
 #include "PreCompiled.h"
 #ifndef _PreComp_
 #endif
-
+
 #include <Base/Console.h>
 #include <Base/Interpreter.h>
 #include <Base/FileInfo.h>
@@ -132,11 +132,244 @@ show(PyObject *self, PyObject *args)
     Py_Return;
 }
 
+// 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;
+
+
+int 
+saveOBJFile (const std::string &file_name,
+             const pcl::PolygonMesh &mesh, unsigned precision=5)
+{
+  if (mesh.cloud.data.empty ())
+  {
+    std::cerr << "[pcl::io::saveOBJFile] Input point cloud has no data!\n";
+    return (-1);
+  }
+  // Open file
+  std::ofstream fs;
+  fs.precision (precision);
+  fs.open (file_name.c_str ());
+
+  /* Write 3D information */
+  // number of points
+  int nr_points  = mesh.cloud.width * mesh.cloud.height;
+  // point size
+  int point_size = mesh.cloud.data.size () / nr_points;
+  // number of faces for header
+  int nr_faces = mesh.polygons.size ();
+  // Do we have vertices normals?
+  int normal_index = getFieldIndex (mesh.cloud, "normal");
+
+  // Write the header information
+  fs << "####" << std::endl;
+  fs << "# OBJ dataFile simple version. File name: " << file_name << std::endl;
+  fs << "# Vertices: " << nr_points << std::endl;
+  if (normal_index != -1)
+    fs << "# Vertices normals : " << nr_points << std::endl;
+  fs << "# Faces: " <<nr_faces << std::endl;
+  fs << "####" << std::endl;
+
+  // Write vertex coordinates
+  fs << "# List of Vertices, with (x,y,z) coordinates, w is optional." << std::endl;
+  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"))
+      {
+        if (mesh.cloud.fields[d].name == "x")
+           // write vertices beginning with v
+          fs << "v ";
+
+        float value;
+        memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
+        fs << value;
+        if (++xyz == 3)
+          break;
+        fs << " ";
+      }
+    }
+    if (xyz != 3)
+    {
+      std::cerr << "[pcl::io::saveOBJFile] Input point cloud has no XYZ data!\n";
+      return (-2);
+    }
+    fs << std::endl;
+  }
+
+  fs << "# "<< nr_points <<" vertices" << std::endl;
+
+  if(normal_index != -1)
+  {    
+    fs << "# Normals in (x,y,z) form; normals might not be unit." <<  std::endl;
+    // Write vertex normals
+    for (int i = 0; i < nr_points; ++i)
+    {
+      int nxyz = 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 == "normal_x" ||
+              mesh.cloud.fields[d].name == "normal_y" ||
+              mesh.cloud.fields[d].name == "normal_z"))
+        {
+          if (mesh.cloud.fields[d].name == "normal_x")
+            // write vertices beginning with vn
+            fs << "vn ";
+          
+          float value;
+          memcpy (&value, &mesh.cloud.data[i * point_size + mesh.cloud.fields[d].offset + c * sizeof (float)], sizeof (float));
+          fs << value;
+          if (++nxyz == 3)
+            break;
+          fs << " ";
+        }
+      }
+      if (nxyz != 3)
+      {
+        std::cerr << "[pcl::io::saveOBJFile] Input point cloud has no normals!\n";
+        return (-2);
+      }
+      fs << std::endl;
+    }
+
+    fs << "# "<< nr_points <<" vertices normals" << std::endl;
+  }
+
+  fs << "# Face Definitions" << std::endl;
+  // Write down faces
+  if(normal_index == -1)
+  {
+    for(int i = 0; i < nr_faces; i++)
+    {
+      fs << "f ";
+      size_t j = 0;    
+      for (; j < mesh.polygons[i].vertices.size () - 1; ++j)
+        fs << mesh.polygons[i].vertices[j] + 1 << " ";
+      fs << mesh.polygons[i].vertices[j] + 1 << std::endl;
+    }
+  }
+  else
+  {
+    for(int i = 0; i < nr_faces; i++)
+    {
+      fs << "f ";
+      size_t j = 0;    
+      for (; j < mesh.polygons[i].vertices.size () - 1; ++j)
+        fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1;
+      fs << mesh.polygons[i].vertices[j] + 1 << "//" << mesh.polygons[i].vertices[j] + 1 << std::endl;
+    }
+  }
+  fs << "# End of File" << std::endl;
+
+  // Close obj file
+  fs.close ();  
+  return 0;
+}
+
+static PyObject * 
+testPCL(PyObject *self, PyObject *args)
+{
+    PyObject *pcObj;
+    if (!PyArg_ParseTuple(args, "O!", &(PointsPy::Type), &pcObj))     // convert args: Python->C
+        return NULL;                             // NULL triggers exception
+        
+    PointsPy* pPoints = static_cast<PointsPy*>(pcObj);
+    PointKernel* kernel = pPoints->getPointKernelPtr();
+    
+    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 (PointKernel::const_iterator it = kernel->begin(); it != kernel->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
+    PolygonMesh triangles;
+    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
+    gp3.reconstruct (triangles);
+
+    // Additional vertex information
+    std::vector<int> parts = gp3.getPartIDs();
+    std::vector<int> states = gp3.getPointStates();
+    int nr_points = cloud_with_normals->width * cloud_with_normals->height;
+    
+    std::cerr << "Number of triangles: " << triangles.polygons.size() << std::endl;
+    saveOBJFile("/tmp/pcl.obj", triangles);
+
+
+    Py_Return;
+}
+#endif // PCL_FOUND
+
 // registration table  
 struct PyMethodDef Points_Import_methods[] = {
     {"open",  open,   1},				/* method name, C func ptr, always-tuple */
     {"insert",insert, 1},
     {"show",show, 1},
+#if defined(PCL_FOUND)
+    {"testPCL",testPCL, 1},
+#endif
 
     {NULL, NULL}                /* end of table marker */
 };
+
diff --git a/src/Mod/Points/App/CMakeLists.txt b/src/Mod/Points/App/CMakeLists.txt
index 9cd657f98..2d71a01a8 100644
--- a/src/Mod/Points/App/CMakeLists.txt
+++ b/src/Mod/Points/App/CMakeLists.txt
@@ -2,6 +2,10 @@ if(WIN32)
     add_definitions(-DFCAppPoints)
 endif(WIN32)
 
+if (PCL_FOUND)
+    add_definitions(-DPCL_FOUND)
+endif(PCL_FOUND)
+
 include_directories(
     ${CMAKE_CURRENT_BINARY_DIR}
     ${CMAKE_CURRENT_SOURCE_DIR}
@@ -9,10 +13,16 @@ include_directories(
     ${PYTHON_INCLUDE_PATH}
     ${XERCESC_INCLUDE_DIR}
     ${ZLIB_INCLUDE_DIR}
+    ${EIGEN3_INCLUDE_DIR}
+    ${PCL_INCLUDE_DIRS}
 )
 
 set(Points_LIBS
     FreeCADApp
+    ${PCL_COMMON_LIBRARIES}
+    ${PCL_KDTREE_LIBRARIES}
+    ${PCL_FEATURES_LIBRARIES}
+    ${PCL_SURFACE_LIBRARIES}
 )
 
 generate_from_xml(PointsPy)