List of usage examples for org.opencv.imgproc Moments get_m01
public double get_m01()
From source file:br.cefetmg.lsi.opencv.multipleObjectTracking.processing.MultipleObjectTracking.java
License:Open Source License
private void trackFilteredObject(Ball theBall, Mat threshold, Mat cameraFeed) { List<Ball> balls = new ArrayList<Ball>(); Mat temp = new Mat(); threshold.copyTo(temp);/* w w w . j ava2s . c o m*/ // The two variables below are the return of "findContours" processing. List<MatOfPoint> contours = new ArrayList<MatOfPoint>(); Mat hierarchy = new Mat(); // find contours of filtered image using openCV findContours function Imgproc.findContours(temp, contours, hierarchy, Imgproc.RETR_CCOMP, Imgproc.CHAIN_APPROX_SIMPLE); // use moments method to find our filtered object boolean objectFound = false; if (contours.size() > 0) { int numObjects = contours.size(); //if number of objects greater than MAX_NUM_OBJECTS we have a noisy filter if (numObjects < MAX_NUM_OBJECTS) { for (int i = 0; i < contours.size(); i++) { Moments moment = Imgproc.moments(contours.get(i)); double area = moment.get_m00(); //if the area is less than 20 px by 20px then it is probably just noise //if the area is the same as the 3/2 of the image size, probably just a bad filter //we only want the object with the largest area so we safe a reference area each //iteration and compare it to the area in the next iteration. if (area > MIN_OBJECT_AREA) { Ball ball = new Ball(); ball.setXPos((int) (moment.get_m10() / area)); ball.setYPos((int) (moment.get_m01() / area)); if (theBall != null) { ball.setType(theBall.getType()); ball.setColour(theBall.getColour()); } balls.add(ball); objectFound = true; } else { objectFound = false; } } //let user know you found an object if (objectFound) { //draw object location on screen drawObject(balls, cameraFeed); } } else { Core.putText(cameraFeed, "TOO MUCH NOISE! ADJUST FILTER", new Point(0, 50), 1, 2, new Scalar(0, 0, 255), 2); } } }
From source file:classes.FloodFiller.java
private void fillFrom(Point seed, int lo, int up, Scalar backgroundColor, Scalar contourFillingColor) { Mat object = ObjectGenerator.extract(image, seed.x, seed.y, 10, 10); this.meanColor = Core.mean(object); Rect ccomp = new Rect(); Mat mask = Mat.zeros(image.rows() + 2, image.cols() + 2, CvType.CV_8UC1); int connectivity = 4; int newMaskVal = 255; int ffillMode = 1; int flags = connectivity + (newMaskVal << 8) + (ffillMode == 1 ? Imgproc.FLOODFILL_FIXED_RANGE : 0); Scalar newVal = new Scalar(0.299, 0.587, 0.114); Imgproc.threshold(mask, mask, 1, 128, Imgproc.THRESH_BINARY); filledArea = Imgproc.floodFill(image.clone(), mask, seed, newVal, ccomp, new Scalar(lo, lo, lo), new Scalar(up, up, up), flags); // Highgui.imwrite("mask.png", mask); ImageUtils.saveImage(mask, "mask.png", request); morphologicalImage = new Mat(image.size(), CvType.CV_8UC3); Mat element = new Mat(3, 3, CvType.CV_8U, new Scalar(1)); ArrayList<Mat> mask3 = new ArrayList<Mat>(); mask3.add(mask);/*from w w w . j av a 2s .c o m*/ mask3.add(mask); mask3.add(mask); Core.merge(mask3, mask); // Applying morphological filters Imgproc.erode(mask, morphologicalImage, element); Imgproc.morphologyEx(morphologicalImage, morphologicalImage, Imgproc.MORPH_CLOSE, element, new Point(-1, -1), 9); Imgproc.morphologyEx(morphologicalImage, morphologicalImage, Imgproc.MORPH_OPEN, element, new Point(-1, -1), 2); Imgproc.resize(morphologicalImage, morphologicalImage, image.size()); // Highgui.imwrite("morphologicalImage.png", morphologicalImage); ImageUtils.saveImage(morphologicalImage, "morphologicalImage.png", request); List<MatOfPoint> contours = new ArrayList<MatOfPoint>(); Core.split(mask, mask3); Mat binarymorphologicalImage = mask3.get(0); Imgproc.findContours(binarymorphologicalImage.clone(), contours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_NONE); contoursImage = new Mat(image.size(), CvType.CV_8UC3, backgroundColor); int thickness = -1; // Thicknes should be lower than zero in order to drawn the filled contours Imgproc.drawContours(contoursImage, contours, -1, contourFillingColor, thickness); // Drawing all the contours found // Highgui.imwrite("allContoursImage.png", contoursImage); ImageUtils.saveImage(contoursImage, "allContoursImage.png", request); if (contours.size() > 1) { int minContourWith = 20; int minContourHeight = 20; int maxContourWith = 6400 / 2; int maxContourHeight = 4800 / 2; contours = filterContours(contours, minContourWith, minContourHeight, maxContourWith, maxContourHeight); } if (contours.size() > 0) { MatOfPoint biggestContour = contours.get(0); // getting the biggest contour contourArea = Imgproc.contourArea(biggestContour); if (contours.size() > 1) { biggestContour = Collections.max(contours, new ContourComparator()); // getting the biggest contour in case there are more than one } Point[] points = biggestContour.toArray(); path = "M " + (int) points[0].x + " " + (int) points[0].y + " "; for (int i = 1; i < points.length; ++i) { Point v = points[i]; path += "L " + (int) v.x + " " + (int) v.y + " "; } path += "Z"; biggestContourImage = new Mat(image.size(), CvType.CV_8UC3, backgroundColor); Imgproc.drawContours(biggestContourImage, contours, 0, contourFillingColor, thickness); // Highgui.imwrite("biggestContourImage.png", biggestContourImage); ImageUtils.saveImage(biggestContourImage, "biggestContourImage.png", request); Mat maskForColorExtraction = biggestContourImage.clone(); if (isWhite(backgroundColor)) { Imgproc.dilate(maskForColorExtraction, maskForColorExtraction, new Mat(), new Point(-1, -1), 3); } else { Imgproc.erode(maskForColorExtraction, maskForColorExtraction, new Mat(), new Point(-1, -1), 3); } // Highgui.imwrite("maskForColorExtraction.png", maskForColorExtraction); ImageUtils.saveImage(maskForColorExtraction, "maskForColorExtraction.png", request); Mat extractedColor = new Mat(); if (isBlack(backgroundColor) && isWhite(contourFillingColor)) { Core.bitwise_and(maskForColorExtraction, image, extractedColor); } else { Core.bitwise_or(maskForColorExtraction, image, extractedColor); } // Highgui.imwrite("extractedColor.png", extractedColor); ImageUtils.saveImage(extractedColor, "extractedColor.png", request); computedSearchWindow = Imgproc.boundingRect(biggestContour); topLeftCorner = computedSearchWindow.tl(); Rect croppingRect = new Rect(computedSearchWindow.x, computedSearchWindow.y, computedSearchWindow.width - 1, computedSearchWindow.height - 1); Mat imageForTextRecognition = new Mat(extractedColor.clone(), croppingRect); // Highgui.imwrite(outImageName, imageForTextRecognition); ImageUtils.saveImage(imageForTextRecognition, outImageName, request); // // // Mat data = new Mat(imageForTextRecognition.size(), CvType.CV_8UC3, backgroundColor); // imageForTextRecognition.copyTo(data); // data.convertTo(data, CvType.CV_8UC3); // // // The meanColor variable represents the color in the GBR space, the following line transforms this to the RGB color space, which // // is assumed in the prepareImage method of the TextRecognitionPreparer class // Scalar userColor = new Scalar(meanColor.val[2], meanColor.val[1], meanColor.val[0]); // // ArrayList<String> recognizableImageNames = TextRecognitionPreparer.generateRecognizableImagesNames(data, backgroundColor, userColor); // for (String imageName : recognizableImageNames) { // // try { // // First recognition step // String recognizedText = TextRecognizer.recognize(imageName, true).trim(); // if (recognizedText != null && !recognizedText.isEmpty()) { // recognizedStrings.add(recognizedText); // } // // Second recognition step // recognizedText = TextRecognizer.recognize(imageName, false).trim(); // if (recognizedText != null && !recognizedText.isEmpty()) { // recognizedStrings.add(recognizedText); // } // // } catch (Exception e) { // } // } // //// ArrayList<BufferedImage> recognizableBufferedImages = TextRecognitionPreparer.generateRecognizableBufferedImages(data, backgroundColor, userColor); //// for (BufferedImage bufferedImage : recognizableBufferedImages) { //// try { //// // First recognition step //// String recognizedText = TextRecognizer.recognize(bufferedImage, true).trim(); //// if (recognizedText != null && !recognizedText.isEmpty()) { //// recognizedStrings.add(recognizedText); //// } //// // Second recognition step //// recognizedText = TextRecognizer.recognize(bufferedImage, false).trim(); //// if (recognizedText != null && !recognizedText.isEmpty()) { //// recognizedStrings.add(recognizedText); //// } //// //// } catch (Exception e) { //// } //// } // // // // compute all moments Moments mom = Imgproc.moments(biggestContour); massCenter = new Point(mom.get_m10() / mom.get_m00(), mom.get_m01() / mom.get_m00()); // draw black dot Core.circle(contoursImage, massCenter, 4, contourFillingColor, 8); } }
From source file:classes.ObjectFinder.java
private void computeSearchWindow() { List<MatOfPoint> contours = new ArrayList<MatOfPoint>(); // a vector of contours // retrieve the external contours // all pixels of each contours Imgproc.findContours(this.morphologicalImage.clone(), contours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_NONE);// w w w. j a v a 2 s .c o m // Draw black contours on a white image this.contoursImage = new Mat(morphologicalImage.size(), CvType.CV_8U, new Scalar(255)); if (contours.size() > 1) { int minContourWith = 20; int minContourHeight = 20; int maxContourWith = 6400 / 2; int maxContourHeight = 4800 / 2; contours = filterContours(contours, minContourWith, minContourHeight, maxContourWith, maxContourHeight); } if (contours.size() > 1) { Collections.sort(contours, new ContourComparator()); // Sorttig the contours to take ONLY the bigger one } computedSearchWindow = new Rect(); massCenter = new Point(-1, -1); if (contours.size() > 0) { this.firstContour = contours.get(0); Mat contournedImage = this.firstContour; // draw all contours in black with a thickness of 2 Scalar color = new Scalar(0); int thickness = 2; Imgproc.drawContours(contoursImage, contours, 0, color, thickness); // // testing the bounding box computedSearchWindow = Imgproc.boundingRect(this.firstContour); topLeftCorner = computedSearchWindow.tl(); // compute all moments Moments mom = Imgproc.moments(contournedImage); massCenter = new Point(mom.get_m10() / mom.get_m00(), mom.get_m01() / mom.get_m00()); // draw black dot Core.circle(contoursImage, massCenter, 4, color, 8); } }
From source file:com.shootoff.camera.autocalibration.AutoCalibrationManager.java
License:Open Source License
private Point massCenterMatOfPoint2f(final MatOfPoint2f map) { final Moments moments = Imgproc.moments(map); final Point centroid = new Point(); centroid.x = moments.get_m10() / moments.get_m00(); centroid.y = moments.get_m01() / moments.get_m00(); return centroid; }
From source file:com.ttolley.pongbot.opencv.CvWorker.java
private Target findTarget(List<MatOfPoint> contours, Mat webcam_image, Filter filter) { Target largestTarget = null;/*from ww w .ja v a 2s . c o m*/ for (MatOfPoint matOfPoint : contours) { Moments moment = Imgproc.moments(matOfPoint); double area = moment.get_m00(); if ((largestTarget == null && area > filter.objectSize * filter.objectSize) || (largestTarget != null && area > largestTarget.area)) { // Found object, do something about it largestTarget = new Target(moment.get_m10() / area, moment.get_m01() / area, area); } } if (largestTarget != null) { xPos.addValue(largestTarget.x); yPos.addValue(largestTarget.y); Core.circle(webcam_image, new Point(xPos.getMean(), yPos.getMean()), 10, new Scalar(0, 0, 255)); Core.putText(webcam_image, "[" + xPos.getMean() + " " + yPos.getMean() + "]", new Point(xPos.getMean() - 40, yPos.getMean() + 25), 1, 1, new Scalar(0, 0, 255)); } return largestTarget; }
From source file:eu.fpetersen.robobrain.behavior.followobject.ColorBlobDetector.java
License:Open Source License
public void process(Mat rgbaImage) { Imgproc.pyrDown(rgbaImage, mPyrDownMat); Imgproc.pyrDown(mPyrDownMat, mPyrDownMat); Imgproc.cvtColor(mPyrDownMat, mHsvMat, Imgproc.COLOR_RGB2HSV_FULL); Core.inRange(mHsvMat, mLowerBound, mUpperBound, mMask); Imgproc.dilate(mMask, mDilatedMask, new Mat()); List<MatOfPoint> contours = new ArrayList<MatOfPoint>(); Imgproc.findContours(mDilatedMask, contours, mHierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); // Find max contour area maxArea = 0;//from w w w . j a v a2s . com Iterator<MatOfPoint> each = contours.iterator(); Mat biggestContour = null; while (each.hasNext()) { MatOfPoint wrapper = each.next(); double area = Imgproc.contourArea(wrapper); if (area > maxArea) { maxArea = area; biggestContour = wrapper.clone(); } } if (biggestContour != null) { Core.multiply(biggestContour, new Scalar(4, 4), biggestContour); Moments mo = Imgproc.moments(biggestContour); centroidOfMaxArea = new Point(mo.get_m10() / mo.get_m00(), mo.get_m01() / mo.get_m00()); } else { centroidOfMaxArea = null; } // Filter contours by area and resize to fit the original image size mContours.clear(); each = contours.iterator(); while (each.hasNext()) { MatOfPoint contour = each.next(); if (Imgproc.contourArea(contour) > mMinContourArea * maxArea) { Core.multiply(contour, new Scalar(4, 4), contour); mContours.add(contour); } } Imgproc.drawContours(rgbaImage, mContours, -1, CONTOUR_COLOR); }
From source file:org.firstinspires.ftc.teamcode.AutonomousVuforia.java
public int getBeaconConfig(Image img, VuforiaTrackable beacon, CameraCalibration camCal) { OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) beacon.getListener()).getRawPose(); telemetry.addData("Stuff", pose != null); telemetry.addData("Stuff", img != null); try {/*from w w w .j a va2s .c om*/ telemetry.addData("Stuff", img.getPixels() != null); } catch (Exception e) { telemetry.addData("Stuff", e); } telemetry.update(); if (pose != null && img != null && img.getPixels() != null) { Matrix34F rawPose = new Matrix34F(); float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12); rawPose.setData(poseData); float[][] corners = new float[4][2]; corners[0] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, 276, 0)).getData(); corners[1] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, 276, 0)).getData(); corners[2] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, 92, 0)).getData(); corners[3] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, 92, 0)).getData(); Bitmap bm = Bitmap.createBitmap(img.getWidth(), img.getHeight(), Bitmap.Config.RGB_565); bm.copyPixelsFromBuffer(img.getPixels()); Mat crop = new Mat(bm.getHeight(), bm.getWidth(), CvType.CV_8UC3); Utils.bitmapToMat(bm, crop); float x = Math.min(Math.min(corners[1][0], corners[3][0]), Math.min(corners[0][0], corners[2][0])); float y = Math.min(Math.min(corners[1][1], corners[3][1]), Math.min(corners[0][1], corners[2][1])); float width = Math.max(Math.abs(corners[0][0] - corners[2][0]), Math.abs(corners[1][0] - corners[3][0])); float height = Math.max(Math.abs(corners[0][1] - corners[2][1]), Math.abs(corners[1][1] - corners[3][1])); x = Math.max(x, 0); y = Math.max(y, 0); width = (x + width > crop.cols()) ? crop.cols() - x : width; height = (y + height > crop.rows()) ? crop.rows() - y : height; Mat cropped = new Mat(crop, new Rect((int) x, (int) y, (int) width, (int) height)); Imgproc.cvtColor(cropped, cropped, Imgproc.COLOR_RGB2HSV_FULL); Mat mask = new Mat(); Core.inRange(cropped, blueLow, blueHigh, mask); Moments mmnts = Imgproc.moments(mask, true); if (mmnts.get_m00() > mask.total() * 0.8) { return BEACON_ALL_BLUE; } else if (mmnts.get_m00() < mask.total() * 0.8) { return BEACON_NO_BLUE; } if ((mmnts.get_m01() / mmnts.get_m00()) < cropped.rows() / 2) { return BEACON_RED_BLUE; } else { return BEACON_BLUERED; } // else } return BEACON_NOT_VISIBLE; }
From source file:org.firstinspires.ftc.teamcode.VuforiaColor.java
public void runOpMode() throws InterruptedException { // frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor"); // backRightMotor = hardwareMap.dcMotor.get("backRightMotor"); // frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor"); // backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor"); // rollerMotor = hardwareMap.dcMotor.get("rollerMotor"); ////w w w. j a va 2 s. c o m // backRightMotor.setDirection(DcMotor.Direction.REVERSE); // backLeftMotor.setDirection(DcMotor.Direction.REVERSE); colorDetector = new ColorBlobDetector(); VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); parameters.vuforiaLicenseKey = "ATjJBiD/////AAAAGc0JoedLjk5flVb2gExO3UVJCpOq5U4cyH9czcMyX5C8h+1AWXo7A0CU24r/IVeoC+7Te9zwJkX6IjHv5c77UNqrsyerM7pbjywj6/2NlzSUwb3jtEd9APhY5cOoSibb5NDRFM9beUWt0k4HuFMaw5OIZRs5YWge7KaJt5SzhqEFMQ6Loo8eugB9BBbPfuV3d7u4sQZBAKeRsR9mmnfvFJTUHHgcPlALU/rJBgw40AeFFvChjzNhwtlWYymeM/0173jH7JB2dyhoNtn/9byIUQzMw8KtaXbD3IfFJySLgJWmYjaA7cKdboL0nvkOoZNFMm2yqenbUDe/CEIMkhAsKjS3sgX4t6Fq+8gkhSnOS/Vd"; parameters.cameraDirection = VuforiaLocalizer.CameraDirection.FRONT; this.vuforia = ClassFactory.createVuforiaLocalizer(parameters); Vuforia.setFrameFormat(PIXEL_FORMAT.RGB565, true); //enables RGB565 format for the image vuforia.setFrameQueueCapacity(1); //tells VuforiaLocalizer to only store one frame at a time piController = new PIController(.0016, 0.00013, 0.00023, 0.000012); Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 1); VuforiaTrackables visionTargets = vuforia.loadTrackablesFromAsset("FTC_2016-17"); VuforiaTrackable wheelsTarget = visionTargets.get(0); wheelsTarget.setName("Wheels"); // Wheels VuforiaTrackable toolsTarget = visionTargets.get(1); toolsTarget.setName("Tools"); // Tools VuforiaTrackable legosTarget = visionTargets.get(2); legosTarget.setName("Legos"); // Legos VuforiaTrackable gearsTarget = visionTargets.get(3); gearsTarget.setName("Gears"); // Gears /** For convenience, gather together all the trackable objects in one easily-iterable collection */ List<VuforiaTrackable> allTrackables = new ArrayList<VuforiaTrackable>(); allTrackables.addAll(visionTargets); /** * We use units of mm here because that's the recommended units of measurement for the * size values specified in the XML for the ImageTarget trackables in data sets. E.g.: * <ImageTarget name="stones" size="247 173"/> * You don't *have to* use mm here, but the units here and the units used in the XML * target configuration files *must* correspond for the math to work out correctly. */ float mmPerInch = 25.4f; float mmBotLength = 16 * mmPerInch; float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot float mmFTCFieldWidth = (12 * 12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels float mmVisionTargetZOffset = 5.75f * mmPerInch; float mmPhoneZOffset = 5.5f * mmPerInch; OpenGLMatrix gearsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-mmFTCFieldWidth / 2, -mmFTCFieldWidth / 12, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); gearsTarget.setLocation(gearsTargetLocationOnField); RobotLog.ii(TAG, "Gears Target=%s", format(gearsTargetLocationOnField)); OpenGLMatrix toolsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-mmFTCFieldWidth / 2, mmFTCFieldWidth / 4, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); toolsTarget.setLocation(toolsTargetLocationOnField); RobotLog.ii(TAG, "Tools Target=%s", format(toolsTargetLocationOnField)); /* * To place the Wheels and Legos Targets on the Blue Audience wall: * - First we rotate it 90 around the field's X axis to flip it upright * - Finally, we translate it along the Y axis towards the blue audience wall. */ OpenGLMatrix wheelsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(mmFTCFieldWidth / 12, mmFTCFieldWidth / 2, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); wheelsTarget.setLocation(wheelsTargetLocationOnField); RobotLog.ii(TAG, "Wheels Target=%s", format(wheelsTargetLocationOnField)); OpenGLMatrix legosTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(-mmFTCFieldWidth / 4, mmFTCFieldWidth / 2, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); legosTarget.setLocation(legosTargetLocationOnField); RobotLog.ii(TAG, "Legos Target=%s", format(legosTargetLocationOnField)); /** * Create a transformation matrix describing where the phone is on the robot. Here, we * put the phone on the right hand side of the robot with the screen facing in (see our * choice of BACK camera above) and in landscape mode. Starting from alignment between the * robot's and phone's axes, this is a rotation of -90deg along the Y axis. * * When determining whether a rotation is positive or negative, consider yourself as looking * down the (positive) axis of rotation from the positive towards the origin. Positive rotations * are then CCW, and negative rotations CW. An example: consider looking down the positive Z * axis towards the origin. A positive rotation about Z (ie: a rotation parallel to the the X-Y * plane) is then CCW, as one would normally expect from the usual classic 2D geometry. */ OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix.translation(mmBotWidth / 2, 0, mmPhoneZOffset) .multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.YZY, AngleUnit.DEGREES, 0, 180, 0)); RobotLog.ii(TAG, "phone=%s", format(phoneLocationOnRobot)); ((VuforiaTrackableDefaultListener) visionTargets.get(0).getListener()) .setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) visionTargets.get(1).getListener()) .setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) visionTargets.get(2).getListener()) .setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) visionTargets.get(3).getListener()) .setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); /** Wait for the game to begin */ telemetry.addData(">", "Press Play to start tracking"); telemetry.addData("OpenCV", Core.NATIVE_LIBRARY_NAME); telemetry.update(); waitForStart(); /** Start tracking the data sets we care about. */ visionTargets.activate(); hitRed = true; isButtonHit = false; directionFoundInARow = 0; directionToHit = ""; telemetry.addData("Loop", "Out"); telemetry.update(); while (opModeIsActive()) { String visibleTarget = ""; Mat img = null; Mat croppedImg = null; Point beaconImageCenter = null; VuforiaLocalizer.CloseableFrame frame = vuforia.getFrameQueue().take(); telemetry.update(); if (frame != null) { Image rgb = null; long numImages = frame.getNumImages(); for (int i = 0; i < numImages; i++) { if (frame.getImage(i).getFormat() == PIXEL_FORMAT.RGB565) { rgb = frame.getImage(i); break; } //if } //for if (rgb != null) { Bitmap bmp = Bitmap.createBitmap(rgb.getWidth(), rgb.getHeight(), Bitmap.Config.RGB_565); bmp.copyPixelsFromBuffer(rgb.getPixels()); img = new Mat(); Utils.bitmapToMat(bmp, img); telemetry.addData("Img", "Converted"); telemetry.update(); } } for (VuforiaTrackable beacon : allTrackables) { // Add beacon to telemetry if visible if (((VuforiaTrackableDefaultListener) beacon.getListener()).isVisible()) { visibleTarget = beacon.getName(); telemetry.addData(visibleTarget, "Visible"); } OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener) beacon.getListener()) .getUpdatedRobotLocation(); if (robotLocationTransform != null) { lastLocation = robotLocationTransform; } OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) beacon.getListener()).getRawPose(); if (pose != null) { Matrix34F rawPose = new Matrix34F(); float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12); rawPose.setData(poseData); // Corners of beacon image in camera image Vec2F upperLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-127, 92, 0)); Vec2F upperRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(127, 92, 0)); Vec2F lowerLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-127, -92, 0)); Vec2F lowerRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(127, -92, 0)); VectorF translation = pose.getTranslation(); /** First argument is get(1) if phone is vertical First argument is get(0) if phone is horizontal */ if (img != null && !isButtonHit) { telemetry.addData(beacon.getName() + "-Translation", translation); // Vectors are stored (y,x). Coordinate system starts in top right int height = (int) (upperLeft.getData()[0] - lowerLeft.getData()[0]); int width = (int) (upperRight.getData()[1] - upperLeft.getData()[1]); int rowStart = (int) upperRight.getData()[0] - height < 0 ? 1 : (int) upperRight.getData()[0] - height; int rowEnd = rowStart + height > img.rows() ? img.rows() - 1 : rowStart + height; int colStart = (int) upperRight.getData()[1] < 0 ? 1 : (int) upperRight.getData()[1]; int colEnd = colStart + width > img.cols() ? img.cols() - 1 : colStart + width; telemetry.addData("Target Location", ""); telemetry.addData("[" + upperLeft.getData()[0] + "," + upperLeft.getData()[1] + "]", "[" + upperRight.getData()[0] + "," + upperRight.getData()[1] + "]"); telemetry.addData("[" + lowerLeft.getData()[0] + "," + lowerLeft.getData()[1] + "]", "[" + lowerRight.getData()[0] + "," + lowerRight.getData()[1] + "]"); telemetry.addData(colStart + "", rowStart); telemetry.addData(colEnd + "", rowEnd); telemetry.addData(img.rows() + "", img.cols()); telemetry.update(); // Crop the image to look only at the beacon // TODO Verify beacon is in cropped image // NEED TO CHECK BEACON HEIGHT FOR INCLUSION IN CROPPED IMAGE croppedImg = img.submat(rowStart, rowEnd, colStart, colEnd); } } } // Process the rgb image if (croppedImg != null && !isButtonHit) { // Find the color of the beacon you need to hit if (hitRed) { colorDetector.setHsvColor(new Scalar(230, 75, 255)); // Red detector, needs verification with beacon } else { colorDetector.setHsvColor(new Scalar(130, 150, 255)); // Blue detector, needs verification with beacon } colorDetector.process(croppedImg); // Calculate the center of the blob detected Point beaconToHitCenter = null; List<Moments> blueMu = new ArrayList<>(colorDetector.getContours().size()); for (int i = 0; i < colorDetector.getContours().size(); i++) { blueMu.add(Imgproc.moments(colorDetector.getContours().get(i), false)); Moments p = blueMu.get(i); int x = (int) (p.get_m10() / p.get_m00()); int y = (int) (p.get_m01() / p.get_m00()); beaconToHitCenter = new Point(x, y); } // Find the color of the beacon you are not hitting if (hitRed) { colorDetector.setHsvColor(new Scalar(130, 150, 255)); // Blue detector, needs verification with beacon } else { colorDetector.setHsvColor(new Scalar(230, 75, 255)); // Red detector, needs verification with beacon } colorDetector.process(croppedImg); // Calculate the center of the blob detected Point secondReferenceCenter = null; List<Moments> redMu = new ArrayList<>(colorDetector.getContours().size()); for (int i = 0; i < colorDetector.getContours().size(); i++) { redMu.add(Imgproc.moments(colorDetector.getContours().get(i), false)); Moments p = redMu.get(i); int x = (int) (p.get_m10() / p.get_m00()); int y = (int) (p.get_m01() / p.get_m00()); secondReferenceCenter = new Point(x, y); } // Use the two centers of the blobs to determine which direction to hit if (beaconToHitCenter != null && secondReferenceCenter != null && !isButtonHit) { // (!isButtonHit) Only hit the button once // (!needToTurn) Do not hit the button if the robot is not straight centered // hitBeaconButton(isLeft(center, beaconImageCenter)); if (isLeft(beaconToHitCenter, secondReferenceCenter)) { if (!directionToHit.equals("Left")) { directionFoundInARow = 0; } directionFoundInARow++; directionToHit = "Left"; } else { if (!directionToHit.equals("Right")) { directionFoundInARow = 0; } directionFoundInARow++; directionToHit = "Right"; } } // Find the color five times in a row before hitting it if (directionFoundInARow >= 3) { isButtonHit = true; } } if (isButtonHit) { telemetry.addData("Hit Button-", directionToHit); } // if(needToTurn) { // turn(degreesToTurn); // telemetry.addData("Turn-", degreesToTurn); // } /** * Provide feedback as to where the robot was last located (if we know). */ if (lastLocation != null) { // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); telemetry.addData("Pos", myFormat(lastLocation)); if (!visibleTarget.equals("")) { telemetry.addData("Move", piController.processLocation(lastLocation, visibleTarget)); } } else { telemetry.addData("Pos", "Unknown"); } telemetry.update(); idle(); } }
From source file:org.firstinspires.ftc.teamcode.VuforiaMovement.java
public void runOpMode() throws InterruptedException { frontRightMotor = hardwareMap.dcMotor.get("frontRightMotor"); backRightMotor = hardwareMap.dcMotor.get("backRightMotor"); frontLeftMotor = hardwareMap.dcMotor.get("frontLeftMotor"); backLeftMotor = hardwareMap.dcMotor.get("backLeftMotor"); rollerMotor = hardwareMap.dcMotor.get("rollerMotor"); backRightMotor.setDirection(DcMotor.Direction.REVERSE); backLeftMotor.setDirection(DcMotor.Direction.REVERSE); colorDetector = new ColorBlobDetector(); VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(R.id.cameraMonitorViewId); parameters.vuforiaLicenseKey = "ATjJBiD/////AAAAGc0JoedLjk5flVb2gExO3UVJCpOq5U4cyH9czcMyX5C8h+1AWXo7A0CU24r/IVeoC+7Te9zwJkX6IjHv5c77UNqrsyerM7pbjywj6/2NlzSUwb3jtEd9APhY5cOoSibb5NDRFM9beUWt0k4HuFMaw5OIZRs5YWge7KaJt5SzhqEFMQ6Loo8eugB9BBbPfuV3d7u4sQZBAKeRsR9mmnfvFJTUHHgcPlALU/rJBgw40AeFFvChjzNhwtlWYymeM/0173jH7JB2dyhoNtn/9byIUQzMw8KtaXbD3IfFJySLgJWmYjaA7cKdboL0nvkOoZNFMm2yqenbUDe/CEIMkhAsKjS3sgX4t6Fq+8gkhSnOS/Vd"; parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK; this.vuforia = new VuforiaLocalizerImplSubclass(parameters); Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 4); VuforiaTrackables beacons = vuforia.loadTrackablesFromAsset("FTC_2016-17"); beacons.get(0).setName("Wheels"); beacons.get(1).setName("Tools"); beacons.get(2).setName("Legos"); beacons.get(3).setName("Gears"); float mmPerInch = 25.4f; float mmBotWidth = 18 * mmPerInch; // ... or whatever is right for your robot float mmFTCFieldWidth = (12 * 12 - 2) * mmPerInch; // the FTC field is ~11'10" center-to-center of the glass panels float mmVisionTargetZOffset = 5.75f * mmPerInch; // Initialize the location of the targets and phone on the field OpenGLMatrix wheelsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(mmFTCFieldWidth / 12, mmFTCFieldWidth / 2, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); beacons.get(0).setLocation(wheelsTargetLocationOnField); RobotLog.ii(TAG, "Wheels Target=%s", format(wheelsTargetLocationOnField)); OpenGLMatrix toolsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-mmFTCFieldWidth / 2, mmFTCFieldWidth / 4, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); beacons.get(1).setLocation(toolsTargetLocationOnField); RobotLog.ii(TAG, "Tools Target=%s", format(toolsTargetLocationOnField)); OpenGLMatrix legosTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the Blue Audience wall. Our translation here is a positive translation in Y.*/ .translation(-mmFTCFieldWidth / 4, mmFTCFieldWidth / 2, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 0, 0)); beacons.get(2).setLocation(legosTargetLocationOnField); RobotLog.ii(TAG, "Legos Target=%s", format(legosTargetLocationOnField)); OpenGLMatrix gearsTargetLocationOnField = OpenGLMatrix /* Then we translate the target off to the RED WALL. Our translation here is a negative translation in X.*/ .translation(-mmFTCFieldWidth / 2, -mmFTCFieldWidth / 12, mmVisionTargetZOffset) .multiplied(Orientation.getRotationMatrix( /* First, in the fixed (field) coordinate system, we rotate 90deg in X, then 90 in Z */ AxesReference.EXTRINSIC, AxesOrder.XZX, AngleUnit.DEGREES, 90, 90, 0)); beacons.get(3).setLocation(gearsTargetLocationOnField); RobotLog.ii(TAG, "Gears Target=%s", format(gearsTargetLocationOnField)); OpenGLMatrix phoneLocationOnRobot = OpenGLMatrix.translation(mmBotWidth / 2, 0, 0).multiplied(Orientation .getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.YZY, AngleUnit.DEGREES, -90, 0, 0)); RobotLog.ii(TAG, "Phone=%s", format(phoneLocationOnRobot)); ((VuforiaTrackableDefaultListener) beacons.get(0).getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) beacons.get(1).getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) beacons.get(2).getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); ((VuforiaTrackableDefaultListener) beacons.get(3).getListener()).setPhoneInformation(phoneLocationOnRobot, parameters.cameraDirection); /** Wait for the game to begin */ telemetry.addData(">", "Press Play to start tracking"); telemetry.addData("OpenCV", Core.NATIVE_LIBRARY_NAME); telemetry.update();// w w w .j av a 2s.com waitForStart(); /** Start tracking the data sets we care about. */ beacons.activate(); hitRed = true; isButtonHit = false; directionFoundInARow = 0; directionToHit = ""; movingToFirstBeacon = false; liningUpWithFirstBeacon = false; movingToSecondBeacon = false; liningUpWithSecondBeacon = false; while (opModeIsActive()) { String visibleTarget = ""; Mat img = null; Mat croppedImg = null; Point beaconImageCenter = null; if (movingToFirstBeacon) { // TODO Estimate distance to the beacon from a point TBD // TODO Estimate distance to move forward and turn to face the beacon until second movement set // Move this outside the loop? // Move forward until you see the beacon while (movingToFirstBeacon) { // Move in increments to minimize the times you check the trackables for (int i = 0; i < 50; i++) { frontRightMotor.setPower(1); backRightMotor.setPower(1); frontLeftMotor.setPower(1); backLeftMotor.setPower(1); } for (VuforiaTrackable beacon : beacons) { // Add beacon to telemetry if visible if (((VuforiaTrackableDefaultListener) beacon.getListener()).isVisible()) { visibleTarget = beacon.getName(); telemetry.addData(visibleTarget, "Visible"); } OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener) beacon .getListener()).getUpdatedRobotLocation(); if (robotLocationTransform != null) { lastLocation = robotLocationTransform; } } // Move to the beacon until the beacon is in sight if (lastLocation != null) { movingToFirstBeacon = false; // Only execute this once } } } while (liningUpWithFirstBeacon) { for (VuforiaTrackable beacon : beacons) { // Add beacon to telemetry if visible if (((VuforiaTrackableDefaultListener) beacon.getListener()).isVisible()) { visibleTarget = beacon.getName(); telemetry.addData(visibleTarget, "Visible"); } OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener) beacon.getListener()) .getUpdatedRobotLocation(); if (robotLocationTransform != null) { lastLocation = robotLocationTransform; } } RobotMovement movement = processLocation(lastLocation, visibleTarget); if (movement.isNoMovement()) { liningUpWithFirstBeacon = false; } processMovement(movement); } if (movingToSecondBeacon) { // TODO Estimate the movements/distance from the first beacon to the second movingToSecondBeacon = false; // Only execute this once } if (vuforia.rgb != null && !isButtonHit) { Bitmap bmp = Bitmap.createBitmap(vuforia.rgb.getWidth(), vuforia.rgb.getHeight(), Bitmap.Config.RGB_565); bmp.copyPixelsFromBuffer(vuforia.rgb.getPixels()); img = new Mat(); Utils.bitmapToMat(bmp, img); } for (VuforiaTrackable beacon : beacons) { // Add beacon to telemetry if visible if (((VuforiaTrackableDefaultListener) beacon.getListener()).isVisible()) { visibleTarget = beacon.getName(); telemetry.addData(visibleTarget, "Visible"); } OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener) beacon.getListener()) .getUpdatedRobotLocation(); if (robotLocationTransform != null) { lastLocation = robotLocationTransform; } OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) beacon.getListener()).getRawPose(); if (pose != null) { Matrix34F rawPose = new Matrix34F(); float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12); rawPose.setData(poseData); // Corners of beacon image in camera image Vec2F upperLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-127, 92, 0)); Vec2F upperRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(127, 92, 0)); Vec2F lowerLeft = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(-127, -92, 0)); Vec2F lowerRight = Tool.projectPoint(vuforia.getCameraCalibration(), rawPose, new Vec3F(127, -92, 0)); VectorF translation = pose.getTranslation(); /** First argument is get(1) if phone is vertical First argument is get(0) if phone is horizontal */ // DOES NOT WORK??? degreesToTurn = Math.toDegrees(Math.atan2(translation.get(1), translation.get(2))); telemetry.addData("Degrees-", degreesToTurn); // TODO Check degreee turning threshold if (Math.abs(degreesToTurn) > 15) { // Turn after doing calculating transformations needToTurn = true; } if (img != null && !isButtonHit) { telemetry.addData(beacon.getName() + "-Translation", translation); telemetry.addData(beacon.getName() + "-Degrees", degreesToTurn); // Vectors are stored (y,x). Coordinate system starts in top right int height = (int) (lowerLeft.getData()[0] - upperLeft.getData()[0]); int width = (int) (upperLeft.getData()[1] - upperRight.getData()[1]); int rowStart = (int) upperRight.getData()[0] - height < 0 ? 0 : (int) upperRight.getData()[0] - height; int rowEnd = rowStart + height > img.rows() ? img.rows() - 1 : rowStart + height; int colStart = (int) upperRight.getData()[1] < 0 ? 0 : (int) upperRight.getData()[1]; int colEnd = colStart + width > img.cols() ? img.cols() - 1 : colStart + width; telemetry.addData("Target Location", ""); telemetry.addData("[" + upperLeft.getData()[0] + "," + upperLeft.getData()[1] + "]", "[" + upperRight.getData()[0] + "," + upperRight.getData()[1] + "]"); telemetry.addData("[" + lowerLeft.getData()[0] + "," + lowerLeft.getData()[1] + "]", "[" + lowerRight.getData()[0] + "," + lowerRight.getData()[1] + "]"); telemetry.addData(colStart + "", rowStart); telemetry.addData(colEnd + "", rowEnd); // Crop the image to look only at the beacon // TODO Verify beacon is in cropped image // NEED TO CHECK BEACON HEIGHT FOR INCLUSION IN CROPPED IMAGE croppedImg = img.submat(rowStart, rowEnd, colStart, colEnd); } } } // Process the rgb image if (croppedImg != null && !isButtonHit) { // Find the color of the beacon you need to hit if (hitRed) { colorDetector.setHsvColor(new Scalar(180, 240, 240)); // Red detector, needs verification with beacon } else { colorDetector.setHsvColor(new Scalar(25, 255, 185)); // Blue detector, needs verification with beacon } colorDetector.process(croppedImg); // Calculate the center of the blob detected Point beaconToHitCenter = null; List<Moments> blueMu = new ArrayList<>(colorDetector.getContours().size()); for (int i = 0; i < colorDetector.getContours().size(); i++) { blueMu.add(Imgproc.moments(colorDetector.getContours().get(i), false)); Moments p = blueMu.get(i); int x = (int) (p.get_m10() / p.get_m00()); int y = (int) (p.get_m01() / p.get_m00()); beaconToHitCenter = new Point(x, y); } // Find the color of the beacon you are not hitting if (hitRed) { colorDetector.setHsvColor(new Scalar(25, 255, 185)); // Blue detector, needs verification with beacon } else { colorDetector.setHsvColor(new Scalar(180, 240, 240)); // Red detector, needs verification with beacon } colorDetector.process(croppedImg); // Calculate the center of the blob detected Point secondReferenceCenter = null; List<Moments> redMu = new ArrayList<>(colorDetector.getContours().size()); for (int i = 0; i < colorDetector.getContours().size(); i++) { redMu.add(Imgproc.moments(colorDetector.getContours().get(i), false)); Moments p = redMu.get(i); int x = (int) (p.get_m10() / p.get_m00()); int y = (int) (p.get_m01() / p.get_m00()); secondReferenceCenter = new Point(x, y); } // Use the two centers of the blobs to determine which direction to hit if (beaconToHitCenter != null && secondReferenceCenter != null && !isButtonHit && !needToTurn) { // (!isButtonHit) Only hit the button once // (!needToTurn) Do not hit the button if the robot is not straight centered // hitBeaconButton(isLeft(center, beaconImageCenter)); if (isLeft(beaconToHitCenter, secondReferenceCenter)) { if (!directionToHit.equals("Left")) { directionFoundInARow = 0; } directionFoundInARow++; directionToHit = "Left"; } else { if (!directionToHit.equals("Right")) { directionFoundInARow = 0; } directionFoundInARow++; directionToHit = "Right"; } } // Find the color five times in a row before hitting it if (directionFoundInARow >= 3) { isButtonHit = true; } } if (isButtonHit) { telemetry.addData("Hit Button-", directionToHit); } // if(needToTurn) { // turn(degreesToTurn); // telemetry.addData("Turn-", degreesToTurn); // } /** * Provide feedback as to where the robot was last located (if we know). */ if (lastLocation != null) { // RobotLog.vv(TAG, "robot=%s", format(lastLocation)); telemetry.addData("Pos", myFormat(lastLocation)); if (!visibleTarget.equals("")) { telemetry.addData("Move", processLocation(lastLocation, visibleTarget)); } } else { telemetry.addData("Pos", "Unknown"); } telemetry.update(); idle(); } }
From source file:org.firstinspires.ftc.teamcode.VuforiaOp.java
public int getBeaconConfig(Image img, VuforiaTrackableDefaultListener beacon, CameraCalibration camCal) { OpenGLMatrix pose = beacon.getRawPose(); if (pose != null && img != null && img.getPixels() != null) { Matrix34F rawPose = new Matrix34F(); float[] poseData = Arrays.copyOfRange(pose.transposed().getData(), 0, 12); rawPose.setData(poseData);//from www .j av a 2 s . c om //calculating pixel coordinates of beacon corners float[][] corners = new float[4][2]; corners[0] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, 276, 0)).getData(); //upper left of beacon corners[1] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, 276, 0)).getData(); //upper right of beacon corners[2] = Tool.projectPoint(camCal, rawPose, new Vec3F(127, 92, 0)).getData(); //lower right of beacon corners[3] = Tool.projectPoint(camCal, rawPose, new Vec3F(-127, 92, 0)).getData(); //lower left of beacon //getting camera image... Bitmap bm = Bitmap.createBitmap(img.getWidth(), img.getHeight(), Bitmap.Config.RGB_565); bm.copyPixelsFromBuffer(img.getPixels()); //turning the corner pixel coordinates into a proper bounding box Mat crop = OCVUtils.bitmapToMat(bm, CvType.CV_8UC3); float x = Math.min(Math.min(corners[1][0], corners[3][0]), Math.min(corners[0][0], corners[2][0])); float y = Math.min(Math.min(corners[1][1], corners[3][1]), Math.min(corners[0][1], corners[2][1])); float width = Math.max(Math.abs(corners[0][0] - corners[2][0]), Math.abs(corners[1][0] - corners[3][0])); float height = Math.max(Math.abs(corners[0][1] - corners[2][1]), Math.abs(corners[1][1] - corners[3][1])); //make sure our bounding box doesn't go outside of the image //OpenCV doesn't like that... x = Math.max(x, 0); y = Math.max(y, 0); width = (x + width > crop.cols()) ? crop.cols() - x : width; height = (y + height > crop.rows()) ? crop.rows() - y : height; //cropping bounding box out of camera image final Mat cropped = new Mat(crop, new Rect((int) x, (int) y, (int) width, (int) height)); Bitmap pic = OCVUtils.matToBitmap(cropped); //filtering out non-beacon-blue colours in HSV colour space Imgproc.cvtColor(cropped, cropped, Imgproc.COLOR_RGB2HSV_FULL); /*try { FileOutputStream out = new FileOutputStream(new File("/storage/emulated/0/", "poop.txt")); out.write((new String("ppoooop")).getBytes()); out.close(); } catch (FileNotFoundException e){} catch (IOException e){} */ /*try { FileOutputStream fos = new FileOutputStream(new File("/storage/emulated/0/", "cropped.png")); //bm.compress(Bitmap.CompressFormat.PNG, 90, fos); if (pic.compress(Bitmap.CompressFormat.PNG, 100, fos)) { } else { } fos.close(); }catch (IOException e) {} try { FileOutputStream fos = new FileOutputStream(new File("/storage/emulated/0/", "non.png")); //bm.compress(Bitmap.CompressFormat.PNG, 90, fos); if (bm.compress(Bitmap.CompressFormat.PNG, 100, fos)) { } else { tempLog("didgfeds"); } fos.close(); }catch (IOException e) {} */ //get filtered mask //if pixel is within acceptable blue-beacon-colour range, it's changed to white. //Otherwise, it's turned to black Mat mask = new Mat(); Core.inRange(cropped, BEACON_BLUE_LOW, BEACON_BLUE_HIGH, mask); Moments mmnts = Imgproc.moments(mask, true); //calculating centroid of the resulting binary mask via image moments Log.i("CentroidX", "" + ((mmnts.get_m10() / mmnts.get_m00()))); Log.i("CentroidY", "" + ((mmnts.get_m01() / mmnts.get_m00()))); //checking if blue either takes up the majority of the image (which means the beacon is all blue) //or if there's barely any blue in the image (which means the beacon is all red or off) // if (mmnts.get_m00() / mask.total() > 0.8) { // return VortexUtils.BEACON_ALL_BLUE; // } else if (mmnts.get_m00() / mask.total() < 0.1) { // return VortexUtils.BEACON_NO_BLUE; // }//elseif //Note: for some reason, we end up with a image that is rotated 90 degrees //if centroid is in the bottom half of the image, the blue beacon is on the left //if the centroid is in the top half, the blue beacon is on the right if ((mmnts.get_m01() / mmnts.get_m00()) < cropped.rows() / 2) { return VortexUtils.BEACON_RED_BLUE; } else { return VortexUtils.BEACON_BLUE_RED; } //else } //if return VortexUtils.NOT_VISIBLE; }