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);
}
/// <summary>
/// (
/// </summary>
/// <param name="matrix"></param>
/// <returns></returns>
public static GeneralMatrix ExpandUtility(GeneralMatrix matrix)
{
double val = 0.0;
int n = matrix.RowDimension;
int m = matrix.ColumnDimension;
if (n != m)
{
throw new ArgumentException("Criteria matrix must be symmetrical");
}
GeneralMatrix newMatrix = matrix.Transpose();
//for all transposed elements calculate their inverse values
//set diagonal elements to 0
for (int i = 0; i < n; i++)
{
for (int j = 0; j <= i; j++)
{
val = newMatrix.GetElement(i, j);
if (val == 0.0)
{
throw new ArgumentException("Criteria comparison values van't be 0");
}
newMatrix.SetElement(i, j, 1 / val);
if (i == j)
{
newMatrix.SetElement(i, j, 0);
}
}
}
//add transposed, inverse matrix to the original one
//create fully expanded matrix
return(newMatrix.Add(matrix));
}
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())));
}
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));
}
public void TestTranspose()
{
GeneralMatrix _gm = new GeneralMatrix(2, 2);
_gm.SetElement(0, 0, 1);
_gm.SetElement(0, 1, 2);
_gm.SetElement(1, 0, 3);
_gm.SetElement(1, 1, 4);
GeneralMatrix _ngm = _gm.Transpose();
Assert.AreEqual(1, 0, 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");
}
// this is the straight forward implementation of the kabsch algorithm.
// see http://en.wikipedia.org/wiki/Kabsch_algorithm for a detailed explanation.
public void Evaluate(int SpreadMax)
{
int newSpreadMax = SpreadUtils.SpreadMax(FInputQ, FInputP);
FOutput.SliceCount = newSpreadMax;
Matrix4x4 mOut;
if (FInputEnabled[0])
{
for (int slice = 0; slice < newSpreadMax; slice++)
{
// ======================== STEP 1 ========================
// translate both sets so that their centroids coincides with the origin
// of the coordinate system.
double[] meanP = new double[3] {
0.0, 0.0, 0.0
}; // mean of first point set
for (int i = 0; i < FInputP[slice].SliceCount; i++)
{
meanP[0] += FInputP[slice][i].x;
meanP[1] += FInputP[slice][i].y;
meanP[2] += FInputP[slice][i].z;
}
meanP[0] /= FInputP[slice].SliceCount;
meanP[1] /= FInputP[slice].SliceCount;
meanP[2] /= FInputP[slice].SliceCount;
double[][] centroidP = new double[3][] { new double[] { meanP[0] }, new double[] { meanP[1] }, new double[] { meanP[2] } };
GeneralMatrix mCentroidP = new GeneralMatrix(centroidP);
double[][] arrayP = new double[FInputP[slice].SliceCount][];
for (int i = 0; i < FInputP[slice].SliceCount; i++)
{
arrayP[i] = new double[3];
arrayP[i][0] = FInputP[slice][i].x - meanP[0]; // subtract the mean values from the incoming pointset
arrayP[i][1] = FInputP[slice][i].y - meanP[1];
arrayP[i][2] = FInputP[slice][i].z - meanP[2];
}
// this is the matrix of the first pointset translated to the origin of the coordinate system
GeneralMatrix P = new GeneralMatrix(arrayP);
double[] meanQ = new double[3] {
0.0, 0.0, 0.0
}; // mean of second point set
for (int i = 0; i < FInputQ[slice].SliceCount; i++)
{
meanQ[0] += FInputQ[slice][i].x;
meanQ[1] += FInputQ[slice][i].y;
meanQ[2] += FInputQ[slice][i].z;
}
meanQ[0] /= FInputQ[slice].SliceCount;
meanQ[1] /= FInputQ[slice].SliceCount;
meanQ[2] /= FInputQ[slice].SliceCount;
double[][] centroidQ = new double[3][] { new double[] { meanQ[0] }, new double[] { meanQ[1] }, new double[] { meanQ[2] } };
GeneralMatrix mCentroidQ = new GeneralMatrix(centroidQ);
double[][] arrayQ = new double[FInputQ[slice].SliceCount][];
for (int i = 0; i < FInputQ[slice].SliceCount; i++)
{
arrayQ[i] = new double[3];
arrayQ[i][0] = FInputQ[slice][i].x - meanQ[0]; // subtract the mean values from the incoming pointset
arrayQ[i][1] = FInputQ[slice][i].y - meanQ[1];
arrayQ[i][2] = FInputQ[slice][i].z - meanQ[2];
}
// this is the matrix of the second pointset translated to the origin of the coordinate system
GeneralMatrix Q = new GeneralMatrix(arrayQ);
// ======================== STEP2 ========================
// calculate a covariance matrix A and compute the optimal rotation matrix
GeneralMatrix A = P.Transpose() * Q;
SingularValueDecomposition svd = A.SVD();
GeneralMatrix U = svd.GetU();
GeneralMatrix V = svd.GetV();
// calculate determinant for a special reflexion case.
double det = (V * U.Transpose()).Determinant();
double[][] arrayD = new double[3][] { new double[] { 1, 0, 0 },
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 } };
arrayD[2][2] = det < 0 ? -1 : 1; // multiply 3rd column with -1 if determinant is < 0
GeneralMatrix D = new GeneralMatrix(arrayD);
// now we can compute the rotation matrix:
GeneralMatrix R = V * D * U.Transpose();
// ======================== STEP3 ========================
// calculate the translation:
GeneralMatrix T = mCentroidP - R.Inverse() * mCentroidQ;
// ================== OUTPUT TRANSFORM ===================
mOut.m11 = (R.Array)[0][0];
mOut.m12 = (R.Array)[0][1];
mOut.m13 = (R.Array)[0][2];
mOut.m14 = 0;
mOut.m21 = (R.Array)[1][0];
mOut.m22 = (R.Array)[1][1];
mOut.m23 = (R.Array)[1][2];
mOut.m24 = 0;
mOut.m31 = (R.Array)[2][0];
mOut.m32 = (R.Array)[2][1];
mOut.m33 = (R.Array)[2][2];
mOut.m34 = 0;
mOut.m41 = (T.Array)[0][0];
mOut.m42 = (T.Array)[1][0];
mOut.m43 = (T.Array)[2][0];
mOut.m44 = 1;
FOutput[slice] = mOut;
}
}
}
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 void Transpose1()
{
T = A.Transpose();
Assert.IsTrue(GeneralTests.Check(A.Transpose(), T));
}
/// <summary>
/// Solves between two point sets
/// </summary>
/// <param name="points">Point set</param>
/// <returns>Affine matrix for each point set</returns>
public Matrix Solve(IReadOnlyList <CameraToCameraPoint> points)
{
if (points == null)
{
throw new ArgumentNullException("points");
}
if (points.Count < 1)
{
throw new ArgumentException("points", "No points provided");
}
double[] meanP = new double[3] {
0.0, 0.0, 0.0
}; // mean of first point set
for (int i = 0; i < points.Count; i++)
{
Vector3 orig = points[i].Origin;
meanP[0] += orig.X;
meanP[1] += orig.Y;
meanP[2] += orig.Z;
}
double invCount = 1.0 / (double)points.Count;
meanP[0] *= invCount;
meanP[1] *= invCount;
meanP[2] *= invCount;
double[][] centroidP = new double[3][] { new double[] { meanP[0] }, new double[] { meanP[1] }, new double[] { meanP[2] } };
GeneralMatrix mCentroidP = new GeneralMatrix(centroidP);
double[][] arrayP = new double[points.Count][];
for (int i = 0; i < points.Count; i++)
{
Vector3 orig = points[i].Origin;
arrayP[i] = new double[3];
arrayP[i][0] = orig.X - meanP[0]; // subtract the mean values from the incoming pointset
arrayP[i][1] = orig.Y - meanP[1];
arrayP[i][2] = orig.Z - meanP[2];
}
// this is the matrix of the first pointset translated to the origin of the coordinate system
GeneralMatrix P = new GeneralMatrix(arrayP);
double[] meanQ = new double[3] {
0.0, 0.0, 0.0
}; // mean of second point set
for (int i = 0; i < points.Count; i++)
{
Vector3 dest = points[i].Destination;
meanQ[0] += dest.X;
meanQ[1] += dest.Y;
meanQ[2] += dest.Z;
}
meanQ[0] *= invCount;
meanQ[1] *= invCount;
meanQ[2] *= invCount;
double[][] centroidQ = new double[3][] { new double[] { meanQ[0] }, new double[] { meanQ[1] }, new double[] { meanQ[2] } };
GeneralMatrix mCentroidQ = new GeneralMatrix(centroidQ);
double[][] arrayQ = new double[points.Count][];
for (int i = 0; i < points.Count; i++)
{
Vector3 dest = points[i].Destination;
arrayQ[i] = new double[3];
arrayQ[i][0] = dest.X - meanQ[0]; // subtract the mean values from the incoming pointset
arrayQ[i][1] = dest.Y - meanQ[1];
arrayQ[i][2] = dest.Z - meanQ[2];
}
// this is the matrix of the second pointset translated to the origin of the coordinate system
GeneralMatrix Q = new GeneralMatrix(arrayQ);
// ======================== STEP2 ========================
// calculate a covariance matrix A and compute the optimal rotation matrix
GeneralMatrix A = P.Transpose() * Q;
SingularValueDecomposition svd = A.SVD();
GeneralMatrix U = svd.GetU();
GeneralMatrix V = svd.GetV();
// calculate determinant for a special reflexion case.
double det = (V * U.Transpose()).Determinant();
double[][] arrayD = new double[3][] { new double[] { 1, 0, 0 },
new double[] { 0, 1, 0 },
new double[] { 0, 0, 1 } };
arrayD[2][2] = det < 0 ? -1 : 1; // multiply 3rd column with -1 if determinant is < 0
GeneralMatrix D = new GeneralMatrix(arrayD);
// now we can compute the rotation matrix:
GeneralMatrix R = V * D * U.Transpose();
// ======================== STEP3 ========================
// calculate the translation:
GeneralMatrix T = mCentroidP - R.Inverse() * mCentroidQ;
return(new Matrix((float)(R.Array)[0][0],
(float)(R.Array)[0][1],
(float)(R.Array)[0][2],
0.0f,
(float)(R.Array)[1][0],
(float)(R.Array)[1][1],
(float)(R.Array)[1][2],
0.0f,
(float)(R.Array)[2][0],
(float)(R.Array)[2][1],
(float)(R.Array)[2][2],
0.0f,
(float)(T.Array)[0][0],
(float)(T.Array)[1][0],
(float)(T.Array)[2][0],
1.0f));
}
/// <summary>
/// ����һ��AT*A*S=AT*B
/// </summary>
/// <param name="st7"></param>
public void CalculateTrans7Param1(List<Coords7ST> st7)
{
double[][] A = new double[0][];
double[][] B = new double[0][];
InitMatrixAB(ref A, ref B, st7);
GeneralMatrix matrixA = new GeneralMatrix(A);
GeneralMatrix matrixB = new GeneralMatrix(B);
GeneralMatrix matrixAT = matrixA.Transpose();
GeneralMatrix matrixATA = matrixAT.Multiply(matrixA);
GeneralMatrix matrixATAInv = matrixATA.Inverse();
GeneralMatrix matrixParam = matrixATAInv.Multiply(matrixAT).Multiply(matrixB);
this.Set4Param(matrixParam.GetElement(0, 0), matrixParam.GetElement(1, 0), matrixParam.GetElement(2, 0), matrixParam.GetElement(6, 0));
this.SetRotationParam(matrixParam.GetElement(3, 0), matrixParam.GetElement(4, 0), matrixParam.GetElement(5, 0));
}
