Track height of targets

Author: James Hagborg

src/main/java/org/usfirst/frc/team69/util/vision/ClosestPairTargetProcessor.java

@@ -7,39 +7,43 @@
 
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
+import org.opencv.core.Point3;
 import org.opencv.core.Rect;
 import org.opencv.core.Scalar;
 import org.opencv.imgproc.Imgproc;
+import org.usfirst.frc.team69.util.pid.DisplacementPIDSource;
 import org.usfirst.frc.team69.util.pref.IntPreference;
 
+import edu.wpi.first.wpilibj.PIDSource;
+
 /**
  * Target processor which finds the closest two targets to the crosshairs, and
  * averages their position. This is the algorithm that was used to track the
  * boiler in the 2017 steamworks game. It can more generally be applied to
  * anything that's two pieces of tape.
  * 
+ * This processor uses the height of the targets to measure distance. However,
+ * it computes x, y, and height all as simple averages, rather than weighting by
+ * depth. This is a good approximation as long as one target is not
+ * significantly closer than the other, meaning the camera is not viewing the
+ * target from a steep angle.
+ * 
  * @author James Hagborg
  *
  */
-public class ClosestPairTargetProcessor extends AbstractTargetProcessor<VisionResult> {
+public class ClosestPairTargetProcessor
+        extends AbstractTargetProcessor<TargetWithHeightResult> {
 
     private final IntSupplier m_xCrosshairs, m_yCrosshairs;
-    
-    /*
-     * The most recent point where a target was found. This is stored in
-     * absolute pixels, rather than relative to the crosshairs, like the result
-     * type stores. This is only used for drawing indicators.
-     */
-    private Point m_lastPoint;
 
     /**
      * Construct a new target processor with the given fixed crosshairs
      * position.
      * 
      * @param xCrosshairs
-     * 			X coordinate for the crosshairs
+     *            X coordinate for the crosshairs
      * @param yCrosshairs
-     * 			Y coordinate for the crosshairs
+     *            Y coordinate for the crosshairs
      */
     public ClosestPairTargetProcessor(int xCrosshairs, int yCrosshairs) {
         this(() -> xCrosshairs, () -> yCrosshairs);
@@ -54,11 +58,12 @@ public ClosestPairTargetProcessor(int xCrosshairs, int yCrosshairs) {
      * from any thread.
      * 
      * @param xCrosshairs
-     * 			X coordinate for the crosshairs
+     *            X coordinate for the crosshairs
      * @param yCrosshairs
-     * 			Y coordinate for the crosshairs
+     *            Y coordinate for the crosshairs
      */
-    public ClosestPairTargetProcessor(IntSupplier xCrosshairs, IntSupplier yCrosshairs) {
+    public ClosestPairTargetProcessor(IntSupplier xCrosshairs,
+            IntSupplier yCrosshairs) {
         m_xCrosshairs = Objects.requireNonNull(xCrosshairs);
         m_yCrosshairs = Objects.requireNonNull(yCrosshairs);
     }
@@ -71,7 +76,7 @@ public ClosestPairTargetProcessor(IntSupplier xCrosshairs, IntSupplier yCrosshai
      *            The target to check
      * @return The distance from the crosshairs
      */
-    private double targetDistance(Point result) {
+    private double targetDistance(Point3 result) {
         double xError = result.x - m_xCrosshairs.getAsInt();
         double yError = result.y - m_yCrosshairs.getAsInt();
         return xError * xError + yError * yError;
@@ -84,76 +89,98 @@ private double targetDistance(Point result) {
      *            The target rectangle.
      * @return The point at the center of the rectangle.
      */
-    private Point targetToPoint(Rect rect) {
+    private Point3 targetToPoint(Rect rect) {
         int xCenter = rect.x + rect.width / 2;
         int yCenter = rect.y + rect.height / 2;
-        return new Point(xCenter, yCenter);
+        int height = rect.height;
+        return new Point3(xCenter, yCenter, height);
     }
 
     /**
      * Convert a point to a result, by taking the position to be relative to the
      * crosshairs. Since this method is only called when the point given is
-     * actually the chosen target, this also saves the point for drawing a marker
-     * later.
+     * actually the chosen target, this also saves the point for drawing a
+     * marker later.
      * 
-     * @param point The point at the center of the target, in absolute pixels.
+     * @param point
+     *            The point at the center of the target, in absolute pixels.
      * @return The new VisionResult
      */
-    private VisionResult pointToResult(Point point) {
-        m_lastPoint = point;
-        return new VisionResult(
-                (int) point.x - m_xCrosshairs.getAsInt(),
-                (int) point.y - m_yCrosshairs.getAsInt(), true);
+    private TargetWithHeightResult pointToResult(Point3 point) {
+        return new TargetWithHeightResult(
+                point.x - m_xCrosshairs.getAsInt(),
+                point.y - m_yCrosshairs.getAsInt(),
+                point.x, point.y, point.z, true);
     }
-    
+
     /**
      * Given two Points, find the average of their positions.
      * 
      * @param a
      * @param b
      * @return
      */
-    private Point averagePoints(Point a, Point b) {
-        return new Point((a.x + b.x) / 2, (a.y + b.y) / 2);
+    private Point3 averagePoints(Point3 a, Point3 b) {
+        return new Point3((a.x + b.x) / 2, (a.y + b.y) / 2, (a.z + b.z) / 2);
     }
-    
+
     /**
      * {@inheritDoc}
      */
     @Override
-    public VisionResult computeResult(List<Rect> targets) {
-        Point[] result = targets.stream()
-                .map(this::targetToPoint)
+    public TargetWithHeightResult computeResult(List<Rect> targets) {
+        Point3[] result = targets.stream().map(this::targetToPoint)
                 .sorted(Comparator.comparingDouble(this::targetDistance))
-                .limit(2)
-                .toArray(Point[]::new);
-        if (result.length == 0) {
-            return getDefaultValue();
-        } else if (result.length == 1) {
+                .limit(2).toArray(Point3[]::new);
+        if (result.length < 2) {
             return getDefaultValue();
         } else {
             return pointToResult(averagePoints(result[0], result[1]));
         }
     }
+    
+    /**
+     * Get a PID source that returns the height of the target.
+     * 
+     * @return A PID source returning the height of the target.
+     */
+    public PIDSource heightPID() {
+        return new DisplacementPIDSource() {
+            @Override
+            public double pidGet() {
+                return getLastResult().height();
+            }
+        };
+    }
 
     private static final Scalar MARKER_COLOR = new Scalar(0, 0, 255);
-    
+    private static final double MARKER_WIDTH = 6;
+
     /**
      * {@inheritDoc}
      */
     @Override
     public void writeOutput(Mat mat) {
-        if (m_lastPoint != null && getLastResult().foundTarget()) {
-            Imgproc.drawMarker(mat, m_lastPoint, MARKER_COLOR);
+        TargetWithHeightResult result = getLastResult();
+        if (result.foundTarget()) {
+            Point center = new Point(result.xAbsolute(), result.yAbsolute());
+            double height = result.height();
+            Point tl = new Point(center.x - MARKER_WIDTH / 2, center.y - height / 2);
+            Point tr = new Point(center.x + MARKER_WIDTH / 2, center.y - height / 2);
+            Point bl = new Point(center.x - MARKER_WIDTH / 2, center.y + height / 2);
+            Point br = new Point(center.x + MARKER_WIDTH / 2, center.y + height / 2);
+            Imgproc.drawMarker(mat, center, MARKER_COLOR);
+            Imgproc.line(mat, tl, tr, MARKER_COLOR);
+            Imgproc.line(mat, bl, br, MARKER_COLOR);
         }
     }
 
     /**
      * {@inheritDoc}
      */
     @Override
-    public VisionResult getDefaultValue() {
-        return new VisionResult(0, 0, false);
+    public TargetWithHeightResult getDefaultValue() {
+        return new TargetWithHeightResult(0, 0, 0, 0, 0, false);
     }
 
 }

src/main/java/org/usfirst/frc/team69/util/vision/ClosestTargetProcessor.java

@@ -99,8 +99,9 @@ private Point targetToPoint(Rect rect) {
     private VisionResult pointToResult(Point point) {
         m_lastPoint = point;
         return new VisionResult(
-                (int) point.x - m_xCrosshairs.getAsInt(),
-                (int) point.y - m_yCrosshairs.getAsInt(), true);
+                point.x - m_xCrosshairs.getAsInt(),
+                point.y - m_yCrosshairs.getAsInt(), 
+                point.x, point.y, true);
     }
 
     /**
@@ -132,7 +133,7 @@ public void writeOutput(Mat mat) {
      */
     @Override
     public VisionResult getDefaultValue() {
-        return new VisionResult(0, 0, false);
+        return new VisionResult(0, 0, 0, 0, false);
     }
 
 }

src/main/java/org/usfirst/frc/team69/util/vision/CrosshairsPipeline.java

@@ -27,12 +27,17 @@ public class CrosshairsPipeline implements VisionGUIPipeline {
      * 
      * @param x
      *            Supplier for x coordinate.
-     * @param y Supplier for y coordinate.
-     * @param b Blue component of color.
-     * @param g Green component of color.
-     * @param r Red component of color.
+     * @param y
+     *            Supplier for y coordinate.
+     * @param b
+     *            Blue component of color.
+     * @param g
+     *            Green component of color.
+     * @param r
+     *            Red component of color.
      */
-    public CrosshairsPipeline(IntSupplier x, IntSupplier y, int b, int g, int r) {
+    public CrosshairsPipeline(IntSupplier x, IntSupplier y, int b, int g,
+            int r) {
         m_color = new Scalar(b, g, r);
         m_x = Objects.requireNonNull(x);
         m_y = Objects.requireNonNull(y);

src/main/java/org/usfirst/frc/team69/util/vision/HeightIndicatorPipeline.java

@@ -0,0 +1,79 @@
+package org.usfirst.frc.team69.util.vision;
+
+import java.util.Objects;
+import java.util.function.IntSupplier;
+import java.util.function.Supplier;
+
+import org.opencv.core.Mat;
+import org.opencv.core.Point;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+/**
+ * Pipeline that draws an indicator around a target, showing what the goal
+ * height should be. This is currently done by drawing two bars above and below
+ * the center of the target.
+ * 
+ * @author James Hagborg
+ *
+ */
+public class HeightIndicatorPipeline implements VisionGUIPipeline {
+
+    private final Supplier<? extends VisionResult> m_targetLocation;
+    private final IntSupplier m_height;
+    private final Scalar m_color;
+
+    /**
+     * Construct a new pipeline with the specified parameters.
+     * 
+     * @param targetLocation
+     *            Supplier of the target location, so this pipeline knows where
+     *            to draw the indicator. Usually this is the getLastResult
+     *            method of some pipeline which runs before this one.
+     * @param height
+     *            The goal height of the target. Usually this is a constant or
+     *            the get method of a preference.
+     * @param b Blue component of color
+     * @param g Green component of color
+     * @param r Red component of color
+     */
+    public HeightIndicatorPipeline(
+            Supplier<? extends VisionResult> targetLocation, IntSupplier height,
+            int b, int g, int r) {
+        m_targetLocation = Objects.requireNonNull(targetLocation);
+        m_height = Objects.requireNonNull(height);
+        m_color = new Scalar(b, g, r);
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public void process(Mat mat) {
+        // do nothing
+    }
+
+    private static final double MARKER_WIDTH = 4;
+
+    /**
+     * {@inheritDoc}
+     */
+    @Override
+    public void writeOutput(Mat mat) {
+        VisionResult res = m_targetLocation.get();
+
+        if (res.foundTarget()) {
+            int height = m_height.getAsInt();
+            double x = res.xAbsolute();
+            double y = res.yAbsolute();
+
+            Point tl = new Point(x - MARKER_WIDTH / 2.0, y - height / 2.0);
+            Point tr = new Point(x + MARKER_WIDTH / 2.0, y - height / 2.0);
+            Point bl = new Point(x - MARKER_WIDTH / 2.0, y + height / 2.0);
+            Point br = new Point(x + MARKER_WIDTH / 2.0, y + height / 2.0);
+            Imgproc.line(mat, tl, tr, m_color);
+            Imgproc.line(mat, bl, br, m_color);
+        }
+    }
+
+}

src/main/java/org/usfirst/frc/team69/util/vision/TargetWithHeightResult.java

@@ -0,0 +1,37 @@
+package org.usfirst.frc.team69.util.vision;
+
+/**
+ * Vision result which stores the height of the target found.
+ * 
+ * @author James Hagoborg
+ *
+ */
+public class TargetWithHeightResult extends VisionResult {
+
+    private final double m_height;
+
+    /**
+     * Construct a result with the given parameters
+     * 
+     * @param xError
+     *            error on x-coordinate
+     * @param yError
+     *            error on y-coordinate
+     * @param xAbs
+     *            absolute x-coordinate
+     * @param yAbs
+     *            absolute y-coordinate
+     * @param height
+     *            height of the target
+     * @param foundTarget
+     *            whether a target was actually found
+     */
+    public TargetWithHeightResult(double xError, double yError, double xAbs,
+            double yAbs, double height, boolean foundTarget) {
+        super(xError, yError, xAbs, yAbs, foundTarget);
+        m_height = height;
+    }
+
+    public double height() { return m_height; }
+
+}