LaptopRectangleTracker.java

package team2200.smartdashboard.extension.rectangletracker;

import edu.wpi.first.smartdashboard.camera.LaptopExtension;
import edu.wpi.first.smartdashboard.properties.BooleanProperty;
import edu.wpi.first.smartdashboard.properties.NumberProperty;
import edu.wpi.first.smartdashboard.types.DataType;
import edu.wpi.first.wpijavacv.WPIBinaryImage;
import edu.wpi.first.wpijavacv.WPIColor;
import edu.wpi.first.wpijavacv.WPIColorImage;
import edu.wpi.first.wpijavacv.WPIContour;
import edu.wpi.first.wpijavacv.WPIImage;
import edu.wpi.first.wpijavacv.WPIPoint;
import edu.wpi.first.wpijavacv.WPIPolygon;
import java.awt.Color;
import java.awt.Graphics;
import java.text.DecimalFormat;
import java.util.ArrayList;

public class LaptopRectangleTracker extends LaptopExtension {
    public static final String NAME = "Laptop Rectangle Tracker";
    public static final DataType[] TYPES = { DataType.NUMBER };
    public final NumberProperty threshold = new NumberProperty(this, "Threshold", 230);
    public final NumberProperty minArea   = new NumberProperty(this, "Minimum Area", 10000);
    public final NumberProperty dilations = new NumberProperty(this, "Dilations", 2);
    public final NumberProperty polygonAccuracy = new NumberProperty(this, "Polygon Accuracy", 4);
    public final BooleanProperty displayVertices = new BooleanProperty(this, "Display Vertices", true);
    public final BooleanProperty displayMatchCenter = new BooleanProperty(this, "Display Match Center", true);
    public final BooleanProperty displayImageCenter = new BooleanProperty(this, "Display Image Center", true);
    public final BooleanProperty displayMatchCoords = new BooleanProperty(this, "Display Match Coordinates", true);
    public final NumberProperty viewingAngle   = new NumberProperty(this, "Viewing Angle", 43.5);
    public final NumberProperty heightOfTarget = new NumberProperty(this, "Height of Target", 89);
    public final NumberProperty heightOfCamera = new NumberProperty(this, "Height of Camera", 36);

    WPIColor contourColour = new WPIColor(51, 153, 255);
    WPIColor centerColour  = new WPIColor(0, 0, 255);
    WPIColor verticeColour = new WPIColor(255, 0, 0);
    ArrayList<Match> matches = new ArrayList<Match>();

    long startTime = 0;
    long numFrames = 0;

    @Override
    public WPIImage processImage(WPIColorImage rawImage) {
        if (startTime == 0) startTime = System.currentTimeMillis();
        int t = threshold.getValue().intValue();
        WPIBinaryImage r = rawImage.getRedChannel().getThreshold(t);
        WPIBinaryImage g = rawImage.getGreenChannel().getThreshold(t);
        WPIBinaryImage b = rawImage.getBlueChannel().getThreshold(t);
        WPIBinaryImage binImage = r.getAnd(g).getAnd(b);
        r.dispose();
        g.dispose();
        b.dispose();

        binImage.dilate(dilations.getValue().intValue());

        WPIContour[] contours = binImage.findContours();

        matches.clear();
        for (WPIContour contour : contours) {
            if (contour.getHeight() * contour.getWidth() < minArea.getValue().intValue())
                continue;

            WPIPolygon temp = contour.approxPolygon(polygonAccuracy.getValue().intValue());
            if (temp.isConvex() && temp.getNumVertices() == 4)
                matches.add(new Match(temp, rawImage.getWidth(), rawImage.getHeight()));
        }

        for (int i = 0; i < matches.size(); i++) {
            for (int j = 0; j < matches.size(); j++) {
                if (matches.get(i).isSubMatchOf(matches.get(j))) {
                    matches.remove(i);
                    i--;
                    break;
                }
            }
        }

        for (Match match : matches) {
            rawImage.drawPolygon(match.polygon, contourColour, 1);

            if (displayMatchCenter.getValue().booleanValue())
                rawImage.drawPoint(new WPIPoint(match.cX, match.cY), centerColour, 1);

            if (displayVertices.getValue().booleanValue())
                for (WPIPoint vertice : match.points)
                    rawImage.drawPoint(vertice, verticeColour, 3);
        }

        if (displayImageCenter.getValue().booleanValue()) {
            int cX = rawImage.getWidth() / 2;
            int cY = rawImage.getHeight() / 2;
            rawImage.drawLine(new WPIPoint(cX, cY - 3), new WPIPoint(cX, cY + 3), centerColour, 1);
            rawImage.drawLine(new WPIPoint(cX - 3, cY), new WPIPoint(cX + 3, cY), centerColour, 1);
        }

        numFrames++;
        binImage.dispose();
        return rawImage;
    }

    @Override
    protected void paintComponent(Graphics g) {
        super.paintComponent(g);
        if (!displayMatchCoords.getValue().booleanValue()) return;
        
        DecimalFormat f = new DecimalFormat("#.##");

        g.setColor(Color.BLUE);
        for (Match match : matches) {
            String values = f.format(match.dX) + ", " + f.format(match.dY);
            g.drawString(values, match.cX + 4, match.cY + 12);

            String distance = f.format(match.distanceInInches(viewingAngle.getValue().doubleValue(), heightOfTarget.getValue().doubleValue(), heightOfCamera.getValue().doubleValue())) + "\"";
            g.drawString(distance, match.cX + 4, match.cY + 32);

            String angle = f.format(match.angleFromTarget(viewingAngle.getValue().doubleValue()));
            g.drawString(angle, match.cX + 4, match.cY + 52);

            int quadrant = 1;
            for (WPIPoint point : match.points) {
                String coordinates = Integer.toString(point.getX()) + ", " + Integer.toString(point.getY());
                int dX = (quadrant == 2 || quadrant == 3) ? 5 : -60;
                int dY = (quadrant == 1 || quadrant == 2) ? -5 : 5;
                g.drawString(coordinates, point.getX() + dX, point.getY() + dY);
                quadrant++;
            }
        }

        String affix = (matches.size() == 1) ? "" : "es";
        String temp  = matches.size() + " match" + affix + " found.";
        g.drawString(temp, 5, 40);

        long timeDifference = System.currentTimeMillis() - startTime;
        float secondsElapsed = timeDifference / 1000;
        float fps = Math.round(numFrames / secondsElapsed);
        g.drawString(Float.toString(fps) + " FPS", 5, 20);
    }
}

Match.java

package team2200.smartdashboard.extension.rectangletracker;

import edu.wpi.first.wpijavacv.WPIPoint;
import edu.wpi.first.wpijavacv.WPIPolygon;

public class Match {
    public WPIPolygon polygon;
    public WPIPoint[] points;
    public int cX, cY;
    public float dX, dY;
    private int imageWidthPixels, imageHeightPixels;

    private static final double targetWidth  = 24;
    private static final double targetHeight = 18;

    public Match(WPIPolygon p, int imageWidth, int imageHeight) {
        polygon = p;
        imageWidthPixels = imageWidth;
        imageHeightPixels = imageHeight;
        cX = polygon.getX() + polygon.getWidth() / 2;
        cY = polygon.getY() + polygon.getHeight() / 2;
        dX = pixelToRealWorld(cX, imageWidth);
        dY = pixelToRealWorld(cY, imageHeight);
        assignPoints();
    }

    public boolean isSubMatchOf(Match m) {
        return (m.points[0].getX() < points[0].getX() &&
                m.points[0].getY() < points[0].getY() &&
                m.points[2].getX() > points[2].getX() &&
                m.points[2].getY() > points[2].getY());
    }

    /**
     * Gets the distance between the center of a target and the camera lens.
     * 
     * @param viewingAngle Viewing angle in degrees of the camera.  43.5 for the
     * Axis M1011 and 47 for the Axis 206.
     * @param heightOfTarget The distance between the center of the target and
     * ground.
     * @param heightOfCamera The height of the camera from the ground in inches.
     * @return The distance in inches.
     */
    public double distanceInInches(double viewingAngle, double heightOfTarget, double heightOfCamera) {
        double imageWidth = (targetWidth * imageWidthPixels) / polygon.getWidth();
        double fovDistance = fovDistance(imageWidth, viewingAngle);

        double opposite = heightOfTarget - heightOfCamera;
        return Math.sqrt(Math.pow(opposite, 2) + Math.pow(fovDistance, 2));
    }

    /**
     * Gets the angle the camera is facing at relative to the target.  0 is
     * straight at the target.
     *
     * @param viewingAngle Viewing angle in degrees of the camera.  43.5 for the
     * Axis M1011 and 47 for the Axis 206.
     * @return Angle in the degrees.  -90 <= 0 <= 90
     */
    public double angleFromTarget(double viewingAngle) {
        double leftSide  = points[3].getY() - points[0].getY();
        double rightSide = points[2].getY() - points[1].getY();
        leftSide  = imageHeightPixels / leftSide * targetHeight;
        rightSide = imageHeightPixels / rightSide * targetHeight;

        double fovDistanceLeft  = fovDistance(leftSide, viewingAngle);
        double fovDistanceRight = fovDistance(rightSide, viewingAngle);
        double targetTheta = calculateCosineTheta(fovDistanceLeft, fovDistanceRight, targetWidth);
        double sideTheta   = calculateCosineTheta(24, fovDistanceRight, fovDistanceLeft);

        // Angle in a triangle will add up to 180, subtract from 90 instead of
        // 180 to offset.
        return (90 - (targetTheta / 2) - sideTheta);
    }

    private double fovDistance(double sideLength, double viewingAngle) {
        return (sideLength / 2) / Math.tan(Math.toRadians(viewingAngle) / 2);
    }

    private double calculateCosineTheta(double lengthA, double lengthB, double lengthC) {
        // cos C = (a^2 + b^2 - c^2) / (2ab)
        return Math.toDegrees(Math.acos((Math.pow(lengthA, 2) + Math.pow(lengthB, 2) - Math.pow(lengthC, 2)) / (2 * lengthA * lengthB)));
    }

    private float pixelToRealWorld(int coord, int resolution) {
        float center = resolution / 2;
        return (coord - center) / center;
    }

    private void assignPoints() {
        points = new WPIPoint[4];
        for (WPIPoint point : polygon.getPoints()) {
            points[getQuadrantIndex(point)] = point;
        }
    }

    private int getQuadrantIndex(WPIPoint point) {
        if (point.getX() < cX && point.getY() < cY) {
            return 0; // Top Left
        } else if (point.getX() >= cX && point.getY() < cY) {
            return 1; // Top Right
        } else if (point.getX() >= cX && point.getY() >= cY) {
            return 2; // Bottom Right
        } else {
            return 3; // Bottom Left
        }
    }
}