Skip to content

Commit

Permalink
Mesh: [skip ci] cleanup cylinder fit
Browse files Browse the repository at this point in the history
  • Loading branch information
@wwmayer
wwmayer committed May 12, 2020
1 parent 6b022b2 commit 99ab82e
Show file tree
Hide file tree
Showing 2 changed files with 27 additions and 49 deletions.
30 changes: 3 additions & 27 deletions src/Mod/Mesh/App/Core/Approximation.cpp
View file
Expand Up @@ -1109,43 +1109,19 @@ float CylinderFit::Fit()
_bIsFitted = true;

#if 1
std::vector<Wm4::Vector3d> 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::Vector3d cnt, axis;
if (_initialGuess) {
cnt = Base::convertTo<Wm4::Vector3d>(_vBase);
axis = Base::convertTo<Wm4::Vector3d>(_vAxis);
}

double radius, height;
Wm4::CylinderFit3<double> fit(input.size(), input.data(), cnt, axis, radius, height, _initialGuess);
_initialGuess = false;

_vBase = Base::convertTo<Base::Vector3f>(cnt);
_vAxis = Base::convertTo<Base::Vector3f>(axis);
_fRadius = float(radius);

_fLastResult = double(fit);

#if defined(FC_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

// Do the cylinder fit
MeshCoreFit::CylinderFit cylFit;
cylFit.AddPoints(_vPoints);
if (_fLastResult < FLOAT_MAX)
if (_initialGuess)
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(FC_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::Console().Log("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::Vector3f>(base);
Expand Down
46 changes: 24 additions & 22 deletions src/Mod/Mesh/App/Core/SphereFit.cpp
View file
Expand Up @@ -72,7 +72,7 @@ void SphereFit::SetConvergenceCriteria(double posConvLimit, double vConvLimit, i

double SphereFit::GetRadius() const
{
if (_bIsFitted)
if (_bIsFitted)
return _dRadius;
else
return 0.0;
Expand All @@ -88,20 +88,20 @@ Base::Vector3d SphereFit::GetCenter() const

int SphereFit::GetNumIterations() const
{
if (_bIsFitted)
if (_bIsFitted)
return _numIter;
else
return 0;
}

float SphereFit::GetDistanceToSphere(const Base::Vector3f &rcPoint) const
{
float fResult = FLOAT_MAX;
if (_bIsFitted)
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;
return fResult;
}

float SphereFit::GetStdDeviation() const
Expand Down Expand Up @@ -130,8 +130,8 @@ float SphereFit::GetStdDeviation() const

void SphereFit::ProjectToSphere()
{
for (std::list< Base::Vector3f >::iterator it = _vPoints.begin(); it != _vPoints.end(); ++it) {
Base::Vector3f& cPnt = *it;
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
Expand All @@ -141,7 +141,7 @@ void SphereFit::ProjectToSphere()
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!
// 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;
}
Expand All @@ -153,7 +153,7 @@ void SphereFit::ProjectToSphere()
cPnt.y = (float)proj.y;
cPnt.z = (float)proj.z;
}
}
}
}

// Compute approximations for the parameters using all points:
Expand Down Expand Up @@ -188,13 +188,13 @@ void SphereFit::ComputeApproximations()

float SphereFit::Fit()
{
_bIsFitted = false;
_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 (CountPoints() < 4)
return FLOAT_MAX;

// If approximations have not been set/computed then compute some now
if (_dRadius == 0.0)
Expand Down Expand Up @@ -245,8 +245,8 @@ float SphereFit::Fit()
if (cont)
return FLOAT_MAX;

_bIsFitted = true;
_fLastResult = sigma0;
_bIsFitted = true;
_fLastResult = sigma0;

return _fLastResult;
}
Expand All @@ -264,9 +264,9 @@ void SphereFit::setupNormalEquationMatrices(const std::vector< Base::Vector3d >
// 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)
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);
Expand All @@ -292,7 +292,7 @@ void SphereFit::setupObservation(const Base::Vector3f &point, const Base::Vector
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;
Expand All @@ -309,7 +309,7 @@ void SphereFit::setupObservation(const Base::Vector3f &point, const Base::Vector

// 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;
Expand Down Expand Up @@ -347,8 +347,10 @@ void SphereFit::addObservationU(double a[4], double li, double pi, Matrix4x4 &at
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
Expand All @@ -363,9 +365,9 @@ bool SphereFit::computeResiduals(const Eigen::VectorXd &x, std::vector< Base::Ve
//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)
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;
Expand Down

0 comments on commit 99ab82e

Please sign in to comment.