public static void RotFromTo2Quat(Matrix x, Matrix y, Matrix q)
{
Matrix axis = new Matrix(3, 1);
axis.Cross(y, x);
axis.Normalize();
double angle = Math.Acos(x.Dot(y));
double s = Math.Sin(angle / 2.0);
q[0] = axis[0] * s;
q[1] = axis[1] * s;
q[2] = axis[2] * s;
q[3] = Math.Cos(angle / 2.0);
}
// quaternion ops; quat is ((X, Y, Z), W)
public static void QuatMult(Matrix a, Matrix b, Matrix c)
{
Matrix v1 = new Matrix(3, 1);
Matrix v2 = new Matrix(3, 1);
Matrix v3 = new Matrix(3, 1);
v1[0] = a[0];
v1[1] = a[1];
v1[2] = a[2];
double s1 = a[3];
v2[0] = b[0];
v2[1] = b[1];
v2[2] = b[2];
double s2 = b[3];
v3.Cross(v1, v2);
c[0] = s1 * v2[0] + s2 * v1[0] + v3[0];
c[1] = s1 * v2[1] + s2 * v1[1] + v3[1];
c[2] = s1 * v2[2] + s2 * v1[2] + v3[2];
c[3] = s1 * s2 - v1.Dot(v2);
}
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;
}
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;
}
// quaternion ops; quat is ((X, Y, Z), W)
public static void QuatMult(Matrix a, Matrix b, Matrix c)
{
Matrix v1 = new Matrix(3,1);
Matrix v2 = new Matrix(3,1);
Matrix v3 = new Matrix(3,1);
v1[0] = a[0];
v1[1] = a[1];
v1[2] = a[2];
double s1 = a[3];
v2[0] = b[0];
v2[1] = b[1];
v2[2] = b[2];
double s2 = b[3];
v3.Cross(v1, v2);
c[0] = s1*v2[0] + s2*v1[0] + v3[0];
c[1] = s1*v2[1] + s2*v1[1] + v3[1];
c[2] = s1*v2[2] + s2*v1[2] + v3[2];
c[3] = s1*s2 - v1.Dot(v2);
}
public static void RotFromTo2Quat(Matrix x, Matrix y, Matrix q)
{
Matrix axis = new Matrix(3,1);
axis.Cross(y, x);
axis.Normalize();
double angle = Math.Acos(x.Dot(y));
double s = Math.Sin(angle/2.0);
q[0] = axis[0]*s;
q[1] = axis[1]*s;
q[2] = axis[2]*s;
q[3] = Math.Cos(angle/2.0);
}
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);
}