public void MultiplyTranspose()
{
double[][] square = { new double[] { 166.0, 188.0, 210.0 }, new double[] { 188.0, 214.0, 240.0 }, new double[] { 210.0, 240.0, 270.0 } };
GeneralMatrix sq = new GeneralMatrix(square);
Assert.IsTrue(GeneralTests.Check(A.Multiply(A.Transpose()), sq));
}
public Point GetActualDisplayPosition(Point p)
{
GeneralMatrix mat = new GeneralMatrix(3, 1);
mat.SetElement(0, 0, (double)p.X);
mat.SetElement(1, 0, (double)p.Y);
mat.SetElement(2, 0, 1.0);
GeneralMatrix ret = m_transform.Multiply(mat);
return(new Point((int)ret.GetElement(0, 0), (int)ret.GetElement(1, 0)));
}
private double[] CalculateNextPoint(double[] pX, double[] pGrad, GeneralMatrix hessian)
{
int i = 0;
double xmin = 0;
double step = _step;
GeneralMatrix alfaX = new GeneralMatrix(_nDim, 1);
GeneralMatrix prevX = new GeneralMatrix(pX, _nDim);
GeneralMatrix prevGrad = new GeneralMatrix(pGrad, _nDim);
double[] intermediate = new double[_nDim];;
alfaX = hessian.Multiply(prevGrad);
//doing a line search to minimize alpha
OneDWrapper wrapper = new OneDWrapper(_f, prevX, alfaX);
LineSearch search = new LineSearch();
double[] interval = new double[Constants.BRACKET_POINTS];
int it1 = search.FindMinInterval(wrapper, _alpha, step, 50, ref interval);
int it2 = search.FindMinimumViaBrent(wrapper, interval[0], interval[1], interval[2], 50, _epsilon, ref xmin);
for (i = 0; i < _nDim; i++)
{
intermediate[i] = prevX.GetElement(i, 0) - xmin * alfaX.GetElement(i, 0);
}
_alpha = xmin;
return(intermediate);
}
public void Inverse()
{
GeneralMatrix r = GeneralMatrix.Random(4, 4);
GeneralMatrix iR = r.Inverse();
Assert.IsTrue(GeneralTests.Check(r.Multiply(iR), GeneralMatrix.Identity(4, 4)));
}
protected override GeneralMatrix CalculateNextHessianApproximation(GeneralMatrix previousH,
double[] prevX, double[] curX, double[] prevGrad, double[] curGrad)
{
GeneralMatrix currentH = new GeneralMatrix(_nDim, _nDim);
GeneralMatrix cX = new GeneralMatrix(curX, _nDim);
GeneralMatrix pX = new GeneralMatrix(prevX, _nDim);
GeneralMatrix cG = new GeneralMatrix(curGrad, _nDim);
GeneralMatrix pG = new GeneralMatrix(prevGrad, _nDim);
GeneralMatrix dX = cX.Subtract(pX);
GeneralMatrix dG = cG.Subtract(pG);
double aK1 = 1 / (dX.Transpose().Multiply(dG).GetElement(0, 0));
GeneralMatrix aK2 = dX.Multiply(dX.Transpose());
GeneralMatrix aK = aK2.Multiply(aK1);
double bK1 = -1 / (dG.Transpose().Multiply(previousH).Multiply(dG).GetElement(0, 0));
GeneralMatrix bK2 = previousH.Multiply(dG).Multiply(dG.Transpose()).Multiply(previousH.Transpose());
GeneralMatrix bK = bK2.Multiply(bK1);
currentH = previousH.Add(aK).Add(bK);
return(currentH);
}
public void CholeskyDecomposition2()
{
double[][] pvals = { new double[] { 1.0, 1.0, 1.0 }, new double[] { 1.0, 2.0, 3.0 }, new double[] { 1.0, 3.0, 6.0 } };
GeneralMatrix A = new GeneralMatrix(pvals);
CholeskyDecomposition chol = A.chol();
GeneralMatrix X = chol.Solve(GeneralMatrix.Identity(3, 3));
Assert.IsTrue(GeneralTests.Check(A.Multiply(X), GeneralMatrix.Identity(3, 3)));
}
public void CholeskyDecomposition1()
{
double[][] pvals = { new double[] { 1.0, 1.0, 1.0 }, new double[] { 1.0, 2.0, 3.0 }, new double[] { 1.0, 3.0, 6.0 } };
GeneralMatrix A = new GeneralMatrix(pvals);
CholeskyDecomposition chol = A.chol();
GeneralMatrix L = chol.GetL();
Assert.IsTrue(GeneralTests.Check(A, L.Multiply(L.Transpose())));
}
public void EigenValueDecomposition2()
{
double[][] evals = { new double[] { 0.0, 1.0, 0.0, 0.0 }, new double[] { 1.0, 0.0, 2e-7, 0.0 }, new double[] { 0.0, -2e-7, 0.0, 1.0 }, new double[] { 0.0, 0.0, 1.0, 0.0 } };
GeneralMatrix A = new GeneralMatrix(evals);
EigenvalueDecomposition Eig = A.Eigen();
GeneralMatrix D = Eig.D;
GeneralMatrix V = Eig.GetV();
Assert.IsTrue(GeneralTests.Check(A.Multiply(V), V.Multiply(D)));
}
private static void CalculatePCA(List <Point3D> _points, out Point3D pivotO,
out Vector3D vecX, out Vector3D vecY, out Vector3D vecZ)
{
pivotO = new Point3D(0, 0, 0);
vecX = new Vector3D(0, 0, 0);
vecY = new Vector3D(0, 0, 0);
vecZ = new Vector3D(0, 0, 0);
if (_points == null || _points.Count < 1)
{
return;
}
Point3D pivot = GeometricTransforms.GetPivot(_points);
pivotO = new Point3D(pivot.X, pivot.Y, pivot.Z);
List <Vector3D> point_deviations = _points.Select(x => x - pivot).ToList();
int nrP = _points.Count;
#region COVARIANCE:Old
//// compute the covariance matrix
//double[] m = new double[3*nrP];
//for(int i = 0; i < nrP; i++)
//{
// m[i*3] = point_deviations[i].X;
// m[i*3 + 1] = point_deviations[i].Y;
// m[i*3 + 2] = point_deviations[i].Z;
//}
//MatrixNxN M = new MatrixNxN(nrP, 3, m);
//MatrixNxN MtxM = MatrixNxN.Squared(M);
//MtxM.Scale(1.0 / nrP);
#endregion
// compute the covariance matrix ...
// using 3rd party library DotNetMatrix
double[][] pd_as_array = new double[nrP][];
for (int i = 0; i < nrP; i++)
{
pd_as_array[i] = new double[] { point_deviations[i].X, point_deviations[i].Y, point_deviations[i].Z };
}
GeneralMatrix gm_M = new GeneralMatrix(pd_as_array);
GeneralMatrix gm_Mt = gm_M.Transpose();
GeneralMatrix gm_Msq = gm_Mt.Multiply(gm_M);
GeneralMatrix gm_Msqn = gm_Msq.Multiply(1.0 / nrP);
// extract the sorted Eigenvalues of the matrix...
// using 3rd party library DotNetMatrix
EigenvalueDecomposition decomp = gm_Msqn.Eigen();
GeneralMatrix gm_EVec = decomp.GetV();
double[] gm_EVal = decomp.RealEigenvalues;
// from smallest to largest eigenvalue
vecX = new Vector3D(gm_EVec.GetElement(0, 0), gm_EVec.GetElement(1, 0), gm_EVec.GetElement(2, 0));
vecY = new Vector3D(gm_EVec.GetElement(0, 1), gm_EVec.GetElement(1, 1), gm_EVec.GetElement(2, 1));
vecZ = new Vector3D(gm_EVec.GetElement(0, 2), gm_EVec.GetElement(1, 2), gm_EVec.GetElement(2, 2));
}
protected override GeneralMatrix CalculateNextHessianApproximation(GeneralMatrix pH,
double[] prevX, double[] curX, double[] prevGrad, double[] curGrad)
{
GeneralMatrix cH = new GeneralMatrix(_nDim, _nDim);
GeneralMatrix cX = new GeneralMatrix(curX, _nDim);
GeneralMatrix pX = new GeneralMatrix(prevX, _nDim);
GeneralMatrix cG = new GeneralMatrix(curGrad, _nDim);
GeneralMatrix pG = new GeneralMatrix(prevGrad, _nDim);
GeneralMatrix sigma = cX.Subtract(pX);
GeneralMatrix gamma = cG.Subtract(pG);
double sigmaTGamma = sigma.Transpose().Multiply(gamma).GetElement(0, 0);
GeneralMatrix hGammaSigmaT = pH.Multiply(gamma.Multiply(sigma.Transpose()));
GeneralMatrix sigmaGammaTH = sigma.Multiply(gamma.Transpose().Multiply(pH));
double gammaTHGamma = (gamma.Transpose().Multiply(pH.Multiply(gamma))).GetElement(0, 0);
GeneralMatrix sigmaSigmaT = sigma.Multiply(sigma.Transpose());
GeneralMatrix term1 = (hGammaSigmaT.Add(sigmaGammaTH)).Multiply(1 / sigmaTGamma);
GeneralMatrix term2 = (sigmaSigmaT.Multiply(1 / sigmaTGamma)).Multiply(1 + gammaTHGamma / sigmaTGamma);
return(pH.Subtract(term1).Add(term2));
}
public static void Main(System.String[] argv)
{
/*
| Tests LU, QR, SVD and symmetric Eig decompositions.
|
| n = order of magic square.
| trace = diagonal sum, should be the magic sum, (n^3 + n)/2.
| max_eig = maximum eigenvalue of (A + A')/2, should equal trace.
| rank = linear algebraic rank,
| should equal n if n is odd, be less than n if n is even.
| cond = L_2 condition number, ratio of singular values.
| lu_res = test of LU factorization, norm1(L*U-A(p,:))/(n*eps).
| qr_res = test of QR factorization, norm1(Q*R-A)/(n*eps).
*/
print("\n Test of GeneralMatrix Class, using magic squares.\n");
print(" See MagicSquareExample.main() for an explanation.\n");
print("\n n trace max_eig rank cond lu_res qr_res\n\n");
System.DateTime start_time = System.DateTime.Now;
double eps = System.Math.Pow(2.0, -52.0);
for (int n = 3; n <= 32; n++)
{
print(fixedWidthIntegertoString(n, 7));
GeneralMatrix M = magic(n);
//UPGRADE_WARNING: Narrowing conversions may produce unexpected results in C#. 'ms-help://MS.VSCC.2003/commoner/redir/redirect.htm?keyword="jlca1042"'
int t = (int)M.Trace();
print(fixedWidthIntegertoString(t, 10));
EigenvalueDecomposition E = new EigenvalueDecomposition(M.Add(M.Transpose()).Multiply(0.5));
double[] d = E.RealEigenvalues;
print(fixedWidthDoubletoString(d[n - 1], 14, 3));
int r = M.Rank();
print(fixedWidthIntegertoString(r, 7));
double c = M.Condition();
print(c < 1 / eps ? fixedWidthDoubletoString(c, 12, 3):" Inf");
LUDecomposition LU = new LUDecomposition(M);
GeneralMatrix L = LU.L;
GeneralMatrix U = LU.U;
int[] p = LU.Pivot;
GeneralMatrix R = L.Multiply(U).Subtract(M.GetMatrix(p, 0, n - 1));
double res = R.Norm1() / (n * eps);
print(fixedWidthDoubletoString(res, 12, 3));
QRDecomposition QR = new QRDecomposition(M);
GeneralMatrix Q = QR.Q;
R = QR.R;
R = Q.Multiply(R).Subtract(M);
res = R.Norm1() / (n * eps);
print(fixedWidthDoubletoString(res, 12, 3));
print("\n");
}
System.DateTime stop_time = System.DateTime.Now;
double etime = (stop_time.Ticks - start_time.Ticks) / 1000.0;
print("\nElapsed Time = " + fixedWidthDoubletoString(etime, 12, 3) + " seconds\n");
print("Adios\n");
}
public void CalculateEigenvalueDecomposition()
{
double[] i = new double[3];
double[] j = new double[3];
double[] k = new double[3];
double[] u = new double[3];
double[] t_vol = new double[3];
double[] vol = new double[3];
vol[0] = 0;
vol[1] = 0;
vol[2] = 0;
double[] func_sum = new double[3];
func_sum[0] = 0;
func_sum[1] = 0;
func_sum[2] = 0;
double[] func_sum_inertia = new double[3];
func_sum_inertia[0] = 0;
func_sum_inertia[1] = 0;
func_sum_inertia[2] = 0;
double func_sum_xy = 0;
double func_sum_xz = 0;
double func_sum_yz = 0;
double surfaceArea = 0;
//loop through all the triangles
for (int count = 0; count < _connections.Length; count++)
{
//get the 3 points of the triangle
double[] p1 = new double[3];
double[] p2 = new double[3];
double[] p3 = new double[3];
//Fix so its relative to the centroid center of mass
p1[0] = (double)_pts[_connections[count][0]][0] - _centroid[0]; //fix x's first
p2[0] = (double)_pts[_connections[count][1]][0] - _centroid[0];
p3[0] = (double)_pts[_connections[count][2]][0] - _centroid[0];
p1[1] = (double)_pts[_connections[count][0]][1] - _centroid[1]; //fix y's
p2[1] = (double)_pts[_connections[count][1]][1] - _centroid[1];
p3[1] = (double)_pts[_connections[count][2]][1] - _centroid[1];
p1[2] = (double)_pts[_connections[count][0]][2] - _centroid[2]; //fix z's
p2[2] = (double)_pts[_connections[count][1]][2] - _centroid[2];
p3[2] = (double)_pts[_connections[count][2]][2] - _centroid[2];
//calculate the i, j, k vectors
i[0] = p2[0] - p1[0]; j[0] = p2[1] - p1[1]; k[0] = p2[2] - p1[2];
i[1] = p3[0] - p1[0]; j[1] = p3[1] - p1[1]; k[1] = p3[2] - p1[2];
i[2] = p3[0] - p2[0]; j[2] = p3[1] - p2[1]; k[2] = p3[2] - p2[2];
//cross product between two vectors, to determine normal vector
u[0] = j[0] * k[1] - k[0] * j[1];
u[1] = k[0] * i[1] - i[0] * k[1];
u[2] = i[0] * j[1] - j[0] * i[1];
//Normalize vector to 1
double norm = Math.Sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
if (norm != 0.0)
{
u[0] = u[0] / norm;
u[1] = u[1] / norm;
u[2] = u[2] / norm;
}
else
{
u[0] = 0.0;
u[1] = 0.0;
u[2] = 0.0;
}
//This is reduced to ...
//area of a triangle...
double a = Math.Sqrt(i[1] * i[1] + j[1] * j[1] + k[1] * k[1]);
double b = Math.Sqrt(i[0] * i[0] + j[0] * j[0] + k[0] * k[0]);
double c = Math.Sqrt(i[2] * i[2] + j[2] * j[2] + k[2] * k[2]);
double s = 0.5 * (a + b + c);
double area = Math.Sqrt(Math.Abs(s * (s - a) * (s - b) * (s - c)));
//patches(count,1) = area
//
surfaceArea += area;
//volume elements ...
double zavg = (p1[2] + p2[2] + p3[2]) / 3.0;
double yavg = (p1[1] + p2[1] + p3[1]) / 3.0;
double xavg = (p1[0] + p2[0] + p3[0]) / 3.0;
//sum of function for centroid calculation
func_sum[0] += t_vol[0] * xavg;
func_sum[1] += t_vol[1] * yavg;
func_sum[2] += t_vol[2] * zavg;
//sum of function for inertia calculation
func_sum_inertia[0] += area * u[0] * xavg * xavg * xavg;
func_sum_inertia[1] += area * u[1] * yavg * yavg * yavg;
func_sum_inertia[2] += area * u[2] * zavg * zavg * zavg;
//sum of function for products of inertia calculation
func_sum_xz += area * u[0] * xavg * xavg * zavg;
func_sum_xy += area * u[1] * yavg * yavg * xavg;
func_sum_yz += area * u[2] * zavg * zavg * yavg;
}
func_sum_inertia[0] /= 3;
func_sum_inertia[1] /= 3;
func_sum_inertia[2] /= 3;
double Ixy = -1 * func_sum_xy / 2;
double Ixz = -1 * func_sum_xz / 2;
double Iyz = -1 * func_sum_yz / 2;
double Iyx = Ixy;
double Izx = Ixz;
double Izy = Iyz;
double Ixx = func_sum_inertia[1] + func_sum_inertia[2];
double Iyy = func_sum_inertia[0] + func_sum_inertia[2];
double Izz = func_sum_inertia[0] + func_sum_inertia[1];
GeneralMatrix i_CoM = new GeneralMatrix(3, 3);
i_CoM.Array[0][0] = Ixx;
i_CoM.Array[0][1] = Ixy;
i_CoM.Array[0][2] = Ixz;
i_CoM.Array[1][0] = Iyx;
i_CoM.Array[1][1] = Iyy;
i_CoM.Array[1][2] = Iyz;
i_CoM.Array[2][0] = Izx;
i_CoM.Array[2][1] = Izy;
i_CoM.Array[2][2] = Izz;
EigenvalueDecomposition eig = i_CoM.Eigen();
_eigenvalues = eig.D;
_eigenvectors = eig.GetV();
//make sure this is a right handed matrix
if (_eigenvectors.Determinant() < 0)
{
_eigenvectors = _eigenvectors.Multiply(-1);
}
}
/// <summary>
/// multiply normalized priority matrix by sum of average rows
/// </summary>
/// <param name="argMatrix"></param>
/// <param name="selection"></param>
/// <returns></returns>
private GeneralMatrix FCalc(GeneralMatrix argMatrix, GeneralMatrix selection)
{
GeneralMatrix matrix = argMatrix.Multiply(selection);
return(matrix.ArrayRightDivide(selection));
}
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;
}
public double[] GenerateCorrelatedShocks()
{
return(_L.Multiply(new GeneralMatrix(new double[][] { _randomNumberGenerators.Select(x => x.GetNormal()).ToArray() }).Transpose()).Array.Select(x => x.First()).ToArray());
}
