From f336b7ed67449895d3b60df365b4fd1256ec9469 Mon Sep 17 00:00:00 2001
From: wmayer <wmayer@users.sourceforge.net>
Date: Wed, 2 Mar 2016 16:14:08 +0100
Subject: [PATCH] + filter out invalid points in surface triangulation
 algorithms

---
 .../App/SurfaceTriangulation.cpp              | 106 ++++++++++--------
 1 file changed, 61 insertions(+), 45 deletions(-)

diff --git a/src/Mod/ReverseEngineering/App/SurfaceTriangulation.cpp b/src/Mod/ReverseEngineering/App/SurfaceTriangulation.cpp
index 4526e3f39..8b2a13457 100644
--- a/src/Mod/ReverseEngineering/App/SurfaceTriangulation.cpp
+++ b/src/Mod/ReverseEngineering/App/SurfaceTriangulation.cpp
@@ -49,6 +49,7 @@
 #include <pcl/surface/ear_clipping.h>
 #include <pcl/common/common.h>
 #include <boost/random.hpp>
+#include <boost/math/special_functions/fpclassify.hpp>
 
 #ifndef PCL_REVISION_VERSION
 #define PCL_REVISION_VERSION 0
@@ -79,7 +80,8 @@ void SurfaceTriangulation::perform(int ksearch)
 
     cloud->reserve(myPoints.size());
     for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
-        cloud->push_back(PointXYZ(it->x, it->y, it->z));
+        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
+            cloud->push_back(PointXYZ(it->x, it->y, it->z));
     }
 
     // Create search tree
@@ -142,14 +144,16 @@ void SurfaceTriangulation::perform(const std::vector<Base::Vector3f>& normals)
     for (std::size_t index=0; index<num_points; index++) {
         const Base::Vector3f& p = points[index];
         const Base::Vector3f& n = normals[index];
-        PointNormal pn;
-        pn.x = p.x;
-        pn.y = p.y;
-        pn.z = p.z;
-        pn.normal_x = n.x;
-        pn.normal_y = n.y;
-        pn.normal_z = n.z;
-        cloud_with_normals->push_back(pn);
+        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
+            PointNormal pn;
+            pn.x = p.x;
+            pn.y = p.y;
+            pn.z = p.z;
+            pn.normal_x = n.x;
+            pn.normal_y = n.y;
+            pn.normal_z = n.z;
+            cloud_with_normals->push_back(pn);
+        }
     }
 
     // Create search tree
@@ -204,7 +208,8 @@ void PoissonReconstruction::perform(int ksearch)
 
     cloud->reserve(myPoints.size());
     for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
-        cloud->push_back(PointXYZ(it->x, it->y, it->z));
+        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
+            cloud->push_back(PointXYZ(it->x, it->y, it->z));
     }
 
     // Create search tree
@@ -261,14 +266,16 @@ void PoissonReconstruction::perform(const std::vector<Base::Vector3f>& normals)
     for (std::size_t index=0; index<num_points; index++) {
         const Base::Vector3f& p = points[index];
         const Base::Vector3f& n = normals[index];
-        PointNormal pn;
-        pn.x = p.x;
-        pn.y = p.y;
-        pn.z = p.z;
-        pn.normal_x = n.x;
-        pn.normal_y = n.y;
-        pn.normal_z = n.z;
-        cloud_with_normals->push_back(pn);
+        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
+            PointNormal pn;
+            pn.x = p.x;
+            pn.y = p.y;
+            pn.z = p.z;
+            pn.normal_x = n.x;
+            pn.normal_y = n.y;
+            pn.normal_z = n.z;
+            cloud_with_normals->push_back(pn);
+        }
     }
 
     // Create search tree
@@ -312,7 +319,8 @@ void GridReconstruction::perform(int ksearch)
 
     cloud->reserve(myPoints.size());
     for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
-        cloud->push_back(PointXYZ(it->x, it->y, it->z));
+        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
+            cloud->push_back(PointXYZ(it->x, it->y, it->z));
     }
 
     // Create search tree
@@ -367,14 +375,16 @@ void GridReconstruction::perform(const std::vector<Base::Vector3f>& normals)
     for (std::size_t index=0; index<num_points; index++) {
         const Base::Vector3f& p = points[index];
         const Base::Vector3f& n = normals[index];
-        PointNormal pn;
-        pn.x = p.x;
-        pn.y = p.y;
-        pn.z = p.z;
-        pn.normal_x = n.x;
-        pn.normal_y = n.y;
-        pn.normal_z = n.z;
-        cloud_with_normals->push_back(pn);
+        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
+            PointNormal pn;
+            pn.x = p.x;
+            pn.y = p.y;
+            pn.z = p.z;
+            pn.normal_x = n.x;
+            pn.normal_y = n.y;
+            pn.normal_z = n.z;
+            cloud_with_normals->push_back(pn);
+        }
     }
 
     // Create search tree
@@ -495,7 +505,8 @@ void Reen::MarchingCubesRBF::perform(int ksearch)
 
     cloud->reserve(myPoints.size());
     for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
-        cloud->push_back(PointXYZ(it->x, it->y, it->z));
+        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
+            cloud->push_back(PointXYZ(it->x, it->y, it->z));
     }
 
     // Create search tree
@@ -551,14 +562,16 @@ void Reen::MarchingCubesRBF::perform(const std::vector<Base::Vector3f>& normals)
     for (std::size_t index=0; index<num_points; index++) {
         const Base::Vector3f& p = points[index];
         const Base::Vector3f& n = normals[index];
-        PointNormal pn;
-        pn.x = p.x;
-        pn.y = p.y;
-        pn.z = p.z;
-        pn.normal_x = n.x;
-        pn.normal_y = n.y;
-        pn.normal_z = n.z;
-        cloud_with_normals->push_back(pn);
+        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
+            PointNormal pn;
+            pn.x = p.x;
+            pn.y = p.y;
+            pn.z = p.z;
+            pn.normal_x = n.x;
+            pn.normal_y = n.y;
+            pn.normal_z = n.z;
+            cloud_with_normals->push_back(pn);
+        }
     }
 
     // Create search tree
@@ -602,7 +615,8 @@ void Reen::MarchingCubesHoppe::perform(int ksearch)
 
     cloud->reserve(myPoints.size());
     for (Points::PointKernel::const_iterator it = myPoints.begin(); it != myPoints.end(); ++it) {
-        cloud->push_back(PointXYZ(it->x, it->y, it->z));
+        if (!boost::math::isnan(it->x) && !boost::math::isnan(it->y) && !boost::math::isnan(it->z))
+            cloud->push_back(PointXYZ(it->x, it->y, it->z));
     }
 
     // Create search tree
@@ -657,14 +671,16 @@ void Reen::MarchingCubesHoppe::perform(const std::vector<Base::Vector3f>& normal
     for (std::size_t index=0; index<num_points; index++) {
         const Base::Vector3f& p = points[index];
         const Base::Vector3f& n = normals[index];
-        PointNormal pn;
-        pn.x = p.x;
-        pn.y = p.y;
-        pn.z = p.z;
-        pn.normal_x = n.x;
-        pn.normal_y = n.y;
-        pn.normal_z = n.z;
-        cloud_with_normals->push_back(pn);
+        if (!boost::math::isnan(p.x) && !boost::math::isnan(p.y) && !boost::math::isnan(p.z)) {
+            PointNormal pn;
+            pn.x = p.x;
+            pn.y = p.y;
+            pn.z = p.z;
+            pn.normal_x = n.x;
+            pn.normal_y = n.y;
+            pn.normal_z = n.z;
+            cloud_with_normals->push_back(pn);
+        }
     }
 
     // Create search tree