RectangleTracker.java

package team2200.smartdashboard.extension.rectangletracker;

import edu.wpi.first.smartdashboard.camera.WPICameraExtension;
import edu.wpi.first.smartdashboard.robot.Robot;
import edu.wpi.first.wpilibj.networking.NetworkTable;
import edu.wpi.first.wpijavacv.*;

public class RectangleTracker extends WPICameraExtension {
    public static final String NAME = "Rectangle Tracker";

    WPIColor contourColor = new WPIColor(51, 153, 255);
    WPIColor centerColor  = new WPIColor(0, 0, 255);

    @Override
    public WPIImage processImage(WPIGrayscaleImage rawImage) {
        // Create a colour image as a canvas.
        WPIColorImage canvas = new WPIColorImage(rawImage.getBufferedImage());

        // Threshold into binary image.
        WPIBinaryImage binImage = rawImage.getThreshold(Robot.getPreferences().getInt("Threshold"));
        
        // Process binary image.
        binImage.erode(2);

        // Find and process contours.
        WPIContour[] contours = binImage.findContours();
        for (WPIContour contour : contours) {
            canvas.drawContour(contour, contourColor, 2);
            
            int cX = contour.getX() + contour.getWidth() / 2;
            int cY = contour.getY() + contour.getHeight() / 2;
            WPIPoint p = new WPIPoint(cX, cY);
            canvas.drawPoint(p, centerColor, 3);
        }

        Robot.getTable().putInt("numContourMatches", contours.length);
        
        return canvas;
    }
}

TEHE