List of usage examples for org.opencv.core MatOfPoint3f MatOfPoint3f
public MatOfPoint3f()
From source file:arlocros.ComputePose.java
License:Apache License
public boolean computePose(Mat rvec, Mat tvec, Mat image2) throws NyARException, FileNotFoundException { // convert image to NyAR style for processing final INyARRgbRaster imageRaster = NyARImageHelper.createFromMat(image2); // create new marker system configuration i_config = new NyARMarkerSystemConfig(i_param); markerSystemState = new NyARMarkerSystem(i_config); // Create wrapper that passes cam pictures to marker system cameraSensorWrapper = new NyARSensor(i_screen_size); ids = new int[markerPatterns.size()]; patternmap = new HashMap<>(); for (int i = 0; i < markerPatterns.size(); i++) { // create marker description from pattern file and add to marker // system ids[i] = markerSystemState.addARMarker(arCodes.get(i), 25, markerConfig.getMarkerSize()); patternmap.put(ids[i], markerPatterns.get(i)); }//from w w w . j a v a 2s . com cameraSensorWrapper.update(imageRaster); markerSystemState.update(cameraSensorWrapper); // init 3D point list final List<Point3> points3dlist = new ArrayList<>(); final List<Point> points2dlist = new ArrayList<>(); for (final int id : ids) { // process only if this marker has been detected if (markerSystemState.isExistMarker(id) && markerSystemState.getConfidence(id) > 0.7) { // read and add 2D points final NyARIntPoint2d[] vertex2d = markerSystemState.getMarkerVertex2D(id); Point p = new Point(vertex2d[0].x, vertex2d[0].y); points2dlist.add(p); p = new Point(vertex2d[1].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[2].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[3].x, vertex2d[3].y); points2dlist.add(p); // final MatOfPoint mop = new MatOfPoint(); // mop.fromList(points2dlist); // final List<MatOfPoint> pts = new ArrayList<>(); // pts.add(mop); // read and add corresponding 3D points points3dlist.addAll(markerConfig.create3dpointlist(patternmap.get(id))); if (visualization) { // draw red rectangle around detected marker Core.rectangle(image2, new Point(vertex2d[0].x, vertex2d[0].y), new Point(vertex2d[2].x, vertex2d[2].y), new Scalar(0, 0, 255)); final String markerFile = patternmap.get(id).replaceAll(".*4x4_", "").replace(".patt", ""); Core.putText(image2, markerFile, new Point((vertex2d[2].x + vertex2d[0].x) / 2.0, vertex2d[0].y - 5), 4, 1, new Scalar(250, 0, 0)); } } } // load 2D and 3D points to Mats for solvePNP final MatOfPoint3f objectPoints = new MatOfPoint3f(); objectPoints.fromList(points3dlist); final MatOfPoint2f imagePoints = new MatOfPoint2f(); imagePoints.fromList(points2dlist); if (visualization) { // show image with markers detected Imshow.show(image2); } // do not call solvePNP with empty intput data (no markers detected) if (points2dlist.size() == 0) { objectPoints.release(); imagePoints.release(); return false; } // uncomment these lines if using RANSAC-based pose estimation (more // shaking) Mat inliers = new Mat(); Calib3d.solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, 300, 5, 16, inliers, Calib3d.CV_P3P); ArMarkerPoseEstimator.getLog() .info("Points detected: " + points2dlist.size() + " inliers: " + inliers.size()); objectPoints.release(); imagePoints.release(); // avoid publish zero pose if localization failed if (inliers.rows() == 0) { inliers.release(); return false; } inliers.release(); return true; }
From source file:com.android.cts.verifier.sensors.RVCVXCheckAnalyzer.java
License:Apache License
/** * Generate pattern geometry like this one * http://docs.opencv.org/trunk/_downloads/acircles_pattern.png * * @return Array of 3D points//from w ww. j a va 2s . com */ private MatOfPoint3f asymmetricalCircleGrid(Size size) { final int cn = 3; int n = (int) (size.width * size.height); float positions[] = new float[n * cn]; float unit = 0.02f; MatOfPoint3f grid = new MatOfPoint3f(); for (int i = 0; i < size.height; i++) { for (int j = 0; j < size.width * cn; j += cn) { positions[(int) (i * size.width * cn + j + 0)] = (2 * (j / cn) + i % 2) * (float) unit; positions[(int) (i * size.width * cn + j + 1)] = i * unit; positions[(int) (i * size.width * cn + j + 2)] = 0; } } grid.create(n, 1, CvType.CV_32FC3); grid.put(0, 0, positions); return grid; }
From source file:com.github.rosjava_catkin_package_a.ARLocROS.ComputePose.java
License:Apache License
public boolean computePose(Mat rvec, Mat tvec, Mat image2) throws NyARException, FileNotFoundException { // convert image to NyAR style for processing final INyARRgbRaster imageRaster = NyARImageHelper.createFromMat(image2); // create new marker system configuration i_config = new NyARMarkerSystemConfig(i_param); markerSystemState = new NyARMarkerSystem(i_config); // Create wrapper that passes cam pictures to marker system cameraSensorWrapper = new NyARSensor(i_screen_size); ids = new int[markerPatterns.size()]; patternmap = new HashMap<>(); for (int i = 0; i < markerPatterns.size(); i++) { // create marker description from pattern file and add to marker // system ids[i] = markerSystemState.addARMarker(arCodes.get(i), 25, markerConfig.getMarkerSize()); patternmap.put(ids[i], markerPatterns.get(i)); }/*from w w w . jav a 2s . c om*/ cameraSensorWrapper.update(imageRaster); markerSystemState.update(cameraSensorWrapper); // init 3D point list final List<Point3> points3dlist = new ArrayList<>(); final List<Point> points2dlist = new ArrayList<>(); for (final int id : ids) { // process only if this marker has been detected if (markerSystemState.isExistMarker(id) && markerSystemState.getConfidence(id) > 0.7) { // read and add 2D points final NyARIntPoint2d[] vertex2d = markerSystemState.getMarkerVertex2D(id); Point p = new Point(vertex2d[0].x, vertex2d[0].y); points2dlist.add(p); p = new Point(vertex2d[1].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[2].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[3].x, vertex2d[3].y); points2dlist.add(p); final MatOfPoint mop = new MatOfPoint(); mop.fromList(points2dlist); final List<MatOfPoint> pts = new ArrayList<>(); pts.add(mop); // read and add corresponding 3D points points3dlist.addAll(markerConfig.create3dpointlist(patternmap.get(id))); // draw red rectangle around detected marker Core.rectangle(image2, new Point(vertex2d[0].x, vertex2d[0].y), new Point(vertex2d[2].x, vertex2d[2].y), new Scalar(0, 0, 255)); } } // load 2D and 3D points to Mats for solvePNP final MatOfPoint3f objectPoints = new MatOfPoint3f(); objectPoints.fromList(points3dlist); final MatOfPoint2f imagePoints = new MatOfPoint2f(); imagePoints.fromList(points2dlist); if (visualization) { // show image with markers detected Imshow.show(image2); } // do not call solvePNP with empty intput data (no markers detected) if (points2dlist.size() == 0) { return false; } // uncomment these lines if using RANSAC-based pose estimation (more // shaking) Mat inliers = new Mat(); Calib3d.solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, 300, 5, 16, inliers, Calib3d.CV_P3P); ARLoc.getLog().info("Points detected: " + points2dlist.size() + " inliers: " + inliers.size()); // avoid publish zero pose if localization failed if (inliers.rows() == 0) { return false; } return true; }
From source file:org.ar.rubik.CubePoseEstimator.java
License:Open Source License
/** * Pose Estimation// www . ja v a 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//www. j a v a 2 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.firstinspires.ftc.robotcontroller.loomis.opmodes.DemoColorVision.java
@Override public void runOpMode() { // Create parameters object to initialize Vuforia with int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName()); VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); parameters.vuforiaLicenseKey = "AXkMfE//////AAAAGeoid3R/QkyvsvhE2vtnsyyDQKIMOCtifA0K0u47e5Rq8EJ1bcCpJu7o7gx5ijO9W3Ec9de3oWpPQF9sWWvPLeU22iBDpOSejIMch+IbHWNKRmDto3Q4N+C1f71g05vCgVx6ko432OZVvSO+kQJRC91pWPU2XYHZ4qXE7k6iKbLUwhsRaUYNstEkQQGDxn9FrBqO5RLwvU8ZfvQEIjOFJcwSK4EBe5F9WzLZtK0P5nWnaqtUcmPvnNcaR7ynTl54HxWyXO8a/ePz6fNj2nVEUYXa6u2NDvzbJ4UTPpgmfKtHIrQbnXC+QuZRlCXqqIvfcz/Wei6jyOiWMvXz8mw8Jf6a+9yYjc11z5aLMwG1ppFZ"; parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; // Initialize Vuforia this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); // We ask Vuforia to also provide RGB888 images. telemetry.addData("RGB565 images now available: ", Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true)); // We ask Vuforia to keep the last 10 images. vuforia.setFrameQueueCapacity(10);//from w ww . j a v a2 s. c om VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark"); VuforiaTrackable relicTemplate = relicTrackables.get(0); relicTemplate.setName("relicVuMarkTemplate"); telemetry.addData(">", "Press Play to start"); telemetry.update(); waitForStart(); relicTrackables.activate(); int nGrayImages = 0; int nNotGrayImages = 0; BlockingQueue<VuforiaLocalizer.CloseableFrame> frameQueue = vuforia.getFrameQueue(); while (opModeIsActive()) { // Look for ViewMark RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate); if (vuMark != RelicRecoveryVuMark.UNKNOWN) { telemetry.addData("VuMark", "%s visible", vuMark); } else { telemetry.addData("VuMark", "not visible"); } // Pull raw images VuforiaLocalizer.CloseableFrame frame; int ct = 0; while ((frame = frameQueue.poll()) != null) { ct++; for (int i = 0; i < frame.getNumImages(); i++) { Image img = frame.getImage(i); RobotLog.v("Format %d, height %d, width %d", img.getFormat(), img.getHeight(), img.getWidth()); if (img.getFormat() == PIXEL_FORMAT.GRAYSCALE) { nGrayImages++; continue; } else { nNotGrayImages++; //telemetry.addData("iteration", ct); Mat image = new Mat(img.getBufferHeight(), img.getBufferWidth(), CV_8UC3); Mat copy = new Mat(img.getBufferHeight(), img.getBufferWidth(), CV_8UC1); MatOfPoint3f points = new MatOfPoint3f(); Bitmap bitmap = Bitmap.createBitmap(img.getBufferHeight(), img.getBufferWidth(), Bitmap.Config.RGB_565); bitmap.copyPixelsFromBuffer(img.getPixels()); // Utils.bitmapToMat(bitmap, image); // Imgproc.cvtColor(image, copy, Imgproc.COLOR_BGR2GRAY); // Imgproc.HoughCircles(copy, points, Imgproc.CV_HOUGH_GRADIENT,3.0,20.0,100,100,100,150); // RobotLog.v("HOUGHCIRCLES PROBABLY RAN!!!"); // Point3[] circles = points.toArray(); // RobotLog.v("CIRCLE LENGTH IS %d", circles.length); // ArrayList<Point3> uniqueCircles = new ArrayList<>(); float leftX = 100; float leftY = img.getBufferWidth() / 2; float rightX = img.getBufferHeight() / 4 * 3; float radius = img.getBufferWidth() / 10; /*for(int j = 0; j < circles.length && j < 10; j++) { RobotLog.v("CIRCLE FOR LOOP INDEX : %s!!!", j); telemetry.addLine("Center: (" + circles[j].x + ", " + circles[j].y + ") Radius: " + circles[j].z); RobotLog.v("X = %f, Y = %f, RADIUS = %f",circles[j].x,circles[j].y, circles[j].z); //Point3 point3 = new Point3(); //point3.x = circles[j].x; //point3.y = circles[j].y; //point3.z = circles[j].z; //avgX += (float)circles[j].x/circles.length; //avgY += (float)circles[j].y/circles.length; }*/ RobotLog.v("LeftX: %s, LeftY: %s, RightX: %s, Radius: %s", leftX, leftY, rightX, radius); int colorA = averageColorDisk(bitmap, leftX, leftY, radius); int colorB = averageColorDisk(bitmap, rightX, leftY, radius); RobotLog.v("Colors for left ballA are Red: %s, Blue: %s, Green: %s", Color.red(colorA), Color.blue(colorA), Color.green(colorA)); RobotLog.v("Colors for right ballB are Red: %s, Blue: %s, Green: %s", Color.red(colorB), Color.blue(colorB), Color.green(colorB)); if (Color.red(colorA) > 120) { telemetry.addLine("Red Ball"); RobotLog.v("The ball is RED"); } if (Color.blue(colorA) > 120) { telemetry.addLine("Blue Ball"); RobotLog.v("The ball is BLUE"); } telemetry.update(); RobotLog.v("LOG MESSAGE : END OF CIRCLE LOOP!!!"); // telemetry.addData("Length of points is ", points.length); //telemetry.update(); } } frame.close(); } // Only reset the telemetry if new images arrived in the queue } }