static public void PlaneFit(IList <Matrix> X, out Matrix R, out Matrix t, out Matrix d2)
{
int n = X.Count;
var mu = new Matrix(3, 1);
for (int i = 0; i < n; i++)
{
mu.Add(X[i]);
}
mu.Scale(1f / (float)n);
var A = new Matrix(3, 3);
var xc = new Matrix(3, 1);
var M = new Matrix(3, 3);
for (int i = 0; i < X.Count; i++)
{
var x = X[i];
xc.Sub(x, mu);
M.Outer(xc, xc);
A.Add(M);
}
var V = new Matrix(3, 3);
var d = new Matrix(3, 1);
A.Eig(V, d); // eigenvalues in ascending order
// arrange in descending order so that z = 0
var V2 = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
V2[i, 2] = V[i, 0];
V2[i, 1] = V[i, 1];
V2[i, 0] = V[i, 2];
}
d2 = new Matrix(3, 1);
d2[2] = d[0];
d2[1] = d[1];
d2[0] = d[2];
R = new Matrix(3, 3);
R.Transpose(V2);
if (R.Det3x3() < 0)
{
R.Scale(-1);
}
t = new Matrix(3, 1);
t.Mult(R, mu);
t.Scale(-1);
// eigenvalues are the sum of squared distances in each direction
// i.e., min eigenvalue is the sum of squared distances to the plane = d2[2]
// compute the distance to the plane by transforming to the plane and take z-coordinate:
// xPlane = R*x + t; distance = xPlane[2]
}
static public void PlaneFit(IList<Matrix> X, out Matrix R, out Matrix t, out Matrix d2)
{
int n = X.Count;
var mu = new Matrix(3, 1);
for (int i = 0; i < n; i++)
mu.Add(X[i]);
mu.Scale(1f / (float)n);
var A = new Matrix(3, 3);
var xc = new Matrix(3, 1);
var M = new Matrix(3, 3);
for (int i = 0; i < X.Count; i++)
{
var x = X[i];
xc.Sub(x, mu);
M.Outer(xc, xc);
A.Add(M);
}
var V = new Matrix(3, 3);
var d = new Matrix(3, 1);
A.Eig(V, d); // eigenvalues in ascending order
// arrange in descending order so that z = 0
var V2 = new Matrix(3, 3);
for (int i = 0; i < 3; i++)
{
V2[i, 2] = V[i, 0];
V2[i, 1] = V[i, 1];
V2[i, 0] = V[i, 2];
}
d2 = new Matrix(3, 1);
d2[2] = d[0];
d2[1] = d[1];
d2[0] = d[2];
R = new Matrix(3, 3);
R.Transpose(V2);
if (R.Det3x3() < 0)
R.Scale(-1);
t = new Matrix(3, 1);
t.Mult(R, mu);
t.Scale(-1);
// eigenvalues are the sum of squared distances in each direction
// i.e., min eigenvalue is the sum of squared distances to the plane = d2[2]
// compute the distance to the plane by transforming to the plane and take z-coordinate:
// xPlane = R*x + t; distance = xPlane[2]
}
public static double PlaneFit(IList<Matrix> points, out Matrix X, out double D)
{
X = new Matrix(3, 1);
var mu = new RoomAliveToolkit.Matrix(3, 1);
for (int i = 0; i < points.Count; i++)
mu.Add(points[i]);
mu.Scale(1f / (float)points.Count);
var A = new RoomAliveToolkit.Matrix(3, 3);
var pc = new RoomAliveToolkit.Matrix(3, 1);
var M = new RoomAliveToolkit.Matrix(3, 3);
for (int i = 0; i < points.Count; i++)
{
var p = points[i];
pc.Sub(p, mu);
M.Outer(pc, pc);
A.Add(M);
}
var V = new RoomAliveToolkit.Matrix(3, 3);
var d = new RoomAliveToolkit.Matrix(3, 1);
A.Eig(V, d); // TODO: replace with 3x3 version?
//Console.WriteLine("------");
//Console.WriteLine(A);
//Console.WriteLine(V);
//Console.WriteLine(d);
double minEigenvalue = Double.MaxValue;
int minEigenvaluei = 0;
for (int i = 0; i < 3; i++)
if (d[i] < minEigenvalue)
{
minEigenvalue = d[i];
minEigenvaluei = i;
}
X.CopyCol(V, minEigenvaluei);
D = -X.Dot(mu);
// min eigenvalue is the sum of squared distances to the plane
// signed distance is: double distance = X.Dot(point) + D;
return minEigenvalue;
}
