Skip to content

Commit

Permalink
Formatting fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@samdev-7
samdev-7 committed Feb 22, 2025
1 parent 8d891d2 commit fff82ae
Show file tree
Hide file tree
Showing 6 changed files with 198 additions and 76 deletions.
7 changes: 1 addition & 6 deletions photon-core/src/main/java/org/photonvision/vision/pipe/CVPipe.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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.
Expand All @@ -60,7 +56,6 @@ public CVPipeResult<O> run(I in) {
return result;
}


public static class CVPipeResult<O> {
public O output;
public long nanosElapsed;
Expand Down
109 changes: 71 additions & 38 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/AlgaeDetectionPipe.java
View file
Original file line number Diff line number Diff line change
@@ -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 <https://www.gnu.org/licenses/>.
*/

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;
Expand All @@ -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<Contour>, List<AlgaeDetectionPipe.AlgaeResult>, AlgaeDetectionPipe.AlgaeDetectionParams> {
public class AlgaeDetectionPipe
extends CVPipe<
List<Contour>,
List<AlgaeDetectionPipe.AlgaeResult>,
AlgaeDetectionPipe.AlgaeDetectionParams> {
// // Constants
// private static final double KNOWN_DIAMETER = 475.00; // mm

Expand All @@ -38,14 +55,15 @@ public class AlgaeDetectionPipe extends CVPipe<List<Contour>, List<AlgaeDetectio
@Override
public void setParams(AlgaeDetectionParams params) {
super.setParams(params);

// Algae Pipes
detector = new ObjectDetection(params.getMinArea(), params.getMinCircularity());

if (params.getCameraCalibration() != null) {
for (int i = 0; i < CAMERA_MATRIX.rows(); i++) {
for (int j = 0; j < CAMERA_MATRIX.cols(); j++) {
CAMERA_MATRIX.put(i, j, params.getCameraCalibration().getIntrinsicsArr()[i * CAMERA_MATRIX.cols() + j]);
CAMERA_MATRIX.put(
i, j, params.getCameraCalibration().getIntrinsicsArr()[i * CAMERA_MATRIX.cols() + j]);
}
}
}
Expand All @@ -59,7 +77,11 @@ public static class AlgaeDetectionParams {
private final double min_circularity;
private final CameraCalibrationCoefficients cameraCalibration;

public AlgaeDetectionParams(double diameter_mm, int min_area, double min_circularity, CameraCalibrationCoefficients cameraCalibration) {
public AlgaeDetectionParams(
double diameter_mm,
int min_area,
double min_circularity,
CameraCalibrationCoefficients cameraCalibration) {
this.diameter_mm = diameter_mm;
this.min_area = min_area;
this.min_circularity = min_circularity;
Expand Down Expand Up @@ -98,17 +120,16 @@ protected List<AlgaeResult> process(List<Contour> 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;
}
Expand All @@ -126,16 +147,23 @@ 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() {
return new CVShape(geContour(), center, radius);
}

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());
}
Expand All @@ -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<AlgaeResult> findLargestAlgae(List<Contour> contoursList) {

List<MatOfPoint> contours = contoursList.stream()
.map(contour -> contour.mat) // Extract the MatOfPoint from each Contour
.collect(Collectors.toList()); // Collect into a List<MatOfPoint>
List<MatOfPoint> contours =
contoursList.stream()
.map(contour -> contour.mat) // Extract the MatOfPoint from each Contour
.collect(Collectors.toList()); // Collect into a List<MatOfPoint>

// Variables to store the largest algae
Point largestBallCenter = null;
Expand All @@ -163,7 +191,6 @@ public Optional<AlgaeResult> findLargestAlgae(List<Contour> contoursList) {

// Iterate over the contours
for (MatOfPoint contour : contours) {

double area = Imgproc.contourArea(contour);

// Convert MatOfPoint to MatOfPoint2f for arcLength
Expand All @@ -187,14 +214,13 @@ public Optional<AlgaeResult> findLargestAlgae(List<Contour> contoursList) {
}
}
}

}

if (largestBallCenter == null) {
return Optional.empty();
}
return Optional.of(new AlgaeResult(largestBallCenter, largestRadius));

return Optional.of(new AlgaeResult(largestBallCenter, largestRadius));
}
}

Expand All @@ -216,33 +242,34 @@ 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;
}

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;

Expand All @@ -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));
Expand Down Expand Up @@ -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));
Expand All @@ -307,4 +340,4 @@ public double calculateVerticalAngle(double objectCenterY) {
// return -realAngle;
// }
}
}
}
38 changes: 33 additions & 5 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/EdgeMaskPipe.java
View file
Original file line number Diff line number Diff line change
@@ -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 <https://www.gnu.org/licenses/>.
*/

package org.photonvision.vision.pipe.impl;

import org.opencv.core.Mat;
Expand All @@ -6,7 +23,6 @@
import org.photonvision.vision.pipe.CVPipe;

public class EdgeMaskPipe extends CVPipe<Mat, Mat, EdgeMaskPipe.MaskParams> {

@Override
protected Mat process(Mat inputMask) {
// Apply morphological transformations to clean up the mask
Expand Down Expand Up @@ -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;
}
Expand All @@ -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;
Expand Down Expand Up @@ -91,4 +119,4 @@ public int getEdgeThresholdHigh() {
return this.edgeThresholdHigh;
}
}
}
}
28 changes: 26 additions & 2 deletions photon-core/src/main/java/org/photonvision/vision/pipe/impl/PaddingPipe.java
View file
Original file line number Diff line number Diff line change
@@ -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 <https://www.gnu.org/licenses/>.
*/

package org.photonvision.vision.pipe.impl;

import org.opencv.core.Core;
Expand All @@ -6,12 +23,19 @@
import org.photonvision.vision.pipe.CVPipe;

public class PaddingPipe extends CVPipe<Mat, Mat, PaddingPipe.PaddingParams> {

@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;
}

Expand Down
Loading

0 comments on commit fff82ae

Please sign in to comment.