From 63fcfb4802d0f472303732baff8163510ff12709 Mon Sep 17 00:00:00 2001 From: wmayer Date: Thu, 16 Apr 2020 14:58:30 +0200 Subject: [PATCH] Mesh: [skip ci] add sphere and cylinder fitting algorithms --- src/Mod/Mesh/App/CMakeLists.txt | 4 + src/Mod/Mesh/App/Core/Approximation.cpp | 97 +++- src/Mod/Mesh/App/Core/CylinderFit.cpp | 654 ++++++++++++++++++++++++ src/Mod/Mesh/App/Core/CylinderFit.h | 151 ++++++ src/Mod/Mesh/App/Core/SphereFit.cpp | 427 ++++++++++++++++ src/Mod/Mesh/App/Core/SphereFit.h | 130 +++++ 6 files changed, 1459 insertions(+), 4 deletions(-) create mode 100644 src/Mod/Mesh/App/Core/CylinderFit.cpp create mode 100644 src/Mod/Mesh/App/Core/CylinderFit.h create mode 100644 src/Mod/Mesh/App/Core/SphereFit.cpp create mode 100644 src/Mod/Mesh/App/Core/SphereFit.h diff --git a/src/Mod/Mesh/App/CMakeLists.txt b/src/Mod/Mesh/App/CMakeLists.txt index ca6c698a70..7ef7aa1354 100644 --- a/src/Mod/Mesh/App/CMakeLists.txt +++ b/src/Mod/Mesh/App/CMakeLists.txt @@ -94,6 +94,10 @@ SET(Core_SRCS Core/Utilities.h Core/Visitor.cpp Core/Visitor.h + Core/CylinderFit.cpp + Core/CylinderFit.h + Core/SphereFit.cpp + Core/SphereFit.h ) SOURCE_GROUP("Core" FILES ${Core_SRCS}) diff --git a/src/Mod/Mesh/App/Core/Approximation.cpp b/src/Mod/Mesh/App/Core/Approximation.cpp index 97658d35fa..c04343db0d 100644 --- a/src/Mod/Mesh/App/Core/Approximation.cpp +++ b/src/Mod/Mesh/App/Core/Approximation.cpp @@ -32,6 +32,8 @@ #include "Approximation.h" #include "Elements.h" #include "Utilities.h" +#include "CylinderFit.h" +#include "SphereFit.h" #include #include @@ -1051,6 +1053,29 @@ float CylinderFit::Fit() _fRadius = float(radius); _fLastResult = double(fit); + +#if defined(_DEBUG) + Base::Console().Message(" WildMagic Cylinder Fit: Base: (%0.4f, %0.4f, %0.4f), Axis: (%0.6f, %0.6f, %0.6f), Radius: %0.4f, Std Dev: %0.4f\n", + _vBase.x, _vBase.y, _vBase.z, _vAxis.x, _vAxis.y, _vAxis.z, _fRadius, GetStdDeviation()); +#endif + + MeshCoreFit::CylinderFit cylFit; + cylFit.AddPoints(_vPoints); + //cylFit.SetApproximations(_fRadius, Base::Vector3d(_vBase.x, _vBase.y, _vBase.z), Base::Vector3d(_vAxis.x, _vAxis.y, _vAxis.z)); + + float result = cylFit.Fit(); + if (result < FLOAT_MAX) { + Base::Vector3d base = cylFit.GetBase(); + Base::Vector3d dir = cylFit.GetAxis(); +#if defined(_DEBUG) + Base::Console().Message("MeshCoreFit::Cylinder Fit: Base: (%0.4f, %0.4f, %0.4f), Axis: (%0.6f, %0.6f, %0.6f), Radius: %0.4f, Std Dev: %0.4f, Iterations: %d\n", + base.x, base.y, base.z, dir.x, dir.y, dir.z, cylFit.GetRadius(), cylFit.GetStdDeviation(), cylFit.GetNumIterations()); +#endif + _vBase = Base::convertTo(base); + _vAxis = Base::convertTo(dir); + _fRadius = (float)cylFit.GetRadius(); + _fLastResult = result; + } #else int m = static_cast(_vPoints.size()); int n = 7; @@ -1238,24 +1263,88 @@ float SphereFit::Fit() _fRadius = float(sphere.Radius); // TODO - _fLastResult = 0; + +#if defined(_DEBUG) + Base::Console().Message(" WildMagic Sphere Fit: Center: (%0.4f, %0.4f, %0.4f), Radius: %0.4f, Std Dev: %0.4f\n", + _vCenter.x, _vCenter.y, _vCenter.z, _fRadius, GetStdDeviation()); +#endif + + MeshCoreFit::SphereFit sphereFit; + sphereFit.AddPoints(_vPoints); + sphereFit.ComputeApproximations(); + float result = sphereFit.Fit(); + if (result < FLOAT_MAX) { + Base::Vector3d center = sphereFit.GetCenter(); +#if defined(_DEBUG) + Base::Console().Message("MeshCoreFit::Sphere Fit: Center: (%0.4f, %0.4f, %0.4f), Radius: %0.4f, Std Dev: %0.4f, Iterations: %d\n", + center.x, center.y, center.z, sphereFit.GetRadius(), sphereFit.GetStdDeviation(), sphereFit.GetNumIterations()); +#endif + _vCenter = Base::convertTo(center); + _fRadius = (float)sphereFit.GetRadius(); + _fLastResult = result; + } + return _fLastResult; } -float SphereFit::GetDistanceToSphere(const Base::Vector3f &) const +float SphereFit::GetDistanceToSphere(const Base::Vector3f& rcPoint) const { - return FLOAT_MAX; + float fResult = FLOAT_MAX; + if (_bIsFitted) { + fResult = Base::Vector3f(rcPoint - _vCenter).Length() - _fRadius; + } + return fResult; } float SphereFit::GetStdDeviation() const { - return FLOAT_MAX; + // Mean: M=(1/N)*SUM Xi + // Variance: VAR=(N/N-3)*[(1/N)*SUM(Xi^2)-M^2] + // Standard deviation: SD=SQRT(VAR) + // Standard error of the mean: SE=SD/SQRT(N) + if (!_bIsFitted) + return FLOAT_MAX; + + float fSumXi = 0.0f, fSumXi2 = 0.0f, + fMean = 0.0f, fDist = 0.0f; + + float ulPtCt = float(CountPoints()); + std::list< Base::Vector3f >::const_iterator cIt; + + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) { + fDist = GetDistanceToSphere( *cIt ); + fSumXi += fDist; + fSumXi2 += ( fDist * fDist ); + } + + fMean = (1.0f / ulPtCt) * fSumXi; + return sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean)); } void SphereFit::ProjectToSphere() { + for (std::list< Base::Vector3f >::iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) { + Base::Vector3f& cPnt = *it; + // Compute unit vector from sphere centre to point. + // Because this vector is orthogonal to the sphere's surface at the + // intersection point we can easily compute the projection point on the + // closest surface point using the radius of the sphere + Base::Vector3f diff = cPnt - _vCenter; + double length = diff.Length(); + if (length == 0.0) + { + // Point is exactly at the sphere center, so it can be projected in any direction onto the sphere! + // So here just project in +Z direction + cPnt.z += _fRadius; + } + else + { + diff /= length; // normalizing the vector + cPnt = _vCenter + diff * _fRadius; + } + } } // ------------------------------------------------------------------------------- diff --git a/src/Mod/Mesh/App/Core/CylinderFit.cpp b/src/Mod/Mesh/App/Core/CylinderFit.cpp new file mode 100644 index 0000000000..3cd2fa770a --- /dev/null +++ b/src/Mod/Mesh/App/Core/CylinderFit.cpp @@ -0,0 +1,654 @@ +/*************************************************************************** + * Copyright (c) 2020 Graeme van der Vlugt * + * * + * 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 * + * * + ***************************************************************************/ + +// Definitions: +// Cylinder axis goes through a point (Xc,Yc,Zc) and has direction (L,M,N) +// Cylinder radius is R +// A point on the axis (X0i,Y0i,Z0i) can be described by: +// (X0i,Y0i,Z0i) = (Xc,Yc,Zc) + s(L,M,N) +// where s is the distance from (Xc,Yc,Zc) to (X0i,Y0i,Z0i) when (L,M,N) is +// of unit length (normalized). +// The distance between a cylinder surface point (Xi,Yi,Zi) and its +// projection onto the axis (X0i,Y0i,Z0i) is the radius: +// (Xi - X0i)^2 + (Yi - Y0i)^2 + (Zi - Z0i)^2 = R^2 +// Also the vector to a cylinder surface point (Xi,Yi,Zi) from its +// projection onto the axis (X0i,Y0i,Z0i) is orthogonal to the axis so we can +// write: +// (Xi - X0i, Yi - Y0i, Zi - Z0i).(L,M,N) = 0 or +// L(Xi - X0i) + M(Yi - Y0i) + N(Zi - Z0i) = 0 +// If we substitute these various equations into each other and further add +// the constraint that L^2 + M^2 + N^2 = 1 then we can arrive at a single +// equation for the cylinder surface points: +// (Xi - Xc + L*L*(Xc - Xi) + L*M*(Yc - Yi) + L*N*(Zc - Zi))^2 + +// (Yi - Yc + M*L*(Xc - Xi) + M*M*(Yc - Yi) + M*N*(Zc - Zi))^2 + +// (Zi - Zc + N*L*(Xc - Xi) + N*M*(Yc - Yi) + N*N*(Zc - Zi))^2 - R^2 = 0 +// This equation is what is used in the least squares solution below. Because +// we are constraining the direction vector to a unit length and also because +// we need to stop the axis point from moving along the axis we need to fix one +// of the ordinates in the solution. So from our initial approximations for the +// axis direction (L0,M0,N0): +// if (L0 > M0) && (L0 > N0) then fix Xc = 0 and use L = sqrt(1 - M^2 - N^2) +// else if (M0 > L0) && (M0 > N0) then fix Yc = 0 and use M = sqrt(1 - L^2 - N^2) +// else if (N0 > L0) && (N0 > M0) then fix Zc = 0 and use N = sqrt(1 - L^2 - M^2) +// We thus solve for 5 unknown parameters. +// Thus for the solution to succeed the initial axis direction should be reasonable. + +#include "PreCompiled.h" + +#ifndef _PreComp_ +# include +# include +# include +#endif + +#include "CylinderFit.h" +#include +#include + +using namespace MeshCoreFit; + +CylinderFit::CylinderFit() + : _vBase(0,0,0) + , _vAxis(0,0,1) + , _dRadius(0) + , _numIter(0) + , _posConvLimit(0.0001) + , _dirConvLimit(0.000001) + , _vConvLimit(0.001) + , _maxIter(50) +{ +} + +CylinderFit::~CylinderFit() +{ +} + +// Set approximations before calling the fitting +void CylinderFit::SetApproximations(double radius, const Base::Vector3d &base, const Base::Vector3d &axis) +{ + _bIsFitted = false; + _fLastResult = FLOAT_MAX; + _numIter = 0; + _dRadius = radius; + _vBase = base; + _vAxis = axis; + _vAxis.Normalize(); +} + +// Set iteration convergence criteria for the fit if special values are needed. +// The default values set in the constructor are suitable for most uses +void CylinderFit::SetConvergenceCriteria(double posConvLimit, double dirConvLimit, double vConvLimit, int maxIter) +{ + if (posConvLimit > 0.0) + _posConvLimit = posConvLimit; + if (dirConvLimit > 0.0) + _dirConvLimit = dirConvLimit; + if (vConvLimit > 0.0) + _vConvLimit = vConvLimit; + if (maxIter > 0) + _maxIter = maxIter; +} + + +double CylinderFit::GetRadius() const +{ + if (_bIsFitted) + return _dRadius; + else + return 0.0; +} + +Base::Vector3d CylinderFit::GetBase() const +{ + if (_bIsFitted) + return _vBase; + else + return Base::Vector3d(); +} + +Base::Vector3d CylinderFit::GetAxis() const +{ + if (_bIsFitted) + return _vAxis; + else + return Base::Vector3d(); +} + +int CylinderFit::GetNumIterations() const +{ + if (_bIsFitted) + return _numIter; + else + return 0; +} + +float CylinderFit::GetDistanceToCylinder(const Base::Vector3f &rcPoint) const +{ + float fResult = FLOAT_MAX; + if (_bIsFitted) + { + fResult = Base::Vector3d(rcPoint.x, rcPoint.y, rcPoint.z).DistanceToLine(_vBase, _vAxis) - _dRadius; + } + return fResult; +} + +float CylinderFit::GetStdDeviation() const +{ + // Mean: M=(1/N)*SUM Xi + // Variance: VAR=(N/N-3)*[(1/N)*SUM(Xi^2)-M^2] + // Standard deviation: SD=SQRT(VAR) + // Standard error of the mean: SE=SD/SQRT(N) + if (!_bIsFitted) + return FLOAT_MAX; + + float fSumXi = 0.0f, fSumXi2 = 0.0f, + fMean = 0.0f, fDist = 0.0f; + + float ulPtCt = float(CountPoints()); + std::list< Base::Vector3f >::const_iterator cIt; + + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) { + fDist = GetDistanceToCylinder( *cIt ); + fSumXi += fDist; + fSumXi2 += ( fDist * fDist ); + } + + fMean = (1.0f / ulPtCt) * fSumXi; + return sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean)); +} + +void CylinderFit::ProjectToCylinder() +{ + Base::Vector3f cBase(_vBase.x, _vBase.y, _vBase.z); + Base::Vector3f cAxis(_vAxis.x, _vAxis.y, _vAxis.z); + + for (std::list< Base::Vector3f >::iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) { + Base::Vector3f& cPnt = *it; + if (cPnt.DistanceToLine(cBase, cAxis) > 0) { + Base::Vector3f proj; + cBase.ProjectToPlane(cPnt, cAxis, proj); + Base::Vector3f diff = cPnt - proj; + diff.Normalize(); + cPnt = proj + diff * _dRadius; + } + else { + // Point is on the cylinder axis, so it can be moved in + // any direction perpendicular to the cylinder axis + Base::Vector3f cMov(cPnt); + do { + float x = (float(rand()) / float(RAND_MAX)); + float y = (float(rand()) / float(RAND_MAX)); + float z = (float(rand()) / float(RAND_MAX)); + cMov.Move(x,y,z); + } + while (cMov.DistanceToLine(cBase, cAxis) == 0); + + Base::Vector3f proj; + cMov.ProjectToPlane(cPnt, cAxis, proj); + Base::Vector3f diff = cPnt - proj; + diff.Normalize(); + cPnt = proj + diff * _dRadius; + } + } +} + +// Compute approximations for the parameters using all points by computing a +// line through the points. This doesn't work well if the points are only from +// one small surface area. +// In that case rather use SetApproximations() with a better estimate. +void CylinderFit::ComputeApproximationsLine() +{ + _bIsFitted = false; + _fLastResult = FLOAT_MAX; + _numIter = 0; + _vBase.Set(0.0, 0.0, 0.0); + _vAxis.Set(0.0, 0.0, 0.0); + _dRadius = 0.0; + if (_vPoints.size() > 0) + { + std::vector input; + std::transform(_vPoints.begin(), _vPoints.end(), std::back_inserter(input), + [](const Base::Vector3f& v) { return Wm4::Vector3d(v.x, v.y, v.z); }); + Wm4::Line3 kLine = Wm4::OrthogonalLineFit3(input.size(), input.data()); + _vBase.Set(kLine.Origin.X(), kLine.Origin.Y(), kLine.Origin.Z()); + _vAxis.Set(kLine.Direction.X(), kLine.Direction.Y(), kLine.Direction.Z()); + + for (std::list< Base::Vector3f >::const_iterator cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) + _dRadius += Base::Vector3d(cIt->x, cIt->y, cIt->z).DistanceToLine(_vBase, _vAxis); + _dRadius /= (double)_vPoints.size(); + } +} + +float CylinderFit::Fit() +{ + _bIsFitted = false; + _fLastResult = FLOAT_MAX; + _numIter = 0; + + // A minimum of 5 surface points is needed to define a cylinder + if (CountPoints() < 5) + return FLOAT_MAX; + + // If approximations have not been set/computed then compute some now using the line fit method + if (_dRadius == 0.0) + ComputeApproximationsLine(); + + // Check parameters to define the best solution direction + // There are 7 parameters but 2 are actually dependent on the others + // so we are actually solving for 5 parameters. + // order of parameters depending on the solution direction: + // solL: Yc, Zc, M, N, R + // solM: Xc, Zc, L, N, R + // solN: Xc, Yc, L, M, R + SolutionD solDir; + findBestSolDirection(solDir); + + // Initialise some matrices and vectors + std::vector< Base::Vector3d > residuals(CountPoints(), Base::Vector3d(0.0, 0.0, 0.0)); + Matrix5x5 atpa; + Eigen::VectorXd atpl(5); + + // Iteration loop... + double sigma0; + bool cont = true; + while (cont && (_numIter < _maxIter)) + { + ++_numIter; + + // Set up the quasi parameteric normal equations + setupNormalEquationMatrices(solDir, residuals, atpa, atpl); + + // Solve the equations for the unknown corrections + Eigen::LLT< Matrix5x5 > llt(atpa); + if (llt.info() != Eigen::Success) + return FLOAT_MAX; + Eigen::VectorXd x = llt.solve(atpl); + + // Check parameter convergence + cont = false; + if ((fabs(x(0)) > _posConvLimit) || (fabs(x(1)) > _posConvLimit) || // the two position parameter corrections + (fabs(x(2)) > _dirConvLimit) || (fabs(x(3)) > _dirConvLimit) || // the two direction parameter corrections + (fabs(x(4)) > _posConvLimit)) // the radius correction + cont = true; + + // Before updating the unknowns, compute the residuals and sigma0 and check the residual convergence + bool vConverged; + if (!computeResiduals(solDir, x, residuals, sigma0, _vConvLimit, vConverged)) + return FLOAT_MAX; + if (!vConverged) + cont = true; + + // Update the parameters + if (!updateParameters(solDir, x)) + return FLOAT_MAX; + } + + // Check for convergence + if (cont) + return FLOAT_MAX; + + _bIsFitted = true; + _fLastResult = sigma0; + + return _fLastResult; +} + +// Checks initial parameter values and defines the best solution direction to use +// Axis direction = (L,M,N) +// solution L: L is biggest axis component and L = f(M,N) and X = 0 (we move the base point along axis so that x = 0) +// solution M: M is biggest axis component and M = f(L,N) and Y = 0 (we move the base point along axis so that y = 0) +// solution N: N is biggest axis component and N = f(L,M) and Z = 0 (we move the base point along axis so that z = 0) +// IMPLEMENT: use fix X,Y,or Z to value of associated centre of gravity coordinate +// (because 0 could be along way away from cylinder points) +void CylinderFit::findBestSolDirection(SolutionD &solDir) +{ + // Choose the best of the three solution 'directions' to use + // This is to avoid a square root of a negative number when computing the dependent parameters + Base::Vector3d dir = _vAxis; + Base::Vector3d pos = _vBase; + dir.Normalize(); + double biggest = dir.x; + solDir = solL; + if (fabs (dir.y) > fabs (biggest)) + { + biggest = dir.y; + solDir = solM; + } + if (fabs (dir.z) > fabs (biggest)) + { + biggest = dir.z; + solDir = solN; + } + if (biggest < 0.0) + dir.Set(-dir.x, -dir.y, -dir.z); // multiplies by -1 + + double lambda; + switch (solDir) + { + case solL: + lambda = -pos.x / dir.x; + pos.x = 0.0; + pos.y = pos.y + lambda * dir.y; + pos.z = pos.z + lambda * dir.z; + break; + case solM: + lambda = -pos.y / dir.y; + pos.x = pos.x + lambda * dir.x; + pos.y = 0.0; + pos.z = pos.z + lambda * dir.z; + break; + case solN: + lambda = -pos.z / dir.z; + pos.x = pos.x + lambda * dir.x; + pos.y = pos.y + lambda * dir.y; + pos.z = 0.0; + break; + } + _vAxis = dir; + _vBase = pos; +} + +// Set up the normal equation matrices +// atpa ... 5x5 normal matrix +// atpl ... 5x1 matrix (right-hand side of equation) +void CylinderFit::setupNormalEquationMatrices(SolutionD solDir, const std::vector< Base::Vector3d > &residuals, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const +{ + // Zero matrices + atpa.setZero(); + atpl.setZero(); + + // For each point, setup the observation equation coefficients and add their + // contribution into the the normal equation matrices + double a[5], b[3]; + double f0, qw; + std::vector< Base::Vector3d >::const_iterator vIt = residuals.begin(); + std::list< Base::Vector3f >::const_iterator cIt; + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) + { + // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc.... + setupObservation(solDir, *cIt, *vIt, a, f0, qw, b); + addObservationU(a, f0, qw, atpa, atpl); + // } + } + setLowerPart(atpa); +} + +// Sets up contributions of given observation to the quasi parameteric +// normal equation matrices. Assumes uncorrelated coordinates. +// point ... point +// residual ... residual for this point computed from previous iteration (zero for first iteration) +// a[5] ... parameter partials +// f0 ... reference to f0 term +// qw ... reference to quasi weight (here we are assuming equal unit weights for each observed point coordinate) +// b[3] ... observation partials +void CylinderFit::setupObservation(SolutionD solDir, const Base::Vector3f &point, const Base::Vector3d &residual, double a[5], double &f0, double &qw, double b[3]) const +{ + // This adjustment requires an update of the observation approximations + // because the residuals do not have a linear relationship. + // New estimates for the observations: + double xEstimate = (double)point.x + residual.x; + double yEstimate = (double)point.y + residual.y; + double zEstimate = (double)point.z + residual.z; + + // intermediate parameters + double lambda = _vAxis.x * (xEstimate - _vBase.x) + _vAxis.y * (yEstimate - _vBase.y) + _vAxis.z * (zEstimate - _vBase.z); + double x0 = _vBase.x + lambda * _vAxis.x; + double y0 = _vBase.y + lambda * _vAxis.y; + double z0 = _vBase.z + lambda * _vAxis.z; + double dx = xEstimate - x0; + double dy = yEstimate - y0; + double dz = zEstimate - z0; + double dx00 = _vBase.x - xEstimate; + double dy00 = _vBase.y - yEstimate; + double dz00 = _vBase.z - zEstimate; + + // partials of the observations + b[0] = 2.0 * (dx - _vAxis.x * _vAxis.x * dx - _vAxis.x * _vAxis.y * dy - _vAxis.x * _vAxis.z * dz); + b[1] = 2.0 * (dy - _vAxis.x * _vAxis.y * dx - _vAxis.y * _vAxis.y * dy - _vAxis.y * _vAxis.z * dz); + b[2] = 2.0 * (dz - _vAxis.x * _vAxis.z * dx - _vAxis.y * _vAxis.z * dy - _vAxis.z * _vAxis.z * dz); + + // partials of the parameters + switch (solDir) + { + double ddxdl, ddydl, ddzdl; + double ddxdm, ddydm, ddzdm; + double ddxdn, ddydn, ddzdn; + case solL: + // order of parameters: Yc, Zc, M, N, R + ddxdm = -2.0 * _vAxis.y * dx00 + (_vAxis.x - _vAxis.y * _vAxis.y / _vAxis.x) * dy00 - (_vAxis.y * _vAxis.z / _vAxis.x) * dz00; + ddydm = (_vAxis.x - _vAxis.y * _vAxis.y / _vAxis.x) * dx00 + 2.0 * _vAxis.y * dy00 + _vAxis.z * dz00; + ddzdm = -(_vAxis.y * _vAxis.z / _vAxis.x) * dx00 + _vAxis.z * dy00; + ddxdn = -2.0 * _vAxis.z * dx00 - (_vAxis.y * _vAxis.z / _vAxis.x) * dy00 + (_vAxis.x - _vAxis.z * _vAxis.z / _vAxis.x) * dz00; + ddydn = -(_vAxis.y * _vAxis.z / _vAxis.x) * dx00 + _vAxis.y * dz00; + ddzdn = (_vAxis.x - _vAxis.z * _vAxis.z / _vAxis.x) * dx00 + _vAxis.y * dy00 + 2.0 * _vAxis.z * dz00; + a[0] = -b[1]; + a[1] = -b[2]; + a[2] = 2.0 * (dx * ddxdm + dy * ddydm + dz * ddzdm); + a[3] = 2.0 * (dx * ddxdn + dy * ddydn + dz * ddzdn); + a[4] = -2.0 * _dRadius; + break; + case solM: + // order of parameters: Xc, Zc, L, N, R + ddxdl = 2.0 * _vAxis.x * dx00 + (_vAxis.y - _vAxis.x * _vAxis.x / _vAxis.y) * dy00 + _vAxis.z * dz00; + ddydl = (_vAxis.y - _vAxis.x * _vAxis.x / _vAxis.y) * dx00 - 2.0 * _vAxis.x * dy00 - (_vAxis.x * _vAxis.z / _vAxis.y) * dz00; + ddzdl = _vAxis.z * dx00 - (_vAxis.x * _vAxis.z / _vAxis.y) * dy00; + ddxdn = -(_vAxis.x * _vAxis.z / _vAxis.y) * dy00 + _vAxis.x * dz00; + ddydn = -(_vAxis.x * _vAxis.z / _vAxis.y) * dx00 - 2.0 * _vAxis.z * dy00 + (_vAxis.y - _vAxis.z * _vAxis.z / _vAxis.y) * dz00; + ddzdn = _vAxis.x * dx00 + (_vAxis.y - _vAxis.z * _vAxis.z / _vAxis.y) * dy00 + 2.0 * _vAxis.z * dz00; + a[0] = -b[0]; + a[1] = -b[2]; + a[2] = 2.0 * (dx * ddxdl + dy * ddydl + dz * ddzdl); + a[3] = 2.0 * (dx * ddxdn + dy * ddydn + dz * ddzdn); + a[4] = -2.0 * _dRadius; + break; + case solN: + // order of parameters: Xc, Yc, L, M, R + ddxdl = 2.0 * _vAxis.x * dx00 + _vAxis.y * dy00 + (_vAxis.z - _vAxis.x * _vAxis.x / _vAxis.z) * dz00; + ddydl = _vAxis.y * dx00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dz00; + ddzdl = (_vAxis.z - _vAxis.x * _vAxis.x / _vAxis.z) * dx00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dy00 - 2.0 * _vAxis.x * dz00; + ddxdm = _vAxis.x * dy00 - (_vAxis.x * _vAxis.y / _vAxis.z) * dz00; + ddydm = _vAxis.x * dx00 + 2.0 * _vAxis.y * dy00 + (_vAxis.z - _vAxis.y * _vAxis.y / _vAxis.z) * dz00; + ddzdm = - (_vAxis.x * _vAxis.y / _vAxis.z) * dx00 + (_vAxis.z - _vAxis.y * _vAxis.y / _vAxis.z) * dy00 - 2.0 * _vAxis.y * dz00; + a[0] = -b[0]; + a[1] = -b[1]; + a[2] = 2.0 * (dx * ddxdl + dy * ddydl + dz * ddzdl); + a[3] = 2.0 * (dx * ddxdm + dy * ddydm + dz * ddzdm); + a[4] = -2.0 * _dRadius; + break; + } + + // free term + f0 = _dRadius * _dRadius - dx * dx - dy * dy - dz * dz + b[0] * residual.x + b[1] * residual.y + b[2] * residual.z; + + // quasi weight (using equal weights for cylinder point coordinate observations) + //w[0] = 1.0; + //w[1] = 1.0; + //w[2] = 1.0; + //qw = 1.0 / (b[0] * b[0] / w[0] + b[1] * b[1] / w[1] + b[2] * b[2] / w[2]); + qw = 1.0 / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]); +} + +// Computes contribution of the given observation equation on the normal equation matrices +// Call this for each observation (point) +// Here we only add the contribution to the upper part of the normal matrix +// and then after all observations have been added we need to set the lower part +// (which is symmetrical to the upper part) +// a[5] ... parameter partials +// li ... free term (f0) +// pi ... weight of observation (= quasi weight qw for this solution) +// atpa ... 5x5 normal equation matrix +// atpl ... 5x1 matrix/vector (right-hand side of equations) +void CylinderFit::addObservationU(double a[5], double li, double pi, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const +{ + for (int i = 0; i < 5; ++i) + { + double aipi = a[i] * pi; + for (int j = i; j < 5; ++j) + { + atpa(i, j) += aipi * a[j]; + //atpa(j, i) = atpa(i, j); // it's a symmetrical matrix, we'll set this later after all observations processed + } + atpl(i) += aipi * li; + } +} + +// Set the lower part of the normal matrix equal to the upper part +// This is done after all the observations have been added +void CylinderFit::setLowerPart(Matrix5x5 &atpa) const +{ + for (int i = 0; i < 5; ++i) + for (int j = i+1; j < 5; ++j) // skip the diagonal elements + atpa(j, i) = atpa(i, j); +} + +// Compute the residuals and sigma0 and check the residual convergence +bool CylinderFit::computeResiduals(SolutionD solDir, const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const +{ + vConverged = true; + int nPtsUsed = 0; + sigma0 = 0.0; + double a[5], b[3]; + double f0, qw; + //double maxdVx = 0.0; + //double maxdVy = 0.0; + //double maxdVz = 0.0; + //double rmsVv = 0.0; + std::vector< Base::Vector3d >::iterator vIt = residuals.begin(); + std::list< Base::Vector3f >::const_iterator cIt; + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) + { + // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc.... + ++nPtsUsed; + Base::Vector3d &v = *vIt; + setupObservation(solDir, *cIt, v, a, f0, qw, b); + double qv = -f0; + for (int i = 0; i < 5; ++i) + qv += a[i] * x(i); + + // We are using equal weights for cylinder point coordinate observations (see setupObservation) + // i.e. w[0] = w[1] = w[2] = 1.0; + //double vx = -qw * qv * b[0] / w[0]; + //double vy = -qw * qv * b[1] / w[1]; + //double vz = -qw * qv * b[2] / w[2]; + double vx = -qw * qv * b[0]; + double vy = -qw * qv * b[1]; + double vz = -qw * qv * b[2]; + double dVx = fabs(vx - v.x); + double dVy = fabs(vy - v.y); + double dVz = fabs(vz - v.z); + v.x = vx; + v.y = vy; + v.z = vz; + + //double vv = v.x * v.x + v.y * v.y + v.z * v.z; + //rmsVv += vv * vv; + + //sigma0 += v.x * w[0] * v.x + v.y * w[1] * v.y + v.z * w[2] * v.z; + sigma0 += v.x * v.x + v.y * v.y + v.z * v.z; + + if ((dVx > vConvLimit) || (dVy > vConvLimit) || (dVz > vConvLimit)) + vConverged = false; + + //if (dVx > maxdVx) + // maxdVx = dVx; + //if (dVy > maxdVy) + // maxdVy = dVy; + //if (dVz > maxdVz) + // maxdVz = dVz; + } + + // Compute degrees of freedom and sigma0 + if (nPtsUsed < 5) // A minimum of 5 surface points is needed to define a cylinder + { + sigma0 = 0.0; + return false; + } + int df = nPtsUsed - 5; + if (df == 0) + sigma0 = 0.0; + else + sigma0 = sqrt (sigma0 / (double)df); + + //rmsVv = sqrt(rmsVv / (double)nPtsUsed); + //Base::Console().Message("X: %0.3e %0.3e %0.3e %0.3e %0.3e , Max dV: %0.4f %0.4f %0.4f , RMS Vv: %0.4f\n", x(0), x(1), x(2), x(3), x(4), maxdVx, maxdVy, maxdVz, rmsVv); + + return true; +} + +// Update the parameters after solving the normal equations +bool CylinderFit::updateParameters(SolutionD solDir, const Eigen::VectorXd &x) +{ + // Update the parameters used as unknowns in the solution + switch (solDir) + { + case solL: // order of parameters: Yc, Zc, M, N, R + _vBase.y += x(0); + _vBase.z += x(1); + _vAxis.y += x(2); + _vAxis.z += x(3); + _dRadius += x(4); + break; + case solM: // order of parameters: Xc, Zc, L, N, R + _vBase.x += x(0); + _vBase.z += x(1); + _vAxis.x += x(2); + _vAxis.z += x(3); + _dRadius += x(4); + break; + case solN: // order of parameters: Xc, Yc, L, M, R + _vBase.x += x(0); + _vBase.y += x(1); + _vAxis.x += x(2); + _vAxis.y += x(3); + _dRadius += x(4); + break; + } + + // Update the dependent axis direction parameter + double l2, m2, n2; + switch (solDir) + { + case solL: + l2 = 1.0 - _vAxis.y * _vAxis.y - _vAxis.z * _vAxis.z; + if (l2 <= 0.0) + return false; // L*L <= 0 ! + _vAxis.x = sqrt(l2); + //_vBase.x = 0.0; // should already be 0 + break; + case solM: + m2 = 1.0 - _vAxis.x * _vAxis.x - _vAxis.z * _vAxis.z; + if (m2 <= 0.0) + return false; // M*M <= 0 ! + _vAxis.y = sqrt(m2); + //_vBase.y = 0.0; // should already be 0 + break; + case solN: + n2 = 1.0 - _vAxis.x * _vAxis.x - _vAxis.y * _vAxis.y; + if (n2 <= 0.0) + return false; // N*N <= 0 ! + _vAxis.z = sqrt(n2); + //_vBase.z = 0.0; // should already be 0 + break; + } + + return true; +} diff --git a/src/Mod/Mesh/App/Core/CylinderFit.h b/src/Mod/Mesh/App/Core/CylinderFit.h new file mode 100644 index 0000000000..520fad2c9f --- /dev/null +++ b/src/Mod/Mesh/App/Core/CylinderFit.h @@ -0,0 +1,151 @@ +/*************************************************************************** + * Copyright (c) 2020 Graeme van der Vlugt * + * * + * 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 * + * * + ***************************************************************************/ + + +#ifndef MESH_CYLINDER_FIT_H +#define MESH_CYLINDER_FIT_H + +#include "Approximation.h" +#include + +// ------------------------------------------------------------------------------- +namespace MeshCoreFit { + +typedef Eigen::Matrix Matrix5x5; + +/** + * Best-fit cylinder for a given set of points. + * Doesn't expect points on any top or bottom end-planes, only points on the side surface + */ +class MeshExport CylinderFit : public MeshCore::Approximation +{ +protected: + // Solution 'direction' enumeration + enum SolutionD {solL = 0, // solution L: L is biggest axis component and L = f(M,N) + solM = 1, // solution M: M is biggest axis component and M = f(L,N) + solN = 2 // solution N: N is biggest axis component and N = f(L,M) + }; +public: + /** + * Construction + */ + CylinderFit(); + /** + * Destruction + */ + virtual ~CylinderFit(); + + /** + * Set approximations before calling Fit() + */ + void SetApproximations(double radius, const Base::Vector3d &base, const Base::Vector3d &axis); + /** + * Set iteration convergence criteria for the fit if special values are needed. + * The default values set in the constructor are suitable for most uses + */ + void SetConvergenceCriteria(double posConvLimit, double dirConvLimit, double vConvLimit, int maxIter); + /** + * Returns the radius of the fitted cylinder. If Fit() has not been called then zero is returned. + */ + double GetRadius() const; + /** + * Returns the base of the fitted cylinder. If Fit() has not been called the null vector is returned. + */ + Base::Vector3d GetBase() const; + /** + * Returns the axis of the fitted cylinder. If Fit() has not been called the null vector is returned. + */ + Base::Vector3d GetAxis() const; + /** + * Returns the number of iterations that Fit() needed to converge. If Fit() has not been called then zero is returned. + */ + int GetNumIterations() const; + /** + * Fit a cylinder into the given points. If the fit fails FLOAT_MAX is returned. + */ + float Fit(); + /** + * Returns the distance from the point \a rcPoint to the fitted cylinder. If Fit() has not been + * called FLOAT_MAX is returned. + */ + float GetDistanceToCylinder(const Base::Vector3f &rcPoint) const; + /** + * Returns the standard deviation from the points to the fitted cylinder. If Fit() has not been + * called FLOAT_MAX is returned. + */ + float GetStdDeviation() const; + /** + * Projects the points onto the fitted cylinder. + */ + void ProjectToCylinder(); + +protected: + /** + * Compute approximations for the parameters using all points using the line fit method + */ + void ComputeApproximationsLine(); + /** + * Checks initial parameter values and defines the best solution direction to use + */ + void findBestSolDirection(SolutionD &solDir); + /** + * Set up the normal equations + */ + void setupNormalEquationMatrices(SolutionD solDir, const std::vector< Base::Vector3d > &residuals, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const; + /** + * Sets up contributions of given observation to the normal equation matrices. + */ + void setupObservation(SolutionD solDir, const Base::Vector3f &point, const Base::Vector3d &residual, double a[5], double &f0, double &qw, double b[3]) const; + /** + * Computes contribution of the given observation equation on the normal equation matrices + */ + void addObservationU(double a[5], double li, double pi, Matrix5x5 &atpa, Eigen::VectorXd &atpl) const; + /** + * Set the lower part of the normal matrix equal to the upper part + */ + void setLowerPart(Matrix5x5 &atpa) const; + + /** + * Compute the residuals and sigma0 and check the residual convergence + */ + bool computeResiduals(SolutionD solDir, const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const; + /** + * Update the parameters after solving the normal equations + */ + bool updateParameters(SolutionD solDir, const Eigen::VectorXd &x); + +protected: + Base::Vector3d _vBase; /**< Base vector of the cylinder (point on axis). */ + Base::Vector3d _vAxis; /**< Axis of the cylinder. */ + double _dRadius; /**< Radius of the cylinder. */ + int _numIter; /**< Number of iterations for solution to converge. */ + double _posConvLimit; /**< Position and radius parameter convergence threshold. */ + double _dirConvLimit; /**< Direction parameter convergence threshold. */ + double _vConvLimit; /**< Residual convergence threshold. */ + int _maxIter; /**< Maximum number of iterations. */ + +}; + + +} // namespace MeshCore + +#endif // MESH_CYLINDER_FIT_H diff --git a/src/Mod/Mesh/App/Core/SphereFit.cpp b/src/Mod/Mesh/App/Core/SphereFit.cpp new file mode 100644 index 0000000000..5285c69d1f --- /dev/null +++ b/src/Mod/Mesh/App/Core/SphereFit.cpp @@ -0,0 +1,427 @@ +/*************************************************************************** + * Copyright (c) 2020 Graeme van der Vlugt * + * * + * 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" + +#ifndef _PreComp_ +# include +# include +# include +#endif + +#include "SphereFit.h" +#include + +using namespace MeshCoreFit; + +SphereFit::SphereFit() + : _vCenter(0,0,0) + , _dRadius(0) + , _numIter(0) + , _posConvLimit(0.0001) + , _vConvLimit(0.001) + , _maxIter(50) +{ +} + +SphereFit::~SphereFit() +{ +} + +// Set approximations before calling the fitting +void SphereFit::SetApproximations(double radius, const Base::Vector3d ¢er) +{ + _bIsFitted = false; + _fLastResult = FLOAT_MAX; + _numIter = 0; + _dRadius = radius; + _vCenter = center; +} + +// Set iteration convergence criteria for the fit if special values are needed. +// The default values set in the constructor are suitable for most uses +void SphereFit::SetConvergenceCriteria(double posConvLimit, double vConvLimit, int maxIter) +{ + if (posConvLimit > 0.0) + _posConvLimit = posConvLimit; + if (vConvLimit > 0.0) + _vConvLimit = vConvLimit; + if (maxIter > 0) + _maxIter = maxIter; +} + + +double SphereFit::GetRadius() const +{ + if (_bIsFitted) + return _dRadius; + else + return 0.0; +} + +Base::Vector3d SphereFit::GetCenter() const +{ + if (_bIsFitted) + return _vCenter; + else + return Base::Vector3d(); +} + +int SphereFit::GetNumIterations() const +{ + if (_bIsFitted) + return _numIter; + else + return 0; +} + +float SphereFit::GetDistanceToSphere(const Base::Vector3f &rcPoint) const +{ + float fResult = FLOAT_MAX; + if (_bIsFitted) + { + fResult = Base::Vector3d((double)rcPoint.x - _vCenter.x, (double)rcPoint.y - _vCenter.y, (double)rcPoint.z - _vCenter.z).Length() - _dRadius; + } + return fResult; +} + +float SphereFit::GetStdDeviation() const +{ + // Mean: M=(1/N)*SUM Xi + // Variance: VAR=(N/N-3)*[(1/N)*SUM(Xi^2)-M^2] + // Standard deviation: SD=SQRT(VAR) + // Standard error of the mean: SE=SD/SQRT(N) + if (!_bIsFitted) + return FLOAT_MAX; + + float fSumXi = 0.0f, fSumXi2 = 0.0f, + fMean = 0.0f, fDist = 0.0f; + + float ulPtCt = float(CountPoints()); + std::list< Base::Vector3f >::const_iterator cIt; + + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) { + fDist = GetDistanceToSphere( *cIt ); + fSumXi += fDist; + fSumXi2 += ( fDist * fDist ); + } + + fMean = (1.0f / ulPtCt) * fSumXi; + return sqrt((ulPtCt / (ulPtCt - 3.0f)) * ((1.0f / ulPtCt) * fSumXi2 - fMean * fMean)); +} + +void SphereFit::ProjectToSphere() +{ + for (std::list< Base::Vector3f >::iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) { + Base::Vector3f& cPnt = *it; + + // Compute unit vector from sphere centre to point. + // Because this vector is orthogonal to the sphere's surface at the + // intersection point we can easily compute the projection point on the + // closest surface point using the radius of the sphere + Base::Vector3d diff((double)cPnt.x - _vCenter.x, (double)cPnt.y - _vCenter.y, (double)cPnt.z - _vCenter.z); + double length = diff.Length(); + if (length == 0.0) + { + // Point is exactly at the sphere center, so it can be projected in any direction onto the sphere! + // So here just project in +Z direction + cPnt.z += (float)_dRadius; + } + else + { + diff /= length; // normalizing the vector + Base::Vector3d proj = _vCenter + diff * _dRadius; + cPnt.x = (float)proj.x; + cPnt.y = (float)proj.y; + cPnt.z = (float)proj.z; + } + } +} + +// Compute approximations for the parameters using all points: +// Set centre to centre of gravity of points and radius to the average +// distance from the centre of gravity to the points. +void SphereFit::ComputeApproximations() +{ + _bIsFitted = false; + _fLastResult = FLOAT_MAX; + _numIter = 0; + _vCenter.Set(0.0, 0.0, 0.0); + _dRadius = 0.0; + if (_vPoints.size() > 0) + { + std::list< Base::Vector3f >::const_iterator cIt; + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) + { + _vCenter.x += cIt->x; + _vCenter.y += cIt->y; + _vCenter.z += cIt->z; + } + _vCenter /= (double)_vPoints.size(); + + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt) + { + Base::Vector3d diff((double)cIt->x - _vCenter.x, (double)cIt->y - _vCenter.y, (double)cIt->z - _vCenter.z); + _dRadius += diff.Length(); + } + _dRadius /= (double)_vPoints.size(); + } +} + +float SphereFit::Fit() +{ + _bIsFitted = false; + _fLastResult = FLOAT_MAX; + _numIter = 0; + + // A minimum of 4 surface points is needed to define a sphere + if (CountPoints() < 4) + return FLOAT_MAX; + + // If approximations have not been set/computed then compute some now + if (_dRadius == 0.0) + ComputeApproximations(); + + // Initialise some matrices and vectors + std::vector< Base::Vector3d > residuals(CountPoints(), Base::Vector3d(0.0, 0.0, 0.0)); + Matrix4x4 atpa; + Eigen::VectorXd atpl(4); + + // Iteration loop... + double sigma0; + bool cont = true; + while (cont && (_numIter < _maxIter)) + { + ++_numIter; + + // Set up the quasi parameteric normal equations + setupNormalEquationMatrices(residuals, atpa, atpl); + + // Solve the equations for the unknown corrections + Eigen::LLT< Matrix4x4 > llt(atpa); + if (llt.info() != Eigen::Success) + return FLOAT_MAX; + Eigen::VectorXd x = llt.solve(atpl); + + // Check parameter convergence (order of parameters: X,Y,Z,R) + cont = false; + if ((fabs(x(0)) > _posConvLimit) || (fabs(x(1)) > _posConvLimit) || + (fabs(x(2)) > _posConvLimit) || (fabs(x(3)) > _posConvLimit)) + cont = true; + + // Before updating the unknowns, compute the residuals and sigma0 and check the residual convergence + bool vConverged; + if (!computeResiduals(x, residuals, sigma0, _vConvLimit, vConverged)) + return FLOAT_MAX; + if (!vConverged) + cont = true; + + // Update the parameters (order of parameters: X,Y,Z,R) + _vCenter.x += x(0); + _vCenter.y += x(1); + _vCenter.z += x(2); + _dRadius += x(3); + } + + // Check for convergence + if (cont) + return FLOAT_MAX; + + _bIsFitted = true; + _fLastResult = sigma0; + + return _fLastResult; +} + +// Set up the normal equation matrices +// atpa ... 4x4 normal matrix +// atpl ... 4x1 matrix (right-hand side of equation) +void SphereFit::setupNormalEquationMatrices(const std::vector< Base::Vector3d > &residuals, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const +{ + // Zero matrices + atpa.setZero(); + atpl.setZero(); + + // For each point, setup the observation equation coefficients and add their + // contribution into the the normal equation matrices + double a[4], b[3]; + double f0, qw; + std::vector< Base::Vector3d >::const_iterator vIt = residuals.begin(); + std::list< Base::Vector3f >::const_iterator cIt; + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) + { + // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc.... + setupObservation(*cIt, *vIt, a, f0, qw, b); + addObservationU(a, f0, qw, atpa, atpl); + // } + } + setLowerPart(atpa); +} + +// Sets up contributions of given observation to the quasi parameteric +// normal equation matrices. Assumes uncorrelated coordinates. +// point ... point +// residual ... residual for this point computed from previous iteration (zero for first iteration) +// a[4] ... parameter partials (order of parameters: X,Y,Z,R) +// f0 ... reference to f0 term +// qw ... reference to quasi weight (here we are assuming equal unit weights for each observed point coordinate) +// b[3] ... observation partials +void SphereFit::setupObservation(const Base::Vector3f &point, const Base::Vector3d &residual, double a[4], double &f0, double &qw, double b[3]) const +{ + // This adjustment requires an update of the observation approximations + // because the residuals do not have a linear relationship. + // New estimates for the observations: + double xEstimate = (double)point.x + residual.x; + double yEstimate = (double)point.y + residual.y; + double zEstimate = (double)point.z + residual.z; + + // partials of the observations + double dx = xEstimate - _vCenter.x; + double dy = yEstimate - _vCenter.y; + double dz = zEstimate - _vCenter.z; + b[0] = 2.0 * dx; + b[1] = 2.0 * dy; + b[2] = 2.0 * dz; + + // partials of the parameters + a[0] = -b[0]; + a[1] = -b[1]; + a[2] = -b[2]; + a[3] = -2.0 * _dRadius; + + // free term + f0 = _dRadius * _dRadius - dx * dx - dy * dy - dz * dz + b[0] * residual.x + b[1] * residual.y + b[2] * residual.z; + + // quasi weight (using equal weights for sphere point coordinate observations) + //w[0] = 1.0; + //w[1] = 1.0; + //w[2] = 1.0; + //qw = 1.0 / (b[0] * b[0] / w[0] + b[1] * b[1] / w[1] + b[2] * b[2] / w[2]); + qw = 1.0 / (b[0] * b[0] + b[1] * b[1] + b[2] * b[2]); +} + +// Computes contribution of the given observation equation on the normal equation matrices +// Call this for each observation (point) +// Here we only add the contribution to the upper part of the normal matrix +// and then after all observations have been added we need to set the lower part +// (which is symmetrical to the upper part) +// a[4] ... parameter partials +// li ... free term (f0) +// pi ... weight of observation (= quasi weight qw for this solution) +// atpa ... 4x4 normal equation matrix +// atpl ... 4x1 matrix/vector (right-hand side of equations) +void SphereFit::addObservationU(double a[4], double li, double pi, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const +{ + for (int i = 0; i < 4; ++i) + { + double aipi = a[i] * pi; + for (int j = i; j < 4; ++j) + { + atpa(i, j) += aipi * a[j]; + //atpa(j, i) = atpa(i, j); // it's a symmetrical matrix, we'll set this later after all observations processed + } + atpl(i) += aipi * li; + } +} + +// Set the lower part of the normal matrix equal to the upper part +// This is done after all the observations have been added +void SphereFit::setLowerPart(Matrix4x4 &atpa) const +{ + for (int i = 0; i < 4; ++i) + for (int j = i+1; j < 4; ++j) // skip the diagonal elements + atpa(j, i) = atpa(i, j); +} + +// Compute the residuals and sigma0 and check the residual convergence +bool SphereFit::computeResiduals(const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const +{ + vConverged = true; + int nPtsUsed = 0; + sigma0 = 0.0; + double a[4], b[3]; + double f0, qw; + //double maxdVx = 0.0; + //double maxdVy = 0.0; + //double maxdVz = 0.0; + //double rmsVv = 0.0; + std::vector< Base::Vector3d >::iterator vIt = residuals.begin(); + std::list< Base::Vector3f >::const_iterator cIt; + for (cIt = _vPoints.begin(); cIt != _vPoints.end(); ++cIt, ++vIt) + { + // if (using this point) { // currently all given points are used (could modify this if eliminating outliers, etc.... + ++nPtsUsed; + Base::Vector3d &v = *vIt; + setupObservation(*cIt, v, a, f0, qw, b); + double qv = -f0; + for (int i = 0; i < 4; ++i) + qv += a[i] * x(i); + + // We are using equal weights for sphere point coordinate observations (see setupObservation) + // i.e. w[0] = w[1] = w[2] = 1.0; + //double vx = -qw * qv * b[0] / w[0]; + //double vy = -qw * qv * b[1] / w[1]; + //double vz = -qw * qv * b[2] / w[2]; + double vx = -qw * qv * b[0]; + double vy = -qw * qv * b[1]; + double vz = -qw * qv * b[2]; + double dVx = fabs(vx - v.x); + double dVy = fabs(vy - v.y); + double dVz = fabs(vz - v.z); + v.x = vx; + v.y = vy; + v.z = vz; + + //double vv = v.x * v.x + v.y * v.y + v.z * v.z; + //rmsVv += vv * vv; + + //sigma0 += v.x * w[0] * v.x + v.y * w[1] * v.y + v.z * w[2] * v.z; + sigma0 += v.x * v.x + v.y * v.y + v.z * v.z; + + if ((dVx > vConvLimit) || (dVy > vConvLimit) || (dVz > vConvLimit)) + vConverged = false; + + //if (dVx > maxdVx) + // maxdVx = dVx; + //if (dVy > maxdVy) + // maxdVy = dVy; + //if (dVz > maxdVz) + // maxdVz = dVz; + } + + // Compute degrees of freedom and sigma0 + if (nPtsUsed < 4) // A minimum of 4 surface points is needed to define a sphere + { + sigma0 = 0.0; + return false; + } + int df = nPtsUsed - 4; + if (df == 0) + sigma0 = 0.0; + else + sigma0 = sqrt (sigma0 / (double)df); + + //rmsVv = sqrt(rmsVv / (double)nPtsUsed); + //Base::Console().Message("X: %0.3e %0.3e %0.3e %0.3e , Max dV: %0.4f %0.4f %0.4f , RMS Vv: %0.4f\n", x(0), x(1), x(2), x(3), maxdVx, maxdVy, maxdVz, rmsVv); + + return true; +} diff --git a/src/Mod/Mesh/App/Core/SphereFit.h b/src/Mod/Mesh/App/Core/SphereFit.h new file mode 100644 index 0000000000..1b475f36a0 --- /dev/null +++ b/src/Mod/Mesh/App/Core/SphereFit.h @@ -0,0 +1,130 @@ +/*************************************************************************** + * Copyright (c) 2020 Graeme van der Vlugt * + * * + * 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 * + * * + ***************************************************************************/ + + +#ifndef MESH_SPHERE_FIT_H +#define MESH_SPHERE_FIT_H + +#include "Approximation.h" +#include + +// ------------------------------------------------------------------------------- +namespace MeshCoreFit { + +typedef Eigen::Matrix Matrix4x4; + +/** + * Best-fit sphere for a given set of points. + */ +class MeshExport SphereFit : public MeshCore::Approximation +{ +public: + /** + * Construction + */ + SphereFit(); + /** + * Destruction + */ + virtual ~SphereFit(); + + /** + * Set approximations before calling Fit() + */ + void SetApproximations(double radius, const Base::Vector3d ¢er); + /** + * Set iteration convergence criteria for the fit if special values are needed. + * The default values set in the constructor are suitable for most uses + */ + void SetConvergenceCriteria(double posConvLimit, double vConvLimit, int maxIter); + /** + * Returns the radius of the fitted sphere. If Fit() has not been called then zero is returned. + */ + double GetRadius() const; + /** + * Returns the center of the fitted sphere. If Fit() has not been called the null vector is returned. + */ + Base::Vector3d GetCenter() const; + /** + * Returns the number of iterations that Fit() needed to converge. If Fit() has not been called then zero is returned. + */ + int GetNumIterations() const; + /** + * Compute approximations for the parameters using all points + */ + void ComputeApproximations(); + /** + * Fit a sphere onto the given points. If the fit fails FLOAT_MAX is returned. + */ + float Fit(); + /** + * Returns the distance from the point \a rcPoint to the fitted sphere. If Fit() has not been + * called FLOAT_MAX is returned. + */ + float GetDistanceToSphere(const Base::Vector3f &rcPoint) const; + /** + * Returns the standard deviation from the points to the fitted sphere. If Fit() has not been + * called FLOAT_MAX is returned. + */ + float GetStdDeviation() const; + /** + * Projects the points onto the fitted sphere. + */ + void ProjectToSphere(); + +protected: + /** + * Set up the normal equations + */ + void setupNormalEquationMatrices(const std::vector< Base::Vector3d > &residuals, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const; + /** + * Sets up contributions of given observation to the normal equation matrices. + */ + void setupObservation(const Base::Vector3f &point, const Base::Vector3d &residual, double a[4], double &f0, double &qw, double b[3]) const; + /** + * Computes contribution of the given observation equation on the normal equation matrices + */ + void addObservationU(double a[4], double li, double pi, Matrix4x4 &atpa, Eigen::VectorXd &atpl) const; + /** + * Set the lower part of the normal matrix equal to the upper part + */ + void setLowerPart(Matrix4x4 &atpa) const; + + /** + * Compute the residuals and sigma0 and check the residual convergence + */ + bool computeResiduals(const Eigen::VectorXd &x, std::vector< Base::Vector3d > &residuals, double &sigma0, double vConvLimit, bool &vConverged) const; + +protected: + Base::Vector3d _vCenter;/**< Center of sphere. */ + double _dRadius; /**< Radius of the sphere. */ + int _numIter; /**< Number of iterations for solution to converge. */ + double _posConvLimit; /**< Position and radius parameter convergence threshold. */ + double _vConvLimit; /**< Residual convergence threshold. */ + int _maxIter; /**< Maximum number of iterations. */ + +}; + + +} // namespace MeshCore + +#endif // MESH_SPHERE_FIT_H