/// <summary>
/// Returns the normal of the plane of best fit of the given points
/// </summary>
/// <param name="points"></param>
/// <returns></returns>
public static Point PlaneOfBestFit(List <Point> points)
{
Point centroid = Centroid(points);
Matrix <double> A = Matrix <double> .Build.Dense(3, points.Count);
for (int i = 0; i < points.Count; ++i)
{
Point temp = points[i] - centroid;
A.SetColumn(i, new double[] { temp.x, temp.y, temp.z });
}
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> svdA = A.Svd();
double[] norm = svdA.U.Column(2).AsArray();
return(new Point(norm));
}
public void reduceDimensions(int dimensions)
{
if (dimensions < data.ColumnCount)
{
MathNet.Numerics.LinearAlgebra.Factorization.Svd <float> svd = this.data.Svd();
float[][] columnArrays = Functions.initArray(dimensions, new float[svd.U.RowCount]);
this.singularValues = new List <float>();
for (int i = 0; i < dimensions; i++)
{
columnArrays[i] = svd.U.Column(i).ToArray();
this.singularValues.Add(svd.S[i]);
}
this.data = Matrix <float> .Build.DenseOfColumnArrays(columnArrays);
this.numberOfColumns = data.ColumnCount;
}
}
// PCA
public static Matrix <double> Fit(List <Point> source, List <Point> target)
{
// Calculate the covariance of the source and target points
Matrix <double> covSource = Covariance(source);
Matrix <double> covTarget = Covariance(target);
// Using Singular Value Decomposition (SVD), get the left unitary matrix U
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> svdSource = covSource.Svd();
MathNet.Numerics.LinearAlgebra.Factorization.Svd <double> svdTarget = covTarget.Svd();
Matrix <double> uSource = svdSource.U;
Matrix <double> uTarget = svdTarget.U;
/////////////////////////here//////////////////
// Get the Householder matrix also known as a reflection matrix about a plane
Matrix <double> householder = getHouseHolderMatrix(source);
// Get all 8 possible rotation matrices based on the determinant
List <Matrix <double> > possibleRotations = new List <Matrix <double> >();
if (uTarget.Multiply(uSource.Inverse()).Determinant() > 0)
{
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, false, false).Inverse()));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, false, true).Inverse()).Multiply(householder));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, true, false).Inverse()).Multiply(householder));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, true, true).Inverse()));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, false, false).Inverse()).Multiply(householder));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, false, true).Inverse()));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, true, false).Inverse()));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, true, true).Inverse()).Multiply(householder));
}
else
{
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, false, false).Inverse()).Multiply(householder));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, false, true).Inverse()));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, true, false).Inverse()));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, false, true, true).Inverse()).Multiply(householder));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, false, false).Inverse()));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, false, true).Inverse()).Multiply(householder));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, true, false).Inverse()).Multiply(householder));
possibleRotations.Add(uTarget.Multiply(NegateEigenvectors(uSource, true, true, true).Inverse()));
}
// Setting up the analysis to find the best rotation matrix
double smallestScore = double.PositiveInfinity;
Matrix <double> bestTransformation = Matrix <double> .Build.DenseIdentity(4, 4);
// Calculate the centroid of both triangles
Point targetCentroid = Centroid(target);
Point sourceCentroid = Centroid(source);
// Iterate through the possible rotation matrices to find the best one
foreach (Matrix <double> R in possibleRotations)
{
Matrix <double> R4x4 = Matrix <double> .Build.DenseIdentity(4, 4);
R4x4.SetSubMatrix(0, 0, R);
// Calculate the translation vector between the two triangles using the current rotation matrix
Point T = targetCentroid - R4x4 * sourceCentroid;
// Generate the total transformation (rotate and translate) using the current rotation matrix and the translation obtained above
Matrix <double> transformation = Matrix <double> .Build.DenseIdentity(4, 4);
transformation.SetSubMatrix(0, 0, R);
transformation[0, 3] = T.x;
transformation[1, 3] = T.y;
transformation[2, 3] = T.z;
double score = 0;
foreach (var p in source)
{
Point transformedPoint = transformation * p;
score += target.Min(x => Distance(x, transformedPoint)); // Gets the smallest distance between p and any point in target
}
if (score < smallestScore)
{
smallestScore = score;
bestTransformation = transformation;
}
}
return(bestTransformation);
}
