Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-21 01:01:41 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix .gitignore, move vision package out of common

Browse Source
This commit is contained in:
Banks Troutman
2020-06-28 04:40:43 -04:00
parent bdbd6b9d18
commit f2fbe9dd6e
214 changed files with 485 additions and 7768 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
photon-server/src/main/java/org/photonvision/common/calibration/CameraCalibrationCoefficients.java
View File

@@ -1,46 +0,0 @@
package org.photonvision.common.calibration;
import org.photonvision.common.vision.opencv.Releasable;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.Size;
public class CameraCalibrationCoefficients implements Releasable {
@JsonProperty("resolution")
public final Size resolution;
@JsonProperty("cameraIntrinsics")
public final JsonMat cameraIntrinsics;
@JsonProperty("cameraExtrinsics")
public final JsonMat cameraExtrinsics;
@JsonCreator
public CameraCalibrationCoefficients(
@JsonProperty("resolution") Size resolution,
@JsonProperty("cameraIntrinsics") JsonMat cameraIntrinsics,
@JsonProperty("cameraExtrinsics") JsonMat cameraExtrinsics) {
this.resolution = resolution;
this.cameraIntrinsics = cameraIntrinsics;
this.cameraExtrinsics = cameraExtrinsics;
}
@JsonIgnore
public Mat getCameraIntrinsicsMat() {
return cameraIntrinsics.getAsMat();
}
@JsonIgnore
public MatOfDouble getCameraExtrinsicsMat() {
return cameraExtrinsics.getAsMatOfDouble();
}
@Override
public void release() {
cameraIntrinsics.release();
cameraExtrinsics.release();
}
}

92
photon-server/src/main/java/org/photonvision/common/calibration/JsonMat.java
View File

@@ -1,92 +0,0 @@
package org.photonvision.common.calibration;
import org.photonvision.common.vision.opencv.Releasable;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.Arrays;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
public class JsonMat implements Releasable {
public final int rows;
public final int cols;
public final int type;
public final double[] data;
@JsonIgnore private Mat wrappedMat;
private MatOfDouble wrappedMatOfDouble;
public JsonMat(int rows, int cols, double[] data) {
this(rows, cols, CvType.CV_64FC1, data);
}
public JsonMat(
@JsonProperty("rows") int rows,
@JsonProperty("cols") int cols,
@JsonProperty("type") int type,
@JsonProperty("data") double[] data) {
this.rows = rows;
this.cols = cols;
this.type = type;
this.data = data;
}
private static boolean isCameraMatrixMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3;
}
private static boolean isDistortionCoeffsMat(Mat mat) {
return mat.type() == CvType.CV_64FC1 && mat.cols() == 5 && mat.rows() == 1;
}
private static boolean isCalibrationMat(Mat mat) {
return isDistortionCoeffsMat(mat) || isCameraMatrixMat(mat);
}
@JsonIgnore
public static double[] getDataFromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
double[] data = new double[(int) (mat.total() * mat.elemSize())];
mat.get(0, 0, data);
int dataLen = -1;
if (isCameraMatrixMat(mat)) dataLen = 9;
if (isDistortionCoeffsMat(mat)) dataLen = 5;
// truncate Mat data to correct number data points.
return Arrays.copyOfRange(data, 0, dataLen);
}
public static JsonMat fromMat(Mat mat) {
if (!isCalibrationMat(mat)) return null;
return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat));
}
@JsonIgnore
public Mat getAsMat() {
if (this.type != CvType.CV_64FC1) return null;
if (wrappedMat == null) {
this.wrappedMat = new Mat(this.rows, this.cols, this.type);
this.wrappedMat.put(0, 0, this.data);
}
return this.wrappedMat;
}
@JsonIgnore
public MatOfDouble getAsMatOfDouble() {
if (this.wrappedMatOfDouble == null) {
this.wrappedMatOfDouble = new MatOfDouble();
getAsMat().convertTo(wrappedMatOfDouble, CvType.CV_64F);
}
return this.wrappedMatOfDouble;
}
@Override
public void release() {
getAsMat().release();
}
}

15
photon-server/src/main/java/org/photonvision/common/configuration/CameraConfiguration.java
View File

@@ -1,18 +1,17 @@
package org.photonvision.common.configuration;
import org.photonvision.common.calibration.CameraCalibrationCoefficients;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.vision.camera.CameraType;
import org.photonvision.common.vision.pipeline.CVPipelineSettings;
import org.photonvision.common.vision.pipeline.DriverModePipelineSettings;
import org.photonvision.common.vision.processes.PipelineManager;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.ArrayList;
import java.util.List;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.vision.calibration.CameraCalibrationCoefficients;
import org.photonvision.vision.camera.CameraType;
import org.photonvision.vision.pipeline.CVPipelineSettings;
import org.photonvision.vision.pipeline.DriverModePipelineSettings;
import org.photonvision.vision.processes.PipelineManager;
public class CameraConfiguration {
private static final Logger logger = new Logger(CameraConfiguration.class, LogGroup.Camera);

11
photon-server/src/main/java/org/photonvision/common/configuration/ConfigManager.java
View File

@@ -1,12 +1,6 @@
package org.photonvision.common.configuration;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.common.vision.pipeline.CVPipelineSettings;
import org.photonvision.common.vision.pipeline.DriverModePipelineSettings;
import com.fasterxml.jackson.core.JsonProcessingException;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
@@ -16,6 +10,11 @@ import java.util.HashMap;
import java.util.List;
import java.util.Objects;
import java.util.stream.Collectors;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.file.JacksonUtils;
import org.photonvision.vision.pipeline.CVPipelineSettings;
import org.photonvision.vision.pipeline.DriverModePipelineSettings;
public class ConfigManager {
private static final Logger logger = new Logger(ConfigManager.class, LogGroup.General);

3
photon-server/src/main/java/org/photonvision/common/datatransfer/DataConsumer.java
View File

@@ -1,7 +1,6 @@
package org.photonvision.common.datatransfer;
import org.photonvision.common.vision.processes.Data;
import java.util.function.Consumer;
import org.photonvision.vision.processes.Data;
public interface DataConsumer extends Consumer<Data> {}

9
photon-server/src/main/java/org/photonvision/common/datatransfer/networktables/NetworkTablesManager.java
View File

@@ -1,14 +1,13 @@
package org.photonvision.common.datatransfer.networktables;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.function.Consumer;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.scripting.ScriptEventType;
import org.photonvision.common.scripting.ScriptManager;
import edu.wpi.first.networktables.LogMessage;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import java.util.function.Consumer;
public class NetworkTablesManager {

4
photon-server/src/main/java/org/photonvision/common/networking/LinuxNetworking.java
View File

@@ -1,7 +1,5 @@
package org.photonvision.common.networking;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import java.io.File;
import java.io.IOException;
import java.net.InetAddress;
@@ -11,6 +9,8 @@ import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.apache.commons.io.FileUtils;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
public class LinuxNetworking extends SysNetworking {
private static final String PATH = "/etc/dhcpcd.conf";

3
photon-server/src/main/java/org/photonvision/common/networking/SysNetworking.java
View File

@@ -1,11 +1,10 @@
package org.photonvision.common.networking;
import org.photonvision.common.util.ShellExec;
import java.io.IOException;
import java.net.InetAddress;
import java.net.SocketException;
import java.util.List;
import org.photonvision.common.util.ShellExec;
public abstract class SysNetworking {
NetworkInterface networkInterface;

3
photon-server/src/main/java/org/photonvision/common/scripting/ScriptEvent.java
View File

@@ -1,11 +1,10 @@
package org.photonvision.common.scripting;
import java.io.IOException;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.ShellExec;
import java.io.IOException;
public class ScriptEvent {
private static final ShellExec executor = new ShellExec(true, true);

11
photon-server/src/main/java/org/photonvision/common/scripting/ScriptManager.java
View File

@@ -1,11 +1,5 @@
package org.photonvision.common.scripting;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.LoopingRunnable;
import org.photonvision.common.util.Platform;
import org.photonvision.common.util.file.JacksonUtils;
import java.io.IOException;
import java.nio.file.Files;
import java.nio.file.Path;
@@ -13,6 +7,11 @@ import java.nio.file.Paths;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.LinkedBlockingDeque;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.LoopingRunnable;
import org.photonvision.common.util.Platform;
import org.photonvision.common.util.file.JacksonUtils;
public class ScriptManager {

7
photon-server/src/main/java/org/photonvision/common/util/file/FileUtils.java
View File

@@ -1,9 +1,5 @@
package org.photonvision.common.util.file;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.Platform;
import java.io.File;
import java.io.IOException;
import java.nio.file.Files;
@@ -13,6 +9,9 @@ import java.nio.file.attribute.PosixFilePermission;
import java.util.Arrays;
import java.util.HashSet;
import java.util.Set;
import org.photonvision.common.logging.LogGroup;
import org.photonvision.common.logging.Logger;
import org.photonvision.common.util.Platform;
public class FileUtils {

5
photon-server/src/main/java/org/photonvision/common/vision/camera/CameraQuirks.java
View File

@@ -1,5 +0,0 @@
package org.photonvision.common.vision.camera;
public enum CameraQuirks {
Gain
}

6
photon-server/src/main/java/org/photonvision/common/vision/camera/CameraType.java
View File

@@ -1,6 +0,0 @@
package org.photonvision.common.vision.camera;
public enum CameraType {
UsbCamera,
HttpCamera
}

35
photon-server/src/main/java/org/photonvision/common/vision/camera/QuirkyCamera.java
View File

@@ -1,35 +0,0 @@
package org.photonvision.common.vision.camera;
import java.util.ArrayList;
import java.util.List;
public class QuirkyCamera {
public static final List<QuirkyCamera> quirkyCameras =
List.of(
// ps3 eye
new QuirkyCamera(0x1415, 0x2000, "PS3Eye", List.of(CameraQuirks.Gain)));
public final int usbVid;
public final int usbPid;
public final String name;
public List<CameraQuirks> quirks;
public QuirkyCamera(int usbVid, int usbPid, String baseName, List<CameraQuirks> quirks) {
this.usbVid = usbVid;
this.usbPid = usbPid;
this.name = baseName;
this.quirks = quirks;
}
public QuirkyCamera(int usbVid, int usbPid, String baseName) {
this(usbVid, usbPid, baseName, new ArrayList<>());
QuirkyCamera quirky =
quirkyCameras.stream()
.filter(quirkyCamera -> quirkyCamera.usbPid == usbPid && quirkyCamera.usbVid == usbVid)
.findFirst()
.orElse(null);
if (quirky != null) {
this.quirks = quirky.quirks;
}
}
}

123
photon-server/src/main/java/org/photonvision/common/vision/camera/USBCameraSource.java
View File

@@ -1,123 +0,0 @@
package org.photonvision.common.vision.camera;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.vision.frame.FrameProvider;
import org.photonvision.common.vision.frame.provider.USBFrameProvider;
import org.photonvision.common.vision.processes.VisionSource;
import org.photonvision.common.vision.processes.VisionSourceSettables;
import edu.wpi.cscore.CvSink;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.VideoMode;
import edu.wpi.first.cameraserver.CameraServer;
import java.util.*;
public class USBCameraSource implements VisionSource {
private final UsbCamera camera;
private final USBCameraSettables usbCameraSettables;
private final USBFrameProvider usbFrameProvider;
private final CameraConfiguration configuration;
private final QuirkyCamera cameraQuirks;
public USBCameraSource(CameraConfiguration config) {
this.configuration = config;
this.camera = new UsbCamera(config.nickname, config.path);
this.cameraQuirks =
new QuirkyCamera(camera.getInfo().productId, camera.getInfo().vendorId, config.baseName);
CvSink cvSink = CameraServer.getInstance().getVideo(this.camera);
this.usbCameraSettables = new USBCameraSettables(config);
this.usbFrameProvider =
new USBFrameProvider(cvSink, usbCameraSettables.getFrameStaticProperties());
}
@Override
public boolean equals(Object obj) {
if (obj.getClass() != USBCameraSource.class) {
return false;
}
USBCameraSource tmp = (USBCameraSource) obj;
boolean i = this.cameraQuirks.quirks.equals(tmp.cameraQuirks.quirks);
boolean r = this.configuration.uniqueName.equals(tmp.configuration.uniqueName);
boolean c = this.configuration.baseName.equals(tmp.configuration.baseName);
boolean j = this.configuration.nickname.equals(tmp.configuration.nickname);
boolean k = this.camera.getInfo().name.equals(tmp.camera.getInfo().name);
boolean x = this.camera.getInfo().productId == tmp.camera.getInfo().productId;
boolean y = this.camera.getInfo().vendorId == tmp.camera.getInfo().vendorId;
var t = i && r && c && j && k && x && y;
return t;
}
@Override
public FrameProvider getFrameProvider() {
return usbFrameProvider;
}
@Override
public VisionSourceSettables getSettables() {
return this.usbCameraSettables;
}
public class USBCameraSettables extends VisionSourceSettables {
protected USBCameraSettables(CameraConfiguration configuration) {
super(configuration);
}
@Override
public int getExposure() {
return camera.getProperty("exposure").get();
}
@Override
public void setExposure(int exposure) {
camera.setExposureManual(exposure);
}
@Override
public int getBrightness() {
return camera.getBrightness();
}
@Override
public void setBrightness(int brightness) {
camera.setBrightness(brightness);
}
@Override
public int getGain() {
return camera.getProperty("gain").get();
}
@Override
public void setGain(int gain) {
if (cameraQuirks.quirks.contains(CameraQuirks.Gain)) {
camera.getProperty("gain_automatic").set(0);
camera.getProperty("gain").set(gain);
}
}
@Override
public VideoMode getCurrentVideoMode() {
return camera.getVideoMode();
}
@Override
public void setCurrentVideoMode(VideoMode videoMode) {
camera.setVideoMode(videoMode);
}
@Override
public HashMap<Integer, VideoMode> getAllVideoModes() {
if (videoModes == null) {
videoModes = new HashMap<>();
List<VideoMode> videoModesList = Arrays.asList(camera.enumerateVideoModes());
for (VideoMode videoMode : videoModesList) {
videoModes.put(videoModesList.indexOf(videoMode), videoMode);
}
}
return videoModes;
}
}
}

37
photon-server/src/main/java/org/photonvision/common/vision/frame/Frame.java
View File

@@ -1,37 +0,0 @@
package org.photonvision.common.vision.frame;
import org.photonvision.common.vision.opencv.CVMat;
import org.photonvision.common.vision.opencv.Releasable;
import org.opencv.core.Mat;
public class Frame implements Releasable {
public final long timestampNanos;
public final CVMat image;
public final FrameStaticProperties frameStaticProperties;
public Frame(CVMat image, long timestampNanos, FrameStaticProperties frameStaticProperties) {
this.image = image;
this.timestampNanos = timestampNanos;
this.frameStaticProperties = frameStaticProperties;
}
public Frame(CVMat image, FrameStaticProperties frameStaticProperties) {
this(image, System.nanoTime(), frameStaticProperties);
}
public void copyTo(Mat destMat) {
image.getMat().copyTo(destMat);
}
public static Frame copyFrom(Frame frame) {
Mat newMat = new Mat();
frame.image.getMat().copyTo(newMat);
frame.release();
return new Frame(new CVMat(newMat), frame.timestampNanos, frame.frameStaticProperties);
}
@Override
public void release() {
image.release();
}
}

5
photon-server/src/main/java/org/photonvision/common/vision/frame/FrameConsumer.java
View File

@@ -1,5 +0,0 @@
package org.photonvision.common.vision.frame;
import java.util.function.Consumer;
public interface FrameConsumer extends Consumer<Frame> {}

14
photon-server/src/main/java/org/photonvision/common/vision/frame/FrameDivisor.java
View File

@@ -1,14 +0,0 @@
package org.photonvision.common.vision.frame;
public enum FrameDivisor {
NONE(1),
HALF(2),
QUARTER(4),
SIXTH(6);
public final Integer value;
FrameDivisor(int value) {
this.value = value;
}
}

7
photon-server/src/main/java/org/photonvision/common/vision/frame/FrameProvider.java
View File

@@ -1,7 +0,0 @@
package org.photonvision.common.vision.frame;
import java.util.function.Supplier;
public interface FrameProvider extends Supplier<Frame> {
String getName();
}

64
photon-server/src/main/java/org/photonvision/common/vision/frame/FrameStaticProperties.java
View File

@@ -1,64 +0,0 @@
package org.photonvision.common.vision.frame;
import edu.wpi.cscore.VideoMode;
import org.apache.commons.math3.fraction.Fraction;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.Point;
/** Represents the properties of a frame. */
public class FrameStaticProperties {
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;
/**
* Instantiates a new Frame static properties.
*
* @param mode The Video Mode of the camera.
* @param fov The fov of the image.
*/
public FrameStaticProperties(VideoMode mode, double fov) {
this(mode.width, mode.height, fov);
}
/**
* Instantiates a new Frame static properties.
*
* @param imageWidth The width of the image.
* @param imageHeight The width of the image.
* @param fov The fov of the image.
*/
public FrameStaticProperties(int imageWidth, int imageHeight, double fov) {
this.imageWidth = imageWidth;
this.imageHeight = imageHeight;
this.fov = fov;
imageArea = this.imageWidth * this.imageHeight;
centerX = ((double) this.imageWidth / 2) - 0.5;
centerY = ((double) this.imageHeight / 2) - 0.5;
centerPoint = new Point(centerX, centerY);
// pinhole model calculations
double diagonalView = FastMath.toRadians(this.fov);
Fraction aspectFraction = new Fraction(this.imageWidth, this.imageHeight);
int horizontalRatio = aspectFraction.getNumerator();
int verticalRatio = aspectFraction.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 = this.imageWidth / (2 * FastMath.tan(horizontalView / 2));
verticalFocalLength = this.imageHeight / (2 * FastMath.tan(verticalView / 2));
}
}

11
photon-server/src/main/java/org/photonvision/common/vision/frame/consumer/DummyFrameConsumer.java
View File

@@ -1,11 +0,0 @@
package org.photonvision.common.vision.frame.consumer;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameConsumer;
public class DummyFrameConsumer implements FrameConsumer {
@Override
public void accept(Frame frame) {
frame.release(); // lol ez
}
}

12
photon-server/src/main/java/org/photonvision/common/vision/frame/consumer/MJPGFrameConsumer.java
View File

@@ -1,12 +0,0 @@
package org.photonvision.common.vision.frame.consumer;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameConsumer;
import org.apache.commons.lang3.NotImplementedException;
public class MJPGFrameConsumer implements FrameConsumer {
@Override
public void accept(Frame frame) {
throw new NotImplementedException("");
}
}

98
photon-server/src/main/java/org/photonvision/common/vision/frame/provider/FileFrameProvider.java
View File

@@ -1,98 +0,0 @@
package org.photonvision.common.vision.frame.provider;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameProvider;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import org.photonvision.common.vision.opencv.CVMat;
import java.nio.file.Files;
import java.nio.file.Path;
import java.nio.file.Paths;
import org.opencv.core.Mat;
import org.opencv.imgcodecs.Imgcodecs;
/**
* A {@link FrameProvider} that will read and provide an image from a {@link java.nio.file.Path
* path}.
*/
public class FileFrameProvider implements FrameProvider {
private static int count = 0;
private Frame m_frame;
private final Path m_path;
private final double m_fov;
private boolean m_reloadImage;
/**
* Instantiates a new FileFrameProvider.
*
* @param path The path of the image to read from.
* @param fov The fov of the image.
*/
public FileFrameProvider(Path path, double fov) {
if (!Files.exists(path))
throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath().toString());
m_path = path;
m_fov = fov;
loadImage();
}
/**
* Instantiates a new File frame provider.
*
* @param pathAsString The path of the image to read from as a string.
* @param fov The fov of the image.
*/
public FileFrameProvider(String pathAsString, double fov) {
this(Paths.get(pathAsString), fov);
}
private void loadImage() {
Mat image = Imgcodecs.imread(m_path.toString());
if (image.cols() > 0 && image.rows() > 0) {
FrameStaticProperties m_properties =
new FrameStaticProperties(image.width(), image.height(), m_fov);
m_frame = new Frame(new CVMat(image), m_properties);
} else {
throw new RuntimeException("Image loading failed!");
}
}
/**
* Set image reloading. If true this will reload the image from the path set in the constructor
* every time {@link FileFrameProvider#get()} is called.
*
* @param reloadImage True to enable image reloading.
*/
public void setImageReloading(boolean reloadImage) {
m_reloadImage = reloadImage;
}
/**
* Returns if image reloading is enabled.
*
* @return True if image reloading is enabled.
*/
public boolean isImageReloading() {
return m_reloadImage;
}
@Override
public Frame get() {
if (m_reloadImage) {
if (m_frame != null) m_frame.release();
m_frame = null;
loadImage();
}
return m_frame;
}
@Override
public String getName() {
return "FileFrameProvider" + count++ + " - " + m_path.getFileName();
}
}

19
photon-server/src/main/java/org/photonvision/common/vision/frame/provider/NetworkFrameProvider.java
View File

@@ -1,19 +0,0 @@
package org.photonvision.common.vision.frame.provider;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameProvider;
import org.apache.commons.lang3.NotImplementedException;
public class NetworkFrameProvider implements FrameProvider {
private int count = 0;
@Override
public Frame get() {
throw new NotImplementedException("");
}
@Override
public String getName() {
return "NetworkFrameProvider" + count++;
}
}

34
photon-server/src/main/java/org/photonvision/common/vision/frame/provider/USBFrameProvider.java
View File

@@ -1,34 +0,0 @@
package org.photonvision.common.vision.frame.provider;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameProvider;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import org.photonvision.common.vision.opencv.CVMat;
import edu.wpi.cscore.CvSink;
public class USBFrameProvider implements FrameProvider {
private static int count = 0;
private CvSink cvSink;
private FrameStaticProperties frameStaticProperties;
private CVMat mat;
public USBFrameProvider(CvSink sink, FrameStaticProperties frameStaticProperties) {
cvSink = sink;
this.frameStaticProperties = frameStaticProperties;
mat = new CVMat();
}
@Override
public Frame get() {
if (mat != null && mat.getMat() != null) {
mat.release();
}
long time = cvSink.grabFrame(mat.getMat());
return new Frame(mat, time, frameStaticProperties);
}
@Override
public String getName() {
return "USBFrameProvider" + count++;
}
}

54
photon-server/src/main/java/org/photonvision/common/vision/opencv/CVMat.java
View File

@@ -1,54 +0,0 @@
package org.photonvision.common.vision.opencv;
import org.photonvision.common.util.ReflectionUtils;
import java.util.HashSet;
import org.opencv.core.Mat;
public class CVMat implements Releasable {
private static final HashSet<Mat> allMats = new HashSet<>();
private static boolean shouldPrint;
private final Mat mat;
public CVMat() {
this.mat = new Mat();
}
public void copyTo(CVMat srcMat) {
copyTo(srcMat.getMat());
}
public void copyTo(Mat srcMat) {
srcMat.copyTo(mat);
}
public CVMat(Mat mat) {
this.mat = mat;
if (allMats.add(mat) && shouldPrint) {
System.out.println(
"(CVMat) Added new Mat (count: "
+ allMats.size()
+ ") from: "
+ ReflectionUtils.getNthCaller(3));
}
}
@Override
public void release() {
allMats.remove(mat);
mat.release();
}
public Mat getMat() {
return mat;
}
public static int getMatCount() {
return allMats.size();
}
public static void enablePrint(boolean enabled) {
shouldPrint = enabled;
}
}

62
photon-server/src/main/java/org/photonvision/common/vision/opencv/CVShape.java
View File

@@ -1,62 +0,0 @@
package org.photonvision.common.vision.opencv;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.imgproc.Imgproc;
public class CVShape {
public final Contour contour;
public final ContourShape shape;
private MatOfPoint3f customTarget = null;
private MatOfPoint2f approxCurve = new MatOfPoint2f();
public CVShape(Contour contour, ContourShape shape) {
this.contour = contour;
this.shape = shape;
}
public CVShape(Contour contour, MatOfPoint3f targetPoints) {
this.contour = contour;
this.shape = ContourShape.Custom;
customTarget = targetPoints;
}
public Contour getContour() {
return contour;
}
public MatOfPoint2f getApproxPolyDp(double epsilon, boolean closed) {
approxCurve.release();
approxCurve = new MatOfPoint2f();
Imgproc.approxPolyDP(contour.getMat2f(), approxCurve, epsilon, closed);
return approxCurve;
}
public MatOfPoint2f getApproxPolyDpConvex(double epsilon, boolean closed) {
approxCurve.release();
approxCurve = new MatOfPoint2f();
Imgproc.approxPolyDP(contour.getConvexHull(), approxCurve, epsilon, closed);
return approxCurve;
}
boolean approxPolyMatchesShape() {
var pointList = approxCurve.toList();
// TODO: @Matt
switch (shape) {
case Custom:
break;
case Circle:
break;
case Triangle:
break;
case Quadrilateral:
break;
}
return true;
}
}

198
photon-server/src/main/java/org/photonvision/common/vision/opencv/Contour.java
View File

@@ -1,198 +0,0 @@
package org.photonvision.common.vision.opencv;
import org.photonvision.common.util.math.MathUtils;
import java.util.Comparator;
import org.opencv.core.CvType;
import org.opencv.core.MatOfInt;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
public class Contour implements Releasable {
public static final Comparator<Contour> SortByMomentsX =
Comparator.comparingDouble(
(contour) -> contour.getMoments().get_m10() / contour.getMoments().get_m00());
public final MatOfPoint mat;
private Double area = Double.NaN;
private Double perimeter = Double.NaN;
private MatOfPoint2f mat2f = null;
private RotatedRect minAreaRect = null;
private Rect boundingRect = null;
private Moments moments = null;
private MatOfPoint2f convexHull = null;
public Contour(MatOfPoint mat) {
this.mat = mat;
}
public MatOfPoint2f getMat2f() {
if (mat2f == null) {
mat2f = new MatOfPoint2f(mat.toArray());
mat.convertTo(mat2f, CvType.CV_32F);
}
return mat2f;
}
public MatOfPoint2f getConvexHull() {
if (this.convexHull == null) {
var ints = new MatOfInt();
Imgproc.convexHull(mat, ints);
this.convexHull = Contour.convertIndexesToPoints(mat, ints);
ints.release();
}
return convexHull;
}
public double getArea() {
if (Double.isNaN(area)) {
area = Imgproc.contourArea(mat);
}
return area;
}
public double getPerimeter() {
if (Double.isNaN(perimeter)) {
perimeter = Imgproc.arcLength(getMat2f(), true);
}
return perimeter;
}
public RotatedRect getMinAreaRect() {
if (minAreaRect == null) {
minAreaRect = Imgproc.minAreaRect(getMat2f());
}
return minAreaRect;
}
public Rect getBoundingRect() {
if (boundingRect == null) {
boundingRect = Imgproc.boundingRect(mat);
}
return boundingRect;
}
public Moments getMoments() {
if (moments == null) {
moments = Imgproc.moments(mat);
}
return moments;
}
public Point getCenterPoint() {
return getMinAreaRect().center;
}
public boolean isEmpty() {
return mat.empty();
}
public boolean isIntersecting(
Contour secondContour, ContourIntersectionDirection intersectionDirection) {
boolean isIntersecting = false;
if (intersectionDirection == ContourIntersectionDirection.None) {
isIntersecting = true;
} else {
try {
MatOfPoint2f intersectMatA = new MatOfPoint2f();
MatOfPoint2f intersectMatB = new MatOfPoint2f();
mat.convertTo(intersectMatA, CvType.CV_32F);
secondContour.mat.convertTo(intersectMatB, CvType.CV_32F);
RotatedRect a = Imgproc.fitEllipse(intersectMatA);
RotatedRect b = Imgproc.fitEllipse(intersectMatB);
double mA = MathUtils.toSlope(a.angle);
double mB = MathUtils.toSlope(b.angle);
double x0A = a.center.x;
double y0A = a.center.y;
double x0B = b.center.x;
double y0B = b.center.y;
double intersectionX = ((mA * x0A) - y0A - (mB * x0B) + y0B) / (mA - mB);
double intersectionY = (mA * (intersectionX - x0A)) + y0A;
double massX = (x0A + x0B) / 2;
double massY = (y0A + y0B) / 2;
switch (intersectionDirection) {
case Up:
if (intersectionY < massY) isIntersecting = true;
break;
case Down:
if (intersectionY > massY) isIntersecting = true;
break;
case Left:
if (intersectionX < massX) isIntersecting = true;
break;
case Right:
if (intersectionX > massX) isIntersecting = true;
break;
}
intersectMatA.release();
intersectMatB.release();
} catch (Exception e) {
// defaults to false
}
}
return isIntersecting;
}
// TODO: refactor to do "infinite" contours ???????
public static Contour groupContoursByIntersection(
Contour firstContour, Contour secondContour, ContourIntersectionDirection intersection) {
if (areIntersecting(firstContour, secondContour, intersection)) {
return combineContours(firstContour, secondContour);
} else {
return null;
}
}
public static boolean areIntersecting(
Contour firstContour,
Contour secondContour,
ContourIntersectionDirection intersectionDirection) {
return firstContour.isIntersecting(secondContour, intersectionDirection)
|| secondContour.isIntersecting(firstContour, intersectionDirection);
}
private static Contour combineContours(Contour... contours) {
var points = new MatOfPoint();
for (var contour : contours) {
points.push_back(contour.mat);
}
var finalContour = new Contour(points);
boolean contourEmpty = finalContour.isEmpty();
return contourEmpty ? null : finalContour;
}
@Override
public void release() {
if (mat != null) mat.release();
if (mat2f != null) mat2f.release();
if (convexHull != null) convexHull.release();
}
public static MatOfPoint2f convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
int[] arrIndex = indexes.toArray();
Point[] arrContour = contour.toArray();
Point[] arrPoints = new Point[arrIndex.length];
for (int i = 0; i < arrIndex.length; i++) {
arrPoints[i] = arrContour[arrIndex[i]];
}
var hull = new MatOfPoint2f();
hull.fromArray(arrPoints);
return hull;
}
}

12
photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourGroupingMode.java
View File

@@ -1,12 +0,0 @@
package org.photonvision.common.vision.opencv;
public enum ContourGroupingMode {
Single(1),
Dual(2);
public final int count;
ContourGroupingMode(int count) {
this.count = count;
}
}

9
photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourIntersectionDirection.java
View File

@@ -1,9 +0,0 @@
package org.photonvision.common.vision.opencv;
public enum ContourIntersectionDirection {
None,
Up,
Down,
Left,
Right
}

29
photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourShape.java
View File

@@ -1,29 +0,0 @@
package org.photonvision.common.vision.opencv;
import java.util.EnumSet;
import java.util.HashMap;
public enum ContourShape {
Custom(-1),
Circle(0),
Triangle(3),
Quadrilateral(4);
public final int sides;
ContourShape(int sides) {
this.sides = sides;
}
private static final HashMap<Integer, ContourShape> sidesToValueMap = new HashMap<>();
static {
for (var value : EnumSet.allOf(ContourShape.class)) {
sidesToValueMap.put(value.sides, value);
}
}
public static ContourShape fromSides(int sides) {
return sidesToValueMap.get(sides);
}
}

29
photon-server/src/main/java/org/photonvision/common/vision/opencv/ContourSortMode.java
View File

@@ -1,29 +0,0 @@
package org.photonvision.common.vision.opencv;
import org.photonvision.common.vision.target.PotentialTarget;
import java.util.Comparator;
import org.apache.commons.math3.util.FastMath;
public enum ContourSortMode {
Largest(Comparator.comparingDouble(PotentialTarget::getArea)),
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(
Comparator.comparingDouble(
rect ->
(FastMath.pow(rect.getMinAreaRect().center.y, 2)
+ FastMath.pow(rect.getMinAreaRect().center.x, 2))));
private Comparator<PotentialTarget> m_comparator;
ContourSortMode(Comparator<PotentialTarget> comparator) {
m_comparator = comparator;
}
public Comparator<PotentialTarget> getComparator() {
return m_comparator;
}
}

8
photon-server/src/main/java/org/photonvision/common/vision/opencv/DualMat.java
View File

@@ -1,8 +0,0 @@
package org.photonvision.common.vision.opencv;
import org.opencv.core.Mat;
public class DualMat {
public Mat first;
public Mat second;
}

5
photon-server/src/main/java/org/photonvision/common/vision/opencv/Releasable.java
View File

@@ -1,5 +0,0 @@
package org.photonvision.common.vision.opencv;
public interface Releasable {
void release();
}

41
photon-server/src/main/java/org/photonvision/common/vision/pipe/CVPipe.java
View File

@@ -1,41 +0,0 @@
package org.photonvision.common.vision.pipe;
import java.util.function.Function;
/**
* Defines a pipe. A pipe is a single step in a pipeline. This class is to be extended, never used
* on its own.
*
* @param <I> Input type for the pipe
* @param <O> Output type for the pipe
* @param <P> Parameters type for the pipe
*/
public abstract class CVPipe<I, O, P> implements Function<I, CVPipeResult<O>> {
protected CVPipeResult<O> result = new CVPipeResult<>();
protected P params;
public void setParams(P params) {
this.params = params;
}
/**
* Runs the process for the pipe.
*
* @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.
*/
@Override
public CVPipeResult<O> apply(I in) {
long pipeStartNanos = System.nanoTime();
result.result = process(in);
result.nanosElapsed = System.nanoTime() - pipeStartNanos;
return result;
}
}

6
photon-server/src/main/java/org/photonvision/common/vision/pipe/CVPipeResult.java
View File

@@ -1,6 +0,0 @@
package org.photonvision.common.vision.pipe;
public class CVPipeResult<O> {
public O result;
public long nanosElapsed;
}

14
photon-server/src/main/java/org/photonvision/common/vision/pipe/ImageFlipMode.java
View File

@@ -1,14 +0,0 @@
package org.photonvision.common.vision.pipe;
public enum ImageFlipMode {
NONE(Integer.MIN_VALUE),
VERTICAL(1),
HORIZONTAL(0),
BOTH(-1);
public final int value;
ImageFlipMode(int value) {
this.value = value;
}
}

18
photon-server/src/main/java/org/photonvision/common/vision/pipe/ImageRotationMode.java
View File

@@ -1,18 +0,0 @@
package org.photonvision.common.vision.pipe;
public enum ImageRotationMode {
DEG_0(-1),
DEG_90(0),
DEG_180(1),
DEG_270(2);
public final int value;
ImageRotationMode(int value) {
this.value = value;
}
public boolean isRotated() {
return this.value == DEG_90.value || this.value == DEG_270.value;
}
}

47
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/BlurPipe.java
View File

@@ -1,47 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.pipe.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<Mat, Mat, BlurPipe.BlurParams> {
/**
* 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);
}
}
}

99
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Collect2dTargetsPipe.java
View File

@@ -1,99 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import org.photonvision.common.vision.pipe.CVPipe;
import com.chameleonvision.common.vision.target.*;
import java.util.ArrayList;
import java.util.List;
import org.opencv.core.Point;
import org.photonvision.common.vision.target.*;
/** Represents a pipe that collects available 2d targets. */
public class Collect2dTargetsPipe
extends CVPipe<
List<PotentialTarget>, List<TrackedTarget>, Collect2dTargetsPipe.Collect2dTargetsParams> {
/**
* Processes this pipeline.
*
* @param in Input for pipe processing.
* @return A list of tracked targets.
*/
@Override
protected List<TrackedTarget> process(List<PotentialTarget> in) {
List<TrackedTarget> targets = new ArrayList<>();
var calculationParams =
new TrackedTarget.TargetCalculationParameters(
params.getOrientation() == TargetOrientation.Landscape,
params.getOffsetPointRegion(),
params.getUserOffsetPoint(),
params.getFrameStaticProperties().centerPoint,
new DoubleCouple(params.getCalibrationB(), params.getCalibrationM()),
params.getOffsetMode(),
params.getFrameStaticProperties().horizontalFocalLength,
params.getFrameStaticProperties().verticalFocalLength,
params.getFrameStaticProperties().imageArea);
for (PotentialTarget target : in) {
targets.add(new TrackedTarget(target, calculationParams));
}
return targets;
}
public static class Collect2dTargetsParams {
private FrameStaticProperties m_captureStaticProperties;
private RobotOffsetPointMode m_offsetMode;
private double m_calibrationM, m_calibrationB;
private Point m_userOffsetPoint;
private TargetOffsetPointEdge m_region;
private TargetOrientation m_orientation;
public Collect2dTargetsParams(
FrameStaticProperties captureStaticProperties,
RobotOffsetPointMode offsetMode,
double calibrationM,
double calibrationB,
Point calibrationPoint,
TargetOffsetPointEdge region,
TargetOrientation orientation) {
m_captureStaticProperties = captureStaticProperties;
m_offsetMode = offsetMode;
m_calibrationM = calibrationM;
m_calibrationB = calibrationB;
m_userOffsetPoint = calibrationPoint;
m_region = region;
m_orientation = orientation;
}
public FrameStaticProperties getFrameStaticProperties() {
return m_captureStaticProperties;
}
public RobotOffsetPointMode getOffsetMode() {
return m_offsetMode;
}
public double getCalibrationM() {
return m_calibrationM;
}
public double getCalibrationB() {
return m_calibrationB;
}
public Point getUserOffsetPoint() {
return m_userOffsetPoint;
}
public TargetOffsetPointEdge getOffsetPointRegion() {
return m_region;
}
public TargetOrientation getOrientation() {
return m_orientation;
}
}
}

213
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/CornerDetectionPipe.java
View File

@@ -1,213 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.target.TrackedTarget;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.Comparator;
import java.util.List;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.imgproc.Imgproc;
public class CornerDetectionPipe
extends CVPipe<
List<TrackedTarget>,
List<TrackedTarget>,
CornerDetectionPipe.CornerDetectionPipeParameters> {
Comparator<Point> leftRightComparator = Comparator.comparingDouble(point -> point.x);
Comparator<Point> verticalComparator = Comparator.comparingDouble(point -> point.y);
MatOfPoint2f polyOutput = new MatOfPoint2f();
@Override
protected List<TrackedTarget> process(List<TrackedTarget> targetList) {
for (var target : targetList) {
// detect corners. Might implement more algorithms later but
// APPROX_POLY_DP_AND_EXTREME_CORNERS should be year agnostic
switch (params.cornerDetectionStrategy) {
case APPROX_POLY_DP_AND_EXTREME_CORNERS:
{
var targetCorners =
detectExtremeCornersByApproxPolyDp(target, params.calculateConvexHulls);
target.setCorners(targetCorners);
break;
}
default:
{
break;
}
}
}
return targetList;
}
/**
* @param target the target to find the corners of.
* @return the corners. left top, left bottom, right bottom, right top
*/
private List<Point> findBoundingBoxCorners(TrackedTarget target) {
// extract the corners
var points = new Point[4];
target.m_mainContour.getMinAreaRect().points(points);
// find the tl/tr/bl/br corners
// first, min by left/right
var list_ = Arrays.asList(points);
list_.sort(leftRightComparator);
// of this, we now have left and right
// sort to get top and bottom
var left = new ArrayList<>(List.of(list_.get(0), list_.get(1)));
left.sort(verticalComparator);
var right = new ArrayList<>(List.of(list_.get(2), list_.get(3)));
right.sort(verticalComparator);
// tl tr bl br
var tl = left.get(0);
var bl = left.get(1);
var tr = right.get(0);
var br = right.get(1);
return List.of(tl, bl, br, tr);
}
/**
* @param a First point.
* @param b Second point.
* @return The straight line distance between them.
*/
private static double distanceBetween(Point a, Point b) {
return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2));
}
/**
* @param a First point.
* @param b Second point.
* @return The straight line distance between them.
*/
private static double distanceBetween(Translation2d a, Translation2d b) {
return FastMath.sqrt(
FastMath.pow(a.getX() - b.getX(), 2) + FastMath.pow(a.getY() - b.getY(), 2));
}
/**
* Find the 4 most extreme corners,
*
* @param target the target to track.
* @param convexHull weather to use the convex hull of the target.
* @return the 4 extreme corners of the contour.
*/
private List<Point> detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) {
var centroid = target.getMinAreaRect().center;
Comparator<Point> distanceProvider =
Comparator.comparingDouble(
(Point point) ->
FastMath.sqrt(
FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2)));
MatOfPoint2f targetContour;
if (convexHull) {
targetContour = target.m_mainContour.getConvexHull();
} else {
targetContour = target.m_mainContour.getMat2f();
}
/*
approximating a shape around the contours
Can be tuned to allow/disallow hulls
we want a number between 0 and 0.16 out of a percentage from 0 to 100
so take accuracy and divide by 600
Furthermore, we know that the contour is open if we haven't done convex hulls
and it has subcontours.
*/
var isOpen = !convexHull && target.hasSubContours();
var peri = Imgproc.arcLength(targetContour, true);
Imgproc.approxPolyDP(
targetContour, polyOutput, params.accuracyPercentage / 600.0 * peri, !isOpen);
// we must have at least 4 corners for this strategy to work.
// If we are looking for an exact side count that is handled here too.
var pointList = new ArrayList<>(polyOutput.toList());
if (pointList.size() < 4 || (params.exactSideCount && params.sideCount != pointList.size()))
return null;
target.setApproximateBoundingPolygon(polyOutput);
// left top, left bottom, right bottom, right top
var boundingBoxCorners = findBoundingBoxCorners(target);
var distanceToTlComparator =
Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(0)));
var distanceToTrComparator =
Comparator.comparingDouble((Point p) -> distanceBetween(p, boundingBoxCorners.get(3)));
// top left and top right are the poly corners closest to the bouding box tl and tr
pointList.sort(distanceToTlComparator);
var tl = pointList.get(0);
pointList.remove(tl);
pointList.sort(distanceToTrComparator);
var tr = pointList.get(0);
pointList.remove(tr);
// at this point we look for points on the left/right of the center of the remaining points
// and maximize their distance from the center of the min area rectangle
var leftList = new ArrayList<Point>();
var rightList = new ArrayList<Point>();
var averageXCoordinate = 0;
for (var p : pointList) {
averageXCoordinate += p.x;
}
averageXCoordinate /= pointList.size();
// add points that are below the center of the min area rectangle of the target
for (var p : pointList) {
if (p.y
> target.m_mainContour.getBoundingRect().y
+ target.m_mainContour.getBoundingRect().height / 2.0)
if (p.x < averageXCoordinate) {
leftList.add(p);
} else {
rightList.add(p);
}
}
if (leftList.isEmpty() || rightList.isEmpty()) return null;
leftList.sort(distanceProvider);
rightList.sort(distanceProvider);
var bl = leftList.get(leftList.size() - 1);
var br = rightList.get(rightList.size() - 1);
System.out.printf("Found points: TL (%s) BL (%s) BR (%s) TR (%s)\n", tl, bl, br, tr);
return List.of(tl, bl, br, tr);
}
public static class CornerDetectionPipeParameters {
private final DetectionStrategy cornerDetectionStrategy;
private final boolean calculateConvexHulls;
private final boolean exactSideCount;
private final int sideCount;
/** This number can be changed to change how "accurate" our approximate polygon must be. */
private final double accuracyPercentage;
public CornerDetectionPipeParameters(
DetectionStrategy cornerDetectionStrategy,
boolean calculateConvexHulls,
boolean exactSideCount,
int sideCount,
double accuracyPercentage) {
this.cornerDetectionStrategy = cornerDetectionStrategy;
this.calculateConvexHulls = calculateConvexHulls;
this.exactSideCount = exactSideCount;
this.sideCount = sideCount;
this.accuracyPercentage = accuracyPercentage;
}
}
public enum DetectionStrategy {
APPROX_POLY_DP_AND_EXTREME_CORNERS
}
}

102
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Draw2dContoursPipe.java
View File

@@ -1,102 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.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.*;
import org.opencv.imgproc.Imgproc;
public class Draw2dContoursPipe
extends CVPipe<Pair<Mat, List<TrackedTarget>>, Mat, Draw2dContoursPipe.Draw2dContoursParams> {
private List<MatOfPoint> m_drawnContours = new ArrayList<>();
@Override
protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
if (!in.getRight().isEmpty()
&& (params.showCentroid
|| params.showMaximumBox
|| params.showRotatedBox
|| params.showShape)) {
var centroidColour = ColorHelper.colorToScalar(params.centroidColor);
var maximumBoxColour = ColorHelper.colorToScalar(params.maximumBoxColor);
var rotatedBoxColour = ColorHelper.colorToScalar(params.rotatedBoxColor);
var shapeColour = ColorHelper.colorToScalar(params.shapeOutlineColour);
for (int i = 0; i < (params.showMultiple ? in.getRight().size() : 1); 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, rotatedBoxColour, 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),
maximumBoxColour,
params.boxOutlineSize);
}
if (params.showShape) {
Imgproc.drawContours(
in.getLeft(),
List.of(target.m_mainContour.mat),
-1,
shapeColour,
params.boxOutlineSize);
}
if (params.showCentroid) {
Imgproc.circle(in.getLeft(), target.getTargetOffsetPoint(), 3, centroidColour, 2);
}
}
}
return in.getLeft();
}
public static class Draw2dContoursParams {
public boolean showCentroid = true;
public boolean showMultiple = true;
public int boxOutlineSize = 1;
public boolean showRotatedBox = true;
public boolean showShape = false;
public boolean showMaximumBox = true;
public Color centroidColor = Color.GREEN;
public Color rotatedBoxColor = Color.BLUE;
public Color maximumBoxColor = Color.RED;
public Color shapeOutlineColour = Color.MAGENTA;
// TODO: set other params from UI/settings file?
public Draw2dContoursParams(boolean showMultipleTargets) {
this.showMultiple = showMultipleTargets;
}
}
}

62
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Draw2dCrosshairPipe.java
View File

@@ -1,62 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.target.RobotOffsetPointMode;
import org.photonvision.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<Pair<Mat, List<TrackedTarget>>, Mat, Draw2dCrosshairPipe.Draw2dCrosshairParams> {
@Override
protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
Mat image = in.getLeft();
if (params.m_showCrosshair) {
double x = image.cols() / 2.0;
double y = image.rows() / 2.0;
double scale = image.cols() / 32.0;
switch (params.m_calibrationMode) {
case Single:
if (!params.m_calibrationPoint.isEmpty()) {
x = params.m_calibrationPoint.getFirst();
y = params.m_calibrationPoint.getSecond();
}
break;
case Dual:
// TODO: draw crosshair based on dual calibration
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(image, xMax, xMin, ColorHelper.colorToScalar(params.m_crosshairColor));
Imgproc.line(image, yMax, yMin, ColorHelper.colorToScalar(params.m_crosshairColor));
}
return image;
}
public static class Draw2dCrosshairParams {
private RobotOffsetPointMode m_calibrationMode;
private DoubleCouple m_calibrationPoint;
public boolean m_showCrosshair = true;
public Color m_crosshairColor = Color.GREEN;
public Draw2dCrosshairParams(
RobotOffsetPointMode calibrationMode, DoubleCouple calibrationPoint) {
m_calibrationMode = calibrationMode;
m_calibrationPoint = calibrationPoint;
}
}
}

122
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java
View File

@@ -1,122 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.calibration.CameraCalibrationCoefficients;
import org.photonvision.common.util.ColorHelper;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.target.TargetModel;
import org.photonvision.common.vision.target.TrackedTarget;
import java.awt.*;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.imgproc.Imgproc;
public class Draw3dTargetsPipe
extends CVPipe<Pair<Mat, List<TrackedTarget>>, Mat, Draw3dTargetsPipe.Draw3dContoursParams> {
@Override
protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
for (var target : in.getRight()) {
// draw convex hull
var pointMat = new MatOfPoint();
target.m_mainContour.getConvexHull().convertTo(pointMat, CvType.CV_32S);
Imgproc.drawContours(
in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.green), 1);
// draw approximate polygon
var poly = target.getApproximateBoundingPolygon();
if (poly != null) {
poly.convertTo(pointMat, CvType.CV_32S);
Imgproc.drawContours(
in.getLeft(), List.of(pointMat), -1, ColorHelper.colorToScalar(Color.blue), 2);
}
// Draw floor and top
if (target.getCameraRelativeRvec() != null && target.getCameraRelativeTvec() != null) {
var tempMat = new MatOfPoint2f();
var jac = new Mat();
var bottomModel = params.targetModel.getVisualizationBoxBottom();
var topModel = params.targetModel.getVisualizationBoxTop();
Calib3d.projectPoints(
bottomModel,
target.getCameraRelativeRvec(),
target.getCameraRelativeTvec(),
params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
tempMat,
jac);
var bottomPoints = tempMat.toList();
Calib3d.projectPoints(
topModel,
target.getCameraRelativeRvec(),
target.getCameraRelativeTvec(),
params.cameraCalibrationCoefficients.getCameraIntrinsicsMat(),
params.cameraCalibrationCoefficients.getCameraExtrinsicsMat(),
tempMat,
jac);
var topPoints = tempMat.toList();
// floor, then pillers, then top
for (int i = 0; i < bottomPoints.size(); i++) {
Imgproc.line(
in.getLeft(),
bottomPoints.get(i),
bottomPoints.get((i + 1) % (bottomPoints.size())),
ColorHelper.colorToScalar(Color.green),
3);
}
for (int i = 0; i < bottomPoints.size(); i++) {
Imgproc.line(
in.getLeft(),
bottomPoints.get(i),
topPoints.get(i),
ColorHelper.colorToScalar(Color.blue),
3);
}
for (int i = 0; i < topPoints.size(); i++) {
Imgproc.line(
in.getLeft(),
topPoints.get(i),
topPoints.get((i + 1) % (bottomPoints.size())),
ColorHelper.colorToScalar(Color.orange),
3);
}
jac.release();
}
pointMat.release();
// draw corners
var corners = target.getTargetCorners();
if (corners != null && !corners.isEmpty()) {
for (var corner : corners) {
Imgproc.circle(
in.getLeft(),
corner,
params.radius,
ColorHelper.colorToScalar(params.color),
params.radius);
}
}
}
return in.getLeft();
}
public static class Draw3dContoursParams {
private final int radius = 2;
private final Color color = Color.RED;
private final TargetModel targetModel;
private final CameraCalibrationCoefficients cameraCalibrationCoefficients;
public Draw3dContoursParams(
CameraCalibrationCoefficients cameraCalibrationCoefficients, TargetModel targetModel) {
this.cameraCalibrationCoefficients = cameraCalibrationCoefficients;
this.targetModel = targetModel;
}
}
}

32
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/DrawCornerDetectionPipe.java
View File

@@ -1,32 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.target.TrackedTarget;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
public class DrawCornerDetectionPipe
extends CVPipe<Pair<Mat, List<TrackedTarget>>, Mat, DrawCornerDetectionPipe.DrawCornerParams> {
@Override
protected Mat process(Pair<Mat, List<TrackedTarget>> in) {
Mat image = in.getLeft();
for (var target : in.getRight()) {
var corners = target.getTargetCorners();
for (var corner : corners) {
Imgproc.circle(image, corner, params.dotRadius, params.dotColor);
}
}
return image;
}
public static class DrawCornerParams {
int dotRadius;
Scalar dotColor;
}
}

44
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/ErodeDilatePipe.java
View File

@@ -1,44 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.pipe.CVPipe;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
public class ErodeDilatePipe extends CVPipe<Mat, Mat, ErodeDilatePipe.ErodeDilateParams> {
@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;
}
}
}

88
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FilterContoursPipe.java
View File

@@ -1,88 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.pipe.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<Contour>, List<Contour>, FilterContoursPipe.FilterContoursParams> {
List<Contour> m_filteredContours = new ArrayList<>();
@Override
protected List<Contour> process(List<Contour> 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 FrameStaticProperties m_camProperties;
public FilterContoursParams(
DoubleCouple area,
DoubleCouple ratio,
DoubleCouple extent,
FrameStaticProperties 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 FrameStaticProperties getCamProperties() {
return m_camProperties;
}
}
}

45
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FilterShapesPipe.java
View File

@@ -1,45 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.CVShape;
import org.photonvision.common.vision.opencv.ContourShape;
import org.photonvision.common.vision.pipe.CVPipe;
import java.util.List;
public class FilterShapesPipe
extends CVPipe<List<CVShape>, List<CVShape>, FilterShapesPipe.FilterShapesPipeParams> {
/**
* Runs the process for the pipe.
*
* @param in Input for pipe processing.
* @return Result of processing.
*/
@Override
protected List<CVShape> process(List<CVShape> in) {
in.removeIf(
shape ->
shape.shape != params.desiredShape
|| shape.contour.getArea() > params.maxArea
|| shape.contour.getArea() < params.minArea
|| shape.contour.getPerimeter() > params.maxPeri
|| shape.contour.getPerimeter() < params.minPeri);
return in;
}
public static class FilterShapesPipeParams {
ContourShape desiredShape;
double minArea;
double maxArea;
double minPeri;
double maxPeri;
public FilterShapesPipeParams(
ContourShape desiredShape, double minArea, double maxArea, double minPeri, double maxPeri) {
this.desiredShape = desiredShape;
this.minArea = minArea;
this.maxArea = maxArea;
this.minPeri = minPeri;
this.maxPeri = maxPeri;
}
}
}

82
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindCirclesPipe.java
View File

@@ -1,82 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.CVShape;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.opencv.ContourShape;
import org.photonvision.common.vision.pipe.CVPipe;
import java.util.ArrayList;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
public class FindCirclesPipe
extends CVPipe<Pair<Mat, List<Contour>>, List<CVShape>, FindCirclesPipe.FindCirclePipeParams> {
double[] c;
Mat circles = new Mat();
Moments mu;
double x_center;
double y_center;
/**
* Runs the process for the pipe.
*
* @param in Input for pipe processing.
* @return Result of processing.
*/
@Override
protected List<CVShape> process(Pair<Mat, List<Contour>> in) {
circles.release();
List<CVShape> output = new ArrayList<>();
Imgproc.HoughCircles(
in.getLeft(),
circles,
Imgproc.HOUGH_GRADIENT,
1.0,
params.minDist,
params.maxCannyThresh,
params.accuracy,
params.minRadius,
params.maxRadius);
for (int x = 0; x < circles.cols(); x++) {
c = circles.get(0, x);
x_center = c[0];
y_center = c[1];
for (Contour contour : in.getRight()) {
mu = contour.getMoments();
if (Math.abs(x_center - (mu.m10 / mu.m00)) <= params.allowableThreshold
&& Math.abs(y_center - (mu.m01 / mu.m00)) <= params.allowableThreshold) {
output.add(new CVShape(contour, ContourShape.Circle));
}
}
}
return output;
}
public static class FindCirclePipeParams {
public int allowableThreshold;
public int minRadius;
public int maxRadius;
public int minDist;
public int maxCannyThresh;
public int accuracy;
public FindCirclePipeParams(
int allowableThreshold,
int minRadius,
int minDist,
int maxRadius,
int maxCannyThresh,
int accuracy) {
this.allowableThreshold = allowableThreshold;
this.minRadius = minRadius;
this.maxRadius = maxRadius;
this.minDist = minDist;
this.maxCannyThresh = maxCannyThresh;
this.accuracy = accuracy;
}
}
}

31
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindContoursPipe.java
View File

@@ -1,31 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.pipe.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<Mat, List<Contour>, FindContoursPipe.FindContoursParams> {
private List<MatOfPoint> m_foundContours = new ArrayList<>();
@Override
protected List<Contour> 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 {}
}

68
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindPolygonPipe.java
View File

@@ -1,68 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.CVShape;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.opencv.ContourShape;
import org.photonvision.common.vision.pipe.CVPipe;
import java.util.ArrayList;
import java.util.List;
import org.opencv.core.*;
import org.opencv.imgproc.Imgproc;
public class FindPolygonPipe
extends CVPipe<List<Contour>, List<CVShape>, FindPolygonPipe.FindPolygonPipeParams> {
private int corners;
private MatOfPoint2f approx = new MatOfPoint2f();
/*
* Runs the process for the pipe.
*
* @param in Input for pipe processing.
* @return Result of processing.
*/
@Override
protected List<CVShape> process(List<Contour> in) {
// List containing all the output shapes
List<CVShape> output = new ArrayList<>();
for (Contour contour : in) output.add(getShape(contour));
return output;
}
private CVShape getShape(Contour in) {
corners = getCorners(in);
if (ContourShape.fromSides(corners) == null) {
return new CVShape(in, ContourShape.Custom);
}
switch (ContourShape.fromSides(corners)) {
case Circle:
return new CVShape(in, ContourShape.Circle);
case Triangle:
return new CVShape(in, ContourShape.Triangle);
case Quadrilateral:
return new CVShape(in, ContourShape.Quadrilateral);
}
return new CVShape(in, ContourShape.Custom);
}
private int getCorners(Contour contour) {
approx.release();
Imgproc.approxPolyDP(
contour.getMat2f(),
approx,
params.accuracyPercentage / 600.0 * Imgproc.arcLength(contour.getMat2f(), true),
true);
return (int) approx.size().height;
}
public static class FindPolygonPipeParams {
double accuracyPercentage;
public FindPolygonPipeParams(double accuracyPercentage) {
this.accuracyPercentage = accuracyPercentage;
}
}
}

41
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/FindShapesPipe.java
View File

@@ -1,41 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.CVShape;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.opencv.ContourShape;
import org.photonvision.common.vision.pipe.CVPipe;
import java.util.List;
import org.opencv.core.MatOfPoint2f;
import org.opencv.imgproc.Imgproc;
public class FindShapesPipe
extends CVPipe<List<Contour>, List<CVShape>, FindShapesPipe.FindShapesParams> {
MatOfPoint2f approxCurve = new MatOfPoint2f();
@Override
protected List<CVShape> process(List<Contour> in) {
approxCurve.release();
approxCurve = new MatOfPoint2f();
for (var contour : in) {
if (params.desiredShape == ContourShape.Circle) {
} else {
int desiredSides = params.desiredShape.sides;
Imgproc.approxPolyDP(contour.getMat2f(), approxCurve, params.approxEpsilon, true);
// int actualSides = approxCurve.
// switch ()
System.out.println("fugg");
}
}
return List.of();
}
public static class FindShapesParams {
double approxEpsilon = 0.05;
ContourShape desiredShape;
}
}

85
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/GroupContoursPipe.java
View File

@@ -1,85 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.opencv.ContourGroupingMode;
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.target.PotentialTarget;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class GroupContoursPipe
extends CVPipe<List<Contour>, List<PotentialTarget>, GroupContoursPipe.GroupContoursParams> {
private List<PotentialTarget> m_targets = new ArrayList<>();
@Override
protected List<PotentialTarget> process(List<Contour> input) {
for (var target : m_targets) {
target.release();
}
m_targets.clear();
if (params.getGroup() == ContourGroupingMode.Single) {
for (var contour : input) {
m_targets.add(new PotentialTarget(contour));
}
} else {
int groupingCount = params.getGroup().count;
if (input.size() > groupingCount) {
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<Contour> groupingSet;
try {
groupingSet = input.subList(i, i + groupingCount);
} catch (IndexOutOfBoundsException e) {
continue;
}
try {
// 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));
i += (groupingCount - 1);
}
} catch (Exception ex) {
ex.printStackTrace();
}
}
}
}
return m_targets;
}
public static class GroupContoursParams {
private ContourGroupingMode m_group;
private ContourIntersectionDirection m_intersection;
public GroupContoursParams(
ContourGroupingMode group, ContourIntersectionDirection intersectionDirection) {
m_group = group;
m_intersection = intersectionDirection;
}
public ContourGroupingMode getGroup() {
return m_group;
}
public ContourIntersectionDirection getIntersection() {
return m_intersection;
}
}
}

50
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/HSVPipe.java
View File

@@ -1,50 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.common.vision.pipe.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<Mat, Mat, HSVPipe.HSVParams> {
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(IntegerCouple hue, IntegerCouple saturation, IntegerCouple value) {
m_hsvLower = new Scalar(hue.getFirst(), saturation.getFirst(), value.getFirst());
m_hsvUpper = new Scalar(hue.getSecond(), saturation.getSecond(), value.getSecond());
}
public HSVParams(Scalar hsvLower, Scalar hsvUpper) {
m_hsvLower = hsvLower;
m_hsvUpper = hsvUpper;
}
public Scalar getHsvLower() {
return m_hsvLower;
}
public Scalar getHsvUpper() {
return m_hsvUpper;
}
}
}

43
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/OutputMatPipe.java
View File

@@ -1,43 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.DualMat;
import org.photonvision.common.vision.pipe.CVPipe;
import org.opencv.core.CvException;
import org.opencv.core.Mat;
import org.opencv.imgproc.Imgproc;
public class OutputMatPipe extends CVPipe<DualMat, Mat, OutputMatPipe.OutputMatParams> {
private Mat m_outputMat = new Mat();
@Override
protected Mat process(DualMat in) {
Mat rawCam = in.first;
Mat hsv = in.second;
if (params.showThreshold()) {
// convert input mat
try {
hsv.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 {
m_outputMat = rawCam;
}
return m_outputMat;
}
public static class OutputMatParams {
private boolean m_showThreshold;
public OutputMatParams(boolean showThreshold) {
m_showThreshold = showThreshold;
}
public boolean showThreshold() {
return m_showThreshold;
}
}
}

70
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/ResizeImagePipe.java
View File

@@ -1,70 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.frame.FrameDivisor;
import org.photonvision.common.vision.pipe.CVPipe;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
/** Pipe that resizes an image to a given resolution */
public class ResizeImagePipe extends CVPipe<Mat, Mat, ResizeImagePipe.ResizeImageParams> {
public ResizeImagePipe() {
setParams(ResizeImageParams.DEFAULT);
}
public ResizeImagePipe(ResizeImageParams params) {
setParams(params);
}
/**
* Process this pipe
*
* @param in {@link Mat} to be resized
* @return Resized {@link Mat}
*/
@Override
protected Mat process(Mat in) {
// if a divisor is set, use that instead of a size.
if (params.getDivisor() != null) {
int width = in.cols() / params.getDivisor().value;
int height = in.rows() / params.getDivisor().value;
setParams(new ResizeImageParams(width, height));
}
Imgproc.resize(in, in, params.getSize());
return in;
}
public static class ResizeImageParams {
public static ResizeImageParams DEFAULT = new ResizeImageParams(320, 240);
private Size size;
private int width;
private int height;
private FrameDivisor divisor;
public ResizeImageParams() {
this(DEFAULT.width, DEFAULT.height);
}
public ResizeImageParams(int width, int height) {
this.width = width;
this.height = height;
size = new Size(new double[] {width, height});
}
public ResizeImageParams(FrameDivisor divisor) {
this.divisor = divisor;
}
public Size getSize() {
return size;
}
public FrameDivisor getDivisor() {
return divisor;
}
}
}

44
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/RotateImagePipe.java
View File

@@ -1,44 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.pipe.ImageRotationMode;
import org.opencv.core.Core;
import org.opencv.core.Mat;
/** Pipe that rotates an image to a given orientation */
public class RotateImagePipe extends CVPipe<Mat, Mat, RotateImagePipe.RotateImageParams> {
public RotateImagePipe() {
setParams(RotateImageParams.DEFAULT);
}
public RotateImagePipe(RotateImageParams params) {
setParams(params);
}
/**
* Process this pipe
*
* @param in {@link Mat} to be rotated
* @return Rotated {@link Mat}
*/
@Override
protected Mat process(Mat in) {
Core.rotate(in, in, params.rotation.value);
return in;
}
public static class RotateImageParams {
public static RotateImageParams DEFAULT = new RotateImageParams(ImageRotationMode.DEG_0);
public ImageRotationMode rotation;
public RotateImageParams() {
rotation = DEFAULT.rotation;
}
public RotateImageParams(ImageRotationMode rotation) {
this.rotation = rotation;
}
}
}

139
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/SolvePNPPipe.java
View File

@@ -1,139 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.calibration.CameraCalibrationCoefficients;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.target.TargetModel;
import org.photonvision.common.vision.target.TrackedTarget;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import java.util.List;
import org.apache.commons.math3.util.FastMath;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Scalar;
public class SolvePNPPipe
extends CVPipe<List<TrackedTarget>, List<TrackedTarget>, SolvePNPPipe.SolvePNPPipeParams> {
private MatOfPoint2f imagePoints = new MatOfPoint2f();
@Override
protected List<TrackedTarget> process(List<TrackedTarget> targetList) {
for (var target : targetList) {
calculateTargetPose(target);
}
return targetList;
}
private void calculateTargetPose(TrackedTarget target) {
Pose2d targetPose;
var corners = target.getTargetCorners();
if (corners == null
|| corners.isEmpty()
|| params.cameraCoefficients.getCameraIntrinsicsMat() == null
|| params.cameraCoefficients.getCameraExtrinsicsMat() == null) {
targetPose = new Pose2d();
return;
}
this.imagePoints.fromList(corners);
var rVec = new Mat();
var tVec = new Mat();
try {
Calib3d.solvePnP(
params.targetModel.getRealWorldTargetCoordinates(),
imagePoints,
params.cameraCoefficients.getCameraIntrinsicsMat(),
params.cameraCoefficients.getCameraExtrinsicsMat(),
rVec,
tVec);
} catch (Exception e) {
e.printStackTrace();
return;
}
target.setCameraRelativeTvec(tVec);
target.setCameraRelativeRvec(rVec);
targetPose = correctLocationForCameraPitch(tVec, rVec, params.cameraPitchAngle);
target.setRobotRelativePose(targetPose);
}
Mat rotationMatrix = new Mat();
Mat inverseRotationMatrix = new Mat();
Mat pzeroWorld = new Mat();
Mat kMat = new Mat();
Mat scaledTvec;
@SuppressWarnings("DuplicatedCode") // yes I know we have another solvePNP pipe
private Pose2d correctLocationForCameraPitch(Mat tVec, Mat rVec, Rotation2d cameraPitchAngle) {
// Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision
// whitepaper
var tiltAngle = cameraPitchAngle.getRadians();
// the left/right distance to the target, unchanged by tilt.
var x = tVec.get(0, 0)[0];
// Z distance in the flat plane is given by
// Z_field = z cos theta + y sin theta.
// Z is the distance "out" of the camera (straight forward).
var zField =
tVec.get(2, 0)[0] * FastMath.cos(tiltAngle) + tVec.get(1, 0)[0] * FastMath.sin(tiltAngle);
Calib3d.Rodrigues(rVec, rotationMatrix);
Core.transpose(rotationMatrix, inverseRotationMatrix);
scaledTvec = matScale(tVec, -1);
Core.gemm(inverseRotationMatrix, scaledTvec, 1, kMat, 0, pzeroWorld);
scaledTvec.release();
var angle2 = FastMath.atan2(pzeroWorld.get(0, 0)[0], pzeroWorld.get(2, 0)[0]);
// target rotation is the rotation of the target relative to straight ahead. this number
// should be unchanged if the robot purely translated left/right.
var targetRotation = -angle2; // radians
// We want a vector that is X forward and Y left.
// We have a Z_field (out of the camera projected onto the field), and an X left/right.
// so Z_field becomes X, and X becomes Y
//noinspection SuspiciousNameCombination
var targetLocation = new Translation2d(zField, -x);
return new Pose2d(targetLocation, new Rotation2d(targetRotation));
}
/**
* Element-wise scale a matrix by a given factor
*
* @param src the source matrix
* @param factor by how much to scale each element
* @return the scaled matrix
*/
private static Mat matScale(Mat src, double factor) {
Mat dst = new Mat(src.rows(), src.cols(), src.type());
Scalar s = new Scalar(factor);
Core.multiply(src, s, dst);
return dst;
}
public static class SolvePNPPipeParams {
private final CameraCalibrationCoefficients cameraCoefficients;
private final Rotation2d cameraPitchAngle;
private final TargetModel targetModel;
public SolvePNPPipeParams(
CameraCalibrationCoefficients cameraCoefficients,
Rotation2d cameraPitchAngle,
TargetModel targetModel) {
this.cameraCoefficients = cameraCoefficients;
this.cameraPitchAngle = cameraPitchAngle;
this.targetModel = targetModel;
}
}
}

64
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/SortContoursPipe.java
View File

@@ -1,64 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import org.photonvision.common.vision.opencv.ContourSortMode;
import org.photonvision.common.vision.pipe.CVPipe;
import org.photonvision.common.vision.target.PotentialTarget;
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<PotentialTarget>, List<PotentialTarget>, SortContoursPipe.SortContoursParams> {
private List<PotentialTarget> m_sortedContours = new ArrayList<>();
@Override
protected List<PotentialTarget> process(List<PotentialTarget> in) {
m_sortedContours.clear();
if (in.size() > 0) {
m_sortedContours.addAll(in);
if (params.getSortMode() != ContourSortMode.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(PotentialTarget 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 static class SortContoursParams {
private ContourSortMode m_sortMode;
private FrameStaticProperties m_camProperties;
private int m_maxTargets;
public SortContoursParams(
ContourSortMode sortMode, FrameStaticProperties camProperties, int maxTargets) {
m_sortMode = sortMode;
m_camProperties = camProperties;
m_maxTargets = maxTargets;
}
public ContourSortMode getSortMode() {
return m_sortMode;
}
public FrameStaticProperties getCamProperties() {
return m_camProperties;
}
public int getMaxTargets() {
return m_maxTargets;
}
}
}

50
photon-server/src/main/java/org/photonvision/common/vision/pipe/impl/SpeckleRejectPipe.java
View File

@@ -1,50 +0,0 @@
package org.photonvision.common.vision.pipe.impl;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.pipe.CVPipe;
import java.util.ArrayList;
import java.util.List;
public class SpeckleRejectPipe
extends CVPipe<List<Contour>, List<Contour>, SpeckleRejectPipe.SpeckleRejectParams> {
private List<Contour> m_despeckledContours = new ArrayList<>();
@Override
protected List<Contour> process(List<Contour> 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;
}
}
}

104
photon-server/src/main/java/org/photonvision/common/vision/pipeline/AdvancedPipelineSettings.java
View File

@@ -1,104 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.util.numbers.IntegerCouple;
import org.photonvision.common.vision.opencv.ContourSortMode;
import org.photonvision.common.vision.target.RobotOffsetPointMode;
import org.photonvision.common.vision.target.TargetOffsetPointEdge;
import org.photonvision.common.vision.target.TargetOrientation;
import java.util.Objects;
public class AdvancedPipelineSettings extends CVPipelineSettings {
public AdvancedPipelineSettings() {
ledMode = true;
}
public IntegerCouple hsvHue = new IntegerCouple(50, 180);
public IntegerCouple hsvSaturation = new IntegerCouple(50, 255);
public IntegerCouple hsvValue = new IntegerCouple(50, 255);
public boolean outputShowThresholded = false;
public boolean outputShowMultipleTargets = false;
public boolean erode = false;
public boolean dilate = false;
public DoubleCouple contourArea = new DoubleCouple(0.0, 100.0);
public DoubleCouple contourRatio = new DoubleCouple(0.0, 20.0);
public DoubleCouple contourExtent = new DoubleCouple(0.0, 100.0);
public int contourSpecklePercentage = 5;
// the order in which to sort contours to find the most desirable
public ContourSortMode contourSortMode = ContourSortMode.Largest;
// the edge (or not) of the target to consider the center point (Top, Bottom, Left, Right,
// Center)
public TargetOffsetPointEdge contourTargetOffsetPointEdge = TargetOffsetPointEdge.Center;
// orientation of the target in terms of aspect ratio
public TargetOrientation contourTargetOrientation = TargetOrientation.Landscape;
// the mode in which to offset target center point based on the camera being offset on the
// robot
// (None, Single Point, Dual Point)
public RobotOffsetPointMode offsetRobotOffsetMode = RobotOffsetPointMode.None;
// the point set by the user in Single Point Offset mode (maybe double too? idr)
public DoubleCouple offsetCalibrationPoint = new DoubleCouple();
// the two values that define the line of the Dual Point Offset calibration (think y=mx+b)
public double offsetDualLineM = 1;
public double offsetDualLineB = 0;
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
if (!super.equals(o)) return false;
AdvancedPipelineSettings that = (AdvancedPipelineSettings) o;
return outputShowThresholded == that.outputShowThresholded
&& outputShowMultipleTargets == that.outputShowMultipleTargets
&& erode == that.erode
&& dilate == that.dilate
&& contourSpecklePercentage == that.contourSpecklePercentage
&& Double.compare(that.offsetDualLineM, offsetDualLineM) == 0
&& Double.compare(that.offsetDualLineB, offsetDualLineB) == 0
&& hsvHue.equals(that.hsvHue)
&& hsvSaturation.equals(that.hsvSaturation)
&& hsvValue.equals(that.hsvValue)
&& contourArea.equals(that.contourArea)
&& contourRatio.equals(that.contourRatio)
&& contourExtent.equals(that.contourExtent)
&& contourSortMode == that.contourSortMode
&& contourTargetOffsetPointEdge == that.contourTargetOffsetPointEdge
&& contourTargetOrientation == that.contourTargetOrientation
&& offsetRobotOffsetMode == that.offsetRobotOffsetMode
&& offsetCalibrationPoint.equals(that.offsetCalibrationPoint);
}
@Override
public int hashCode() {
return Objects.hash(
super.hashCode(),
hsvHue,
hsvSaturation,
hsvValue,
outputShowThresholded,
outputShowMultipleTargets,
erode,
dilate,
contourArea,
contourRatio,
contourExtent,
contourSpecklePercentage,
contourSortMode,
contourTargetOffsetPointEdge,
contourTargetOrientation,
offsetRobotOffsetMode,
offsetCalibrationPoint,
offsetDualLineM,
offsetDualLineB);
}
}

32
photon-server/src/main/java/org/photonvision/common/vision/pipeline/CVPipeline.java
View File

@@ -1,32 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameStaticProperties;
public abstract class CVPipeline<R extends CVPipelineResult, S extends CVPipelineSettings> {
protected S settings;
protected abstract void setPipeParams(FrameStaticProperties frameStaticProperties, S settings);
protected abstract R process(Frame frame, S settings);
public S getSettings() {
return settings;
}
public R run(Frame frame) {
long pipelineStartNanos = System.nanoTime();
if (settings == null) {
throw new RuntimeException("No settings provided for pipeline!");
}
setPipeParams(frame.frameStaticProperties, settings);
R result = process(frame, settings);
result.setLatencyMillis(MathUtils.nanosToMillis(System.nanoTime() - pipelineStartNanos));
return result;
}
}

40
photon-server/src/main/java/org/photonvision/common/vision/pipeline/CVPipelineResult.java
View File

@@ -1,40 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.opencv.Releasable;
import org.photonvision.common.vision.target.TrackedTarget;
import java.util.List;
public class CVPipelineResult implements Releasable {
private double latencyMillis;
public final double processingMillis;
public final List<TrackedTarget> targets;
public final Frame outputFrame;
public CVPipelineResult(double processingMillis, List<TrackedTarget> targets, Frame outputFrame) {
this.processingMillis = processingMillis;
this.targets = targets;
this.outputFrame = Frame.copyFrom(outputFrame);
}
public boolean hasTargets() {
return !targets.isEmpty();
}
public void release() {
for (TrackedTarget tt : targets) {
tt.release();
}
outputFrame.release();
}
public double getLatencyMillis() {
return latencyMillis;
}
protected void setLatencyMillis(double latencyMillis) {
this.latencyMillis = latencyMillis;
}
}

69
photon-server/src/main/java/org/photonvision/common/vision/pipeline/CVPipelineSettings.java
View File

@@ -1,69 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.vision.frame.FrameDivisor;
import org.photonvision.common.vision.pipe.ImageFlipMode;
import org.photonvision.common.vision.pipe.ImageRotationMode;
import com.fasterxml.jackson.annotation.JsonSubTypes;
import com.fasterxml.jackson.annotation.JsonTypeInfo;
import java.util.Objects;
@JsonTypeInfo(
use = JsonTypeInfo.Id.NAME,
include = JsonTypeInfo.As.WRAPPER_ARRAY,
property = "type")
@JsonSubTypes({
@JsonSubTypes.Type(value = ColoredShapePipelineSettings.class),
@JsonSubTypes.Type(value = ReflectivePipelineSettings.class),
@JsonSubTypes.Type(value = DriverModePipelineSettings.class)
})
public class CVPipelineSettings {
public int pipelineIndex = 0;
public PipelineType pipelineType = PipelineType.DriverMode;
public ImageFlipMode inputImageFlipMode = ImageFlipMode.NONE;
public ImageRotationMode inputImageRotationMode = ImageRotationMode.DEG_0;
public String pipelineNickname = "New Pipeline";
public double cameraExposure = 50.0;
public double cameraBrightness = 50.0;
public double cameraGain = 50.0;
public int cameraVideoModeIndex = 0;
public FrameDivisor inputFrameDivisor = FrameDivisor.NONE;
public FrameDivisor outputFrameDivisor = FrameDivisor.NONE;
public boolean ledMode = false;
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
CVPipelineSettings that = (CVPipelineSettings) o;
return pipelineIndex == that.pipelineIndex
&& Double.compare(that.cameraExposure, cameraExposure) == 0
&& Double.compare(that.cameraBrightness, cameraBrightness) == 0
&& Double.compare(that.cameraGain, cameraGain) == 0
&& cameraVideoModeIndex == that.cameraVideoModeIndex
&& ledMode == that.ledMode
&& pipelineType == that.pipelineType
&& inputImageFlipMode == that.inputImageFlipMode
&& inputImageRotationMode == that.inputImageRotationMode
&& pipelineNickname.equals(that.pipelineNickname)
&& inputFrameDivisor == that.inputFrameDivisor
&& outputFrameDivisor == that.outputFrameDivisor;
}
@Override
public int hashCode() {
return Objects.hash(
pipelineIndex,
pipelineType,
inputImageFlipMode,
inputImageRotationMode,
pipelineNickname,
cameraExposure,
cameraBrightness,
cameraGain,
cameraVideoModeIndex,
inputFrameDivisor,
outputFrameDivisor,
ledMode);
}
}

22
photon-server/src/main/java/org/photonvision/common/vision/pipeline/Calibration3dPipeline.java
View File

@@ -1,22 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameStaticProperties;
public class Calibration3dPipeline extends CVPipeline<CVPipelineResult, CVPipelineSettings> {
// TODO: Everything here
public Calibration3dPipeline() {
settings = new CVPipelineSettings();
}
@Override
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, CVPipelineSettings settings) {}
@Override
protected CVPipelineResult process(Frame frame, CVPipelineSettings settings) {
return null;
}
}

257
photon-server/src/main/java/org/photonvision/common/vision/pipeline/ColoredShapePipeline.java
View File

@@ -1,257 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import com.chameleonvision.common.vision.opencv.*;
import org.photonvision.common.vision.pipe.CVPipeResult;
import com.chameleonvision.common.vision.pipe.impl.*;
import org.photonvision.common.vision.target.PotentialTarget;
import org.photonvision.common.vision.target.TrackedTarget;
import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.photonvision.common.vision.opencv.*;
import org.photonvision.common.vision.pipe.impl.*;
public class ColoredShapePipeline
extends CVPipeline<CVPipelineResult, ColoredShapePipelineSettings> {
private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
private final ErodeDilatePipe erodeDilatePipe = new ErodeDilatePipe();
private final HSVPipe hsvPipe = new HSVPipe();
private final OutputMatPipe outputMatPipe = new OutputMatPipe();
private final SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe();
private final FindContoursPipe findContoursPipe = new FindContoursPipe();
private final FindPolygonPipe findPolygonPipe = new FindPolygonPipe();
private final FindCirclesPipe findCirclesPipe = new FindCirclesPipe();
private final FilterShapesPipe filterShapesPipe = new FilterShapesPipe();
private final GroupContoursPipe groupContoursPipe = new GroupContoursPipe();
private final SortContoursPipe sortContoursPipe = new SortContoursPipe();
private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe();
private final CornerDetectionPipe cornerDetectionPipe = new CornerDetectionPipe();
private final SolvePNPPipe solvePNPPipe = new SolvePNPPipe();
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
private final Draw2dContoursPipe draw2dContoursPipe = new Draw2dContoursPipe();
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
private final Mat rawInputMat = new Mat();
private final DualMat outputMats = new DualMat();
private List<CVShape> shapes;
private CVPipeResult<Mat> result;
private CVPipeResult<List<TrackedTarget>> targetList;
private final Point[] rectPoints = new Point[4];
ColoredShapePipeline() {
settings = new ColoredShapePipelineSettings();
}
@Override
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, ColoredShapePipelineSettings settings) {
RotateImagePipe.RotateImageParams rotateImageParams =
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
rotateImagePipe.setParams(rotateImageParams);
ErodeDilatePipe.ErodeDilateParams erodeDilateParams =
new ErodeDilatePipe.ErodeDilateParams(settings.erode, settings.dilate, 5);
// TODO: add kernel size to pipeline settings
erodeDilatePipe.setParams(erodeDilateParams);
HSVPipe.HSVParams hsvParams =
new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
hsvPipe.setParams(hsvParams);
OutputMatPipe.OutputMatParams outputMatParams =
new OutputMatPipe.OutputMatParams(settings.outputShowThresholded);
outputMatPipe.setParams(outputMatParams);
SpeckleRejectPipe.SpeckleRejectParams speckleRejectParams =
new SpeckleRejectPipe.SpeckleRejectParams(settings.contourSpecklePercentage);
speckleRejectPipe.setParams(speckleRejectParams);
FindContoursPipe.FindContoursParams findContoursParams =
new FindContoursPipe.FindContoursParams();
findContoursPipe.setParams(findContoursParams);
FindPolygonPipe.FindPolygonPipeParams findPolygonPipeParams =
new FindPolygonPipe.FindPolygonPipeParams(settings.accuracyPercentage);
findPolygonPipe.setParams(findPolygonPipeParams);
FindCirclesPipe.FindCirclePipeParams findCirclePipeParams =
new FindCirclesPipe.FindCirclePipeParams(
settings.allowableThreshold,
settings.minRadius,
settings.minDist,
settings.maxRadius,
settings.maxCannyThresh,
settings.accuracy);
findCirclesPipe.setParams(findCirclePipeParams);
FilterShapesPipe.FilterShapesPipeParams filterShapesPipeParams =
new FilterShapesPipe.FilterShapesPipeParams(
settings.desiredShape,
settings.minArea,
settings.maxArea,
settings.minPeri,
settings.maxPeri);
filterShapesPipe.setParams(filterShapesPipeParams);
GroupContoursPipe.GroupContoursParams groupContoursParams =
new GroupContoursPipe.GroupContoursParams(
settings.contourGroupingMode, settings.contourIntersection);
groupContoursPipe.setParams(groupContoursParams);
SortContoursPipe.SortContoursParams sortContoursParams =
new SortContoursPipe.SortContoursParams(settings.contourSortMode, frameStaticProperties, 5);
sortContoursPipe.setParams(sortContoursParams);
Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
new Collect2dTargetsPipe.Collect2dTargetsParams(
frameStaticProperties,
settings.offsetRobotOffsetMode,
settings.offsetDualLineM,
settings.offsetDualLineB,
settings.offsetCalibrationPoint.toPoint(),
settings.contourTargetOffsetPointEdge,
settings.contourTargetOrientation);
collect2dTargetsPipe.setParams(collect2dTargetsParams);
var params =
new CornerDetectionPipe.CornerDetectionPipeParameters(
settings.cornerDetectionStrategy,
settings.cornerDetectionUseConvexHulls,
settings.cornerDetectionExactSideCount,
settings.cornerDetectionSideCount,
settings.cornerDetectionAccuracyPercentage);
cornerDetectionPipe.setParams(params);
var solvePNPParams =
new SolvePNPPipe.SolvePNPPipeParams(
settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
solvePNPPipe.setParams(solvePNPParams);
Draw2dContoursPipe.Draw2dContoursParams draw2dContoursParams =
new Draw2dContoursPipe.Draw2dContoursParams(settings.outputShowMultipleTargets);
draw2dContoursParams.showShape = true;
draw2dContoursParams.showMaximumBox = false;
draw2dContoursParams.showRotatedBox = false;
draw2dContoursParams.boxOutlineSize = 2;
draw2dContoursPipe.setParams(draw2dContoursParams);
Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
settings.offsetRobotOffsetMode, settings.offsetCalibrationPoint);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
var draw3dContoursParams =
new Draw3dTargetsPipe.Draw3dContoursParams(
settings.cameraCalibration, settings.targetModel);
draw3dTargetsPipe.setParams(draw3dContoursParams);
}
@Override
protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings settings) {
setPipeParams(frame.frameStaticProperties, settings);
long sumPipeNanosElapsed = 0L;
frame.image.getMat().copyTo(rawInputMat);
CVPipeResult<Mat> rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
CVPipeResult<Mat> erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result);
sumPipeNanosElapsed += erodeDilateResult.nanosElapsed;
CVPipeResult<Mat> hsvPipeResult = hsvPipe.apply(erodeDilateResult.result);
sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
outputMats.first = rawInputMat;
outputMats.second = hsvPipeResult.result;
CVPipeResult<Mat> outputMatResult = outputMatPipe.apply(outputMats);
sumPipeNanosElapsed += outputMatResult.nanosElapsed;
CVPipeResult<List<Contour>> findContoursResult = findContoursPipe.apply(hsvPipeResult.result);
sumPipeNanosElapsed += findContoursResult.nanosElapsed;
CVPipeResult<List<Contour>> speckleRejectResult =
speckleRejectPipe.apply(findContoursResult.result);
sumPipeNanosElapsed += speckleRejectResult.nanosElapsed;
if (settings.desiredShape == ContourShape.Circle) {
CVPipeResult<List<CVShape>> findCirclesResult =
findCirclesPipe.apply(Pair.of(hsvPipeResult.result, speckleRejectResult.result));
sumPipeNanosElapsed += findCirclesResult.nanosElapsed;
shapes = findCirclesResult.result;
} else {
CVPipeResult<List<CVShape>> findPolygonsResult =
findPolygonPipe.apply(speckleRejectResult.result);
sumPipeNanosElapsed += findPolygonsResult.nanosElapsed;
shapes = findPolygonsResult.result;
}
CVPipeResult<List<CVShape>> filterShapeResult = filterShapesPipe.apply(shapes);
sumPipeNanosElapsed += filterShapeResult.nanosElapsed;
CVPipeResult<List<PotentialTarget>> groupContoursResult =
groupContoursPipe.apply(
filterShapeResult.result.stream()
.map(CVShape::getContour)
.collect(Collectors.toList()));
sumPipeNanosElapsed += groupContoursResult.nanosElapsed;
CVPipeResult<List<PotentialTarget>> sortContoursResult =
sortContoursPipe.apply(groupContoursResult.result);
sumPipeNanosElapsed += sortContoursResult.nanosElapsed;
CVPipeResult<List<TrackedTarget>> collect2dTargetsResult =
collect2dTargetsPipe.apply(sortContoursResult.result);
sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed;
if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {
var cornerDetectionResult = cornerDetectionPipe.apply(collect2dTargetsResult.result);
collect2dTargetsResult.result.forEach(
shape -> {
shape.getMinAreaRect().points(rectPoints);
shape.setCorners(Arrays.asList(rectPoints));
});
sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed;
var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result);
sumPipeNanosElapsed += solvePNPResult.nanosElapsed;
targetList = solvePNPResult;
} else {
targetList = collect2dTargetsResult;
}
CVPipeResult<Mat> draw2dCrosshairResult =
draw2dCrosshairPipe.apply(Pair.of(outputMatResult.result, targetList.result));
sumPipeNanosElapsed += draw2dCrosshairResult.nanosElapsed;
CVPipeResult<Mat> draw2dContoursResult =
draw2dContoursPipe.apply(
Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
sumPipeNanosElapsed += draw2dContoursResult.nanosElapsed;
if (settings.solvePNPEnabled && settings.desiredShape == ContourShape.Circle) {
result =
draw3dTargetsPipe.apply(
Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
sumPipeNanosElapsed += result.nanosElapsed;
} else {
result = draw2dContoursResult;
}
return new CVPipelineResult(
MathUtils.nanosToMillis(sumPipeNanosElapsed),
collect2dTargetsResult.result,
new Frame(new CVMat(result.result), frame.frameStaticProperties));
}
}

114
photon-server/src/main/java/org/photonvision/common/vision/pipeline/ColoredShapePipelineSettings.java
View File

@@ -1,114 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.calibration.CameraCalibrationCoefficients;
import org.photonvision.common.vision.opencv.ContourGroupingMode;
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
import org.photonvision.common.vision.opencv.ContourShape;
import org.photonvision.common.vision.pipe.impl.CornerDetectionPipe;
import org.photonvision.common.vision.target.TargetModel;
import com.fasterxml.jackson.annotation.JsonTypeName;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
@JsonTypeName("ColoredShapePipelineSettings")
public class ColoredShapePipelineSettings extends AdvancedPipelineSettings {
public ContourShape desiredShape = ContourShape.Triangle;
public double minArea = Integer.MIN_VALUE;
public double maxArea = Integer.MAX_VALUE;
public double minPeri = Integer.MIN_VALUE;
public double maxPeri = Integer.MAX_VALUE;
public double accuracyPercentage = 10.0;
// Circle detection
public int allowableThreshold = 5;
public int minRadius = 0;
public int maxRadius = 0;
public int minDist = 10;
public int maxCannyThresh = 90;
public int accuracy = 20;
// how many contours to attempt to group (Single, Dual)
public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;
// the direction in which contours must intersect to be considered intersecting
public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
// 3d settings
public boolean solvePNPEnabled = false;
public CameraCalibrationCoefficients cameraCalibration;
public TargetModel targetModel;
public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0);
// Corner detection settings
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
public boolean cornerDetectionUseConvexHulls = true;
public boolean cornerDetectionExactSideCount = false;
public int cornerDetectionSideCount = 4;
public double cornerDetectionAccuracyPercentage = 10;
public ColoredShapePipelineSettings() {
super();
pipelineType = PipelineType.ColoredShape;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
if (!super.equals(o)) return false;
ColoredShapePipelineSettings that = (ColoredShapePipelineSettings) o;
return Double.compare(that.minArea, minArea) == 0
&& Double.compare(that.maxArea, maxArea) == 0
&& Double.compare(that.minPeri, minPeri) == 0
&& Double.compare(that.maxPeri, maxPeri) == 0
&& Double.compare(that.accuracyPercentage, accuracyPercentage) == 0
&& allowableThreshold == that.allowableThreshold
&& minRadius == that.minRadius
&& maxRadius == that.maxRadius
&& minDist == that.minDist
&& maxCannyThresh == that.maxCannyThresh
&& accuracy == that.accuracy
&& solvePNPEnabled == that.solvePNPEnabled
&& cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
&& cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
&& cornerDetectionSideCount == that.cornerDetectionSideCount
&& Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
== 0
&& desiredShape == that.desiredShape
&& contourGroupingMode == that.contourGroupingMode
&& contourIntersection == that.contourIntersection
&& Objects.equals(cameraCalibration, that.cameraCalibration)
&& Objects.equals(targetModel, that.targetModel)
&& Objects.equals(cameraPitch, that.cameraPitch)
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
}
@Override
public int hashCode() {
return Objects.hash(
super.hashCode(),
desiredShape,
minArea,
maxArea,
minPeri,
maxPeri,
accuracyPercentage,
allowableThreshold,
minRadius,
maxRadius,
minDist,
maxCannyThresh,
accuracy,
contourGroupingMode,
contourIntersection,
solvePNPEnabled,
cameraCalibration,
targetModel,
cameraPitch,
cornerDetectionStrategy,
cornerDetectionUseConvexHulls,
cornerDetectionExactSideCount,
cornerDetectionSideCount,
cornerDetectionAccuracyPercentage);
}
}

61
photon-server/src/main/java/org/photonvision/common/vision/pipeline/DriverModePipeline.java
View File

@@ -1,61 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import org.photonvision.common.vision.opencv.CVMat;
import org.photonvision.common.vision.pipe.impl.Draw2dCrosshairPipe;
import org.photonvision.common.vision.pipe.impl.ResizeImagePipe;
import org.photonvision.common.vision.pipe.impl.RotateImagePipe;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
public class DriverModePipeline
extends CVPipeline<DriverModePipelineResult, DriverModePipelineSettings> {
private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe();
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
public DriverModePipeline() {
settings = new DriverModePipelineSettings();
}
@Override
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, DriverModePipelineSettings settings) {
RotateImagePipe.RotateImageParams rotateImageParams =
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
rotateImagePipe.setParams(rotateImageParams);
ResizeImagePipe.ResizeImageParams resizeImageParams =
new ResizeImagePipe.ResizeImageParams(settings.inputFrameDivisor);
resizeImagePipe.setParams(resizeImageParams);
Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
settings.offsetPointMode, settings.offsetPoint);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
}
@Override
public DriverModePipelineResult process(Frame frame, DriverModePipelineSettings settings) {
// apply pipes
var rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
var resizeImageResult = resizeImagePipe.apply(rotateImageResult.result);
var draw2dCrosshairResult =
draw2dCrosshairPipe.apply(Pair.of(resizeImageResult.result, List.of()));
// calculate elapsed nanoseconds
long totalNanos =
rotateImageResult.nanosElapsed
+ resizeImageResult.nanosElapsed
+ draw2dCrosshairResult.nanosElapsed;
return new DriverModePipelineResult(
MathUtils.nanosToMillis(totalNanos),
new Frame(new CVMat(draw2dCrosshairResult.result), frame.frameStaticProperties));
}
}

11
photon-server/src/main/java/org/photonvision/common/vision/pipeline/DriverModePipelineResult.java
View File

@@ -1,11 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.vision.frame.Frame;
import java.util.List;
public class DriverModePipelineResult extends CVPipelineResult {
public DriverModePipelineResult(double latencyMillis, Frame outputFrame) {
super(latencyMillis, List.of(), outputFrame);
}
}

16
photon-server/src/main/java/org/photonvision/common/vision/pipeline/DriverModePipelineSettings.java
View File

@@ -1,16 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.vision.target.RobotOffsetPointMode;
import com.fasterxml.jackson.annotation.JsonTypeName;
@JsonTypeName("DriverModePipelineSettings")
public class DriverModePipelineSettings extends CVPipelineSettings {
public RobotOffsetPointMode offsetPointMode = RobotOffsetPointMode.None;
public DoubleCouple offsetPoint = new DoubleCouple();
public DriverModePipelineSettings() {
super();
pipelineType = PipelineType.DriverMode;
}
}

17
photon-server/src/main/java/org/photonvision/common/vision/pipeline/PipelineType.java
View File

@@ -1,17 +0,0 @@
package org.photonvision.common.vision.pipeline;
@SuppressWarnings("rawtypes")
public enum PipelineType {
Calib3d(-2, Calibration3dPipeline.class),
DriverMode(-1, DriverModePipeline.class),
Reflective(0, ReflectivePipeline.class),
ColoredShape(0, ColoredShapePipeline.class);
public final int baseIndex;
public final Class clazz;
PipelineType(int baseIndex, Class clazz) {
this.baseIndex = baseIndex;
this.clazz = clazz;
}
}

231
photon-server/src/main/java/org/photonvision/common/vision/pipeline/ReflectivePipeline.java
View File

@@ -1,231 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.util.math.MathUtils;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import org.photonvision.common.vision.opencv.CVMat;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.opencv.DualMat;
import org.photonvision.common.vision.pipe.CVPipeResult;
import org.photonvision.common.vision.pipe.impl.Collect2dTargetsPipe;
import org.photonvision.common.vision.pipe.impl.CornerDetectionPipe;
import org.photonvision.common.vision.pipe.impl.Draw2dContoursPipe;
import org.photonvision.common.vision.pipe.impl.Draw2dCrosshairPipe;
import org.photonvision.common.vision.pipe.impl.Draw3dTargetsPipe;
import org.photonvision.common.vision.pipe.impl.ErodeDilatePipe;
import org.photonvision.common.vision.pipe.impl.FilterContoursPipe;
import org.photonvision.common.vision.pipe.impl.FindContoursPipe;
import org.photonvision.common.vision.pipe.impl.GroupContoursPipe;
import org.photonvision.common.vision.pipe.impl.HSVPipe;
import org.photonvision.common.vision.pipe.impl.OutputMatPipe;
import org.photonvision.common.vision.pipe.impl.RotateImagePipe;
import org.photonvision.common.vision.pipe.impl.SolvePNPPipe;
import org.photonvision.common.vision.pipe.impl.SortContoursPipe;
import org.photonvision.common.vision.pipe.impl.SpeckleRejectPipe;
import org.photonvision.common.vision.target.PotentialTarget;
import org.photonvision.common.vision.target.TrackedTarget;
import java.util.List;
import org.apache.commons.lang3.tuple.Pair;
import org.opencv.core.Mat;
/** Represents a pipeline for tracking retro-reflective targets. */
public class ReflectivePipeline extends CVPipeline<CVPipelineResult, ReflectivePipelineSettings> {
private final RotateImagePipe rotateImagePipe = new RotateImagePipe();
private final ErodeDilatePipe erodeDilatePipe = new ErodeDilatePipe();
private final HSVPipe hsvPipe = new HSVPipe();
private final OutputMatPipe outputMatPipe = new OutputMatPipe();
private final FindContoursPipe findContoursPipe = new FindContoursPipe();
private final SpeckleRejectPipe speckleRejectPipe = new SpeckleRejectPipe();
private final FilterContoursPipe filterContoursPipe = new FilterContoursPipe();
private final GroupContoursPipe groupContoursPipe = new GroupContoursPipe();
private final SortContoursPipe sortContoursPipe = new SortContoursPipe();
private final Collect2dTargetsPipe collect2dTargetsPipe = new Collect2dTargetsPipe();
private final CornerDetectionPipe cornerDetectionPipe = new CornerDetectionPipe();
private final SolvePNPPipe solvePNPPipe = new SolvePNPPipe();
private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe();
private final Draw2dContoursPipe draw2dContoursPipe = new Draw2dContoursPipe();
private final Draw3dTargetsPipe draw3dTargetsPipe = new Draw3dTargetsPipe();
private Mat rawInputMat = new Mat();
private DualMat outputMats = new DualMat();
public ReflectivePipeline() {
settings = new ReflectivePipelineSettings();
}
@Override
protected void setPipeParams(
FrameStaticProperties frameStaticProperties, ReflectivePipelineSettings settings) {
RotateImagePipe.RotateImageParams rotateImageParams =
new RotateImagePipe.RotateImageParams(settings.inputImageRotationMode);
rotateImagePipe.setParams(rotateImageParams);
ErodeDilatePipe.ErodeDilateParams erodeDilateParams =
new ErodeDilatePipe.ErodeDilateParams(settings.erode, settings.dilate, 5);
// TODO: add kernel size to pipeline settings
erodeDilatePipe.setParams(erodeDilateParams);
HSVPipe.HSVParams hsvParams =
new HSVPipe.HSVParams(settings.hsvHue, settings.hsvSaturation, settings.hsvValue);
hsvPipe.setParams(hsvParams);
OutputMatPipe.OutputMatParams outputMatParams =
new OutputMatPipe.OutputMatParams(settings.outputShowThresholded);
outputMatPipe.setParams(outputMatParams);
FindContoursPipe.FindContoursParams findContoursParams =
new FindContoursPipe.FindContoursParams();
findContoursPipe.setParams(findContoursParams);
SpeckleRejectPipe.SpeckleRejectParams speckleRejectParams =
new SpeckleRejectPipe.SpeckleRejectParams(settings.contourSpecklePercentage);
speckleRejectPipe.setParams(speckleRejectParams);
FilterContoursPipe.FilterContoursParams filterContoursParams =
new FilterContoursPipe.FilterContoursParams(
settings.contourArea,
settings.contourRatio,
settings.contourExtent,
frameStaticProperties);
filterContoursPipe.setParams(filterContoursParams);
GroupContoursPipe.GroupContoursParams groupContoursParams =
new GroupContoursPipe.GroupContoursParams(
settings.contourGroupingMode, settings.contourIntersection);
groupContoursPipe.setParams(groupContoursParams);
SortContoursPipe.SortContoursParams sortContoursParams =
new SortContoursPipe.SortContoursParams(settings.contourSortMode, frameStaticProperties, 5);
sortContoursPipe.setParams(sortContoursParams);
Collect2dTargetsPipe.Collect2dTargetsParams collect2dTargetsParams =
new Collect2dTargetsPipe.Collect2dTargetsParams(
frameStaticProperties,
settings.offsetRobotOffsetMode,
settings.offsetDualLineM,
settings.offsetDualLineB,
settings.offsetCalibrationPoint.toPoint(),
settings.contourTargetOffsetPointEdge,
settings.contourTargetOrientation);
collect2dTargetsPipe.setParams(collect2dTargetsParams);
var params =
new CornerDetectionPipe.CornerDetectionPipeParameters(
settings.cornerDetectionStrategy,
settings.cornerDetectionUseConvexHulls,
settings.cornerDetectionExactSideCount,
settings.cornerDetectionSideCount,
settings.cornerDetectionAccuracyPercentage);
cornerDetectionPipe.setParams(params);
Draw2dContoursPipe.Draw2dContoursParams draw2dContoursParams =
new Draw2dContoursPipe.Draw2dContoursParams(settings.outputShowMultipleTargets);
draw2dContoursPipe.setParams(draw2dContoursParams);
Draw2dCrosshairPipe.Draw2dCrosshairParams draw2dCrosshairParams =
new Draw2dCrosshairPipe.Draw2dCrosshairParams(
settings.offsetRobotOffsetMode, settings.offsetCalibrationPoint);
draw2dCrosshairPipe.setParams(draw2dCrosshairParams);
var draw3dContoursParams =
new Draw3dTargetsPipe.Draw3dContoursParams(
settings.cameraCalibration, settings.targetModel);
draw3dTargetsPipe.setParams(draw3dContoursParams);
var solvePNPParams =
new SolvePNPPipe.SolvePNPPipeParams(
settings.cameraCalibration, settings.cameraPitch, settings.targetModel);
solvePNPPipe.setParams(solvePNPParams);
}
@Override
public CVPipelineResult process(Frame frame, ReflectivePipelineSettings settings) {
setPipeParams(frame.frameStaticProperties, settings);
long sumPipeNanosElapsed = 0L;
frame.image.getMat().copyTo(rawInputMat);
CVPipeResult<Mat> rotateImageResult = rotateImagePipe.apply(frame.image.getMat());
sumPipeNanosElapsed += rotateImageResult.nanosElapsed;
CVPipeResult<Mat> erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result);
sumPipeNanosElapsed += erodeDilateResult.nanosElapsed;
CVPipeResult<Mat> hsvPipeResult = hsvPipe.apply(erodeDilateResult.result);
sumPipeNanosElapsed += hsvPipeResult.nanosElapsed;
// mat leak fix attempt
outputMats.first = rawInputMat;
outputMats.second = hsvPipeResult.result;
CVPipeResult<Mat> outputMatResult = outputMatPipe.apply(outputMats);
sumPipeNanosElapsed += outputMatResult.nanosElapsed;
CVPipeResult<List<Contour>> findContoursResult = findContoursPipe.apply(hsvPipeResult.result);
sumPipeNanosElapsed += findContoursResult.nanosElapsed;
CVPipeResult<List<Contour>> speckleRejectResult =
speckleRejectPipe.apply(findContoursResult.result);
sumPipeNanosElapsed += speckleRejectResult.nanosElapsed;
CVPipeResult<List<PotentialTarget>> groupContoursResult =
groupContoursPipe.apply(speckleRejectResult.result);
sumPipeNanosElapsed += groupContoursResult.nanosElapsed;
CVPipeResult<List<PotentialTarget>> sortContoursResult =
sortContoursPipe.apply(groupContoursResult.result);
sumPipeNanosElapsed += sortContoursResult.nanosElapsed;
CVPipeResult<List<TrackedTarget>> collect2dTargetsResult =
collect2dTargetsPipe.apply(sortContoursResult.result);
sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed;
CVPipeResult<List<TrackedTarget>> targetList;
// 3d stuff
if (settings.solvePNPEnabled) {
var cornerDetectionResult = cornerDetectionPipe.apply(collect2dTargetsResult.result);
sumPipeNanosElapsed += cornerDetectionResult.nanosElapsed;
var solvePNPResult = solvePNPPipe.apply(cornerDetectionResult.result);
sumPipeNanosElapsed += solvePNPResult.nanosElapsed;
targetList = solvePNPResult;
} else {
targetList = collect2dTargetsResult;
}
CVPipeResult<Mat> result;
CVPipeResult<Mat> draw2dCrosshairResult =
draw2dCrosshairPipe.apply(Pair.of(outputMatResult.result, targetList.result));
sumPipeNanosElapsed += draw2dCrosshairResult.nanosElapsed;
CVPipeResult<Mat> draw2dContoursResult =
draw2dContoursPipe.apply(
Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
sumPipeNanosElapsed += draw2dContoursResult.nanosElapsed;
if (settings.solvePNPEnabled) {
result =
draw3dTargetsPipe.apply(
Pair.of(draw2dCrosshairResult.result, collect2dTargetsResult.result));
sumPipeNanosElapsed += result.nanosElapsed;
} else {
result = draw2dContoursResult;
}
// TODO: better way?
if (settings.outputShowThresholded) {
rawInputMat.release();
}
// TODO: Implement all the things
return new CVPipelineResult(
MathUtils.nanosToMillis(sumPipeNanosElapsed),
collect2dTargetsResult.result,
new Frame(new CVMat(result.result), frame.frameStaticProperties));
}
}

76
photon-server/src/main/java/org/photonvision/common/vision/pipeline/ReflectivePipelineSettings.java
View File

@@ -1,76 +0,0 @@
package org.photonvision.common.vision.pipeline;
import org.photonvision.common.calibration.CameraCalibrationCoefficients;
import org.photonvision.common.vision.opencv.ContourGroupingMode;
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
import org.photonvision.common.vision.pipe.impl.CornerDetectionPipe;
import org.photonvision.common.vision.target.TargetModel;
import com.fasterxml.jackson.annotation.JsonTypeName;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import java.util.Objects;
@JsonTypeName("ReflectivePipelineSettings")
public class ReflectivePipelineSettings extends AdvancedPipelineSettings {
// how many contours to attempt to group (Single, Dual)
public ContourGroupingMode contourGroupingMode = ContourGroupingMode.Single;
// the direction in which contours must intersect to be considered intersecting
public ContourIntersectionDirection contourIntersection = ContourIntersectionDirection.Up;
// 3d settings
public boolean solvePNPEnabled = false;
public CameraCalibrationCoefficients cameraCalibration;
public TargetModel targetModel;
public Rotation2d cameraPitch = Rotation2d.fromDegrees(0.0);
// Corner detection settings
public CornerDetectionPipe.DetectionStrategy cornerDetectionStrategy =
CornerDetectionPipe.DetectionStrategy.APPROX_POLY_DP_AND_EXTREME_CORNERS;
public boolean cornerDetectionUseConvexHulls = true;
public boolean cornerDetectionExactSideCount = false;
public int cornerDetectionSideCount = 4;
public double cornerDetectionAccuracyPercentage = 10;
public ReflectivePipelineSettings() {
super();
pipelineType = PipelineType.Reflective;
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (o == null || getClass() != o.getClass()) return false;
if (!super.equals(o)) return false;
ReflectivePipelineSettings that = (ReflectivePipelineSettings) o;
return solvePNPEnabled == that.solvePNPEnabled
&& cornerDetectionUseConvexHulls == that.cornerDetectionUseConvexHulls
&& cornerDetectionExactSideCount == that.cornerDetectionExactSideCount
&& cornerDetectionSideCount == that.cornerDetectionSideCount
&& Double.compare(that.cornerDetectionAccuracyPercentage, cornerDetectionAccuracyPercentage)
== 0
&& contourGroupingMode == that.contourGroupingMode
&& contourIntersection == that.contourIntersection
&& Objects.equals(cameraCalibration, that.cameraCalibration)
&& targetModel.equals(that.targetModel)
&& cameraPitch.equals(that.cameraPitch)
&& cornerDetectionStrategy == that.cornerDetectionStrategy;
}
@Override
public int hashCode() {
return Objects.hash(
super.hashCode(),
contourGroupingMode,
contourIntersection,
solvePNPEnabled,
cameraCalibration,
targetModel,
cameraPitch,
cornerDetectionStrategy,
cornerDetectionUseConvexHulls,
cornerDetectionExactSideCount,
cornerDetectionSideCount,
cornerDetectionAccuracyPercentage);
}
}

8
photon-server/src/main/java/org/photonvision/common/vision/processes/Data.java
View File

@@ -1,8 +0,0 @@
package org.photonvision.common.vision.processes;
import org.photonvision.common.vision.pipeline.CVPipelineResult;
// TODO replace with CTT's data class
public class Data {
public CVPipelineResult result;
}

148
photon-server/src/main/java/org/photonvision/common/vision/processes/PipelineManager.java
View File

@@ -1,148 +0,0 @@
package org.photonvision.common.vision.processes;
import org.photonvision.common.vision.pipeline.CVPipeline;
import org.photonvision.common.vision.pipeline.CVPipelineSettings;
import org.photonvision.common.vision.pipeline.Calibration3dPipeline;
import org.photonvision.common.vision.pipeline.DriverModePipeline;
import java.util.Comparator;
import java.util.List;
@SuppressWarnings({"rawtypes", "unused"})
public class PipelineManager {
private static final int DRIVERMODE_INDEX = -1;
private static final int CAL_3D_INDEX = -2;
public final List<CVPipeline> userPipelines;
private final Calibration3dPipeline calibration3dPipeline = new Calibration3dPipeline();
private final DriverModePipeline driverModePipeline = new DriverModePipeline();
/** Index of the currently active pipeline. */
private int currentPipelineIndex = DRIVERMODE_INDEX;
/**
* Index of the last active user-created pipeline. <br>
* <br>
* Used only when switching from any of the built-in pipelines back to a user-created pipeline.
*/
private int lastPipelineIndex;
/**
* Creates a PipelineManager with a DriverModePipeline, a Calibration3dPipeline, and all provided
* pipelines.
*
* @param userPipelines Pipelines to add to the manager.
*/
public PipelineManager(List<CVPipeline> userPipelines) {
this.userPipelines = userPipelines;
}
/** Creates a PipelineManager with a DriverModePipeline, and a Calibration3dPipeline. */
public PipelineManager() {
this(List.of());
}
/**
* Get a pipeline by index.
*
* @param index Index of desired pipeline.
* @return The desired pipeline.
*/
public CVPipeline getPipeline(int index) {
if (index < 0) {
switch (index) {
case DRIVERMODE_INDEX:
return driverModePipeline;
case CAL_3D_INDEX:
return calibration3dPipeline;
}
}
return userPipelines.get(index);
}
/**
* Get the settings for a pipeline by index.
*
* @param index Index of pipeline whose settings need getting.
* @return The gotten settings of the pipeline whose index was provided.
*/
public CVPipelineSettings getPipelineSettings(int index) {
return getPipeline(index).getSettings();
}
/**
* Get the currently active pipeline.
*
* @return The currently active pipeline.
*/
public CVPipeline getCurrentPipeline() {
return getPipeline(currentPipelineIndex);
}
/**
* Get the currently active pipelines settings
*
* @return The currently active pipelines settings
*/
public CVPipelineSettings getCurrentPipelineSettings() {
return getPipelineSettings(currentPipelineIndex);
}
/**
* Internal method for setting the active pipeline. <br>
* <br>
* All externally accessible methods that intend to change the active pipeline MUST go through
* here to ensure all proper steps are taken.
*
* @param index Index of pipeline to be active
*/
private void setPipelineInternal(int index) {
if (index < 0) {
lastPipelineIndex = currentPipelineIndex;
}
currentPipelineIndex = index;
}
/**
* Leaves the current built-in pipeline, if applicable, and sets the active pipeline to the most
* recently active user-created pipeline.
*/
public void exitAuxiliaryPipeline() {
if (currentPipelineIndex < 0) {
setPipelineInternal(lastPipelineIndex);
}
}
public static final Comparator<CVPipelineSettings> PipelineSettingsIndexComparator =
(o1, o2) -> {
int o1Index = o1.pipelineIndex;
int o2Index = o2.pipelineIndex;
if (o1Index == o2Index) {
return 0;
} else if (o1Index < o2Index) {
return -1;
}
return 1;
};
public static final Comparator<CVPipeline> PipelineIndexComparator =
(o1, o2) -> PipelineSettingsIndexComparator.compare(o1.getSettings(), o2.getSettings());
/**
* Sorts the pipeline list by index, and reassigns their indexes to match the new order. <br>
* <br>
* I don't like this but I have no other ideas, and it works so ¯\_(ツ)_/¯
*/
private void reassignIndexes() {
userPipelines.sort(PipelineIndexComparator);
for (int i = 0; i < userPipelines.size(); i++) {
getPipelineSettings(i).pipelineIndex = i;
}
}
// TODO: adding/removing pipelines
}

67
photon-server/src/main/java/org/photonvision/common/vision/processes/VisionModule.java
View File

@@ -1,67 +0,0 @@
package org.photonvision.common.vision.processes;
import org.photonvision.common.datatransfer.DataConsumer;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameConsumer;
import org.photonvision.common.vision.pipeline.CVPipelineResult;
import java.util.LinkedList;
/**
* This is the God Class
*
* <p>VisionModule has a pipeline manager, vision runner, and data providers. The data providers
* provide info on settings changes. VisionModuleManager holds a list of all current vision modules.
*/
public class VisionModule {
private final PipelineManager pipelineManager;
private final VisionSource visionSource;
private final VisionRunner visionRunner;
private final LinkedList<DataConsumer> dataConsumers = new LinkedList<>();
private final LinkedList<FrameConsumer> frameConsumers = new LinkedList<>();
public VisionModule(PipelineManager pipelineManager, VisionSource visionSource) {
this.pipelineManager = pipelineManager;
this.visionSource = visionSource;
this.visionRunner =
new VisionRunner(
this.visionSource.getFrameProvider(),
this.pipelineManager::getCurrentPipeline,
this::consumeResult);
}
public void start() {
visionRunner.startProcess();
}
void consumeResult(CVPipelineResult result) {
// TODO: put result in to Data (not this way!)
var data = new Data();
data.result = result;
consumeData(data);
var frame = result.outputFrame;
consumeFrame(frame);
}
void consumeData(Data data) {
for (var dataConsumer : dataConsumers) {
dataConsumer.accept(data);
}
}
public void addDataConsumer(DataConsumer dataConsumer) {
dataConsumers.add(dataConsumer);
}
public void addFrameConsumer(FrameConsumer frameConsumer) {
frameConsumers.add(frameConsumer);
}
void consumeFrame(Frame frame) {
for (var frameConsumer : frameConsumers) {
frameConsumer.accept(frame);
}
}
}

25
photon-server/src/main/java/org/photonvision/common/vision/processes/VisionModuleManager.java
View File

@@ -1,25 +0,0 @@
package org.photonvision.common.vision.processes;
import java.util.ArrayList;
import java.util.List;
/** VisionModuleManager has many VisionModules, and provides camera configuration data to them. */
public class VisionModuleManager {
protected final List<VisionModule> visionModules = new ArrayList<>();
public VisionModuleManager(List<VisionSource> visionSources) {
for (var visionSource : visionSources) {
// TODO: loading existing pipelines from config
var pipelineManager = new PipelineManager();
visionModules.add(new VisionModule(pipelineManager, visionSource));
}
}
public void startModules() {
for (var visionModule : visionModules) {
visionModule.start();
}
}
}

67
photon-server/src/main/java/org/photonvision/common/vision/processes/VisionRunner.java
View File

@@ -1,67 +0,0 @@
package org.photonvision.common.vision.processes;
import org.photonvision.common.vision.frame.Frame;
import org.photonvision.common.vision.frame.FrameProvider;
import org.photonvision.common.vision.pipeline.CVPipeline;
import org.photonvision.common.vision.pipeline.CVPipelineResult;
import java.util.function.Consumer;
import java.util.function.Supplier;
/** VisionRunner has a frame supplier, a pipeline supplier, and a result consumer */
@SuppressWarnings("rawtypes")
public class VisionRunner {
private final Thread visionProcessThread;
private final Supplier<Frame> frameSupplier;
private final Supplier<CVPipeline> pipelineSupplier;
private final Consumer<CVPipelineResult> pipelineResultConsumer;
private long loopCount;
/**
* VisionRunner contains a <see cref="Thread">Thread</see> to run a pipeline, given a frame, and
* will give the result to the consumer.
*
* @param frameSupplier The supplier of the latest frame.
* @param pipelineSupplier The supplier of the current pipeline.
* @param pipelineResultConsumer The consumer of the latest result.
*/
public VisionRunner(
FrameProvider frameSupplier,
Supplier<CVPipeline> pipelineSupplier,
Consumer<CVPipelineResult> pipelineResultConsumer) {
this.frameSupplier = frameSupplier;
this.pipelineSupplier = pipelineSupplier;
this.pipelineResultConsumer = pipelineResultConsumer;
this.visionProcessThread = new Thread(this::update);
this.visionProcessThread.setName("VisionRunner - " + frameSupplier.getName());
}
public void startProcess() {
visionProcessThread.start();
}
private boolean hasThrown;
private void update() {
while (!Thread.interrupted()) {
loopCount++;
var pipeline = pipelineSupplier.get();
var frame = frameSupplier.get();
try {
var pipelineResult = pipeline.run(frame);
pipelineResultConsumer.accept(pipelineResult);
} catch (Exception ex) {
if (hasThrown) {
System.err.println(
"Exception in thread \"" + visionProcessThread.getName() + "\", loop " + loopCount);
ex.printStackTrace();
hasThrown = true;
}
}
}
}
}

10
photon-server/src/main/java/org/photonvision/common/vision/processes/VisionSource.java
View File

@@ -1,10 +0,0 @@
package org.photonvision.common.vision.processes;
import org.photonvision.common.vision.frame.FrameProvider;
public interface VisionSource {
FrameProvider getFrameProvider();
VisionSourceSettables getSettables();
}

94
photon-server/src/main/java/org/photonvision/common/vision/processes/VisionSourceManager.java
View File

@@ -1,94 +0,0 @@
package org.photonvision.common.vision.processes;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.vision.camera.CameraType;
import org.photonvision.common.vision.camera.USBCameraSource;
import org.photonvision.common.vision.frame.provider.NetworkFrameProvider;
import edu.wpi.cscore.UsbCamera;
import edu.wpi.cscore.UsbCameraInfo;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;
import org.apache.commons.lang3.NotImplementedException;
import org.apache.commons.lang3.StringUtils;
public class VisionSourceManager {
public List<VisionSource> LoadAllSources(List<CameraConfiguration> camerasConfiguration) {
return LoadAllSources(camerasConfiguration, Arrays.asList(UsbCamera.enumerateUsbCameras()));
}
public List<VisionSource> LoadAllSources(
List<CameraConfiguration> camerasConfiguration, List<UsbCameraInfo> usbCameraInfos) {
var UsbCamerasConfiguration =
camerasConfiguration.stream()
.filter(configuration -> configuration.cameraType == CameraType.UsbCamera)
.collect(Collectors.toList());
// var HttpCamerasConfiguration = camerasConfiguration.stream().filter(configuration ->
// configuration.cameraType == CameraType.HttpCamera);
var matchedCameras = matchUSBCameras(usbCameraInfos, UsbCamerasConfiguration);
return loadUSBCameraSources(matchedCameras);
}
private NetworkFrameProvider loadHTTPCamera(CameraConfiguration config) {
throw new NotImplementedException("");
}
private List<CameraConfiguration> matchUSBCameras(
List<UsbCameraInfo> infos, List<CameraConfiguration> cameraConfigurationList) {
ArrayList<UsbCameraInfo> loopableInfo = new ArrayList<>(infos);
List<CameraConfiguration> cameraConfigurations = new ArrayList<>();
for (CameraConfiguration config : cameraConfigurationList) {
UsbCameraInfo cameraInfo;
if (!StringUtils.isNumeric(config.path)) {
// matching by path
cameraInfo =
loopableInfo.stream()
.filter(usbCameraInfo -> usbCameraInfo.path.equals(config.path))
.findFirst()
.orElse(null);
} else {
// match by index
cameraInfo =
loopableInfo.stream()
.filter(usbCameraInfo -> usbCameraInfo.dev == Integer.parseInt(config.path))
.findFirst()
.orElse(null);
}
if (cameraInfo != null) {
loopableInfo.remove(cameraInfo);
cameraConfigurations.add(config);
}
}
for (UsbCameraInfo info : loopableInfo) {
// create new camera config for all new cameras
String name = info.name.replaceAll("[^\\x00-\\x7F]", "");
String uniqueName = name;
int suffix = 0;
while (containsName(cameraConfigurations, uniqueName)) {
suffix++;
uniqueName = String.format("%s (%d)", uniqueName, suffix);
}
CameraConfiguration configuration =
new CameraConfiguration(name, uniqueName, uniqueName, ((Integer) info.dev).toString());
cameraConfigurations.add(configuration);
}
return cameraConfigurations;
}
private List<VisionSource> loadUSBCameraSources(List<CameraConfiguration> configurations) {
List<VisionSource> usbCameraSources = new ArrayList<>();
configurations.forEach(
configuration -> usbCameraSources.add(new USBCameraSource(configuration)));
return usbCameraSources;
}
private boolean containsName(final List<CameraConfiguration> list, final String name) {
return list.stream().anyMatch(configuration -> configuration.uniqueName.equals(name));
}
}

48
photon-server/src/main/java/org/photonvision/common/vision/processes/VisionSourceSettables.java
View File

@@ -1,48 +0,0 @@
package org.photonvision.common.vision.processes;
import org.photonvision.common.configuration.CameraConfiguration;
import org.photonvision.common.vision.frame.FrameStaticProperties;
import edu.wpi.cscore.VideoMode;
import java.util.HashMap;
public abstract class VisionSourceSettables {
private CameraConfiguration configuration;
protected VisionSourceSettables(CameraConfiguration configuration) {
this.configuration = configuration;
}
FrameStaticProperties frameStaticProperties;
protected HashMap<Integer, VideoMode> videoModes;
public abstract int getExposure();
public abstract void setExposure(int exposure);
public abstract int getBrightness();
public abstract void setBrightness(int brightness);
public abstract int getGain();
public abstract void setGain(int gain);
public abstract VideoMode getCurrentVideoMode();
public abstract void setCurrentVideoMode(VideoMode videoMode);
public abstract HashMap<Integer, VideoMode> getAllVideoModes();
public double getFOV() {
return configuration.FOV;
}
public void setFOV(double fov) {
configuration.FOV = fov;
}
public FrameStaticProperties getFrameStaticProperties() {
return frameStaticProperties;
}
}

40
photon-server/src/main/java/org/photonvision/common/vision/target/PotentialTarget.java
View File

@@ -1,40 +0,0 @@
package org.photonvision.common.vision.target;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.opencv.Releasable;
import java.util.ArrayList;
import java.util.List;
import org.opencv.core.RotatedRect;
public class PotentialTarget implements Releasable {
public final Contour m_mainContour;
public final List<Contour> m_subContours;
public PotentialTarget(Contour inputContour) {
m_mainContour = inputContour;
m_subContours = new ArrayList<>(); // empty
}
public PotentialTarget(Contour inputContour, List<Contour> subContours) {
m_mainContour = inputContour;
m_subContours = new ArrayList<>(subContours);
}
public RotatedRect getMinAreaRect() {
return m_mainContour.getMinAreaRect();
}
public double getArea() {
return m_mainContour.getArea();
}
@Override
public void release() {
m_mainContour.release();
for (var sc : m_subContours) {
sc.release();
}
m_subContours.clear();
}
}

7
photon-server/src/main/java/org/photonvision/common/vision/target/RobotOffsetPointMode.java
View File

@@ -1,7 +0,0 @@
package org.photonvision.common.vision.target;
public enum RobotOffsetPointMode {
None,
Single,
Dual
}

78
photon-server/src/main/java/org/photonvision/common/vision/target/TargetCalculations.java
View File

@@ -1,78 +0,0 @@
package org.photonvision.common.vision.target;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.apache.commons.math3.util.FastMath;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
public class TargetCalculations {
public static double calculateYaw(
double offsetCenterX, double targetCenterX, double horizontalFocalLength) {
return FastMath.toDegrees(FastMath.atan(offsetCenterX - targetCenterX) / horizontalFocalLength);
}
public static double calculatePitch(
double offsetCenterY, double targetCenterY, double verticalFocalLength) {
return -FastMath.toDegrees(FastMath.atan(offsetCenterY - targetCenterY) / verticalFocalLength);
}
public static Point calculateTargetOffsetPoint(
boolean isLandscape, TargetOffsetPointEdge offsetRegion, RotatedRect minAreaRect) {
Point[] vertices = new Point[4];
minAreaRect.points(vertices);
Point bl = getMiddle(vertices[0], vertices[1]);
Point tl = getMiddle(vertices[1], vertices[2]);
Point tr = getMiddle(vertices[2], vertices[3]);
Point br = getMiddle(vertices[3], vertices[0]);
boolean orientation;
if (isLandscape) {
orientation = minAreaRect.size.width > minAreaRect.size.height;
} else {
orientation = minAreaRect.size.width < minAreaRect.size.height;
}
switch (offsetRegion) {
case Top:
return orientation ? tl : tr;
case Bottom:
return orientation ? br : bl;
case Left:
return orientation ? bl : tl;
case Right:
return orientation ? tr : br;
default:
return minAreaRect.center;
}
}
private static Point getMiddle(Point p1, Point p2) {
return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2));
}
public static Point calculateRobotOffsetPoint(
Point offsetPoint,
Point camCenterPoint,
DoubleCouple offsetEquationValues,
RobotOffsetPointMode offsetMode) {
switch (offsetMode) {
case None:
default:
return camCenterPoint;
case Single:
if (offsetPoint.x == 0 && offsetPoint.y == 0) {
return camCenterPoint;
} else {
return offsetPoint;
}
case Dual:
Point resultPoint = new Point();
resultPoint.x =
(offsetPoint.x - offsetEquationValues.getFirst()) / offsetEquationValues.getSecond();
resultPoint.y =
(offsetPoint.y * offsetEquationValues.getSecond()) + offsetEquationValues.getFirst();
return resultPoint;
}
}
}

120
photon-server/src/main/java/org/photonvision/common/vision/target/TargetModel.java
View File

@@ -1,120 +0,0 @@
package org.photonvision.common.vision.target;
import org.photonvision.common.vision.opencv.Releasable;
import com.fasterxml.jackson.annotation.JsonCreator;
import com.fasterxml.jackson.annotation.JsonIgnore;
import com.fasterxml.jackson.annotation.JsonProperty;
import java.util.ArrayList;
import java.util.List;
import java.util.Objects;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point3;
public class TargetModel implements Releasable {
@JsonIgnore private final MatOfPoint3f realWorldTargetCoordinates;
@JsonIgnore private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f();
@JsonIgnore private final MatOfPoint3f visualizationBoxTop = new MatOfPoint3f();
public final List<Point3> realWorldCoordinatesArray;
public final double boxHeight;
public TargetModel(MatOfPoint3f realWorldTargetCoordinates, double boxHeight) {
this.realWorldTargetCoordinates = realWorldTargetCoordinates;
this.realWorldCoordinatesArray = realWorldTargetCoordinates.toList();
this.boxHeight = boxHeight;
var bottomList = realWorldTargetCoordinates.toList();
var topList = new ArrayList<Point3>();
for (var c : bottomList) {
topList.add(new Point3(c.x, c.y, c.z + boxHeight));
}
this.visualizationBoxBottom.fromList(bottomList);
this.visualizationBoxTop.fromList(topList);
}
@JsonCreator
public TargetModel(
@JsonProperty(value = "realWorldCoordinatesArray") List<Point3> points,
@JsonProperty(value = "boxHeight") double boxHeight) {
this(listToMat(points), boxHeight);
}
private static MatOfPoint3f listToMat(List<Point3> points) {
var mat = new MatOfPoint3f();
mat.fromList(points);
return mat;
}
public MatOfPoint3f getRealWorldTargetCoordinates() {
return realWorldTargetCoordinates;
}
public MatOfPoint3f getVisualizationBoxBottom() {
return visualizationBoxBottom;
}
public MatOfPoint3f getVisualizationBoxTop() {
return visualizationBoxTop;
}
public static TargetModel get2020Target() {
return get2020Target(0);
}
public static TargetModel get2020TargetInnerPort() {
return get2020Target(2d * 12d + 5.25); // Inches, TODO switch to meters
}
public static TargetModel get2020Target(double offset) {
var corners =
List.of(
new Point3(-19.625, 0, offset),
new Point3(-9.819867, -17, offset),
new Point3(9.819867, -17, offset),
new Point3(19.625, 0, offset));
return new TargetModel(corners, 12); // TODO switch to meters
}
public static TargetModel get2019Target() {
var corners =
List.of(
new Point3(-5.936, 2.662, 0),
new Point3(-7.313, -2.662, 0),
new Point3(7.313, -2.662, 0),
new Point3(5.936, 2.662, 0));
return new TargetModel(corners, 4);
}
public static TargetModel getCircleTarget(double radius) {
var corners =
List.of(
new Point3(-radius / 2, -radius / 2, -radius / 2),
new Point3(-radius / 2, radius / 2, -radius / 2),
new Point3(radius / 2, radius / 2, -radius / 2),
new Point3(radius / 2, -radius / 2, -radius / 2));
return new TargetModel(corners, 0);
}
@Override
public boolean equals(Object o) {
if (this == o) return true;
if (!(o instanceof TargetModel)) return false;
TargetModel that = (TargetModel) o;
return Double.compare(that.boxHeight, boxHeight) == 0
&& Objects.equals(realWorldCoordinatesArray, that.realWorldCoordinatesArray);
}
@Override
public int hashCode() {
return Objects.hash(realWorldCoordinatesArray, boxHeight);
}
@Override
public void release() {
realWorldTargetCoordinates.release();
visualizationBoxBottom.release();
visualizationBoxTop.release();
}
}

9
photon-server/src/main/java/org/photonvision/common/vision/target/TargetOffsetPointEdge.java
View File

@@ -1,9 +0,0 @@
package org.photonvision.common.vision.target;
public enum TargetOffsetPointEdge {
Center,
Top,
Bottom,
Left,
Right
}

6
photon-server/src/main/java/org/photonvision/common/vision/target/TargetOrientation.java
View File

@@ -1,6 +0,0 @@
package org.photonvision.common.vision.target;
public enum TargetOrientation {
Portrait,
Landscape
}

188
photon-server/src/main/java/org/photonvision/common/vision/target/TrackedTarget.java
View File

@@ -1,188 +0,0 @@
package org.photonvision.common.vision.target;
import org.photonvision.common.util.numbers.DoubleCouple;
import org.photonvision.common.vision.opencv.Contour;
import org.photonvision.common.vision.opencv.Releasable;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import java.util.List;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.RotatedRect;
public class TrackedTarget implements Releasable {
public final Contour m_mainContour;
List<Contour> m_subContours; // can be empty
private MatOfPoint2f m_approximateBoundingPolygon;
private List<Point> m_targetCorners;
private Point m_targetOffsetPoint;
private Point m_robotOffsetPoint;
private double m_pitch;
private double m_yaw;
private double m_area;
private Pose2d m_robotRelativePose;
private Mat m_cameraRelativeTvec, m_cameraRelativeRvec;
public TrackedTarget(PotentialTarget origTarget, TargetCalculationParameters params) {
this.m_mainContour = origTarget.m_mainContour;
this.m_subContours = origTarget.m_subContours;
calculateValues(params);
}
/**
* Set the approximate bouding polygon.
*
* @param boundingPolygon List of points to copy. Not modified.
*/
public void setApproximateBoundingPolygon(MatOfPoint2f boundingPolygon) {
if (m_approximateBoundingPolygon == null) m_approximateBoundingPolygon = new MatOfPoint2f();
boundingPolygon.copyTo(m_approximateBoundingPolygon);
}
public Point getTargetOffsetPoint() {
return m_targetOffsetPoint;
}
public Point getRobotOffsetPoint() {
return m_robotOffsetPoint;
}
public double getPitch() {
return m_pitch;
}
public double getYaw() {
return m_yaw;
}
public double getArea() {
return m_area;
}
public RotatedRect getMinAreaRect() {
return m_mainContour.getMinAreaRect();
}
public MatOfPoint2f getApproximateBoundingPolygon() {
return m_approximateBoundingPolygon;
}
public void calculateValues(TargetCalculationParameters params) {
// this MUST happen in this exact order!
m_targetOffsetPoint =
TargetCalculations.calculateTargetOffsetPoint(
params.isLandscape, params.targetOffsetPointEdge, getMinAreaRect());
m_robotOffsetPoint =
TargetCalculations.calculateRobotOffsetPoint(
m_targetOffsetPoint,
params.cameraCenterPoint,
params.offsetEquationValues,
params.robotOffsetPointMode);
// order of this stuff doesnt matter though
m_pitch =
TargetCalculations.calculatePitch(
m_targetOffsetPoint.y, m_robotOffsetPoint.y, params.verticalFocalLength);
m_yaw =
TargetCalculations.calculateYaw(
m_targetOffsetPoint.x, m_robotOffsetPoint.x, params.horizontalFocalLength);
m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea;
}
@Override
public void release() {
m_mainContour.release();
for (var sc : m_subContours) {
sc.release();
}
if (m_cameraRelativeTvec != null) m_cameraRelativeTvec.release();
if (m_cameraRelativeRvec != null) m_cameraRelativeRvec.release();
}
public void setCorners(List<Point> targetCorners) {
this.m_targetCorners = targetCorners;
}
public List<Point> getTargetCorners() {
return m_targetCorners;
}
public boolean hasSubContours() {
return !m_subContours.isEmpty();
}
public Pose2d getRobotRelativePose() {
return m_robotRelativePose;
}
public void setRobotRelativePose(Pose2d robotRelativePose) {
this.m_robotRelativePose = robotRelativePose;
}
public Mat getCameraRelativeTvec() {
return m_cameraRelativeTvec;
}
public void setCameraRelativeTvec(Mat cameraRelativeTvec) {
if (this.m_cameraRelativeTvec == null) m_cameraRelativeTvec = new Mat();
cameraRelativeTvec.copyTo(this.m_cameraRelativeTvec);
}
public Mat getCameraRelativeRvec() {
return m_cameraRelativeRvec;
}
public void setCameraRelativeRvec(Mat cameraRelativeRvec) {
if (this.m_cameraRelativeRvec == null) m_cameraRelativeRvec = new Mat();
cameraRelativeRvec.copyTo(this.m_cameraRelativeRvec);
}
public static class TargetCalculationParameters {
// TargetOffset calculation values
final boolean isLandscape;
final TargetOffsetPointEdge targetOffsetPointEdge;
// RobotOffset calculation values
final Point userOffsetPoint;
final Point cameraCenterPoint;
final DoubleCouple offsetEquationValues;
final RobotOffsetPointMode robotOffsetPointMode;
// yaw calculation values
final double horizontalFocalLength;
// pitch calculation values
final double verticalFocalLength;
// area calculation values
final double imageArea;
public TargetCalculationParameters(
boolean isLandscape,
TargetOffsetPointEdge targetOffsetPointEdge,
Point userOffsetPoint,
Point cameraCenterPoint,
DoubleCouple offsetEquationValues,
RobotOffsetPointMode robotOffsetPointMode,
double horizontalFocalLength,
double verticalFocalLength,
double imageArea) {
this.isLandscape = isLandscape;
this.targetOffsetPointEdge = targetOffsetPointEdge;
this.userOffsetPoint = userOffsetPoint;
this.cameraCenterPoint = cameraCenterPoint;
this.offsetEquationValues = offsetEquationValues;
this.robotOffsetPointMode = robotOffsetPointMode;
this.horizontalFocalLength = horizontalFocalLength;
this.verticalFocalLength = verticalFocalLength;
this.imageArea = imageArea;
}
}
}