List of usage examples for org.opencv.core Core solve
public static boolean solve(Mat src1, Mat src2, Mat dst, int flags)
From source file:org.ar.rubik.RubikFace.java
License:Open Source License
/** * Calculate the optimum fit for the given layout of Rhombus in the Face. * /* w w w . j a v a 2s . co m*/ * Set Up BIG Linear Equation: Y = AX * Where: * Y is a 2k x 1 matrix of actual x and y location from rhombus centers (known values) * X is a 3 x 1 matrix of { x_origin, y_origin, and alpha_lattice } (values we wish to find) * A is a 2k x 3 matrix of coefficients derived from m, n, alpha, beta, and gamma. * * Notes: * k := Twice the number of available rhombus. * n := integer axis of the face. * m := integer axis of the face. * * gamma := ratio of beta to alpha lattice size. * * Also, calculate sum of errors squared. * E = Y - AX * @return */ private LeastMeansSquare findOptimumFaceFit() { // Count how many non-empty cell actually have a rhombus in it. int k = 0; for (int n = 0; n < 3; n++) for (int m = 0; m < 3; m++) if (faceRhombusArray[n][m] != null) k++; Log.i(Constants.TAG, "Big K: " + k); Mat bigAmatrix = new Mat(2 * k, 3, CvType.CV_64FC1); Mat bigYmatrix = new Mat(2 * k, 1, CvType.CV_64FC1); Mat bigXmatrix = new Mat(3, 1, CvType.CV_64FC1); //{ origin_x, origin_y, latticeAlpha } // Load up matrices Y and A // X_k = X + n * L_alpha * cos(alpha) + m * L_beta * cos(beta) // Y_k = Y + n * L_alpha * sin(alpha) + m * L_beta * sin(beta) int index = 0; for (int n = 0; n < 3; n++) { for (int m = 0; m < 3; m++) { Rhombus rhombus = faceRhombusArray[n][m]; if (rhombus != null) { { // Actual X axis value of Rhombus in this location double bigY = rhombus.center.x; // Express expected X axis value : i.e. x = func( x_origin, n, m, alpha, beta, alphaLattice, gamma) double bigA = n * Math.cos(alphaAngle) + gammaRatio * m * Math.cos(betaAngle); bigYmatrix.put(index, 0, new double[] { bigY }); bigAmatrix.put(index, 0, new double[] { 1.0 }); bigAmatrix.put(index, 1, new double[] { 0.0 }); bigAmatrix.put(index, 2, new double[] { bigA }); index++; } { // Actual Y axis value of Rhombus in this location double bigY = rhombus.center.y; // Express expected Y axis value : i.e. y = func( y_origin, n, m, alpha, beta, alphaLattice, gamma) double bigA = n * Math.sin(alphaAngle) + gammaRatio * m * Math.sin(betaAngle); bigYmatrix.put(index, 0, new double[] { bigY }); bigAmatrix.put(index, 0, new double[] { 0.0 }); bigAmatrix.put(index, 1, new double[] { 1.0 }); bigAmatrix.put(index, 2, new double[] { bigA }); index++; } } } } // Log.v(Constants.TAG, "Big A Matrix: " + bigAmatrix.dump()); // Log.v(Constants.TAG, "Big Y Matrix: " + bigYmatrix.dump()); // Least Means Square Regression to find best values of origin_x, origin_y, and alpha_lattice. // Problem: Y=AX Known Y and A, but find X. // Tactic: Find minimum | AX - Y | (actually sum square of elements?) // OpenCV: Core.solve(Mat src1, Mat src2, Mat dst, int) // OpenCV: dst = arg min _X|src1 * X - src2| // Thus: src1 = A { 2k rows and 3 columns } // src2 = Y { 2k rows and 1 column } // dst = X { 3 rows and 1 column } // boolean solveFlag = Core.solve(bigAmatrix, bigYmatrix, bigXmatrix, Core.DECOMP_NORMAL); // Log.v(Constants.TAG, "Big X Matrix Result: " + bigXmatrix.dump()); // Sum of error square // Given X from above, the Y_estimate = AX // E = Y - AX Mat bigEmatrix = new Mat(2 * k, 1, CvType.CV_64FC1); for (int r = 0; r < (2 * k); r++) { double y = bigYmatrix.get(r, 0)[0]; double error = y; for (int c = 0; c < 3; c++) { double a = bigAmatrix.get(r, c)[0]; double x = bigXmatrix.get(c, 0)[0]; error -= a * x; } bigEmatrix.put(r, 0, error); } // sigma^2 = diagonal_sum( Et * E) double sigma = 0; for (int r = 0; r < (2 * k); r++) { double error = bigEmatrix.get(r, 0)[0]; sigma += error * error; } sigma = Math.sqrt(sigma); // Log.v(Constants.TAG, "Big E Matrix Result: " + bigEmatrix.dump()); // =+= not currently in use, could be deleted. // Retrieve Error terms and compose an array of error vectors: one of each occupied // cell who's vector point from tile center to actual location of rhombus. Point[][] errorVectorArray = new Point[3][3]; index = 0; for (int n = 0; n < 3; n++) { for (int m = 0; m < 3; m++) { Rhombus rhombus = faceRhombusArray[n][m]; // We expect this array to not have change from above. if (rhombus != null) { errorVectorArray[n][m] = new Point(bigEmatrix.get(index++, 0)[0], bigEmatrix.get(index++, 0)[0]); } } } double x = bigXmatrix.get(0, 0)[0]; double y = bigXmatrix.get(1, 0)[0]; double alphaLatice = bigXmatrix.get(2, 0)[0]; boolean valid = !Double.isNaN(x) && !Double.isNaN(y) && !Double.isNaN(alphaLatice) && !Double.isNaN(sigma); Log.i(Constants.TAG, String.format("Rubik Solution: x=%4.0f y=%4.0f alphaLattice=%4.0f sigma=%4.0f flag=%b", x, y, alphaLatice, sigma, solveFlag)); return new LeastMeansSquare(x, y, alphaLatice, errorVectorArray, sigma, valid); }