You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
396 lines
16 KiB
396 lines
16 KiB
// David Eberly, Geometric Tools, Redmond WA 98052
|
|
// Copyright (c) 1998-2021
|
|
// Distributed under the Boost Software License, Version 1.0.
|
|
// https://www.boost.org/LICENSE_1_0.txt
|
|
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
|
|
// Version: 4.0.2019.08.13
|
|
|
|
#pragma once
|
|
|
|
#include <Mathematics/ArbitraryPrecision.h>
|
|
#include <Mathematics/ApprOrthogonalPlane3.h>
|
|
#include <Mathematics/RootsPolynomial.h>
|
|
#include <Mathematics/GaussNewtonMinimizer.h>
|
|
#include <Mathematics/LevenbergMarquardtMinimizer.h>
|
|
|
|
// Let the torus center be C with plane of symmetry containing C and having
|
|
// directions D0 and D1. The axis of symmetry is the line containing C and
|
|
// having direction N (the plane normal). The radius from the center of the
|
|
// torus is r0 and the radius of the tube of the torus is r1. A point P may
|
|
// be written as P = C + x*D0 + y*D1 + z*N, where matrix [D0 D1 N] is
|
|
// orthogonal and has determinant 1. Thus, x = Dot(D0,P-C), y = Dot(D1,P-C)
|
|
// and z = Dot(N,P-C). The implicit equation defining the torus is
|
|
// (|P-C|^2 + r0^2 - r1^2)^2 - 4*r0^2*(|P-C|^2 - (Dot(N,P-C))^2) = 0
|
|
// Observe that D0 and D1 are not present in the equation, which is to be
|
|
// expected by the symmetry.
|
|
//
|
|
// Define u = r0^2 and v = r0^2 - r1^2. Define
|
|
// F(X;C,N,u,v) = (|P-C|^2 + v)^2 - 4*u*(|P-C|^2 - (Dot(N,P-C))^2)
|
|
// The nonlinear least-squares fitting of points {X[i]}_{i=0}^{n-1} computes
|
|
// C, N, u and v to minimize the error function
|
|
// E(C,N,u,v) = sum_{i=0}^{n-1} F(X[i];C,N,u,v)^2
|
|
// When the sample points are distributed so that there is large coverage
|
|
// by a purported fitted torus, a variation on fitting is the following.
|
|
// Compute the least-squares plane with origin C and normal N that fits the
|
|
// points. Define G(X;u,v) = F(X;C,N,u,v); the only variables now are u and
|
|
// v. Define L[i] = |X[i]-C|^2 and S[i] = 4 * (L[i] - (Dot(N,X[i]-C))^2).
|
|
// Define the error function
|
|
// H(u,v) = sum_{i=0}^{n-1} G(X[i];u,v)^2
|
|
// = sum_{i=0}^{n-1} ((v + L[i])^2 - S[i]*u)^2
|
|
// The first-order partial derivatives are
|
|
// dH/du = -2 sum_{i=0}^{n-1} ((v + L[i])^2 - S[i]*u) * S[i]
|
|
// dH/dv = 4 sum_{i=0}^{n-1} ((v + L[i])^2 - S[i]*u) * (v + L[i])
|
|
// Setting these to zero and expanding the terms, we have
|
|
// 0 = a2 * v^2 + a1 * v + a0 - b0 * u
|
|
// 0 = c3 * v^3 + c2 * v^2 + c1 * v + c0 - u * (d1 * v + d0)
|
|
// where a2 = sum(S[i]), a1 = 2*sum(S[i]*L[i]), a2 = sum(S[i]*L[i]^2),
|
|
// b0 = sum(S[i]^2), c3 = sum(1) = n, c2 = 3*sum(L[i]), c1 = 3*sum(L[i]^2),
|
|
// c0 = sum(L[i]^3), d1 = sum(S[i]) = a2 and d0 = sum(S[i]*L[i]) = a1/2.
|
|
// The first equation is solved for
|
|
// u = (a2 * v^2 + a1 * v + a0) / b0 = e2 * v^2 + e1 * v + e0
|
|
// and substituted into the second equation to obtain a cubic polynomial
|
|
// equation
|
|
// 0 = f3 * v^3 + f2 * v^2 + f1 * v + f0
|
|
// where f3 = c3 - d1 * e2, f2 = c2 - d1 * e1 - d0 * e2,
|
|
// f1 = c1 - d1 * e0 - d0 * e1 and f0 = c0 - d0 * e0. The positive v-roots
|
|
// are computed. For each root compute the corresponding u. For all pairs
|
|
// (u,v) with u > v > 0, evaluate H(u,v) and choose the pair that minimizes
|
|
// H(u,v). The torus radii are r0 = sqrt(u) and r1 = sqrt(u - v).
|
|
|
|
namespace gte
|
|
{
|
|
template <typename Real>
|
|
class ApprTorus3
|
|
{
|
|
public:
|
|
ApprTorus3()
|
|
{
|
|
// The unit-length normal is
|
|
// N = (cos(theta)*sin(phi), sin(theta)*sin(phi), cos(phi)
|
|
// for theta in [0,2*pi) and phi in [0,*pi). The radii are
|
|
// encoded as
|
|
// u = r0^2, v = r0^2 - r1^2
|
|
// with 0 < v < u. Let D = C - X[i] where X[i] is a sample point.
|
|
// The parameters P = (C0,C1,C2,theta,phi,u,v).
|
|
|
|
// F[i](C,theta,phi,u,v) =
|
|
// (|D|^2 + v)^2 - 4*u*(|D|^2 - Dot(N,D)^2)
|
|
mFFunction = [this](GVector<Real> const& P, GVector<Real>& F)
|
|
{
|
|
Real csTheta = std::cos(P[3]);
|
|
Real snTheta = std::sin(P[3]);
|
|
Real csPhi = std::cos(P[4]);
|
|
Real snPhi = std::sin(P[4]);
|
|
Vector<3, Real> C = { P[0], P[1], P[2] };
|
|
Vector<3, Real> N = { csTheta * snPhi, snTheta * snPhi, csPhi };
|
|
Real u = P[5];
|
|
Real v = P[6];
|
|
for (int i = 0; i < mNumPoints; ++i)
|
|
{
|
|
Vector<3, Real> D = C - mPoints[i];
|
|
Real DdotD = Dot(D, D), NdotD = Dot(N, D);
|
|
Real sum = DdotD + v;
|
|
F[i] = sum * sum - (Real)4 * u * (DdotD - NdotD * NdotD);
|
|
}
|
|
};
|
|
|
|
// dF[i]/dC = 4 * (|D|^2 + v) * D - 8 * u * (I - N*N^T) * D
|
|
// dF[i]/dTheta = 8 * u * Dot(dN/dTheta, D)
|
|
// dF[i]/dPhi = 8 * u * Dot(dN/dPhi, D)
|
|
// dF[i]/du = -4 * u * (|D|^2 - Dot(N,D)^2)
|
|
// dF[i]/dv = 2 * (|D|^2 + v)
|
|
mJFunction = [this](GVector<Real> const& P, GMatrix<Real>& J)
|
|
{
|
|
Real const r2(2), r4(4), r8(8);
|
|
Real csTheta = std::cos(P[3]);
|
|
Real snTheta = std::sin(P[3]);
|
|
Real csPhi = std::cos(P[4]);
|
|
Real snPhi = std::sin(P[4]);
|
|
Vector<3, Real> C = { P[0], P[1], P[2] };
|
|
Vector<3, Real> N = { csTheta * snPhi, snTheta * snPhi, csPhi };
|
|
Real u = P[5];
|
|
Real v = P[6];
|
|
for (int row = 0; row < mNumPoints; ++row)
|
|
{
|
|
Vector<3, Real> D = C - mPoints[row];
|
|
Real DdotD = Dot(D, D), NdotD = Dot(N, D);
|
|
Real sum = DdotD + v;
|
|
Vector<3, Real> dNdTheta{ -snTheta * snPhi, csTheta * snPhi, (Real)0 };
|
|
Vector<3, Real> dNdPhi{ csTheta * csPhi, snTheta * csPhi, -snPhi };
|
|
Vector<3, Real> temp = r4 * sum * D - r8 * u * (D - NdotD * N);
|
|
J(row, 0) = temp[0];
|
|
J(row, 1) = temp[1];
|
|
J(row, 2) = temp[2];
|
|
J(row, 3) = r8 * u * Dot(dNdTheta, D);
|
|
J(row, 4) = r8 * u * Dot(dNdPhi, D);
|
|
J(row, 5) = -r4 * u * (DdotD - NdotD * NdotD);
|
|
J(row, 6) = r2 * sum;
|
|
}
|
|
};
|
|
}
|
|
|
|
// When the samples are distributed approximately uniformly near a
|
|
// torus, use this method. For example, if the purported torus has
|
|
// center (0,0,0) and normal (0,0,1), you want the (x,y,z) samples
|
|
// to occur in all 8 octants. If the samples occur, say, only in
|
|
// one octant, this method will estimate a C and N that are nowhere
|
|
// near (0,0,0) and (0,0,1). The function sets the output variables
|
|
// C, N, r0 and r1 as the fitted torus.
|
|
//
|
|
// The return value is a pair <bool,Real>. The first element is
|
|
// 'true' when the estimate is valid, in which case the second
|
|
// element is the least-squares error for that estimate. If any
|
|
// unexpected condition occurs that prevents computing an estimate,
|
|
// the first element is 'false' and the second element is
|
|
// std::numeric_limits<Real>::max().
|
|
std::pair<bool, Real>
|
|
operator()(int numPoints, Vector<3, Real> const* points,
|
|
Vector<3, Real>& C, Vector<3, Real>& N, Real& r0, Real& r1) const
|
|
{
|
|
ApprOrthogonalPlane3<Real> fitter;
|
|
if (!fitter.Fit(numPoints, points))
|
|
{
|
|
return std::make_pair(false, std::numeric_limits<Real>::max());
|
|
}
|
|
C = fitter.GetParameters().first;
|
|
N = fitter.GetParameters().second;
|
|
|
|
Real const zero(0);
|
|
Real a0 = zero, a1 = zero, a2 = zero, b0 = zero;
|
|
Real c0 = zero, c1 = zero, c2 = zero, c3 = (Real)numPoints;
|
|
for (int i = 0; i < numPoints; ++i)
|
|
{
|
|
Vector<3, Real> delta = points[i] - C;
|
|
Real dot = Dot(N, delta);
|
|
Real L = Dot(delta, delta), L2 = L * L, L3 = L * L2;
|
|
Real S = (Real)4 * (L - dot * dot), S2 = S * S;
|
|
a2 += S;
|
|
a1 += S * L;
|
|
a0 += S * L2;
|
|
b0 += S2;
|
|
c2 += L;
|
|
c1 += L2;
|
|
c0 += L3;
|
|
}
|
|
Real d1 = a2;
|
|
Real d0 = a1;
|
|
a1 *= (Real)2;
|
|
c2 *= (Real)3;
|
|
c1 *= (Real)3;
|
|
Real invB0 = (Real)1 / b0;
|
|
Real e0 = a0 * invB0;
|
|
Real e1 = a1 * invB0;
|
|
Real e2 = a2 * invB0;
|
|
|
|
Rational f0 = c0 - d0 * e0;
|
|
Rational f1 = c1 - d1 * e0 - d0 * e1;
|
|
Rational f2 = c2 - d1 * e1 - d0 * e2;
|
|
Rational f3 = c3 - d1 * e2;
|
|
std::map<Real, int> rmMap;
|
|
RootsPolynomial<Real>::SolveCubic(f0, f1, f2, f3, rmMap);
|
|
|
|
Real hmin = std::numeric_limits<Real>::max();
|
|
Real umin = zero, vmin = zero;
|
|
for (auto const& element : rmMap)
|
|
{
|
|
Real v = element.first;
|
|
if (v > zero)
|
|
{
|
|
Real u = e0 + v * (e1 + v * e2);
|
|
if (u > v)
|
|
{
|
|
Real h = zero;
|
|
for (int i = 0; i < numPoints; ++i)
|
|
{
|
|
Vector<3, Real> delta = points[i] - C;
|
|
Real dot = Dot(N, delta);
|
|
Real L = Dot(delta, delta);
|
|
Real S = (Real)4 * (L - dot * dot);
|
|
Real sum = v + L;
|
|
Real term = sum * sum - S * u;
|
|
h += term * term;
|
|
}
|
|
if (h < hmin)
|
|
{
|
|
hmin = h;
|
|
umin = u;
|
|
vmin = v;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (hmin == std::numeric_limits<Real>::max())
|
|
{
|
|
return std::make_pair(false, std::numeric_limits<Real>::max());
|
|
}
|
|
|
|
r0 = std::sqrt(umin);
|
|
r1 = std::sqrt(umin - vmin);
|
|
return std::make_pair(true, hmin);
|
|
}
|
|
|
|
// If you want to specify that C, N, r0 and r1 are the initial guesses
|
|
// for the minimizer, set the parameter useTorusInputAsInitialGuess to
|
|
// 'true'. If you want the function to compute initial guesses, set
|
|
// useTorusInputAsInitialGuess to 'false'. A Gauss-Newton minimizer
|
|
// is used to fit a torus using nonlinear least-squares. The fitted
|
|
// torus is returned in C, N, r0 and r1. See GteGaussNewtonMinimizer.h
|
|
// for a description of the least-squares algorithm and the parameters
|
|
// that it requires.
|
|
typename GaussNewtonMinimizer<Real>::Result
|
|
operator()(int numPoints, Vector<3, Real> const* points,
|
|
size_t maxIterations, Real updateLengthTolerance, Real errorDifferenceTolerance,
|
|
bool useTorusInputAsInitialGuess,
|
|
Vector<3, Real>& C, Vector<3, Real>& N, Real& r0, Real& r1) const
|
|
{
|
|
mNumPoints = numPoints;
|
|
mPoints = points;
|
|
GaussNewtonMinimizer<Real> minimizer(7, mNumPoints, mFFunction, mJFunction);
|
|
|
|
if (!useTorusInputAsInitialGuess)
|
|
{
|
|
operator()(numPoints, points, C, N, r0, r1);
|
|
}
|
|
|
|
GVector<Real> initial(7);
|
|
|
|
// The initial guess for the plane origin.
|
|
initial[0] = C[0];
|
|
initial[1] = C[1];
|
|
initial[2] = C[2];
|
|
|
|
// The initial guess for the plane normal. The angles must be
|
|
// extracted for spherical coordinates.
|
|
if (std::fabs(N[2]) < (Real)1)
|
|
{
|
|
initial[3] = std::atan2(N[1], N[0]);
|
|
initial[4] = std::acos(N[2]);
|
|
}
|
|
else
|
|
{
|
|
initial[3] = (Real)0;
|
|
initial[4] = (Real)0;
|
|
}
|
|
|
|
// The initial guess for the radii-related parameters.
|
|
initial[5] = r0 * r0;
|
|
initial[6] = initial[5] - r1 * r1;
|
|
|
|
auto result = minimizer(initial, maxIterations, updateLengthTolerance,
|
|
errorDifferenceTolerance);
|
|
|
|
// No test is made for result.converged so that we return some
|
|
// estimates of the torus. The caller can decide how to respond
|
|
// when result.converged is false.
|
|
C[0] = result.minLocation[0];
|
|
C[1] = result.minLocation[1];
|
|
C[2] = result.minLocation[2];
|
|
|
|
Real theta = result.minLocation[3];
|
|
Real phi = result.minLocation[4];
|
|
Real csTheta = std::cos(theta);
|
|
Real snTheta = std::sin(theta);
|
|
Real csPhi = std::cos(phi);
|
|
Real snPhi = std::sin(phi);
|
|
N[0] = csTheta * snPhi;
|
|
N[1] = snTheta * snPhi;
|
|
N[2] = csPhi;
|
|
|
|
Real u = result.minLocation[5];
|
|
Real v = result.minLocation[6];
|
|
r0 = std::sqrt(u);
|
|
r1 = std::sqrt(u - v);
|
|
|
|
mNumPoints = 0;
|
|
mPoints = nullptr;
|
|
return result;
|
|
}
|
|
|
|
// If you want to specify that C, N, r0 and r1 are the initial guesses
|
|
// for the minimizer, set the parameter useTorusInputAsInitialGuess to
|
|
// 'true'. If you want the function to compute initial guesses, set
|
|
// useTorusInputAsInitialGuess to 'false'. A Gauss-Newton minimizer
|
|
// is used to fit a torus using nonlinear least-squares. The fitted
|
|
// torus is returned in C, N, r0 and r1. See GteGaussNewtonMinimizer.h
|
|
// for a description of the least-squares algorithm and the parameters
|
|
// that it requires.
|
|
typename LevenbergMarquardtMinimizer<Real>::Result
|
|
operator()(int numPoints, Vector<3, Real> const* points,
|
|
size_t maxIterations, Real updateLengthTolerance, Real errorDifferenceTolerance,
|
|
Real lambdaFactor, Real lambdaAdjust, size_t maxAdjustments,
|
|
bool useTorusInputAsInitialGuess,
|
|
Vector<3, Real>& C, Vector<3, Real>& N, Real& r0, Real& r1) const
|
|
{
|
|
mNumPoints = numPoints;
|
|
mPoints = points;
|
|
LevenbergMarquardtMinimizer<Real> minimizer(7, mNumPoints, mFFunction, mJFunction);
|
|
|
|
if (!useTorusInputAsInitialGuess)
|
|
{
|
|
operator()(numPoints, points, C, N, r0, r1);
|
|
}
|
|
|
|
GVector<Real> initial(7);
|
|
|
|
// The initial guess for the plane origin.
|
|
initial[0] = C[0];
|
|
initial[1] = C[1];
|
|
initial[2] = C[2];
|
|
|
|
// The initial guess for the plane normal. The angles must be
|
|
// extracted for spherical coordinates.
|
|
if (std::fabs(N[2]) < (Real)1)
|
|
{
|
|
initial[3] = std::atan2(N[1], N[0]);
|
|
initial[4] = std::acos(N[2]);
|
|
}
|
|
else
|
|
{
|
|
initial[3] = (Real)0;
|
|
initial[4] = (Real)0;
|
|
}
|
|
|
|
// The initial guess for the radii-related parameters.
|
|
initial[5] = r0 * r0;
|
|
initial[6] = initial[5] - r1 * r1;
|
|
|
|
auto result = minimizer(initial, maxIterations, updateLengthTolerance,
|
|
errorDifferenceTolerance, lambdaFactor, lambdaAdjust, maxAdjustments);
|
|
|
|
// No test is made for result.converged so that we return some
|
|
// estimates of the torus. The caller can decide how to respond
|
|
// when result.converged is false.
|
|
C[0] = result.minLocation[0];
|
|
C[1] = result.minLocation[1];
|
|
C[2] = result.minLocation[2];
|
|
|
|
Real theta = result.minLocation[3];
|
|
Real phi = result.minLocation[4];
|
|
Real csTheta = std::cos(theta);
|
|
Real snTheta = std::sin(theta);
|
|
Real csPhi = std::cos(phi);
|
|
Real snPhi = std::sin(phi);
|
|
N[0] = csTheta * snPhi;
|
|
N[1] = snTheta * snPhi;
|
|
N[2] = csPhi;
|
|
|
|
Real u = result.minLocation[5];
|
|
Real v = result.minLocation[6];
|
|
r0 = std::sqrt(u);
|
|
r1 = std::sqrt(u - v);
|
|
|
|
mNumPoints = 0;
|
|
mPoints = nullptr;
|
|
return result;
|
|
}
|
|
|
|
private:
|
|
typedef BSRational<UIntegerAP32> Rational;
|
|
|
|
mutable int mNumPoints;
|
|
mutable Vector<3, Real> const* mPoints;
|
|
std::function<void(GVector<Real> const&, GVector<Real>&)> mFFunction;
|
|
std::function<void(GVector<Real> const&, GMatrix<Real>&)> mJFunction;
|
|
};
|
|
}
|
|
|