mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-20 00:51:41 +00:00
Rename to PhotonVision
This commit is contained in:
Matt
@@ -0,0 +1,182 @@
|
||||
package org.photonvision.common;
|
||||
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.util.math.MathUtils;
|
||||
import org.photonvision.common.util.numbers.NumberListUtils;
|
||||
import org.photonvision.common.vision.frame.FrameProvider;
|
||||
import org.photonvision.common.vision.frame.provider.FileFrameProvider;
|
||||
import org.photonvision.common.vision.opencv.CVMat;
|
||||
import org.photonvision.common.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
|
||||
import com.chameleonvision.common.vision.pipeline.*;
|
||||
import java.util.ArrayList;
|
||||
import java.util.Collections;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.BeforeAll;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.photonvision.common.vision.pipeline.CVPipeline;
|
||||
import org.photonvision.common.vision.pipeline.CVPipelineResult;
|
||||
import org.photonvision.common.vision.pipeline.ReflectivePipeline;
|
||||
|
||||
/** Various tests that check performance on long-running tasks (i.e. a pipeline) */
|
||||
public class BenchmarkTest {
|
||||
@BeforeAll
|
||||
public static void init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
@Test
|
||||
public void Reflective240pBenchmark() {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(100, 255);
|
||||
pipeline.getSettings().hsvValue.set(190, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
pipeline.getSettings().outputShowMultipleTargets = true;
|
||||
pipeline.getSettings().contourGroupingMode = ContourGroupingMode.Dual;
|
||||
pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoSideStraightDark72in),
|
||||
TestUtils.WPI2019Image.FOV);
|
||||
|
||||
frameProvider.setImageReloading(true);
|
||||
|
||||
benchmarkPipeline(frameProvider, pipeline, 5);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void Reflective480pBenchmark() {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(200, 255);
|
||||
pipeline.getSettings().hsvValue.set(200, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center),
|
||||
TestUtils.WPI2020Image.FOV);
|
||||
|
||||
frameProvider.setImageReloading(true);
|
||||
|
||||
benchmarkPipeline(frameProvider, pipeline, 5);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void Reflective720pBenchmark() {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(200, 255);
|
||||
pipeline.getSettings().hsvValue.set(200, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_084in_Center_720p),
|
||||
TestUtils.WPI2020Image.FOV);
|
||||
|
||||
frameProvider.setImageReloading(true);
|
||||
|
||||
benchmarkPipeline(frameProvider, pipeline, 5);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void Reflective1920x1440Benchmark() {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(100, 255);
|
||||
pipeline.getSettings().hsvValue.set(190, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
pipeline.getSettings().outputShowMultipleTargets = true;
|
||||
pipeline.getSettings().contourGroupingMode = ContourGroupingMode.Dual;
|
||||
pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes),
|
||||
TestUtils.WPI2019Image.FOV);
|
||||
|
||||
frameProvider.setImageReloading(true);
|
||||
|
||||
benchmarkPipeline(frameProvider, pipeline, 5);
|
||||
}
|
||||
|
||||
private static <P extends CVPipeline> void benchmarkPipeline(
|
||||
FrameProvider frameProvider, P pipeline, int secondsToRun) {
|
||||
CVMat.enablePrint(false);
|
||||
// warmup for 5 loops.
|
||||
System.out.println("Warming up for 5 loops...");
|
||||
for (int i = 0; i < 5; i++) {
|
||||
pipeline.run(frameProvider.get());
|
||||
}
|
||||
|
||||
final List<Double> processingTimes = new ArrayList<>();
|
||||
final List<Double> latencyTimes = new ArrayList<>();
|
||||
|
||||
var frameProps = frameProvider.get().frameStaticProperties;
|
||||
|
||||
// begin benchmark
|
||||
System.out.println(
|
||||
"Beginning "
|
||||
+ secondsToRun
|
||||
+ " second benchmark at resolution "
|
||||
+ frameProps.imageWidth
|
||||
+ "x"
|
||||
+ frameProps.imageHeight);
|
||||
var benchmarkStartMillis = System.currentTimeMillis();
|
||||
do {
|
||||
CVPipelineResult pipelineResult = pipeline.run(frameProvider.get());
|
||||
pipelineResult.release();
|
||||
processingTimes.add(pipelineResult.processingMillis);
|
||||
latencyTimes.add(pipelineResult.getLatencyMillis());
|
||||
} while (System.currentTimeMillis() - benchmarkStartMillis < secondsToRun * 1000);
|
||||
System.out.println("Benchmark complete.");
|
||||
|
||||
var processingMin = Collections.min(processingTimes);
|
||||
var processingMean = NumberListUtils.mean(processingTimes);
|
||||
var processingMax = Collections.max(processingTimes);
|
||||
|
||||
var latencyMin = Collections.min(latencyTimes);
|
||||
var latencyMean = NumberListUtils.mean(latencyTimes);
|
||||
var latencyMax = Collections.max(latencyTimes);
|
||||
|
||||
String processingResult =
|
||||
"Processing times - "
|
||||
+ "Min: "
|
||||
+ MathUtils.roundTo(processingMin, 3)
|
||||
+ "ms ("
|
||||
+ MathUtils.roundTo(1000 / processingMin, 3)
|
||||
+ " FPS), "
|
||||
+ "Mean: "
|
||||
+ MathUtils.roundTo(processingMean, 3)
|
||||
+ "ms ("
|
||||
+ MathUtils.roundTo(1000 / processingMean, 3)
|
||||
+ " FPS), "
|
||||
+ "Max: "
|
||||
+ MathUtils.roundTo(processingMax, 3)
|
||||
+ "ms ("
|
||||
+ MathUtils.roundTo(1000 / processingMax, 3)
|
||||
+ " FPS)";
|
||||
System.out.println(processingResult);
|
||||
String latencyResult =
|
||||
"Latency times - "
|
||||
+ "Min: "
|
||||
+ MathUtils.roundTo(latencyMin, 3)
|
||||
+ "ms ("
|
||||
+ MathUtils.roundTo(1000 / latencyMin, 3)
|
||||
+ " FPS), "
|
||||
+ "Mean: "
|
||||
+ MathUtils.roundTo(latencyMean, 3)
|
||||
+ "ms ("
|
||||
+ MathUtils.roundTo(1000 / latencyMean, 3)
|
||||
+ " FPS), "
|
||||
+ "Max: "
|
||||
+ MathUtils.roundTo(latencyMax, 3)
|
||||
+ "ms ("
|
||||
+ MathUtils.roundTo(1000 / latencyMax, 3)
|
||||
+ " FPS)";
|
||||
System.out.println(latencyResult);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
package org.photonvision.common.configuration;
|
||||
|
||||
import org.photonvision.common.logging.Level;
|
||||
import org.photonvision.common.logging.LogGroup;
|
||||
import org.photonvision.common.logging.Logger;
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.util.file.JacksonUtils;
|
||||
import org.photonvision.common.vision.pipeline.ColoredShapePipelineSettings;
|
||||
import org.photonvision.common.vision.pipeline.ReflectivePipelineSettings;
|
||||
import org.photonvision.common.vision.target.TargetModel;
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import java.nio.file.Paths;
|
||||
import org.junit.jupiter.api.*;
|
||||
|
||||
public class ConfigTest {
|
||||
|
||||
private static final ConfigManager configMgr;
|
||||
private static final CameraConfiguration cameraConfig =
|
||||
new CameraConfiguration("TestCamera", "/dev/video420");
|
||||
private static final ReflectivePipelineSettings REFLECTIVE_PIPELINE_SETTINGS =
|
||||
new ReflectivePipelineSettings();
|
||||
private static final ColoredShapePipelineSettings COLORED_SHAPE_PIPELINE_SETTINGS =
|
||||
new ColoredShapePipelineSettings();
|
||||
|
||||
static {
|
||||
TestUtils.loadLibraries();
|
||||
configMgr = ConfigManager.getInstance();
|
||||
}
|
||||
|
||||
@BeforeAll
|
||||
public static void init() {
|
||||
TestUtils.loadLibraries();
|
||||
Logger.setLevel(LogGroup.General, Level.DE_PEST);
|
||||
|
||||
REFLECTIVE_PIPELINE_SETTINGS.pipelineNickname = "2019Tape";
|
||||
REFLECTIVE_PIPELINE_SETTINGS.targetModel = TargetModel.get2019Target();
|
||||
|
||||
COLORED_SHAPE_PIPELINE_SETTINGS.pipelineNickname = "2019Cargo";
|
||||
COLORED_SHAPE_PIPELINE_SETTINGS.pipelineIndex = 1;
|
||||
|
||||
cameraConfig.addPipelineSetting(REFLECTIVE_PIPELINE_SETTINGS);
|
||||
cameraConfig.addPipelineSetting(COLORED_SHAPE_PIPELINE_SETTINGS);
|
||||
}
|
||||
|
||||
@Test
|
||||
@Order(1)
|
||||
public void serializeConfig() throws IOException {
|
||||
TestUtils.loadLibraries();
|
||||
JacksonUtils.serializer(Path.of("settings.json"), REFLECTIVE_PIPELINE_SETTINGS);
|
||||
|
||||
Logger.setLevel(LogGroup.General, Level.DE_PEST);
|
||||
configMgr.getConfig().addCameraConfig(cameraConfig);
|
||||
configMgr.save();
|
||||
|
||||
var camConfDir =
|
||||
new File(
|
||||
Path.of(ConfigManager.getRootFolder().toString(), "cameras", "TestCamera")
|
||||
.toAbsolutePath()
|
||||
.toString());
|
||||
Assertions.assertTrue(camConfDir.exists(), "TestCamera config folder not found!");
|
||||
|
||||
Assertions.assertTrue(
|
||||
Files.exists(Path.of(ConfigManager.getRootFolder().toString(), "hardwareConfig.json")),
|
||||
"hardwareConfig.json file not found!");
|
||||
Assertions.assertTrue(
|
||||
Files.exists(Path.of(ConfigManager.getRootFolder().toString(), "networkSettings.json")),
|
||||
"networkSettings.json file not found!");
|
||||
}
|
||||
|
||||
@Test
|
||||
@Order(2)
|
||||
public void deserializeConfig() {
|
||||
configMgr.load();
|
||||
|
||||
var reflectivePipelineSettings =
|
||||
configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(0);
|
||||
var coloredShapePipelineSettings =
|
||||
configMgr.getConfig().getCameraConfigurations().get("TestCamera").pipelineSettings.get(1);
|
||||
|
||||
Assertions.assertEquals(REFLECTIVE_PIPELINE_SETTINGS, reflectivePipelineSettings);
|
||||
Assertions.assertEquals(COLORED_SHAPE_PIPELINE_SETTINGS, coloredShapePipelineSettings);
|
||||
|
||||
Assertions.assertTrue(
|
||||
reflectivePipelineSettings instanceof ReflectivePipelineSettings,
|
||||
"Conig loaded pipeline settings for index 0 not of expected type ReflectivePipelineSettings!");
|
||||
Assertions.assertTrue(
|
||||
coloredShapePipelineSettings instanceof ColoredShapePipelineSettings,
|
||||
"Conig loaded pipeline settings for index 1 not of expected type ColoredShapePipelineSettings!");
|
||||
}
|
||||
|
||||
@AfterAll
|
||||
public static void cleanup() {
|
||||
try {
|
||||
Files.deleteIfExists(Paths.get("settings.json"));
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
new File(ConfigManager.getRootFolder().toAbsolutePath().toString()).delete();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package org.photonvision.common.vision;
|
||||
|
||||
import org.photonvision.common.vision.camera.CameraQuirks;
|
||||
import org.photonvision.common.vision.camera.QuirkyCamera;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
public class QuirkyCameraTest {
|
||||
@Test
|
||||
public void ps3EyeTest() {
|
||||
QuirkyCamera psEye = new QuirkyCamera(0x1415, 0x2000, "psEye");
|
||||
Assertions.assertEquals(psEye.quirks, List.of(CameraQuirks.Gain));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void quirklessCameraTest() {
|
||||
QuirkyCamera noQuirk = new QuirkyCamera(1234, 888, "empty");
|
||||
Assertions.assertEquals(noQuirk.quirks, new ArrayList<>());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,94 @@
|
||||
package org.photonvision.common.vision.frame.provider;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.*;
|
||||
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.vision.frame.Frame;
|
||||
import edu.wpi.cscore.CameraServerCvJNI;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Paths;
|
||||
import org.junit.jupiter.api.BeforeAll;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
public class FileFrameProviderTest {
|
||||
|
||||
@BeforeAll
|
||||
public static void initPath() {
|
||||
|
||||
try {
|
||||
CameraServerCvJNI.forceLoad();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
public void TestFilesExist() {
|
||||
assertTrue(Files.exists(TestUtils.getTestImagesPath()));
|
||||
}
|
||||
|
||||
@Test
|
||||
public void Load2019ImageOnceTest() {
|
||||
var goodFilePath = TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in);
|
||||
|
||||
assertTrue(Files.exists(goodFilePath));
|
||||
|
||||
FileFrameProvider goodFrameProvider = new FileFrameProvider(goodFilePath, 68.5);
|
||||
|
||||
Frame goodFrame = goodFrameProvider.get();
|
||||
|
||||
int goodFrameCols = goodFrame.image.getMat().cols();
|
||||
int goodFrameRows = goodFrame.image.getMat().rows();
|
||||
|
||||
// 2019 Images are at 320x240
|
||||
assertEquals(320, goodFrameCols);
|
||||
assertEquals(240, goodFrameRows);
|
||||
|
||||
TestUtils.showImage(goodFrame.image.getMat(), "2019");
|
||||
|
||||
var badFilePath = Paths.get("bad.jpg"); // this file does not exist
|
||||
|
||||
FileFrameProvider badFrameProvider = null;
|
||||
|
||||
try {
|
||||
badFrameProvider = new FileFrameProvider(badFilePath, 68.5);
|
||||
} catch (Exception e) {
|
||||
// ignored
|
||||
}
|
||||
|
||||
assertNull(badFrameProvider);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void Load2020ImageOnceTest() {
|
||||
var goodFilePath = TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center);
|
||||
|
||||
assertTrue(Files.exists(goodFilePath));
|
||||
|
||||
FileFrameProvider goodFrameProvider = new FileFrameProvider(goodFilePath, 68.5);
|
||||
|
||||
Frame goodFrame = goodFrameProvider.get();
|
||||
|
||||
int goodFrameCols = goodFrame.image.getMat().cols();
|
||||
int goodFrameRows = goodFrame.image.getMat().rows();
|
||||
|
||||
// 2020 Images are at 640x480
|
||||
assertEquals(640, goodFrameCols);
|
||||
assertEquals(480, goodFrameRows);
|
||||
|
||||
TestUtils.showImage(goodFrame.image.getMat(), "2020");
|
||||
|
||||
var badFilePath = Paths.get("bad.jpg"); // this file does not exist
|
||||
|
||||
FileFrameProvider badFrameProvider = null;
|
||||
|
||||
try {
|
||||
badFrameProvider = new FileFrameProvider(badFilePath, 68.5);
|
||||
} catch (Exception e) {
|
||||
// ignored
|
||||
}
|
||||
|
||||
assertNull(badFrameProvider);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
package org.photonvision.common.vision.opencv;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertTrue;
|
||||
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.opencv.core.MatOfPoint;
|
||||
import org.opencv.core.Point;
|
||||
|
||||
public class ContourTest {
|
||||
@BeforeEach
|
||||
public void Init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
@Test
|
||||
public void simpleContourTest() {
|
||||
var mat = new MatOfPoint();
|
||||
mat.fromList(List.of(new Point(0, 0), new Point(10, 0), new Point(10, 10), new Point(0, 10)));
|
||||
var contour = new Contour(mat);
|
||||
assertEquals(100, contour.getArea());
|
||||
assertEquals(40, contour.getPerimeter());
|
||||
assertEquals(new Point(5, 5), contour.getCenterPoint());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void test2019() {
|
||||
var firstMat = new MatOfPoint();
|
||||
// contour 0 and 1 data from kCargoStraightDark72in_HighRes
|
||||
firstMat.fromList(
|
||||
List.of(
|
||||
new Point(1328, 976),
|
||||
new Point(1272, 985),
|
||||
new Point(1230, 832),
|
||||
new Point(1326, 948),
|
||||
new Point(1328, 971)));
|
||||
|
||||
var secondMat = new MatOfPoint();
|
||||
secondMat.fromList(
|
||||
List.of(
|
||||
new Point(956, 832),
|
||||
new Point(882, 978),
|
||||
new Point(927, 810),
|
||||
new Point(954, 821),
|
||||
new Point(956, 825)));
|
||||
var firstContour = new Contour(firstMat);
|
||||
var secondContour = new Contour(secondMat);
|
||||
boolean result = firstContour.isIntersecting(secondContour, ContourIntersectionDirection.Up);
|
||||
assertTrue(result);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,164 @@
|
||||
package org.photonvision.common.vision.pipeline;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotNull;
|
||||
|
||||
import org.photonvision.common.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.vision.frame.Frame;
|
||||
import org.photonvision.common.vision.frame.provider.FileFrameProvider;
|
||||
import org.photonvision.common.vision.opencv.CVMat;
|
||||
import org.photonvision.common.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
|
||||
import org.photonvision.common.vision.opencv.ContourShape;
|
||||
import org.photonvision.common.vision.target.TargetModel;
|
||||
import org.photonvision.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.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
public class CirclePNPTest {
|
||||
|
||||
private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
|
||||
private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
|
||||
|
||||
@BeforeEach
|
||||
public void Init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
@Test
|
||||
public void loadCameraIntrinsics() {
|
||||
var lifecam240pCal = getCoeffs(LIFECAM_240P_CAL_FILE);
|
||||
var lifecam480pCal = getCoeffs(LIFECAM_480P_CAL_FILE);
|
||||
|
||||
assertNotNull(lifecam240pCal);
|
||||
checkCameraCoefficients(lifecam240pCal);
|
||||
assertNotNull(lifecam480pCal);
|
||||
checkCameraCoefficients(lifecam480pCal);
|
||||
}
|
||||
|
||||
private CameraCalibrationCoefficients getCoeffs(String filename) {
|
||||
try {
|
||||
var cameraCalibration =
|
||||
new ObjectMapper()
|
||||
.readValue(
|
||||
(Path.of(TestUtils.getCalibrationPath().toString(), filename).toFile()),
|
||||
CameraCalibrationCoefficients.class);
|
||||
|
||||
checkCameraCoefficients(cameraCalibration);
|
||||
|
||||
return cameraCalibration;
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibration) {
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.rows);
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.cols);
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows());
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols());
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows());
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols());
|
||||
assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows());
|
||||
assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols());
|
||||
assertEquals(1, cameraCalibration.cameraExtrinsics.rows);
|
||||
assertEquals(5, cameraCalibration.cameraExtrinsics.cols);
|
||||
assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMat().rows());
|
||||
assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMat().cols());
|
||||
assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows());
|
||||
assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols());
|
||||
assertEquals(1, cameraCalibration.getCameraExtrinsicsMat().rows());
|
||||
assertEquals(5, cameraCalibration.getCameraExtrinsicsMat().cols());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void testCircle() {
|
||||
var pipeline = new ColoredShapePipeline();
|
||||
|
||||
pipeline.getSettings().hsvHue.set(0, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(100, 255);
|
||||
pipeline.getSettings().hsvValue.set(100, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
pipeline.getSettings().maxCannyThresh = 50;
|
||||
pipeline.getSettings().accuracy = 15;
|
||||
pipeline.getSettings().allowableThreshold = 5;
|
||||
pipeline.getSettings().solvePNPEnabled = true;
|
||||
pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
|
||||
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
|
||||
pipeline.getSettings().cameraCalibration = getCoeffs(LIFECAM_480P_CAL_FILE);
|
||||
pipeline.getSettings().targetModel = TargetModel.getCircleTarget(7);
|
||||
pipeline.getSettings().cameraPitch = Rotation2d.fromDegrees(0.0);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
pipeline.getSettings().outputShowMultipleTargets = false;
|
||||
pipeline.getSettings().contourGroupingMode = ContourGroupingMode.Single;
|
||||
pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up;
|
||||
pipeline.getSettings().desiredShape = ContourShape.Circle;
|
||||
pipeline.getSettings().allowableThreshold = 10;
|
||||
pipeline.getSettings().minRadius = 30;
|
||||
pipeline.getSettings().accuracyPercentage = 30.0;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6),
|
||||
TestUtils.WPI2020Image.FOV);
|
||||
|
||||
CVPipelineResult pipelineResult = pipeline.run(frameProvider.get());
|
||||
printTestResults(pipelineResult);
|
||||
|
||||
TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
|
||||
}
|
||||
|
||||
private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
pipeline.settings = settings;
|
||||
|
||||
while (true) {
|
||||
CVPipelineResult pipelineResult = pipeline.run(frame);
|
||||
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, which 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.get(), 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()));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
package org.photonvision.common.vision.pipeline;
|
||||
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.vision.frame.Frame;
|
||||
import org.photonvision.common.vision.frame.FrameStaticProperties;
|
||||
import org.photonvision.common.vision.frame.provider.FileFrameProvider;
|
||||
import org.photonvision.common.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
|
||||
import org.photonvision.common.vision.opencv.ContourShape;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
public class ColoredShapePipelineTest {
|
||||
|
||||
public static void testTriangleDetection(
|
||||
ColoredShapePipeline pipeline,
|
||||
ColoredShapePipelineSettings settings,
|
||||
FrameStaticProperties frameStaticProperties,
|
||||
Frame frame) {
|
||||
pipeline.setPipeParams(frameStaticProperties, settings);
|
||||
CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
|
||||
TestUtils.showImage(
|
||||
colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Triangle.");
|
||||
printTestResults(colouredShapePipelineResult);
|
||||
}
|
||||
|
||||
public static void testQuadrilateralDetection(
|
||||
ColoredShapePipeline pipeline,
|
||||
ColoredShapePipelineSettings settings,
|
||||
FrameStaticProperties frameStaticProperties,
|
||||
Frame frame) {
|
||||
settings.desiredShape = ContourShape.Quadrilateral;
|
||||
pipeline.setPipeParams(frameStaticProperties, settings);
|
||||
CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
|
||||
TestUtils.showImage(
|
||||
colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Quadrilateral.");
|
||||
printTestResults(colouredShapePipelineResult);
|
||||
}
|
||||
|
||||
public static void testCustomShapeDetection(
|
||||
ColoredShapePipeline pipeline,
|
||||
ColoredShapePipelineSettings settings,
|
||||
FrameStaticProperties frameStaticProperties,
|
||||
Frame frame) {
|
||||
settings.desiredShape = ContourShape.Custom;
|
||||
pipeline.setPipeParams(frameStaticProperties, settings);
|
||||
CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
|
||||
TestUtils.showImage(
|
||||
colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Custom.");
|
||||
printTestResults(colouredShapePipelineResult);
|
||||
}
|
||||
|
||||
@Test
|
||||
public static void testCircleShapeDetection(
|
||||
ColoredShapePipeline pipeline,
|
||||
ColoredShapePipelineSettings settings,
|
||||
FrameStaticProperties frameStaticProperties,
|
||||
Frame frame) {
|
||||
settings.desiredShape = ContourShape.Circle;
|
||||
pipeline.setPipeParams(frameStaticProperties, settings);
|
||||
CVPipelineResult colouredShapePipelineResult = pipeline.run(frame);
|
||||
TestUtils.showImage(
|
||||
colouredShapePipelineResult.outputFrame.image.getMat(), "Pipeline output: Circle.");
|
||||
printTestResults(colouredShapePipelineResult);
|
||||
}
|
||||
|
||||
@Test
|
||||
public static void testPowercellDetection(
|
||||
ColoredShapePipelineSettings settings, ColoredShapePipeline pipeline) {
|
||||
|
||||
settings.hsvHue.set(10, 40);
|
||||
settings.hsvSaturation.set(100, 255);
|
||||
settings.hsvValue.set(100, 255);
|
||||
settings.maxCannyThresh = 50;
|
||||
settings.accuracy = 15;
|
||||
settings.allowableThreshold = 5;
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getPowercellImagePath(TestUtils.PowercellTestImages.kPowercell_test_6),
|
||||
TestUtils.WPI2019Image.FOV);
|
||||
testCircleShapeDetection(
|
||||
pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
|
||||
}
|
||||
|
||||
public static void main(String[] args) {
|
||||
TestUtils.loadLibraries();
|
||||
System.out.println(TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center));
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getPolygonImagePath(TestUtils.PolygonTestImages.kPolygons),
|
||||
TestUtils.WPI2019Image.FOV);
|
||||
var settings = new ColoredShapePipelineSettings();
|
||||
settings.hsvHue.set(0, 100);
|
||||
settings.hsvSaturation.set(100, 255);
|
||||
settings.hsvValue.set(100, 255);
|
||||
settings.outputShowThresholded = true;
|
||||
settings.outputShowMultipleTargets = true;
|
||||
settings.contourGroupingMode = ContourGroupingMode.Single;
|
||||
settings.contourIntersection = ContourIntersectionDirection.Up;
|
||||
settings.desiredShape = ContourShape.Triangle;
|
||||
settings.allowableThreshold = 10;
|
||||
settings.accuracyPercentage = 30.0;
|
||||
|
||||
ColoredShapePipeline pipeline = new ColoredShapePipeline();
|
||||
testTriangleDetection(
|
||||
pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
|
||||
testQuadrilateralDetection(
|
||||
pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
|
||||
testCustomShapeDetection(
|
||||
pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
|
||||
testCircleShapeDetection(
|
||||
pipeline, settings, frameProvider.get().frameStaticProperties, frameProvider.get());
|
||||
testPowercellDetection(settings, pipeline);
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
package org.photonvision.common.vision.pipeline;
|
||||
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.vision.frame.Frame;
|
||||
import org.photonvision.common.vision.frame.provider.FileFrameProvider;
|
||||
import org.photonvision.common.vision.opencv.CVMat;
|
||||
import org.photonvision.common.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
|
||||
import org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
public class ReflectivePipelineTest {
|
||||
|
||||
@Test
|
||||
public void test2019() {
|
||||
TestUtils.loadLibraries();
|
||||
var pipeline = new ReflectivePipeline();
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(100, 255);
|
||||
pipeline.getSettings().hsvValue.set(190, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
pipeline.getSettings().outputShowMultipleTargets = true;
|
||||
pipeline.getSettings().contourGroupingMode = ContourGroupingMode.Dual;
|
||||
pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes),
|
||||
TestUtils.WPI2019Image.FOV);
|
||||
|
||||
TestUtils.showImage(frameProvider.get().image.getMat(), "Pipeline input", 1);
|
||||
|
||||
CVPipelineResult pipelineResult;
|
||||
|
||||
pipelineResult = pipeline.run(frameProvider.get());
|
||||
printTestResults(pipelineResult);
|
||||
|
||||
Assertions.assertTrue(pipelineResult.hasTargets());
|
||||
Assertions.assertEquals(2, pipelineResult.targets.size(), "Target count wrong!");
|
||||
|
||||
TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output");
|
||||
}
|
||||
|
||||
@Test
|
||||
public void test2020() {
|
||||
TestUtils.loadLibraries();
|
||||
var pipeline = new ReflectivePipeline();
|
||||
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(200, 255);
|
||||
pipeline.getSettings().hsvValue.set(200, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_108in_Center),
|
||||
TestUtils.WPI2020Image.FOV);
|
||||
|
||||
CVPipelineResult pipelineResult = pipeline.run(frameProvider.get());
|
||||
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);
|
||||
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.get(), 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");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,215 @@
|
||||
package org.photonvision.common.vision.pipeline;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
import static org.junit.jupiter.api.Assertions.assertNotNull;
|
||||
|
||||
import org.photonvision.common.calibration.CameraCalibrationCoefficients;
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.vision.frame.Frame;
|
||||
import org.photonvision.common.vision.frame.provider.FileFrameProvider;
|
||||
import org.photonvision.common.vision.opencv.CVMat;
|
||||
import org.photonvision.common.vision.opencv.ContourGroupingMode;
|
||||
import org.photonvision.common.vision.opencv.ContourIntersectionDirection;
|
||||
import org.photonvision.common.vision.target.TargetModel;
|
||||
import org.photonvision.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.Assertions;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
public class SolvePNPTest {
|
||||
|
||||
private static final String LIFECAM_240P_CAL_FILE = "lifecam240p.json";
|
||||
private static final String LIFECAM_480P_CAL_FILE = "lifecam480p.json";
|
||||
|
||||
@BeforeEach
|
||||
public void Init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
@Test
|
||||
public void loadCameraIntrinsics() {
|
||||
var lifecam240pCal = getCoeffs(LIFECAM_240P_CAL_FILE);
|
||||
var lifecam480pCal = getCoeffs(LIFECAM_480P_CAL_FILE);
|
||||
|
||||
assertNotNull(lifecam240pCal);
|
||||
checkCameraCoefficients(lifecam240pCal);
|
||||
assertNotNull(lifecam480pCal);
|
||||
checkCameraCoefficients(lifecam480pCal);
|
||||
}
|
||||
|
||||
private CameraCalibrationCoefficients getCoeffs(String filename) {
|
||||
try {
|
||||
var cameraCalibration =
|
||||
new ObjectMapper()
|
||||
.readValue(
|
||||
(Path.of(TestUtils.getCalibrationPath().toString(), filename).toFile()),
|
||||
CameraCalibrationCoefficients.class);
|
||||
|
||||
checkCameraCoefficients(cameraCalibration);
|
||||
|
||||
return cameraCalibration;
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private void checkCameraCoefficients(CameraCalibrationCoefficients cameraCalibration) {
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.rows);
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.cols);
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().rows());
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMat().cols());
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().rows());
|
||||
assertEquals(3, cameraCalibration.cameraIntrinsics.getAsMatOfDouble().cols());
|
||||
assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().rows());
|
||||
assertEquals(3, cameraCalibration.getCameraIntrinsicsMat().cols());
|
||||
assertEquals(1, cameraCalibration.cameraExtrinsics.rows);
|
||||
assertEquals(5, cameraCalibration.cameraExtrinsics.cols);
|
||||
assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMat().rows());
|
||||
assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMat().cols());
|
||||
assertEquals(1, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().rows());
|
||||
assertEquals(5, cameraCalibration.cameraExtrinsics.getAsMatOfDouble().cols());
|
||||
assertEquals(1, cameraCalibration.getCameraExtrinsicsMat().rows());
|
||||
assertEquals(5, cameraCalibration.getCameraExtrinsicsMat().cols());
|
||||
}
|
||||
|
||||
@Test
|
||||
public void test2019() {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(100, 255);
|
||||
pipeline.getSettings().hsvValue.set(190, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
pipeline.getSettings().outputShowMultipleTargets = true;
|
||||
pipeline.getSettings().solvePNPEnabled = true;
|
||||
pipeline.getSettings().contourGroupingMode = ContourGroupingMode.Dual;
|
||||
pipeline.getSettings().contourIntersection = ContourIntersectionDirection.Up;
|
||||
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
|
||||
pipeline.getSettings().targetModel = TargetModel.get2019Target();
|
||||
pipeline.getSettings().cameraCalibration = getCoeffs(LIFECAM_240P_CAL_FILE);
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark48in),
|
||||
TestUtils.WPI2019Image.FOV);
|
||||
|
||||
CVPipelineResult pipelineResult;
|
||||
|
||||
pipelineResult = pipeline.run(frameProvider.get());
|
||||
printTestResults(pipelineResult);
|
||||
|
||||
// these numbers are not *accurate*, but they are known and expected
|
||||
var pose = pipelineResult.targets.get(0).getRobotRelativePose();
|
||||
Assertions.assertEquals(41.96, pose.getTranslation().getX(), 0.05);
|
||||
Assertions.assertEquals(-1.03, pose.getTranslation().getY(), 0.05);
|
||||
Assertions.assertEquals(1.46, pose.getRotation().getDegrees(), 0.05);
|
||||
|
||||
TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 1000 * 90);
|
||||
}
|
||||
|
||||
@Test
|
||||
public void test2020() {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
|
||||
pipeline.getSettings().hsvHue.set(60, 100);
|
||||
pipeline.getSettings().hsvSaturation.set(100, 255);
|
||||
pipeline.getSettings().hsvValue.set(60, 255);
|
||||
pipeline.getSettings().outputShowThresholded = true;
|
||||
pipeline.getSettings().solvePNPEnabled = true;
|
||||
pipeline.getSettings().cornerDetectionAccuracyPercentage = 4;
|
||||
pipeline.getSettings().cornerDetectionUseConvexHulls = true;
|
||||
pipeline.getSettings().cameraCalibration = getCoeffs(LIFECAM_480P_CAL_FILE);
|
||||
pipeline.getSettings().targetModel = TargetModel.get2020Target(36);
|
||||
pipeline.getSettings().cameraPitch = Rotation2d.fromDegrees(0.0);
|
||||
|
||||
var frameProvider =
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2020Image.kBlueGoal_224in_Left),
|
||||
TestUtils.WPI2020Image.FOV);
|
||||
|
||||
CVPipelineResult pipelineResult = pipeline.run(frameProvider.get());
|
||||
printTestResults(pipelineResult);
|
||||
|
||||
// these numbers are not *accurate*, but they are known and expected
|
||||
var pose = pipelineResult.targets.get(0).getRobotRelativePose();
|
||||
Assertions.assertEquals(260.26, pose.getTranslation().getX(), 0.05);
|
||||
Assertions.assertEquals(64.26, pose.getTranslation().getY(), 0.05);
|
||||
Assertions.assertEquals(36.88, pose.getRotation().getDegrees(), 0.05);
|
||||
|
||||
TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output", 999999);
|
||||
}
|
||||
|
||||
// @Test
|
||||
// public void junk() {
|
||||
// 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 continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) {
|
||||
var pipeline = new ReflectivePipeline();
|
||||
pipeline.settings = settings;
|
||||
|
||||
while (true) {
|
||||
CVPipelineResult pipelineResult = pipeline.run(frame);
|
||||
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, which 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.get(), 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()));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
package org.photonvision.common.vision.processes;
|
||||
|
||||
import org.photonvision.common.configuration.CameraConfiguration;
|
||||
import org.photonvision.common.datatransfer.DataConsumer;
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.vision.frame.FrameProvider;
|
||||
import org.photonvision.common.vision.frame.provider.FileFrameProvider;
|
||||
import org.photonvision.common.vision.pipeline.CVPipelineResult;
|
||||
import edu.wpi.cscore.VideoMode;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import org.junit.jupiter.api.*;
|
||||
|
||||
public class VisionModuleManagerTest {
|
||||
|
||||
@BeforeEach
|
||||
public void init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
private static class TestSource implements VisionSource {
|
||||
|
||||
private final FrameProvider provider;
|
||||
|
||||
public TestSource(FrameProvider provider) {
|
||||
|
||||
this.provider = provider;
|
||||
}
|
||||
|
||||
@Override
|
||||
public FrameProvider getFrameProvider() {
|
||||
return provider;
|
||||
}
|
||||
|
||||
@Override
|
||||
public VisionSourceSettables getSettables() {
|
||||
return new TestSettables(new CameraConfiguration("", "", "", ""));
|
||||
}
|
||||
}
|
||||
|
||||
private static class TestSettables extends VisionSourceSettables {
|
||||
|
||||
protected TestSettables(CameraConfiguration configuration) {
|
||||
super(configuration);
|
||||
}
|
||||
|
||||
@Override
|
||||
public int getExposure() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setExposure(int exposure) {}
|
||||
|
||||
@Override
|
||||
public int getBrightness() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setBrightness(int brightness) {}
|
||||
|
||||
@Override
|
||||
public int getGain() {
|
||||
return 0;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setGain(int gain) {}
|
||||
|
||||
@Override
|
||||
public VideoMode getCurrentVideoMode() {
|
||||
return null;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void setCurrentVideoMode(VideoMode videoMode) {}
|
||||
|
||||
@Override
|
||||
public HashMap<Integer, VideoMode> getAllVideoModes() {
|
||||
return null;
|
||||
}
|
||||
}
|
||||
|
||||
private static class TestDataConsumer implements DataConsumer {
|
||||
private Data data;
|
||||
|
||||
@Override
|
||||
public void accept(Data data) {
|
||||
this.data = data;
|
||||
}
|
||||
|
||||
public Data getData() {
|
||||
return data;
|
||||
}
|
||||
}
|
||||
|
||||
@Test
|
||||
public void setupManager() {
|
||||
var sources = new ArrayList<VisionSource>();
|
||||
sources.add(
|
||||
new TestSource(
|
||||
new FileFrameProvider(
|
||||
TestUtils.getWPIImagePath(TestUtils.WPI2019Image.kCargoStraightDark72in_HighRes),
|
||||
TestUtils.WPI2019Image.FOV)));
|
||||
|
||||
var moduleManager = new VisionModuleManager(sources);
|
||||
var module0DataConsumer = new TestDataConsumer();
|
||||
|
||||
moduleManager.visionModules.get(0).addDataConsumer(module0DataConsumer);
|
||||
|
||||
moduleManager.startModules();
|
||||
|
||||
sleep(500);
|
||||
|
||||
Assertions.assertNotNull(module0DataConsumer.data);
|
||||
Assertions.assertNotNull(module0DataConsumer.data.result);
|
||||
printTestResults(module0DataConsumer.data.result);
|
||||
}
|
||||
|
||||
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");
|
||||
}
|
||||
|
||||
private void sleep(int millis) {
|
||||
try {
|
||||
Thread.sleep(millis);
|
||||
} catch (InterruptedException e) {
|
||||
// ignored
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
package org.photonvision.common.vision.processes;
|
||||
|
||||
import org.photonvision.common.configuration.CameraConfiguration;
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.vision.camera.USBCameraSource;
|
||||
import edu.wpi.cscore.UsbCameraInfo;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.Assertions;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
|
||||
public class VisionSourceManagerTest {
|
||||
@BeforeEach
|
||||
public void init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
final List<UsbCameraInfo> usbCameraInfos =
|
||||
List.of(
|
||||
new UsbCameraInfo(0, "/this-is-a-real-path", "cameraByPath", new String[] {""}, 1, 1),
|
||||
new UsbCameraInfo(2, "/this-is-a-fake-path1", "cameraById", new String[] {""}, 420, 1),
|
||||
new UsbCameraInfo(1, "/this-is-a-real-path2", "cameraByPath", new String[] {""}, 1, 1),
|
||||
new UsbCameraInfo(3, "/this-is-a-fake-path2", "cameraById", new String[] {""}, 420, 1),
|
||||
new UsbCameraInfo(4, "/fake-path420", "notExisting", new String[] {""}, 420, 1),
|
||||
new UsbCameraInfo(5, "/fake-path421", "notExisting", new String[] {""}, 420, 1));
|
||||
|
||||
final List<CameraConfiguration> camConfig =
|
||||
List.of(
|
||||
new CameraConfiguration("cameraByPath", "dank meme", "good name", "/this-is-a-real-path"),
|
||||
new CameraConfiguration(
|
||||
"cameraByPath", "dank meme2", "very original", "/this-is-a-real-path2"),
|
||||
new CameraConfiguration("cameraById", "camera", "my camera", "2"),
|
||||
new CameraConfiguration("cameraById", "camera2", "my camera", "3"));
|
||||
|
||||
final List<USBCameraSource> usbCameraSources =
|
||||
List.of(
|
||||
new USBCameraSource(camConfig.get(0)),
|
||||
new USBCameraSource(camConfig.get(1)),
|
||||
new USBCameraSource(camConfig.get(2)),
|
||||
new USBCameraSource(camConfig.get(3)),
|
||||
new USBCameraSource(
|
||||
new CameraConfiguration("notExisting", "notExisting", "notExisting", "4")),
|
||||
new USBCameraSource(
|
||||
new CameraConfiguration("notExisting", "notExisting (1)", "notExisting (1)", "5")));
|
||||
|
||||
@Test
|
||||
public void visionSourceTest() {
|
||||
VisionSourceManager visionSourceManager = new VisionSourceManager();
|
||||
List<VisionSource> i = visionSourceManager.LoadAllSources(camConfig, usbCameraInfos);
|
||||
for (var source : i) {
|
||||
Assertions.assertEquals(source, usbCameraSources.get(i.indexOf(source)));
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
package org.photonvision.common.vision.target;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.opencv.core.*;
|
||||
|
||||
public class TargetCalculationsTest {
|
||||
|
||||
private static Size imageSize = new Size(800, 600);
|
||||
private static Point imageCenterPoint = new Point(imageSize.width / 2, imageSize.height / 2);
|
||||
private static double CameraHorizontalFocalLength = 61;
|
||||
private static double CameraVerticalFocalLength = 34.3;
|
||||
|
||||
@BeforeEach
|
||||
public void Init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
@Test
|
||||
public void yawTest() {
|
||||
var targetPixelOffsetX = 100;
|
||||
var targetCenterPoint = new Point(imageCenterPoint.x + targetPixelOffsetX, imageCenterPoint.y);
|
||||
|
||||
var yaw =
|
||||
TargetCalculations.calculateYaw(
|
||||
imageCenterPoint.x, targetCenterPoint.x, CameraHorizontalFocalLength);
|
||||
|
||||
assertEquals(-1.466, yaw, 0.025, "Yaw not as expected");
|
||||
}
|
||||
|
||||
@Test
|
||||
public void pitchTest() {
|
||||
var targetPixelOffsetY = 100;
|
||||
var targetCenterPoint = new Point(imageCenterPoint.x, imageCenterPoint.y + targetPixelOffsetY);
|
||||
|
||||
var pitch =
|
||||
TargetCalculations.calculatePitch(
|
||||
imageCenterPoint.y, targetCenterPoint.y, CameraVerticalFocalLength);
|
||||
|
||||
assertEquals(2.607, pitch, 0.025, "Pitch not as expected");
|
||||
}
|
||||
|
||||
@Test
|
||||
public void targetOffsetTest() {
|
||||
Point center = new Point(0, 0);
|
||||
Size rectSize = new Size(10, 5);
|
||||
double angle = 30;
|
||||
RotatedRect rect = new RotatedRect(center, rectSize, angle);
|
||||
Point result =
|
||||
TargetCalculations.calculateTargetOffsetPoint(false, TargetOffsetPointEdge.Top, rect);
|
||||
assertEquals(4.3, result.x, 0.33, "Target offset x not as expected");
|
||||
assertEquals(2.5, result.y, 0.05, "Target offset Y not as expected");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,54 @@
|
||||
package org.photonvision.common.vision.target;
|
||||
|
||||
import static org.junit.jupiter.api.Assertions.assertEquals;
|
||||
|
||||
import org.photonvision.common.util.TestUtils;
|
||||
import org.photonvision.common.util.numbers.DoubleCouple;
|
||||
import org.photonvision.common.vision.opencv.Contour;
|
||||
import java.util.List;
|
||||
import org.junit.jupiter.api.BeforeEach;
|
||||
import org.junit.jupiter.api.Test;
|
||||
import org.opencv.core.Mat;
|
||||
import org.opencv.core.MatOfPoint;
|
||||
import org.opencv.core.Point;
|
||||
import org.opencv.core.Size;
|
||||
|
||||
public class TrackedTargetTest {
|
||||
@BeforeEach
|
||||
public void Init() {
|
||||
TestUtils.loadLibraries();
|
||||
}
|
||||
|
||||
@Test
|
||||
void axisTest() {
|
||||
Mat background = new Mat();
|
||||
|
||||
MatOfPoint mat = new MatOfPoint();
|
||||
mat.fromList(
|
||||
List.of(
|
||||
new Point(400, 298),
|
||||
new Point(426.22, 298),
|
||||
new Point(426.22, 302),
|
||||
new Point(400, 302))); // gives contour with center of 426, 300
|
||||
Contour contour = new Contour(mat);
|
||||
var pTarget = new PotentialTarget(contour);
|
||||
|
||||
var imageSize = new Size(800, 600);
|
||||
|
||||
var setting =
|
||||
new TrackedTarget.TargetCalculationParameters(
|
||||
false,
|
||||
TargetOffsetPointEdge.Center,
|
||||
new Point(0, 0),
|
||||
new Point(imageSize.width / 2, imageSize.height / 2),
|
||||
new DoubleCouple(0.0, 0.0),
|
||||
RobotOffsetPointMode.None,
|
||||
61,
|
||||
34.3,
|
||||
imageSize.area());
|
||||
|
||||
var trackedTarget = new TrackedTarget(pTarget, setting);
|
||||
assertEquals(1.4, trackedTarget.getYaw(), 0.025, "Yaw was incorrect");
|
||||
assertEquals(0, trackedTarget.getPitch(), 0.025, "Pitch was incorrect");
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user