public double MinimizeOneStep(Matrix parameters)
{
// initial value of the function; callee knows the size of the returned vector
var errorVector = function(parameters);
var error = errorVector.Dot(errorVector);
// Jacobian; callee knows the size of the returned matrix
var J = jacobianFunction(parameters);
// J'*J
var JtJ = new Matrix(parameters.Size, parameters.Size);
//stopWatch.Restart();
//JtJ.MultATA(J, J); // this is the big calculation that could be parallelized
JtJ.MultATAParallel(J, J);
//Console.WriteLine("JtJ: J size {0}x{1} {2}ms", J.Rows, J.Cols, stopWatch.ElapsedMilliseconds);
// J'*error
var JtError = new Matrix(parameters.Size, 1);
//stopWatch.Restart();
JtError.MultATA(J, errorVector); // error vector must be a column vector
//Console.WriteLine("JtError: errorVector size {0}x{1} {2}ms", errorVector.Rows, errorVector.Cols, stopWatch.ElapsedMilliseconds);
// allocate some space
var JtJaugmented = new Matrix(parameters.Size, parameters.Size);
var JtJinv = new Matrix(parameters.Size, parameters.Size);
var delta = new Matrix(parameters.Size, 1);
var newParameters = new Matrix(parameters.Size, 1);
// find a value of lambda that reduces error
double lambda = initialLambda;
while (true)
{
// augment J'*J: J'*J += lambda*(diag(J))
JtJaugmented.Copy(JtJ);
for (int i = 0; i < parameters.Size; i++)
JtJaugmented[i, i] = (1.0 + lambda) * JtJ[i, i];
//WriteMatrixToFile(errorVector, "errorVector");
//WriteMatrixToFile(J, "J");
//WriteMatrixToFile(JtJaugmented, "JtJaugmented");
//WriteMatrixToFile(JtError, "JtError");
// solve for delta: (J'*J + lambda*(diag(J)))*delta = J'*error
JtJinv.Inverse(JtJaugmented);
delta.Mult(JtJinv, JtError);
// new parameters = parameters - delta [why not add?]
newParameters.Sub(parameters, delta);
// evaluate function, compute error
var newErrorVector = function(newParameters);
double newError = newErrorVector.Dot(newErrorVector);
// if error is reduced, divide lambda by 10
bool improvement;
if (newError < error)
{
lambda /= lambdaIncrement;
improvement = true;
}
else // if not, multiply lambda by 10
{
lambda *= lambdaIncrement;
improvement = false;
}
// termination criteria:
// reduction in error is too small
var diff = new Matrix(errorVector.Size, 1);
diff.Sub(errorVector, newErrorVector);
double diffSq = diff.Dot(diff);
double errorDelta = Math.Sqrt(diffSq / error);
if (errorDelta < minimumReduction)
state = States.ReductionStepTooSmall;
// lambda is too big
if (lambda > maximumLambda)
state = States.LambdaTooLarge;
// change in parameters is too small [not implemented]
// if we made an improvement, accept the new parameters
if (improvement)
{
parameters.Copy(newParameters);
error = newError;
break;
}
// if we meet termination criteria, break
if (state != States.Running)
break;
}
rmsError = Math.Sqrt(error / errorVector.Size);
return rmsError;
}
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 double MinimizeOneStep(Matrix parameters)
{
// initial value of the function; callee knows the size of the returned vector
var errorVector = function(parameters);
var error = errorVector.Dot(errorVector);
// Jacobian; callee knows the size of the returned matrix
var J = jacobianFunction(parameters);
// J'*J
var JtJ = new Matrix(parameters.Size, parameters.Size);
//stopWatch.Restart();
//JtJ.MultATA(J, J); // this is the big calculation that could be parallelized
JtJ.MultATAParallel(J, J);
//Console.WriteLine("JtJ: J size {0}x{1} {2}ms", J.Rows, J.Cols, stopWatch.ElapsedMilliseconds);
// J'*error
var JtError = new Matrix(parameters.Size, 1);
//stopWatch.Restart();
JtError.MultATA(J, errorVector); // error vector must be a column vector
//Console.WriteLine("JtError: errorVector size {0}x{1} {2}ms", errorVector.Rows, errorVector.Cols, stopWatch.ElapsedMilliseconds);
// allocate some space
var JtJaugmented = new Matrix(parameters.Size, parameters.Size);
var JtJinv = new Matrix(parameters.Size, parameters.Size);
var delta = new Matrix(parameters.Size, 1);
var newParameters = new Matrix(parameters.Size, 1);
// find a value of lambda that reduces error
double lambda = initialLambda;
while (true)
{
// augment J'*J: J'*J += lambda*(diag(J))
JtJaugmented.Copy(JtJ);
for (int i = 0; i < parameters.Size; i++)
{
JtJaugmented[i, i] = (1.0 + lambda) * JtJ[i, i];
}
//WriteMatrixToFile(errorVector, "errorVector");
//WriteMatrixToFile(J, "J");
//WriteMatrixToFile(JtJaugmented, "JtJaugmented");
//WriteMatrixToFile(JtError, "JtError");
// solve for delta: (J'*J + lambda*(diag(J)))*delta = J'*error
JtJinv.Inverse(JtJaugmented);
delta.Mult(JtJinv, JtError);
// new parameters = parameters - delta [why not add?]
newParameters.Sub(parameters, delta);
// evaluate function, compute error
var newErrorVector = function(newParameters);
double newError = newErrorVector.Dot(newErrorVector);
// if error is reduced, divide lambda by 10
bool improvement;
if (newError < error)
{
lambda /= lambdaIncrement;
improvement = true;
}
else // if not, multiply lambda by 10
{
lambda *= lambdaIncrement;
improvement = false;
}
// termination criteria:
// reduction in error is too small
var diff = new Matrix(errorVector.Size, 1);
diff.Sub(errorVector, newErrorVector);
double diffSq = diff.Dot(diff);
double errorDelta = Math.Sqrt(diffSq / error);
if (errorDelta < minimumReduction)
{
state = States.ReductionStepTooSmall;
}
// lambda is too big
if (lambda > maximumLambda)
{
state = States.LambdaTooLarge;
}
// change in parameters is too small [not implemented]
// if we made an improvement, accept the new parameters
if (improvement)
{
parameters.Copy(newParameters);
error = newError;
break;
}
// if we meet termination criteria, break
if (state != States.Running)
{
break;
}
}
rmsError = Math.Sqrt(error / errorVector.Size);
return(rmsError);
}
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;
}