From fff82aeb2256922a815744615edadb1f96f0a191 Mon Sep 17 00:00:00 2001 From: Sam Liu Date: Fri, 21 Feb 2025 23:59:06 -0500 Subject: [PATCH] Formatting fixes --- .../org/photonvision/vision/pipe/CVPipe.java | 7 +- .../vision/pipe/impl/AlgaeDetectionPipe.java | 109 ++++++++++++------ .../vision/pipe/impl/EdgeMaskPipe.java | 38 +++++- .../vision/pipe/impl/PaddingPipe.java | 28 ++++- .../vision/pipeline/AlgaePipeline.java | 71 ++++++++---- .../pipeline/AlgaePipelineSettings.java | 21 +++- 6 files changed, 198 insertions(+), 76 deletions(-) diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/CVPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/CVPipe.java index c4ddc9e9e3..74d9fdc989 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/CVPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/CVPipe.java @@ -17,10 +17,6 @@ package org.photonvision.vision.pipe; -import java.util.List; - -import org.photonvision.vision.opencv.Contour; - /** * Defines a pipe. A pipe is a single step in a pipeline. This class is to be extended, never used * on its own. @@ -48,7 +44,7 @@ public P getParams() { * @return Result of processing. */ protected abstract O process(I in); - + /** * @param in Input for pipe processing. * @return Result of processing. @@ -60,7 +56,6 @@ public CVPipeResult run(I in) { return result; } - public static class CVPipeResult { public O output; public long nanosElapsed; diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AlgaeDetectionPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AlgaeDetectionPipe.java index d3d660dc35..5a35a41ad4 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AlgaeDetectionPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/AlgaeDetectionPipe.java @@ -1,11 +1,27 @@ -// TODO: Make a blur pipe, pipe the org image to the hsv pipe and pip that to algae. Keep in mind unit conversions +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ package org.photonvision.vision.pipe.impl; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; import java.util.List; import java.util.Optional; import java.util.stream.Collectors; - import org.opencv.core.CvType; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; @@ -19,10 +35,11 @@ import org.photonvision.vision.opencv.Contour; import org.photonvision.vision.pipe.CVPipe; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; - -public class AlgaeDetectionPipe extends CVPipe, List, AlgaeDetectionPipe.AlgaeDetectionParams> { +public class AlgaeDetectionPipe + extends CVPipe< + List, + List, + AlgaeDetectionPipe.AlgaeDetectionParams> { // // Constants // private static final double KNOWN_DIAMETER = 475.00; // mm @@ -38,14 +55,15 @@ public class AlgaeDetectionPipe extends CVPipe, List process(List in) { public class AlgaeResult { private Point center; private double radius; - + public AlgaeResult(Point center, double radius) { this.center = center; this.radius = radius; } - public Point getCenter() { return center; } - + public double getRadius() { return radius; } @@ -126,7 +147,7 @@ public double getYAngle() { } public Contour geContour() { - return new Contour(new Rect2d(center.x-radius, center.y-radius, radius*2, radius*2)); + return new Contour(new Rect2d(center.x - radius, center.y - radius, radius * 2, radius * 2)); } public CVShape getShape() { @@ -134,8 +155,15 @@ public CVShape getShape() { } public Transform3d getCameraToAlgaeTransform() { - double xTranslation = getDistance() * Math.cos(Math.toRadians(getYAngle())) * Math.cos(Math.toRadians(getXAngle())); - double yTranslation = -1 * getDistance() * Math.cos(Math.toRadians(getYAngle())) * Math.sin(Math.toRadians(getXAngle())); + double xTranslation = + getDistance() + * Math.cos(Math.toRadians(getYAngle())) + * Math.cos(Math.toRadians(getXAngle())); + double yTranslation = + -1 + * getDistance() + * Math.cos(Math.toRadians(getYAngle())) + * Math.sin(Math.toRadians(getXAngle())); double zTranslation = getDistance() * Math.sin(Math.toRadians(getYAngle())); return new Transform3d(xTranslation, yTranslation, zTranslation, new Rotation3d()); } @@ -145,16 +173,16 @@ public class ObjectDetection { private int minArea; private double minCircularity; - public ObjectDetection( int minArea, double minCircularity) { + public ObjectDetection(int minArea, double minCircularity) { this.minArea = minArea; this.minCircularity = minCircularity; } public Optional findLargestAlgae(List contoursList) { - - List contours = contoursList.stream() - .map(contour -> contour.mat) // Extract the MatOfPoint from each Contour - .collect(Collectors.toList()); // Collect into a List + List contours = + contoursList.stream() + .map(contour -> contour.mat) // Extract the MatOfPoint from each Contour + .collect(Collectors.toList()); // Collect into a List // Variables to store the largest algae Point largestBallCenter = null; @@ -163,7 +191,6 @@ public Optional findLargestAlgae(List contoursList) { // Iterate over the contours for (MatOfPoint contour : contours) { - double area = Imgproc.contourArea(contour); // Convert MatOfPoint to MatOfPoint2f for arcLength @@ -187,14 +214,13 @@ public Optional findLargestAlgae(List contoursList) { } } } - } if (largestBallCenter == null) { return Optional.empty(); } - return Optional.of(new AlgaeResult(largestBallCenter, largestRadius)); + return Optional.of(new AlgaeResult(largestBallCenter, largestRadius)); } } @@ -216,14 +242,14 @@ public double calculateHorizontalAngle(double objectCenterX) { // Extract focal length (fx) and principal point (cx) from the camera matrix double fx = CAMERA_MATRIX.get(0, 0)[0]; double cx = CAMERA_MATRIX.get(0, 2)[0]; - + // Compute the normalized x coordinate: (u - cx) / fx double normalizedX = (objectCenterX - cx) / fx; - + // Calculate the horizontal angle in radians and convert to degrees double angleRad = Math.atan(normalizedX); double angleDeg = Math.toDegrees(angleRad); - + return angleDeg; } @@ -231,18 +257,19 @@ public double calculateVerticalAngle(double objectCenterY) { // Extract focal length (fy) and principal point (cy) from the camera matrix double fy = CAMERA_MATRIX.get(1, 1)[0]; double cy = CAMERA_MATRIX.get(1, 2)[0]; - + // Compute the normalized y coordinate: (objectCenterY - cy) / fy double normalizedY = (objectCenterY - cy) / fy; - + // Calculate the vertical angle in radians and convert to degrees double angleRad = Math.atan(normalizedY); double realAngle = Math.toDegrees(angleRad); - + return realAngle; } - // public double calculateHorizontalAngle(Mat frame, double objectCenterX, double cameraOffset) { + // public double calculateHorizontalAngle(Mat frame, double objectCenterX, double cameraOffset) + // { // double screenCenterX = frame.width() / 2; // double screenCenterY = frame.height() / 2; @@ -261,10 +288,13 @@ public double calculateVerticalAngle(double objectCenterY) { // float[] vec2Arr = vector2.toArray(); // // Perform the dot product and angle calculation - // double dotProduct = vec1Arr[0] * vec2Arr[0] + vec1Arr[1] * vec2Arr[1] + vec1Arr[2] * vec2Arr[2]; + // double dotProduct = vec1Arr[0] * vec2Arr[0] + vec1Arr[1] * vec2Arr[1] + vec1Arr[2] * + // vec2Arr[2]; - // double norm1 = Math.sqrt(vec1Arr[0] * vec1Arr[0] + vec1Arr[1] * vec1Arr[1] + vec1Arr[2] * vec1Arr[2]); - // double norm2 = Math.sqrt(vec2Arr[0] * vec2Arr[0] + vec2Arr[1] * vec2Arr[1] + vec2Arr[2] * vec2Arr[2]); + // double norm1 = Math.sqrt(vec1Arr[0] * vec1Arr[0] + vec1Arr[1] * vec1Arr[1] + vec1Arr[2] * + // vec1Arr[2]); + // double norm2 = Math.sqrt(vec2Arr[0] * vec2Arr[0] + vec2Arr[1] * vec2Arr[1] + vec2Arr[2] * + // vec2Arr[2]); // double cosAngle = dotProduct / (norm1 * norm2); // double realAngle = Math.toDegrees(Math.acos(cosAngle)); @@ -292,10 +322,13 @@ public double calculateVerticalAngle(double objectCenterY) { // float[] vec2Arr = vector2.toArray(); // // Perform the dot product and angle calculation - // double dotProduct = vec1Arr[0] * vec2Arr[0] + vec1Arr[1] * vec2Arr[1] + vec1Arr[2] * vec2Arr[2]; + // double dotProduct = vec1Arr[0] * vec2Arr[0] + vec1Arr[1] * vec2Arr[1] + vec1Arr[2] * + // vec2Arr[2]; - // double norm1 = Math.sqrt(vec1Arr[0] * vec1Arr[0] + vec1Arr[1] * vec1Arr[1] + vec1Arr[2] * vec1Arr[2]); - // double norm2 = Math.sqrt(vec2Arr[0] * vec2Arr[0] + vec2Arr[1] * vec2Arr[1] + vec2Arr[2] * vec2Arr[2]); + // double norm1 = Math.sqrt(vec1Arr[0] * vec1Arr[0] + vec1Arr[1] * vec1Arr[1] + vec1Arr[2] * + // vec1Arr[2]); + // double norm2 = Math.sqrt(vec2Arr[0] * vec2Arr[0] + vec2Arr[1] * vec2Arr[1] + vec2Arr[2] * + // vec2Arr[2]); // double cosAngle = dotProduct / (norm1 * norm2); // double realAngle = Math.toDegrees(Math.acos(cosAngle)); @@ -307,4 +340,4 @@ public double calculateVerticalAngle(double objectCenterY) { // return -realAngle; // } } -} \ No newline at end of file +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/EdgeMaskPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/EdgeMaskPipe.java index 568dc6f164..076dcf303c 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/EdgeMaskPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/EdgeMaskPipe.java @@ -1,3 +1,20 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + package org.photonvision.vision.pipe.impl; import org.opencv.core.Mat; @@ -6,7 +23,6 @@ import org.photonvision.vision.pipe.CVPipe; public class EdgeMaskPipe extends CVPipe { - @Override protected Mat process(Mat inputMask) { // Apply morphological transformations to clean up the mask @@ -39,11 +55,17 @@ private Mat detectEdges(Mat mask) { private Mat dilateEdges(Mat edges) { // Dilate the edges to fill gaps and connect broken parts Mat dilatedEdges = new Mat(); - Imgproc.dilate(edges, dilatedEdges, new Mat(), new Point(-1, -1), params.getEdgeDilationIterations()); + Imgproc.dilate( + edges, dilatedEdges, new Mat(), new Point(-1, -1), params.getEdgeDilationIterations()); // Further dilation to ensure continuity of the edges Mat filledEdges = new Mat(); - Imgproc.dilate(dilatedEdges, filledEdges, new Mat(), new Point(-1, -1), params.getFinalDilationIterations()); + Imgproc.dilate( + dilatedEdges, + filledEdges, + new Mat(), + new Point(-1, -1), + params.getFinalDilationIterations()); return filledEdges; } @@ -58,7 +80,13 @@ public static class MaskParams { private final int finalDilationIterations; // Constructor for setting the iterations - public MaskParams(int erosion, int initialDilate, int edgeLow, int edgeHigh, int edgeDilate, int finalDilate) { + public MaskParams( + int erosion, + int initialDilate, + int edgeLow, + int edgeHigh, + int edgeDilate, + int finalDilate) { this.erosionIterations = erosion; this.initialDilationIterations = initialDilate; this.edgeThresholdLow = edgeLow; @@ -91,4 +119,4 @@ public int getEdgeThresholdHigh() { return this.edgeThresholdHigh; } } -} \ No newline at end of file +} diff --git a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/PaddingPipe.java b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/PaddingPipe.java index e579033ccc..54e79a9dd3 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipe/impl/PaddingPipe.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipe/impl/PaddingPipe.java @@ -1,3 +1,20 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + package org.photonvision.vision.pipe.impl; import org.opencv.core.Core; @@ -6,12 +23,19 @@ import org.photonvision.vision.pipe.CVPipe; public class PaddingPipe extends CVPipe { - @Override protected Mat process(Mat in) { // Apply padding to the image Mat paddedImage = new Mat(); - Core.copyMakeBorder(in, paddedImage, params.getPadding(), params.getPadding(), params.getPadding(), params.getPadding(), Core.BORDER_CONSTANT, new Scalar(0, 0, 0)); + Core.copyMakeBorder( + in, + paddedImage, + params.getPadding(), + params.getPadding(), + params.getPadding(), + params.getPadding(), + Core.BORDER_CONSTANT, + new Scalar(0, 0, 0)); return paddedImage; } diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipeline.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipeline.java index 13b5faefa7..82e19a6a2b 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipeline.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipeline.java @@ -1,8 +1,25 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + package org.photonvision.vision.pipeline; +import edu.wpi.first.math.geometry.Transform3d; import java.util.List; import java.util.stream.Collectors; - import org.opencv.core.Mat; import org.photonvision.vision.frame.Frame; import org.photonvision.vision.frame.FrameThresholdType; @@ -11,26 +28,24 @@ import org.photonvision.vision.opencv.DualOffsetValues; import org.photonvision.vision.pipe.CVPipe.CVPipeResult; import org.photonvision.vision.pipe.impl.AlgaeDetectionPipe; +import org.photonvision.vision.pipe.impl.AlgaeDetectionPipe.AlgaeResult; import org.photonvision.vision.pipe.impl.CalculateFPSPipe; import org.photonvision.vision.pipe.impl.Collect2dTargetsPipe; import org.photonvision.vision.pipe.impl.Draw2dCrosshairPipe; import org.photonvision.vision.pipe.impl.Draw2dTargetsPipe; import org.photonvision.vision.pipe.impl.Draw3dTargetsPipe; +import org.photonvision.vision.pipe.impl.EdgeMaskPipe; import org.photonvision.vision.pipe.impl.FindContoursPipe; import org.photonvision.vision.pipe.impl.HSVPipe; -import org.photonvision.vision.pipe.impl.EdgeMaskPipe; import org.photonvision.vision.pipe.impl.PaddingPipe; -import org.photonvision.vision.pipe.impl.AlgaeDetectionPipe.AlgaeResult; import org.photonvision.vision.pipeline.result.CVPipelineResult; import org.photonvision.vision.target.PotentialTarget; import org.photonvision.vision.target.TrackedTarget; -import edu.wpi.first.math.geometry.Transform3d; - public class AlgaePipeline extends CVPipeline { private final PaddingPipe paddedPipe = new PaddingPipe(); private final HSVPipe hsvPipe = new HSVPipe(); - private final EdgeMaskPipe maskPipe = new EdgeMaskPipe(); + private final EdgeMaskPipe maskPipe = new EdgeMaskPipe(); private final FindContoursPipe findContoursPipe = new FindContoursPipe(); private final AlgaeDetectionPipe algaeDetectionPipe = new AlgaeDetectionPipe(); private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe(); @@ -39,7 +54,6 @@ public class AlgaePipeline extends CVPipeline paddedResult = paddedPipe.run(frame.colorImage.getMat()); sumPipeNanosElapsed += paddedResult.nanosElapsed; CVPipeResult hsvResult = hsvPipe.run(paddedResult.output); - sumPipeNanosElapsed+= hsvResult.nanosElapsed; + sumPipeNanosElapsed += hsvResult.nanosElapsed; CVPipeResult maskResult = maskPipe.run(hsvResult.output); sumPipeNanosElapsed += maskResult.nanosElapsed; @@ -136,16 +155,22 @@ protected CVPipelineResult process(Frame frame, AlgaePipelineSettings settings) CVPipeResult> algaeDetectionResult = algaeDetectionPipe.run(contours); sumPipeNanosElapsed += algaeDetectionResult.nanosElapsed; List algaeResults = algaeDetectionResult.output; - - long currentTimeNanos = System.nanoTime(); - List algaeShapes = algaeResults.stream().map(AlgaeResult::getShape).collect(Collectors.toList()); - List potentialTargets = algaeShapes.stream().map(shape -> { - return new PotentialTarget(shape.getContour(), shape); - }).collect(Collectors.toList()); + long currentTimeNanos = System.nanoTime(); + List algaeShapes = + algaeResults.stream().map(AlgaeResult::getShape).collect(Collectors.toList()); + + List potentialTargets = + algaeShapes.stream() + .map( + shape -> { + return new PotentialTarget(shape.getContour(), shape); + }) + .collect(Collectors.toList()); sumPipeNanosElapsed += System.nanoTime() - currentTimeNanos; - CVPipeResult> collect2dTargetsResult = collect2dTargetsPipe.run(potentialTargets); + CVPipeResult> collect2dTargetsResult = + collect2dTargetsPipe.run(potentialTargets); sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed; currentTimeNanos = System.nanoTime(); diff --git a/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipelineSettings.java b/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipelineSettings.java index f4e7ff7687..8331e9eed7 100644 --- a/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipelineSettings.java +++ b/photon-core/src/main/java/org/photonvision/vision/pipeline/AlgaePipelineSettings.java @@ -1,11 +1,28 @@ +/* + * Copyright (C) Photon Vision. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + package org.photonvision.vision.pipeline; import com.fasterxml.jackson.annotation.JsonTypeName; import java.util.Objects; @JsonTypeName("AlgaePipelineSettings") -public class AlgaePipelineSettings extends AdvancedPipelineSettings{ - +public class AlgaePipelineSettings extends AdvancedPipelineSettings { + public AlgaePipelineSettings() { super(); pipelineType = PipelineType.Algae;