Java tutorial
package org.firstinspires.ftc.teamcode.vision; import android.os.Environment; import android.util.Log; import com.vuforia.HINT; import com.vuforia.Image; import com.vuforia.Vuforia; import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix; import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; import org.opencv.core.Core; import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; import org.opencv.core.Point; import org.opencv.core.Rect; import org.opencv.core.Scalar; import org.opencv.highgui.Highgui; import org.opencv.imgproc.Imgproc; import java.io.File; import java.util.ArrayList; import java.util.List; public class VisionLib { public static final double CAM_DISTANCE_ERROR_MULT = .9430088912; public static final int BLUE_LEFT = 0; public static final int BLUE_RIGHT = 1; public static final int TEST_FAILED = -1; public static final Scalar OPEN_CV_GREEN = new Scalar(255, 255, 255); public static final Scalar BLUE_LOWER_THRESH = new Scalar(0, 200, 243); public static final Scalar BLUE_UPPER_THRESH = new Scalar(36, 255, 255); public static final String TAG = "VL"; private LightningLocalizerImpl vuforia; private VuforiaTrackable wheels, tools, legos, gears; static { System.load("/data/app-lib/org.opencv.engine-1/libopencv_java.so");//load opencv } public void init() { VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters( com.qualcomm.ftcrobotcontroller.R.id.cameraMonitorViewId); parameters.vuforiaLicenseKey = "AfUtsLP/////AAAAGRiqbo6V4UQRoUCC4RCoUW9sftlkIpkiNHWrpK5ctlPKrMUPdgNIX930d2QwLQr8uKKgvbSxioci2VobN9tyecuLM6tFx1+N0nKqmiFerfNYtGcEsx0UvZtq0lQ8gt9lLsNp80E2vlFp9twYLYa/BK5BptP3o8AZoGR70yj/tOcQM9bP0o2Mhf5rdT9fwW2fXm8O1PeUVbv7dLD41uMsB2FX8fNEyTvgwwPT5EBXAXOEEy9ZARJKN0bvdCtFbazuo2VAOA7dg5XRt4FLi5xiHmp9/0y0zQ7Ur/PMEDOW4XQhsV3zZtEjBfImw4ilccLirqtI09ec97s+Rz7hzMBoxp5imndGE836E66CFln1DvJC"; parameters.cameraDirection = VuforiaLocalizer.CameraDirection.FRONT; parameters.cameraMonitorFeedback = VuforiaLocalizer.Parameters.CameraMonitorFeedback.AXES; vuforia = new LightningLocalizerImpl(parameters); vuforia.setFrameQueueCapacity(5); Vuforia.setHint(HINT.HINT_MAX_SIMULTANEOUS_IMAGE_TARGETS, 4); VuforiaTrackables vvDB = this.vuforia.loadTrackablesFromAsset("vv"); wheels = vvDB.get(0); wheels.setName("wheels"); tools = vvDB.get(1); tools.setName("tools"); legos = vvDB.get(2); legos.setName("legos"); gears = vvDB.get(3); gears.setName("gears"); List<VuforiaTrackable> allTrackables = new ArrayList<VuforiaTrackable>(); allTrackables.addAll(vvDB); vvDB.activate(); } public double[] getLegosDisplacement() { OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) legos.getListener()).getPose(); double theta = 0, z = -1;//use -1 because actual z can never be this if (pose != null) { VectorF translation = pose.getTranslation(); //this theta calculation should be much easier TODO -- FIX IT theta = Math.atan2(translation.get(0), translation.get(2)); if (theta > Math.PI / 72) theta -= Math.PI; else if (theta < Math.PI / 72) theta += Math.PI; else theta = 0; z = Math.abs(CAM_DISTANCE_ERROR_MULT * translation.get(2)); } double[] displacement = { theta, z }; return displacement; } public double getDistanceToTarget(VuforiaTrackable target) { OpenGLMatrix pose = ((VuforiaTrackableDefaultListener) target.getListener()).getPose(); if (pose != null) { VectorF translation = pose.getTranslation(); return Math.abs(CAM_DISTANCE_ERROR_MULT * translation.get(2)); } return -1; } public LightningLocalizerImpl getLocalizer() { return vuforia; } private Mat getCameraMat() { Image img = vuforia.getCurrentImage(); if (img != null) { //construct mat to store image data Mat matIn = new Mat(img.getHeight(), img.getWidth(), CvType.CV_8UC3); //convert pixel ByteBuffer to byte[] byte[] pixels = new byte[img.getPixels().remaining()]; img.getPixels().get(pixels); //put data matIn.put(0, 0, pixels); //rotate and convert Core.transpose(matIn, matIn); Core.flip(matIn, matIn, 1); Imgproc.cvtColor(matIn, matIn, Imgproc.COLOR_BGR2RGB); return matIn; } return null; } /* @return constant for blue right or left, null on failure */ public double getCenterVortexWidth() { Mat matIn = getCameraMat(); if (matIn != null) { Log.d(TAG, "mat null"); Imgproc.cvtColor(matIn, matIn, Imgproc.COLOR_RGB2HSV); Mat matMasked = new Mat(matIn.rows(), matIn.cols(), CvType.CV_8UC1); Scalar vortexLowerThresh = new Scalar(37, 46, 34); Scalar vortexUpperThresh = new Scalar(163, 255, 255); Core.inRange(matIn, BLUE_LOWER_THRESH, BLUE_UPPER_THRESH, matMasked); //find largest contour (the part of the beacon we are interested in ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>(); Mat hierarchy = new Mat(); Mat contourMat = matMasked.clone(); Imgproc.findContours(contourMat, contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); if (contours.size() > 1) { int largestContourIndex = 0; double lastContourArea = 0; for (int i = 0; i < contours.size(); i++) { double contourArea = Imgproc.contourArea(contours.get(i)); if (contourArea > lastContourArea) { largestContourIndex = i; lastContourArea = contourArea; } } //get bounding rect Rect boundingRect = Imgproc .boundingRect(new MatOfPoint(contours.get(largestContourIndex).toArray())); Core.rectangle(matIn, new Point(boundingRect.x, boundingRect.y), new Point(boundingRect.x + boundingRect.width, boundingRect.y + boundingRect.height), OPEN_CV_GREEN); saveMatToDisk(matIn);//debug only return boundingRect.width; } } return -1; } public int getBlueSide() { Mat matIn = getCameraMat(); if (matIn != null) { Imgproc.cvtColor(matIn, matIn, Imgproc.COLOR_RGB2HSV); Mat matMasked = new Mat(matIn.rows(), matIn.cols(), CvType.CV_8UC1); Core.inRange(matIn, BLUE_LOWER_THRESH, BLUE_UPPER_THRESH, matMasked); //find largest contour (the part of the beacon we are interested in ArrayList<MatOfPoint> contours = new ArrayList<MatOfPoint>(); Mat hierarchy = new Mat(); Mat contourMat = matMasked.clone(); Imgproc.findContours(contourMat, contours, hierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE); if (contours.size() > 1) { int largestContourIndex = 0; double lastContourArea = 0; for (int i = 0; i < contours.size(); i++) { double contourArea = Imgproc.contourArea(contours.get(i)); if (contourArea > lastContourArea) { largestContourIndex = i; lastContourArea = contourArea; } } //get bounding rect Rect boundingRect = Imgproc .boundingRect(new MatOfPoint(contours.get(largestContourIndex).toArray())); Core.rectangle(matIn, new Point(boundingRect.x, boundingRect.y), new Point(boundingRect.x + boundingRect.width, boundingRect.y + boundingRect.height), OPEN_CV_GREEN); // saveMatToDisk(matIn);//debug only //find which side its on if (boundingRect.x > matIn.cols() / 2) {//depends on which camera we use Log.d(TAG, "left"); return BLUE_LEFT; } else { Log.d(TAG, "right"); return BLUE_RIGHT; } } Log.d(TAG, "countors:" + contours.size()); return TEST_FAILED; } return TEST_FAILED; } private boolean saveMatToDisk(Mat matIn) { File path = Environment.getExternalStoragePublicDirectory(Environment.DIRECTORY_PICTURES); String filename = "test" + System.currentTimeMillis() + ".png"; File file = new File(path, filename); filename = file.toString(); return Highgui.imwrite(filename, matIn); } public VuforiaTrackable getWheelsTarget() { return wheels; } public VuforiaTrackable getToolsTarget() { return tools; } public VuforiaTrackable getLegosTarget() { return legos; } public VuforiaTrackable getGearsTarget() { return gears; } }