List of usage examples for org.opencv.core Mat get
public double[] get(int row, int col)
From source file:org.akvo.caddisfly.helper.ImageHelper.java
License:Open Source License
/** * Gets the center of the backdrop in the test chamber * * @param bitmap the photo to analyse//from ww w . j a v a2 s.c o m * @return the center point of the found circle */ public static Point getCenter(@NonNull Bitmap bitmap) { // convert bitmap to mat Mat mat = new Mat(bitmap.getWidth(), bitmap.getHeight(), CvType.CV_8UC1); Mat grayMat = new Mat(bitmap.getWidth(), bitmap.getHeight(), CvType.CV_8UC1); Utils.bitmapToMat(bitmap, mat); // convert to grayScale int colorChannels = (mat.channels() == 3) ? Imgproc.COLOR_BGR2GRAY : ((mat.channels() == 4) ? Imgproc.COLOR_BGRA2GRAY : 1); Imgproc.cvtColor(mat, grayMat, colorChannels); // reduce the noise so we avoid false circle detection //Imgproc.GaussianBlur(grayMat, grayMat, new Size(9, 9), 2, 2); // param1 = gradient value used to handle edge detection // param2 = Accumulator threshold value for the // cv2.CV_HOUGH_GRADIENT method. // The smaller the threshold is, the more circles will be // detected (including false circles). // The larger the threshold is, the more circles will // potentially be returned. double param1 = 10, param2 = 100; // create a Mat object to store the circles detected Mat circles = new Mat(bitmap.getWidth(), bitmap.getHeight(), CvType.CV_8UC1); // find the circle in the image Imgproc.HoughCircles(grayMat, circles, Imgproc.CV_HOUGH_GRADIENT, RESOLUTION_INVERSE_RATIO, (double) MIN_CIRCLE_CENTER_DISTANCE, param1, param2, MIN_RADIUS, MAX_RADIUS); int numberOfCircles = (circles.rows() == 0) ? 0 : circles.cols(); // draw the circles found on the image if (numberOfCircles > 0) { double[] circleCoordinates = circles.get(0, 0); int x = (int) circleCoordinates[0], y = (int) circleCoordinates[1]; org.opencv.core.Point center = new org.opencv.core.Point(x, y); int foundRadius = (int) circleCoordinates[2]; // circle outline Imgproc.circle(mat, center, foundRadius, COLOR_GREEN, 4); Utils.matToBitmap(mat, bitmap); return new Point((int) center.x, (int) center.y); } return null; }
From source file:org.akvo.caddisfly.sensor.colorimetry.strip.calibration.CalibrationCard.java
License:Open Source License
@NonNull private static double[] getWhiteVal(@NonNull Mat lab, int x, int y, int dp) { double totLum = 0; double totA = 0; double totB = 0; int totNum = 0; for (int i = -dp; i <= dp; i++) { for (int ii = -dp; ii <= dp; ii++) { totLum = totLum + lab.get(y + i, x + ii)[0]; totA = totA + lab.get(y + i, x + ii)[1]; totB = totB + lab.get(y + i, x + ii)[2]; totNum++;// w ww . j a va 2s . c om } } return new double[] { totLum / totNum, totA / totNum, totB / totNum }; }
From source file:org.akvo.caddisfly.sensor.colorimetry.strip.util.OpenCVUtil.java
License:Open Source License
@SuppressWarnings("UnusedParameters") public static Mat detectStrip(Mat stripArea, StripTest.Brand brand, double ratioW, double ratioH) { List<Mat> channels = new ArrayList<>(); Mat sArea = stripArea.clone();/*from w w w. j av a 2 s .c o m*/ // Gaussian blur Imgproc.medianBlur(sArea, sArea, 3); Core.split(sArea, channels); // create binary image Mat binary = new Mat(); // determine min and max NOT USED Imgproc.threshold(channels.get(0), binary, 128, MAX_RGB_INT_VALUE, Imgproc.THRESH_BINARY); // compute first approximation of line through length of the strip final WeightedObservedPoints points = new WeightedObservedPoints(); final WeightedObservedPoints corrPoints = new WeightedObservedPoints(); double tot, yTot; for (int i = 0; i < binary.cols(); i++) { // iterate over cols tot = 0; yTot = 0; for (int j = 0; j < binary.rows(); j++) { // iterate over rows if (binary.get(j, i)[0] > 128) { yTot += j; tot++; } } if (tot > 0) { points.add((double) i, yTot / tot); } } // order of coefficients is (b + ax), so [b, a] final PolynomialCurveFitter fitter = PolynomialCurveFitter.create(1); List<WeightedObservedPoint> pointsList = points.toList(); final double[] coefficient = fitter.fit(pointsList); // second pass, remove outliers double estimate, actual; for (int i = 0; i < pointsList.size(); i++) { estimate = coefficient[1] * pointsList.get(i).getX() + coefficient[0]; actual = pointsList.get(i).getY(); if (actual > LOWER_PERCENTAGE_BOUND * estimate && actual < UPPER_PERCENTAGE_BOUND * estimate) { //if the point differs less than +/- 10%, keep the point corrPoints.add(pointsList.get(i).getX(), pointsList.get(i).getY()); } } final double[] coefficientCorr = fitter.fit(corrPoints.toList()); double slope = coefficientCorr[1]; double offset = coefficientCorr[0]; // compute rotation angle double rotAngleDeg = Math.atan(slope) * 180 / Math.PI; //determine a point on the line, in the middle of strip, in the horizontal middle of the whole image int midPointX = binary.cols() / 2; int midPointY = (int) Math.round(midPointX * slope + offset); // rotate around the midpoint, to straighten the binary strip Mat dstBinary = new Mat(binary.rows(), binary.cols(), binary.type()); Point center = new Point(midPointX, midPointY); Mat rotMat = Imgproc.getRotationMatrix2D(center, rotAngleDeg, 1.0); Imgproc.warpAffine(binary, dstBinary, rotMat, binary.size(), Imgproc.INTER_CUBIC + Imgproc.WARP_FILL_OUTLIERS); // also apply rotation to colored strip Mat dstStrip = new Mat(stripArea.rows(), stripArea.cols(), stripArea.type()); Imgproc.warpAffine(stripArea, dstStrip, rotMat, binary.size(), Imgproc.INTER_CUBIC + Imgproc.WARP_FILL_OUTLIERS); // Compute white points in each row double[] rowCount = new double[dstBinary.rows()]; int rowTot; for (int i = 0; i < dstBinary.rows(); i++) { // iterate over rows rowTot = 0; for (int j = 0; j < dstBinary.cols(); j++) { // iterate over cols if (dstBinary.get(i, j)[0] > 128) { rowTot++; } } rowCount[i] = rowTot; } // find width by finding rising and dropping edges // rising edge = largest positive difference // falling edge = largest negative difference int risePos = 0; int fallPos = 0; double riseVal = 0; double fallVal = 0; for (int i = 0; i < dstBinary.rows() - 1; i++) { if (rowCount[i + 1] - rowCount[i] > riseVal) { riseVal = rowCount[i + 1] - rowCount[i]; risePos = i + 1; } if (rowCount[i + 1] - rowCount[i] < fallVal) { fallVal = rowCount[i + 1] - rowCount[i]; fallPos = i; } } // cut out binary strip Point stripTopLeft = new Point(0, risePos); Point stripBottomRight = new Point(dstBinary.cols(), fallPos); org.opencv.core.Rect stripAreaRect = new org.opencv.core.Rect(stripTopLeft, stripBottomRight); Mat binaryStrip = dstBinary.submat(stripAreaRect); // also cut out colored strip Mat colorStrip = dstStrip.submat(stripAreaRect); // now right end of strip // method: first rising edge double[] colCount = new double[binaryStrip.cols()]; int colTotal; for (int i = 0; i < binaryStrip.cols(); i++) { // iterate over cols colTotal = 0; for (int j = 0; j < binaryStrip.rows(); j++) { // iterate over rows if (binaryStrip.get(j, i)[0] > 128) { colTotal++; } } //Log.d("Caddisfly", String.valueOf(colTotal)); colCount[i] = colTotal; } stripAreaRect = getStripRectangle(binaryStrip, colCount, brand.getStripLength(), ratioW); Mat resultStrip = colorStrip.submat(stripAreaRect).clone(); // release Mat objects stripArea.release(); sArea.release(); binary.release(); dstBinary.release(); dstStrip.release(); binaryStrip.release(); colorStrip.release(); return resultStrip; }
From source file:org.ar.rubik.CubePoseEstimator.java
License:Open Source License
/** * Pose Estimation/*from w ww .j a va 2s . c o m*/ * * Deduce real world cube coordinates and rotation * * @param rubikFace * @param image * @param stateModel * @return */ public static CubePose poseEstimation(RubikFace rubikFace, Mat image, StateModel stateModel) { if (rubikFace == null) return null; if (rubikFace.faceRecognitionStatus != FaceRecognitionStatusEnum.SOLVED) return null; LeastMeansSquare lmsResult = rubikFace.lmsResult; if (lmsResult == null) return null; // OpenCV Pose Estimate requires at least four points. if (rubikFace.rhombusList.size() <= 4) return null; if (cameraMatrix == null) { cameraMatrix = stateModel.cameraCalibration.getOpenCVCameraMatrix((int) (image.size().width), (int) (image.size().height)); distCoeffs = new MatOfDouble(stateModel.cameraCalibration.getDistortionCoefficients()); } /* * For the purposes of external camera calibration: i.e., where the cube is * located in camera coordinates, we define the geometry of the face of a * cube composed of nine 3D locations each representing the center of each tile. * Correspondence between these points and nine 2D points from the actual * camera image, along with camera calibration data, are using to calculate * the Pose of the Cube (i.e. "Cube Pose"). * * The geometry of the cube here is defined as having center at {0,0,0}, * and edge size of 2 units (i.e., +/- 1.0). */ // List of real world point and screen points that correspond. List<Point3> objectPointsList = new ArrayList<Point3>(9); List<Point> imagePointsList = new ArrayList<Point>(9); // Create list of image (in 2D) and object (in 3D) points. // Loop over Rubik Face Tiles for (int n = 0; n < 3; n++) { for (int m = 0; m < 3; m++) { Rhombus rhombus = rubikFace.faceRhombusArray[n][m]; // Only use if Rhombus was non null. if (rhombus != null) { // Obtain center of Rhombus in screen image coordinates // Convention: // o X is zero on the left, and increases to the right. // o Y is zero on the top and increases downward. Point imagePoint = new Point(rhombus.center.x, rhombus.center.y); imagePointsList.add(imagePoint); // N and M are actual not conceptual (as in design doc). int mm = 2 - n; int nn = 2 - m; // above now matches design doc. // that is: // o the nn vector is to the right and upwards. // o the mm vector is to the left and upwards. // Calculate center of Tile in OpenCV World Space Coordinates // Convention: // o X is zero in the center, and increases to the left. // o Y is zero in the center and increases downward. // o Z is zero (at the world coordinate origin) and increase away for the camera. float x = (1 - mm) * 0.66666f; float y = -1.0f; float z = -1.0f * (1 - nn) * 0.666666f; Point3 objectPoint = new Point3(x, y, z); objectPointsList.add(objectPoint); } } } // Cast image point list into OpenCV Matrix. MatOfPoint2f imagePoints = new MatOfPoint2f(); imagePoints.fromList(imagePointsList); // Cast object point list into OpenCV Matrix. MatOfPoint3f objectPoints = new MatOfPoint3f(); objectPoints.fromList(objectPointsList); Mat rvec = new Mat(); Mat tvec = new Mat(); // Log.e(Constants.TAG, "Image Points: " + imagePoints.dump()); // Log.e(Constants.TAG, "Object Points: " + objectPoints.dump()); // =+= sometimes a "count >= 4" exception Calib3d.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); Log.v(Constants.TAG, String.format("Open CV Rotation Vector x=%4.2f y=%4.2f z=%4.2f", rvec.get(0, 0)[0], rvec.get(1, 0)[0], rvec.get(2, 0)[0])); // Convert from OpenCV to OpenGL World Coordinates float x = +1.0f * (float) tvec.get(0, 0)[0]; float y = -1.0f * (float) tvec.get(1, 0)[0]; float z = -1.0f * (float) tvec.get(2, 0)[0]; // // =+= Add manual offset correction to translation // x += MenuAndParams.xTranslationOffsetParam.value; // y += MenuAndParams.yTranslationOffsetParam.value; // z += MenuAndParams.zTranslationOffsetParam.value; // Convert Rotation Vector from OpenCL polarity axes definition to OpenGL definition // Note, polarity of x-axis is OK, no need to invert. rvec.put(1, 0, -1.0f * rvec.get(1, 0)[0]); // y-axis rvec.put(2, 0, -1.0f * rvec.get(2, 0)[0]); // z-axis // // =+= Add manual offset correction to Rotation // rvec.put(0, 0, rvec.get(0, 0)[0] + MenuAndParams.xRotationOffsetParam.value * Math.PI / 180.0); // X rotation // rvec.put(1, 0, rvec.get(1, 0)[0] + MenuAndParams.yRotationOffsetParam.value * Math.PI / 180.0); // Y rotation // rvec.put(2, 0, rvec.get(2, 0)[0] + MenuAndParams.zRotationOffsetParam.value * Math.PI / 180.0); // Z rotation // Package up as CubePose object CubePose cubePose = new CubePose(); cubePose.x = x; cubePose.y = y; cubePose.z = z; cubePose.xRotation = rvec.get(0, 0)[0]; cubePose.yRotation = rvec.get(1, 0)[0]; cubePose.zRotation = rvec.get(2, 0)[0]; // Log.e(Constants.TAG, "Result: " + result); // Log.e(Constants.TAG, "Camera: " + cameraMatrix.dump()); // Log.e(Constants.TAG, "Rotation: " + rvec.dump()); // Log.e(Constants.TAG, "Translation: " + tvec.dump()); // // Reporting in OpenGL World Coordinates // Core.rectangle(image, new Point(0, 50), new Point(1270, 150), Constants.ColorBlack, -1); // Core.putText(image, String.format("Translation x=%4.2f y=%4.2f z=%4.2f", x, y, z), new Point(50, 100), Constants.FontFace, 3, Constants.ColorWhite, 3); // Core.putText(image, String.format("Rotation x=%4.0f y=%4.0f z=%4.0f", cubeXrotation, cubeYrotation, cubeZrotation), new Point(50, 150), Constants.FontFace, 3, Constants.ColorWhite, 3); Log.v(Constants.TAG, "Cube Pose: " + cubePose); return cubePose; }
From source file:org.ar.rubik.CubeReconstructor.java
License:Open Source License
/** * Pose Estimation//from ww w .ja v a2 s . c o m * * Deduce real world cube coordinates and rotation * * @param rubikFace * @param image * @param stateModel */ public void poseEstimation(RubikFace rubikFace, Mat image, StateModel stateModel) { if (rubikFace == null) return; if (rubikFace.faceRecognitionStatus != FaceRecognitionStatusEnum.SOLVED) return; LeastMeansSquare lmsResult = rubikFace.lmsResult; if (lmsResult == null) return; // OpenCV Pose Estimate requires at least four points. if (rubikFace.rhombusList.size() <= 4) return; // List of real world point and screen points that correspond. List<Point3> objectPointsList = new ArrayList<Point3>(9); List<Point> imagePointsList = new ArrayList<Point>(9); // Create list of image (in 2D) and object (in 3D) points. // Loop over Rubik Face Tiles/Rhombi for (int n = 0; n < 3; n++) { for (int m = 0; m < 3; m++) { Rhombus rhombus = rubikFace.faceRhombusArray[n][m]; // Only use if Rhombus was non null. if (rhombus != null) { // Obtain center of Rhombus in screen image coordinates // Convention: // o X is zero on the left, and increases to the right. // o Y is zero on the top and increases downward. Point imagePoint = new Point(rhombus.center.x, rhombus.center.y); imagePointsList.add(imagePoint); // N and M are actual not conceptual (as in design doc). int mm = 2 - n; int nn = 2 - m; // above now matches design doc. // that is: // o the nn vector is to the right and upwards. // o the mm vector is to the left and upwards. // Calculate center of Tile in OpenCV World Space Coordinates // Convention: // o X is zero in the center, and increases to the left. // o Y is zero in the center and increases downward. // o Z is zero (at the world coordinate origin) and increase away for the camera. float x = (1 - mm) * 0.66666f; float y = -1.0f; float z = -1.0f * (1 - nn) * 0.666666f; Point3 objectPoint = new Point3(x, y, z); objectPointsList.add(objectPoint); } } } // Cast image point list into OpenCV Matrix. MatOfPoint2f imagePoints = new MatOfPoint2f(); imagePoints.fromList(imagePointsList); // Cast object point list into OpenCV Matrix. MatOfPoint3f objectPoints = new MatOfPoint3f(); objectPoints.fromList(objectPointsList); Mat cameraMatrix = stateModel.cameraParameters.getOpenCVCameraMatrix(); MatOfDouble distCoeffs = new MatOfDouble(); Mat rvec = new Mat(); Mat tvec = new Mat(); // Log.e(Constants.TAG, "Image Points: " + imagePoints.dump()); // Log.e(Constants.TAG, "Object Points: " + objectPoints.dump()); // boolean result = Calib3d.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec); Log.v(Constants.TAG, String.format("Open CV Rotation Vector x=%4.2f y=%4.2f z=%4.2f", rvec.get(0, 0)[0], rvec.get(1, 0)[0], rvec.get(2, 0)[0])); // Convert from OpenCV to OpenGL World Coordinates x = +1.0f * (float) tvec.get(0, 0)[0]; y = -1.0f * (float) tvec.get(1, 0)[0]; z = -1.0f * (float) tvec.get(2, 0)[0]; // =+= Add manual offset correction to translation x += MenuAndParams.xTranslationOffsetParam.value; y += MenuAndParams.yTranslationOffsetParam.value; z += MenuAndParams.zTranslationOffsetParam.value; // Convert Rotation Vector from OpenCL polarity axes definition to OpenGL definition rvec.put(1, 0, -1.0f * rvec.get(1, 0)[0]); rvec.put(2, 0, -1.0f * rvec.get(2, 0)[0]); // =+= Add manual offset correction to Rotation rvec.put(0, 0, rvec.get(0, 0)[0] + MenuAndParams.xRotationOffsetParam.value * Math.PI / 180.0); // X rotation rvec.put(1, 0, rvec.get(1, 0)[0] + MenuAndParams.yRotationOffsetParam.value * Math.PI / 180.0); // Y rotation rvec.put(2, 0, rvec.get(2, 0)[0] + MenuAndParams.zRotationOffsetParam.value * Math.PI / 180.0); // Z rotation // Create an OpenCV Rotation Matrix from a Rotation Vector Mat rMatrix = new Mat(4, 4, CvType.CV_32FC2); Calib3d.Rodrigues(rvec, rMatrix); Log.v(Constants.TAG, "Rodrigues Matrix: " + rMatrix.dump()); /* * Create an OpenGL Rotation Matrix * Notes: * o OpenGL is in column-row order (correct?). * o OpenCV Rodrigues Rotation Matrix is 3x3 where OpenGL Rotation Matrix is 4x4. */ // Initialize all Rotational Matrix elements to zero. for (int i = 0; i < 16; i++) rotationMatrix[i] = 0.0f; // Initialize to zero // Initialize element [3,3] to 1.0: i.e., "w" component in homogenous coordinates rotationMatrix[3 * 4 + 3] = 1.0f; // Copy OpenCV matrix to OpenGL matrix element by element. for (int r = 0; r < 3; r++) for (int c = 0; c < 3; c++) rotationMatrix[r + c * 4] = (float) (rMatrix.get(r, c)[0]); // Diagnostics for (int r = 0; r < 4; r++) Log.v(Constants.TAG, String.format("Rotation Matrix r=%d [%5.2f %5.2f %5.2f %5.2f]", r, rotationMatrix[r + 0], rotationMatrix[r + 4], rotationMatrix[r + 8], rotationMatrix[r + 12])); // Log.e(Constants.TAG, "Result: " + result); // Log.e(Constants.TAG, "Camera: " + cameraMatrix.dump()); // Log.e(Constants.TAG, "Rotation: " + rvec.dump()); // Log.e(Constants.TAG, "Translation: " + tvec.dump()); // // Reporting in OpenGL World Coordinates // Core.rectangle(image, new Point(0, 50), new Point(1270, 150), Constants.ColorBlack, -1); // Core.putText(image, String.format("Translation x=%4.2f y=%4.2f z=%4.2f", x, y, z), new Point(50, 100), Constants.FontFace, 3, Constants.ColorWhite, 3); // Core.putText(image, String.format("Rotation x=%4.0f y=%4.0f z=%4.0f", cubeXrotation, cubeYrotation, cubeZrotation), new Point(50, 150), Constants.FontFace, 3, Constants.ColorWhite, 3); }
From source file:org.ar.rubik.gl.GLRenderer.java
License:Open Source License
/** * Compute Pose Rotation Matrix and return it. * /*w w w. j ava2s. c o m*/ * @param cubePose * @return Pose Rotation Matrix */ private float[] computePoseRotationMatrix(CubePose cubePose) { // Rotational matrix suitable for consumption by OpenGL float[] poseRotationMatrix = new float[16]; // Recreate Open CV matrix for processing by Rodriques algorithm. Mat rvec = new Mat(3, 1, CvType.CV_64FC1); rvec.put(0, 0, new double[] { cubePose.xRotation }); rvec.put(1, 0, new double[] { cubePose.yRotation }); rvec.put(2, 0, new double[] { cubePose.zRotation }); // Log.v(Constants.TAG, "Rotation Vector: " + rvec.dump()); // Create an OpenCV Rotation Matrix from a Rotation Vector Mat rMatrix = new Mat(4, 4, CvType.CV_64FC1); Calib3d.Rodrigues(rvec, rMatrix); // Log.v(Constants.TAG, "Rodrigues Matrix: " + rMatrix.dump()); /* * Create an OpenGL Rotation Matrix * Notes: * o OpenGL is in column-row order (correct?). * o OpenCV Rodrigues Rotation Matrix is 3x3 where OpenGL Rotation Matrix is 4x4. * o OpenGL Rotation Matrix is simple float array variable */ // Initialize all Rotational Matrix elements to zero. for (int i = 0; i < 16; i++) poseRotationMatrix[i] = 0.0f; // Initialize to zero // Initialize element [3,3] to 1.0: i.e., "w" component in homogenous coordinates poseRotationMatrix[3 * 4 + 3] = 1.0f; // Copy OpenCV matrix to OpenGL matrix element by element. for (int r = 0; r < 3; r++) for (int c = 0; c < 3; c++) poseRotationMatrix[r + c * 4] = (float) (rMatrix.get(r, c)[0]); // Diagnostics // for(int r=0; r<4; r++) // Log.v(Constants.TAG, String.format("Rotation Matrix r=%d [%5.2f %5.2f %5.2f %5.2f]", r, poseRotationMatrix[r + 0], poseRotationMatrix[r+4], poseRotationMatrix[r+8], poseRotationMatrix[r+12])); return poseRotationMatrix; }
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. * /*from w ww .ja va2 s. 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); }
From source file:org.firstinspires.ftc.teamcode.libraries.VuforiaBallLib.java
protected static Scalar drawSquare(Mat src, int[] ballPoint, int ballDist) { //find average left and right ball square //find the average color for all the pixels in that square if (ballPoint[0] >= 0 && ballPoint[1] >= 0 && ballPoint[0] + ballDist < src.cols() && ballPoint[1] + ballDist < src.rows()) { double total[] = new double[3]; for (int x = 0; x < ballDist; x++) for (int y = 0; y < ballDist; y++) { double[] pixel = src.get(y + ballPoint[1], x + ballPoint[0]); total[0] += pixel[0];/*from w w w. j av a2 s .c o m*/ total[1] += pixel[1]; total[2] += pixel[2]; } //make average color Scalar color = new Scalar(total[0] / (ballDist * ballDist), total[1] / (ballDist * ballDist), total[2] / (ballDist * ballDist)); Imgproc.rectangle(src, new Point(ballPoint[0], ballPoint[1]), new Point(ballPoint[0] + ballDist, ballPoint[1] + ballDist), color, -1); return color; } else return null; }
From source file:org.firstinspires.ftc.teamcode.opmodes.demo.VumarkHiJackVideo.java
private static Scalar drawSquare(Mat src, int[] ballPoint, int ballDist) { //find average left and right ball square //find the average color for all the pixels in that square if (ballPoint[0] >= 0 && ballPoint[1] >= 0 && ballPoint[0] + ballDist < src.cols() && ballPoint[1] + ballDist < src.rows()) { double total[] = new double[3]; for (int x = 0; x < ballDist; x++) for (int y = 0; y < ballDist; y++) { double[] pixel = src.get(y + ballPoint[1], x + ballPoint[0]); total[0] += pixel[0];/*from w ww .j ava 2 s . co m*/ total[1] += pixel[1]; total[2] += pixel[2]; } //make average color Scalar color = new Scalar(total[0] / (ballDist * ballDist), total[1] / (ballDist * ballDist), total[2] / (ballDist * ballDist)); Imgproc.rectangle(src, new Point(ballPoint[0], ballPoint[1]), new Point(ballPoint[0] + ballDist, ballPoint[1] + ballDist), color, -1); return color; } else return null; }
From source file:org.firstinspires.ftc.teamcode.opmodes.demo.VumarkOpenCV.java
private static void drawSquare(Mat src, int[] ballPoint, int ballDist) { //find average left and right ball square //find the average color for all the pixels in that square if (ballPoint[0] >= 0 && ballPoint[1] >= 0 && ballPoint[0] + ballDist < src.cols() && ballPoint[1] + ballDist < src.rows()) { double total[] = new double[3]; for (int x = 0; x < ballDist; x++) for (int y = 0; y < ballDist; y++) { double[] pixel = src.get(y + ballPoint[1], x + ballPoint[0]); total[0] += pixel[0];/*from w ww . j a v a 2s. c o m*/ total[1] += pixel[1]; total[2] += pixel[2]; } //make average color Scalar color = new Scalar(total[0] / (ballDist * ballDist), total[1] / (ballDist * ballDist), total[2] / (ballDist * ballDist)); Imgproc.rectangle(src, new Point(ballPoint[0], ballPoint[1]), new Point(ballPoint[0] + ballDist, ballPoint[1] + ballDist), color, -1); } }