428 lines
14 KiB
C++
428 lines
14 KiB
C++
/***************************************************************************
|
|
* 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 <algorithm>
|
|
# include <cstdlib>
|
|
# include <iterator>
|
|
#endif
|
|
|
|
#include "SphereFit.h"
|
|
#include <Base/Console.h>
|
|
|
|
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-1)*[(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 - 1.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;
|
|
}
|