971CV/src/org/frc971/Recognizer2013.java

package org.frc971;

import java.util.ArrayList;
import java.util.logging.Logger;

import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_core.CvSize;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.googlecode.javacv.cpp.opencv_imgproc;
import com.googlecode.javacv.cpp.opencv_imgproc.IplConvKernel;

import edu.wpi.first.wpijavacv.DaisyExtensions;
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.WPIPoint;
import edu.wpi.first.wpijavacv.WPIPolygon;

/**
 * Vision target recognizer for FRC 2013.
 *
 * @author jrussell
 * @author jerry
 */
public class Recognizer2013 implements Recognizer {
	
	private final static Logger LOG = Logger.getLogger(Logger.GLOBAL_LOGGER_NAME);

    // --- Tunable recognizer constants.
    static final double kRoughlyHorizontalSlope = Math.tan(Math.toRadians(30));
    static final double kRoughlyVerticalSlope = Math.tan(Math.toRadians(90 - 30));
    static final int kHoleClosingIterations = 2;
    static final double kPolygonPercentFit = 12;
    static final int kMinWidthAt320 = 35; // for high goal and middle goals

    // --- Field dimensions.
    // The target aspect ratios are for the midlines of the vision target tape.
    static final double kGoalWidthIn = 54; // of the high and middle targets
    static final double kTargetWidthIn = kGoalWidthIn + 4;
    static final double kHighGoalAspect = (21 + 4) / kTargetWidthIn;
    static final double kMiddleGoalAspect = (24 + 4) / kTargetWidthIn;
    static final double kMinAspect = kHighGoalAspect * 0.6;
    static final double kMaxAspect = kMiddleGoalAspect * 1.4;
    static final double kTopTargetHeightIn = 104.125 + 21.0/2; // center of target

    // --- Robot and camera dimensions.
    static final double kShooterOffsetDeg = 0; // azimuth offset from camera to shooter
    static final double kHorizontalFOVDeg = 44.0; // Logitech C210 camera
    static final double kVerticalFOVDeg = 480.0 / 640.0 * kHorizontalFOVDeg;
    static final double kCameraHeightIn = 24.0; // TODO
    static final double kCameraPitchDeg = 21.0; // TODO
    static final double kTanHFOV2 = Math.tan(Math.toRadians(kHorizontalFOVDeg / 2));
    static final double kTanVFOV2 = Math.tan(Math.toRadians(kVerticalFOVDeg / 2));

    // --- Colors for drawing indicators on the image.
    private static final WPIColor reject1Color = WPIColor.GRAY;
    private static final WPIColor reject2Color = WPIColor.YELLOW;
    private static final WPIColor candidateColor = WPIColor.BLUE;
    private static final WPIColor targetColor = WPIColor.RED;

    // --- Color thresholds, initialized in the constructor.
    private int min1Hue, max1Hue, min1Sat, min1Val;

    // Show intermediate images for parameter tuning.
    private final DebugCanvas thresholdedCanvas = new DebugCanvas("thresholded");
    private final DebugCanvas morphedCanvas = new DebugCanvas("morphed");

    // Data to reuse for each frame.
    private final DaisyExtensions daisyExtensions = new DaisyExtensions();
    private final IplConvKernel morphKernel = IplConvKernel.create(3, 3, 1, 1,
            opencv_imgproc.CV_SHAPE_RECT, null);
    private final ArrayList<WPIPolygon> polygons = new ArrayList<WPIPolygon>();

    // Frame-size-dependent data to reuse for each frame.
    private CvSize size = null;
    private WPIColorImage rawImage;
    private IplImage bin;
    private IplImage hsv;
    private IplImage hue;
    private IplImage sat;
    private IplImage val;
    private int minWidth;
    private WPIPoint linePt1, linePt2; // crosshair endpoints

    public Recognizer2013() {
        setHSVRange(70, 106, 137, 27);
    }

    @Override
    public void setHSVRange(int minHue, int maxHue, int minSat, int minVal) {
        min1Hue = minHue - 1; // - 1 because cvThreshold() does > instead of >=
        max1Hue = maxHue + 1;
        min1Sat = minSat - 1;
        min1Val = minVal - 1;
    }
    @Override
    public int getHueMin() { return min1Hue + 1; }
    @Override
    public int getHueMax() { return max1Hue - 1; }
    @Override
    public int getSatMin() { return min1Sat + 1; }
    @Override
    public int getValMin() { return min1Val + 1; }

    @Override
    public void showIntermediateStages(boolean enable) {
        thresholdedCanvas.show = enable;
        morphedCanvas.show = enable;
    }

    @Override
    public Target processImage(WPIColorImage cameraImage) {
        // (Re)allocate the intermediate images if the input is a different
        // size than the previous image.
        if (size == null || size.width() != cameraImage.getWidth()
                || size.height() != cameraImage.getHeight()) {
            size = opencv_core.cvSize(cameraImage.getWidth(),
                    cameraImage.getHeight());
            rawImage = DaisyExtensions.makeWPIColorImage(
                    DaisyExtensions.getIplImage(cameraImage));
            bin = IplImage.create(size, 8, 1);
            hsv = IplImage.create(size, 8, 3);
            hue = IplImage.create(size, 8, 1);
            sat = IplImage.create(size, 8, 1);
            val = IplImage.create(size, 8, 1);
            minWidth = (kMinWidthAt320 * cameraImage.getWidth() + 319) / 320;

            int horizontalOffsetPixels = (int)Math.round(
                    kShooterOffsetDeg * size.width() / kHorizontalFOVDeg);
            int x = size.width() / 2 + horizontalOffsetPixels;
            linePt1 = new WPIPoint(x, size.height() - 1);
            linePt2 = new WPIPoint(x, 0);
        } else {
            // Copy the camera image so it's safe to draw on.
            opencv_core.cvCopy(DaisyExtensions.getIplImage(cameraImage),
                    DaisyExtensions.getIplImage(rawImage));
        }

        IplImage input = DaisyExtensions.getIplImage(rawImage);

        // Threshold the pixels in HSV color space.
        // TODO(jerry): Do this in one pass of a pixel-processing loop.
        opencv_imgproc.cvCvtColor(input, hsv, opencv_imgproc.CV_BGR2HSV_FULL);
        opencv_core.cvSplit(hsv, hue, sat, val, null);

        // NOTE: Since red is at the end of the cyclic color space, you can OR
        // a threshold and an inverted threshold to match red pixels.
        opencv_imgproc.cvThreshold(hue, bin, min1Hue, 255, opencv_imgproc.CV_THRESH_BINARY);
        opencv_imgproc.cvThreshold(hue, hue, max1Hue, 255, opencv_imgproc.CV_THRESH_BINARY_INV);
        opencv_imgproc.cvThreshold(sat, sat, min1Sat, 255, opencv_imgproc.CV_THRESH_BINARY);
        opencv_imgproc.cvThreshold(val, val, min1Val, 255, opencv_imgproc.CV_THRESH_BINARY);

        // Combine the results to obtain a binary image which is mostly the
        // interesting pixels.
        opencv_core.cvAnd(hue, bin, bin, null);
        opencv_core.cvAnd(bin, sat, bin, null);
        opencv_core.cvAnd(bin, val, bin, null);

        thresholdedCanvas.showImage(bin);

        // Fill in gaps using binary morphology.
        opencv_imgproc.cvMorphologyEx(bin, bin, null, morphKernel,
                opencv_imgproc.CV_MOP_CLOSE, kHoleClosingIterations);

        morphedCanvas.showImage(bin);

        // Find contours.
        //
        // NOTE: If we distinguished between the inner and outer boundaries of
        // the vision target rectangles, we could apply a more accurate width
        // filter and more accurately compute the target range.
        WPIBinaryImage binWpi = DaisyExtensions.makeWPIBinaryImage(bin);
        WPIContour[] contours = daisyExtensions.findConvexContours(binWpi);

        // Simplify the contours to polygons and filter by size and aspect ratio.
        //
        // TODO(jerry): Also look for the two vertical stripe vision targets.
        // They'll greatly increase the precision of measuring the distance. If
        // both stripes are visible, they'll increase the accuracy for
        // identifying the high goal.
        polygons.clear();
        for (WPIContour c : contours) {
            if (c.getWidth() >= minWidth) {
                double ratio = ((double) c.getHeight()) / c.getWidth();
                if (ratio >= kMinAspect && ratio <= kMaxAspect) {
                    polygons.add(c.approxPolygon(kPolygonPercentFit));
                    //        	    System.out.println("  Accepted aspect ratio " + ratio);
                } else {
                    //        	    System.out.println("  Rejected aspect ratio " + ratio);
                }
            }
        }

        // Pick the target with the highest center-point that matches yet more
        // filter criteria.
        WPIPolygon bestTarget = null;
        int highestY = Integer.MAX_VALUE;

        for (WPIPolygon p : polygons) {
            // TODO(jerry): Replace boolean filters with a scoring function?
            if (p.isConvex() && p.getNumVertices() == 4) { // quadrilateral
                WPIPoint[] points = p.getPoints();
                // Filter for polygons with 2 ~horizontal and 2 ~vertical sides.
                int numRoughlyHorizontal = 0;
                int numRoughlyVertical = 0;
                for (int i = 0; i < 4; ++i) {
                    double dy = points[i].getY() - points[(i + 1) % 4].getY();
                    double dx = points[i].getX() - points[(i + 1) % 4].getX();
                    double slope = Double.MAX_VALUE;
                    if (dx != 0) {
                        slope = Math.abs(dy / dx);
                    }

                    if (slope < kRoughlyHorizontalSlope) {
                        ++numRoughlyHorizontal;
                    } else if (slope > kRoughlyVerticalSlope) {
                        ++numRoughlyVertical;
                    }
                }

                if (numRoughlyHorizontal >= 2 && numRoughlyVertical == 2) {
                    int pCenterX = p.getX() + p.getWidth() / 2;
                    int pCenterY = p.getY() + p.getHeight() / 2;

                    rawImage.drawPolygon(p, candidateColor, 2);
                    rawImage.drawPoint(new WPIPoint(pCenterX, pCenterY),
                            targetColor, 2);
                    if (pCenterY < highestY) {
                        bestTarget = p;
                        highestY = pCenterY;
                    }
                } else {
                    rawImage.drawPolygon(p, reject2Color, 1);
                }
            } else {
                rawImage.drawPolygon(p, reject1Color, 1);
            }
        }

        Target found = null;
        if (bestTarget != null) {
            rawImage.drawPolygon(bestTarget, targetColor, 2);
            found = measureTarget(bestTarget);
        } else {
            LOG.fine("No target found");
        }

        // Draw a crosshair
        rawImage.drawLine(linePt1, linePt2, targetColor, 1);
        
        if (found == null) {
        	found = new Target();
        }
        found.editedPicture = rawImage;

        daisyExtensions.releaseMemory();
        //System.gc();
        
        return found;
    }

    /**
     * Uses the camera, field, and robot dimensions to compute targeting info.
     */
    private Target measureTarget(WPIPolygon target) {
        double w = target.getWidth();
        double h = target.getHeight();
        double x = target.getX() + w / 2; // target center in view coords
        double y = target.getY() + h / 2;

        double vw = size.width();
        double vh = size.height();
        double xc = x - vw / 2; // target center pos'n ±from view center
        double yc = vh / 2 - y; // ... in world coords on the viewing plane

        // Target angles relative to the camera.
        double azimuthCam = Math.atan2(xc * 2 * kTanHFOV2, vw);
        double elevationCam = Math.atan2(yc * 2 * kTanVFOV2, vh);
        double rangeIn = kTargetWidthIn * vw / (w * 2 * kTanHFOV2);

        //Put results in target
        Target data = new Target();
        data.azimuth = (Math.toDegrees(azimuthCam) - kShooterOffsetDeg);
        data.elevation = (Math.toDegrees(elevationCam));
        data.range = (rangeIn / 12);
        
        LOG.fine("Best target at (" + x + ", " + y + ") " + w +" x " + h
                + ", shot azimuth=" + (Math.toDegrees(azimuthCam) - kShooterOffsetDeg) + 
                " elevation=" + (Math.toDegrees(elevationCam) + kCameraPitchDeg) + 
                " range=" + (rangeIn / 12));
        
        return data;
    }

}