diff --git a/.gitignore b/.gitignore index 06e0ca5f7..beb360ffd 100644 --- a/.gitignore +++ b/.gitignore @@ -122,3 +122,4 @@ New client/chameleon-client/* .DS_Store # *.iml chameleon-server/build +chameleon-server/chameleon-vision (1).iml diff --git a/chameleon-server/build.gradle b/chameleon-server/build.gradle index 26e4ee0e3..d8ee1a321 100644 --- a/chameleon-server/build.gradle +++ b/chameleon-server/build.gradle @@ -68,6 +68,8 @@ dependencies { compile "edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:$openCVVersion:osxx86-64" compile "edu.wpi.first.thirdparty.frc2020.opencv:opencv-jni:$openCVVersion:windowsx86-64" + testCompile "ch.qos.logback:logback-classic:0.9.26" + // javacv (ew) // def withoutJunk = { // exclude group: 'org.bytedeco', module: 'artoolkitplus' @@ -104,7 +106,6 @@ dependencies { sourceSets { main { java { - srcDir 'src' exclude '**/_2/**' } } @@ -113,9 +114,6 @@ sourceSets { test { test { useJUnitPlatform() - testLogging { - events "passed", "skipped", "failed" - } } } @@ -125,5 +123,6 @@ spotless { paddedCell() indentWithTabs(2) indentWithSpaces(4) + removeUnusedImports() } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/Main.java b/chameleon-server/src/main/java/com/chameleonvision/_2/Main.java index 6719f055d..984945bce 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/Main.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/Main.java @@ -1,5 +1,7 @@ package com.chameleonvision._2; +import static com.chameleonvision.common.util.Platform.CurrentPlatform; + import com.chameleonvision._2.config.ConfigManager; import com.chameleonvision._2.vision.VisionManager; import com.chameleonvision._2.web.Server; @@ -11,15 +13,14 @@ import com.chameleonvision.common.util.Platform; import com.chameleonvision.common.util.math.IPUtils; import edu.wpi.cscore.CameraServerCvJNI; import edu.wpi.cscore.CameraServerJNI; - import java.io.IOException; -import static com.chameleonvision.common.util.Platform.CurrentPlatform; - public class Main { private static final String NT_SERVERMODE_KEY = "--nt-servermode"; // no args for this setting - private static final String NT_CLIENTMODESERVER_KEY = "--nt-client-server"; // expects String representing an IP address (hostnames will be rejected!) + private static final String NT_CLIENTMODESERVER_KEY = + "--nt-client-server"; // expects String representing an IP address (hostnames will be + // rejected!) private static final String NETWORK_MANAGE_KEY = "--unmanage-network"; // no args for this setting private static final String IGNORE_ROOT_KEY = "--ignore-root"; // no args for this setting private static final String TEST_MODE_KEY = "--cv-development"; @@ -44,14 +45,18 @@ public class Main { case NT_CLIENTMODESERVER_KEY: var potentialValue = args[i + 1]; // ensures this "value" isnt null, blank, nor another argument - if (potentialValue != null && !potentialValue.isBlank() && !potentialValue.startsWith("-") & !potentialValue.startsWith("--")) { + if (potentialValue != null + && !potentialValue.isBlank() + && !potentialValue.startsWith("-") & !potentialValue.startsWith("--")) { value = potentialValue.toLowerCase(); } i++; // increment to skip an 'arg' next go-around of for loop, as that would be this value break; case UI_PORT_KEY: var potentialPort = args[i + 1]; - if (potentialPort != null && !potentialPort.isBlank() && !potentialPort.startsWith("-") & !potentialPort.startsWith("--")) { + if (potentialPort != null + && !potentialPort.isBlank() + && !potentialPort.startsWith("-") & !potentialPort.startsWith("--")) { value = potentialPort; } i++; @@ -81,7 +86,8 @@ public class Main { continue; } } - System.err.println("Argument for NT Server Host was invalid, defaulting to team number host"); + System.err.println( + "Argument for NT Server Host was invalid, defaulting to team number host"); break; case NETWORK_MANAGE_KEY: manageNetwork = false; @@ -96,7 +102,7 @@ public class Main { if (value != null) { try { uiPort = Integer.parseInt(value); - } catch (NumberFormatException e){ + } catch (NumberFormatException e) { System.err.println("ui Port was not a valid number using port 5800"); } } @@ -107,10 +113,13 @@ public class Main { public static void main(String[] args) { - Runtime.getRuntime().addShutdownHook(new Thread(() -> ScriptManager.queueEvent(ScriptEventType.kProgramExit))); + Runtime.getRuntime() + .addShutdownHook(new Thread(() -> ScriptManager.queueEvent(ScriptEventType.kProgramExit))); if (CurrentPlatform.equals(Platform.UNSUPPORTED)) { - System.err.printf("Sorry, this platform is not supported. Give these details to the developers.\n%s\n", CurrentPlatform.toString()); + System.err.printf( + "Sorry, this platform is not supported. Give these details to the developers.\n%s\n", + CurrentPlatform.toString()); return; } else { System.out.printf("Starting Chameleon Vision on platform %s\n", CurrentPlatform.toString()); @@ -135,7 +144,8 @@ public class Main { CameraServerCvJNI.forceLoad(); } catch (UnsatisfiedLinkError | IOException e) { if (CurrentPlatform.isWindows()) { - System.err.println("Try to download the VC++ Redistributable, https://aka.ms/vs/16/release/vc_redist.x64.exe"); + System.err.println( + "Try to download the VC++ Redistributable, https://aka.ms/vs/16/release/vc_redist.x64.exe"); } throw new RuntimeException("Failed to load JNI Libraries!"); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java index c2326d1ea..9a7303f63 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraCalibrationConfig.java @@ -8,14 +8,19 @@ import org.opencv.core.Mat; import org.opencv.core.MatOfDouble; import org.opencv.core.Size; -/** - * A class that holds a camera matrix and distortion coefficients for a given resolution - */ +/** A class that holds a camera matrix and distortion coefficients for a given resolution */ public class CameraCalibrationConfig { - @JsonProperty("resolution") public final Size resolution; - @JsonProperty("cameraMatrix") public final JsonMat cameraMatrix; - @JsonProperty("distortionCoeffs") public final JsonMat distortionCoeffs; - @JsonProperty("squareSize") public final double squareSize; + @JsonProperty("resolution") + public final Size resolution; + + @JsonProperty("cameraMatrix") + public final JsonMat cameraMatrix; + + @JsonProperty("distortionCoeffs") + public final JsonMat distortionCoeffs; + + @JsonProperty("squareSize") + public final double squareSize; @JsonCreator public CameraCalibrationConfig( @@ -29,7 +34,8 @@ public class CameraCalibrationConfig { this.squareSize = squareSize; } - public CameraCalibrationConfig(Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) { + public CameraCalibrationConfig( + Size resolution, Mat cameraMatrix, Mat distortionCoeffs, double squareSize) { this.resolution = resolution; this.cameraMatrix = JsonMat.fromMat(cameraMatrix); this.distortionCoeffs = JsonMat.fromMat(distortionCoeffs); @@ -49,16 +55,15 @@ public class CameraCalibrationConfig { cameraMatrix = config.cameraMatrix.data; distortionCoeffs = config.distortionCoeffs.data; } - } @JsonIgnore public Mat getCameraMatrixAsMat() { - return cameraMatrix.toMat(); + return cameraMatrix.getAsMat(); } @JsonIgnore public MatOfDouble getDistortionCoeffsAsMat() { - return new MatOfDouble(distortionCoeffs.toMat()); + return new MatOfDouble(distortionCoeffs.getAsMat()); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java index 21586c33d..eef4da9a2 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraConfig.java @@ -1,9 +1,8 @@ package com.chameleonvision._2.config; +import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; import com.chameleonvision.common.util.file.FileUtils; import com.chameleonvision.common.util.file.JacksonUtils; -import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; - import java.io.File; import java.io.IOException; import java.nio.file.Files; @@ -15,7 +14,8 @@ import java.util.Objects; public class CameraConfig { - private static final Path camerasConfigFolderPath = Path.of(ConfigManager.SettingsPath.toString(), "cameras"); + private static final Path camerasConfigFolderPath = + Path.of(ConfigManager.SettingsPath.toString(), "cameras"); private final CameraJsonConfig preliminaryConfig; private final Path configFolderPath; @@ -45,7 +45,8 @@ public class CameraConfig { checkCalibration(); pipelineConfig.check(); - return new FullCameraConfiguration(loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this); + return new FullCameraConfiguration( + loadConfig(), pipelineConfig.load(), loadDriverMode(), loadCalibration(), this); } private CameraJsonConfig loadConfig() { @@ -53,7 +54,8 @@ public class CameraConfig { try { config = JacksonUtils.deserialize(configPath, CameraJsonConfig.class); } catch (IOException e) { - System.err.printf("Failed to load camera config: %s - using default.\n", configPath.toString()); + System.err.printf( + "Failed to load camera config: %s - using default.\n", configPath.toString()); } return config; } @@ -75,7 +77,10 @@ public class CameraConfig { private List loadCalibration() { List calibrations = new ArrayList<>(); try { - calibrations = List.of(Objects.requireNonNull(JacksonUtils.deserialize(calibrationPath, CameraCalibrationConfig[].class))); + calibrations = + List.of( + Objects.requireNonNull( + JacksonUtils.deserialize(calibrationPath, CameraCalibrationConfig[].class))); } catch (Exception e) { System.err.println("Failed to load camera calibration: " + driverModePath.toString()); } @@ -104,7 +109,6 @@ public class CameraConfig { } } - public void saveCalibration(List cal) { CameraCalibrationConfig[] configs = cal.toArray(new CameraCalibrationConfig[0]); try { @@ -119,7 +123,8 @@ public class CameraConfig { if (!configFolderExists()) { try { if (!(new File(configFolderPath.toUri()).mkdirs())) { - System.err.println("Failed to create camera config folder: " + configFolderPath.toString()); + System.err.println( + "Failed to create camera config folder: " + configFolderPath.toString()); } FileUtils.setFilePerms(configFolderPath); } catch (Exception e) { @@ -158,7 +163,8 @@ public class CameraConfig { List calibrations = new ArrayList<>(); JacksonUtils.serializer(calibrationPath, calibrations.toArray(), true); } catch (IOException e) { - System.err.println("Failed to create camera calibration file: " + calibrationPath.toString()); + System.err.println( + "Failed to create camera calibration file: " + calibrationPath.toString()); } } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java index 9ca60c571..4ac2ee57f 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/CameraJsonConfig.java @@ -48,6 +48,13 @@ public class CameraJsonConfig { int videomode = camProps.getCurrentVideoModeIndex(); StreamDivisor streamDivisor = process.cameraStreamer.getDivisor(); double tilt = process.getCamera().getProperties().getTilt().getDegrees(); - return new CameraJsonConfig(camProps.getFOV(), camProps.path, camProps.name, camProps.getNickname(), videomode, streamDivisor, tilt); + return new CameraJsonConfig( + camProps.getFOV(), + camProps.path, + camProps.name, + camProps.getNickname(), + videomode, + streamDivisor, + tilt); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java index 414afcefd..2574c21c4 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/ConfigManager.java @@ -6,7 +6,6 @@ import com.chameleonvision.common.util.Platform; import com.chameleonvision.common.util.ShellExec; import com.chameleonvision.common.util.file.FileUtils; import com.chameleonvision.common.util.file.JacksonUtils; - import java.io.File; import java.io.IOException; import java.nio.file.Files; @@ -17,10 +16,10 @@ import java.util.LinkedHashMap; import java.util.List; public class ConfigManager { - private ConfigManager() { - } + private ConfigManager() {} - public static final Path SettingsPath = Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), "settings"); + public static final Path SettingsPath = + Paths.get(ProgramDirectoryUtilities.getProgramDirectory(), "settings"); private static final Path settingsFilePath = Paths.get(SettingsPath.toString(), "settings.json"); private static final LinkedHashMap cameraConfigs = new LinkedHashMap<>(); @@ -46,14 +45,14 @@ public class ConfigManager { new ShellExec().executeBashCommand("sudo chmod -R 0777 " + SettingsPath.toString()); } } catch (IOException e) { - if (!(e instanceof java.nio.file.FileAlreadyExistsException)) - e.printStackTrace(); + if (!(e instanceof java.nio.file.FileAlreadyExistsException)) e.printStackTrace(); } } } private static void checkSettingsFile() { - boolean settingsFileEmpty = settingsFileExists() && new File(settingsFilePath.toString()).length() == 0; + boolean settingsFileEmpty = + settingsFileExists() && new File(settingsFilePath.toString()).length() == 0; if (settingsFileEmpty || !settingsFileExists()) { try { JacksonUtils.serializer(settingsFilePath, settings, true); @@ -91,7 +90,8 @@ public class ConfigManager { saveSettingsFile(); } - public static List initializeCameras(List preliminaryConfigs) { + public static List initializeCameras( + List preliminaryConfigs) { List configList = new ArrayList<>(); checkSettingsFolder(); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java index b4c66e083..22d11c6d8 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/FullCameraConfiguration.java @@ -1,7 +1,6 @@ package com.chameleonvision._2.config; import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; - import java.util.List; public class FullCameraConfiguration { @@ -11,7 +10,12 @@ public class FullCameraConfiguration { public final List calibration; public final CameraConfig fileConfig; - FullCameraConfiguration(CameraJsonConfig cameraConfig, List pipelines, CVPipelineSettings driverMode, List calibration, CameraConfig fileConfig) { + FullCameraConfiguration( + CameraJsonConfig cameraConfig, + List pipelines, + CVPipelineSettings driverMode, + List calibration, + CameraConfig fileConfig) { this.cameraConfig = cameraConfig; this.pipelines = pipelines; this.driverMode = driverMode; diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java index 647b65f71..9b28492bb 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/GeneralSettings.java @@ -3,12 +3,12 @@ package com.chameleonvision._2.config; import com.chameleonvision.common.networking.NetworkMode; public class GeneralSettings { - public int teamNumber = 1577; - public NetworkMode connectionType = NetworkMode.DHCP; - public String ip = ""; - public String gateway = ""; - public String netmask = ""; - public String hostname = "Chameleon-vision"; - public String currentCamera = ""; - public Integer currentPipeline = null; + public int teamNumber = 1577; + public NetworkMode connectionType = NetworkMode.DHCP; + public String ip = ""; + public String gateway = ""; + public String netmask = ""; + public String hostname = "Chameleon-vision"; + public String currentCamera = ""; + public Integer currentPipeline = null; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java index 69b8341ce..8ec5e3013 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/JsonMat.java @@ -1,18 +1,22 @@ package com.chameleonvision._2.config; +import com.chameleonvision.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; -import java.util.Arrays; - - -public class JsonMat { +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); } @@ -28,10 +32,6 @@ public class JsonMat { this.data = data; } - public Mat toMat() { - return toMat(this); - } - private static boolean isCameraMatrixMat(Mat mat) { return mat.type() == CvType.CV_64FC1 && mat.cols() == 3 && mat.rows() == 3; } @@ -44,10 +44,11 @@ public class JsonMat { 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())]; + double[] data = new double[(int) (mat.total() * mat.elemSize())]; mat.get(0, 0, data); int dataLen = -1; @@ -64,11 +65,28 @@ public class JsonMat { return new JsonMat(mat.rows(), mat.cols(), getDataFromMat(mat)); } - public static Mat toMat(JsonMat jsonMat) { - if (jsonMat.type != CvType.CV_64FC1) return null; + @JsonIgnore + public Mat getAsMat() { + if (this.type != CvType.CV_64FC1) return null; - Mat retMat = new Mat(jsonMat.rows, jsonMat.cols, jsonMat.type); - retMat.put(0, 0, jsonMat.data); - return retMat; + 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(); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java index fdc95042b..ae48c7917 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/PipelineConfig.java @@ -6,7 +6,6 @@ import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; import com.chameleonvision.common.util.file.FileUtils; import com.chameleonvision.common.util.file.JacksonUtils; - import java.io.File; import java.io.IOException; import java.nio.file.Files; @@ -20,10 +19,10 @@ public class PipelineConfig { private final CameraConfig cameraConfig; /** - * Construct a new PipelineConfig - * - * @param cameraConfig the CameraConfig (parent folder, kinda?) - */ + * Construct a new PipelineConfig + * + * @param cameraConfig the CameraConfig (parent folder, kinda?) + */ PipelineConfig(CameraConfig cameraConfig) { this.cameraConfig = cameraConfig; } @@ -76,7 +75,12 @@ public class PipelineConfig { if (settings instanceof StandardCVPipelineSettings) { try { - JacksonUtils.serialize(path, (StandardCVPipelineSettings) settings, StandardCVPipelineSettings.class, new StandardCVPipelineSettingsSerializer(), true); + JacksonUtils.serialize( + path, + (StandardCVPipelineSettings) settings, + StandardCVPipelineSettings.class, + new StandardCVPipelineSettingsSerializer(), + true); FileUtils.setFilePerms(path); } catch (IOException e) { e.printStackTrace(); @@ -131,7 +135,11 @@ public class PipelineConfig { } else { for (File pipelineFile : pipelineFiles) { try { - var pipe = JacksonUtils.deserialize(Paths.get(pipelineFile.getPath()), StandardCVPipelineSettings.class, new StandardCVPipelineSettingsDeserializer()); + var pipe = + JacksonUtils.deserialize( + Paths.get(pipelineFile.getPath()), + StandardCVPipelineSettings.class, + new StandardCVPipelineSettingsDeserializer()); deserializedList.add(pipe); } catch (IOException e) { System.err.println("couldn't load cvpipeline2d"); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java index 863e4ba11..9de4c304a 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseDeserializer.java @@ -8,12 +8,11 @@ import com.fasterxml.jackson.databind.ObjectMapper; import com.fasterxml.jackson.databind.deser.std.StdDeserializer; import com.fasterxml.jackson.databind.type.CollectionType; import com.fasterxml.jackson.databind.type.TypeFactory; -import org.opencv.core.MatOfPoint3f; -import org.opencv.core.Point3; - import java.io.IOException; import java.util.ArrayList; import java.util.List; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point3; public abstract class BaseDeserializer extends StdDeserializer { protected BaseDeserializer(Class vc) { @@ -22,14 +21,18 @@ public abstract class BaseDeserializer extends StdDeserializer { JsonNode baseNode; - private static final CollectionType numberListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Number.class); - private CollectionType pointListColType = TypeFactory.defaultInstance().constructCollectionType(List.class, Object.class); + private static final CollectionType numberListColType = + TypeFactory.defaultInstance().constructCollectionType(List.class, Number.class); + private CollectionType pointListColType = + TypeFactory.defaultInstance().constructCollectionType(List.class, Object.class); private static final ObjectMapper mapper = new ObjectMapper(); + private static boolean nodeGood(JsonNode node) { return node != null && !node.toString().equals(""); } - IntegerCouple getNumberCouple(String name, IntegerCouple defaultValue) throws JsonProcessingException { + IntegerCouple getNumberCouple(String name, IntegerCouple defaultValue) + throws JsonProcessingException { JsonNode node = baseNode.get(name); if (nodeGood(node)) { @@ -39,7 +42,8 @@ public abstract class BaseDeserializer extends StdDeserializer { return defaultValue; } - DoubleCouple getNumberCouple(String name, DoubleCouple defaultValue) throws JsonProcessingException { + DoubleCouple getNumberCouple(String name, DoubleCouple defaultValue) + throws JsonProcessingException { JsonNode node = baseNode.get(name); if (nodeGood(node)) { @@ -49,7 +53,8 @@ public abstract class BaseDeserializer extends StdDeserializer { return defaultValue; } - List getNumberList(String name, List defaultValue) throws JsonProcessingException { + List getNumberList(String name, List defaultValue) + throws JsonProcessingException { JsonNode node = baseNode.get(name); if (nodeGood(node)) { @@ -69,7 +74,7 @@ public abstract class BaseDeserializer extends StdDeserializer { } int getInt(String name, int defaultValue) { - return (int) getDouble(name, defaultValue); + return (int) getDouble(name, defaultValue); } double getDouble(String name, double defaultValue) { @@ -92,7 +97,8 @@ public abstract class BaseDeserializer extends StdDeserializer { return defaultValue; } - > E getEnum(String name, Class enumClass, E defaultValue) throws IOException { + > E getEnum(String name, Class enumClass, E defaultValue) + throws IOException { JsonNode node = baseNode.get(name); if (nodeGood(node)) { @@ -108,12 +114,14 @@ public abstract class BaseDeserializer extends StdDeserializer { return defaultValue; } - MatOfPoint3f getMatOfPoint3f(String name, MatOfPoint3f defaultValue) throws JsonProcessingException { + + MatOfPoint3f getMatOfPoint3f(String name, MatOfPoint3f defaultValue) + throws JsonProcessingException { JsonNode node = baseNode.get(name); - if (nodeGood(node)){ + if (nodeGood(node)) { List> numberList = mapper.readValue(node.toString(), pointListColType); List point3List = new ArrayList<>(); - for (List tmp : numberList){ + for (List tmp : numberList) { Point3 p = new Point3(); p.x = tmp.get(0).doubleValue(); p.y = tmp.get(1).doubleValue(); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java index 6826332ca..fcb3f9e9d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/BaseSerializer.java @@ -3,11 +3,10 @@ package com.chameleonvision._2.config.serializers; import com.chameleonvision.common.util.numbers.NumberCouple; import com.fasterxml.jackson.core.JsonGenerator; import com.fasterxml.jackson.databind.ser.std.StdSerializer; -import org.opencv.core.MatOfPoint3f; -import org.opencv.core.Point3; - import java.io.IOException; import java.util.List; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point3; public abstract class BaseSerializer extends StdSerializer { protected BaseSerializer(Class t) { @@ -16,7 +15,8 @@ public abstract class BaseSerializer extends StdSerializer { JsonGenerator generator; - void writeNumberCoupleAsNumberArray(String name, N couple) throws IOException { + void writeNumberCoupleAsNumberArray(String name, N couple) + throws IOException { generator.writeArrayFieldStart(name); generator.writeObject(couple.getFirst()); generator.writeObject(couple.getSecond()); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java index 679545cba..012010010 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/config/serializers/StandardCVPipelineSettingsDeserializer.java @@ -5,10 +5,10 @@ import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; import com.fasterxml.jackson.core.JsonParser; import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationContext; - import java.io.IOException; -public class StandardCVPipelineSettingsDeserializer extends BaseDeserializer { +public class StandardCVPipelineSettingsDeserializer + extends BaseDeserializer { public StandardCVPipelineSettingsDeserializer() { this(null); } @@ -18,7 +18,8 @@ public class StandardCVPipelineSettingsDeserializer extends BaseDeserializer { +public class StandardCVPipelineSettingsSerializer + extends BaseSerializer { public StandardCVPipelineSettingsSerializer() { this(null); } @@ -16,7 +16,9 @@ public class StandardCVPipelineSettingsSerializer extends BaseSerializer getNetworkInterfaces() throws SocketException { List netInterfaces; @@ -98,6 +97,5 @@ public class LinuxNetworking extends SysNetworking { goodInterfaces.add(netInterface); } return goodInterfaces; - } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java index d68a2074a..95a4d1053 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetmaskToCIDR.java @@ -3,7 +3,8 @@ package com.chameleonvision._2.network; import java.net.InetAddress; public class NetmaskToCIDR { - //code belongs to https://stackoverflow.com/questions/19531411/calculate-cidr-from-a-given-netmask-java + // code belongs to + // https://stackoverflow.com/questions/19531411/calculate-cidr-from-a-given-netmask-java public static int convertNetmaskToCIDR(InetAddress netmask) { byte[] netmaskBytes = netmask.getAddress(); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java index 6b369e692..577b84e4f 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkInterface.java @@ -4,19 +4,19 @@ import java.net.InterfaceAddress; @SuppressWarnings("WeakerAccess") public class NetworkInterface { - public final String name; - public final String displayName; - public final String IPAddress; - public final String Netmask; - public final String Gateway; - public final String Broadcast; + public final String name; + public final String displayName; + public final String IPAddress; + public final String Netmask; + public final String Gateway; + public final String Broadcast; - public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) { - name = inetface.getName(); - displayName = inetface.getDisplayName(); + public NetworkInterface(java.net.NetworkInterface inetface, InterfaceAddress ifaceAddress) { + name = inetface.getName(); + displayName = inetface.getDisplayName(); - var inetAddress = ifaceAddress.getAddress(); - IPAddress = inetAddress.getHostAddress(); + var inetAddress = ifaceAddress.getAddress(); + IPAddress = inetAddress.getHostAddress(); Netmask = getIPv4LocalNetMask(ifaceAddress); // TODO: (low) hack to "get" gateway, this is gross and bad, pls fix @@ -25,14 +25,14 @@ public class NetworkInterface { Gateway = String.join(".", splitIPAddr); splitIPAddr[3] = "255"; Broadcast = String.join(".", splitIPAddr); - } + } private static String getIPv4LocalNetMask(InterfaceAddress interfaceAddress) { - var netPrefix = interfaceAddress.getNetworkPrefixLength(); + var netPrefix = interfaceAddress.getNetworkPrefixLength(); try { // Since this is for IPv4, it's 32 bits, so set the sign value of // the int to "negative"... - int shiftby = (1<<31); + int shiftby = (1 << 31); // For the number of bits of the prefix -1 (we already set the sign bit) for (int i = netPrefix - 1; i > 0; i--) { // Shift the sign right... Java makes the sign bit sticky on a shift... @@ -41,11 +41,16 @@ public class NetworkInterface { } // Transform the resulting value in xxx.xxx.xxx.xxx format, like if /// it was a standard address... - // Return the address thus created... - return ((shiftby >> 24) & 255) + "." + ((shiftby >> 16) & 255) + "." + ((shiftby >> 8) & 255) + "." + (shiftby & 255); -// return InetAddress.getByName(maskString); - } - catch(Exception e) { + // Return the address thus created... + return ((shiftby >> 24) & 255) + + "." + + ((shiftby >> 16) & 255) + + "." + + ((shiftby >> 8) & 255) + + "." + + (shiftby & 255); + // return InetAddress.getByName(maskString); + } catch (Exception e) { e.printStackTrace(); } // Something went wrong here... diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java index be5e28ea7..ea96d41ec 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/NetworkManager.java @@ -2,14 +2,12 @@ package com.chameleonvision._2.network; import com.chameleonvision._2.config.ConfigManager; import com.chameleonvision.common.util.Platform; - import java.net.SocketException; import java.util.ArrayList; import java.util.List; public class NetworkManager { - private NetworkManager() { - } + private NetworkManager() {} private static SysNetworking networking; private static boolean isManaged = false; @@ -25,7 +23,7 @@ public class NetworkManager { if (platform.isLinux()) { networking = new LinuxNetworking(); } else if (platform.isWindows()) { -// networking = new WindowsNetworking(); + // networking = new WindowsNetworking(); System.out.println("Windows networking is not yet supported. Running unmanaged."); return; } @@ -71,10 +69,9 @@ public class NetworkManager { } private static byte[] GetTeamNumberIPBytes(int teamNumber) { - return new byte[]{(byte) (teamNumber / 100), (byte) (teamNumber % 100)}; + return new byte[] {(byte) (teamNumber / 100), (byte) (teamNumber % 100)}; } - private static boolean setDHCP() { if (!isManaged) { return true; diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java index 707a9c384..68291f04d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/SysNetworking.java @@ -1,36 +1,38 @@ package com.chameleonvision._2.network; import com.chameleonvision.common.util.ShellExec; - import java.io.IOException; import java.net.SocketException; import java.util.List; public abstract class SysNetworking { - NetworkInterface networkInterface; - ShellExec shell = new ShellExec(true, true); + NetworkInterface networkInterface; + ShellExec shell = new ShellExec(true, true); - public String getHostname() { - try { - var retCode = shell.execute("hostname", null, true); - if (retCode == 0) { - while(!shell.isOutputCompleted()) {} - return shell.getOutput(); - } else { - return null; - } - } catch (IOException e) { - return null; - } - } + public String getHostname() { + try { + var retCode = shell.execute("hostname", null, true); + if (retCode == 0) { + while (!shell.isOutputCompleted()) {} + return shell.getOutput(); + } else { + return null; + } + } catch (IOException e) { + return null; + } + } - public void setNetworkInterface(NetworkInterface networkInterface) { - this.networkInterface = networkInterface; - } - public abstract boolean setDHCP(); - public abstract boolean setHostname(String hostname); - public abstract boolean setStatic(String ipAddress, String netmask, String gateway); - public abstract List getNetworkInterfaces() throws SocketException; + public void setNetworkInterface(NetworkInterface networkInterface) { + this.networkInterface = networkInterface; + } + public abstract boolean setDHCP(); + + public abstract boolean setHostname(String hostname); + + public abstract boolean setStatic(String ipAddress, String netmask, String gateway); + + public abstract List getNetworkInterfaces() throws SocketException; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java index 9e77d3b8b..7565585f6 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/network/WindowsNetworking.java @@ -7,48 +7,51 @@ import java.util.List; public class WindowsNetworking extends SysNetworking { - @Override - public boolean setDHCP() { - return false; - } + @Override + public boolean setDHCP() { + return false; + } - @Override - public boolean setHostname(String newHostname) { - var currentHostname = getHostname(); + @Override + public boolean setHostname(String newHostname) { + var currentHostname = getHostname(); - if (getHostname() == null) { - return false; - } + if (getHostname() == null) { + return false; + } - String command = String.format("wmic computersystem where name=\"%s\" call rename name=\"%s\"", currentHostname, newHostname); + String command = + String.format( + "wmic computersystem where name=\"%s\" call rename name=\"%s\"", + currentHostname, newHostname); - try { - var process = Runtime.getRuntime().exec(command); - var returnCode = process.waitFor(); - return returnCode == 0; - } catch(Exception e) { - return false; - } - } + try { + var process = Runtime.getRuntime().exec(command); + var returnCode = process.waitFor(); + return returnCode == 0; + } catch (Exception e) { + return false; + } + } - @Override - public boolean setStatic(String ipAddress, String netmask, String gateway) { - return false; - } + @Override + public boolean setStatic(String ipAddress, String netmask, String gateway) { + return false; + } - @Override - public List getNetworkInterfaces() throws SocketException { - var netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces()); + @Override + public List getNetworkInterfaces() throws SocketException { + var netInterfaces = Collections.list(java.net.NetworkInterface.getNetworkInterfaces()); - List goodInterfaces = new ArrayList<>(); + List goodInterfaces = new ArrayList<>(); - for (var netInterface : netInterfaces) { - if (netInterface.getDisplayName().toLowerCase().contains("bluetooth")) continue; - if (netInterface.getDisplayName().toLowerCase().contains("virtual")) continue; - if (netInterface.getDisplayName().toLowerCase().contains("loopback")) continue; - if (!netInterface.isUp()) continue; - goodInterfaces.add(netInterface); - } - return goodInterfaces; - } + for (var netInterface : netInterfaces) { + if (netInterface.getDisplayName().toLowerCase().contains("bluetooth")) continue; + if (netInterface.getDisplayName().toLowerCase().contains("virtual")) continue; + if (netInterface.getDisplayName().toLowerCase().contains("loopback")) continue; + if (!netInterface.isUp()) continue; + goodInterfaces.add(netInterface); + } + return goodInterfaces; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java b/chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java index 2d48b5575..fa6a044da 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/util/Helpers.java @@ -1,7 +1,6 @@ package com.chameleonvision._2.util; import edu.wpi.cscore.VideoMode; - import java.io.File; import java.io.FileWriter; import java.io.IOException; @@ -13,31 +12,32 @@ public class Helpers { // TODO: MOVE public static HashMap VideoModeToHashMap(VideoMode videoMode) { - return new HashMap() {{ - put("width", videoMode.width); - put("height", videoMode.height); - put("fps", videoMode.fps); - put("pixelFormat", videoMode.pixelFormat.toString()); - }}; + return new HashMap() { + { + put("width", videoMode.width); + put("height", videoMode.height); + put("fps", videoMode.fps); + put("pixelFormat", videoMode.pixelFormat.toString()); + } + }; } - // TODO: MOVE private static final String kServicePath = "/etc/systemd/system/chameleonVision.service"; - private static final String kServiceString = "[Unit]\n" + - "Description=chameleon vision\n" + - "\n" + - "[Service]\n" + - "ExecStart=/usr/bin/java -jar %s \n" + - "StandardOutput=file:/var/log/chameleon.out.txt\n" + - "StandardError=file:/var/log/chameleon.err.txt\n" + - "Type=simple\n" + - "WorkingDirectory=/usr/local/bin\n" + - "\n" + - "[Install]\n" + - "WantedBy=multi-user.target\n" + - "\n"; - + private static final String kServiceString = + "[Unit]\n" + + "Description=chameleon vision\n" + + "\n" + + "[Service]\n" + + "ExecStart=/usr/bin/java -jar %s \n" + + "StandardOutput=file:/var/log/chameleon.out.txt\n" + + "StandardError=file:/var/log/chameleon.err.txt\n" + + "Type=simple\n" + + "WorkingDirectory=/usr/local/bin\n" + + "\n" + + "[Install]\n" + + "WantedBy=multi-user.target\n" + + "\n"; public static void setService(Path filePath) throws IOException, InterruptedException { String newService = String.format(kServiceString, filePath.toString()); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java b/chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java index 90bb10545..3b231563e 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/util/ProgramDirectoryUtilities.java @@ -5,10 +5,12 @@ import java.net.URISyntaxException; public class ProgramDirectoryUtilities { private static String getJarName() { - return new File(ProgramDirectoryUtilities.class.getProtectionDomain() - .getCodeSource() - .getLocation() - .getPath()) + return new File( + ProgramDirectoryUtilities.class + .getProtectionDomain() + .getCodeSource() + .getLocation() + .getPath()) .getName(); } @@ -27,11 +29,18 @@ public class ProgramDirectoryUtilities { private static String getCurrentJARDirectory() { try { - return new File(ProgramDirectoryUtilities.class.getProtectionDomain().getCodeSource().getLocation().toURI().getPath()).getParent(); + return new File( + ProgramDirectoryUtilities.class + .getProtectionDomain() + .getCodeSource() + .getLocation() + .toURI() + .getPath()) + .getParent(); } catch (URISyntaxException exception) { exception.printStackTrace(); } return null; } -} \ No newline at end of file +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java index 4c11e6dc3..7e0d1628b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionManager.java @@ -8,17 +8,17 @@ import com.chameleonvision._2.vision.pipeline.CVPipelineSettings; import com.chameleonvision.common.util.Platform; import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.UsbCameraInfo; -import org.opencv.videoio.VideoCapture; - import java.util.*; import java.util.stream.Collectors; import java.util.stream.Stream; +import org.opencv.videoio.VideoCapture; @SuppressWarnings("rawtypes") public class VisionManager { private VisionManager() {} - private static final LinkedHashMap usbCameraInfosByCameraName = new LinkedHashMap<>(); + private static final LinkedHashMap usbCameraInfosByCameraName = + new LinkedHashMap<>(); private static final LinkedList loadedCameraConfigs = new LinkedList<>(); private static final LinkedList visionProcesses = new LinkedList<>(); @@ -43,7 +43,8 @@ public class VisionManager { VideoCapture cap = new VideoCapture(info.dev); if (cap.isOpened()) { cap.release(); - // Filter non-ascii characters because ext4 doesn't play nice with unicode in directory names + // Filter non-ascii characters because ext4 doesn't play nice with unicode in directory + // names String name = info.name.replaceAll("[^\\x00-\\x7F]", ""); while (usbCameraInfosByCameraName.containsKey(name)) { suffix++; @@ -61,17 +62,22 @@ public class VisionManager { // load the config List preliminaryConfigs = new ArrayList<>(); - usbCameraInfosByCameraName.forEach((suffixedName, cameraInfo) -> { - String truePath; + usbCameraInfosByCameraName.forEach( + (suffixedName, cameraInfo) -> { + String truePath; - if (Platform.CurrentPlatform.isWindows()) { - truePath = cameraInfo.path; - } else { - truePath = Arrays.stream(cameraInfo.otherPaths).filter(x -> x.contains("/dev/v4l/by-path")).findFirst().orElse(cameraInfo.path); - } + if (Platform.CurrentPlatform.isWindows()) { + truePath = cameraInfo.path; + } else { + truePath = + Arrays.stream(cameraInfo.otherPaths) + .filter(x -> x.contains("/dev/v4l/by-path")) + .findFirst() + .orElse(cameraInfo.path); + } - preliminaryConfigs.add(new CameraJsonConfig(truePath, suffixedName)); - }); + preliminaryConfigs.add(new CameraJsonConfig(truePath, suffixedName)); + }); loadedCameraConfigs.addAll(ConfigManager.initializeCameras(preliminaryConfigs)); System.out.printf("[VisionManager] Loaded %s cameras!\n", loadedCameraConfigs.size()); @@ -92,7 +98,9 @@ public class VisionManager { currentUIVisionProcess = getVisionProcessByIndex(0); ConfigManager.settings.currentCamera = visionProcesses.get(0).name; - System.out.printf("[VisionManager] Loaded %s vision processes! Current process: %s\n", visionProcesses.size(), visionProcesses.get(0).name); + System.out.printf( + "[VisionManager] Loaded %s vision processes! Current process: %s\n", + visionProcesses.size(), visionProcesses.get(0).name); return true; } @@ -110,7 +118,12 @@ public class VisionManager { public static CameraConfig getCameraConfig(VisionProcess process) { String cameraName = process.getCamera().getProperties().name; - return Objects.requireNonNull(loadedCameraConfigs.stream().filter(x -> x.cameraConfig.name.equals(cameraName)).findFirst().orElse(null)).fileConfig; + return Objects.requireNonNull( + loadedCameraConfigs.stream() + .filter(x -> x.cameraConfig.name.equals(cameraName)) + .findFirst() + .orElse(null)) + .fileConfig; } public static void setCurrentProcessByIndex(int processIndex) { @@ -127,32 +140,42 @@ public class VisionManager { return null; } - VisionProcessManageable vpm = visionProcesses.stream().filter(manageable -> manageable.index == processIndex).findFirst().orElse(null); + VisionProcessManageable vpm = + visionProcesses.stream() + .filter(manageable -> manageable.index == processIndex) + .findFirst() + .orElse(null); return vpm != null ? vpm.visionProcess : null; } public static List getAllCameraNicknames() { - return visionProcesses.stream().map(vpm -> vpm.visionProcess.getCamera() - .getProperties().getNickname()).collect(Collectors.toList()); + return visionProcesses.stream() + .map(vpm -> vpm.visionProcess.getCamera().getProperties().getNickname()) + .collect(Collectors.toList()); } public static List getCurrentCameraPipelineNicknames() { - return currentUIVisionProcess.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings.nickname).collect(Collectors.toList()); + return currentUIVisionProcess.pipelineManager.pipelines.stream() + .map(cvPipeline -> cvPipeline.settings.nickname) + .collect(Collectors.toList()); } - public static void saveAllCameras() { - visionProcesses.forEach((vpm) -> { - VisionProcess process = vpm.visionProcess; - String cameraName = process.getCamera().getProperties().name; - Stream pipelineStream = process.pipelineManager.pipelines.stream(); - List pipelines = process.pipelineManager.pipelines.stream().map(cvPipeline -> cvPipeline.settings).collect(Collectors.toList()); - CVPipelineSettings driverMode = process.getDriverModeSettings(); - CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(process); - ConfigManager.saveCameraPipelines(cameraName, pipelines); - ConfigManager.saveCameraDriverMode(cameraName, driverMode); - ConfigManager.saveCameraConfig(cameraName, config); - }); + visionProcesses.forEach( + (vpm) -> { + VisionProcess process = vpm.visionProcess; + String cameraName = process.getCamera().getProperties().name; + Stream pipelineStream = process.pipelineManager.pipelines.stream(); + List pipelines = + process.pipelineManager.pipelines.stream() + .map(cvPipeline -> cvPipeline.settings) + .collect(Collectors.toList()); + CVPipelineSettings driverMode = process.getDriverModeSettings(); + CameraJsonConfig config = CameraJsonConfig.fromVisionProcess(process); + ConfigManager.saveCameraPipelines(cameraName, pipelines); + ConfigManager.saveCameraDriverMode(cameraName, driverMode); + ConfigManager.saveCameraConfig(cameraName, config); + }); } private static String getCurrentCameraName() { @@ -173,7 +196,9 @@ public class VisionManager { } private static List getCameraResolutionList(USBCameraCapture capture) { - return capture.getProperties().getVideoModes().stream().map(Helpers::VideoModeToHashMap).collect(Collectors.toList()); + return capture.getProperties().getVideoModes().stream() + .map(Helpers::VideoModeToHashMap) + .collect(Collectors.toList()); } public static List getCurrentCameraResolutionList() { @@ -181,7 +206,11 @@ public class VisionManager { } public static int getCurrentUIVisionProcessIndex() { - VisionProcessManageable vpm = visionProcesses.stream().filter(v -> v.visionProcess == currentUIVisionProcess).findFirst().orElse(null); + VisionProcessManageable vpm = + visionProcesses.stream() + .filter(v -> v.visionProcess == currentUIVisionProcess) + .findFirst() + .orElse(null); return vpm != null ? vpm.index : -1; } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java index 26fcba551..22ec5c603 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/VisionProcess.java @@ -23,14 +23,12 @@ import edu.wpi.cscore.VideoMode; import edu.wpi.first.networktables.*; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpiutil.CircularBuffer; -import org.apache.commons.lang3.tuple.Pair; -import org.opencv.core.Mat; - import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.stream.Collectors; - +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; @SuppressWarnings("rawtypes") public class VisionProcess { @@ -75,31 +73,38 @@ public class VisionProcess { pipelineManager = new PipelineManager(this, config.pipelines); // Thread to put frames on the dashboard - this.cameraStreamer = new CameraStreamer(cameraCapture, config.cameraConfig.name, pipelineManager.getCurrentPipeline().settings.streamDivisor); + this.cameraStreamer = + new CameraStreamer( + cameraCapture, + config.cameraConfig.name, + pipelineManager.getCurrentPipeline().settings.streamDivisor); // Thread to process vision data this.visionRunnable = new VisionProcessRunnable(); // network table - defaultTable = NetworkTableInstance.getDefault().getTable("/chameleon-vision/" + cameraCapture.getProperties().getNickname()); + defaultTable = + NetworkTableInstance.getDefault() + .getTable("/chameleon-vision/" + cameraCapture.getProperties().getNickname()); } public void start() { - System.out.printf("[%s Process] Creating network table...\n", getCamera().getProperties().getNickname()); + System.out.printf( + "[%s Process] Creating network table...\n", getCamera().getProperties().getNickname()); initNT(defaultTable); - System.out.printf("[%s Process] Starting vision thread...\n", getCamera().getProperties().getNickname()); + System.out.printf( + "[%s Process] Starting vision thread...\n", getCamera().getProperties().getNickname()); var visionThread = new Thread(visionRunnable); visionThread.setName(getCamera().getProperties().name + " - Vision Thread"); visionThread.start(); } /** - * Removes the old value change listeners - * calls {@link #initNT} - * - * @param newTable passed to {@link #initNT} - */ + * Removes the old value change listeners calls {@link #initNT} + * + * @param newTable passed to {@link #initNT} + */ public void resetNT(NetworkTable newTable) { ntDriverModeEntry.removeListener(ntDriveModeListenerID); ntPipelineEntry.removeListener(ntPipelineListenerID); @@ -128,8 +133,10 @@ public class VisionProcess { ntBoundingHeightEntry = camTable.getEntry("targetBoundingHeight"); ntBoundingWidthEntry = camTable.getEntry("targetBoundingWidth"); ntTargetRotation = camTable.getEntry("targetRotation"); - ntDriveModeListenerID = ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate); - ntPipelineListenerID = ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate); + ntDriveModeListenerID = + ntDriverModeEntry.addListener(this::setDriverMode, EntryListenerFlags.kUpdate); + ntPipelineListenerID = + ntPipelineEntry.addListener(this::setPipeline, EntryListenerFlags.kUpdate); ntDriverModeEntry.setBoolean(false); ntPipelineEntry.setNumber(pipelineManager.getCurrentPipelineIndex()); pipelineManager.ntIndexEntry = ntPipelineEntry; @@ -141,15 +148,16 @@ public class VisionProcess { public void setDriverMode(boolean driverMode) { pipelineManager.setDriverMode(driverMode); - ScriptManager.queueEvent(driverMode ? ScriptEventType.kEnterDriverMode : ScriptEventType.kExitDriverMode); + ScriptManager.queueEvent( + driverMode ? ScriptEventType.kEnterDriverMode : ScriptEventType.kExitDriverMode); SocketHandler.sendFullSettings(); } /** - * Method called by the nt entry listener to update the next pipeline. - * - * @param notification the notification - */ + * Method called by the nt entry listener to update the next pipeline. + * + * @param notification the notification + */ private void setPipeline(EntryNotification notification) { var wantedPipelineIndex = (int) notification.value.getDouble(); if (pipelineManager.pipelines.size() - 1 < wantedPipelineIndex) { @@ -173,7 +181,6 @@ public class VisionProcess { if (currentMillis - lastUIUpdateMs > 1000 / 30) { lastUIUpdateMs = currentMillis; - if (cameraCapture.getProperties().name.equals(ConfigManager.settings.currentCamera)) { HashMap WebSend = new HashMap<>(); HashMap point = new HashMap<>(); @@ -181,13 +188,14 @@ public class VisionProcess { ArrayList webTargets = new ArrayList<>(); List center = new ArrayList<>(); - if (data.hasTarget) { if (data instanceof StandardCVPipeline.StandardCVPipelineResult) { - StandardCVPipeline.StandardCVPipelineResult result = (StandardCVPipeline.StandardCVPipelineResult) data; + StandardCVPipeline.StandardCVPipelineResult result = + (StandardCVPipeline.StandardCVPipelineResult) data; StandardCVPipeline.TrackedTarget bestTarget = result.targets.get(0); try { - if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings).multiple) { + if (((StandardCVPipelineSettings) pipelineManager.getCurrentPipeline().settings) + .multiple) { for (var target : result.targets) { pointMap = new HashMap<>(); pointMap.put("pitch", target.pitch); @@ -206,7 +214,7 @@ public class VisionProcess { center.add(bestTarget.minAreaRect.center.x); center.add(bestTarget.minAreaRect.center.y); } catch (ClassCastException ignored) { - + } } else { pointMap.put("pitch", null); @@ -236,7 +244,8 @@ public class VisionProcess { if (data instanceof StandardCVPipeline.StandardCVPipelineResult) { //noinspection unchecked - List targets = (List) data.targets; + List targets = + (List) data.targets; StandardCVPipeline.TrackedTarget bestTarget = targets.get(0); ntLatencyEntry.setDouble(MathUtils.roundTo(data.processTime * 1e-6, 3)); ntPitchEntry.setDouble(bestTarget.pitch); @@ -249,12 +258,30 @@ public class VisionProcess { ntTargetRotation.setDouble(bestTarget.minAreaRect.angle); try { Pose2d targetPose = targets.get(0).cameraRelativePose; - double[] targetArray = {targetPose.getTranslation().getX(), targetPose.getTranslation().getY(), targetPose.getRotation().getDegrees()}; + double[] targetArray = { + targetPose.getTranslation().getX(), + targetPose.getTranslation().getY(), + targetPose.getRotation().getDegrees() + }; ntPoseEntry.setDoubleArray(targetArray); -// ntPoseEntry.setString(objectMapper.writeValueAsString(targets.get(0).cameraRelativePose)); - ntAuxListEntry.setString(objectMapper.writeValueAsString(targets.stream() - .map(it -> List.of(it.pitch, it.yaw, it.area, it.boundingRect.width, it.boundingRect.height, it.minAreaRect.size.width, it.minAreaRect.size.height, it.minAreaRect.angle, it.cameraRelativePose)) - .collect(Collectors.toList()))); + // + // ntPoseEntry.setString(objectMapper.writeValueAsString(targets.get(0).cameraRelativePose)); + ntAuxListEntry.setString( + objectMapper.writeValueAsString( + targets.stream() + .map( + it -> + List.of( + it.pitch, + it.yaw, + it.area, + it.boundingRect.width, + it.boundingRect.height, + it.minAreaRect.size.width, + it.minAreaRect.size.height, + it.minAreaRect.angle, + it.cameraRelativePose)) + .collect(Collectors.toList()))); } catch (JsonProcessingException e) { e.printStackTrace(); } @@ -311,9 +338,7 @@ public class VisionProcess { return false; } - /** - * VisionProcessRunnable will process images as quickly as possible - */ + /** VisionProcessRunnable will process images as quickly as possible */ private class VisionProcessRunnable implements Runnable { volatile Double fps = 0.0; @@ -324,21 +349,23 @@ public class VisionProcess { var lastUpdateTimeNanos = System.nanoTime(); var lastStreamTimeMs = System.currentTimeMillis(); - System.out.printf("[%s Process] Vision Process Thread -- first run!\n", getCamera().getProperties().getNickname()); + System.out.printf( + "[%s Process] Vision Process Thread -- first run!\n", + getCamera().getProperties().getNickname()); while (!Thread.interrupted()) { // blocking call, will block until camera has a new frame. Pair camData = cameraCapture.getFrame(); - Mat camFrame = camData.getLeft(); if (camFrame.cols() > 0 && camFrame.rows() > 0) { CVPipelineResult result = null; try { result = pipelineManager.getCurrentPipeline().runPipeline(camFrame); } catch (Exception e) { - System.err.println("Exception in vision process " + getCamera().getProperties().getNickname() + "!"); + System.err.println( + "Exception in vision process " + getCamera().getProperties().getNickname() + "!"); e.printStackTrace(); } @@ -355,18 +382,22 @@ public class VisionProcess { try { var currentTime = System.currentTimeMillis(); if ((currentTime - lastStreamTimeMs) / 1000d > 1.0 / 30.0) { - if(lastPipelineResult != null) { + if (lastPipelineResult != null) { cameraStreamer.runStream(lastPipelineResult.outputMat); lastStreamTimeMs = currentTime; lastPipelineResult.outputMat.release(); } else { - System.err.printf("[%s Process] Last pipeline result was null!\n", getCamera().getProperties().getNickname()); + System.err.printf( + "[%s Process] Last pipeline result was null!\n", + getCamera().getProperties().getNickname()); } } } catch (Exception e) { -// Debug.printInfo("Vision running faster than stream."); - System.err.printf("[%s Process] Exception in vision thread!\n", getCamera().getProperties().getNickname()); + // Debug.printInfo("Vision running faster than stream."); + System.err.printf( + "[%s Process] Exception in vision thread!\n", + getCamera().getProperties().getNickname()); e.printStackTrace(); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java index 8d13b8a91..0fdd42955 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraCapture.java @@ -4,7 +4,6 @@ import com.chameleonvision._2.config.CameraCalibrationConfig; import com.chameleonvision._2.vision.image.CaptureProperties; import com.chameleonvision._2.vision.image.ImageCapture; import edu.wpi.cscore.VideoMode; - import java.util.List; public interface CameraCapture extends ImageCapture { @@ -13,36 +12,41 @@ public interface CameraCapture extends ImageCapture { VideoMode getCurrentVideoMode(); /** - * Set the exposure of the camera - * @param exposure the new exposure to set the camera to - */ + * Set the exposure of the camera + * + * @param exposure the new exposure to set the camera to + */ void setExposure(int exposure); /** - * Set the brightness of the camera - * @param brightness the new brightness to set the camera to - */ + * Set the brightness of the camera + * + * @param brightness the new brightness to set the camera to + */ void setBrightness(int brightness); /** - * Set the video mode (fps and resolution) of the camera - * @param mode the desired mode - */ + * Set the video mode (fps and resolution) of the camera + * + * @param mode the desired mode + */ void setVideoMode(VideoMode mode); /** - * Set the video mode (fps and resolution) of the camera - * @param index the index of the desired mode - */ + * Set the video mode (fps and resolution) of the camera + * + * @param index the index of the desired mode + */ void setVideoMode(int index); /** - * Set the gain of the camera - * NOTE - Not all cameras support this. - * @param gain the new gain to set the camera to - */ + * Set the gain of the camera NOTE - Not all cameras support this. + * + * @param gain the new gain to set the camera to + */ void setGain(int gain); CameraCalibrationConfig getCurrentCalibrationData(); + List getAllCalibrationData(); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java index f4f4bb31d..d4f839d37 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CameraStreamer.java @@ -24,14 +24,17 @@ public class CameraStreamer { this.divisor = div; this.cameraCapture = cameraCapture; this.name = name; - this.cvSource = CameraServer.getInstance().putVideo(name, - cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, - cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value); + this.cvSource = + CameraServer.getInstance() + .putVideo( + name, + cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, + cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value); //noinspection IntegerDivisionInFloatingPointContext - this.size = new Size( - cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, - cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value - ); + this.size = + new Size( + cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, + cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value); setDivisor(divisor, false); } @@ -44,15 +47,16 @@ public class CameraStreamer { synchronized (streamBufferLock) { this.streamBuffer = new Mat(newWidth, newHeight, CvType.CV_8UC3); VideoMode oldVideoMode = cvSource.getVideoMode(); - cvSource.setVideoMode(new VideoMode(oldVideoMode.pixelFormat, - cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, - cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value, - oldVideoMode.fps)); + cvSource.setVideoMode( + new VideoMode( + oldVideoMode.pixelFormat, + cameraCapture.getProperties().getStaticProperties().imageWidth / divisor.value, + cameraCapture.getProperties().getStaticProperties().imageHeight / divisor.value, + oldVideoMode.fps)); } if (updateUI) { SocketHandler.sendFullSettings(); } - } public StreamDivisor getDivisor() { @@ -80,21 +84,24 @@ public class CameraStreamer { } if (divisor.value != 1) { -// var camVal = cameraProcess.getProperties().staticProperties; -// var newWidth = camVal.imageWidth / divisor.value; -// var newHeight = camVal.imageHeight / divisor.value; -// Size newSize = new Size(newWidth, newHeight); - Imgproc.resize(streamBuffer, streamBuffer, this.size); + // var camVal = cameraProcess.getProperties().staticProperties; + // var newWidth = camVal.imageWidth / divisor.value; + // var newHeight = camVal.imageHeight / divisor.value; + // Size newSize = new Size(newWidth, newHeight); + Imgproc.resize(streamBuffer, streamBuffer, this.size); } var sourceVideoMode = cvSource.getVideoMode(); var imageSize = streamBuffer.size(); - if(sourceVideoMode.width != (int) imageSize.width || sourceVideoMode.height != (int) imageSize.height) { + if (sourceVideoMode.width != (int) imageSize.width + || sourceVideoMode.height != (int) imageSize.height) { synchronized (streamBufferLock) { - cvSource.setVideoMode(new VideoMode(sourceVideoMode.pixelFormat, - (int)imageSize.width, - (int) imageSize.height, - sourceVideoMode.fps)); + cvSource.setVideoMode( + new VideoMode( + sourceVideoMode.pixelFormat, + (int) imageSize.width, + (int) imageSize.height, + sourceVideoMode.fps)); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java index 20149c207..81f854b9d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/CaptureStaticProperties.java @@ -31,9 +31,11 @@ public class CaptureStaticProperties { 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)); + 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)); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java index 937750219..844c542ef 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCameraCapture.java @@ -8,13 +8,12 @@ import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.VideoException; import edu.wpi.cscore.VideoMode; import edu.wpi.first.cameraserver.CameraServer; -import org.apache.commons.lang3.tuple.Pair; -import org.opencv.core.Mat; -import org.opencv.core.Size; - import java.util.ArrayList; import java.util.Arrays; import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Size; public class USBCameraCapture implements CameraCapture { private final UsbCamera baseCamera; @@ -25,22 +24,26 @@ public class USBCameraCapture implements CameraCapture { public USBCameraCapture(FullCameraConfiguration fullCameraConfiguration) { var config = fullCameraConfiguration.cameraConfig; - this.calibrationList = new ArrayList<>(); //fullCameraConfiguration.calibration; + this.calibrationList = new ArrayList<>(); // fullCameraConfiguration.calibration; calibrationList.addAll(fullCameraConfiguration.calibration); baseCamera = new UsbCamera(config.name, config.path); cvSink = CameraServer.getInstance().getVideo(baseCamera); try { properties = new USBCaptureProperties(baseCamera, config); - } catch(VideoException e) { - System.err.println("Camera cannot be found on the saved USB port!" + - " Ensure that the camera has not been plugged into a different USB port, and if so, correct it."); + } catch (VideoException e) { + System.err.println( + "Camera cannot be found on the saved USB port!" + + " Ensure that the camera has not been plugged into a different USB port, and if so, correct it."); e.printStackTrace(); } var videoModes = properties.getVideoModes(); - if(videoModes.size() < 1) { - throw new VideoException("0 video modes are valid! Full list provided by camera: \n\n" - + Arrays.stream(baseCamera.enumerateVideoModes()).map(Helpers::VideoModeToHashMap).toString() ); + if (videoModes.size() < 1) { + throw new VideoException( + "0 video modes are valid! Full list provided by camera: \n\n" + + Arrays.stream(baseCamera.enumerateVideoModes()) + .map(Helpers::VideoModeToHashMap) + .toString()); } int videoMode = properties.videoModes.size() - 1 <= config.videomode ? config.videomode : 0; @@ -48,8 +51,8 @@ public class USBCameraCapture implements CameraCapture { } public CameraCalibrationConfig getCalibration(Size size) { - for(var calibration: calibrationList) { - if(calibration.resolution.equals(size)) return calibration; + for (var calibration : calibrationList) { + if (calibration.resolution.equals(size)) return calibration; } return null; } @@ -59,7 +62,10 @@ public class USBCameraCapture implements CameraCapture { } public void addCalibrationData(CameraCalibrationConfig newConfig) { - calibrationList.removeIf(c -> newConfig.resolution.height == c.resolution.height && newConfig.resolution.width == c.resolution.width); + calibrationList.removeIf( + c -> + newConfig.resolution.height == c.resolution.height + && newConfig.resolution.width == c.resolution.width); calibrationList.add(newConfig); } @@ -79,7 +85,8 @@ public class USBCameraCapture implements CameraCapture { // TODO: Why multiply by 1000 here? Mat tempMat = new Mat(); deltaTime = cvSink.grabFrame(tempMat) * 1000L; -// tempMat = Imgcodecs.imread("C:\\Users\\imadu\\Documents\\GitHub\\chameleon-vision\\chameleon-server\\testimages\\2020\\image.png"); + // tempMat = + // Imgcodecs.imread("C:\\Users\\imadu\\Documents\\GitHub\\chameleon-vision\\chameleon-server\\testimages\\2020\\image.png"); tempMat.copyTo(imageBuffer); tempMat.release(); return Pair.of(imageBuffer, deltaTime); @@ -113,7 +120,7 @@ public class USBCameraCapture implements CameraCapture { } } - public void setVideoMode(int index){ + public void setVideoMode(int index) { VideoMode mode = properties.getVideoModes().get(index); setVideoMode(mode); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java index 365870cbc..a53975d22 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/camera/USBCaptureProperties.java @@ -6,7 +6,6 @@ import com.chameleonvision.common.util.Platform; import edu.wpi.cscore.UsbCamera; import edu.wpi.cscore.VideoMode; import edu.wpi.first.wpilibj.geometry.Rotation2d; - import java.util.Arrays; import java.util.List; import java.util.function.Predicate; @@ -25,11 +24,16 @@ public class USBCaptureProperties extends CaptureProperties { private static final int PS3EYE_VID = 0x1415; private static final int PS3EYE_PID = 0x2000; - private static final List ALLOWED_PIXEL_FORMATS = Arrays.asList(VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG, VideoMode.PixelFormat.kBGR); + private static final List ALLOWED_PIXEL_FORMATS = + Arrays.asList( + VideoMode.PixelFormat.kYUYV, VideoMode.PixelFormat.kMJPEG, VideoMode.PixelFormat.kBGR); - private static final Predicate kMinFPSPredicate = (videoMode -> videoMode.fps >= MINIMUM_FPS); - private static final Predicate kMinSizePredicate = (videoMode -> videoMode.width >= MINIMUM_WIDTH && videoMode.height >= MINIMUM_HEIGHT); - private static final Predicate kPixelFormatPredicate = (videoMode -> ALLOWED_PIXEL_FORMATS.contains(videoMode.pixelFormat)); + private static final Predicate kMinFPSPredicate = + (videoMode -> videoMode.fps >= MINIMUM_FPS); + private static final Predicate kMinSizePredicate = + (videoMode -> videoMode.width >= MINIMUM_WIDTH && videoMode.height >= MINIMUM_HEIGHT); + private static final Predicate kPixelFormatPredicate = + (videoMode -> ALLOWED_PIXEL_FORMATS.contains(videoMode.pixelFormat)); public final String name; public final String path; @@ -89,7 +93,8 @@ public class USBCaptureProperties extends CaptureProperties { } private List filterVideoModes(VideoMode[] videoModes) { - Predicate fullPredicate = kMinFPSPredicate.and(kMinSizePredicate).and(kPixelFormatPredicate); + Predicate fullPredicate = + kMinFPSPredicate.and(kMinSizePredicate).and(kPixelFormatPredicate); Stream validModes = Arrays.stream(videoModes).filter(fullPredicate); return validModes.collect(Collectors.toList()); } @@ -102,16 +107,15 @@ public class USBCaptureProperties extends CaptureProperties { return videoModes; } - public VideoMode getVideoMode(int index){ + public VideoMode getVideoMode(int index) { return videoModes.get(index); } - public VideoMode getCurrentVideoMode() { return staticProperties.mode; } - - public int getCurrentVideoModeIndex(){ - return getVideoModes().indexOf(getCurrentVideoMode()); + public VideoMode getCurrentVideoMode() { + return staticProperties.mode; } - - + public int getCurrentVideoModeIndex() { + return getVideoModes().indexOf(getCurrentVideoMode()); + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java index 71d62d17e..f3a9d221f 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/CalibrationMode.java @@ -1,5 +1,7 @@ package com.chameleonvision._2.vision.enums; public enum CalibrationMode { - None,Single,Dual + None, + Single, + Dual } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java index 7328dc02d..ab95ef7e3 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/ImageRotationMode.java @@ -12,5 +12,7 @@ public enum ImageRotationMode { this.value = value; } - public boolean isRotated(){return this.value==DEG_90.value||this.value==DEG_270.value;} + public boolean isRotated() { + return this.value == DEG_90.value || this.value == DEG_270.value; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java index db7acefbe..f3afb4ed9 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/SortMode.java @@ -1,5 +1,11 @@ package com.chameleonvision._2.vision.enums; public enum SortMode { - Largest,Smallest,Highest,Lowest,Rightmost,Leftmost,Centermost + Largest, + Smallest, + Highest, + Lowest, + Rightmost, + Leftmost, + Centermost } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java index 8e754faf9..52317e181 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetIntersection.java @@ -1,5 +1,9 @@ package com.chameleonvision._2.vision.enums; public enum TargetIntersection { - None,Up,Down,Left,Right + None, + Up, + Down, + Left, + Right } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java index 8276ecb1d..f37d67d08 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetOrientation.java @@ -1,5 +1,6 @@ package com.chameleonvision._2.vision.enums; public enum TargetOrientation { - Portrait, Landscape + Portrait, + Landscape } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java index 743046b4b..b15d176d3 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/enums/TargetRegion.java @@ -1,5 +1,9 @@ package com.chameleonvision._2.vision.enums; public enum TargetRegion { - Center, Top, Bottom, Left, Right + Center, + Top, + Bottom, + Left, + Right } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java index 816f927da..457de409b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/CaptureProperties.java @@ -9,13 +9,16 @@ public class CaptureProperties { protected CaptureStaticProperties staticProperties; private Rotation2d tilt = new Rotation2d(); - protected CaptureProperties() { - } + protected CaptureProperties() {} public CaptureProperties(VideoMode videoMode, double fov) { staticProperties = new CaptureStaticProperties(videoMode, fov); } - public void setStaticProperties(CaptureStaticProperties staticProperties) {this.staticProperties = staticProperties;} + + public void setStaticProperties(CaptureStaticProperties staticProperties) { + this.staticProperties = staticProperties; + } + public CaptureStaticProperties getStaticProperties() { return staticProperties; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java index cd8e3a4d2..1b5eafe0d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/ImageCapture.java @@ -5,8 +5,9 @@ import org.opencv.core.Mat; public interface ImageCapture { /** - * Get the next camera frame - * @return a Pair of the captured image and the Linux epoch of when the frame was grabbed (in uS) - */ + * Get the next camera frame + * + * @return a Pair of the captured image and the Linux epoch of when the frame was grabbed (in uS) + */ Pair getFrame(); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java index e10d51e63..6fcd581f2 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/image/StaticImageCapture.java @@ -3,13 +3,12 @@ package com.chameleonvision._2.vision.image; import com.chameleonvision._2.config.CameraCalibrationConfig; import com.chameleonvision._2.vision.camera.CameraCapture; import edu.wpi.cscore.VideoMode; -import org.apache.commons.lang3.tuple.Pair; -import org.opencv.core.Mat; -import org.opencv.imgcodecs.Imgcodecs; - import java.nio.file.Files; import java.nio.file.Path; import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.imgcodecs.Imgcodecs; public class StaticImageCapture implements CameraCapture { diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java index ccac6cbfc..ada78af57 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/CVPipeline.java @@ -3,10 +3,7 @@ package com.chameleonvision._2.vision.pipeline; import com.chameleonvision._2.vision.camera.CameraCapture; import org.opencv.core.Mat; -/** - * - * @param Pipeline result type - */ +/** @param Pipeline result type */ public abstract class CVPipeline { protected Mat outputMat = new Mat(); protected CameraCapture cameraCapture; @@ -28,5 +25,6 @@ public abstract class CVPipeline { public final List targets; @@ -14,7 +13,7 @@ public abstract class CVPipelineResult { public CVPipelineResult(List targets, Mat outputMat, long processTime) { this.targets = targets; hasTarget = targets != null && !targets.isEmpty(); -// this.outputMat = outputMat; + // this.outputMat = outputMat; outputMat.copyTo(this.outputMat); outputMat.release(); this.processTime = processTime; diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java index 0cb962d0b..2ca5eec9e 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/Pipe.java @@ -4,10 +4,8 @@ import org.apache.commons.lang3.tuple.Pair; public interface Pipe { /** - * - * @param input Input object for pipe - * @return Returns a Pair containing the process time in Nanoseconds, - * and the output object - */ + * @param input Input object for pipe + * @return Returns a Pair containing the process time in Nanoseconds, and the output object + */ Pair run(I input); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java index f75b89357..d88c6b7c3 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/PipelineManager.java @@ -13,7 +13,6 @@ import com.chameleonvision.common.scripting.ScriptEventType; import com.chameleonvision.common.scripting.ScriptManager; import edu.wpi.cscore.VideoMode; import edu.wpi.first.networktables.NetworkTableEntry; - import java.util.Comparator; import java.util.HashMap; import java.util.LinkedList; @@ -28,14 +27,16 @@ public class PipelineManager { public final LinkedList pipelines = new LinkedList<>(); public final CVPipeline driverModePipeline = new DriverVisionPipeline(new CVPipelineSettings()); - public final Calibrate3dPipeline calib3dPipe = new Calibrate3dPipeline(new StandardCVPipelineSettings()); + public final Calibrate3dPipeline calib3dPipe = + new Calibrate3dPipeline(new StandardCVPipelineSettings()); private final VisionProcess parentProcess; private int lastPipelineIndex; private int currentPipelineIndex; public NetworkTableEntry ntIndexEntry; - public PipelineManager(VisionProcess visionProcess, List loadedPipelineSettings) { + public PipelineManager( + VisionProcess visionProcess, List loadedPipelineSettings) { parentProcess = visionProcess; if (loadedPipelineSettings == null || loadedPipelineSettings.size() == 0) { pipelines.add(new StandardCVPipeline("New Pipeline")); @@ -115,7 +116,7 @@ public class PipelineManager { if (currentPipelineIndex == DRIVERMODE_INDEX) { return driverModePipeline; } else if (currentPipelineIndex <= CAL_3D_INDEX) { - return calib3dPipe; + return calib3dPipe; } else { return pipelines.get(currentPipelineIndex); } @@ -135,25 +136,24 @@ public class PipelineManager { newPipeline = calib3dPipe; } else { - if (index < pipelines.size()&&index>=0) { + if (index < pipelines.size() && index >= 0) { newPipeline = pipelines.get(index); // if we're switching out of driver mode, try to set the nt entry to false parentProcess.setDriverModeEntry(false); ScriptManager.queueEvent(ScriptEventType.kLEDOn); + } else { + // TODO alert/warn user that pipeline doesnt exsits + System.err.println("Index is out of bounds"); } - else - { - //TODO alert/warn user that pipeline doesnt exsits - System.err.println("Index is out of bounds"); - } } if (newPipeline != null) { lastPipelineIndex = currentPipelineIndex; currentPipelineIndex = index; getCurrentPipeline().initPipeline(parentProcess.getCamera()); - if (ConfigManager.settings.currentCamera.equals(parentProcess.getCamera().getProperties().name)) { + if (ConfigManager.settings.currentCamera.equals( + parentProcess.getCamera().getProperties().name)) { ConfigManager.settings.currentPipeline = currentPipelineIndex; HashMap pipeChange = new HashMap<>(); @@ -208,9 +208,10 @@ public class PipelineManager { public void duplicatePipeline(CVPipelineSettings pipeline, VisionProcess destinationProcess) { pipeline.index = destinationProcess.pipelineManager.pipelines.size(); pipeline.nickname += "(Copy)"; - if (destinationProcess.pipelineManager.pipelines.stream().anyMatch(c -> c.settings.nickname.equals(pipeline.nickname))){ -// throw new DuplicatedKeyException("key Already exists"); - } else{ + if (destinationProcess.pipelineManager.pipelines.stream() + .anyMatch(c -> c.settings.nickname.equals(pipeline.nickname))) { + // throw new DuplicatedKeyException("key Already exists"); + } else { destinationProcess.pipelineManager.addPipeline(pipeline); } } @@ -237,15 +238,16 @@ public class PipelineManager { getConfig().saveDriverMode(driverModePipeline.settings); } - private static final Comparator IndexComparator = (o1, o2) -> { - int o1Index = o1.settings.index; - int o2Index = o2.settings.index; + private static final Comparator IndexComparator = + (o1, o2) -> { + int o1Index = o1.settings.index; + int o2Index = o2.settings.index; - if (o1Index == o2Index) { - return 0; - } else if (o1Index < o2Index) { - return -1; - } - return 1; - }; + if (o1Index == o2Index) { + return 0; + } else if (o1Index < o2Index) { + return -1; + } + return 1; + }; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java index 6c474d595..7069fef2d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/Calibrate3dPipeline.java @@ -2,26 +2,27 @@ package com.chameleonvision._2.vision.pipeline.impl; import com.chameleonvision._2.config.CameraCalibrationConfig; import com.chameleonvision._2.config.ConfigManager; -import com.chameleonvision._2.vision.pipeline.CVPipeline; import com.chameleonvision._2.vision.VisionManager; import com.chameleonvision._2.vision.camera.CameraCapture; +import com.chameleonvision._2.vision.pipeline.CVPipeline; import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.cscore.VideoMode; import edu.wpi.first.wpilibj.util.Units; +import java.util.ArrayList; +import java.util.List; import org.opencv.calib3d.Calib3d; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; -import java.util.ArrayList; -import java.util.List; - -public class Calibrate3dPipeline extends CVPipeline { +public class Calibrate3dPipeline + extends CVPipeline { private int checkerboardSquaresHigh = 7; private int checkerboardSquaresWide = 7; - private MatOfPoint3f objP;// new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide, 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3); + private MatOfPoint3f objP; // new MatOfPoint3f(checkerboardSquaresHigh + checkerboardSquaresWide, + // 3);//(checkerboardSquaresWide * checkerboardSquaresHigh, 3); private Size patternSize = new Size(checkerboardSquaresHigh, checkerboardSquaresWide); private Size imageSize; double checkerboardSquareSize = 1; // inches! @@ -35,7 +36,9 @@ public class Calibrate3dPipeline extends CVPipeline tvecs = new ArrayList<>(); try { - calibrationAccuracy = Calib3d.calibrateCamera(objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs); - } catch(Exception e) { + calibrationAccuracy = + Calib3d.calibrateCamera( + objpoints, imgpoints, imageSize, cameraMatrix, distortionCoeffs, rvecs, tvecs); + } catch (Exception e) { System.err.println("Camera calibration failed!"); initPipeline(cameraCapture); return false; @@ -139,32 +144,35 @@ public class Calibrate3dPipeline extends CVPipeline { +public class DriverVisionPipeline + extends CVPipeline { private RotateFlipPipe rotateFlipPipe; private Draw2dCrosshairPipe drawCrosshairPipe; - private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings crosshairPipeSettings = new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings(); + private Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings crosshairPipeSettings = + new Draw2dCrosshairPipe.Draw2dCrosshairPipeSettings(); private final MemoryManager memoryManager = new MemoryManager(200, 20000); @@ -31,8 +31,9 @@ public class DriverVisionPipeline extends CVPipeline rotateFlipResult = rotateFlipPipe.run(inputMat); - Pair draw2dCrosshairResult = drawCrosshairPipe.run(Pair.of(rotateFlipResult.getLeft(),null)); + Pair draw2dCrosshairResult = + drawCrosshairPipe.run(Pair.of(rotateFlipResult.getLeft(), null)); memoryManager.run(); return new DriverPipelineResult(null, draw2dCrosshairResult.getLeft(), 0); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java index 6a827983d..9def4d7ba 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/impl/StandardCVPipelineSettings.java @@ -32,18 +32,17 @@ public class StandardCVPipelineSettings extends CVPipelineSettings { // 3d stuff public MatOfPoint3f targetCornerMat = new MatOfPoint3f(); public Number accuracy = 5; - private static MatOfPoint3f hexTargetMat = new MatOfPoint3f( - new Point3(-19.625, 0, 0), - new Point3(-9.819867, -17, 0), - new Point3(9.819867, -17, 0), - new Point3(19.625, 0, 0) - ); + private static MatOfPoint3f hexTargetMat = + new MatOfPoint3f( + new Point3(-19.625, 0, 0), + new Point3(-9.819867, -17, 0), + new Point3(9.819867, -17, 0), + new Point3(19.625, 0, 0)); public StandardCVPipelineSettings() { super(); hexTargetMat.copyTo(targetCornerMat); } - public boolean is3D = false; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java index e99bcf434..6a480b200 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/BlurPipe.java @@ -23,18 +23,19 @@ public class BlurPipe implements Pipe { public Pair run(Mat input) { long processStartNanos = System.nanoTime(); -// if (blurSize > 0) { -// input.copyTo(processBuffer); -// try { -// Imgproc.blur(processBuffer, processBuffer, new Size(blurSize, blurSize)); -// processBuffer.copyTo(outputMat); -// processBuffer.release(); -// } catch (CvException e) { -// System.err.println("(BlurPipe) Exception thrown by OpenCV: \n" + e.getMessage()); -// } -// } else { -// input.copyTo(outputMat); -// } + // if (blurSize > 0) { + // input.copyTo(processBuffer); + // try { + // Imgproc.blur(processBuffer, processBuffer, new Size(blurSize, blurSize)); + // processBuffer.copyTo(outputMat); + // processBuffer.release(); + // } catch (CvException e) { + // System.err.println("(BlurPipe) Exception thrown by OpenCV: \n" + + // e.getMessage()); + // } + // } else { + // input.copyTo(outputMat); + // } long processTime = System.nanoTime() - processStartNanos; return Pair.of(input, processTime); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java index 74a8c6ab3..8d4be1201 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/Collect2dTargetsPipe.java @@ -7,15 +7,16 @@ import com.chameleonvision._2.vision.enums.TargetRegion; import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; import com.chameleonvision.common.util.numbers.DoubleCouple; +import java.util.ArrayList; +import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.math3.util.FastMath; import org.opencv.core.Point; -import java.util.ArrayList; -import java.util.List; - -public class Collect2dTargetsPipe implements Pipe, CaptureStaticProperties>, List> { - +public class Collect2dTargetsPipe + implements Pipe< + Pair, CaptureStaticProperties>, + List> { private CaptureStaticProperties camProps; private CalibrationMode calibrationMode; @@ -26,11 +27,32 @@ public class Collect2dTargetsPipe implements Pipe targets = new ArrayList<>(); private Point[] vertices = new Point[4]; - public Collect2dTargetsPipe(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, DoubleCouple calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) { - setConfig(calibrationMode, targetRegion, targetOrientation, calibrationPoint, calibrationM, calibrationB, camProps); + public Collect2dTargetsPipe( + CalibrationMode calibrationMode, + TargetRegion targetRegion, + TargetOrientation targetOrientation, + DoubleCouple calibrationPoint, + double calibrationM, + double calibrationB, + CaptureStaticProperties camProps) { + setConfig( + calibrationMode, + targetRegion, + targetOrientation, + calibrationPoint, + calibrationM, + calibrationB, + camProps); } - public void setConfig(CalibrationMode calibrationMode, TargetRegion targetRegion, TargetOrientation targetOrientation, DoubleCouple calibrationPoint, double calibrationM, double calibrationB, CaptureStaticProperties camProps) { + public void setConfig( + CalibrationMode calibrationMode, + TargetRegion targetRegion, + TargetOrientation targetOrientation, + DoubleCouple calibrationPoint, + double calibrationM, + double calibrationB, + CaptureStaticProperties camProps) { this.calibrationMode = calibrationMode; this.calibrationPoint = calibrationPoint; this.calibrationM = calibrationM; @@ -41,7 +63,8 @@ public class Collect2dTargetsPipe implements Pipe, Long> run(Pair, CaptureStaticProperties> inputPair) { + public Pair, Long> run( + Pair, CaptureStaticProperties> inputPair) { long processStartNanos = System.nanoTime(); targets.clear(); @@ -64,22 +87,26 @@ public class Collect2dTargetsPipe implements Pipe>, Mat> { +public class Draw2dContoursPipe + implements Pipe>, Mat> { private final Draw2dContoursSettings settings; private CaptureStaticProperties camProps; @@ -27,13 +27,12 @@ public class Draw2dContoursPipe implements Pipe 0) { for (int i = 0; i < input.getRight().size(); i++) { - if (i != 0 && !settings.showMultiple){ + if (i != 0 && !settings.showMultiple) { break; } StandardCVPipeline.TrackedTarget target = input.getRight().get(i); @@ -61,31 +60,44 @@ public class Draw2dContoursPipe implements Pipe>, Mat> { -public class Draw2dCrosshairPipe implements Pipe>, Mat> { - - //Settings + // Settings private Draw2dCrosshairPipeSettings crosshairSettings; private CalibrationMode calibrationMode; private DoubleCouple calibrationPoint; private double calibrationM, calibrationB; - - private Point xMax = new Point(), xMin = new Point(), yMax = new Point(), yMin = new Point(); - public Draw2dCrosshairPipe(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, DoubleCouple calibrationPoint, double calibrationM, double calibrationB) { + public Draw2dCrosshairPipe( + Draw2dCrosshairPipeSettings crosshairSettings, + CalibrationMode calibrationMode, + DoubleCouple calibrationPoint, + double calibrationM, + double calibrationB) { setConfig(crosshairSettings, calibrationMode, calibrationPoint, calibrationM, calibrationB); } - public void setConfig(Draw2dCrosshairPipeSettings crosshairSettings, CalibrationMode calibrationMode, DoubleCouple calibrationPoint, double calibrationM, double calibrationB) { + public void setConfig( + Draw2dCrosshairPipeSettings crosshairSettings, + CalibrationMode calibrationMode, + DoubleCouple calibrationPoint, + double calibrationM, + double calibrationB) { this.crosshairSettings = crosshairSettings; this.calibrationMode = calibrationMode; this.calibrationPoint = calibrationPoint; @@ -40,7 +48,7 @@ public class Draw2dCrosshairPipe implements Pipe run(Pair> inputPair) { long processStartNanos = System.nanoTime(); Mat image = inputPair.getLeft(); -// List targets = inputPair.getRight(); + // List targets = inputPair.getRight(); double x, y; double scale = image.cols() / 32.0; @@ -50,28 +58,37 @@ public class Draw2dCrosshairPipe implements Pipe>, Mat> { +public class DrawSolvePNPPipe + implements Pipe>, Mat> { private MatOfPoint3f boxCornerMat = new MatOfPoint3f(); @@ -25,22 +25,22 @@ public class DrawSolvePNPPipe implements Pipe new Point3(it.x, it.y, it.z + 6)).collect(Collectors.toList()); + var auxList = + list.stream().map(it -> new Point3(it.x, it.y, it.z + 6)).collect(Collectors.toList()); var finalList = new ArrayList<>(list); finalList.addAll(auxList); boxCornerMat.fromList(finalList); } - public void setConfig(StandardCVPipelineSettings settings) { setBox(settings.targetCornerMat); } @@ -71,7 +71,15 @@ public class DrawSolvePNPPipe implements Pipe { long processStartNanos = System.nanoTime(); if (erode || dilate) { -// input.copyTo(processBuffer); + // input.copyTo(processBuffer); if (erode) { Imgproc.erode(input, input, kernel); @@ -39,10 +39,10 @@ public class ErodeDilatePipe implements Pipe { Imgproc.dilate(input, input, kernel); } -// processBuffer.copyTo(outputMat); -// processBuffer.release(); + // processBuffer.copyTo(outputMat); + // processBuffer.release(); } else { -// input.copyTo(outputMat); + // input.copyTo(outputMat); } long processTime = System.nanoTime() - processStartNanos; diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java index 85372b8a5..10d8c44a8 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FilterContoursPipe.java @@ -5,15 +5,12 @@ import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision.common.util.math.MathUtils; import com.chameleonvision.common.util.numbers.DoubleCouple; import com.chameleonvision.common.vision.opencv.Contour; -import org.apache.commons.lang3.tuple.Pair; -import org.opencv.core.MatOfPoint; -import org.opencv.core.MatOfPoint2f; -import org.opencv.core.Rect; -import org.opencv.core.RotatedRect; -import org.opencv.imgproc.Imgproc; - import java.util.ArrayList; import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.MatOfPoint; +import org.opencv.core.Rect; +import org.opencv.core.RotatedRect; public class FilterContoursPipe implements Pipe, List> { @@ -24,14 +21,22 @@ public class FilterContoursPipe implements Pipe, List> private List filteredContours = new ArrayList<>(); - public FilterContoursPipe(DoubleCouple area, DoubleCouple ratio, DoubleCouple extent, CaptureStaticProperties camProps) { + public FilterContoursPipe( + DoubleCouple area, + DoubleCouple ratio, + DoubleCouple extent, + CaptureStaticProperties camProps) { this.area = area; this.ratio = ratio; this.extent = extent; this.camProps = camProps; } - public void setConfig(DoubleCouple area, DoubleCouple ratio, DoubleCouple extent, CaptureStaticProperties camProps) { + public void setConfig( + DoubleCouple area, + DoubleCouple ratio, + DoubleCouple extent, + CaptureStaticProperties camProps) { this.area = area; this.ratio = ratio; this.extent = extent; @@ -61,7 +66,7 @@ public class FilterContoursPipe implements Pipe, List> // AspectRatio filtering Rect boundingRect = realContour.getBoundingRect(); - double aspectRatio = ((double)boundingRect.width / boundingRect.height); + double aspectRatio = ((double) boundingRect.width / boundingRect.height); if (aspectRatio < ratio.getFirst() || aspectRatio > ratio.getSecond()) { return; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java index 374a82976..1966855f9 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/FindContoursPipe.java @@ -2,15 +2,14 @@ package com.chameleonvision._2.vision.pipeline.pipes; import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision.common.vision.opencv.Contour; +import java.util.ArrayList; +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.MatOfPoint; import org.opencv.imgproc.Imgproc; -import java.util.ArrayList; -import java.util.List; -import java.util.stream.Collectors; - public class FindContoursPipe implements Pipe> { private List foundContours = new ArrayList<>(); @@ -23,9 +22,11 @@ public class FindContoursPipe implements Pipe> { foundContours.clear(); - Imgproc.findContours(input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); + Imgproc.findContours( + input, foundContours, new Mat(), Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_TC89_L1); long processTime = System.nanoTime() - processStartNanos; - return Pair.of(foundContours.stream().map(Contour::new).collect(Collectors.toList()), processTime); + return Pair.of( + foundContours.stream().map(Contour::new).collect(Collectors.toList()), processTime); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java index 981186758..243e2db10 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/GroupContoursPipe.java @@ -5,17 +5,17 @@ import com.chameleonvision._2.vision.enums.TargetIntersection; import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; import com.chameleonvision.common.util.math.MathUtils; +import java.util.ArrayList; +import java.util.Collections; +import java.util.Comparator; +import java.util.List; import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.*; import org.opencv.imgproc.Imgproc; import org.opencv.imgproc.Moments; -import java.util.ArrayList; -import java.util.Collections; -import java.util.Comparator; -import java.util.List; - -public class GroupContoursPipe implements Pipe, List> { +public class GroupContoursPipe + implements Pipe, List> { private static final Comparator sortByMomentsX = Comparator.comparingDouble(GroupContoursPipe::calcMomentsX); @@ -55,76 +55,78 @@ public class GroupContoursPipe implements Pipe, List { - contourBuffer.fromArray(c.toArray()); - if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) { - RotatedRect rect = Imgproc.minAreaRect(contourBuffer); - Rect boundingRect = Imgproc.boundingRect(contourBuffer); - var target = new StandardCVPipeline.TrackedTarget(); - target.minAreaRect = rect; - target.rawContour = contourBuffer; - target.boundingRect = boundingRect; - groupedContours.add(target); - } - }); - break; - } - case Dual: { - for (var i = 0; i < input.size(); i++) { - List finalContourList = new ArrayList<>(input.get(i).toList()); - - try { - MatOfPoint firstContour = input.get(i); - MatOfPoint secondContour = input.get(i + 1); - - if (isIntersecting(firstContour, secondContour)) { - finalContourList.addAll(secondContour.toList()); - } else { - finalContourList.clear(); - continue; - } - - intersectMatA.release(); - intersectMatB.release(); - - contourBuffer.fromList(finalContourList); - - if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) { - RotatedRect rect = Imgproc.minAreaRect(contourBuffer); - Rect boundingRect = Imgproc.boundingRect(contourBuffer); - var target = new StandardCVPipeline.TrackedTarget(); - target.minAreaRect = rect; - target.boundingRect = boundingRect; - // find left and right bouding rectangles - target.leftRightDualTargetPair = - Pair.of(Imgproc.boundingRect(firstContour), - Imgproc.boundingRect(secondContour)); - - // find left and right min area rectangles - tempRectMat.fromArray(firstContour.toArray()); - var minAreaRect1 = Imgproc.minAreaRect(tempRectMat); - tempRectMat.fromArray(secondContour.toArray()); - var minAreaRect2 = Imgproc.minAreaRect(tempRectMat); - target.leftRightRotatedRect = - Pair.of(minAreaRect1, minAreaRect2); - - target.rawContour = contourBuffer; - - groupedContours.add(target); - - firstContour.release(); - secondContour.release(); - - // skip the next contour because it's been grouped already - i += 1; - } - } catch (IndexOutOfBoundsException e) { - finalContourList.clear(); - } + case Single: + { + input.forEach( + c -> { + contourBuffer.fromArray(c.toArray()); + if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) { + RotatedRect rect = Imgproc.minAreaRect(contourBuffer); + Rect boundingRect = Imgproc.boundingRect(contourBuffer); + var target = new StandardCVPipeline.TrackedTarget(); + target.minAreaRect = rect; + target.rawContour = contourBuffer; + target.boundingRect = boundingRect; + groupedContours.add(target); + } + }); + break; + } + case Dual: + { + for (var i = 0; i < input.size(); i++) { + List finalContourList = new ArrayList<>(input.get(i).toList()); + + try { + MatOfPoint firstContour = input.get(i); + MatOfPoint secondContour = input.get(i + 1); + + if (isIntersecting(firstContour, secondContour)) { + finalContourList.addAll(secondContour.toList()); + } else { + finalContourList.clear(); + continue; + } + + intersectMatA.release(); + intersectMatB.release(); + + contourBuffer.fromList(finalContourList); + + if (contourBuffer.cols() != 0 && contourBuffer.rows() != 0) { + RotatedRect rect = Imgproc.minAreaRect(contourBuffer); + Rect boundingRect = Imgproc.boundingRect(contourBuffer); + var target = new StandardCVPipeline.TrackedTarget(); + target.minAreaRect = rect; + target.boundingRect = boundingRect; + // find left and right bouding rectangles + target.leftRightDualTargetPair = + Pair.of( + Imgproc.boundingRect(firstContour), Imgproc.boundingRect(secondContour)); + + // find left and right min area rectangles + tempRectMat.fromArray(firstContour.toArray()); + var minAreaRect1 = Imgproc.minAreaRect(tempRectMat); + tempRectMat.fromArray(secondContour.toArray()); + var minAreaRect2 = Imgproc.minAreaRect(tempRectMat); + target.leftRightRotatedRect = Pair.of(minAreaRect1, minAreaRect2); + + target.rawContour = contourBuffer; + + groupedContours.add(target); + + firstContour.release(); + secondContour.release(); + + // skip the next contour because it's been grouped already + i += 1; + } + } catch (IndexOutOfBoundsException e) { + finalContourList.clear(); + } + } + break; } - break; - } } } @@ -160,36 +162,40 @@ public class GroupContoursPipe implements Pipe, List massY) { + case Down: + { + if (intersectionY > massY) { return true; - } + } - break; - } - case Left: { - if (intersectionX < massX) { + break; + } + case Left: + { + if (intersectionX < massX) { - return true; + return true; + } + break; } - break; - } - case Right: { - if (intersectionX > massX) { - return true; + case Right: + { + if (intersectionX > massX) { + return true; + } + break; } - break; - } } return false; } catch (Exception e) { return false; } } -} \ No newline at end of file +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java index be879d236..a42b4a8be 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/HsvPipe.java @@ -43,4 +43,3 @@ public class HsvPipe implements Pipe { return Pair.of(outputMat, processTime); } } - diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java index 63113e7f2..f4de7b263 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/OutputMatPipe.java @@ -22,11 +22,10 @@ public class OutputMatPipe implements Pipe, Mat> { } /** - * - * @param input Input object for pipe - * Left is raw camera mat (8UC3), Right is HSV threshold mat (8UC1) - * @return Returns desired output Mat, and processing time in nanoseconds - */ + * @param input Input object for pipe Left is raw camera mat (8UC3), Right is HSV threshold mat + * (8UC1) + * @return Returns desired output Mat, and processing time in nanoseconds + */ @Override public Pair run(Pair input) { long processStartNanos = System.nanoTime(); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java index cc7c27585..dbb2c2e22 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/RotateFlipPipe.java @@ -1,8 +1,8 @@ package com.chameleonvision._2.vision.pipeline.pipes; -import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision._2.vision.enums.ImageFlipMode; import com.chameleonvision._2.vision.enums.ImageRotationMode; +import com.chameleonvision._2.vision.pipeline.Pipe; import org.apache.commons.lang3.tuple.Pair; import org.opencv.core.Core; import org.opencv.core.Mat; @@ -33,7 +33,7 @@ public class RotateFlipPipe implements Pipe { boolean shouldRotate = !rotation.equals(ImageRotationMode.DEG_0); if (shouldFlip || shouldRotate) { -// input.copyTo(processBuffer); + // input.copyTo(processBuffer); if (shouldFlip) { Core.flip(input, input, flip.value); @@ -43,10 +43,10 @@ public class RotateFlipPipe implements Pipe { Core.rotate(input, input, rotation.value); } -// processBuffer.copyTo(outputMat); -// processBuffer.release(); + // processBuffer.copyTo(outputMat); + // processBuffer.release(); } else { -// input.copyTo(outputMat); + // input.copyTo(outputMat); } long processTime = System.nanoTime() - processStartNanos; diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java index 61a20da56..bd2d4f6ef 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SolvePNPPipe.java @@ -1,12 +1,5 @@ package com.chameleonvision._2.vision.pipeline.pipes; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.Comparator; -import java.util.List; -import java.util.NoSuchElementException; -import java.util.stream.Collectors; - import com.chameleonvision._2.config.CameraCalibrationConfig; import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; @@ -14,6 +7,12 @@ import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipelineSettings; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; +import java.util.NoSuchElementException; +import java.util.stream.Collectors; import org.apache.commons.lang3.tuple.Pair; import org.apache.commons.math3.util.FastMath; import org.opencv.calib3d.Calib3d; @@ -30,477 +29,506 @@ import org.opencv.core.Size; import org.opencv.core.TermCriteria; import org.opencv.imgproc.Imgproc; -/** - * Handles detecting target corners and calculating robot-relative pose. - */ -public class SolvePNPPipe implements Pipe, Mat>, - List> { +/** Handles detecting target corners and calculating robot-relative pose. */ +public class SolvePNPPipe + implements Pipe< + Pair, Mat>, List> { - private Double tilt_angle; - private MatOfPoint3f objPointsMat = new MatOfPoint3f(); - private Mat rVec = new Mat(); - private Mat tVec = new Mat(); - private Mat rodriguez = new Mat(); - private Mat pzero_world = new Mat(); - private Mat cameraMatrix = new Mat(); - Mat rot_inv = new Mat(); - Mat kMat = new Mat(); - private MatOfDouble distortionCoefficients = new MatOfDouble(); - private List targetList = new ArrayList<>(); - Comparator leftRightComparator = Comparator.comparingDouble(point -> point.x); - Comparator verticalComparator = Comparator.comparingDouble(point -> point.y); - private double distanceDivisor = 1.0; - Mat scaledTvec = new Mat(); - MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f(); - MatOfPoint2f polyOutput = new MatOfPoint2f(); - private Mat greyImg = new Mat(); - private double accuracyPercentage = 0.2; + private Double tilt_angle; + private MatOfPoint3f objPointsMat = new MatOfPoint3f(); + private Mat rVec = new Mat(); + private Mat tVec = new Mat(); + private Mat rodriguez = new Mat(); + private Mat pzero_world = new Mat(); + private Mat cameraMatrix = new Mat(); + Mat rot_inv = new Mat(); + Mat kMat = new Mat(); + private MatOfDouble distortionCoefficients = new MatOfDouble(); + private List targetList = new ArrayList<>(); + Comparator leftRightComparator = Comparator.comparingDouble(point -> point.x); + Comparator verticalComparator = Comparator.comparingDouble(point -> point.y); + private double distanceDivisor = 1.0; + Mat scaledTvec = new Mat(); + MatOfPoint2f boundingBoxResultMat = new MatOfPoint2f(); + MatOfPoint2f polyOutput = new MatOfPoint2f(); + private Mat greyImg = new Mat(); + private double accuracyPercentage = 0.2; - /** - * @param settings unused :bolb: - * @param calibration the camera intrinsics and extrinsics - * @param tilt The pitch of the camera relative to horzontal. used to account for - * distances in calculate pose - */ - public SolvePNPPipe(StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, - Rotation2d tilt) { - super(); - setCameraCoeffs(calibration); -// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); - // TODO add proper year differentiation - set2020Target(true); + /** + * @param settings unused :bolb: + * @param calibration the camera intrinsics and extrinsics + * @param tilt The pitch of the camera relative to horzontal. used to account for distances in + * calculate pose + */ + public SolvePNPPipe( + StandardCVPipelineSettings settings, CameraCalibrationConfig calibration, Rotation2d tilt) { + super(); + setCameraCoeffs(calibration); + // setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); + // TODO add proper year differentiation + set2020Target(true); - this.tilt_angle = tilt.getRadians(); - } - - public void set2020Target(boolean isHighGoal) { - if (isHighGoal) { - // tl, bl, br, tr is the order - List corners = List.of( - - new Point3(-19.625, 0, 0), - new Point3(-9.819867, -17, 0), - new Point3(9.819867, -17, 0), - new Point3(19.625, 0, 0)); - setObjectCorners(corners); - } else { - setBoundingBoxTarget(7, 11); - } - } - - public void setBoundingBoxTarget(double targetWidth, double targetHeight) { - // order is left top, left bottom, right bottom, right top - - List corners = List.of( - new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0), - new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0), - new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0), - new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0) - ); - setObjectCorners(corners); - } - - public void setObjectCorners(List objectCorners) { - objPointsMat.release(); - objPointsMat = new MatOfPoint3f(); - objPointsMat.fromList(objectCorners); - } - - public void setConfig(StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, - Rotation2d tilt) { - setCameraCoeffs(camConfig); -// setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); - // TODO add proper year differentiation - tilt_angle = tilt.getRadians(); - this.objPointsMat = settings.targetCornerMat; - this.accuracyPercentage = settings.accuracy.doubleValue(); - } - - private void setCameraCoeffs(CameraCalibrationConfig settings) { - if (settings == null) { - System.err.println("SolvePNP can only run on a calibrated resolution, and this one is not!" + - " Please calibrate to use solvePNP."); - return; - } - if (cameraMatrix != settings.getCameraMatrixAsMat()) { - cameraMatrix.release(); - settings.getCameraMatrixAsMat().copyTo(cameraMatrix); - } - if (distortionCoefficients != settings.getDistortionCoeffsAsMat()) { - distortionCoefficients.release(); - settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients); - } - this.distanceDivisor = settings.squareSize; - } - - @Override - public Pair, Long> run(Pair, Mat> imageTargetPair) { - long processStartNanos = System.nanoTime(); - var targets = imageTargetPair.getLeft(); - var image = imageTargetPair.getRight(); - Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY); - targetList.clear(); - for (var target : targets) { - MatOfPoint2f corners; - // if it's a dual target use 2019, but default to 2020 - if (target.leftRightRotatedRect == null) { - corners = find2020VisionTarget(target, accuracyPercentage);//, imageTargetPair.getRight - // ()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? - // findCorner2019(target) : findBoundingBoxCorners(target); - } else { - corners = findCorner2019(target); - } -// var corners = findCorner2019(target); - if (corners == null) continue; - - // convert the corners into a Pose2d - var pose = calculatePose(corners, target); - targetList.add(pose); // TODO null check null poses. DO NOT ADD A NULL CHECK HERE, otherwise - // the order will be wrong. - } - long processTime = System.nanoTime() - processStartNanos; - return Pair.of(targetList, processTime); - } - - /** - * basically we split the target's two tapes, find the min area rectangle for each, and take - * the outermost 4 corners out of the 2 rectangles - * - * @param target the target to use - * @return the 4 outermost corners. - */ - private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) { - if (target.leftRightDualTargetPair == null) return null; - - var left = target.leftRightDualTargetPair.getLeft(); - var right = target.leftRightDualTargetPair.getRight(); - - // flip if the "left" target is to the right - if (left.x > right.x) { - var temp = left; - left = right; - right = temp; + this.tilt_angle = tilt.getRadians(); } - var points = new MatOfPoint2f(); - points.fromArray( - new Point(left.x, left.y + left.height), - new Point(left.x, left.y), - new Point(right.x + right.width, right.y), - new Point(right.x + right.width, right.y + right.height) - ); - return points; - } - - MatOfPoint2f target2020ResultMat = new MatOfPoint2f(); - - private double distanceBetween(Point a, Point b) { - return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2)); - } - - /** - * Find the target using the outermost tape corners and a 2020 target. Uses approxPolyDP to - * approximate the target outline. - * - * @param target the target. - * @return The four outermost tape corners. - */ - private MatOfPoint2f find2020VisionTarget(StandardCVPipeline.TrackedTarget target, - double accuracyPercentage) { - if (target.rawContour.cols() < 1) return null; - - var centroid = target.minAreaRect.center; - Comparator distanceProvider = - Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); - - // algorithm from team 4915 - - // Contour perimeter - var peri = Imgproc.arcLength(target.rawContour, true); - // approximating a shape around the contours - // Can be tuned to allow/disallow hulls - // Approx is the number of vertices - // Ramer–Douglas–Peucker algorithm - // we want a number between 0 and 0.16 out of a percentage from 0 to 100 - // so take accuracy and divide by 600 - Imgproc.approxPolyDP(target.rawContour, polyOutput, accuracyPercentage / 600.0 * peri, true); - - var area = Imgproc.moments(polyOutput); - -// if (area.get_m00() < 200) { -// return null; -// } - - var polyList = polyOutput.toList(); - - polyOutput.copyTo(target.approxPoly); - - // left top, left bottom, right bottom, right top - var boundingBoxCorners = findBoundingBoxCorners(target).toList(); - - try { - - // top left and top right are the poly corners closest to the bouding box tl and tr - var tl = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, - boundingBoxCorners.get(0)))).get(); - var tr = polyList.stream().min(Comparator.comparingDouble((Point p) -> distanceBetween(p, - boundingBoxCorners.get(3)))).get(); - - // bottom left and bottom right have to be in the correct quadrant and are the furthest - // from the center - var bl = - polyList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); - var br = - polyList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); - -// polyList = new ArrayList<>(polyList); -// polyList.removeAll(List.of(tl, tr, bl, br)); -// -// var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> -// distanceBetween(p, boundingBoxCorners.get(0)))).get(); -// var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> -// distanceBetween(p, boundingBoxCorners.get(3)))).get(); -// -// var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > -// centroid.y).max(distanceProvider).get(); -// var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > -// centroid.y).max(distanceProvider).get(); - - target2020ResultMat.release(); - target2020ResultMat.fromList(List.of(tl, bl, br, tr));//, tr2, br2, bl2, tl2)); - - return target2020ResultMat; - } catch (NoSuchElementException e) { - return null; - } - } - - /** - * Find the target using the outermost tape corners and a dual target. - * - * @param target the target. - * @return The four outermost tape corners. - */ - private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) { - if (target.leftRightRotatedRect == null) return null; - - var centroid = target.minAreaRect.center; - Comparator distanceProvider = - Comparator.comparingDouble((Point point) -> FastMath.sqrt(FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); - - var left = target.leftRightRotatedRect.getLeft(); - var right = target.leftRightRotatedRect.getRight(); - - // flip if the "left" target is to the right - if (left.center.x > right.center.x) { - var temp = left; - left = right; - right = temp; + public void set2020Target(boolean isHighGoal) { + if (isHighGoal) { + // tl, bl, br, tr is the order + List corners = + List.of( + new Point3(-19.625, 0, 0), + new Point3(-9.819867, -17, 0), + new Point3(9.819867, -17, 0), + new Point3(19.625, 0, 0)); + setObjectCorners(corners); + } else { + setBoundingBoxTarget(7, 11); + } } - var leftPoints = new Point[4]; - left.points(leftPoints); - var rightPoints = new Point[4]; - right.points(rightPoints); - ArrayList combinedList = new ArrayList<>(List.of(leftPoints)); - combinedList.addAll(List.of(rightPoints)); + public void setBoundingBoxTarget(double targetWidth, double targetHeight) { + // order is left top, left bottom, right bottom, right top - // start looking in the top left quadrant - var tl = - combinedList.stream().filter(point -> point.x < centroid.x && point.y < centroid.y).max(distanceProvider).get(); - var tr = - combinedList.stream().filter(point -> point.x > centroid.x && point.y < centroid.y).max(distanceProvider).get(); - var bl = - combinedList.stream().filter(point -> point.x < centroid.x && point.y > centroid.y).max(distanceProvider).get(); - var br = - combinedList.stream().filter(point -> point.x > centroid.x && point.y > centroid.y).max(distanceProvider).get(); - - boundingBoxResultMat.release(); - boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); - - return boundingBoxResultMat; - } - - /** - * @param target the target to find the corners of. - * @return the corners. left top, left bottom, right bottom, right top - */ - private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) { - -// List> list = new ArrayList<>(); -// // find the corners based on the bounding box -// // order is left top, left bottom, right bottom, right top - - // extract the corners - var points = new Point[4]; - target.minAreaRect.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); - - boundingBoxResultMat.release(); - boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); - - return boundingBoxResultMat; - } - - MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f(); - - private MatOfPoint2f refineCornersByBestTrack(MatOfPoint2f corners, Mat greyImg, - StandardCVPipeline.TrackedTarget target) { - - MatOfPoint approxf1 = new MatOfPoint(); - var origCornerList = new ArrayList<>(corners.toList()); - approxf1.fromList(origCornerList.stream() - .map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y)) - .collect(Collectors.toList()) - ); - var croppedImage = greyImg.submat(target.boundingRect); - - Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.1, 5); - - // at this point corners is still unmodified so let's map it - List tempList = new ArrayList<>(); - - // shift all points back into global pose - var reshiftedList = - approxf1.toList().stream().map(it -> new Point(it.x + target.boundingRect.x, - it.y + target.boundingRect.y)) - .collect(Collectors.toList()); - for (Point p : origCornerList) { - // find the goodFeaturesToTrack corner closest to me - var closestPoint = - reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p))); - if (closestPoint.isEmpty()) { - tempList.add(p); - reshiftedList.remove(p); - } else { - tempList.add(closestPoint.get()); - reshiftedList.remove(closestPoint.get()); - } + List corners = + List.of( + new Point3(-targetWidth / 2.0, targetHeight / 2.0, 0.0), + new Point3(-targetWidth / 2.0, -targetHeight / 2.0, 0.0), + new Point3(targetWidth / 2.0, -targetHeight / 2.0, 0.0), + new Point3(targetWidth / 2.0, targetHeight / 2.0, 0.0)); + setObjectCorners(corners); } - goodFeatureToTrackRetval.fromList(tempList); - return goodFeatureToTrackRetval; - } - - // Set the needed parameters to find the refined corners - Size winSize = new Size(4, 4); - Size zeroZone = new Size(-1, -1); // we don't need a zero zone - TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001); - - private boolean shouldRefineCorners = true; - - /** - * Refine an estimated corner position using the cornerSubPixel algorithm. - *

- * TODO should this be here or before the points are chosen? - * - * @param corners the corners detected -- this mat is modified! - * @param greyImg the image taken by the camera as color - * @return the updated mat, same as the corner mat passed in. - */ - private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) { - if (!shouldRefineCorners) return corners; // just return - Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria); - - return corners; - } - -// NetworkTableEntry tvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard") -// .getEntry("tvec"); -// NetworkTableEntry rvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard") -// .getEntry("rvec"); - - /** - * Calculate the pose of the vision target - * - * @param imageCornerPoints the corners we found. - * @param target the target to process, mutated. - * @return the target, with the pose2d added to it. - */ - public StandardCVPipeline.TrackedTarget calculatePose(MatOfPoint2f imageCornerPoints, - StandardCVPipeline.TrackedTarget target) { - if (objPointsMat.rows() != imageCornerPoints.rows() || cameraMatrix.rows() < 2 || distortionCoefficients.cols() < 4) { - System.err.println("can't do solvePNP with invalid params!"); - return null; + public void setObjectCorners(List objectCorners) { + objPointsMat.release(); + objPointsMat = new MatOfPoint3f(); + objPointsMat.fromList(objectCorners); } - imageCornerPoints.copyTo(target.imageCornerPoints); - - try { - Calib3d.solvePnP(objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, - rVec, tVec); - } catch (Exception e) { - e.printStackTrace(); - return null; + public void setConfig( + StandardCVPipelineSettings settings, CameraCalibrationConfig camConfig, Rotation2d tilt) { + setCameraCoeffs(camConfig); + // setBoundingBoxTarget(settings.targetWidth, settings.targetHeight); + // TODO add proper year differentiation + tilt_angle = tilt.getRadians(); + this.objPointsMat = settings.targetCornerMat; + this.accuracyPercentage = settings.accuracy.doubleValue(); } -// tvecE.setString(tVec.dump()); -// rvecE.setString(rVec.dump()); + private void setCameraCoeffs(CameraCalibrationConfig settings) { + if (settings == null) { + System.err.println( + "SolvePNP can only run on a calibrated resolution, and this one is not!" + + " Please calibrate to use solvePNP."); + return; + } + if (cameraMatrix != settings.getCameraMatrixAsMat()) { + cameraMatrix.release(); + settings.getCameraMatrixAsMat().copyTo(cameraMatrix); + } + if (distortionCoefficients != settings.getDistortionCoeffsAsMat()) { + distortionCoefficients.release(); + settings.getDistortionCoeffsAsMat().copyTo(distortionCoefficients); + } + this.distanceDivisor = settings.squareSize; + } - // Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision - // whitepaper + @Override + public Pair, Long> run( + Pair, Mat> imageTargetPair) { + long processStartNanos = System.nanoTime(); + var targets = imageTargetPair.getLeft(); + var image = imageTargetPair.getRight(); + Imgproc.cvtColor(image, greyImg, Imgproc.COLOR_BGR2GRAY); + targetList.clear(); + for (var target : targets) { + MatOfPoint2f corners; + // if it's a dual target use 2019, but default to 2020 + if (target.leftRightRotatedRect == null) { + corners = find2020VisionTarget(target, accuracyPercentage); // , imageTargetPair.getRight + // ()); //find2020VisionTarget(target);// (target.leftRightDualTargetPair != null) ? + // findCorner2019(target) : findBoundingBoxCorners(target); + } else { + corners = findCorner2019(target); + } + // var corners = findCorner2019(target); + if (corners == null) continue; - // the left/right distance to the target, unchanged by tilt. Inches - var x = tVec.get(0, 0)[0]; + // convert the corners into a Pose2d + var pose = calculatePose(corners, target); + targetList.add(pose); // TODO null check null poses. DO NOT ADD A NULL CHECK HERE, otherwise + // the order will be wrong. + } + long processTime = System.nanoTime() - processStartNanos; + return Pair.of(targetList, processTime); + } - // 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). Inches. - var z = tVec.get(2, 0)[0] * FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle); + /** + * basically we split the target's two tapes, find the min area rectangle for each, and take the + * outermost 4 corners out of the 2 rectangles + * + * @param target the target to use + * @return the 4 outermost corners. + */ + private MatOfPoint2f findCorner2019(StandardCVPipeline.TrackedTarget target) { + if (target.leftRightDualTargetPair == null) return null; - Calib3d.Rodrigues(rVec, rodriguez); - Core.transpose(rodriguez, rot_inv); // rodrigurz.t() + var left = target.leftRightDualTargetPair.getLeft(); + var right = target.leftRightDualTargetPair.getRight(); - scaledTvec = matScale(tVec, -1); - Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world); + // flip if the "left" target is to the right + if (left.x > right.x) { + var temp = left; + left = right; + right = temp; + } - var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.get(2, 0)[0]); + var points = new MatOfPoint2f(); + points.fromArray( + new Point(left.x, left.y + left.height), + new Point(left.x, left.y), + new Point(right.x + right.width, right.y), + new Point(right.x + right.width, right.y + right.height)); + return points; + } - // 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 + MatOfPoint2f target2020ResultMat = new MatOfPoint2f(); - // 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 + private double distanceBetween(Point a, Point b) { + return FastMath.sqrt(FastMath.pow(a.x - b.x, 2) + FastMath.pow(a.y - b.y, 2)); + } - //noinspection SuspiciousNameCombination - var targetLocation = new Translation2d(z, -x).times(25.4 / 1000d / distanceDivisor); - target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation)); - target.rVector = rVec; - target.tVector = tVec; + /** + * Find the target using the outermost tape corners and a 2020 target. Uses approxPolyDP to + * approximate the target outline. + * + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f find2020VisionTarget( + StandardCVPipeline.TrackedTarget target, double accuracyPercentage) { + if (target.rawContour.cols() < 1) return null; - return target; - } + var centroid = target.minAreaRect.center; + Comparator distanceProvider = + Comparator.comparingDouble( + (Point point) -> + FastMath.sqrt( + FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); - /** - * 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 - */ - public Mat matScale(Mat src, double factor) { - Mat dst = new Mat(src.rows(), src.cols(), src.type()); - Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this - Core.multiply(src, s, dst); - return dst; - } + // algorithm from team 4915 + // Contour perimeter + var peri = Imgproc.arcLength(target.rawContour, true); + // approximating a shape around the contours + // Can be tuned to allow/disallow hulls + // Approx is the number of vertices + // Ramer–Douglas–Peucker algorithm + // we want a number between 0 and 0.16 out of a percentage from 0 to 100 + // so take accuracy and divide by 600 + Imgproc.approxPolyDP(target.rawContour, polyOutput, accuracyPercentage / 600.0 * peri, true); + + var area = Imgproc.moments(polyOutput); + + // if (area.get_m00() < 200) { + // return null; + // } + + var polyList = polyOutput.toList(); + + polyOutput.copyTo(target.approxPoly); + + // left top, left bottom, right bottom, right top + var boundingBoxCorners = findBoundingBoxCorners(target).toList(); + + try { + + // top left and top right are the poly corners closest to the bouding box tl and tr + var tl = + polyList.stream() + .min( + Comparator.comparingDouble( + (Point p) -> distanceBetween(p, boundingBoxCorners.get(0)))) + .get(); + var tr = + polyList.stream() + .min( + Comparator.comparingDouble( + (Point p) -> distanceBetween(p, boundingBoxCorners.get(3)))) + .get(); + + // bottom left and bottom right have to be in the correct quadrant and are the furthest + // from the center + var bl = + polyList.stream() + .filter(point -> point.x < centroid.x && point.y > centroid.y) + .max(distanceProvider) + .get(); + var br = + polyList.stream() + .filter(point -> point.x > centroid.x && point.y > centroid.y) + .max(distanceProvider) + .get(); + + // polyList = new ArrayList<>(polyList); + // polyList.removeAll(List.of(tl, tr, bl, br)); + // + // var tl2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> + // distanceBetween(p, boundingBoxCorners.get(0)))).get(); + // var tr2 = polyList.stream().min(Comparator.comparingDouble((Point p) -> + // distanceBetween(p, boundingBoxCorners.get(3)))).get(); + // + // var bl2 = polyList.stream().filter(point -> point.x < centroid.x && point.y > + // centroid.y).max(distanceProvider).get(); + // var br2 = polyList.stream().filter(point -> point.x > centroid.x && point.y > + // centroid.y).max(distanceProvider).get(); + + target2020ResultMat.release(); + target2020ResultMat.fromList(List.of(tl, bl, br, tr)); // , tr2, br2, bl2, tl2)); + + return target2020ResultMat; + } catch (NoSuchElementException e) { + return null; + } + } + + /** + * Find the target using the outermost tape corners and a dual target. + * + * @param target the target. + * @return The four outermost tape corners. + */ + private MatOfPoint2f findDualTargetCornerMinAreaRect(StandardCVPipeline.TrackedTarget target) { + if (target.leftRightRotatedRect == null) return null; + + var centroid = target.minAreaRect.center; + Comparator distanceProvider = + Comparator.comparingDouble( + (Point point) -> + FastMath.sqrt( + FastMath.pow(centroid.x - point.x, 2) + FastMath.pow(centroid.y - point.y, 2))); + + var left = target.leftRightRotatedRect.getLeft(); + var right = target.leftRightRotatedRect.getRight(); + + // flip if the "left" target is to the right + if (left.center.x > right.center.x) { + var temp = left; + left = right; + right = temp; + } + + var leftPoints = new Point[4]; + left.points(leftPoints); + var rightPoints = new Point[4]; + right.points(rightPoints); + ArrayList combinedList = new ArrayList<>(List.of(leftPoints)); + combinedList.addAll(List.of(rightPoints)); + + // start looking in the top left quadrant + var tl = + combinedList.stream() + .filter(point -> point.x < centroid.x && point.y < centroid.y) + .max(distanceProvider) + .get(); + var tr = + combinedList.stream() + .filter(point -> point.x > centroid.x && point.y < centroid.y) + .max(distanceProvider) + .get(); + var bl = + combinedList.stream() + .filter(point -> point.x < centroid.x && point.y > centroid.y) + .max(distanceProvider) + .get(); + var br = + combinedList.stream() + .filter(point -> point.x > centroid.x && point.y > centroid.y) + .max(distanceProvider) + .get(); + + boundingBoxResultMat.release(); + boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); + + return boundingBoxResultMat; + } + + /** + * @param target the target to find the corners of. + * @return the corners. left top, left bottom, right bottom, right top + */ + private MatOfPoint2f findBoundingBoxCorners(StandardCVPipeline.TrackedTarget target) { + // extract the corners + var points = new Point[4]; + target.minAreaRect.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); + + boundingBoxResultMat.release(); + boundingBoxResultMat.fromList(List.of(tl, bl, br, tr)); + + return boundingBoxResultMat; + } + + MatOfPoint2f goodFeatureToTrackRetval = new MatOfPoint2f(); + + private MatOfPoint2f refineCornersByBestTrack( + MatOfPoint2f corners, Mat greyImg, StandardCVPipeline.TrackedTarget target) { + + MatOfPoint approxf1 = new MatOfPoint(); + var origCornerList = new ArrayList<>(corners.toList()); + approxf1.fromList( + origCornerList.stream() + .map(it -> new Point(it.x - target.boundingRect.x, it.y - target.boundingRect.y)) + .collect(Collectors.toList())); + var croppedImage = greyImg.submat(target.boundingRect); + + Imgproc.goodFeaturesToTrack(croppedImage, approxf1, 0, 0.1, 5); + + // at this point corners is still unmodified so let's map it + List tempList = new ArrayList<>(); + + // shift all points back into global pose + var reshiftedList = + approxf1.toList().stream() + .map(it -> new Point(it.x + target.boundingRect.x, it.y + target.boundingRect.y)) + .collect(Collectors.toList()); + for (Point p : origCornerList) { + // find the goodFeaturesToTrack corner closest to me + var closestPoint = + reshiftedList.stream().min(Comparator.comparingDouble(p_ -> distanceBetween(p_, p))); + if (closestPoint.isEmpty()) { + tempList.add(p); + reshiftedList.remove(p); + } else { + tempList.add(closestPoint.get()); + reshiftedList.remove(closestPoint.get()); + } + } + + goodFeatureToTrackRetval.fromList(tempList); + return goodFeatureToTrackRetval; + } + + // Set the needed parameters to find the refined corners + Size winSize = new Size(4, 4); + Size zeroZone = new Size(-1, -1); // we don't need a zero zone + TermCriteria criteria = new TermCriteria(TermCriteria.EPS + TermCriteria.COUNT, 90, 0.001); + + private boolean shouldRefineCorners = true; + + /** + * Refine an estimated corner position using the cornerSubPixel algorithm. + * + *

TODO should this be here or before the points are chosen? + * + * @param corners the corners detected -- this mat is modified! + * @param greyImg the image taken by the camera as color + * @return the updated mat, same as the corner mat passed in. + */ + private MatOfPoint2f refineCornerEstimateSubPix(MatOfPoint2f corners, Mat greyImg) { + if (!shouldRefineCorners) return corners; // just return + Imgproc.cornerSubPix(greyImg, corners, winSize, zeroZone, criteria); + + return corners; + } + + // NetworkTableEntry tvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard") + // .getEntry("tvec"); + // NetworkTableEntry rvecE = NetworkTableInstance.getDefault().getTable("SmartDashboard") + // .getEntry("rvec"); + + /** + * Calculate the pose of the vision target + * + * @param imageCornerPoints the corners we found. + * @param target the target to process, mutated. + * @return the target, with the pose2d added to it. + */ + public StandardCVPipeline.TrackedTarget calculatePose( + MatOfPoint2f imageCornerPoints, StandardCVPipeline.TrackedTarget target) { + if (objPointsMat.rows() != imageCornerPoints.rows() + || cameraMatrix.rows() < 2 + || distortionCoefficients.cols() < 4) { + System.err.println("can't do solvePNP with invalid params!"); + return null; + } + + imageCornerPoints.copyTo(target.imageCornerPoints); + + try { + Calib3d.solvePnP( + objPointsMat, imageCornerPoints, cameraMatrix, distortionCoefficients, rVec, tVec); + } catch (Exception e) { + e.printStackTrace(); + return null; + } + + // tvecE.setString(tVec.dump()); + // rvecE.setString(rVec.dump()); + + // Algorithm from team 5190 Green Hope Falcons. Can also be found in Ligerbot's vision + // whitepaper + + // the left/right distance to the target, unchanged by tilt. Inches + 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). Inches. + var z = + tVec.get(2, 0)[0] * FastMath.cos(tilt_angle) + tVec.get(1, 0)[0] * FastMath.sin(tilt_angle); + + Calib3d.Rodrigues(rVec, rodriguez); + Core.transpose(rodriguez, rot_inv); // rodrigurz.t() + + scaledTvec = matScale(tVec, -1); + Core.gemm(rot_inv, scaledTvec, 1, kMat, 0, pzero_world); + + var angle2 = FastMath.atan2(pzero_world.get(0, 0)[0], pzero_world.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(z, -x).times(25.4 / 1000d / distanceDivisor); + target.cameraRelativePose = new Pose2d(targetLocation, new Rotation2d(targetRotation)); + target.rVector = rVec; + target.tVector = tVec; + + return target; + } + + /** + * 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 + */ + public Mat matScale(Mat src, double factor) { + Mat dst = new Mat(src.rows(), src.cols(), src.type()); + Scalar s = new Scalar(factor); // TODO check if we need to add more elements to this + Core.multiply(src, s, dst); + return dst; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java index c67f5fca4..c4c7681e8 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/vision/pipeline/pipes/SortContoursPipe.java @@ -4,25 +4,34 @@ import com.chameleonvision._2.vision.camera.CaptureStaticProperties; import com.chameleonvision._2.vision.enums.SortMode; import com.chameleonvision._2.vision.pipeline.Pipe; import com.chameleonvision._2.vision.pipeline.impl.StandardCVPipeline; -import org.apache.commons.lang3.tuple.Pair; -import org.apache.commons.math3.util.FastMath; - import java.util.ArrayList; import java.util.Comparator; import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.apache.commons.math3.util.FastMath; -public class SortContoursPipe implements Pipe, List> { +public class SortContoursPipe + implements Pipe< + List, List> { - private final Comparator SortByCentermostComparator = Comparator.comparingDouble(this::calcSquareCenterDistance); + private final Comparator SortByCentermostComparator = + Comparator.comparingDouble(this::calcSquareCenterDistance); - private static final Comparator SortByLargestComparator = (rect1, rect2) -> Double.compare(rect2.minAreaRect.size.area(), rect1.minAreaRect.size.area()); - private static final Comparator SortBySmallestComparator = SortByLargestComparator.reversed(); + private static final Comparator SortByLargestComparator = + (rect1, rect2) -> + Double.compare(rect2.minAreaRect.size.area(), rect1.minAreaRect.size.area()); + private static final Comparator SortBySmallestComparator = + SortByLargestComparator.reversed(); - private static final Comparator SortByHighestComparator = (rect1, rect2) -> Double.compare(rect1.minAreaRect.center.y, rect2.minAreaRect.center.y); - private static final Comparator SortByLowestComparator = SortByHighestComparator.reversed(); + private static final Comparator SortByHighestComparator = + (rect1, rect2) -> Double.compare(rect1.minAreaRect.center.y, rect2.minAreaRect.center.y); + private static final Comparator SortByLowestComparator = + SortByHighestComparator.reversed(); - public static final Comparator SortByLeftmostComparator = Comparator.comparingDouble(target -> target.minAreaRect.center.x); - private static final Comparator SortByRightmostComparator = SortByLeftmostComparator.reversed(); + public static final Comparator SortByLeftmostComparator = + Comparator.comparingDouble(target -> target.minAreaRect.center.x); + private static final Comparator SortByRightmostComparator = + SortByLeftmostComparator.reversed(); private SortMode sort; private CaptureStaticProperties camProps; @@ -43,7 +52,8 @@ public class SortContoursPipe implements Pipe, Long> run(List input) { + public Pair, Long> run( + List input) { long processStartNanos = System.nanoTime(); sortedContours.clear(); @@ -78,8 +88,11 @@ public class SortContoursPipe implements Pipe(sortedContours.subList(0, Math.min(input.size(), maxTargets - 1))); - sortedContours.subList(Math.min(input.size(), maxTargets - 1), sortedContours.size()).forEach(StandardCVPipeline.TrackedTarget::release); + var sublistedContors = + new ArrayList<>(sortedContours.subList(0, Math.min(input.size(), maxTargets - 1))); + sortedContours + .subList(Math.min(input.size(), maxTargets - 1), sortedContours.size()) + .forEach(StandardCVPipeline.TrackedTarget::release); sortedContours.clear(); sortedContours = new ArrayList<>(); @@ -88,6 +101,8 @@ public class SortContoursPipe implements Pipe, List> { private double minPercentOfAvg; diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java b/chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java index 80a5f911d..852f2a9c3 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/web/RequestHandler.java @@ -19,8 +19,6 @@ import com.fasterxml.jackson.databind.ObjectMapper; import edu.wpi.first.wpilibj.geometry.Rotation2d; import io.javalin.http.Context; import io.javalin.http.UploadedFile; -import org.opencv.core.Point3; - import java.io.File; import java.io.FileOutputStream; import java.io.OutputStream; @@ -30,6 +28,7 @@ import java.util.ArrayList; import java.util.HashMap; import java.util.List; import java.util.Map; +import org.opencv.core.Point3; public class RequestHandler { @@ -56,7 +55,13 @@ public class RequestHandler { // setting up network config after saving boolean isStatic = ConfigManager.settings.connectionType.equals(NetworkMode.STATIC); - boolean state = NetworkManager.setHostname(ConfigManager.settings.hostname) && NetworkManager.setNetwork(isStatic, ConfigManager.settings.ip, ConfigManager.settings.netmask, ConfigManager.settings.gateway); + boolean state = + NetworkManager.setHostname(ConfigManager.settings.hostname) + && NetworkManager.setNetwork( + isStatic, + ConfigManager.settings.ip, + ConfigManager.settings.netmask, + ConfigManager.settings.gateway); if (state) { ctx.status(200); } else { @@ -77,9 +82,15 @@ public class RequestHandler { int cameraIndex = (Integer) data.getOrDefault("camera", -1); var pipelineIndex = (Integer) data.get("pipeline"); - StandardCVPipelineSettings origPipeline = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getPipeline(pipelineIndex).settings; + StandardCVPipelineSettings origPipeline = + (StandardCVPipelineSettings) + VisionManager.getCurrentUIVisionProcess() + .pipelineManager + .getPipeline(pipelineIndex) + .settings; String tmp = objectMapper.writeValueAsString(origPipeline); - StandardCVPipelineSettings newPipeline = objectMapper.readValue(tmp, StandardCVPipelineSettings.class); + StandardCVPipelineSettings newPipeline = + objectMapper.readValue(tmp, StandardCVPipelineSettings.class); if (cameraIndex == -1) { // same camera @@ -92,12 +103,17 @@ public class RequestHandler { newPipeline.videoModeIndex = cam.getCamera().getProperties().videoModes.size() - 1; } if (newPipeline.is3D) { - var calibration = cam.getCamera().getCalibration(cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex)); + var calibration = + cam.getCamera() + .getCalibration( + cam.getCamera().getProperties().getVideoMode(newPipeline.videoModeIndex)); if (calibration == null) { newPipeline.is3D = false; } } - VisionManager.getCurrentUIVisionProcess().pipelineManager.duplicatePipeline(newPipeline, cam); + VisionManager.getCurrentUIVisionProcess() + .pipelineManager + .duplicatePipeline(newPipeline, cam); ctx.status(200); } else { ctx.status(500); @@ -108,7 +124,6 @@ public class RequestHandler { } } - public static void onCameraSettings(Context ctx) { ObjectMapper objectMapper = kObjectMapper; try { @@ -152,13 +167,15 @@ public class RequestHandler { } // convert from mm to meters pipeManager.calib3dPipe.setSquareSize(squareSize); - VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = resolutionIndex; + VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe.settings.videoModeIndex = + resolutionIndex; VisionManager.getCurrentUIVisionProcess().pipelineManager.setCalibrationMode(true); VisionManager.getCurrentUIVisionProcess().getCamera().setVideoMode(resolutionIndex); } public static void onSnapshot(Context ctx) { - Calibrate3dPipeline calPipe = VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe; + Calibrate3dPipeline calPipe = + VisionManager.getCurrentUIVisionProcess().pipelineManager.calib3dPipe; calPipe.takeSnapshot(); @@ -213,8 +230,14 @@ public class RequestHandler { pointsList.add(pointToAdd); } System.out.println(pointsList.toString()); - if (VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings instanceof StandardCVPipelineSettings) { - var settings = (StandardCVPipelineSettings) VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings; + if (VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings + instanceof StandardCVPipelineSettings) { + var settings = + (StandardCVPipelineSettings) + VisionManager.getCurrentUIVisionProcess() + .pipelineManager + .getCurrentPipeline() + .settings; settings.targetCornerMat.fromList(pointsList); } } catch (Exception e) { @@ -235,7 +258,10 @@ public class RequestHandler { file.getContent().transferTo(stream); stream.close(); } else { - filePath = Paths.get(new File(Main.class.getProtectionDomain().getCodeSource().getLocation().toURI()).getPath()); // quirk to get the current file directory + filePath = + Paths.get( + new File(Main.class.getProtectionDomain().getCodeSource().getLocation().toURI()) + .getPath()); // quirk to get the current file directory } Helpers.setService(filePath); ctx.status(200); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java b/chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java index a888d95e3..47774456b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/web/Server.java @@ -9,25 +9,32 @@ public class Server { public static void main(int port) { socketHandler = new SocketHandler(); - Javalin app = Javalin.create(javalinConfig -> { - javalinConfig.showJavalinBanner = false; - javalinConfig.addStaticFiles("web"); - javalinConfig.enableCorsForAllOrigins(); - }); - app.ws("/websocket", ws -> { - ws.onConnect(ctx -> { - socketHandler.onConnect(ctx); - System.out.println("Socket Connected"); - }); - ws.onClose(ctx -> { - socketHandler.onClose(ctx); - System.out.println("Socket Disconnected"); - ConfigManager.saveGeneralSettings(); - }); - ws.onBinaryMessage(ctx -> { - socketHandler.onBinaryMessage(ctx); - }); - }); + Javalin app = + Javalin.create( + javalinConfig -> { + javalinConfig.showJavalinBanner = false; + javalinConfig.addStaticFiles("web"); + javalinConfig.enableCorsForAllOrigins(); + }); + app.ws( + "/websocket", + ws -> { + ws.onConnect( + ctx -> { + socketHandler.onConnect(ctx); + System.out.println("Socket Connected"); + }); + ws.onClose( + ctx -> { + socketHandler.onClose(ctx); + System.out.println("Socket Disconnected"); + ConfigManager.saveGeneralSettings(); + }); + ws.onBinaryMessage( + ctx -> { + socketHandler.onBinaryMessage(ctx); + }); + }); app.post("/api/settings/general", RequestHandler::onGeneralSettings); app.post("/api/settings/camera", RequestHandler::onCameraSettings); app.post("/api/vision/duplicate", RequestHandler::onDuplicatePipeline); diff --git a/chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java b/chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java index 7361bdb57..4b1b99ef5 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java +++ b/chameleon-server/src/main/java/com/chameleonvision/_2/web/SocketHandler.java @@ -20,9 +20,6 @@ import io.javalin.websocket.WsBinaryMessageContext; import io.javalin.websocket.WsCloseContext; import io.javalin.websocket.WsConnectContext; import io.javalin.websocket.WsContext; -import org.apache.commons.lang3.ArrayUtils; -import org.msgpack.jackson.dataformat.MessagePackFactory; - import java.lang.reflect.Field; import java.nio.ByteBuffer; import java.util.ArrayList; @@ -30,7 +27,8 @@ import java.util.HashMap; import java.util.List; import java.util.Map; import java.util.stream.Collectors; - +import org.apache.commons.lang3.ArrayUtils; +import org.msgpack.jackson.dataformat.MessagePackFactory; public class SocketHandler { @@ -55,144 +53,179 @@ public class SocketHandler { @SuppressWarnings("unchecked") void onBinaryMessage(WsBinaryMessageContext context) throws Exception { - Map deserialized = objectMapper.readValue((byte[]) ArrayUtils.toPrimitive(context.data()), - new TypeReference<>() { - }); + Map deserialized = + objectMapper.readValue( + (byte[]) ArrayUtils.toPrimitive(context.data()), new TypeReference<>() {}); for (Map.Entry entry : deserialized.entrySet()) { try { VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess(); CameraCapture currentCamera = currentProcess.getCamera(); CVPipeline currentPipeline = currentProcess.pipelineManager.getCurrentPipeline(); -// System.out.println("entry.getKey()+entry.getValue()= " + entry.getKey() + entry.getValue()); + // System.out.println("entry.getKey()+entry.getValue()= " + entry.getKey() + + // entry.getValue()); switch (entry.getKey()) { - case "driverMode": { - HashMap data = (HashMap) entry.getValue(); - currentProcess.getDriverModeSettings().exposure = (Integer) data.get("driverExposure"); - currentProcess.getDriverModeSettings().brightness = (Integer) data.get("driverBrightness"); - currentProcess.setDriverMode((Boolean) data.get("isDriver")); + case "driverMode": + { + HashMap data = (HashMap) entry.getValue(); + currentProcess.getDriverModeSettings().exposure = + (Integer) data.get("driverExposure"); + currentProcess.getDriverModeSettings().brightness = + (Integer) data.get("driverBrightness"); + currentProcess.setDriverMode((Boolean) data.get("isDriver")); - VisionManager.saveCurrentCameraDriverMode(); - break; - } - case "changeCameraName": { - currentProcess.setCameraNickname((String) entry.getValue()); - sendFullSettings(); - VisionManager.saveCurrentCameraSettings(); - break; - } - case "changePipelineName": { - currentProcess.pipelineManager.renameCurrentPipeline((String) entry.getValue()); - sendFullSettings(); - VisionManager.saveCurrentCameraPipelines(); - break; - } - case "addNewPipeline": { -// HashMap data = (HashMap) entry.getValue(); - String pipeName = (String) entry.getValue(); - // TODO: add to UI selection for new 2d/3d - currentProcess.pipelineManager.addNewPipeline(pipeName); - sendFullSettings(); - VisionManager.saveCurrentCameraPipelines(); - break; - } - case "command": { - switch ((String) entry.getValue()) { - case "deleteCurrentPipeline": - currentProcess.pipelineManager.deleteCurrentPipeline(); - sendFullSettings(); - VisionManager.saveCurrentCameraPipelines(); - break; - case "save": - ConfigManager.saveGeneralSettings(); - VisionManager.saveAllCameras(); - System.out.println("Saved Settings"); - break; + VisionManager.saveCurrentCameraDriverMode(); + break; } - // used to define all incoming commands - break; - } - case "currentCamera": { - VisionManager.setCurrentProcessByIndex((Integer) entry.getValue()); - sendFullSettings(); - break; - } - case "is3D": { - VisionManager.getCurrentUIVisionProcess().setIs3d((Boolean) entry.getValue()); - break; - } - case "currentPipeline": { - currentProcess.pipelineManager.setCurrentPipeline((Integer) entry.getValue()); - sendFullSettings(); - break; - } - case "isPNPCalibration": { - currentProcess.pipelineManager.setCalibrationMode((Boolean) entry.getValue()); - break; - } - case "takeCalibrationSnapshot": { - currentProcess.pipelineManager.calib3dPipe.takeSnapshot(); - } - default: { - - switch (entry.getKey()) {//Pre field value set - case "rotationMode": {//Create new CaptureStaticProperties with new width and height, reset crosshair calib - ImageRotationMode oldRot = currentPipeline.settings.rotationMode; - ImageRotationMode newRot = ImageRotationMode.class.getEnumConstants()[(Integer) entry.getValue()]; - CaptureStaticProperties prop = currentCamera.getProperties().getStaticProperties(); - int width, height; - if (oldRot.isRotated() != newRot.isRotated()) { - width = prop.mode.height; - height = prop.mode.width; - //Creates new video mode with new width and height to create new CaptureStaticProperties and applies it - currentCamera.getProperties().setStaticProperties(new CaptureStaticProperties( - new VideoMode(prop.mode.pixelFormat, width, height, prop.mode.fps), prop.fov)); - } - prop = currentCamera.getProperties().getStaticProperties(); - currentProcess.cameraStreamer.recalculateDivision(); - if (currentPipeline instanceof StandardCVPipeline) - ((StandardCVPipeline) currentPipeline).settings.point.set(prop.mode.width / 2.0, prop.mode.height / 2.0);//Reset Crosshair in single point calib - break; - } - + case "changeCameraName": + { + currentProcess.setCameraNickname((String) entry.getValue()); + sendFullSettings(); + VisionManager.saveCurrentCameraSettings(); + break; } - - - if (currentProcess.pipelineManager.getDriverMode()) { - setField(currentProcess.pipelineManager.driverModePipeline.settings, entry.getKey(), entry.getValue()); - } else { - setField(currentPipeline.settings, entry.getKey(), entry.getValue()); + case "changePipelineName": + { + currentProcess.pipelineManager.renameCurrentPipeline((String) entry.getValue()); + sendFullSettings(); + VisionManager.saveCurrentCameraPipelines(); + break; } - - //Post field value set - switch (entry.getKey()) { - case "exposure": { - currentCamera.setExposure((Integer) entry.getValue()); - break; - } - case "brightness": { - currentCamera.setBrightness((Integer) entry.getValue()); - break; - } - case "gain": { - currentCamera.setGain((Integer) entry.getValue()); - break; - } - case "videoModeIndex": { - if (currentPipeline instanceof StandardCVPipeline) - ((StandardCVPipeline) currentPipeline).settings.point = new DoubleCouple();//This will reset the calibration - currentCamera.setVideoMode((Integer) entry.getValue()); - currentProcess.cameraStreamer.recalculateDivision(); - break; - } - case "streamDivisor": { - currentProcess.cameraStreamer.setDivisor(StreamDivisor.values()[(Integer) entry.getValue()], true); - break; - } + case "addNewPipeline": + { + // HashMap data = (HashMap) + // entry.getValue(); + String pipeName = (String) entry.getValue(); + // TODO: add to UI selection for new 2d/3d + currentProcess.pipelineManager.addNewPipeline(pipeName); + sendFullSettings(); + VisionManager.saveCurrentCameraPipelines(); + break; } + case "command": + { + switch ((String) entry.getValue()) { + case "deleteCurrentPipeline": + currentProcess.pipelineManager.deleteCurrentPipeline(); + sendFullSettings(); + VisionManager.saveCurrentCameraPipelines(); + break; + case "save": + ConfigManager.saveGeneralSettings(); + VisionManager.saveAllCameras(); + System.out.println("Saved Settings"); + break; + } + // used to define all incoming commands + break; + } + case "currentCamera": + { + VisionManager.setCurrentProcessByIndex((Integer) entry.getValue()); + sendFullSettings(); + break; + } + case "is3D": + { + VisionManager.getCurrentUIVisionProcess().setIs3d((Boolean) entry.getValue()); + break; + } + case "currentPipeline": + { + currentProcess.pipelineManager.setCurrentPipeline((Integer) entry.getValue()); + sendFullSettings(); + break; + } + case "isPNPCalibration": + { + currentProcess.pipelineManager.setCalibrationMode((Boolean) entry.getValue()); + break; + } + case "takeCalibrationSnapshot": + { + currentProcess.pipelineManager.calib3dPipe.takeSnapshot(); + } + default: + { + switch (entry.getKey()) { // Pre field value set + case "rotationMode": + { // Create new CaptureStaticProperties with new width and height, reset crosshair + // calib + ImageRotationMode oldRot = currentPipeline.settings.rotationMode; + ImageRotationMode newRot = + ImageRotationMode.class.getEnumConstants()[(Integer) entry.getValue()]; + CaptureStaticProperties prop = + currentCamera.getProperties().getStaticProperties(); + int width, height; + if (oldRot.isRotated() != newRot.isRotated()) { + width = prop.mode.height; + height = prop.mode.width; + // Creates new video mode with new width and height to create new + // CaptureStaticProperties and applies it + currentCamera + .getProperties() + .setStaticProperties( + new CaptureStaticProperties( + new VideoMode( + prop.mode.pixelFormat, width, height, prop.mode.fps), + prop.fov)); + } + prop = currentCamera.getProperties().getStaticProperties(); + currentProcess.cameraStreamer.recalculateDivision(); + if (currentPipeline instanceof StandardCVPipeline) + ((StandardCVPipeline) currentPipeline) + .settings.point.set( + prop.mode.width / 2.0, + prop.mode.height / 2.0); // Reset Crosshair in single point calib + break; + } + } - VisionManager.saveCurrentCameraPipelines(); - break; - } + if (currentProcess.pipelineManager.getDriverMode()) { + setField( + currentProcess.pipelineManager.driverModePipeline.settings, + entry.getKey(), + entry.getValue()); + } else { + setField(currentPipeline.settings, entry.getKey(), entry.getValue()); + } + + // Post field value set + switch (entry.getKey()) { + case "exposure": + { + currentCamera.setExposure((Integer) entry.getValue()); + break; + } + case "brightness": + { + currentCamera.setBrightness((Integer) entry.getValue()); + break; + } + case "gain": + { + currentCamera.setGain((Integer) entry.getValue()); + break; + } + case "videoModeIndex": + { + if (currentPipeline instanceof StandardCVPipeline) + ((StandardCVPipeline) currentPipeline).settings.point = + new DoubleCouple(); // This will reset the calibration + currentCamera.setVideoMode((Integer) entry.getValue()); + currentProcess.cameraStreamer.recalculateDivision(); + break; + } + case "streamDivisor": + { + currentProcess.cameraStreamer.setDivisor( + StreamDivisor.values()[(Integer) entry.getValue()], true); + break; + } + } + + VisionManager.saveCurrentCameraPipelines(); + break; + } } } catch (Exception e) { System.err.println(e.getMessage()); @@ -206,8 +239,7 @@ public class SocketHandler { Field field = obj.getClass().getField(fieldName); if (field.getType().isEnum()) field.set(obj, field.getType().getEnumConstants()[(Integer) value]); - else - field.set(obj, value); + else field.set(obj, value); } catch (NoSuchFieldException | IllegalAccessException ex) { ex.printStackTrace(); } @@ -233,17 +265,32 @@ public class SocketHandler { } public static void broadcastMessage(Object obj) { - broadcastMessage(obj, null);//Broadcasts the message to every user + broadcastMessage(obj, null); // Broadcasts the message to every user } - private static HashMap getOrdinalPipeline(Class cvClass) throws IllegalAccessException { + private static HashMap getOrdinalPipeline(Class cvClass) + throws IllegalAccessException { HashMap tmp = new HashMap<>(); for (Field field : cvClass.getFields()) { // iterate over every field in CVPipelineSettings try { - if (!field.getType().isEnum()) { // if the field is not an enum, get it based on the current pipeline - tmp.put(field.getName(), field.get(VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings)); + if (!field + .getType() + .isEnum()) { // if the field is not an enum, get it based on the current pipeline + tmp.put( + field.getName(), + field.get( + VisionManager.getCurrentUIVisionProcess() + .pipelineManager + .getCurrentPipeline() + .settings)); } else { - var ordinal = (Enum) field.get(VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipeline().settings); + var ordinal = + (Enum) + field.get( + VisionManager.getCurrentUIVisionProcess() + .pipelineManager + .getCurrentPipeline() + .settings); tmp.put(field.getName(), ordinal.ordinal()); } } catch (IllegalArgumentException e) { @@ -271,18 +318,21 @@ public class SocketHandler { tmp.put("fov", currentCamera.getProperties().getFOV()); tmp.put("streamDivisor", currentVisionProcess.cameraStreamer.getDivisor().ordinal()); - tmp.put("resolution", currentVisionProcess.getCamera().getProperties().getCurrentVideoModeIndex()); + tmp.put( + "resolution", currentVisionProcess.getCamera().getProperties().getCurrentVideoModeIndex()); tmp.put("tilt", currentVisionProcess.getCamera().getProperties().getTilt().getDegrees()); - List calibrations = currentCamera.getAllCalibrationData().stream() - .map(CameraCalibrationConfig.UICameraCalibrationConfig::new).collect(Collectors.toList()); + List calibrations = + currentCamera.getAllCalibrationData().stream() + .map(CameraCalibrationConfig.UICameraCalibrationConfig::new) + .collect(Collectors.toList()); tmp.put("calibration", calibrations); return tmp; } public static void sendFullSettings() { - //General settings + // General settings Map fullSettings = new HashMap<>(); VisionProcess currentProcess = VisionManager.getCurrentUIVisionProcess(); @@ -296,7 +346,9 @@ public class SocketHandler { fullSettings.put("pipelineList", VisionManager.getCurrentCameraPipelineNicknames()); fullSettings.put("resolutionList", VisionManager.getCurrentCameraResolutionList()); fullSettings.put("port", currentProcess.cameraStreamer.getStreamPort()); - fullSettings.put("currentPipelineIndex", VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipelineIndex()); + fullSettings.put( + "currentPipelineIndex", + VisionManager.getCurrentUIVisionProcess().pipelineManager.getCurrentPipelineIndex()); fullSettings.put("currentCameraIndex", VisionManager.getCurrentUIVisionProcessIndex()); } catch (IllegalAccessException e) { System.err.println("No camera found!"); diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/calibration/CameraCalibrationCoefficients.java b/chameleon-server/src/main/java/com/chameleonvision/common/calibration/CameraCalibrationCoefficients.java new file mode 100644 index 000000000..29f9e8085 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/calibration/CameraCalibrationCoefficients.java @@ -0,0 +1,46 @@ +package com.chameleonvision.common.calibration; + +import com.chameleonvision.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(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/calibration/JsonMat.java b/chameleon-server/src/main/java/com/chameleonvision/common/calibration/JsonMat.java new file mode 100644 index 000000000..5677963ab --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/calibration/JsonMat.java @@ -0,0 +1,92 @@ +package com.chameleonvision.common.calibration; + +import com.chameleonvision.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(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java index c79dcabaa..e7e79c65b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/datatransfer/networktables/NetworkTablesManager.java @@ -6,11 +6,15 @@ 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.slf4j.Logger; +import org.slf4j.LoggerFactory; public class NetworkTablesManager { private NetworkTablesManager() {} + private static final Logger logger = LoggerFactory.getLogger(NetworkTablesManager.class); + private static final NetworkTableInstance ntInstance = NetworkTableInstance.getDefault(); public static final String kRootTableName = "/chameleon-vision"; @@ -32,10 +36,10 @@ public class NetworkTablesManager { @Override public void accept(LogMessage logMessage) { if (!hasReportedConnectionFailure && logMessage.message.contains("timed out")) { - System.err.println("NT Connection has failed! Will retry in background."); + logger.error("NT Connection has failed! Will retry in background."); hasReportedConnectionFailure = true; } else if (logMessage.message.contains("connected")) { - System.out.println("NT Connected!"); + logger.info("NT Connected!"); hasReportedConnectionFailure = false; ScriptManager.queueEvent(ScriptEventType.kNTConnected); } @@ -48,16 +52,16 @@ public class NetworkTablesManager { public static void setClientMode(String host) { isServer = false; - System.out.println("Starting NT Client"); + logger.info("Starting NT Client"); ntInstance.stopServer(); if (host != null) { ntInstance.startClient(host); } else { ntInstance.startClientTeam(getTeamNumber()); if (ntInstance.isConnected()) { - System.out.println("[NetworkTablesManager] Connected to the robot!"); + logger.info("[NetworkTablesManager] Connected to the robot!"); } else { - System.out.println( + logger.info( "[NetworkTablesManager] Could NOT to the robot! Will retry in the background..."); } } @@ -69,7 +73,7 @@ public class NetworkTablesManager { public static void setServerMode() { isServer = true; - System.out.println("Starting NT Server"); + logger.info("Starting NT Server"); ntInstance.stopClient(); ntInstance.startServer(); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java b/chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java deleted file mode 100644 index b37f982e1..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/logging/DebugLogger.java +++ /dev/null @@ -1,20 +0,0 @@ -package com.chameleonvision.common.logging; - -public class DebugLogger { - - private final boolean verbose; - - public DebugLogger(boolean verbose) { - this.verbose = verbose; - } - - public void printInfo(String infoMessage) { - if (verbose) { - System.out.println(infoMessage); - } - } - - public void printInfo(String smallInfo, String largeInfo) { - System.out.println(verbose ? String.format("%s - %s", smallInfo, largeInfo) : smallInfo); - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java b/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java index 1df9dc0f3..b02e9df16 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/networking/LinuxNetworking.java @@ -9,13 +9,18 @@ import java.util.ArrayList; import java.util.Collections; import java.util.List; import org.apache.commons.io.FileUtils; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; public class LinuxNetworking extends SysNetworking { private static final String PATH = "/etc/dhcpcd.conf"; + private Logger logger = LoggerFactory.getLogger(LinuxNetworking.class); + @Override public boolean setDHCP() { File dhcpConf = new File(PATH); + logger.debug("Removing static IP from {}", PATH); if (dhcpConf.exists()) { try { List lines = FileUtils.readLines(dhcpConf, StandardCharsets.UTF_8); @@ -44,7 +49,7 @@ public class LinuxNetworking extends SysNetworking { } } else { - System.err.println("dhcpcd5 is not installed, unable to set IP."); + logger.error("dhcpcd5 is not installed, unable to set IP."); return false; } return true; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java index e2c545808..4dd6e67b7 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptEvent.java @@ -1,14 +1,15 @@ package com.chameleonvision.common.scripting; -import com.chameleonvision.common.logging.DebugLogger; import com.chameleonvision.common.util.ShellExec; import java.io.IOException; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; public class ScriptEvent { - private static final DebugLogger logger = new DebugLogger(true); private static final ShellExec executor = new ShellExec(true, true); public final ScriptConfig config; + private final Logger logger = LoggerFactory.getLogger(ScriptEvent.class); public ScriptEvent(ScriptConfig config) { this.config = config; @@ -23,12 +24,13 @@ public class ScriptEvent { if (!error.isEmpty()) { System.err.printf("Error when running \"%s\" script: %s\n", config.eventType.name(), error); } else if (!output.isEmpty()) { - logger.printInfo( + logger.info( String.format("Output from \"%s\" script: %s\n", config.eventType.name(), output)); } - logger.printInfo( + logger.info( String.format( - "Script for %s ran with command line: \"%s\", exit code: %d, output: %s, error: %s\n", + "Script for %s ran with command line: \"%s\", exit code: %d, output: %s, " + + "error: %s\n", config.eventType.name(), config.command, retVal, output, error)); return retVal; } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java index 4edf0a883..e3839b1a9 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/scripting/ScriptManager.java @@ -1,6 +1,5 @@ package com.chameleonvision.common.scripting; -import com.chameleonvision.common.logging.DebugLogger; import com.chameleonvision.common.util.LoopingRunnable; import com.chameleonvision.common.util.Platform; import com.chameleonvision.common.util.file.JacksonUtils; @@ -11,10 +10,12 @@ import java.nio.file.Paths; import java.util.ArrayList; import java.util.List; import java.util.concurrent.LinkedBlockingDeque; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; public class ScriptManager { - private static DebugLogger logger = new DebugLogger(true); + private static final Logger logger = LoggerFactory.getLogger(ScriptManager.class); private ScriptManager() {} @@ -124,7 +125,7 @@ public class ScriptManager { if (!Platform.CurrentPlatform.isWindows()) { try { queuedEvents.putLast(eventType); - logger.printInfo("Queued event: " + eventType.name()); + logger.info("Queued event: " + eventType.name()); } catch (InterruptedException e) { System.err.println("Failed to add event to queue: " + eventType.name()); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/ReflectionUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/ReflectionUtils.java new file mode 100644 index 000000000..ec6590cae --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/ReflectionUtils.java @@ -0,0 +1,42 @@ +package com.chameleonvision.common.util; + +public class ReflectionUtils { + + public static StackTraceElement[] getFullStackTrace() { + return Thread.currentThread().getStackTrace(); + } + + public static StackTraceElement getNthCaller(int n) { + if (n < 0) n = 0; + return Thread.currentThread().getStackTrace()[n]; + } + + public static String getCallerClassName() { + StackTraceElement[] stElements = Thread.currentThread().getStackTrace(); + for (int i = 1; i < stElements.length; i++) { + StackTraceElement ste = stElements[i]; + if (!ste.getClassName().equals(ReflectionUtils.class.getName()) + && ste.getClassName().indexOf("java.lang.Thread") != 0) { + return ste.getClassName(); + } + } + return null; + } + + public static String getCallerCallerClassName() { + StackTraceElement[] stElements = Thread.currentThread().getStackTrace(); + String callerClassName = null; + for (int i = 1; i < stElements.length; i++) { + StackTraceElement ste = stElements[i]; + if (!ste.getClassName().equals(ReflectionUtils.class.getName()) + && ste.getClassName().indexOf("java.lang.Thread") != 0) { + if (callerClassName == null) { + callerClassName = ste.getClassName(); + } else if (!callerClassName.equals(ste.getClassName())) { + return ste.getClassName(); + } + } + } + return null; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/TestUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/TestUtils.java new file mode 100644 index 000000000..5c5e4db4f --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/TestUtils.java @@ -0,0 +1,123 @@ +package com.chameleonvision.common.util; + +import edu.wpi.cscore.CameraServerCvJNI; +import java.io.File; +import java.io.IOException; +import java.nio.file.Path; +import java.util.Optional; +import org.opencv.core.Mat; +import org.opencv.highgui.HighGui; + +public class TestUtils { + + public enum WPI2019Image { + kCargoAngledDark48in(1.2192), + kCargoSideStraightDark36in(0.9144), + kCargoSideStraightDark60in(1.524), + kCargoSideStraightDark72in(1.8288), + kCargoSideStraightPanelDark36in(0.9144), + kCargoStraightDark19in(0.4826), + kCargoStraightDark24in(0.6096), + kCargoStraightDark48in(1.2192), + kCargoStraightDark72in(1.8288), + kCargoStraightDark72in_HighRes(1.8288), + kCargoStraightDark90in(2.286); + + public static double FOV = 68.5; + + public final double distanceMeters; + public final String path; + + String getPath() { + var filename = this.toString().substring(1); + return "\\2019\\WPI\\" + filename + ".jpg"; + } + + WPI2019Image(double distanceMeters) { + this.distanceMeters = distanceMeters; + this.path = getPath(); + } + } + + public enum WPI2020Image { + kBlueGoal_060in_Center(1.524), + kBlueGoal_084in_Center(2.1336), + kBlueGoal_108in_Center(2.7432), + kBlueGoal_132in_Center(3.3528), + kBlueGoal_156in_Center(3.9624), + kBlueGoal_180in_Center(4.572), + kBlueGoal_156in_Left(3.9624), + kBlueGoal_224in_Left(5.6896), + kBlueGoal_228in_ProtectedZone(5.7912), + kBlueGoal_330in_ProtectedZone(8.382), + kBlueGoal_Far_ProtectedZone(10.668), // TODO: find a more accurate distance + kRedLoading_016in_Down(0.4064), + kRedLoading_030in_Down(0.762), + kRedLoading_048in_Down(1.2192), + kRedLoading_048in(1.2192), + kRedLoading_060in(1.524), + kRedLoading_084in(2.1336), + kRedLoading_108in(2.7432); + + public static double FOV = 68.5; + + public final double distanceMeters; + public final String path; + + String getPath() { + var filename = this.toString().substring(1).replace('_', '-'); + return "\\2020\\WPI\\" + filename + ".jpg"; + } + + WPI2020Image(double distanceMeters) { + this.distanceMeters = distanceMeters; + this.path = getPath(); + } + } + + private static Path getTestImagesPath() { + var folder = TestUtils.class.getClassLoader().getResource("testimages"); + return Optional.ofNullable(folder).map(url -> new File(url.getFile()).toPath()).orElse(null); + } + + public static Path getCalibrationPath() { + var folder = TestUtils.class.getClassLoader().getResource("calibration"); + return Optional.ofNullable(folder).map(url -> new File(url.getFile()).toPath()).orElse(null); + } + + public static Path getWPIImagePath(WPI2020Image image) { + return Path.of(getTestImagesPath().toString(), image.path); + } + + public static Path getWPIImagePath(WPI2019Image image) { + return Path.of(getTestImagesPath().toString(), image.path); + } + + public static void loadLibraries() { + try { + CameraServerCvJNI.forceLoad(); + } catch (IOException e) { + // ignored + } + } + + private static int DefaultTimeoutMillis = 5000; + + public static void showImage(Mat frame, String title, int timeoutMs) { + HighGui.imshow(title, frame); + HighGui.waitKey(timeoutMs); + HighGui.destroyAllWindows(); + } + + public static void showImage(Mat frame, int timeoutMs) { + showImage(frame, "", timeoutMs); + } + + public static void showImage(Mat frame, String title) { + showImage(frame, title, DefaultTimeoutMillis); + } + + public static void showImage(Mat frame) { + showImage(frame, DefaultTimeoutMillis); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java index b02366622..9c838133b 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/file/FileUtils.java @@ -1,6 +1,5 @@ package com.chameleonvision.common.util.file; -import com.chameleonvision.common.logging.DebugLogger; import com.chameleonvision.common.util.Platform; import java.io.File; import java.io.IOException; @@ -11,9 +10,14 @@ import java.nio.file.attribute.PosixFilePermission; import java.util.Arrays; import java.util.HashSet; import java.util.Set; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; public class FileUtils { - private static DebugLogger logger = new DebugLogger(true); + + private FileUtils() {} + + private static Logger logger = LoggerFactory.getLogger(FileUtils.class); private static final Set allReadWriteExecutePerms = new HashSet<>(Arrays.asList(PosixFilePermission.values())); @@ -23,7 +27,7 @@ public class FileUtils { Set perms = Files.readAttributes(path, PosixFileAttributes.class).permissions(); if (!perms.equals(allReadWriteExecutePerms)) { - logger.printInfo("Setting perms on" + path.toString()); + logger.info("Setting perms on" + path.toString()); Files.setPosixFilePermissions(path, perms); if (thisFile.isDirectory()) { for (File subfile : thisFile.listFiles()) { @@ -46,8 +50,7 @@ public class FileUtils { } } else { // TODO file perms on Windows - System.out.println( - "File permission setting not available on Windows. Not changing file permissions."); + logger.info("Cannot set directory permissions on Windows!"); } } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java index f08fa672f..61337cf45 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/math/MathUtils.java @@ -26,4 +26,8 @@ public class MathUtils { double toMult = Math.pow(10, to); return (double) Math.round(value * toMult) / toMult; } + + public static double nanosToMillis(long nanos) { + return nanos / 1000000.0; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java index a69d6e439..bf1ceea87 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/DoubleCouple.java @@ -1,5 +1,7 @@ package com.chameleonvision.common.util.numbers; +import org.opencv.core.Point; + public class DoubleCouple extends NumberCouple { public DoubleCouple() { @@ -9,4 +11,17 @@ public class DoubleCouple extends NumberCouple { public DoubleCouple(Double first, Double second) { super(first, second); } + + public DoubleCouple(Point point) { + super(point.x, point.y); + } + + public Point toPoint() { + return new Point(first, second); + } + + public void fromPoint(Point point) { + first = point.x; + second = point.y; + } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java index 8535a713e..bc5c0b14c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/numbers/NumberCouple.java @@ -2,8 +2,8 @@ package com.chameleonvision.common.util.numbers; public abstract class NumberCouple { - private T first; - private T second; + protected T first; + protected T second; public NumberCouple(T first, T second) { this.first = first; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/camera/CaptureStaticProperties.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/camera/CaptureStaticProperties.java deleted file mode 100644 index f2dee217e..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/camera/CaptureStaticProperties.java +++ /dev/null @@ -1,49 +0,0 @@ -package com.chameleonvision.common.vision.camera; - -import edu.wpi.cscore.VideoMode; -import org.apache.commons.math3.fraction.Fraction; -import org.apache.commons.math3.util.FastMath; -import org.opencv.core.Point; - -public class CaptureStaticProperties { - public final int imageWidth; - public final int imageHeight; - public final double fov; - public final double imageArea; - public final double centerX; - public final double centerY; - public final Point centerPoint; - public final double horizontalFocalLength; - public final double verticalFocalLength; - public final VideoMode mode; - - public CaptureStaticProperties(VideoMode mode, double fov) { - this.mode = mode; - - this.imageWidth = mode.width; - this.imageHeight = mode.height; - this.fov = fov; - - imageArea = imageHeight * imageWidth; - centerX = imageWidth / 2.0 - 0.5; - centerY = imageHeight / 2.0 - 0.5; - centerPoint = new Point(centerX, centerY); - - // Calculations from pinhole-model. - double diagonalView = FastMath.toRadians(this.fov); - Fraction aspectRatio = new Fraction(imageWidth, imageHeight); - - int horizontalRatio = aspectRatio.getNumerator(); - int verticalRatio = aspectRatio.getDenominator(); - - double diagonalAspect = FastMath.hypot(horizontalRatio, verticalRatio); - - double horizontalView = - FastMath.atan(FastMath.tan(diagonalView / 2) * (horizontalRatio / diagonalAspect)) * 2; - double verticalView = - FastMath.atan(FastMath.tan(diagonalView / 2) * (verticalRatio / diagonalAspect)) * 2; - - horizontalFocalLength = imageWidth / (2 * FastMath.tan(horizontalView / 2)); - verticalFocalLength = imageHeight / (2 * FastMath.tan(verticalView / 2)); - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/Frame.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/Frame.java index e932572f9..1176b7c06 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/Frame.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/Frame.java @@ -1,18 +1,37 @@ package com.chameleonvision.common.vision.frame; +import com.chameleonvision.common.vision.opencv.CVMat; +import com.chameleonvision.common.vision.opencv.Releasable; import org.opencv.core.Mat; -public class Frame { - public long timestampNanos; - public Mat image; +public class Frame implements Releasable { + public final long timestampNanos; + public final CVMat image; + public final FrameStaticProperties frameStaticProperties; - public Frame(Mat image) { - this.image = image; - timestampNanos = System.nanoTime(); - } - - public Frame(Mat image, long timestampNanos) { + 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(); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameDivisor.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameDivisor.java new file mode 100644 index 000000000..ddadf951b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameDivisor.java @@ -0,0 +1,14 @@ +package com.chameleonvision.common.vision.frame; + +public enum FrameDivisor { + NONE(1), + HALF(2), + QUARTER(4), + SIXTH(6); + + public final Integer value; + + FrameDivisor(int value) { + this.value = value; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java index 1827df9f7..37cd24078 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameProvider.java @@ -2,6 +2,4 @@ package com.chameleonvision.common.vision.frame; public interface FrameProvider { Frame getFrame(); - - FrameStaticProperties getFrameProperties(); } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameStaticProperties.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameStaticProperties.java index 640acfbcb..9408ba5b4 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameStaticProperties.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/FrameStaticProperties.java @@ -3,6 +3,7 @@ package com.chameleonvision.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 { @@ -12,6 +13,7 @@ public class FrameStaticProperties { public final double imageArea; public final double centerX; public final double centerY; + public final Point centerPoint; public final double horizontalFocalLength; public final double verticalFocalLength; @@ -41,6 +43,7 @@ public class FrameStaticProperties { 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); diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java new file mode 100644 index 000000000..3ea069317 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/consumer/DummyFrameConsumer.java @@ -0,0 +1,11 @@ +package com.chameleonvision.common.vision.frame.consumer; + +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameConsumer; + +public class DummyFrameConsumer implements FrameConsumer { + @Override + public void consume(Frame frame) { + frame.release(); // lol ez + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java index 4e39704e2..f923e1e01 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/FileFrameProvider.java @@ -3,6 +3,7 @@ package com.chameleonvision.common.vision.frame.provider; import com.chameleonvision.common.vision.frame.Frame; import com.chameleonvision.common.vision.frame.FrameProvider; import com.chameleonvision.common.vision.frame.FrameStaticProperties; +import com.chameleonvision.common.vision.opencv.CVMat; import java.nio.file.Files; import java.nio.file.Path; import java.nio.file.Paths; @@ -51,7 +52,7 @@ public class FileFrameProvider implements FrameProvider { if (image.cols() > 0 && image.rows() > 0) { m_properties = new FrameStaticProperties(image.width(), image.height(), m_fov); - m_frame = new Frame(image); + m_frame = new Frame(new CVMat(image), m_properties); } else { throw new RuntimeException("Image loading failed!"); } @@ -76,11 +77,6 @@ public class FileFrameProvider implements FrameProvider { return m_reloadImage; } - @Override - public FrameStaticProperties getFrameProperties() { - return m_properties; - } - @Override public Frame getFrame() { if (m_reloadImage) { diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java index b111bde37..ad63a283c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/NetworkFrameProvider.java @@ -2,7 +2,6 @@ package com.chameleonvision.common.vision.frame.provider; import com.chameleonvision.common.vision.frame.Frame; import com.chameleonvision.common.vision.frame.FrameProvider; -import com.chameleonvision.common.vision.frame.FrameStaticProperties; import org.apache.commons.lang3.NotImplementedException; public class NetworkFrameProvider implements FrameProvider { @@ -10,9 +9,4 @@ public class NetworkFrameProvider implements FrameProvider { public Frame getFrame() { throw new NotImplementedException(""); } - - @Override - public FrameStaticProperties getFrameProperties() { - throw new NotImplementedException(""); - } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java index f5bdbed62..e942429d4 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/frame/provider/USBFrameProvider.java @@ -2,7 +2,6 @@ package com.chameleonvision.common.vision.frame.provider; import com.chameleonvision.common.vision.frame.Frame; import com.chameleonvision.common.vision.frame.FrameProvider; -import com.chameleonvision.common.vision.frame.FrameStaticProperties; import org.apache.commons.lang3.NotImplementedException; public class USBFrameProvider implements FrameProvider { @@ -10,9 +9,4 @@ public class USBFrameProvider implements FrameProvider { public Frame getFrame() { throw new NotImplementedException(""); } - - @Override - public FrameStaticProperties getFrameProperties() { - throw new NotImplementedException(""); - } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVMat.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVMat.java new file mode 100644 index 000000000..116e3b97c --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVMat.java @@ -0,0 +1,44 @@ +package com.chameleonvision.common.vision.opencv; + +import com.chameleonvision.common.util.ReflectionUtils; +import java.util.HashSet; +import org.opencv.core.Mat; + +public class CVMat implements Releasable { + private static final HashSet allMats = new HashSet<>(); + + 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)) { + System.out.println("(CVMat) Added new Mat from: \n" + ReflectionUtils.getNthCaller(3)); + } + } + + @Override + public void release() { + allMats.remove(mat); + mat.release(); + } + + public Mat getMat() { + return mat; + } + + public static int getMatCount() { + return allMats.size(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVShape.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVShape.java new file mode 100644 index 000000000..1e01dc0a8 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/CVShape.java @@ -0,0 +1,60 @@ +package com.chameleonvision.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 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 Square: + break; + case Rectangle: + break; + } + return true; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java index e977114d7..740189530 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Contour.java @@ -1,14 +1,18 @@ package com.chameleonvision.common.vision.opencv; import com.chameleonvision.common.util.math.MathUtils; -import java.util.ArrayList; import java.util.Comparator; -import java.util.List; -import org.opencv.core.*; +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 { +public class Contour implements Releasable { public static final Comparator SortByMomentsX = Comparator.comparingDouble( @@ -17,14 +21,36 @@ public class Contour { 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); @@ -32,11 +58,16 @@ public class Contour { return area; } + public double getPerimeter() { + if (Double.isNaN(perimeter)) { + perimeter = Imgproc.arcLength(getMat2f(), true); + } + return perimeter; + } + public RotatedRect getMinAreaRect() { if (minAreaRect == null) { - MatOfPoint2f temp = new MatOfPoint2f(mat.toArray()); - minAreaRect = Imgproc.minAreaRect(temp); - temp.release(); + minAreaRect = Imgproc.minAreaRect(getMat2f()); } return minAreaRect; } @@ -60,20 +91,23 @@ public class Contour { } public boolean isEmpty() { - return mat.cols() != 0 && mat.rows() != 0; + return mat.empty(); } - public boolean isIntersecting(Contour secondContour, ContourIntersection intersection) { + public boolean isIntersecting( + Contour secondContour, ContourIntersectionDirection intersectionDirection) { boolean isIntersecting = false; - if (intersection == ContourIntersection.None) { + if (intersectionDirection == ContourIntersectionDirection.None) { isIntersecting = true; } else { try { MatOfPoint2f intersectMatA = new MatOfPoint2f(); MatOfPoint2f intersectMatB = new MatOfPoint2f(); - intersectMatA.fromArray(mat.toArray()); - intersectMatB.fromArray(secondContour.mat.toArray()); + + 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); @@ -86,7 +120,7 @@ public class Contour { double intersectionY = (mA * (intersectionX - x0A)) + y0A; double massX = (x0A + x0B) / 2; double massY = (y0A + y0B) / 2; - switch (intersection) { + switch (intersectionDirection) { case Up: if (intersectionY < massY) isIntersecting = true; break; @@ -112,47 +146,53 @@ public class Contour { // TODO: refactor to do "infinite" contours public static Contour groupContoursByIntersection( - Contour firstContour, Contour secondContour, ContourIntersection intersection) { - if (firstContour.isIntersecting(secondContour, intersection)) { + Contour firstContour, Contour secondContour, ContourIntersectionDirection intersection) { + if (areIntersecting(firstContour, secondContour, intersection)) { return combineContours(firstContour, secondContour); } else { return null; } } - // TODO: does this leak? + 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) { - List fullContourPoints = new ArrayList<>(); + var points = new MatOfPoint(); for (var contour : contours) { - fullContourPoints.addAll(contour.mat.toList()); + points.push_back(contour.mat); } - var points = new MatOfPoint(fullContourPoints.toArray(new Point[0])); var finalContour = new Contour(points); - if (!finalContour.isEmpty()) { - return finalContour; - } else return null; + boolean contourEmpty = finalContour.isEmpty(); + return contourEmpty ? null : finalContour; } - // TODO: move these? also docs plox - public enum ContourIntersection { - None, - Up, - Down, - Left, - Right + @Override + public void release() { + mat.release(); + mat2f.release(); + convexHull.release(); } - public enum ContourGrouping { - Single(1), - Dual(2); + public static MatOfPoint2f convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) { + int[] arrIndex = indexes.toArray(); + Point[] arrContour = contour.toArray(); + Point[] arrPoints = new Point[arrIndex.length]; - public final int count; - - ContourGrouping(int count) { - this.count = count; + for (int i = 0; i < arrIndex.length; i++) { + arrPoints[i] = arrContour[arrIndex[i]]; } + + var hull = new MatOfPoint2f(); + hull.fromArray(arrPoints); + return hull; } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourGroupingMode.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourGroupingMode.java new file mode 100644 index 000000000..dfed75685 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourGroupingMode.java @@ -0,0 +1,12 @@ +package com.chameleonvision.common.vision.opencv; + +public enum ContourGroupingMode { + Single(1), + Dual(2); + + public final int count; + + ContourGroupingMode(int count) { + this.count = count; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourIntersectionDirection.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourIntersectionDirection.java new file mode 100644 index 000000000..e14ff1ae2 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourIntersectionDirection.java @@ -0,0 +1,9 @@ +package com.chameleonvision.common.vision.opencv; + +public enum ContourIntersectionDirection { + None, + Up, + Down, + Left, + Right +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourShape.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourShape.java new file mode 100644 index 000000000..dc036c427 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourShape.java @@ -0,0 +1,15 @@ +package com.chameleonvision.common.vision.opencv; + +public enum ContourShape { + Custom(-1), + Circle(0), + Triangle(3), + Square(4), + Rectangle(4); + + public final int sides; + + ContourShape(int sides) { + this.sides = sides; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourSortMode.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourSortMode.java new file mode 100644 index 000000000..d530528e7 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/ContourSortMode.java @@ -0,0 +1,29 @@ +package com.chameleonvision.common.vision.opencv; + +import com.chameleonvision.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 m_comparator; + + ContourSortMode(Comparator comparator) { + m_comparator = comparator; + } + + public Comparator getComparator() { + return m_comparator; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/DualMat.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/DualMat.java new file mode 100644 index 000000000..9b65219dc --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/DualMat.java @@ -0,0 +1,8 @@ +package com.chameleonvision.common.vision.opencv; + +import org.opencv.core.Mat; + +public class DualMat { + public Mat first; + public Mat second; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Releasable.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Releasable.java new file mode 100644 index 000000000..1f5c08db5 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/opencv/Releasable.java @@ -0,0 +1,5 @@ +package com.chameleonvision.common.vision.opencv; + +public interface Releasable { + void release(); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/CVPipe.java similarity index 78% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/CVPipe.java index 72f391bc3..31d0d99c9 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/CVPipe.java @@ -1,4 +1,4 @@ -package com.chameleonvision.common.vision.pipeline; +package com.chameleonvision.common.vision.pipe; import java.util.function.Function; @@ -10,9 +10,9 @@ import java.util.function.Function; * @param Output type for the pipe * @param

Parameters type for the pipe */ -public abstract class CVPipe implements Function> { +public abstract class CVPipe implements Function> { - protected PipeResult result = new PipeResult<>(); + protected CVPipeResult result = new CVPipeResult<>(); protected P params; public void setParams(P params) { @@ -32,7 +32,7 @@ public abstract class CVPipe implements Function> { * @return Result of processing. */ @Override - public PipeResult apply(I in) { + public CVPipeResult apply(I in) { long pipeStartNanos = System.nanoTime(); result.result = process(in); result.nanosElapsed = System.nanoTime() - pipeStartNanos; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/CVPipeResult.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/CVPipeResult.java new file mode 100644 index 000000000..4fbd96e46 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/CVPipeResult.java @@ -0,0 +1,6 @@ +package com.chameleonvision.common.vision.pipe; + +public class CVPipeResult { + public O result; + public long nanosElapsed; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/ImageFlipMode.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/ImageFlipMode.java new file mode 100644 index 000000000..59345518c --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/ImageFlipMode.java @@ -0,0 +1,14 @@ +package com.chameleonvision.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; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/ImageRotationMode.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/ImageRotationMode.java new file mode 100644 index 000000000..517736d4e --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/ImageRotationMode.java @@ -0,0 +1,18 @@ +package com.chameleonvision.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; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/BlurPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/BlurPipe.java similarity index 90% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/BlurPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/BlurPipe.java index e48008860..b32c5c6b1 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/BlurPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/BlurPipe.java @@ -1,6 +1,6 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.pipe.CVPipe; import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Collect2dTargetsPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Collect2dTargetsPipe.java similarity index 59% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Collect2dTargetsPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Collect2dTargetsPipe.java index c47c30d29..10e833ec6 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Collect2dTargetsPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Collect2dTargetsPipe.java @@ -1,10 +1,9 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; import com.chameleonvision.common.util.numbers.DoubleCouple; -import com.chameleonvision.common.vision.camera.CaptureStaticProperties; -import com.chameleonvision.common.vision.pipeline.CVPipe; -import com.chameleonvision.common.vision.target.PotentialTarget; -import com.chameleonvision.common.vision.target.TrackedTarget; +import com.chameleonvision.common.vision.frame.FrameStaticProperties; +import com.chameleonvision.common.vision.pipe.CVPipe; +import com.chameleonvision.common.vision.target.*; import java.util.ArrayList; import java.util.List; import org.opencv.core.Point; @@ -26,15 +25,15 @@ public class Collect2dTargetsPipe var calculationParams = new TrackedTarget.TargetCalculationParameters( - params.getOrientation() == TrackedTarget.TargetOrientation.Landscape, + params.getOrientation() == TargetOrientation.Landscape, params.getOffsetPointRegion(), params.getUserOffsetPoint(), - params.getCaptureStaticProperties().centerPoint, + params.getFrameStaticProperties().centerPoint, new DoubleCouple(params.getCalibrationB(), params.getCalibrationM()), params.getOffsetMode(), - params.getCaptureStaticProperties().horizontalFocalLength, - params.getCaptureStaticProperties().verticalFocalLength, - params.getCaptureStaticProperties().imageArea); + params.getFrameStaticProperties().horizontalFocalLength, + params.getFrameStaticProperties().verticalFocalLength, + params.getFrameStaticProperties().imageArea); for (PotentialTarget target : in) { targets.add(new TrackedTarget(target, calculationParams)); @@ -44,21 +43,21 @@ public class Collect2dTargetsPipe } public static class Collect2dTargetsParams { - private CaptureStaticProperties m_captureStaticProperties; - private TrackedTarget.RobotOffsetPointMode m_offsetMode; + private FrameStaticProperties m_captureStaticProperties; + private RobotOffsetPointMode m_offsetMode; private double m_calibrationM, m_calibrationB; private Point m_userOffsetPoint; - private TrackedTarget.TargetOffsetPointRegion m_region; - private TrackedTarget.TargetOrientation m_orientation; + private TargetOffsetPointEdge m_region; + private TargetOrientation m_orientation; public Collect2dTargetsParams( - CaptureStaticProperties captureStaticProperties, - TrackedTarget.RobotOffsetPointMode offsetMode, + FrameStaticProperties captureStaticProperties, + RobotOffsetPointMode offsetMode, double calibrationM, double calibrationB, Point calibrationPoint, - TrackedTarget.TargetOffsetPointRegion region, - TrackedTarget.TargetOrientation orientation) { + TargetOffsetPointEdge region, + TargetOrientation orientation) { m_captureStaticProperties = captureStaticProperties; m_offsetMode = offsetMode; m_calibrationM = calibrationM; @@ -68,11 +67,11 @@ public class Collect2dTargetsPipe m_orientation = orientation; } - public CaptureStaticProperties getCaptureStaticProperties() { + public FrameStaticProperties getFrameStaticProperties() { return m_captureStaticProperties; } - public TrackedTarget.RobotOffsetPointMode getOffsetMode() { + public RobotOffsetPointMode getOffsetMode() { return m_offsetMode; } @@ -88,11 +87,11 @@ public class Collect2dTargetsPipe return m_userOffsetPoint; } - public TrackedTarget.TargetOffsetPointRegion getOffsetPointRegion() { + public TargetOffsetPointEdge getOffsetPointRegion() { return m_region; } - public TrackedTarget.TargetOrientation getOrientation() { + public TargetOrientation getOrientation() { return m_orientation; } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/CornerDetectionPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/CornerDetectionPipe.java new file mode 100644 index 000000000..875b20d2e --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/CornerDetectionPipe.java @@ -0,0 +1,213 @@ +package com.chameleonvision.common.vision.pipe.impl; + +import com.chameleonvision.common.vision.pipe.CVPipe; +import com.chameleonvision.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, + List, + CornerDetectionPipe.CornerDetectionPipeParameters> { + + Comparator leftRightComparator = Comparator.comparingDouble(point -> point.x); + Comparator verticalComparator = Comparator.comparingDouble(point -> point.y); + MatOfPoint2f polyOutput = new MatOfPoint2f(); + + @Override + protected List process(List 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 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 detectExtremeCornersByApproxPolyDp(TrackedTarget target, boolean convexHull) { + var centroid = target.getMinAreaRect().center; + Comparator 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(); + var rightList = new ArrayList(); + 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 + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw2dContoursPipe.java similarity index 79% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dContoursPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw2dContoursPipe.java index 5611d1c15..95190d1a1 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw2dContoursPipe.java @@ -1,7 +1,7 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; import com.chameleonvision.common.util.ColorHelper; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.pipe.CVPipe; import com.chameleonvision.common.vision.target.TrackedTarget; import java.awt.Color; import java.util.ArrayList; @@ -21,8 +21,9 @@ public class Draw2dContoursPipe @Override protected Mat process(Pair> in) { - if (params.showCentroid || params.showMaximumBox || params.showRotatedBox) { - for (int i = 0; i < in.getRight().size(); i++) { + if (!in.getRight().isEmpty() + && (params.showCentroid || params.showMaximumBox || params.showRotatedBox)) { + for (int i = 0; i < (params.showMultiple ? in.getRight().size() : 1); i++) { Point[] vertices = new Point[4]; MatOfPoint contour = new MatOfPoint(); @@ -77,13 +78,18 @@ public class Draw2dContoursPipe } public static class Draw2dContoursParams { - public boolean showCentroid = false; - public boolean showMultiple = false; - public int boxOutlineSize = 0; - public boolean showRotatedBox = false; - public boolean showMaximumBox = false; + public boolean showCentroid = true; + public boolean showMultiple = true; + public int boxOutlineSize = 1; + public boolean showRotatedBox = true; + public boolean showMaximumBox = true; public Color centroidColor = Color.GREEN; public Color rotatedBoxColor = Color.BLUE; public Color maximumBoxColor = Color.RED; + + // TODO: set other params from UI/settings file? + public Draw2dContoursParams(boolean showMultipleTargets) { + this.showMultiple = showMultipleTargets; + } } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw2dCrosshairPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw2dCrosshairPipe.java new file mode 100644 index 000000000..05ce9cadf --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw2dCrosshairPipe.java @@ -0,0 +1,62 @@ +package com.chameleonvision.common.vision.pipe.impl; + +import com.chameleonvision.common.util.ColorHelper; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.vision.pipe.CVPipe; +import com.chameleonvision.common.vision.target.RobotOffsetPointMode; +import com.chameleonvision.common.vision.target.TrackedTarget; +import java.awt.Color; +import java.util.List; +import org.apache.commons.lang3.tuple.Pair; +import org.opencv.core.Mat; +import org.opencv.core.Point; +import org.opencv.imgproc.Imgproc; + +public class Draw2dCrosshairPipe + extends CVPipe>, Mat, Draw2dCrosshairPipe.Draw2dCrosshairParams> { + + @Override + protected Mat process(Pair> in) { + Mat image = in.getLeft(); + + 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 + 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; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java new file mode 100644 index 000000000..1b5014121 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/Draw3dTargetsPipe.java @@ -0,0 +1,122 @@ +package com.chameleonvision.common.vision.pipe.impl; + +import com.chameleonvision.common.calibration.CameraCalibrationCoefficients; +import com.chameleonvision.common.util.ColorHelper; +import com.chameleonvision.common.vision.pipe.CVPipe; +import com.chameleonvision.common.vision.target.TargetModel; +import com.chameleonvision.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>, Mat, Draw3dTargetsPipe.Draw3dContoursParams> { + + private static MatOfPoint tempMat = new MatOfPoint(); + + @Override + protected Mat process(Pair> 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 = TargetModel.get2020Target(); + private final CameraCalibrationCoefficients cameraCalibrationCoefficients; + + public Draw3dContoursParams(CameraCalibrationCoefficients cameraCalibrationCoefficients) { + this.cameraCalibrationCoefficients = cameraCalibrationCoefficients; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/DrawCornerDetectionPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/DrawCornerDetectionPipe.java new file mode 100644 index 000000000..003f216e3 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/DrawCornerDetectionPipe.java @@ -0,0 +1,32 @@ +package com.chameleonvision.common.vision.pipe.impl; + +import com.chameleonvision.common.vision.pipe.CVPipe; +import com.chameleonvision.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>, Mat, DrawCornerDetectionPipe.DrawCornerParams> { + + @Override + protected Mat process(Pair> 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; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ErodeDilatePipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/ErodeDilatePipe.java similarity index 90% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ErodeDilatePipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/ErodeDilatePipe.java index f507703d4..01c75cc0c 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ErodeDilatePipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/ErodeDilatePipe.java @@ -1,6 +1,6 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.pipe.CVPipe; import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FilterContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FilterContoursPipe.java similarity index 88% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FilterContoursPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FilterContoursPipe.java index 076b78b23..27a533aec 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FilterContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FilterContoursPipe.java @@ -1,10 +1,10 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; import com.chameleonvision.common.util.math.MathUtils; import com.chameleonvision.common.util.numbers.DoubleCouple; -import com.chameleonvision.common.vision.camera.CaptureStaticProperties; +import com.chameleonvision.common.vision.frame.FrameStaticProperties; import com.chameleonvision.common.vision.opencv.Contour; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.pipe.CVPipe; import java.util.ArrayList; import java.util.List; import org.opencv.core.Rect; @@ -56,13 +56,13 @@ public class FilterContoursPipe private DoubleCouple m_area; private DoubleCouple m_ratio; private DoubleCouple m_extent; - private CaptureStaticProperties m_camProperties; + private FrameStaticProperties m_camProperties; public FilterContoursParams( DoubleCouple area, DoubleCouple ratio, DoubleCouple extent, - CaptureStaticProperties camProperties) { + FrameStaticProperties camProperties) { this.m_area = area; this.m_ratio = ratio; this.m_extent = extent; @@ -81,7 +81,7 @@ public class FilterContoursPipe return m_extent; } - public CaptureStaticProperties getCamProperties() { + public FrameStaticProperties getCamProperties() { return m_camProperties; } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FindContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindContoursPipe.java similarity index 88% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FindContoursPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindContoursPipe.java index 789ab759b..37b960d50 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/FindContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindContoursPipe.java @@ -1,7 +1,7 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; import com.chameleonvision.common.vision.opencv.Contour; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.pipe.CVPipe; import java.util.ArrayList; import java.util.List; import java.util.stream.Collectors; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindShapesPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindShapesPipe.java new file mode 100644 index 000000000..f10218418 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/FindShapesPipe.java @@ -0,0 +1,41 @@ +package com.chameleonvision.common.vision.pipe.impl; + +import com.chameleonvision.common.vision.opencv.CVShape; +import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.opencv.ContourShape; +import com.chameleonvision.common.vision.pipe.CVPipe; +import java.util.List; +import org.opencv.core.MatOfPoint2f; +import org.opencv.imgproc.Imgproc; + +public class FindShapesPipe + extends CVPipe, List, FindShapesPipe.FindShapesParams> { + + MatOfPoint2f approxCurve = new MatOfPoint2f(); + + @Override + protected List process(List 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; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/GroupContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/GroupContoursPipe.java similarity index 56% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/GroupContoursPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/GroupContoursPipe.java index 6a2bf9341..0962d9f5d 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/GroupContoursPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/GroupContoursPipe.java @@ -1,7 +1,9 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; import com.chameleonvision.common.vision.opencv.Contour; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.opencv.ContourGroupingMode; +import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection; +import com.chameleonvision.common.vision.pipe.CVPipe; import com.chameleonvision.common.vision.target.PotentialTarget; import java.util.ArrayList; import java.util.Collections; @@ -14,9 +16,13 @@ public class GroupContoursPipe @Override protected List process(List input) { + for (var target : m_targets) { + target.release(); + } + m_targets.clear(); - if (params.getGroup() == Contour.ContourGrouping.Single) { + if (params.getGroup() == ContourGroupingMode.Single) { for (var contour : input) { m_targets.add(new PotentialTarget(contour)); } @@ -36,18 +42,23 @@ public class GroupContoursPipe // make a list of the desired count of contours to group List groupingSet; try { - groupingSet = input.subList(i, i + groupingCount - 1); + 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()); + // 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)); + if (groupedContour != null) { + m_targets.add(new PotentialTarget(groupedContour, groupingSet)); + i += (groupingCount - 1); + } + } catch (Exception ex) { + ex.printStackTrace(); } } } @@ -56,20 +67,20 @@ public class GroupContoursPipe } public static class GroupContoursParams { - private Contour.ContourGrouping m_group; - private Contour.ContourIntersection m_intersection; + private ContourGroupingMode m_group; + private ContourIntersectionDirection m_intersection; public GroupContoursParams( - Contour.ContourGrouping group, Contour.ContourIntersection intersection) { + ContourGroupingMode group, ContourIntersectionDirection intersectionDirection) { m_group = group; - m_intersection = intersection; + m_intersection = intersectionDirection; } - public Contour.ContourGrouping getGroup() { + public ContourGroupingMode getGroup() { return m_group; } - public Contour.ContourIntersection getIntersection() { + public ContourIntersectionDirection getIntersection() { return m_intersection; } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/HSVPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/HSVPipe.java similarity index 70% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/HSVPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/HSVPipe.java index c4c3b582b..aaea7cfbe 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/HSVPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/HSVPipe.java @@ -1,6 +1,7 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.util.numbers.IntegerCouple; +import com.chameleonvision.common.vision.pipe.CVPipe; import org.opencv.core.Core; import org.opencv.core.CvException; import org.opencv.core.Mat; @@ -28,6 +29,11 @@ public class HSVPipe extends CVPipe { 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; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/OutputMatPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/OutputMatPipe.java similarity index 63% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/OutputMatPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/OutputMatPipe.java index d167b609e..c9bdda714 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/OutputMatPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/OutputMatPipe.java @@ -1,26 +1,29 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; -import com.chameleonvision.common.vision.pipeline.CVPipe; -import org.apache.commons.lang3.tuple.Pair; +import com.chameleonvision.common.vision.opencv.DualMat; +import com.chameleonvision.common.vision.pipe.CVPipe; import org.opencv.core.CvException; import org.opencv.core.Mat; import org.opencv.imgproc.Imgproc; -public class OutputMatPipe extends CVPipe, Mat, OutputMatPipe.OutputMatParams> { +public class OutputMatPipe extends CVPipe { private Mat m_outputMat = new Mat(); @Override - protected Mat process(Pair in) { + protected Mat process(DualMat in) { + Mat rawCam = in.first; + Mat hsv = in.second; if (params.showThreshold()) { + // convert input mat try { - in.getRight().copyTo(m_outputMat); + 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 { - in.getLeft().copyTo(m_outputMat); + m_outputMat = rawCam; } return m_outputMat; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ResizeImagePipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/ResizeImagePipe.java similarity index 61% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ResizeImagePipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/ResizeImagePipe.java index c2aef7884..777bf2899 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/ResizeImagePipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/ResizeImagePipe.java @@ -1,6 +1,7 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.frame.FrameDivisor; +import com.chameleonvision.common.vision.pipe.CVPipe; import org.opencv.core.Mat; import org.opencv.core.Size; import org.opencv.imgproc.Imgproc; @@ -24,6 +25,14 @@ public class ResizeImagePipe extends CVPipe, List, SolvePNPPipe.SolvePNPPipeParams> { + + private MatOfPoint2f imagePoints = new MatOfPoint2f(); + + @Override + protected List process(List 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; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/SortContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/SortContoursPipe.java new file mode 100644 index 000000000..51dfba9d8 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/SortContoursPipe.java @@ -0,0 +1,64 @@ +package com.chameleonvision.common.vision.pipe.impl; + +import com.chameleonvision.common.vision.frame.FrameStaticProperties; +import com.chameleonvision.common.vision.opencv.ContourSortMode; +import com.chameleonvision.common.vision.pipe.CVPipe; +import com.chameleonvision.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, List, SortContoursPipe.SortContoursParams> { + + private List m_sortedContours = new ArrayList<>(); + + @Override + protected List process(List in) { + m_sortedContours.clear(); + if (in.size() > 0) { + m_sortedContours.addAll(in); + if (params.getSortMode() != 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; + } + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SpeckleRejectPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/SpeckleRejectPipe.java similarity index 91% rename from chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SpeckleRejectPipe.java rename to chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/SpeckleRejectPipe.java index 8c101c3fb..2d918f889 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SpeckleRejectPipe.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipe/impl/SpeckleRejectPipe.java @@ -1,7 +1,7 @@ -package com.chameleonvision.common.vision.pipeline.pipe; +package com.chameleonvision.common.vision.pipe.impl; import com.chameleonvision.common.vision.opencv.Contour; -import com.chameleonvision.common.vision.pipeline.CVPipe; +import com.chameleonvision.common.vision.pipe.CVPipe; import java.util.ArrayList; import java.util.List; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipeline.java new file mode 100644 index 000000000..cd846a802 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipeline.java @@ -0,0 +1,24 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.util.math.MathUtils; +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameStaticProperties; + +public abstract class CVPipeline { + + protected abstract void setPipeParams(S settings, FrameStaticProperties frameStaticProperties); + + protected abstract R process(Frame frame, S settings); + + public R run(Frame frame, S settings) { + long pipelineStartNanos = System.nanoTime(); + + setPipeParams(settings, frame.frameStaticProperties); + + R result = process(frame, settings); + + result.setLatencyMillis(MathUtils.nanosToMillis(System.nanoTime() - pipelineStartNanos)); + + return result; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipelineResult.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipelineResult.java new file mode 100644 index 000000000..22145847b --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipelineResult.java @@ -0,0 +1,40 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.opencv.Releasable; +import com.chameleonvision.common.vision.target.TrackedTarget; +import java.util.List; + +public class CVPipelineResult implements Releasable { + private double latencyMillis; + public final double processingMillis; + public final List targets; + public final Frame outputFrame; + + public CVPipelineResult(double processingMillis, List targets, Frame outputFrame) { + this.processingMillis = processingMillis; + this.targets = targets; + + // TODO: is this the best way to go about this? + 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; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipelineSettings.java new file mode 100644 index 000000000..eb57ac9e1 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/CVPipelineSettings.java @@ -0,0 +1,19 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.vision.frame.FrameDivisor; +import com.chameleonvision.common.vision.pipe.ImageFlipMode; +import com.chameleonvision.common.vision.pipe.ImageRotationMode; + +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; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/Calibration3dPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/Calibration3dPipeline.java new file mode 100644 index 000000000..c4fcfef8f --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/Calibration3dPipeline.java @@ -0,0 +1,3 @@ +package com.chameleonvision.common.vision.pipeline; + +public class Calibration3dPipeline {} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java new file mode 100644 index 000000000..22ecc0055 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipeline.java @@ -0,0 +1,16 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameStaticProperties; + +public class ColoredShapePipeline + extends CVPipeline { + @Override + protected void setPipeParams( + ColoredShapePipelineSettings settings, FrameStaticProperties frameStaticProperties) {} + + @Override + protected CVPipelineResult process(Frame frame, ColoredShapePipelineSettings settings) { + return null; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java new file mode 100644 index 000000000..d61fbae92 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ColoredShapePipelineSettings.java @@ -0,0 +1,7 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.vision.opencv.ContourShape; + +public class ColoredShapePipelineSettings extends CVPipelineSettings { + ContourShape desiredShape; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipeline.java new file mode 100644 index 000000000..10930bd1e --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipeline.java @@ -0,0 +1,57 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.util.math.MathUtils; +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameStaticProperties; +import com.chameleonvision.common.vision.opencv.CVMat; +import com.chameleonvision.common.vision.pipe.impl.Draw2dCrosshairPipe; +import com.chameleonvision.common.vision.pipe.impl.ResizeImagePipe; +import com.chameleonvision.common.vision.pipe.impl.RotateImagePipe; +import java.util.List; +import org.apache.commons.lang3.tuple.Pair; + +public class DriverModePipeline + extends CVPipeline { + + private final RotateImagePipe rotateImagePipe = new RotateImagePipe(); + + private final ResizeImagePipe resizeImagePipe = new ResizeImagePipe(); + + private final Draw2dCrosshairPipe draw2dCrosshairPipe = new Draw2dCrosshairPipe(); + + @Override + protected void setPipeParams( + DriverModePipelineSettings settings, FrameStaticProperties frameStaticProperties) { + 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)); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipelineResult.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipelineResult.java new file mode 100644 index 000000000..e8846c00f --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipelineResult.java @@ -0,0 +1,10 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.vision.frame.Frame; +import java.util.List; + +public class DriverModePipelineResult extends CVPipelineResult { + public DriverModePipelineResult(double latencyMillis, Frame outputFrame) { + super(latencyMillis, List.of(), outputFrame); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipelineSettings.java new file mode 100644 index 000000000..458507485 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DriverModePipelineSettings.java @@ -0,0 +1,9 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.vision.target.RobotOffsetPointMode; + +public class DriverModePipelineSettings extends CVPipelineSettings { + public RobotOffsetPointMode offsetPointMode = RobotOffsetPointMode.None; + public DoubleCouple offsetPoint = new DoubleCouple(); +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DummyPipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DummyPipeline.java deleted file mode 100644 index d5302a0b8..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/DummyPipeline.java +++ /dev/null @@ -1,31 +0,0 @@ -package com.chameleonvision.common.vision.pipeline; - -import com.chameleonvision.common.vision.pipeline.pipe.ResizeImagePipe; -import com.chameleonvision.common.vision.pipeline.pipe.RotateImagePipe; -import edu.wpi.cscore.CameraServerCvJNI; -import java.io.IOException; -import org.opencv.core.CvType; -import org.opencv.core.Mat; - -/** This class exists for the sole purpose of showing how pipes would interact in a pipeline */ -public class DummyPipeline { - private static ResizeImagePipe resizePipe = new ResizeImagePipe(); - private static RotateImagePipe rotatePipe = new RotateImagePipe(); - - public static void main(String[] args) { - try { - CameraServerCvJNI.forceLoad(); - } catch (UnsatisfiedLinkError | IOException e) { - throw new RuntimeException("Failed to load JNI Libraries!"); - } - - // obviously not a useful test, purely for example. - Mat fakeCameraMat = new Mat(640, 480, CvType.CV_8UC3); - - PipeResult resizeResult = resizePipe.apply(fakeCameraMat); - PipeResult rotateResult = rotatePipe.apply(resizeResult.result); - - long fullTime = resizeResult.nanosElapsed + rotateResult.nanosElapsed; - System.out.println(fullTime / 1.0e+6 + "ms elapsed"); - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/PipeResult.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/PipeResult.java deleted file mode 100644 index 451b172eb..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/PipeResult.java +++ /dev/null @@ -1,6 +0,0 @@ -package com.chameleonvision.common.vision.pipeline; - -public class PipeResult { - O result; - long nanosElapsed; -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/PipelineType.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/PipelineType.java new file mode 100644 index 000000000..f49154fb7 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/PipelineType.java @@ -0,0 +1,17 @@ +package com.chameleonvision.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; + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipeline.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipeline.java new file mode 100644 index 000000000..6864ead35 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipeline.java @@ -0,0 +1,228 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.util.math.MathUtils; +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.FrameStaticProperties; +import com.chameleonvision.common.vision.opencv.CVMat; +import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.opencv.DualMat; +import com.chameleonvision.common.vision.pipe.CVPipeResult; +import com.chameleonvision.common.vision.pipe.impl.Collect2dTargetsPipe; +import com.chameleonvision.common.vision.pipe.impl.CornerDetectionPipe; +import com.chameleonvision.common.vision.pipe.impl.Draw2dContoursPipe; +import com.chameleonvision.common.vision.pipe.impl.Draw2dCrosshairPipe; +import com.chameleonvision.common.vision.pipe.impl.Draw3dTargetsPipe; +import com.chameleonvision.common.vision.pipe.impl.ErodeDilatePipe; +import com.chameleonvision.common.vision.pipe.impl.FilterContoursPipe; +import com.chameleonvision.common.vision.pipe.impl.FindContoursPipe; +import com.chameleonvision.common.vision.pipe.impl.GroupContoursPipe; +import com.chameleonvision.common.vision.pipe.impl.HSVPipe; +import com.chameleonvision.common.vision.pipe.impl.OutputMatPipe; +import com.chameleonvision.common.vision.pipe.impl.RotateImagePipe; +import com.chameleonvision.common.vision.pipe.impl.SolvePNPPipe; +import com.chameleonvision.common.vision.pipe.impl.SortContoursPipe; +import com.chameleonvision.common.vision.pipe.impl.SpeckleRejectPipe; +import com.chameleonvision.common.vision.target.PotentialTarget; +import com.chameleonvision.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 { + + 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(); + + @Override + protected void setPipeParams( + ReflectivePipelineSettings settings, FrameStaticProperties frameStaticProperties) { + 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); + + // TODO: necessary? offer different contour methods? + 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); + 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(settings, frame.frameStaticProperties); + + long sumPipeNanosElapsed = 0L; + + frame.image.getMat().copyTo(rawInputMat); + + CVPipeResult rotateImageResult = rotateImagePipe.apply(frame.image.getMat()); + sumPipeNanosElapsed += rotateImageResult.nanosElapsed; + + CVPipeResult erodeDilateResult = erodeDilatePipe.apply(rotateImageResult.result); + sumPipeNanosElapsed += erodeDilateResult.nanosElapsed; + + CVPipeResult hsvPipeResult = hsvPipe.apply(erodeDilateResult.result); + sumPipeNanosElapsed += hsvPipeResult.nanosElapsed; + + // mat leak fix attempt + outputMats.first = rawInputMat; + outputMats.second = hsvPipeResult.result; + + CVPipeResult outputMatResult = outputMatPipe.apply(outputMats); + sumPipeNanosElapsed += outputMatResult.nanosElapsed; + + CVPipeResult> findContoursResult = findContoursPipe.apply(hsvPipeResult.result); + sumPipeNanosElapsed += findContoursResult.nanosElapsed; + + CVPipeResult> speckleRejectResult = + speckleRejectPipe.apply(findContoursResult.result); + sumPipeNanosElapsed += speckleRejectResult.nanosElapsed; + + CVPipeResult> groupContoursResult = + groupContoursPipe.apply(speckleRejectResult.result); + sumPipeNanosElapsed += groupContoursResult.nanosElapsed; + + CVPipeResult> sortContoursResult = + sortContoursPipe.apply(groupContoursResult.result); + sumPipeNanosElapsed += sortContoursResult.nanosElapsed; + + CVPipeResult> collect2dTargetsResult = + collect2dTargetsPipe.apply(sortContoursResult.result); + sumPipeNanosElapsed += collect2dTargetsResult.nanosElapsed; + + CVPipeResult> 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 result; + + CVPipeResult draw2dCrosshairResult = + draw2dCrosshairPipe.apply(Pair.of(outputMatResult.result, targetList.result)); + sumPipeNanosElapsed += draw2dCrosshairResult.nanosElapsed; + + CVPipeResult 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)); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineSettings.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineSettings.java new file mode 100644 index 000000000..a46428072 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineSettings.java @@ -0,0 +1,73 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.calibration.CameraCalibrationCoefficients; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.common.util.numbers.IntegerCouple; +import com.chameleonvision.common.vision.opencv.ContourGroupingMode; +import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection; +import com.chameleonvision.common.vision.opencv.ContourSortMode; +import com.chameleonvision.common.vision.pipe.impl.CornerDetectionPipe; +import com.chameleonvision.common.vision.target.RobotOffsetPointMode; +import com.chameleonvision.common.vision.target.TargetModel; +import com.chameleonvision.common.vision.target.TargetOffsetPointEdge; +import com.chameleonvision.common.vision.target.TargetOrientation; +import edu.wpi.first.wpilibj.geometry.Rotation2d; + +public class ReflectivePipelineSettings extends CVPipelineSettings { + 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; + + // 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; + + // 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; + + // 3d settings + public boolean solvePNPEnabled = false; + public CameraCalibrationCoefficients cameraCalibration; + public TargetModel targetModel; + public Rotation2d cameraPitch; + + // 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; +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dCrosshairPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dCrosshairPipe.java deleted file mode 100644 index 19a3bf491..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/Draw2dCrosshairPipe.java +++ /dev/null @@ -1,56 +0,0 @@ -package com.chameleonvision.common.vision.pipeline.pipe; - -import com.chameleonvision.common.util.ColorHelper; -import com.chameleonvision.common.vision.pipeline.CVPipe; -import com.chameleonvision.common.vision.target.TrackedTarget; -import java.awt.Color; -import java.util.List; -import org.apache.commons.lang3.tuple.Pair; -import org.opencv.core.Mat; -import org.opencv.core.Point; -import org.opencv.imgproc.Imgproc; - -public class Draw2dCrosshairPipe - extends CVPipe>, Mat, Draw2dCrosshairPipe.Draw2dCrosshairParams> { - - @Override - protected Mat process(Pair> in) { - Mat image = in.getLeft(); - - double x, y; - double scale = image.cols() / 32.0; - - if (params.showCrosshair) { - x = image.cols() / 2.0; - y = image.rows() / 2.0; - - switch (params.calibrationMode) { - case Single: - if (params.calibrationPoint.equals(new Point())) { - params.calibrationPoint.set(new double[] {x, y}); - } - x = (int) params.calibrationPoint.x; - y = (int) params.calibrationPoint.y; - break; - case Dual: - // TODO - break; - } - Point xMax = new Point(x + scale, y); - Point xMin = new Point(x - scale, y); - Point yMax = new Point(x, y + scale); - Point yMin = new Point(x, y - scale); - - Imgproc.line(in.getLeft(), xMax, xMin, ColorHelper.colorToScalar(params.crosshairColor)); - Imgproc.line(in.getLeft(), yMax, yMin, ColorHelper.colorToScalar(params.crosshairColor)); - } - return in.getLeft(); - } - - public static class Draw2dCrosshairParams { - public TrackedTarget.RobotOffsetPointMode calibrationMode; - public Point calibrationPoint; - public boolean showCrosshair = true; - public Color crosshairColor = Color.GREEN; - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SortContoursPipe.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SortContoursPipe.java deleted file mode 100644 index d9f648ba7..000000000 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/pipeline/pipe/SortContoursPipe.java +++ /dev/null @@ -1,84 +0,0 @@ -package com.chameleonvision.common.vision.pipeline.pipe; - -import com.chameleonvision.common.vision.camera.CaptureStaticProperties; -import com.chameleonvision.common.vision.pipeline.CVPipe; -import com.chameleonvision.common.vision.target.TrackedTarget; -import java.util.ArrayList; -import java.util.Comparator; -import java.util.List; -import org.apache.commons.math3.util.FastMath; - -public class SortContoursPipe - extends CVPipe, List, SortContoursPipe.SortContoursParams> { - - private List m_sortedContours = new ArrayList<>(); - - @Override - protected List process(List in) { - m_sortedContours.clear(); - if (in.size() > 0) { - m_sortedContours.addAll(in); - if (params.getSortMode() != SortMode.Centermost) { - m_sortedContours.sort(params.getSortMode().getComparator()); - } else { - m_sortedContours.sort(Comparator.comparingDouble(this::calcSquareCenterDistance)); - } - } - - return new ArrayList<>( - m_sortedContours.subList(0, Math.min(in.size(), params.getMaxTargets() - 1))); - } - - private double calcSquareCenterDistance(TrackedTarget rect) { - return FastMath.sqrt( - FastMath.pow(params.getCamProperties().centerX - rect.getMinAreaRect().center.x, 2) - + FastMath.pow(params.getCamProperties().centerY - rect.getMinAreaRect().center.y, 2)); - } - - public enum SortMode { - Largest( - (rect1, rect2) -> - Double.compare(rect2.getMinAreaRect().size.area(), rect1.getMinAreaRect().size.area())), - Smallest(Largest.getComparator().reversed()), - Highest(Comparator.comparingDouble(rect -> rect.getMinAreaRect().center.y)), - Lowest(Highest.getComparator().reversed()), - Leftmost(Comparator.comparingDouble(target -> target.getMinAreaRect().center.x)), - Rightmost(Leftmost.getComparator().reversed()), - Centermost(null); - - private Comparator m_comparator; - - SortMode(Comparator comparator) { - m_comparator = comparator; - } - - public Comparator getComparator() { - return m_comparator; - } - } - - public static class SortContoursParams { - private SortMode m_sortMode; - private CaptureStaticProperties m_camProperties; - private int m_maxTargets; - - public SortContoursParams( - SortMode sortMode, CaptureStaticProperties camProperties, int maxTargets) { - m_sortMode = sortMode; - m_camProperties = camProperties; - m_maxTargets = maxTargets; - } - - public SortMode getSortMode() { - return m_sortMode; - } - - public CaptureStaticProperties getCamProperties() { - return m_camProperties; - } - - public int getMaxTargets() { - return m_maxTargets; - } - } -} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java index 0b7e4468b..8df7751e1 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/PotentialTarget.java @@ -1,13 +1,15 @@ package com.chameleonvision.common.vision.target; import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.opencv.Releasable; import java.util.ArrayList; import java.util.List; +import org.opencv.core.RotatedRect; -public class PotentialTarget { +public class PotentialTarget implements Releasable { - final Contour m_mainContour; - final List m_subContours; + public final Contour m_mainContour; + public final List m_subContours; public PotentialTarget(Contour inputContour) { m_mainContour = inputContour; @@ -16,6 +18,23 @@ public class PotentialTarget { public PotentialTarget(Contour inputContour, List subContours) { m_mainContour = inputContour; - m_subContours = subContours; + 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(); } } diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/RobotOffsetPointMode.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/RobotOffsetPointMode.java new file mode 100644 index 000000000..c582874a6 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/RobotOffsetPointMode.java @@ -0,0 +1,7 @@ +package com.chameleonvision.common.vision.target; + +public enum RobotOffsetPointMode { + None, + Single, + Dual +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java new file mode 100644 index 000000000..cf073c37a --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetModel.java @@ -0,0 +1,75 @@ +package com.chameleonvision.common.vision.target; + +import com.chameleonvision.common.vision.opencv.Releasable; +import java.util.ArrayList; +import java.util.List; +import org.opencv.core.MatOfPoint3f; +import org.opencv.core.Point3; + +public class TargetModel implements Releasable { + + private final MatOfPoint3f realWorldTargetCoordinates; + + private final MatOfPoint3f visualizationBoxBottom = new MatOfPoint3f(); + private final MatOfPoint3f visualizationBoxTop = new MatOfPoint3f(); + + public TargetModel(MatOfPoint3f realWorldTargetCoordinates, double boxHeight) { + this.realWorldTargetCoordinates = realWorldTargetCoordinates; + + var bottomList = realWorldTargetCoordinates.toList(); + var topList = new ArrayList(); + for (var c : bottomList) { + topList.add(new Point3(c.x, c.y, c.z + boxHeight)); + } + + this.visualizationBoxBottom.fromList(bottomList); + this.visualizationBoxTop.fromList(topList); + } + + public TargetModel(List points, double boxHeight) { + this(listToMat(points), boxHeight); + } + + private static MatOfPoint3f listToMat(List 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 + } + + @Override + public void release() { + realWorldTargetCoordinates.release(); + visualizationBoxBottom.release(); + visualizationBoxTop.release(); + } +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetOffsetPointEdge.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetOffsetPointEdge.java new file mode 100644 index 000000000..6672b7f97 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetOffsetPointEdge.java @@ -0,0 +1,9 @@ +package com.chameleonvision.common.vision.target; + +public enum TargetOffsetPointEdge { + Center, + Top, + Bottom, + Left, + Right +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetOrientation.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetOrientation.java new file mode 100644 index 000000000..cd7a9b085 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetOrientation.java @@ -0,0 +1,6 @@ +package com.chameleonvision.common.vision.target; + +public enum TargetOrientation { + Portrait, + Landscape +} diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java index c6df22343..c4bfdfce7 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TrackedTarget.java @@ -2,16 +2,24 @@ package com.chameleonvision.common.vision.target; import com.chameleonvision.common.util.numbers.DoubleCouple; import com.chameleonvision.common.vision.opencv.Contour; +import com.chameleonvision.common.vision.opencv.Releasable; +import edu.wpi.first.wpilibj.geometry.Pose2d; import java.util.List; import org.apache.commons.math3.util.FastMath; +import org.opencv.core.Mat; +import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; import org.opencv.core.RotatedRect; // TODO: banks fix -public class TrackedTarget { - final Contour m_mainContour; +public class TrackedTarget implements Releasable { + public final Contour m_mainContour; List m_subContours; // can be empty + private MatOfPoint2f m_approximateBoundingPolygon; + + private List m_targetCorners; + private Point m_targetOffsetPoint; private Point m_robotOffsetPoint; @@ -19,12 +27,26 @@ public class TrackedTarget { 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; } @@ -49,8 +71,11 @@ public class TrackedTarget { return m_mainContour.getMinAreaRect(); } - private void calculateTargetOffsetPoint( - boolean isLandscape, TargetOffsetPointRegion offsetRegion) { + public MatOfPoint2f getApproximateBoundingPolygon() { + return m_approximateBoundingPolygon; + } + + private void calculateTargetOffsetPoint(boolean isLandscape, TargetOffsetPointEdge offsetRegion) { Point[] vertices = new Point[4]; var minAreaRect = getMinAreaRect(); @@ -145,7 +170,7 @@ public class TrackedTarget { public void calculateValues(TargetCalculationParameters params) { // this MUST happen in this exact order! - calculateTargetOffsetPoint(params.isLandscape, params.targetOffsetPointRegion); + calculateTargetOffsetPoint(params.isLandscape, params.targetOffsetPointEdge); calculateRobotOffsetPoint( m_targetOffsetPoint, params.cameraCenterPoint, @@ -158,10 +183,59 @@ public class TrackedTarget { calculateArea(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 targetCorners) { + this.m_targetCorners = targetCorners; + } + + public List 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 TargetOffsetPointRegion targetOffsetPointRegion; + final TargetOffsetPointEdge targetOffsetPointEdge; // RobotOffset calculation values final Point userOffsetPoint; @@ -180,7 +254,7 @@ public class TrackedTarget { public TargetCalculationParameters( boolean isLandscape, - TargetOffsetPointRegion targetOffsetPointRegion, + TargetOffsetPointEdge targetOffsetPointEdge, Point userOffsetPoint, Point cameraCenterPoint, DoubleCouple offsetEquationValues, @@ -189,7 +263,7 @@ public class TrackedTarget { double verticalFocalLength, double imageArea) { this.isLandscape = isLandscape; - this.targetOffsetPointRegion = targetOffsetPointRegion; + this.targetOffsetPointEdge = targetOffsetPointEdge; this.userOffsetPoint = userOffsetPoint; this.cameraCenterPoint = cameraCenterPoint; this.offsetEquationValues = offsetEquationValues; @@ -199,24 +273,4 @@ public class TrackedTarget { this.imageArea = imageArea; } } - - // TODO: move these? also docs plox - public enum TargetOrientation { - Portrait, - Landscape - } - - public enum TargetOffsetPointRegion { - Center, - Top, - Bottom, - Left, - Right - } - - public enum RobotOffsetPointMode { - None, - Single, - Dual - } } diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java index 24931c273..e1a05c0e1 100644 --- a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java +++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,22 +7,15 @@ package edu.wpi.first.wpilibj.geometry; -import com.fasterxml.jackson.core.JsonGenerator; -import com.fasterxml.jackson.core.JsonParser; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationContext; -import com.fasterxml.jackson.databind.JsonNode; -import com.fasterxml.jackson.databind.SerializerProvider; -import com.fasterxml.jackson.databind.annotation.JsonDeserialize; -import com.fasterxml.jackson.databind.annotation.JsonSerialize; -import com.fasterxml.jackson.databind.deser.std.StdDeserializer; -import com.fasterxml.jackson.databind.ser.std.StdSerializer; -import java.io.IOException; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; import java.util.Objects; /** Represents a 2d pose containing translational and rotational elements. */ -@JsonSerialize(using = Pose2d.PoseSerializer.class) -@JsonDeserialize(using = Pose2d.PoseDeserializer.class) +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Pose2d { private final Translation2d m_translation; private final Rotation2d m_rotation; @@ -42,7 +35,10 @@ public class Pose2d { * @param translation The translational component of the pose. * @param rotation The rotational component of the pose. */ - public Pose2d(Translation2d translation, Rotation2d rotation) { + @JsonCreator + public Pose2d( + @JsonProperty(required = true, value = "translation") Translation2d translation, + @JsonProperty(required = true, value = "rotation") Rotation2d rotation) { m_translation = translation; m_rotation = rotation; } @@ -90,6 +86,7 @@ public class Pose2d { * * @return The translational component of the pose. */ + @JsonProperty public Translation2d getTranslation() { return m_translation; } @@ -99,6 +96,7 @@ public class Pose2d { * * @return The rotational component of the pose. */ + @JsonProperty public Rotation2d getRotation() { return m_rotation; } @@ -230,37 +228,4 @@ public class Pose2d { public int hashCode() { return Objects.hash(m_translation, m_rotation); } - - static class PoseSerializer extends StdSerializer { - PoseSerializer() { - super(Pose2d.class); - } - - @Override - public void serialize(Pose2d value, JsonGenerator jgen, SerializerProvider provider) - throws IOException, JsonProcessingException { - - jgen.writeStartObject(); - jgen.writeObjectField("translation", value.m_translation); - jgen.writeObjectField("rotation", value.m_rotation); - jgen.writeEndObject(); - } - } - - static class PoseDeserializer extends StdDeserializer { - PoseDeserializer() { - super(Pose2d.class); - } - - @Override - public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt) - throws IOException, JsonProcessingException { - JsonNode node = jp.getCodec().readTree(jp); - - Translation2d translation = - jp.getCodec().treeToValue(node.get("translation"), Translation2d.class); - Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class); - return new Pose2d(translation, rotation); - } - } } diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java index 73e211900..39a4450ff 100644 --- a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java +++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,22 +7,15 @@ package edu.wpi.first.wpilibj.geometry; -import com.fasterxml.jackson.core.JsonGenerator; -import com.fasterxml.jackson.core.JsonParser; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationContext; -import com.fasterxml.jackson.databind.JsonNode; -import com.fasterxml.jackson.databind.SerializerProvider; -import com.fasterxml.jackson.databind.annotation.JsonDeserialize; -import com.fasterxml.jackson.databind.annotation.JsonSerialize; -import com.fasterxml.jackson.databind.deser.std.StdDeserializer; -import com.fasterxml.jackson.databind.ser.std.StdSerializer; -import java.io.IOException; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; import java.util.Objects; /** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */ -@JsonSerialize(using = Rotation2d.RotationSerializer.class) -@JsonDeserialize(using = Rotation2d.RotationDeserializer.class) +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation2d { private final double m_value; private final double m_cos; @@ -40,7 +33,8 @@ public class Rotation2d { * * @param value The value of the angle in radians. */ - public Rotation2d(double value) { + @JsonCreator + public Rotation2d(@JsonProperty(required = true, value = "radians") double value) { m_value = value; m_cos = Math.cos(value); m_sin = Math.sin(value); @@ -133,11 +127,12 @@ public class Rotation2d { m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos); } - /* + /** * Returns the radian value of the rotation. * * @return The radian value of the rotation. */ + @JsonProperty public double getRadians() { return m_value; } @@ -201,34 +196,4 @@ public class Rotation2d { public int hashCode() { return Objects.hash(m_value); } - - static class RotationSerializer extends StdSerializer { - RotationSerializer() { - super(Rotation2d.class); - } - - @Override - public void serialize(Rotation2d value, JsonGenerator jgen, SerializerProvider provider) - throws IOException, JsonProcessingException { - - jgen.writeStartObject(); - jgen.writeNumberField("radians", value.m_value); - jgen.writeEndObject(); - } - } - - static class RotationDeserializer extends StdDeserializer { - RotationDeserializer() { - super(Rotation2d.class); - } - - @Override - public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt) - throws IOException, JsonProcessingException { - JsonNode node = jp.getCodec().readTree(jp); - double radians = node.get("radians").numberValue().doubleValue(); - - return new Rotation2d(radians); - } - } } diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java index 5ab4fae01..9ec6ef2bf 100644 --- a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java +++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -77,6 +77,20 @@ public class Transform2d { return m_rotation; } + /** + * Invert the transformation. This is useful for undoing a transformation. + * + * @return The inverted transformation. + */ + public Transform2d inverse() { + // We are rotating the difference between the translations + // using a clockwise rotation matrix. This transforms the global + // delta into a local delta (relative to the initial pose). + return new Transform2d( + getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()), + getRotation().unaryMinus()); + } + @Override public String toString() { return String.format("Transform2d(%s, %s)", m_translation, m_rotation); diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java index 2cdbba574..a67412d85 100644 --- a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java +++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -7,17 +7,10 @@ package edu.wpi.first.wpilibj.geometry; -import com.fasterxml.jackson.core.JsonGenerator; -import com.fasterxml.jackson.core.JsonParser; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationContext; -import com.fasterxml.jackson.databind.JsonNode; -import com.fasterxml.jackson.databind.SerializerProvider; -import com.fasterxml.jackson.databind.annotation.JsonDeserialize; -import com.fasterxml.jackson.databind.annotation.JsonSerialize; -import com.fasterxml.jackson.databind.deser.std.StdDeserializer; -import com.fasterxml.jackson.databind.ser.std.StdSerializer; -import java.io.IOException; +import com.fasterxml.jackson.annotation.JsonAutoDetect; +import com.fasterxml.jackson.annotation.JsonCreator; +import com.fasterxml.jackson.annotation.JsonIgnoreProperties; +import com.fasterxml.jackson.annotation.JsonProperty; import java.util.Objects; /** @@ -27,9 +20,9 @@ import java.util.Objects; * the origin, facing toward the X direction, moving forward increases the X, whereas moving to the * left increases the Y. */ -@JsonSerialize(using = Translation2d.TranslationSerializer.class) -@JsonDeserialize(using = Translation2d.TranslationDeserializer.class) @SuppressWarnings({"ParameterName", "MemberName"}) +@JsonIgnoreProperties(ignoreUnknown = true) +@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Translation2d { private final double m_x; private final double m_y; @@ -45,7 +38,10 @@ public class Translation2d { * @param x The x component of the translation. * @param y The y component of the translation. */ - public Translation2d(double x, double y) { + @JsonCreator + public Translation2d( + @JsonProperty(required = true, value = "x") double x, + @JsonProperty(required = true, value = "y") double y) { m_x = x; m_y = y; } @@ -68,6 +64,7 @@ public class Translation2d { * * @return The x component of the translation. */ + @JsonProperty public double getX() { return m_x; } @@ -77,6 +74,7 @@ public class Translation2d { * * @return The y component of the translation. */ + @JsonProperty public double getY() { return m_y; } @@ -170,10 +168,6 @@ public class Translation2d { return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y); } - public static Translation2d fromRotation2d(Rotation2d rotation) { - return new Translation2d(rotation.getCos(), rotation.getSin()); - } - /** * Checks equality between this Translation2d and another object. * @@ -193,36 +187,4 @@ public class Translation2d { public int hashCode() { return Objects.hash(m_x, m_y); } - - static class TranslationSerializer extends StdSerializer { - TranslationSerializer() { - super(Translation2d.class); - } - - @Override - public void serialize(Translation2d value, JsonGenerator jgen, SerializerProvider provider) - throws IOException, JsonProcessingException { - - jgen.writeStartObject(); - jgen.writeNumberField("x", value.m_x); - jgen.writeNumberField("y", value.m_y); - jgen.writeEndObject(); - } - } - - static class TranslationDeserializer extends StdDeserializer { - TranslationDeserializer() { - super(Translation2d.class); - } - - @Override - public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt) - throws IOException, JsonProcessingException { - JsonNode node = jp.getCodec().readTree(jp); - double xval = node.get("x").numberValue().doubleValue(); - double yval = node.get("y").numberValue().doubleValue(); - - return new Translation2d(xval, yval); - } - } } diff --git a/chameleon-server/src/main/resources/log4j.properties b/chameleon-server/src/main/resources/log4j.properties new file mode 100644 index 000000000..636e70811 --- /dev/null +++ b/chameleon-server/src/main/resources/log4j.properties @@ -0,0 +1 @@ +log4j.rootLogger=INFO, STDOUT \ No newline at end of file diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java new file mode 100644 index 000000000..08ad99c8b --- /dev/null +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/ReflectivePipelineTest.java @@ -0,0 +1,125 @@ +package com.chameleonvision.common.vision.pipeline; + +import com.chameleonvision.common.util.TestUtils; +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.provider.FileFrameProvider; +import com.chameleonvision.common.vision.opencv.CVMat; +import com.chameleonvision.common.vision.opencv.ContourGroupingMode; +import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import org.slf4j.LoggerFactory; + +public class ReflectivePipelineTest { + + public static void setLoggingLevel(ch.qos.logback.classic.Level level) { + ch.qos.logback.classic.Logger root = + (ch.qos.logback.classic.Logger) + org.slf4j.LoggerFactory.getLogger(ch.qos.logback.classic.Logger.ROOT_LOGGER_NAME); + root.setLevel(level); + } + + @Test + public void testDebug() { + var logger = LoggerFactory.getLogger(ReflectivePipelineTest.class); + setLoggingLevel(ch.qos.logback.classic.Level.WARN); + logger.warn(String.valueOf(logger.isDebugEnabled())); + logger.info("hi"); + logger.debug("debug"); + } + + @Test + public void test2019() { + TestUtils.loadLibraries(); + var pipeline = new ReflectivePipeline(); + + var settings = new ReflectivePipelineSettings(); + settings.hsvHue.set(60, 100); + settings.hsvSaturation.set(100, 255); + settings.hsvValue.set(190, 255); + settings.outputShowThresholded = true; + settings.outputShowMultipleTargets = true; + settings.contourGroupingMode = ContourGroupingMode.Dual; + settings.contourIntersection = ContourIntersectionDirection.Up; + + var frameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.WPI2019Image.FOV); + + TestUtils.showImage(frameProvider.getFrame().image.getMat(), "Pipeline input", 1); + + CVPipelineResult pipelineResult; + + pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + printTestResults(pipelineResult); + + Assertions.assertTrue(pipelineResult.hasTargets()); + Assertions.assertEquals(2, pipelineResult.targets.size()); + + TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output"); + } + + @Test + public void test2020() { + TestUtils.loadLibraries(); + var pipeline = new ReflectivePipeline(); + + var settings = new ReflectivePipelineSettings(); + settings.hsvHue.set(60, 100); + settings.hsvSaturation.set(200, 255); + settings.hsvValue.set(200, 255); + settings.outputShowThresholded = true; + + var frameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center), + TestUtils.WPI2020Image.FOV); + + CVPipelineResult pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + printTestResults(pipelineResult); + + TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output"); + } + + private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) { + var pipeline = new ReflectivePipeline(); + + while (true) { + CVPipelineResult pipelineResult = pipeline.run(frame, settings); + printTestResults(pipelineResult); + int preRelease = CVMat.getMatCount(); + pipelineResult.release(); + int postRelease = CVMat.getMatCount(); + + System.out.printf("Pre: %d, Post: %d\n", preRelease, postRelease); + } + } + + // used to run VisualVM for profiling. It won't run on unit tests. + public static void main(String[] args) { + TestUtils.loadLibraries(); + var frameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.WPI2019Image.FOV); + + var settings = new ReflectivePipelineSettings(); + settings.hsvHue.set(60, 100); + settings.hsvSaturation.set(100, 255); + settings.hsvValue.set(190, 255); + settings.outputShowThresholded = true; + settings.outputShowMultipleTargets = true; + settings.contourGroupingMode = ContourGroupingMode.Dual; + settings.contourIntersection = ContourIntersectionDirection.Up; + + continuouslyRunPipeline(frameProvider.getFrame(), settings); + } + + private static void printTestResults(CVPipelineResult pipelineResult) { + double fps = 1000 / pipelineResult.getLatencyMillis(); + System.out.print( + "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " fps), "); + System.out.println("Found " + pipelineResult.targets.size() + " valid targets"); + } +} diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java new file mode 100644 index 000000000..7b63cbed0 --- /dev/null +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/pipeline/SolvePNPTest.java @@ -0,0 +1,197 @@ +package com.chameleonvision.common.vision.pipeline; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; + +import com.chameleonvision.common.calibration.CameraCalibrationCoefficients; +import com.chameleonvision.common.util.TestUtils; +import com.chameleonvision.common.vision.frame.Frame; +import com.chameleonvision.common.vision.frame.provider.FileFrameProvider; +import com.chameleonvision.common.vision.opencv.CVMat; +import com.chameleonvision.common.vision.opencv.ContourGroupingMode; +import com.chameleonvision.common.vision.opencv.ContourIntersectionDirection; +import com.chameleonvision.common.vision.target.TargetModel; +import com.chameleonvision.common.vision.target.TrackedTarget; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import java.io.IOException; +import java.nio.file.Path; +import java.util.stream.Collectors; +import org.junit.jupiter.api.Test; + +public class SolvePNPTest { + + @Test + public void meme() throws IOException { + TestUtils.loadLibraries(); + + var lowres = (Path.of(TestUtils.getCalibrationPath().toString(), "lifecamcal.json").toFile()); + var cal1 = new ObjectMapper().readValue(lowres, CameraCalibrationCoefficients.class); + + var highres = (Path.of(TestUtils.getCalibrationPath().toString(), "lifecamcal2.json").toFile()); + var cal2 = new ObjectMapper().readValue(highres, CameraCalibrationCoefficients.class); + } + + private CameraCalibrationCoefficients get640p() { + try { + var cameraCalibration = + new ObjectMapper() + .readValue( + (Path.of(TestUtils.getCalibrationPath().toString(), "lifecam640p.json").toFile()), + CameraCalibrationCoefficients.class); + + assertEquals(3, cameraCalibration.cameraIntrinsics.rows); + assertEquals(3, cameraCalibration.cameraIntrinsics.cols); + assertEquals(1, cameraCalibration.cameraExtrinsics.rows); + assertEquals(5, cameraCalibration.cameraExtrinsics.cols); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows()); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols()); + assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMat().rows()); + assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMat().cols()); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows()); + assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols()); + assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows()); + assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols()); + assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows()); + assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols()); + assertEquals(1, cameraCalibration.getCameraExtrinsicsMat().rows()); + assertEquals(5, cameraCalibration.getCameraExtrinsicsMat().cols()); + + return cameraCalibration; + } catch (IOException e) { + e.printStackTrace(); + return null; + } + } + + @Test + public void test2019() { + TestUtils.loadLibraries(); + var pipeline = new ReflectivePipeline(); + + var settings = new ReflectivePipelineSettings(); + settings.hsvHue.set(60, 100); + settings.hsvSaturation.set(100, 255); + settings.hsvValue.set(190, 255); + settings.outputShowThresholded = true; + settings.outputShowMultipleTargets = true; + settings.solvePNPEnabled = true; + settings.contourGroupingMode = ContourGroupingMode.Dual; + settings.contourIntersection = ContourIntersectionDirection.Up; + settings.cornerDetectionUseConvexHulls = true; + + var frameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in), + TestUtils.WPI2019Image.FOV); + + CVPipelineResult pipelineResult; + + pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + + TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 1000 * 90); + } + + @Test + public void test2020() { + TestUtils.loadLibraries(); + var pipeline = new ReflectivePipeline(); + + var settings = new ReflectivePipelineSettings(); + settings.hsvHue.set(60, 100); + settings.hsvSaturation.set(100, 255); + settings.hsvValue.set(60, 255); + settings.outputShowThresholded = true; + settings.solvePNPEnabled = true; + settings.cornerDetectionAccuracyPercentage = 4; + settings.cornerDetectionUseConvexHulls = true; + settings.cameraCalibration = get640p(); + settings.targetModel = TargetModel.get2020Target(36); + settings.cameraPitch = Rotation2d.fromDegrees(0.0); + + assertNotNull(settings.cameraCalibration); + assertEquals(3, settings.cameraCalibration.cameraIntrinsics.rows); + assertEquals(3, settings.cameraCalibration.cameraIntrinsics.cols); + assertEquals(1, settings.cameraCalibration.cameraExtrinsics.rows); + assertEquals(5, settings.cameraCalibration.cameraExtrinsics.cols); + + assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMat().rows()); + assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMat().cols()); + assertEquals(1, settings.cameraCalibration.cameraExtrinsics.getAsMat().rows()); + assertEquals(5, settings.cameraCalibration.cameraExtrinsics.getAsMat().cols()); + + assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows()); + assertEquals(3, settings.cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols()); + assertEquals(1, settings.cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows()); + assertEquals(5, settings.cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols()); + + assertEquals(3, settings.cameraCalibration.getCameraIntrinsicsMat().rows()); + assertEquals(3, settings.cameraCalibration.getCameraIntrinsicsMat().cols()); + assertEquals(1, settings.cameraCalibration.getCameraExtrinsicsMat().rows()); + assertEquals(5, settings.cameraCalibration.getCameraExtrinsicsMat().cols()); + + var frameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left), + TestUtils.WPI2020Image.FOV); + + // TestUtils.showImage(frameProvider.getFrame().image.getMat(), "Pipeline output", + // 999999); + + CVPipelineResult pipelineResult = pipeline.run(frameProvider.getFrame(), settings); + printTestResults(pipelineResult); + + var pose = pipelineResult.targets.get(0).getRobotRelativePose(); + // assertEquals(180, pose.getTranslation().getX(), 20); + // assertEquals(0, pose.getTranslation().getY(), 20); + // assertEquals(0, pose.getRotation().getDegrees(), 5); + + TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999); + } + + private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) { + var pipeline = new ReflectivePipeline(); + + while (true) { + CVPipelineResult pipelineResult = pipeline.run(frame, settings); + printTestResults(pipelineResult); + int preRelease = CVMat.getMatCount(); + pipelineResult.release(); + int postRelease = CVMat.getMatCount(); + + System.out.printf("Pre: %d, Post: %d\n", preRelease, postRelease); + } + } + + // used to run VisualVM for profiling. It won't run on unit tests. + public static void main(String[] args) { + TestUtils.loadLibraries(); + var frameProvider = + new FileFrameProvider( + TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes), + TestUtils.WPI2019Image.FOV); + + var settings = new ReflectivePipelineSettings(); + settings.hsvHue.set(60, 100); + settings.hsvSaturation.set(100, 255); + settings.hsvValue.set(190, 255); + settings.outputShowThresholded = true; + settings.outputShowMultipleTargets = true; + settings.contourGroupingMode = ContourGroupingMode.Dual; + settings.contourIntersection = ContourIntersectionDirection.Up; + + continuouslyRunPipeline(frameProvider.getFrame(), settings); + } + + private static void printTestResults(CVPipelineResult pipelineResult) { + double fps = 1000 / pipelineResult.getLatencyMillis(); + System.out.println( + "Pipeline ran in " + pipelineResult.getLatencyMillis() + "ms (" + fps + " " + "fps)"); + System.out.println("Found " + pipelineResult.targets.size() + " valid targets"); + System.out.println( + "Found targets at " + + pipelineResult.targets.stream() + .map(TrackedTarget::getRobotRelativePose) + .collect(Collectors.toList())); + } +} diff --git a/chameleon-server/src/test/resources/calibration/lifecam320p.json b/chameleon-server/src/test/resources/calibration/lifecam320p.json new file mode 100644 index 000000000..349c450c8 --- /dev/null +++ b/chameleon-server/src/test/resources/calibration/lifecam320p.json @@ -0,0 +1,34 @@ +{ + "resolution": { + "width": 320.0, + "height": 240.0 + }, + "cameraIntrinsics": { + "rows": 3, + "cols": 3, + "type": 6, + "data": [ + 353.74653217742724, + 0.0, + 163.55407989211918, + 0.0, + 340.77624878700817, + 119.8945718300403, + 0.0, + 0.0, + 1.0 + ] + }, + "cameraExtrinsics": { + "rows": 1, + "cols": 5, + "type": 6, + "data": [ + 0.10322037759535845, + -0.2890556437050186, + 0.00406400648501475, + 2.5573586808275763E-4, + -1.462385758978924 + ] + } +} \ No newline at end of file diff --git a/chameleon-server/src/test/resources/calibration/lifecam640p.json b/chameleon-server/src/test/resources/calibration/lifecam640p.json new file mode 100644 index 000000000..fff629bf6 --- /dev/null +++ b/chameleon-server/src/test/resources/calibration/lifecam640p.json @@ -0,0 +1,34 @@ +{ + "resolution": { + "width": 640.0, + "height": 480.0 + }, + "cameraIntrinsics": { + "rows": 3, + "cols": 3, + "type": 6, + "data": [ + 699.3778103158814, + 0.0, + 345.6059345433618, + 0.0, + 677.7161226393544, + 207.12741326228522, + 0.0, + 0.0, + 1.0 + ] + }, + "cameraExtrinsics": { + "rows": 1, + "cols": 5, + "type": 6, + "data": [ + 0.14382207979312617, + -0.9851192814987014, + -0.018168751047242335, + 0.011034504043795105, + 1.9833437176538498 + ] + } +} \ No newline at end of file diff --git a/testimages/2019/CargoAngledDark48in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoAngledDark48in.jpg similarity index 100% rename from testimages/2019/CargoAngledDark48in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoAngledDark48in.jpg diff --git a/testimages/2019/CargoSideStraightDark36in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightDark36in.jpg similarity index 100% rename from testimages/2019/CargoSideStraightDark36in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightDark36in.jpg diff --git a/testimages/2019/CargoSideStraightDark60in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightDark60in.jpg similarity index 100% rename from testimages/2019/CargoSideStraightDark60in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightDark60in.jpg diff --git a/testimages/2019/CargoSideStraightDark72in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightDark72in.jpg similarity index 100% rename from testimages/2019/CargoSideStraightDark72in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightDark72in.jpg diff --git a/testimages/2019/CargoSideStraightPanelDark36in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightPanelDark36in.jpg similarity index 100% rename from testimages/2019/CargoSideStraightPanelDark36in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoSideStraightPanelDark36in.jpg diff --git a/testimages/2019/CargoStraightDark19in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark19in.jpg similarity index 100% rename from testimages/2019/CargoStraightDark19in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark19in.jpg diff --git a/testimages/2019/CargoStraightDark24in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark24in.jpg similarity index 100% rename from testimages/2019/CargoStraightDark24in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark24in.jpg diff --git a/testimages/2019/CargoStraightDark48in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark48in.jpg similarity index 100% rename from testimages/2019/CargoStraightDark48in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark48in.jpg diff --git a/testimages/2019/CargoStraightDark72in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark72in.jpg similarity index 100% rename from testimages/2019/CargoStraightDark72in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark72in.jpg diff --git a/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark72in_HighRes.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark72in_HighRes.jpg new file mode 100644 index 000000000..e56d05291 Binary files /dev/null and b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark72in_HighRes.jpg differ diff --git a/testimages/2019/CargoStraightDark90in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark90in.jpg similarity index 100% rename from testimages/2019/CargoStraightDark90in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/CargoStraightDark90in.jpg diff --git a/testimages/2019/LoadingAngle36in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngle36in.jpg similarity index 100% rename from testimages/2019/LoadingAngle36in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngle36in.jpg diff --git a/testimages/2019/LoadingAngleDark36in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngleDark36in.jpg similarity index 100% rename from testimages/2019/LoadingAngleDark36in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngleDark36in.jpg diff --git a/testimages/2019/LoadingAngleDark60in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngleDark60in.jpg similarity index 100% rename from testimages/2019/LoadingAngleDark60in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngleDark60in.jpg diff --git a/testimages/2019/LoadingAngleDark96in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngleDark96in.jpg similarity index 100% rename from testimages/2019/LoadingAngleDark96in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingAngleDark96in.jpg diff --git a/testimages/2019/LoadingStraight108in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraight108in.jpg similarity index 100% rename from testimages/2019/LoadingStraight108in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraight108in.jpg diff --git a/testimages/2019/LoadingStraight36in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraight36in.jpg similarity index 100% rename from testimages/2019/LoadingStraight36in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraight36in.jpg diff --git a/testimages/2019/LoadingStraightDark108in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark108in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark108in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark108in.jpg diff --git a/testimages/2019/LoadingStraightDark10in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark10in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark10in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark10in.jpg diff --git a/testimages/2019/LoadingStraightDark13in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark13in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark13in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark13in.jpg diff --git a/testimages/2019/LoadingStraightDark21in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark21in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark21in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark21in.jpg diff --git a/testimages/2019/LoadingStraightDark36in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark36in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark36in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark36in.jpg diff --git a/testimages/2019/LoadingStraightDark48in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark48in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark48in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark48in.jpg diff --git a/testimages/2019/LoadingStraightDark60in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark60in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark60in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark60in.jpg diff --git a/testimages/2019/LoadingStraightDark84in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark84in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark84in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark84in.jpg diff --git a/testimages/2019/LoadingStraightDark9in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark9in.jpg similarity index 100% rename from testimages/2019/LoadingStraightDark9in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/LoadingStraightDark9in.jpg diff --git a/testimages/2019/RocketBallStraightDark19in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark19in.jpg similarity index 100% rename from testimages/2019/RocketBallStraightDark19in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark19in.jpg diff --git a/testimages/2019/RocketBallStraightDark24in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark24in.jpg similarity index 100% rename from testimages/2019/RocketBallStraightDark24in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark24in.jpg diff --git a/testimages/2019/RocketBallStraightDark29in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark29in.jpg similarity index 100% rename from testimages/2019/RocketBallStraightDark29in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark29in.jpg diff --git a/testimages/2019/RocketBallStraightDark48in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark48in.jpg similarity index 100% rename from testimages/2019/RocketBallStraightDark48in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketBallStraightDark48in.jpg diff --git a/testimages/2019/RocketPanelAngleDark48in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelAngleDark48in.jpg similarity index 100% rename from testimages/2019/RocketPanelAngleDark48in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelAngleDark48in.jpg diff --git a/testimages/2019/RocketPanelAngleDark60in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelAngleDark60in.jpg similarity index 100% rename from testimages/2019/RocketPanelAngleDark60in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelAngleDark60in.jpg diff --git a/testimages/2019/RocketPanelAngleDark84in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelAngleDark84in.jpg similarity index 100% rename from testimages/2019/RocketPanelAngleDark84in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelAngleDark84in.jpg diff --git a/testimages/2019/RocketPanelStraight48in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraight48in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraight48in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraight48in.jpg diff --git a/testimages/2019/RocketPanelStraight84in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraight84in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraight84in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraight84in.jpg diff --git a/testimages/2019/RocketPanelStraightDark12in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark12in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark12in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark12in.jpg diff --git a/testimages/2019/RocketPanelStraightDark16in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark16in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark16in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark16in.jpg diff --git a/testimages/2019/RocketPanelStraightDark24in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark24in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark24in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark24in.jpg diff --git a/testimages/2019/RocketPanelStraightDark36in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark36in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark36in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark36in.jpg diff --git a/testimages/2019/RocketPanelStraightDark48in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark48in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark48in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark48in.jpg diff --git a/testimages/2019/RocketPanelStraightDark60in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark60in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark60in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark60in.jpg diff --git a/testimages/2019/RocketPanelStraightDark72in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark72in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark72in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark72in.jpg diff --git a/testimages/2019/RocketPanelStraightDark96in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark96in.jpg similarity index 100% rename from testimages/2019/RocketPanelStraightDark96in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketPanelStraightDark96in.jpg diff --git a/testimages/2019/RocketStraightDark96in.jpg b/chameleon-server/src/test/resources/testimages/2019/WPI/RocketStraightDark96in.jpg similarity index 100% rename from testimages/2019/RocketStraightDark96in.jpg rename to chameleon-server/src/test/resources/testimages/2019/WPI/RocketStraightDark96in.jpg diff --git a/testimages/2019/info.txt b/chameleon-server/src/test/resources/testimages/2019/WPI/info.txt similarity index 100% rename from testimages/2019/info.txt rename to chameleon-server/src/test/resources/testimages/2019/WPI/info.txt diff --git a/testimages/2020/BlueGoal-060in-Center.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-060in-Center.jpg similarity index 100% rename from testimages/2020/BlueGoal-060in-Center.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-060in-Center.jpg diff --git a/testimages/2020/BlueGoal-084in-Center.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-084in-Center.jpg similarity index 100% rename from testimages/2020/BlueGoal-084in-Center.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-084in-Center.jpg diff --git a/testimages/2020/BlueGoal-108in-Center.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-108in-Center.jpg similarity index 100% rename from testimages/2020/BlueGoal-108in-Center.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-108in-Center.jpg diff --git a/testimages/2020/BlueGoal-132in-Center.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-132in-Center.jpg similarity index 100% rename from testimages/2020/BlueGoal-132in-Center.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-132in-Center.jpg diff --git a/testimages/2020/BlueGoal-156in-Center.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-156in-Center.jpg similarity index 100% rename from testimages/2020/BlueGoal-156in-Center.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-156in-Center.jpg diff --git a/testimages/2020/BlueGoal-156in-Left.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-156in-Left.jpg similarity index 100% rename from testimages/2020/BlueGoal-156in-Left.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-156in-Left.jpg diff --git a/testimages/2020/BlueGoal-180in-Center.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-180in-Center.jpg similarity index 100% rename from testimages/2020/BlueGoal-180in-Center.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-180in-Center.jpg diff --git a/testimages/2020/BlueGoal-224in-Center.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-224in-Left.jpg similarity index 100% rename from testimages/2020/BlueGoal-224in-Center.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-224in-Left.jpg diff --git a/testimages/2020/BlueGoal-228in-ProtectedZone.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-228in-ProtectedZone.jpg similarity index 100% rename from testimages/2020/BlueGoal-228in-ProtectedZone.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-228in-ProtectedZone.jpg diff --git a/testimages/2020/BlueGoal-330in-ProtectedZone.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-330in-ProtectedZone.jpg similarity index 100% rename from testimages/2020/BlueGoal-330in-ProtectedZone.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-330in-ProtectedZone.jpg diff --git a/testimages/2020/BlueGoal-Far-ProtectedZone.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-Far-ProtectedZone.jpg similarity index 100% rename from testimages/2020/BlueGoal-Far-ProtectedZone.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/BlueGoal-Far-ProtectedZone.jpg diff --git a/testimages/2020/RedLoading-016in-Down.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-016in-Down.jpg similarity index 100% rename from testimages/2020/RedLoading-016in-Down.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-016in-Down.jpg diff --git a/testimages/2020/RedLoading-030in-Down.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-030in-Down.jpg similarity index 100% rename from testimages/2020/RedLoading-030in-Down.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-030in-Down.jpg diff --git a/testimages/2020/RedLoading-048in-Down.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-048in-Down.jpg similarity index 100% rename from testimages/2020/RedLoading-048in-Down.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-048in-Down.jpg diff --git a/testimages/2020/RedLoading-048in.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-048in.jpg similarity index 100% rename from testimages/2020/RedLoading-048in.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-048in.jpg diff --git a/testimages/2020/RedLoading-060in.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-060in.jpg similarity index 100% rename from testimages/2020/RedLoading-060in.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-060in.jpg diff --git a/testimages/2020/RedLoading-084in.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-084in.jpg similarity index 100% rename from testimages/2020/RedLoading-084in.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-084in.jpg diff --git a/testimages/2020/RedLoading-108in.jpg b/chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-108in.jpg similarity index 100% rename from testimages/2020/RedLoading-108in.jpg rename to chameleon-server/src/test/resources/testimages/2020/WPI/RedLoading-108in.jpg diff --git a/chameleon-server/src/test/resources/testimages/2020/WPI/info.txt b/chameleon-server/src/test/resources/testimages/2020/WPI/info.txt new file mode 100644 index 000000000..cc4714e73 --- /dev/null +++ b/chameleon-server/src/test/resources/testimages/2020/WPI/info.txt @@ -0,0 +1,3 @@ +Taken on Microsoft LifeCam HD-3000 +FOV = 68.5 +Credit to WPILib for these images. \ No newline at end of file diff --git a/testimages/2020/image.png b/testimages/2020/image.png deleted file mode 100644 index 0e137e860..000000000 Binary files a/testimages/2020/image.png and /dev/null differ