From 1d04ce9ea03c46f85cbbac98714996e497bbdd73 Mon Sep 17 00:00:00 2001 From: wmayer Date: Tue, 13 Oct 2015 20:03:47 +0200 Subject: [PATCH] + port SurfaceFit to Eigen3 and add unit tests --- src/Mod/Mesh/App/AppMeshPy.cpp | 97 +++++++++-- src/Mod/Mesh/App/CMakeLists.txt | 4 +- src/Mod/Mesh/App/Core/Approximation.cpp | 204 ++++++++++++++++-------- src/Mod/Mesh/App/Core/Approximation.h | 17 +- src/Mod/Mesh/App/MeshTestsApp.py | 61 ++++++- 5 files changed, 288 insertions(+), 95 deletions(-) diff --git a/src/Mod/Mesh/App/AppMeshPy.cpp b/src/Mod/Mesh/App/AppMeshPy.cpp index 0170f751f..83694278c 100644 --- a/src/Mod/Mesh/App/AppMeshPy.cpp +++ b/src/Mod/Mesh/App/AppMeshPy.cpp @@ -40,6 +40,7 @@ #include "Core/MeshIO.h" #include "Core/Evaluation.h" #include "Core/Iterator.h" +#include "Core/Approximation.h" #include "MeshPy.h" #include "Mesh.h" @@ -387,7 +388,7 @@ createBox(PyObject *self, PyObject *args) } PY_CATCH; } -static PyObject * +static PyObject * calculateEigenTransform(PyObject *self, PyObject *args) { PyObject *input; @@ -395,8 +396,8 @@ calculateEigenTransform(PyObject *self, PyObject *args) if (!PyArg_ParseTuple(args, "O",&input)) return NULL; - if(! PySequence_Check(input) ){ - PyErr_SetString(Base::BaseExceptionFreeCADError, "Input have to be a sequence of Base.Vector()"); + if (!PySequence_Check(input)) { + PyErr_SetString(Base::BaseExceptionFreeCADError, "Input has to be a sequence of Base.Vector()"); return NULL; } @@ -416,25 +417,84 @@ calculateEigenTransform(PyObject *self, PyObject *args) Base::Vector3d* val = pcObject->getVectorPtr(); - current_node.Set(float(val->x),float(val->y),float(val->z)); - vertices.push_back(current_node); + current_node.Set(float(val->x),float(val->y),float(val->z)); + vertices.push_back(current_node); } - } + } - MeshCore::MeshFacet aFacet; - aFacet._aulPoints[0] = 0;aFacet._aulPoints[1] = 1;aFacet._aulPoints[2] = 2; - faces.push_back(aFacet); - //Fill the Kernel with the temp smesh structure and delete the current containers - aMesh.Adopt(vertices,faces); - MeshCore::MeshEigensystem pca(aMesh); - pca.Evaluate(); - Base::Matrix4D Trafo = pca.Transform(); + MeshCore::MeshFacet aFacet; + aFacet._aulPoints[0] = 0;aFacet._aulPoints[1] = 1;aFacet._aulPoints[2] = 2; + faces.push_back(aFacet); + //Fill the Kernel with the temp mesh structure and delete the current containers + aMesh.Adopt(vertices,faces); + MeshCore::MeshEigensystem pca(aMesh); + pca.Evaluate(); + Base::Matrix4D Trafo = pca.Transform(); return new Base::PlacementPy(new Base::Placement(Trafo) ); - } PY_CATCH; + } PY_CATCH; - Py_Return; + Py_Return; +} + +static PyObject * +polynomialFit(PyObject *self, PyObject *args) +{ + PyObject *input; + + if (!PyArg_ParseTuple(args, "O",&input)) + return NULL; + + if (!PySequence_Check(input)) { + PyErr_SetString(Base::BaseExceptionFreeCADError, "Input has to be a sequence of Base.Vector()"); + return NULL; + } + + PY_TRY { + MeshCore::SurfaceFit polyFit; + + Base::Vector3f point; + Py::Sequence list(input); + for (Py::Sequence::iterator it = list.begin(); it != list.end(); ++it) { + PyObject* value = (*it).ptr(); + if (PyObject_TypeCheck(value, &(Base::VectorPy::Type))) { + Base::VectorPy *pcObject = static_cast(value); + Base::Vector3d* val = pcObject->getVectorPtr(); + point.Set(float(val->x),float(val->y),float(val->z)); + polyFit.AddPoint(point); + } + } + + // fit quality + float fit = polyFit.Fit(); + Py::Dict dict; + dict.setItem(Py::String("Sigma"), Py::Float(fit)); + + // coefficients + double a,b,c,d,e,f; + polyFit.GetCoefficients(a,b,c,d,e,f); + Py::Tuple p(6); + p.setItem(0, Py::Float(a)); + p.setItem(1, Py::Float(b)); + p.setItem(2, Py::Float(c)); + p.setItem(3, Py::Float(d)); + p.setItem(4, Py::Float(e)); + p.setItem(5, Py::Float(f)); + dict.setItem(Py::String("Coefficients"), p); + + // residuals + std::vector local = polyFit.GetLocalPoints(); + Py::Tuple r(local.size()); + for (std::vector::iterator it = local.begin(); it != local.end(); ++it) { + double z = polyFit.Value(it->x, it->y); + double d = it->z - z; + r.setItem(it-local.begin(), Py::Float(d)); + } + dict.setItem(Py::String("Residuals"), r); + + return Py::new_reference_to(dict); + } PY_CATCH; } @@ -455,6 +515,10 @@ PyDoc_STRVAR(calculateEigenTransform_doc, "The local coordinate system is right-handed.\n" ); +PyDoc_STRVAR(polynomialFit_doc, +"polynomialFit(seq(Base.Vector)) -- Calculates a polynomial fit.\n" +); + /* List of functions defined in the module */ struct PyMethodDef Mesh_Import_methods[] = { @@ -471,5 +535,6 @@ struct PyMethodDef Mesh_Import_methods[] = { {"createCone",createCone, Py_NEWARGS, "Create a tessellated cone"}, {"createTorus",createTorus, Py_NEWARGS, "Create a tessellated torus"}, {"calculateEigenTransform",calculateEigenTransform, METH_VARARGS, calculateEigenTransform_doc}, + {"polynomialFit",polynomialFit, METH_VARARGS, polynomialFit_doc}, {NULL, NULL} /* sentinel */ }; diff --git a/src/Mod/Mesh/App/CMakeLists.txt b/src/Mod/Mesh/App/CMakeLists.txt index 3ee8aabfa..8934480b0 100644 --- a/src/Mod/Mesh/App/CMakeLists.txt +++ b/src/Mod/Mesh/App/CMakeLists.txt @@ -1,7 +1,5 @@ if(WIN32) - add_definitions(-DFCAppMesh -DWM4_FOUNDATION_DLL_EXPORT -DEIGEN2_SUPPORT) -else (Win32) - add_definitions(-DEIGEN2_SUPPORT) + add_definitions(-DFCAppMesh -DWM4_FOUNDATION_DLL_EXPORT) endif(WIN32) include_directories( diff --git a/src/Mod/Mesh/App/Core/Approximation.cpp b/src/Mod/Mesh/App/Core/Approximation.cpp index dd53f38ff..9f1e56233 100644 --- a/src/Mod/Mesh/App/Core/Approximation.cpp +++ b/src/Mod/Mesh/App/Core/Approximation.cpp @@ -38,28 +38,22 @@ #include //#define FC_USE_EIGEN -//#define FC_USE_BOOST -#if defined(FC_USE_BOOST) -#include -#include -#include - -#define BOOST_NUMERIC_BINDINGS_USE_CLAPACK -#include -#include -#include - -namespace ublas = boost::numeric::ublas; -extern "C" void LAPACK_DGESV (int const* n, int const* nrhs, - double* a, int const* lda, int* ipiv, - double* b, int const* ldb, int* info); - +#include +#ifdef FC_USE_EIGEN +#include #endif -# include - using namespace MeshCore; +Approximation::Approximation() + : _bIsFitted(false), _fLastResult(FLOAT_MAX) +{ +} + +Approximation::~Approximation() +{ + Clear(); +} void Approximation::Convert( const Wm4::Vector3& Wm4, Base::Vector3f& pt) { @@ -142,6 +136,18 @@ bool Approximation::Done() const // ------------------------------------------------------------------------------- +PlaneFit::PlaneFit() + : _vBase(0,0,0) + , _vDirU(1,0,0) + , _vDirV(0,1,0) + , _vDirW(0,0,1) +{ +} + +PlaneFit::~PlaneFit() +{ +} + float PlaneFit::Fit() { _bIsFitted = true; @@ -166,21 +172,7 @@ float PlaneFit::Fit() syz = syz - my*mz/((double)nSize); szz = szz - mz*mz/((double)nSize); -#if defined(FC_USE_BOOST) - ublas::matrix A(3,3); - A(0,0) = sxx; - A(1,1) = syy; - A(2,2) = szz; - A(0,1) = sxy; A(1,0) = sxy; - A(0,2) = sxz; A(2,0) = sxz; - A(1,2) = syz; A(2,1) = syz; - namespace lapack= boost::numeric::bindings::lapack; - ublas::vector eigenval(3); - int r = lapack::syev('V','U',A,eigenval,lapack::optimal_workspace()); - if (r) { - } - float sigma = 0; -#elif defined(FC_USE_EIGEN) +#if defined(FC_USE_EIGEN) Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero(); covMat(0,0) = sxx; covMat(1,1) = syy; @@ -398,6 +390,26 @@ void PlaneFit::Dimension(float& length, float& width) const width = bbox.MaxY - bbox.MinY; } +std::vector PlaneFit::GetLocalPoints() const +{ + std::vector localPoints; + if (_bIsFitted && _fLastResult < FLOAT_MAX) { + Base::Vector3d bs(this->_vBase.x,this->_vBase.y,this->_vBase.z); + Base::Vector3d ex(this->_vDirU.x,this->_vDirU.y,this->_vDirU.z); + Base::Vector3d ey(this->_vDirV.x,this->_vDirV.y,this->_vDirV.z); + Base::Vector3d ez(this->_vDirW.x,this->_vDirW.y,this->_vDirW.z); + + localPoints.insert(localPoints.begin(), _vPoints.begin(), _vPoints.end()); + for (std::vector::iterator it = localPoints.begin(); it != localPoints.end(); ++it) { + Base::Vector3d clPoint(it->x,it->y,it->z); + clPoint.TransformToCoordinateSystem(bs, ex, ey); + it->Set(static_cast(clPoint.x), static_cast(clPoint.y), static_cast(clPoint.z)); + } + } + + return localPoints; +} + // ------------------------------------------------------------------------------- bool QuadraticFit::GetCurvatureInfo(double x, double y, double z, @@ -608,24 +620,34 @@ double SurfaceFit::PolynomFit() Base::Vector3d ey(this->_vDirV.x,this->_vDirV.y,this->_vDirV.z); Base::Vector3d ez(this->_vDirW.x,this->_vDirW.y,this->_vDirW.z); -#if defined(FC_USE_BOOST) - ublas::matrix A(6,6); - ublas::vector b(6); - for (int i=0; i<6; i++) { - for (int j=0; j<6; j++) { - A(i,j) = 0.0; - } - b(i) = 0.0; - } -#else + // A*x = b + // See also www.cs.jhu.edu/~misha/Fall05/10.23.05.pdf + // z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f + // z = P * Vi with Vi=(xi^2,yi^2,xiyi,xi,yi,1) and P=(a,b,c,d,e,f) + // To get the best-fit values the sum needs to be minimized: + // S = sum[(z-zi)^2} -> min with zi=z coordinates of the given points + // <=> S = sum[z^2 - 2*z*zi + zi^2] -> min + // <=> S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2] -> min + // To get the optimum the gradient of the expression must be the null vector + // Note: grad F(P) = (P*Vi)^2 = 2*(P*Vi)*Vi + // grad G(P) = -2*(P*Vi)*zi = -2*Vi*zi + // grad H(P) = zi^2 = 0 + // => grad S(P) = sum[2*(P*Vi)*Vi - 2*Vi*zi] = 0 + // <=> sum[(P*Vi)*Vi] = sum[Vi*zi] + // <=> sum[(Vi*Vi^t)*P] = sum[Vi*zi] where (Vi*Vi^t) is a symmetric matrix + // <=> sum[(Vi*Vi^t)]*P = sum[Vi*zi] Eigen::Matrix A = Eigen::Matrix::Zero(); Eigen::Matrix b = Eigen::Matrix::Zero(); Eigen::Matrix x = Eigen::Matrix::Zero(); -#endif + std::vector transform; + transform.reserve(_vPoints.size()); + + double dW2 = 0; for (std::list::const_iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) { Base::Vector3d clPoint(it->x,it->y,it->z); clPoint.TransformToCoordinateSystem(bs, ex, ey); + transform.push_back(clPoint); double dU = clPoint.x; double dV = clPoint.y; double dW = clPoint.z; @@ -634,6 +656,8 @@ double SurfaceFit::PolynomFit() double dV2 = dV*dV; double dUV = dU*dV; + dW2 += dW*dW; + A(0,0) = A(0,0) + dU2*dU2; A(0,1) = A(0,1) + dU2*dV2; A(0,2) = A(0,2) + dU2*dUV; @@ -690,44 +714,84 @@ double SurfaceFit::PolynomFit() A(5,4) = A(4,5); -#if defined(FC_USE_BOOST) - namespace lapack= boost::numeric::bindings::lapack; - //std::clog << A << std::endl; - //std::clog << b << std::endl; - //lapack::gesv(A,b); - ublas::vector x(6); - x = b; - //std::clog << b << std::endl; -#else - // A.llt().solve(b,&x); // not sure if always positive definite - A.qr().solve(b,&x); -#endif + Eigen::HouseholderQR< Eigen::Matrix > qr(A); + x = qr.solve(b); - _fCoeff[0] = (float)(-x(5)); - _fCoeff[1] = (float)(-x(3)); - _fCoeff[2] = (float)(-x(4)); - _fCoeff[3] = 1.0f; - _fCoeff[4] = (float)(-x(0)); - _fCoeff[5] = (float)(-x(1)); - _fCoeff[6] = 0.0f; - _fCoeff[7] = (float)(-x(2)); - _fCoeff[8] = 0.0f; - _fCoeff[9] = 0.0f; + // FunctionContainer gets an implicit function F(x,y,z) = 0 of this form + // _fCoeff[0] + + // _fCoeff[1]*x + _fCoeff[2]*y + _fCoeff[3]*z + + // _fCoeff[4]*x^2 + _fCoeff[5]*y^2 + _fCoeff[6]*z^2 + + // _fCoeff[7]*x*y + _fCoeff[8]*x*z + _fCoeff[9]*y*z + // + // The bivariate polynomial surface we get here is of the form + // z = f(x,y) = a*x^2 + b*y^2 + c*x*y + d*x + e*y + f + // Writing it as implicit surface F(x,y,z) = 0 gives this form + // F(x,y,z) = f(x,y) - z = a*x^2 + b*y^2 + c*x*y + d*x + e*y - z + f + // Thus: + // _fCoeff[0] = f + // _fCoeff[1] = d + // _fCoeff[2] = e + // _fCoeff[3] = -1 + // _fCoeff[4] = a + // _fCoeff[5] = b + // _fCoeff[6] = 0 + // _fCoeff[7] = c + // _fCoeff[8] = 0 + // _fCoeff[9] = 0 - return 0.0f; + _fCoeff[0] = x(5); + _fCoeff[1] = x(3); + _fCoeff[2] = x(4); + _fCoeff[3] = -1.0; + _fCoeff[4] = x(0); + _fCoeff[5] = x(1); + _fCoeff[6] = 0.0; + _fCoeff[7] = x(2); + _fCoeff[8] = 0.0; + _fCoeff[9] = 0.0; + + // Get S(P) = sum[(P*Vi)^2 - 2*(P*Vi)*zi + zi^2] + double sigma = 0; + FunctionContainer clFuncCont(_fCoeff); + for (std::vector::const_iterator it = transform.begin(); it != transform.end(); ++it) { + double u = it->x; + double v = it->y; + double z = clFuncCont.F(u, v, 0.0); + sigma += z*z; + } + + sigma += dW2 - 2 * x.dot(b); + // This must be caused by some round-off errors. Theoretically it's impossible + // that 'sigma' becomes negative. + if (sigma < 0) + sigma = 0; + sigma = sqrt(sigma/_vPoints.size()); + + _fLastResult = static_cast(sigma); + return _fLastResult; } double SurfaceFit::Value(double x, double y) const { - float z = 0.0f; + double z = 0.0; if (_bIsFitted) { FunctionContainer clFuncCont(_fCoeff); - z = (float) clFuncCont.F(x, y, 0.0f); + z = clFuncCont.F(x, y, 0.0); } return z; } +void SurfaceFit::GetCoefficients(double& a,double& b,double& c,double& d,double& e,double& f) const +{ + a = _fCoeff[4]; + b = _fCoeff[5]; + c = _fCoeff[7]; + d = _fCoeff[1]; + e = _fCoeff[2]; + f = _fCoeff[0]; +} + // ----------------------------------------------------------------------------- PolynomialFit::PolynomialFit() @@ -773,7 +837,7 @@ float PolynomialFit::Value(float x, float y) const _fCoeff[3] * y + _fCoeff[4] * x * y + _fCoeff[5] * x * x * y + - _fCoeff[6] * y * y + + _fCoeff[6] * y * y + _fCoeff[7] * x * y * y + _fCoeff[8] * x * x * y * y; return fValue; diff --git a/src/Mod/Mesh/App/Core/Approximation.h b/src/Mod/Mesh/App/Core/Approximation.h index e0ff07fa9..c48d3b317 100644 --- a/src/Mod/Mesh/App/Core/Approximation.h +++ b/src/Mod/Mesh/App/Core/Approximation.h @@ -103,11 +103,11 @@ public: /** * Construction */ - Approximation() : _bIsFitted(false) { } + Approximation(); /** * Destroys the object and frees any allocated resources. */ - virtual ~Approximation(){ Clear(); } + virtual ~Approximation(); /** * Add point for the fit algorithm. */ @@ -188,11 +188,11 @@ public: /** * Construction */ - PlaneFit(){}; + PlaneFit(); /** * Destruction */ - virtual ~PlaneFit(){}; + virtual ~PlaneFit(); Base::Vector3f GetBase() const; Base::Vector3f GetDirU() const; Base::Vector3f GetDirV() const; @@ -229,6 +229,12 @@ public: * Get the dimension of the fitted plane. */ void Dimension(float& length, float& width) const; + /** + * Returns an array of the transformed points relative to the coordinate system + * of the plane. If this method is called before the plane is computed an empty + * array is returned. + */ + std::vector GetLocalPoints() const; protected: Base::Vector3f _vBase; /**< Base vector of the plane. */ @@ -319,7 +325,7 @@ protected: * to get a parametrisation of the points afterwards. The coordinates of the points with respect to the local * coordinate system of the plane are determined and then a quadratic polynomial function of the form: * w = f(u,v) = a*u^2 + b*v^2 + c*u*v + d*u + e*v + f - * is deermined. + * is determined. * This approach was developed as an alternative for the 3D approach with quadrics because * the latter suffers from strange artifacts in planar areas. */ @@ -340,6 +346,7 @@ public: bool GetCurvatureInfo(double x, double y, double z, double &rfCurv0, double &rfcurv1); float Fit(); double Value(double x, double y) const; + void GetCoefficients(double& a,double& b,double& c,double& d,double& e,double& f) const; protected: double PolynomFit(); diff --git a/src/Mod/Mesh/App/MeshTestsApp.py b/src/Mod/Mesh/App/MeshTestsApp.py index e798c111e..d4834d27f 100644 --- a/src/Mod/Mesh/App/MeshTestsApp.py +++ b/src/Mod/Mesh/App/MeshTestsApp.py @@ -1,7 +1,7 @@ # (c) Juergen Riegel (juergen.riegel@web.de) 2007 LGPL import FreeCAD, os, sys, unittest, Mesh -import thread, time, tempfile +import thread, time, tempfile, math #--------------------------------------------------------------------------- @@ -161,3 +161,62 @@ class LoadMeshInThreadsCases(unittest.TestCase): def tearDown(self): pass + + +class PolynomialFitCases(unittest.TestCase): + def setUp(self): + pass + + def testFitGood(self): + # symmetric + v=[] + v.append(FreeCAD.Vector(0,0,0.0)) + v.append(FreeCAD.Vector(1,0,0.5)) + v.append(FreeCAD.Vector(2,0,0.0)) + v.append(FreeCAD.Vector(0,1,0.5)) + v.append(FreeCAD.Vector(1,1,1.0)) + v.append(FreeCAD.Vector(2,1,0.5)) + v.append(FreeCAD.Vector(0,2,0.0)) + v.append(FreeCAD.Vector(1,2,0.5)) + v.append(FreeCAD.Vector(2,2,0.0)) + d = Mesh.polynomialFit(v) + c = d["Coefficients"] + print ("Polynomial: f(x,y)=%f*x^2%+f*y^2%+f*x*y%+f*x%+f*y%+f" % (c[0],c[1],c[2],c[3],c[4],c[5])) + for i in d["Residuals"]: + self.failUnless(math.fabs(i) < 0.0001, "Too high residual %f" % math.fabs(i)) + + def testFitExact(self): + # symmetric + v=[] + v.append(FreeCAD.Vector(0,0,0.0)) + v.append(FreeCAD.Vector(1,0,0.0)) + v.append(FreeCAD.Vector(2,0,0.0)) + v.append(FreeCAD.Vector(0,1,0.0)) + v.append(FreeCAD.Vector(1,1,1.0)) + v.append(FreeCAD.Vector(2,1,0.0)) + d = Mesh.polynomialFit(v) + c = d["Coefficients"] + print ("Polynomial: f(x,y)=%f*x^2%+f*y^2%+f*x*y%+f*x%+f*y%+f" % (c[0],c[1],c[2],c[3],c[4],c[5])) + for i in d["Residuals"]: + self.failUnless(math.fabs(i) < 0.0001, "Too high residual %f" % math.fabs(i)) + + def testFitBad(self): + # symmetric + v=[] + v.append(FreeCAD.Vector(0,0,0.0)) + v.append(FreeCAD.Vector(1,0,0.0)) + v.append(FreeCAD.Vector(2,0,0.0)) + v.append(FreeCAD.Vector(0,1,0.0)) + v.append(FreeCAD.Vector(1,1,1.0)) + v.append(FreeCAD.Vector(2,1,0.0)) + v.append(FreeCAD.Vector(0,2,0.0)) + v.append(FreeCAD.Vector(1,2,0.0)) + v.append(FreeCAD.Vector(2,2,0.0)) + d = Mesh.polynomialFit(v) + c = d["Coefficients"] + print ("Polynomial: f(x,y)=%f*x^2%+f*y^2%+f*x*y%+f*x%+f*y%+f" % (c[0],c[1],c[2],c[3],c[4],c[5])) + for i in d["Residuals"]: + self.failIf(math.fabs(i) < 0.0001, "Residual %f must be higher" % math.fabs(i)) + + def tearDown(self): + pass