131 lines
5.2 KiB
C++
131 lines
5.2 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 *
|
|
* *
|
|
***************************************************************************/
|
|
|
|
|
|
#ifndef MESH_SPHERE_FIT_H
|
|
#define MESH_SPHERE_FIT_H
|
|
|
|
#include "Approximation.h"
|
|
#include <Eigen/Eigenvalues>
|
|
|
|
// -------------------------------------------------------------------------------
|
|
namespace MeshCoreFit {
|
|
|
|
typedef Eigen::Matrix<double,4,4,Eigen::RowMajor> 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
|