public GeneralMatrix[] decompose(int svCount, GeneralMatrix m)
{
SingularValueDecomposition s = new SingularValueDecomposition(m);
try
{
sv = svCount;
frameScale = m.RowDimension;
if (m.RowDimension > m.ColumnDimension)
{
frameScale = m.ColumnDimension;
}
else if (m.RowDimension < m.ColumnDimension)
{
frameScale = m.RowDimension;
}
// Make square matrix of data
if (m.RowDimension != m.ColumnDimension)
{
squareM = m.GetMatrix(0, frameScale - 1, 0, frameScale - 1);
}
else
{
squareM = m;
}
//perform the SVD here:
SingularValueDecomposition svd = new SingularValueDecomposition(squareM);
GeneralMatrix U = svd.GetU().GetMatrix(0, frameScale - 1, 0, sv - 1);
GeneralMatrix V = svd.GetV();
double[] D = svd.SingularValues;
GeneralMatrix dMat = makeDiagonalSquare(D, sv);
GeneralMatrix vMat = V.Transpose().GetMatrix(0, sv - 1, 0, frameScale - 1);
/*Console.WriteLine(U.RowDimension + "x" + U.ColumnDimension
+ " * " + dMat.RowDimension + "x" + dMat.ColumnDimension
+ " * " + vMat.RowDimension + "x" + vMat.ColumnDimension);
recomposition = U.Multiply(dMat).Multiply(vMat);
int[,] recompositionData = new int[recomposition.ColumnDimension, recomposition.RowDimension];
for (int i = 0; i < recomposition.ColumnDimension; i++)
{
for (int j = 0; j < recomposition.RowDimension; j++)
{
recompositionData[j, i] = (int)(recomposition.GetElement(j, i));
}
}*/
GeneralMatrix[] result = new GeneralMatrix[2];
result[0] = U;
result[1] = vMat;
return result;
}
catch (Exception ex)
{
Console.WriteLine(ex.ToString());
}
return null;
}
private void computeaccCalButton_Click(object sender, EventArgs e)
{
int i,j;
calStatusText.Text = "Computing Calibration...";
// Construct D matrix
// D = [x.^2, y.^2, z.^2, x.*y, x.*z, y.*z, x, y, z, ones(N,1)];
for (i = 0; i < SAMPLES; i++ )
{
// x^2 term
D.SetElement(i,0, loggedData[i,0]*loggedData[i,0]);
// y^2 term
D.SetElement(i,1,loggedData[i,1]*loggedData[i,1]);
// z^2 term
D.SetElement(i, 2, loggedData[i, 2] * loggedData[i, 2]);
// x*y term
D.SetElement(i,3,loggedData[i,0]*loggedData[i,1]);
// x*z term
D.SetElement(i,4,loggedData[i,0]*loggedData[i,2]);
// y*z term
D.SetElement(i,5,loggedData[i,1]*loggedData[i,2]);
// x term
D.SetElement(i,6,loggedData[i,0]);
// y term
D.SetElement(i,7,loggedData[i,1]);
// z term
D.SetElement(i,8,loggedData[i,2]);
// Constant term
D.SetElement(i,9,1);
}
// QR=triu(qr(D))
QRDecomposition QR = new QRDecomposition(D);
// [U,S,V] = svd(D)
SingularValueDecomposition SVD = new SingularValueDecomposition(QR.R);
GeneralMatrix V = SVD.GetV();
GeneralMatrix A = new GeneralMatrix(3, 3);
double[] p = new double[V.RowDimension];
for (i = 0; i < V.RowDimension; i++ )
{
p[i] = V.GetElement(i,V.ColumnDimension-1);
}
/*
A = [p(1) p(4)/2 p(5)/2;
p(4)/2 p(2) p(6)/2;
p(5)/2 p(6)/2 p(3)];
*/
if (p[0] < 0)
{
for (i = 0; i < V.RowDimension; i++)
{
p[i] = -p[i];
}
}
A.SetElement(0,0,p[0]);
A.SetElement(0,1,p[3]/2);
A.SetElement(1,2,p[4]/2);
A.SetElement(1,0,p[3]/2);
A.SetElement(1,1,p[1]);
A.SetElement(1,2,p[5]/2);
A.SetElement(2,0,p[4]/2);
A.SetElement(2,1,p[5]/2);
A.SetElement(2,2,p[2]);
CholeskyDecomposition Chol = new CholeskyDecomposition(A);
GeneralMatrix Ut = Chol.GetL();
GeneralMatrix U = Ut.Transpose();
double[] bvect = {p[6]/2,p[7]/2,p[8]/2};
double d = p[9];
GeneralMatrix b = new GeneralMatrix(bvect,3);
GeneralMatrix v = Ut.Solve(b);
double vnorm_sqrd = v.GetElement(0,0)*v.GetElement(0,0) + v.GetElement(1,0)*v.GetElement(1,0) + v.GetElement(2,0)*v.GetElement(2,0);
double s = 1/Math.Sqrt(vnorm_sqrd - d);
GeneralMatrix c = U.Solve(v);
for (i = 0; i < 3; i++)
{
c.SetElement(i, 0, -c.GetElement(i, 0));
}
U = U.Multiply(s);
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
calMat[i, j] = U.GetElement(i, j);
}
}
for (i = 0; i < 3; i++)
{
bias[i] = c.GetElement(i, 0);
}
accAlignment00.Text = calMat[0, 0].ToString();
accAlignment01.Text = calMat[0, 1].ToString();
accAlignment02.Text = calMat[0, 2].ToString();
accAlignment10.Text = calMat[1, 0].ToString();
accAlignment11.Text = calMat[1, 1].ToString();
accAlignment12.Text = calMat[1, 2].ToString();
accAlignment20.Text = calMat[2, 0].ToString();
accAlignment21.Text = calMat[2, 1].ToString();
accAlignment22.Text = calMat[2, 2].ToString();
biasX.Text = bias[0].ToString();
biasY.Text = bias[1].ToString();
biasZ.Text = bias[2].ToString();
calStatusText.Text = "Done";
flashCommitButton.Enabled = true;
accAlignmentCommitButton.Enabled = true;
}
/**
* Computes the Moore–Penrose pseudoinverse using the SVD method.
*
* Modified version of the original implementation by Kim van der Linde.
*/
public static GeneralMatrix pinv(GeneralMatrix x)
{
if (x.Rank() < 1)
return null;
if (x.ColumnDimension > x.RowDimension)
return pinv(x.Transpose()).Transpose();
SingularValueDecomposition svdX = new SingularValueDecomposition(x);
double[] singularValues = svdX.SingularValues;
double tol = Math.Max(x.ColumnDimension, x.RowDimension)
* singularValues[0] * 2E-16;
double[] singularValueReciprocals = new double[singularValues.Count()];
for (int i = 0; i < singularValues.Count(); i++)
singularValueReciprocals[i] = Math.Abs(singularValues[i]) < tol ? 0
: (1.0 / singularValues[i]);
double[][] u = svdX.GetU().Array;
double[][] v = svdX.GetV().Array;
int min = Math.Min(x.ColumnDimension, u[0].Count());
double[][] inverse = new double[x.ColumnDimension][];
for (int i = 0; i < x.ColumnDimension; i++) {
inverse[i] = new double[x.RowDimension];
for (int j = 0; j < u.Count(); j++)
for (int k = 0; k < min; k++)
inverse[i][j] += v[i][k] * singularValueReciprocals[k] * u[j][k];
}
return new GeneralMatrix(inverse);
}