kindred/create
1
1
Fork 0
Code Issues 37 Pull Requests Actions Packages Projects 9 Releases 2 Wiki Activity

Mesh: [skip ci] cleanup cylinder fit

Browse Source
This commit is contained in:
wmayer
2020-05-12 15:11:01 +02:00
parent 6b022b277a
commit 99ab82ed0b
2 changed files with 27 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/Mod/Mesh/App/Core/Approximation.cpp
View File

@@ -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);

46
src/Mod/Mesh/App/Core/SphereFit.cpp
View File

@@ -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;
@@ -88,7 +88,7 @@ Base::Vector3d SphereFit::GetCenter() const
int SphereFit::GetNumIterations() const
{
if (_bIsFitted)
if (_bIsFitted)
return _numIter;
else
return 0;
@@ -96,12 +96,12 @@ int SphereFit::GetNumIterations() const
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
@@ -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
@@ -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;
}
@@ -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:
@@ -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)
@@ -245,8 +245,8 @@ float SphereFit::Fit()
if (cont)
return FLOAT_MAX;
_bIsFitted = true;
_fLastResult = sigma0;
_bIsFitted = true;
_fLastResult = sigma0;
return _fLastResult;
}
@@ -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);
@@ -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;
@@ -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;
@@ -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
@@ -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;