diff --git a/chameleon-server/build.gradle b/chameleon-server/build.gradle index 87c16aec4..26e4ee0e3 100644 --- a/chameleon-server/build.gradle +++ b/chameleon-server/build.gradle @@ -126,4 +126,4 @@ spotless { indentWithTabs(2) indentWithSpaces(4) } -} \ No newline at end of file +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java index fe90e5dea..51ec0aacf 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipeline.java @@ -159,14 +159,14 @@ public class StandardCVPipeline extends CVPipeline hsvResult = hsvPipe.run(erodeDilateResult.getLeft()); totalPipelineTimeNanos += hsvResult.getRight(); - Pair, Long> findContoursResult = findContoursPipe.run(hsvResult.getLeft()); + Pair, Long> findContoursResult = findContoursPipe.run(hsvResult.getLeft()); totalPipelineTimeNanos += findContoursResult.getRight(); Pair, Long> filterContoursResult = filterContoursPipe.run(findContoursResult.getLeft()); totalPipelineTimeNanos += filterContoursResult.getRight(); // ignore ! - Pair, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft()); + Pair, Long> speckleRejectResult = speckleRejectPipe.run(filterContoursResult.getLeft()); totalPipelineTimeNanos += speckleRejectResult.getRight(); Pair, Long> groupContoursResult = groupContoursPipe.run(speckleRejectResult.getLeft()); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java index 16d4face7..374a82976 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java @@ -1,6 +1,7 @@ package com.chameleonvision._2.vision.pipeline.pipes; import com.chameleonvision._2.vision.pipeline.Pipe; +import com.chameleonvision.common.vision.opencv.Contour; import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; @@ -8,15 +9,16 @@ import org.opencv.imgproc.Imgproc; import java.util.ArrayList; import java.util.List; +import java.util.stream.Collectors; -public class FindContoursPipe implements Pipe> { +public class FindContoursPipe implements Pipe> { private List foundContours = new ArrayList<>(); public FindContoursPipe() {} @Override - public Pair, Long> run(Mat input) { + public Pair, Long> run(Mat input) { long processStartNanos = System.nanoTime(); foundContours.clear(); @@ -24,6 +26,6 @@ public class FindContoursPipe implements Pipe> { Imgproc.findContours(input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); long processTime = System.nanoTime() - processStartNanos; - return Pair.of(foundContours, processTime); + return Pair.of(foundContours.stream().map(Contour::new).collect(Collectors.toList()), processTime); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/camera/CaptureStaticProperties.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/camera/CaptureStaticProperties.java new file mode 100644 index 000000000..f2dee217e --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/camera/CaptureStaticProperties.java @@ -0,0 +1,49 @@ +package com.chameleonvision.common.vision.camera; + +import edu.wpi.cscore.VideoMode; +import org.apache.commons.math3.fraction.Fraction; +import org.apache.commons.math3.util.FastMath; +import org.opencv.core.Point; + +public class CaptureStaticProperties { + public final int imageWidth; + public final int imageHeight; + public final double fov; + public final double imageArea; + public final double centerX; + public final double centerY; + public final Point centerPoint; + public final double horizontalFocalLength; + public final double verticalFocalLength; + public final VideoMode mode; + + public CaptureStaticProperties(VideoMode mode, double fov) { + this.mode = mode; + + this.imageWidth = mode.width; + this.imageHeight = mode.height; + this.fov = fov; + + imageArea = imageHeight * imageWidth; + centerX = imageWidth / 2.0 - 0.5; + centerY = imageHeight / 2.0 - 0.5; + centerPoint = new Point(centerX, centerY); + + // Calculations from pinhole-model. + double diagonalView = FastMath.toRadians(this.fov); + Fraction aspectRatio = new Fraction(imageWidth, imageHeight); + + int horizontalRatio = aspectRatio.getNumerator(); + int verticalRatio = aspectRatio.getDenominator(); + + double diagonalAspect = FastMath.hypot(horizontalRatio, verticalRatio); + + double horizontalView = + FastMath.atan(FastMath.tan(diagonalView / 2) * (horizontalRatio / diagonalAspect)) * 2; + double verticalView = + FastMath.atan(FastMath.tan(diagonalView / 2) * (verticalRatio / diagonalAspect)) * 2; + + horizontalFocalLength = imageWidth / (2 * FastMath.tan(horizontalView / 2)); + verticalFocalLength = imageHeight / (2 * FastMath.tan(verticalView / 2)); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java index dfbc34e8a..e977114d7 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java @@ -1,8 +1,9 @@ package com.chameleonvision.common.vision.opencv; import com.chameleonvision.common.util.math.MathUtils; -import com.chameleonvision.common.vision.target.PotentialTarget; +import java.util.ArrayList; import java.util.Comparator; +import java.util.List; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import org.opencv.imgproc.Moments; @@ -58,11 +59,14 @@ public class Contour { return getMinAreaRect().center; } - public boolean isIntersecting( - Contour secondContour, PotentialTarget.TargetContourIntersection intersection) { + public boolean isEmpty() { + return mat.cols() != 0 && mat.rows() != 0; + } + + public boolean isIntersecting(Contour secondContour, ContourIntersection intersection) { boolean isIntersecting = false; - if (intersection == PotentialTarget.TargetContourIntersection.None) { + if (intersection == ContourIntersection.None) { isIntersecting = true; } else { try { @@ -105,4 +109,50 @@ public class Contour { return isIntersecting; } + + // TODO: refactor to do "infinite" contours + public static Contour groupContoursByIntersection( + Contour firstContour, Contour secondContour, ContourIntersection intersection) { + if (firstContour.isIntersecting(secondContour, intersection)) { + return combineContours(firstContour, secondContour); + } else { + return null; + } + } + + // TODO: does this leak? + private static Contour combineContours(Contour... contours) { + List fullContourPoints = new ArrayList<>(); + + for (var contour : contours) { + fullContourPoints.addAll(contour.mat.toList()); + } + + var points = new MatOfPoint(fullContourPoints.toArray(new Point[0])); + var finalContour = new Contour(points); + + if (!finalContour.isEmpty()) { + return finalContour; + } else return null; + } + + // TODO: move these? also docs plox + public enum ContourIntersection { + None, + Up, + Down, + Left, + Right + } + + public enum ContourGrouping { + Single(1), + Dual(2); + + public final int count; + + ContourGrouping(int count) { + this.count = count; + } + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java index f9a01ecde..72f391bc3 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java @@ -22,14 +22,14 @@ public abstract class CVPipe implements Function> { /** * Runs the process for the pipe. * - * @param in Input for pipe processing - * @return Result of processing + * @param in Input for pipe processing. + * @return Result of processing. */ protected abstract O process(I in); /** - * @param in Input for pipe processing - * @return Result of processing + * @param in Input for pipe processing. + * @return Result of processing. */ @Override public PipeResult apply(I in) { diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/BlurPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/BlurPipe.java new file mode 100644 index 000000000..e48008860 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/BlurPipe.java @@ -0,0 +1,47 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.pipeline.CVPipe; +import org.opencv.core.Mat; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +/** Represents a pipeline that blurs the image. */ +public class BlurPipe extends CVPipe { + /** + * Processes thos pipe. + * + * @param in Input for pipe processing. + * @return The processed frame. + */ + @Override + protected Mat process(Mat in) { + Imgproc.blur(in, in, params.getBlurSize()); + return in; + } + + public static class BlurParams { + // Default BlurImagePrams with zero blur. + public static BlurParams DEFAULT = new BlurParams(0); + + // Member to store the blur size. + private int m_blurSize; + + /** + * Constructs a new BlurImageParams. + * + * @param blurSize The blur size. + */ + public BlurParams(int blurSize) { + m_blurSize = blurSize; + } + + /** + * Returns the blur size. + * + * @return The blur size. + */ + public Size getBlurSize() { + return new Size(m_blurSize, m_blurSize); + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Collect2dTargetsPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Collect2dTargetsPipe.java new file mode 100644 index 000000000..c47c30d29 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Collect2dTargetsPipe.java @@ -0,0 +1,99 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.vision.camera.CaptureStaticProperties; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.target.PotentialTarget; +import com.chameleonvision.common.vision.target.TrackedTarget; +import java.util.ArrayList; +import java.util.List; +import org.opencv.core.Point; + +/** Represents a pipe that collects available 2d targets. */ +public class Collect2dTargetsPipe + extends CVPipe< + List, List, Collect2dTargetsPipe.Collect2dTargetsParams> { + + /** + * Processes this pipeline. + * + * @param in Input for pipe processing. + * @return A list of tracked targets. + */ + @Override + protected List process(List in) { + List targets = new ArrayList<>(); + + var calculationParams = + new TrackedTarget.TargetCalculationParameters( + params.getOrientation() == TrackedTarget.TargetOrientation.Landscape, + params.getOffsetPointRegion(), + params.getUserOffsetPoint(), + params.getCaptureStaticProperties().centerPoint, + new DoubleCouple(params.getCalibrationB(), params.getCalibrationM()), + params.getOffsetMode(), + params.getCaptureStaticProperties().horizontalFocalLength, + params.getCaptureStaticProperties().verticalFocalLength, + params.getCaptureStaticProperties().imageArea); + + for (PotentialTarget target : in) { + targets.add(new TrackedTarget(target, calculationParams)); + } + + return targets; + } + + public static class Collect2dTargetsParams { + private CaptureStaticProperties m_captureStaticProperties; + private TrackedTarget.RobotOffsetPointMode m_offsetMode; + private double m_calibrationM, m_calibrationB; + private Point m_userOffsetPoint; + private TrackedTarget.TargetOffsetPointRegion m_region; + private TrackedTarget.TargetOrientation m_orientation; + + public Collect2dTargetsParams( + CaptureStaticProperties captureStaticProperties, + TrackedTarget.RobotOffsetPointMode offsetMode, + double calibrationM, + double calibrationB, + Point calibrationPoint, + TrackedTarget.TargetOffsetPointRegion region, + TrackedTarget.TargetOrientation orientation) { + m_captureStaticProperties = captureStaticProperties; + m_offsetMode = offsetMode; + m_calibrationM = calibrationM; + m_calibrationB = calibrationB; + m_userOffsetPoint = calibrationPoint; + m_region = region; + m_orientation = orientation; + } + + public CaptureStaticProperties getCaptureStaticProperties() { + return m_captureStaticProperties; + } + + public TrackedTarget.RobotOffsetPointMode getOffsetMode() { + return m_offsetMode; + } + + public double getCalibrationM() { + return m_calibrationM; + } + + public double getCalibrationB() { + return m_calibrationB; + } + + public Point getUserOffsetPoint() { + return m_userOffsetPoint; + } + + public TrackedTarget.TargetOffsetPointRegion getOffsetPointRegion() { + return m_region; + } + + public TrackedTarget.TargetOrientation getOrientation() { + return m_orientation; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dContoursPipe.java new file mode 100644 index 000000000..5611d1c15 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dContoursPipe.java @@ -0,0 +1,89 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.util.ColorHelper; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.target.TrackedTarget; +import java.awt.Color; +import java.util.ArrayList; +import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Point; +import org.opencv.core.Rect; +import org.opencv.core.RotatedRect; +import org.opencv.imgproc.Imgproc; + +public class Draw2dContoursPipe + extends CVPipe>, Mat, Draw2dContoursPipe.Draw2dContoursParams> { + + private List m_drawnContours = new ArrayList<>(); + + @Override + protected Mat process(Pair> in) { + if (params.showCentroid || params.showMaximumBox || params.showRotatedBox) { + for (int i = 0; i < in.getRight().size(); i++) { + Point[] vertices = new Point[4]; + MatOfPoint contour = new MatOfPoint(); + + if (i != 0 && !params.showMultiple) { + break; + } + + TrackedTarget target = in.getRight().get(i); + RotatedRect r = target.getMinAreaRect(); + + if (r == null) continue; + + m_drawnContours.forEach(Mat::release); + m_drawnContours.clear(); + m_drawnContours = new ArrayList<>(); + + r.points(vertices); + contour.fromArray(vertices); + m_drawnContours.add(contour); + + if (params.showRotatedBox) { + Imgproc.drawContours( + in.getLeft(), + m_drawnContours, + 0, + ColorHelper.colorToScalar(params.rotatedBoxColor), + params.boxOutlineSize); + } + + if (params.showMaximumBox) { + Rect box = Imgproc.boundingRect(contour); + Imgproc.rectangle( + in.getLeft(), + new Point(box.x, box.y), + new Point(box.x + box.width, box.y + box.height), + ColorHelper.colorToScalar(params.maximumBoxColor), + params.boxOutlineSize); + } + + if (params.showCentroid) { + Imgproc.circle( + in.getLeft(), + target.getTargetOffsetPoint(), + 3, + ColorHelper.colorToScalar(params.centroidColor), + 2); + } + } + } + + return in.getLeft(); + } + + public static class Draw2dContoursParams { + public boolean showCentroid = false; + public boolean showMultiple = false; + public int boxOutlineSize = 0; + public boolean showRotatedBox = false; + public boolean showMaximumBox = false; + public Color centroidColor = Color.GREEN; + public Color rotatedBoxColor = Color.BLUE; + public Color maximumBoxColor = Color.RED; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dCrosshairPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dCrosshairPipe.java new file mode 100644 index 000000000..19a3bf491 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dCrosshairPipe.java @@ -0,0 +1,56 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.util.ColorHelper; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.target.TrackedTarget; +import java.awt.Color; +import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.imgproc.Imgproc; + +public class Draw2dCrosshairPipe + extends CVPipe>, Mat, Draw2dCrosshairPipe.Draw2dCrosshairParams> { + + @Override + protected Mat process(Pair> in) { + Mat image = in.getLeft(); + + double x, y; + double scale = image.cols() / 32.0; + + if (params.showCrosshair) { + x = image.cols() / 2.0; + y = image.rows() / 2.0; + + switch (params.calibrationMode) { + case Single: + if (params.calibrationPoint.equals(new Point())) { + params.calibrationPoint.set(new double[] {x, y}); + } + x = (int) params.calibrationPoint.x; + y = (int) params.calibrationPoint.y; + break; + case Dual: + // TODO + break; + } + Point xMax = new Point(x + scale, y); + Point xMin = new Point(x - scale, y); + Point yMax = new Point(x, y + scale); + Point yMin = new Point(x, y - scale); + + Imgproc.line(in.getLeft(), xMax, xMin, ColorHelper.colorToScalar(params.crosshairColor)); + Imgproc.line(in.getLeft(), yMax, yMin, ColorHelper.colorToScalar(params.crosshairColor)); + } + return in.getLeft(); + } + + public static class Draw2dCrosshairParams { + public TrackedTarget.RobotOffsetPointMode calibrationMode; + public Point calibrationPoint; + public boolean showCrosshair = true; + public Color crosshairColor = Color.GREEN; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ErodeDilatePipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ErodeDilatePipe.java new file mode 100644 index 000000000..f507703d4 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ErodeDilatePipe.java @@ -0,0 +1,44 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.pipeline.CVPipe; +import org.opencv.core.Mat; +import org.opencv.core.Size; +import org.opencv.imgproc.Imgproc; + +public class ErodeDilatePipe extends CVPipe { + @Override + protected Mat process(Mat in) { + if (params.shouldErode()) { + Imgproc.erode(in, in, params.getKernel()); + } + if (params.shouldDilate()) { + Imgproc.dilate(in, in, params.getKernel()); + } + return in; + } + + public static class ErodeDilateParams { + private boolean m_erode; + private boolean m_dilate; + private Mat m_kernel; + + public ErodeDilateParams(boolean erode, boolean dilate, int kernelSize) { + m_erode = erode; + m_dilate = dilate; + m_kernel = + Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(kernelSize, kernelSize)); + } + + public boolean shouldErode() { + return m_erode; + } + + public boolean shouldDilate() { + return m_dilate; + } + + public Mat getKernel() { + return m_kernel; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FilterContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FilterContoursPipe.java new file mode 100644 index 000000000..076b78b23 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FilterContoursPipe.java @@ -0,0 +1,88 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.util.math.MathUtils; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.vision.camera.CaptureStaticProperties; +import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import java.util.ArrayList; +import java.util.List; +import org.opencv.core.Rect; +import org.opencv.core.RotatedRect; + +public class FilterContoursPipe + extends CVPipe, List, FilterContoursPipe.FilterContoursParams> { + + List m_filteredContours = new ArrayList<>(); + + @Override + protected List process(List in) { + m_filteredContours.clear(); + for (Contour contour : in) { + try { + filterContour(contour); + } catch (Exception e) { + System.err.println("An error occurred while filtering contours."); + e.printStackTrace(); + } + } + return m_filteredContours; + } + + private void filterContour(Contour contour) { + // Area Filtering. + double contourArea = contour.getArea(); + double areaRatio = (contourArea / params.getCamProperties().imageArea); + double minArea = MathUtils.sigmoid(params.getArea().getFirst()); + double maxArea = MathUtils.sigmoid(params.getArea().getSecond()); + if (areaRatio < minArea || areaRatio > maxArea) return; + + // Extent Filtering. + RotatedRect minAreaRect = contour.getMinAreaRect(); + double minExtent = params.getExtent().getFirst() * minAreaRect.size.area() / 100; + double maxExtent = params.getExtent().getSecond() * minAreaRect.size.area() / 100; + if (contourArea <= minExtent || contourArea >= maxExtent) return; + + // Aspect Ratio Filtering. + Rect boundingRect = contour.getBoundingRect(); + double aspectRatio = (double) boundingRect.width / boundingRect.height; + if (aspectRatio < params.getRatio().getFirst() || aspectRatio > params.getRatio().getSecond()) + return; + + m_filteredContours.add(contour); + } + + public static class FilterContoursParams { + private DoubleCouple m_area; + private DoubleCouple m_ratio; + private DoubleCouple m_extent; + private CaptureStaticProperties m_camProperties; + + public FilterContoursParams( + DoubleCouple area, + DoubleCouple ratio, + DoubleCouple extent, + CaptureStaticProperties camProperties) { + this.m_area = area; + this.m_ratio = ratio; + this.m_extent = extent; + this.m_camProperties = camProperties; + } + + public DoubleCouple getArea() { + return m_area; + } + + public DoubleCouple getRatio() { + return m_ratio; + } + + public DoubleCouple getExtent() { + return m_extent; + } + + public CaptureStaticProperties getCamProperties() { + return m_camProperties; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FindContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FindContoursPipe.java new file mode 100644 index 000000000..789ab759b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FindContoursPipe.java @@ -0,0 +1,31 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import java.util.ArrayList; +import java.util.List; +import java.util.stream.Collectors; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint; +import org.opencv.imgproc.Imgproc; + +public class FindContoursPipe + extends CVPipe, FindContoursPipe.FindContoursParams> { + + private List m_foundContours = new ArrayList<>(); + + @Override + protected List process(Mat in) { + for (var m : m_foundContours) { + m.release(); // necessary? + } + m_foundContours.clear(); + + Imgproc.findContours( + in, m_foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); + + return m_foundContours.stream().map(Contour::new).collect(Collectors.toList()); + } + + public static class FindContoursParams {} +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/GroupContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/GroupContoursPipe.java new file mode 100644 index 000000000..6a2bf9341 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/GroupContoursPipe.java @@ -0,0 +1,76 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.target.PotentialTarget; +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +public class GroupContoursPipe + extends CVPipe, List, GroupContoursPipe.GroupContoursParams> { + + private List m_targets = new ArrayList<>(); + + @Override + protected List process(List input) { + m_targets.clear(); + + if (params.getGroup() == Contour.ContourGrouping.Single) { + for (var contour : input) { + m_targets.add(new PotentialTarget(contour)); + } + } else { + int groupingCount = params.getGroup().count; + + if (input.size() > groupingCount) { + // todo: is it OK to mutate the input list? + // or should we clone it like before? + // what is the perf hit on cloning? + input.sort(Contour.SortByMomentsX); + // also why reverse? shouldn't the sort comparator just get reversed? + Collections.reverse(input); + // find out next time on Code Mysteries... + + for (int i = 0; i < input.size() - 1; i++) { + // make a list of the desired count of contours to group + List groupingSet; + try { + groupingSet = input.subList(i, i + groupingCount - 1); + } catch (IndexOutOfBoundsException e) { + continue; + } + + // FYI: This method only takes 2 contours! + Contour groupedContour = + Contour.groupContoursByIntersection( + groupingSet.get(0), groupingSet.get(1), params.getIntersection()); + + if (groupedContour != null) { + m_targets.add(new PotentialTarget(groupedContour, groupingSet)); + } + } + } + } + return m_targets; + } + + public static class GroupContoursParams { + private Contour.ContourGrouping m_group; + private Contour.ContourIntersection m_intersection; + + public GroupContoursParams( + Contour.ContourGrouping group, Contour.ContourIntersection intersection) { + m_group = group; + m_intersection = intersection; + } + + public Contour.ContourGrouping getGroup() { + return m_group; + } + + public Contour.ContourIntersection getIntersection() { + return m_intersection; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/HSVPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/HSVPipe.java new file mode 100644 index 000000000..c4c3b582b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/HSVPipe.java @@ -0,0 +1,44 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.pipeline.CVPipe; +import org.opencv.core.Core; +import org.opencv.core.CvException; +import org.opencv.core.Mat; +import org.opencv.core.Scalar; +import org.opencv.imgproc.Imgproc; + +public class HSVPipe extends CVPipe { + + private Mat m_outputMat = new Mat(); + + @Override + protected Mat process(Mat in) { + in.copyTo(m_outputMat); + try { + Imgproc.cvtColor(m_outputMat, m_outputMat, Imgproc.COLOR_BGR2HSV, 3); + Core.inRange(m_outputMat, params.getHsvLower(), params.getHsvUpper(), m_outputMat); + } catch (CvException e) { + System.err.println("(HSVPipe) Exception thrown by OpenCV: \n" + e.getMessage()); + } + + return m_outputMat; + } + + public static class HSVParams { + private Scalar m_hsvLower; + private Scalar m_hsvUpper; + + public HSVParams(Scalar hsvLower, Scalar hsvUpper) { + m_hsvLower = hsvLower; + m_hsvUpper = hsvUpper; + } + + public Scalar getHsvLower() { + return m_hsvLower; + } + + public Scalar getHsvUpper() { + return m_hsvUpper; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/OutputMatPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/OutputMatPipe.java new file mode 100644 index 000000000..d167b609e --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/OutputMatPipe.java @@ -0,0 +1,40 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.pipeline.CVPipe; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.CvException; +import org.opencv.core.Mat; +import org.opencv.imgproc.Imgproc; + +public class OutputMatPipe extends CVPipe, Mat, OutputMatPipe.OutputMatParams> { + + private Mat m_outputMat = new Mat(); + + @Override + protected Mat process(Pair in) { + if (params.showThreshold()) { + try { + in.getRight().copyTo(m_outputMat); + Imgproc.cvtColor(m_outputMat, m_outputMat, Imgproc.COLOR_GRAY2BGR, 3); + } catch (CvException e) { + System.err.println("(OutputMatPipe) Exception thrown by OpenCV: \n" + e.getMessage()); + } + } else { + in.getLeft().copyTo(m_outputMat); + } + + return m_outputMat; + } + + public static class OutputMatParams { + private boolean m_showThreshold; + + public OutputMatParams(boolean showThreshold) { + m_showThreshold = showThreshold; + } + + public boolean showThreshold() { + return m_showThreshold; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SortContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SortContoursPipe.java new file mode 100644 index 000000000..d9f648ba7 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SortContoursPipe.java @@ -0,0 +1,84 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.camera.CaptureStaticProperties; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.target.TrackedTarget; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; +import org.apache.commons.math3.util.FastMath; + +public class SortContoursPipe + extends CVPipe, List, SortContoursPipe.SortContoursParams> { + + private List m_sortedContours = new ArrayList<>(); + + @Override + protected List process(List in) { + m_sortedContours.clear(); + if (in.size() > 0) { + m_sortedContours.addAll(in); + if (params.getSortMode() != SortMode.Centermost) { + m_sortedContours.sort(params.getSortMode().getComparator()); + } else { + m_sortedContours.sort(Comparator.comparingDouble(this::calcSquareCenterDistance)); + } + } + + return new ArrayList<>( + m_sortedContours.subList(0, Math.min(in.size(), params.getMaxTargets() - 1))); + } + + private double calcSquareCenterDistance(TrackedTarget rect) { + return FastMath.sqrt( + FastMath.pow(params.getCamProperties().centerX - rect.getMinAreaRect().center.x, 2) + + FastMath.pow(params.getCamProperties().centerY - rect.getMinAreaRect().center.y, 2)); + } + + public enum SortMode { + Largest( + (rect1, rect2) -> + Double.compare(rect2.getMinAreaRect().size.area(), rect1.getMinAreaRect().size.area())), + Smallest(Largest.getComparator().reversed()), + Highest(Comparator.comparingDouble(rect -> rect.getMinAreaRect().center.y)), + Lowest(Highest.getComparator().reversed()), + Leftmost(Comparator.comparingDouble(target -> target.getMinAreaRect().center.x)), + Rightmost(Leftmost.getComparator().reversed()), + Centermost(null); + + private Comparator m_comparator; + + SortMode(Comparator comparator) { + m_comparator = comparator; + } + + public Comparator getComparator() { + return m_comparator; + } + } + + public static class SortContoursParams { + private SortMode m_sortMode; + private CaptureStaticProperties m_camProperties; + private int m_maxTargets; + + public SortContoursParams( + SortMode sortMode, CaptureStaticProperties camProperties, int maxTargets) { + m_sortMode = sortMode; + m_camProperties = camProperties; + m_maxTargets = maxTargets; + } + + public SortMode getSortMode() { + return m_sortMode; + } + + public CaptureStaticProperties getCamProperties() { + return m_camProperties; + } + + public int getMaxTargets() { + return m_maxTargets; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SpeckleRejectPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SpeckleRejectPipe.java new file mode 100644 index 000000000..8c101c3fb --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SpeckleRejectPipe.java @@ -0,0 +1,49 @@ +package com.chameleonvision.common.vision.pipeline.pipe; + +import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.pipeline.CVPipe; +import java.util.ArrayList; +import java.util.List; + +public class SpeckleRejectPipe + extends CVPipe, List, SpeckleRejectPipe.SpeckleRejectParams> { + + private List m_despeckledContours = new ArrayList<>(); + + @Override + protected List process(List in) { + for (var c : m_despeckledContours) { + c.mat.release(); + } + m_despeckledContours.clear(); + + if (in.size() > 0) { + double averageArea = 0.0; + for (Contour c : in) { + averageArea += c.getArea(); + } + averageArea /= in.size(); + + double minAllowedArea = params.getMinPercentOfAvg() / 100.0 * averageArea; + for (Contour c : in) { + if (c.getArea() >= minAllowedArea) { + m_despeckledContours.add(c); + } + } + } + + return m_despeckledContours; + } + + public static class SpeckleRejectParams { + private double m_minPercentOfAvg; + + public SpeckleRejectParams(double minPercentOfAvg) { + m_minPercentOfAvg = minPercentOfAvg; + } + + public double getMinPercentOfAvg() { + return m_minPercentOfAvg; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java index 7fd9fb92a..0b7e4468b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java @@ -2,84 +2,20 @@ package com.chameleonvision.common.vision.target; import com.chameleonvision.common.vision.opencv.Contour; import java.util.ArrayList; -import java.util.Collections; import java.util.List; -import org.opencv.core.MatOfPoint; -import org.opencv.core.Point; public class PotentialTarget { - final Contour mainContour; - final List subContours = new ArrayList<>(); + final Contour m_mainContour; + final List m_subContours; public PotentialTarget(Contour inputContour) { - mainContour = inputContour; + m_mainContour = inputContour; + m_subContours = new ArrayList<>(); // empty } - public PotentialTarget( - List subContours, - TargetContourIntersection intersection, - TargetContourGrouping grouping) { - // do contour grouping - mainContour = getGroupedContour(subContours, intersection, grouping); - if (mainContour == null) { - // this means we don't have a valid grouped target. what do we do??? - throw new RuntimeException("Something went fucky wucky"); - } - this.subContours.addAll(subContours); - } - - private Contour getGroupedContour( - List input, TargetContourIntersection intersection, TargetContourGrouping grouping) { - int reqSize = grouping == TargetContourGrouping.Single ? 1 : 2; - - if (input.size() != reqSize) { - return null; - // throw new RuntimeException("Insufficient contours for target grouping!"); - } - - switch (grouping) { - // technically should never happen but :shrug: - case Single: - return input.get(0); - case Dual: - input.sort(Contour.SortByMomentsX); - Collections.reverse(input); // why? - - Contour firstContour = input.get(0); - Contour secondContour = input.get(1); - - // total contour for both. add the first one for now - List fullContourPoints = new ArrayList<>(firstContour.mat.toList()); - - // add second contour if it is intersecting - if (firstContour.isIntersecting(secondContour, intersection)) { - fullContourPoints.addAll(secondContour.mat.toList()); - } else { - return null; - } - - MatOfPoint finalContour = new MatOfPoint(fullContourPoints.toArray(new Point[0])); - - if (finalContour.cols() != 0 && finalContour.rows() != 0) { - return new Contour(finalContour); - } - break; - } - return null; - } - - // TODO: move these? also docs plox - public enum TargetContourIntersection { - None, - Up, - Down, - Left, - Right - } - - public enum TargetContourGrouping { - Single, - Dual + public PotentialTarget(Contour inputContour, List subContours) { + m_mainContour = inputContour; + m_subContours = subContours; } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java index 62e1d630f..c6df22343 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java @@ -9,44 +9,44 @@ import org.opencv.core.RotatedRect; // TODO: banks fix public class TrackedTarget { - final Contour mainContour; - List subContours; // can be empty + final Contour m_mainContour; + List m_subContours; // can be empty - private Point targetOffsetPoint; - private Point robotOffsetPoint; + private Point m_targetOffsetPoint; + private Point m_robotOffsetPoint; - private double pitch; - private double yaw; - private double area; + private double m_pitch; + private double m_yaw; + private double m_area; public TrackedTarget(PotentialTarget origTarget, TargetCalculationParameters params) { - this.mainContour = origTarget.mainContour; - this.subContours = origTarget.subContours; + this.m_mainContour = origTarget.m_mainContour; + this.m_subContours = origTarget.m_subContours; calculateValues(params); } public Point getTargetOffsetPoint() { - return targetOffsetPoint; + return m_targetOffsetPoint; } public Point getRobotOffsetPoint() { - return robotOffsetPoint; + return m_robotOffsetPoint; } public double getPitch() { - return pitch; + return m_pitch; } public double getYaw() { - return yaw; + return m_yaw; } public double getArea() { - return area; + return m_area; } public RotatedRect getMinAreaRect() { - return mainContour.getMinAreaRect(); + return m_mainContour.getMinAreaRect(); } private void calculateTargetOffsetPoint( @@ -90,7 +90,7 @@ public class TrackedTarget { break; } } - targetOffsetPoint = resultPoint; + m_targetOffsetPoint = resultPoint; } private void calculateRobotOffsetPoint( @@ -118,25 +118,25 @@ public class TrackedTarget { break; } - robotOffsetPoint = resultPoint; + m_robotOffsetPoint = resultPoint; } private void calculatePitch(double verticalFocalLength) { - double contourCenterY = mainContour.getCenterPoint().y; - double targetCenterY = targetOffsetPoint.y; - pitch = + double contourCenterY = m_mainContour.getCenterPoint().y; + double targetCenterY = m_targetOffsetPoint.y; + m_pitch = -FastMath.toDegrees(FastMath.atan((contourCenterY - targetCenterY) / verticalFocalLength)); } private void calculateYaw(double horizontalFocalLength) { - double contourCenterX = mainContour.getCenterPoint().x; - double targetCenterX = targetOffsetPoint.x; - yaw = + double contourCenterX = m_mainContour.getCenterPoint().x; + double targetCenterX = m_targetOffsetPoint.x; + m_yaw = FastMath.toDegrees(FastMath.atan((contourCenterX - targetCenterX) / horizontalFocalLength)); } private void calculateArea(double imageArea) { - area = mainContour.getMinAreaRect().size.area() / imageArea; + m_area = m_mainContour.getMinAreaRect().size.area() / imageArea; } private Point getMiddle(Point p1, Point p2) { @@ -147,7 +147,7 @@ public class TrackedTarget { // this MUST happen in this exact order! calculateTargetOffsetPoint(params.isLandscape, params.targetOffsetPointRegion); calculateRobotOffsetPoint( - targetOffsetPoint, + m_targetOffsetPoint, params.cameraCenterPoint, params.offsetEquationValues, params.robotOffsetPointMode);