void do_solve(double sumXX, double sumXY, double sumXZ, double sumYY, double sumYZ, double sumZZ, double invSumMultiplier)
{
sumXX *= invSumMultiplier;
sumXY *= invSumMultiplier;
sumXZ *= invSumMultiplier;
sumYY *= invSumMultiplier;
sumYZ *= invSumMultiplier;
sumZZ *= invSumMultiplier;
double[] matrix = new double[] {
sumXX, sumXY, sumXZ,
sumXY, sumYY, sumYZ,
sumXZ, sumYZ, sumZZ
};
// Setup the eigensolver.
SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096);
int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Increasing);
ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE);
if (ResultValid)
{
Box.Extent = new Vector3d(solver.GetEigenvalues());
double[] evectors = solver.GetEigenvectors();
Box.AxisX = new Vector3d(evectors[0], evectors[1], evectors[2]);
Box.AxisY = new Vector3d(evectors[3], evectors[4], evectors[5]);
Box.AxisZ = new Vector3d(evectors[6], evectors[7], evectors[8]);
}
}
public GaussPointsFit3(IEnumerable <Vector3d> points)
{
Box = new Box3d(Vector3d.Zero, Vector3d.One);
// Compute the mean of the points.
int numPoints = 0;
foreach (Vector3d v in points)
{
Box.Center += v;
numPoints++;
}
double invNumPoints = (1.0) / numPoints;
Box.Center *= invNumPoints;
// Compute the covariance matrix of the points.
double sumXX = (double)0, sumXY = (double)0, sumXZ = (double)0;
double sumYY = (double)0, sumYZ = (double)0, sumZZ = (double)0;
foreach (Vector3d p in points)
{
Vector3d diff = p - Box.Center;
sumXX += diff[0] * diff[0];
sumXY += diff[0] * diff[1];
sumXZ += diff[0] * diff[2];
sumYY += diff[1] * diff[1];
sumYZ += diff[1] * diff[2];
sumZZ += diff[2] * diff[2];
}
sumXX *= invNumPoints;
sumXY *= invNumPoints;
sumXZ *= invNumPoints;
sumYY *= invNumPoints;
sumYZ *= invNumPoints;
sumZZ *= invNumPoints;
double[] matrix = new double[] {
sumXX, sumXY, sumXZ,
sumXY, sumYY, sumYZ,
sumXZ, sumYZ, sumZZ
};
// Setup the eigensolver.
SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096);
int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Increasing);
ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE);
Box.Extent = new Vector3d(solver.GetEigenvalues());
double[] evectors = solver.GetEigenvectors();
Box.AxisX = new Vector3d(evectors[0], evectors[1], evectors[2]);
Box.AxisY = new Vector3d(evectors[3], evectors[4], evectors[5]);
Box.AxisZ = new Vector3d(evectors[6], evectors[7], evectors[8]);
}
public OrthogonalPlaneFit3(IEnumerable <Vector3d> points)
{
// Compute the mean of the points.
Origin = Vector3d.Zero;
int numPoints = 0;
foreach (Vector3d v in points)
{
Origin += v;
numPoints++;
}
double invNumPoints = (1.0) / numPoints;
Origin *= invNumPoints;
// Compute the covariance matrix of the points.
double sumXX = (double)0, sumXY = (double)0, sumXZ = (double)0;
double sumYY = (double)0, sumYZ = (double)0, sumZZ = (double)0;
foreach (Vector3d p in points)
{
Vector3d diff = p - Origin;
sumXX += diff[0] * diff[0];
sumXY += diff[0] * diff[1];
sumXZ += diff[0] * diff[2];
sumYY += diff[1] * diff[1];
sumYZ += diff[1] * diff[2];
sumZZ += diff[2] * diff[2];
}
sumXX *= invNumPoints;
sumXY *= invNumPoints;
sumXZ *= invNumPoints;
sumYY *= invNumPoints;
sumYZ *= invNumPoints;
sumZZ *= invNumPoints;
double[] matrix = new double[] {
sumXX, sumXY, sumXZ,
sumXY, sumYY, sumYZ,
sumXZ, sumYZ, sumZZ
};
// Setup the eigensolver.
SymmetricEigenSolver solver = new SymmetricEigenSolver(3, 4096);
int iters = solver.Solve(matrix, SymmetricEigenSolver.SortType.Decreasing);
ResultValid = (iters > 0 && iters < SymmetricEigenSolver.NO_CONVERGENCE);
Normal = new Vector3d(solver.GetEigenvector(2));
}
// The quadratic fit is
//
// 0 = C[0] + C[1]*X + C[2]*Y + C[3]*X^2 + C[4]*Y^2 + C[5]*X*Y
//
// subject to Length(C) = 1. Minimize E(C) = C^t M C with Length(C) = 1
// and M = (sum_i V_i)(sum_i V_i)^t where
//
// V = (1, X, Y, X^2, Y^2, X*Y)
//
// The minimum value is the smallest eigenvalue of M and C is a corresponding
// unit length eigenvector.
//
// Input:
// n = number of points to fit
// p[0..n-1] = array of points to fit
//
// Output:
// c[0..5] = coefficients of quadratic fit (the eigenvector)
// return value of function is nonnegative and a measure of the fit
// (the minimum eigenvalue; 0 = exact fit, positive otherwise)
// Canonical forms. The quadratic equation can be factored into
// P^T A P + B^T P + K = 0 where P = (X,Y,Z), K = C[0], B = (C[1],C[2],C[3]),
// and A is a 3x3 symmetric matrix with A00 = C[4], A11 = C[5], A22 = C[6],
// A01 = C[7]/2, A02 = C[8]/2, and A12 = C[9]/2. Matrix A = R^T D R where
// R is orthogonal and D is diagonal (using an eigendecomposition). Define
// V = R P = (v0,v1,v2), E = R B = (e0,e1,e2), D = diag(d0,d1,d2), and f = K
// to obtain
//
// d0 v0^2 + d1 v1^2 + d2 v^2 + e0 v0 + e1 v1 + e2 v2 + f = 0
//
// The characterization depends on the signs of the d_i.
public static double Fit(Vector2d[] points, double[] coefficients)
{
DenseMatrix A = new DenseMatrix(6, 6);
int numPoints = points.Length;
for (int i = 0; i < numPoints; ++i)
{
double x = points[i].x;
double y = points[i].y;
double x2 = x * x;
double y2 = y * y;
double xy = x * y;
double x3 = x * x2;
double xy2 = x * y2;
double x2y = x * xy;
double y3 = y * y2;
double x4 = x * x3;
double x2y2 = x * xy2;
double x3y = x * x2y;
double y4 = y * y3;
double xy3 = x * y3;
A[0, 1] += x;
A[0, 2] += y;
A[0, 3] += x2;
A[0, 4] += y2;
A[0, 5] += xy;
A[1, 3] += x3;
A[1, 4] += xy2;
A[1, 5] += x2y;
A[2, 4] += y3;
A[3, 3] += x4;
A[3, 4] += x2y2;
A[3, 5] += x3y;
A[4, 4] += y4;
A[4, 5] += xy3;
}
A[0, 0] = (double)numPoints;
A[1, 1] = A[0, 3];
A[1, 2] = A[0, 5];
A[2, 2] = A[0, 4];
A[2, 3] = A[1, 5];
A[2, 5] = A[1, 4];
A[5, 5] = A[3, 4];
for (int row = 0; row < 6; ++row)
{
for (int col = 0; col < row; ++col)
{
A[row, col] = A[col, row];
}
}
double invNumPoints = 1.0 / (double)numPoints;
for (int row = 0; row < 6; ++row)
{
for (int col = 0; col < 6; ++col)
{
A[row, col] *= invNumPoints;
}
}
SymmetricEigenSolver es = new SymmetricEigenSolver(6, 1024);
es.Solve(A.Buffer, SymmetricEigenSolver.SortType.Increasing);
es.GetEigenvector(0, coefficients);
// For an exact fit, numeric round-off errors might make the minimum
// eigenvalue just slightly negative. Return the absolute value since
// the application might rely on the return value being nonnegative.
return(Math.Abs(es.GetEigenvalue(0)));
}
// If you think your points are nearly circular, use this. The circle is of
// the form C'[0]+C'[1]*X+C'[2]*Y+C'[3]*(X^2+Y^2), where Length(C') = 1. The
// function returns C = (C'[0]/C'[3],C'[1]/C'[3],C'[2]/C'[3]), so the fitted
// circle is C[0]+C[1]*X+C[2]*Y+X^2+Y^2. The center is (xc,yc) =
// -0.5*(C[1],C[2]) and the radius is r = sqrt(xc*xc+yc*yc-C[0]).
public static double FitCircle2(Vector2d[] points, out Circle2d circle)
{
DenseMatrix A = new DenseMatrix(4, 4);
int numPoints = points.Length;
for (int i = 0; i < numPoints; ++i)
{
double x = points[i].x;
double y = points[i].y;
double x2 = x * x;
double y2 = y * y;
double xy = x * y;
double r2 = x2 + y2;
double xr2 = x * r2;
double yr2 = y * r2;
double r4 = r2 * r2;
A[0, 1] += x;
A[0, 2] += y;
A[0, 3] += r2;
A[1, 1] += x2;
A[1, 2] += xy;
A[1, 3] += xr2;
A[2, 2] += y2;
A[2, 3] += yr2;
A[3, 3] += r4;
}
A[0, 0] = (double)numPoints;
for (int row = 0; row < 4; ++row)
{
for (int col = 0; col < row; ++col)
{
A[row, col] = A[col, row];
}
}
double invNumPoints = 1.0 / (double)numPoints;
for (int row = 0; row < 4; ++row)
{
for (int col = 0; col < 4; ++col)
{
A[row, col] *= invNumPoints;
}
}
SymmetricEigenSolver es = new SymmetricEigenSolver(4, 1024);
es.Solve(A.Buffer, SymmetricEigenSolver.SortType.Increasing);
double[] evector = new double[4];
es.GetEigenvector(0, evector);
double inv = 1.0 / evector[3]; // TODO: Guard against zero divide?
Vector3d coefficients = Vector3d.Zero;
for (int row = 0; row < 3; ++row)
{
coefficients[row] = inv * evector[row];
}
Vector2d center = new Vector2d(-0.5 * coefficients[1], -0.5 * coefficients[2]);
double r = Math.Sqrt(Math.Abs(center.LengthSquared - coefficients[0]));
circle = new Circle2d(center, r);
// For an exact fit, numeric round-off errors might make the minimum
// eigenvalue just slightly negative. Return the absolute value since
// the application might rely on the return value being nonnegative.
return(Math.Abs(es.GetEigenvalue(0)));
}
