public void SingularValueDecomposition() { GeneralMatrix A = new GeneralMatrix(columnwise, 4); SingularValueDecomposition SVD = A.SVD(); Assert.IsTrue(GeneralTests.Check(A, SVD.GetU().Multiply(SVD.S.Multiply(SVD.GetV().Transpose())))); }
public void SingularValues() { double[][] condmat = { new double[] { 1.0, 3.0 }, new double[] { 7.0, 9.0 } }; GeneralMatrix B = new GeneralMatrix(condmat); SingularValueDecomposition SVD = B.SVD(); double[] singularvalues = SVD.SingularValues; Assert.IsTrue(GeneralTests.Check(B.Condition(), singularvalues[0] / singularvalues[System.Math.Min(B.RowDimension, B.ColumnDimension) - 1])); }
// 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; } } }
/// <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)); }