SkystonePipeline.java

package org.firstinspires.ftc.teamcode;

import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.OpenCvPipeline;

import java.util.ArrayList;
import java.util.Collections;

public class SkystoneSampler extends OpenCvPipeline {
    enum SkystonePosition {
        LEFT_STONE, CENTER_STONE, RIGHT_STONE
    }
    // These are the mats we need, I will be explaining them as we go
    private Mat matYCrCb = new Mat();

    private ArrayList<Mat> matCb = new ArrayList<>();
    private ArrayList<Mat> stones = new ArrayList<>();
    private ArrayList<Integer> order = new ArrayList();
    private ArrayList<Scalar> means = new ArrayList();
    private ArrayList<Integer> skystones = new ArrayList<>();

    private double firstSkystonePercent;
    private double secondSkystonePercent;
    private double thirdSkystonePercent;

    SkystonePosition position;

    ArrayList<Rect> blocks;

    public SkystoneSampler(int width, int height) {
        this(width, height, 25, 15, 15);
    }

    public SkystoneSampler(int width, int height, double percentSpacing, double stoneWidth, double stoneHeight){


        firstSkystonePercent  = percentSpacing / 100;
        secondSkystonePercent  = firstSkystonePercent * 2;
        thirdSkystonePercent  = firstSkystonePercent * 3;


        blocks.add(new Rect(new Point(firstSkystonePercent * width - (stoneWidth / 2), 0.50 * height - (stoneHeight / 2)),
                new Point(firstSkystonePercent * width + (stoneWidth / 2), 0.50 * height + (stoneHeight / 2))));
        blocks.add(new Rect(new Point(secondSkystonePercent * width - (stoneWidth / 2), 0.50 * height - (stoneHeight / 2)),
                new Point(secondSkystonePercent * width + (stoneWidth / 2), 0.50 * height + (stoneHeight / 2))));
        blocks.add(new Rect(new Point(thirdSkystonePercent * width - (stoneWidth / 2), 0.50 * height - (stoneHeight / 2)),
                new Point(thirdSkystonePercent * width + (stoneWidth / 2), 0.50 * height + (stoneHeight / 2))));
        position = null;


    }
    //These will be the points for our rectangle


    /**
     * This will create the rectangles
     * @param frame the input mat
     * @param rect the rectangle
     * @param color the color of the rectangle when it is displayed on screen
     * @param thickness the thickness of the rectangle
     */
    public Mat drawRectangle(Mat frame,Rect rect,Scalar color,int thickness){
        Imgproc.rectangle(frame, rect, color, thickness);
        //submat simply put is cropping the mat
        return frame.submat(rect);

    }

    @Override
    public Mat processFrame(Mat input) {
        /**
         *input which is in RGB is the frame the camera gives
         *We convert the input frame to the color space matYCrCb
         *Then we store this converted color space in the mat matYCrCb
         *For all the color spaces go to
         *https://docs.opencv.org/3.4/d8/d01/group__imgproc__color__conversions.html
         */
        Imgproc.cvtColor(input, matYCrCb, Imgproc.COLOR_RGB2YCrCb);
        
        for(Rect stone: blocks) {
            stones.add(drawRectangle(matYCrCb, stone, new Scalar (255, 0, 255), 2));
            matCb.add(new Mat());
        }

        /**
         *This will extract the value of the CB channel in both rectangles
         *0 is the Y channel, 1 is the Cr, 2 is Cb
         */

        for(int i = 0; i < stones.size(); i++) {
            Core.extractChannel(stones.get(i), matCb.get(i), 2);
            means.add(Core.mean(matCb.get(i)));
        }

        Scalar max = means.get(0);
        int biggestIndex = 0;

        for (Scalar k : means) {
            if (k.val[0] > max.val[0]) {
                max = k;
                biggestIndex = means.indexOf(k);
            }
        }

        switch (biggestIndex) {
            case 0:
                position = SkystonePosition.LEFT_STONE;
                Imgproc.rectangle(input, blocks.get(0), new Scalar(0, 255, 0));
                Imgproc.rectangle(input, blocks.get(1), new Scalar(255, 0, 0));
                Imgproc.rectangle(input, blocks.get(2), new Scalar(255, 0, 0));

                break;
            case 1:
                position = SkystonePosition.CENTER_STONE;
                Imgproc.rectangle(input, blocks.get(0), new Scalar(255, 0, 0));
                Imgproc.rectangle(input, blocks.get(1), new Scalar(0, 255, 0));
                Imgproc.rectangle(input, blocks.get(2), new Scalar(255, 0, 0));
                break;
            case 2:
                Imgproc.rectangle(input, blocks.get(0), new Scalar(255, 0, 0));
                Imgproc.rectangle(input, blocks.get(1), new Scalar(255, 0, 0));
                Imgproc.rectangle(input, blocks.get(2), new Scalar(0, 255, 0));
                position = SkystonePosition.RIGHT_STONE;
                break;
            default:
                position = SkystonePosition.RIGHT_STONE;
                Imgproc.rectangle(input, blocks.get(0), new Scalar(255, 0, 0));
                Imgproc.rectangle(input, blocks.get(1), new Scalar(255, 0, 0));
                Imgproc.rectangle(input, blocks.get(2), new Scalar(255, 0, 0));
                // Default go for right stone;
                break;

        }



        order.clear();
        stones.clear();
        matCb.clear();
        means.clear();
        skystones.clear();

        for(Mat mat: stones) {
            mat.release();
        }
        return matYCrCb;
    }


    public SkystonePosition getSkystonePosition() {
        return position;
    }
}