From e652b429122d56931e01c1633dc370e1ac8c5286 Mon Sep 17 00:00:00 2001 From: Banks T Date: Mon, 25 May 2020 15:50:34 -0400 Subject: [PATCH] Pipe unit tests (#97) * Merge to fix git history commit df76353dd5d4c9db7d4843c63bae2dcaf4a9b478 Author: Matt Date: Sun Apr 12 12:21:31 2020 -0700 Add skeleton Manager and Server Skeleton VisionModule/VisionModuleManager/Visionprocess Add blank temp data class, flesh out VisionModule more added server and socket handler template Create Logger class Run spotless Add async to Logger Revert "Add async to Logger" This reverts commit 130df7b895dc73db6e4c0b7b101d383a2952b49e. Add asyncronous file appending commit 621e3a829ddace486680a9198538cfad278076da Author: Matt Date: Sun Apr 12 09:04:52 2020 -0700 Add offset method to get2020Target commit 75e104770f36084a19fbf98e2470b937203e8dc0 Merge: 292071a 525cf52 Author: Matt Date: Sat Apr 11 21:33:12 2020 -0700 Merge branch 'logging' into pipeline-bringup commit 292071adefd90c85355859a5407e9ab786aa2436 Author: Matt Date: Sat Apr 11 21:32:25 2020 -0700 Update .gitignore commit a31f64fb65d42a81d1ecf0b4a8ebf03869c4ba03 Author: Matt Date: Sat Apr 11 21:31:17 2020 -0700 Refactor calibration into `common.calibration` commit eb91324a263025afdd7fd59a5001c9e95f3d3e6c Author: Matt Date: Sat Apr 11 16:48:52 2020 -0700 run spotless commit a6e3f0dda55964c8fb1819ad9386bb21de58cdf9 Author: Matt Date: Sat Apr 11 16:48:28 2020 -0700 Fix JSON mat bug and lifecam default calibration for tests, fix 3d drawing commit f2d5caea43747c80c9d7417b4ffda32dad95cd71 Author: Matt Date: Sat Apr 11 15:47:12 2020 -0700 Implement solvePNP, bounding box top and bottom commit bbf1ea445d72876d5e3c53ae3f41be68adf401ca Author: Matt Date: Sat Apr 11 15:46:32 2020 -0700 Add lifecam calibration data commit 82684920224e5f5ba2924106cbe09beb3d30d42e Author: Matt Date: Sat Apr 11 15:46:20 2020 -0700 Update geometry classes commit cab6a68184b74055ae088d59660ecd7ddff58400 Author: Matt Date: Sat Apr 11 12:23:04 2020 -0700 Clean up draw 3d, fix convex hull bug in corner detection commit bb3c247e398b72e93c77ece8c97929982edded3e Author: Matt Date: Sat Apr 11 11:44:57 2020 -0700 Update Contour.java commit 94c9b1dd342b17ceed5dca1020f1253cd6f59f62 Author: Matt Date: Sat Apr 11 11:41:05 2020 -0700 Make Draw2dContours pipe respect showMultiple commit 0f231888accb19d4018ee2a2c1ccee5e28b48f69 Author: Matt Date: Sat Apr 11 11:40:38 2020 -0700 Add point detection, fix convex hull calculation in Contour commit ef360ba7f814fad74b2f07025c08634f09eacf97 Author: Banks Troutman Date: Fri Apr 10 04:51:50 2020 -0400 Add ContourShape class for approxPolyDp Start on ColoredShape tracking commit a4e9dd3d4221085e76425e99496bc7d03536a689 Author: Matt Date: Thu Apr 9 20:53:14 2020 -0700 Create CornerDetectionPipe based on old solvePNPPipe commit 88cae18d63f8045a77e5c394af6b3f4d2fab98db Author: Banks Troutman Date: Thu Apr 9 20:19:57 2020 -0400 Add perimeter, MatOfPoint2f getters to Contour commit 525cf52ec4ac204f7996310fa3d12ad127362bf7 Author: Matt Date: Tue Mar 31 10:56:57 2020 -0700 Add slv4j logger to replace the current debugLogger I'm waiting on stuff to be less skeletoned to add more commit 50e70b907317e630ee66fb12b31d0b216e331156 Author: Matt Date: Tue Mar 31 10:40:42 2020 -0700 Add logback commit b739ba287829c07a13079434751214e4ad4e769f Author: Banks Troutman Date: Thu Apr 9 00:51:22 2020 -0400 various cleanups, add DummyFrameConsumer commit 91c36cb60ee4d40ac3715e3c7f4249e172b15e2b Author: Banks Troutman Date: Thu Apr 9 00:48:42 2020 -0400 Add CVMat, ReflectionUtils to help track rogue Mats commit 7999c9ee935052436d44ff51d64d60a266aad1fd Author: Matt Date: Wed Apr 8 21:48:02 2020 -0700 Apply spotless commit c39d9dc6fd13aedd2f684d8b2654f8a277ccf587 Author: Matt Date: Wed Apr 8 21:45:19 2020 -0700 Resolve memory leaks due to unnecessary instantiation of Points commit 7b507da3c860c2a68d70d968224648819695c489 Author: Matt Date: Wed Apr 8 21:29:53 2020 -0700 Fix ConcurrentModificationException bug in group contours pipe with potential targets commit d5c7b26f73f9c74bb2b29723f9987c3f66f8c288 Author: Banks Troutman Date: Wed Apr 8 03:49:10 2020 -0400 Refactor CVPipeline, add ReflectivePipelineTest commit 2e6a64862cc9c27faf30545cbf89053a4e9a9ed7 Author: Banks Troutman Date: Wed Apr 8 03:48:50 2020 -0400 add TestUtils class, move testimages commit 6653eb981224f4851cb2ad1219c6ebe08ca70b8f Author: Banks Troutman Date: Wed Apr 8 03:45:11 2020 -0400 add Releasable interface, implement in classes commit ef1e3024a1ef8fdfee58d4a8ef00b8aa96146721 Author: Banks Troutman Date: Tue Apr 7 19:53:41 2020 -0400 Move test images commit e25e736741d7432fca42a32f707099e62a1e5a14 Author: Banks Troutman Date: Tue Apr 7 18:42:43 2020 -0400 Apply Spotless commit ff5cee953f80b59b938df5c1a6e1bef4e9fb6573 Author: Banks Troutman Date: Tue Apr 7 01:52:06 2020 -0400 Finish ReflectivePipeline, various tweaks commit 7e6e65127a54ec784d048914fb49ca23b6ee4d29 Author: Banks Troutman Date: Tue Apr 7 01:49:14 2020 -0400 Add FrameStaticProperties as member in Frame Add FrameStaticProperties as member in Frame commit 09bf8cb500d89b8f3e11afe5e4d2c56d42ce18f9 Author: Banks Troutman Date: Mon Apr 6 17:45:25 2020 -0400 Add crosshair to DriverMode, cleanups commit 570531afb28e422f1e866454cfb8f5b745979ec9 Author: Banks Troutman Date: Mon Apr 6 14:10:39 2020 -0400 Add DriverPipeline classes, apply spotless commit 0376bdbdcb6bb9e468d353fff24d4c2ac1a0684a Author: ori agranat Date: Mon Apr 6 11:48:36 2020 +0300 updated Largest ContourSortMode and added centermost commit 378ba923c06b9a68f4db4972ef5f2b643d8051fd Author: Banks Troutman Date: Sun Apr 5 23:29:51 2020 -0400 Add pipeline classes, settings, separate enums commit 3b41afe125d8cf34e71bcd9c30d85f4f7dac30a8 Author: Banks Troutman Date: Sun Apr 5 23:29:32 2020 -0400 Refactor package structure, various cleanups * Apply spotless, attempt fix for SolvePNPTest * Fix SolvePNP Draw, fix 2019 PNP * ModuleManager bringup, spotless applied * Pipeline Test fixes and cleanups * Fix build.gradle test config, add FileFrameProvider test * further FileFrameProvider test work * added unit test before building jar * Update FileFrameProviderTest * Run tests as separate CircleCI task * Fix path for CircleCI test result and caching * More CircleCI futzery * Fix file paths and HeadlessException when testing on Linux * Fix reflective unit test * initial contour test and tracked target * Fix settings access in pipeline tests, apply spotless * Fix TrackedTarget calculations, moved to TargetCalculations and added tests * Rebased on 3.0 commit 47c2f8cab0369d6b87684a3aa2c4517535a5025f Author: Banks T Date: Mon May 25 14:55:03 2020 -0400 Module bringup (#95) * Merge to fix git history commit df76353dd5d4c9db7d4843c63bae2dcaf4a9b478 Author: Matt Date: Sun Apr 12 12:21:31 2020 -0700 Add skeleton Manager and Server Skeleton VisionModule/VisionModuleManager/Visionprocess Add blank temp data class, flesh out VisionModule more added server and socket handler template Create Logger class Run spotless Add async to Logger Revert "Add async to Logger" This reverts commit 130df7b895dc73db6e4c0b7b101d383a2952b49e. Add asyncronous file appending commit 621e3a829ddace486680a9198538cfad278076da Author: Matt Date: Sun Apr 12 09:04:52 2020 -0700 Add offset method to get2020Target commit 75e104770f36084a19fbf98e2470b937203e8dc0 Merge: 292071a 525cf52 Author: Matt Date: Sat Apr 11 21:33:12 2020 -0700 Merge branch 'logging' into pipeline-bringup commit 292071adefd90c85355859a5407e9ab786aa2436 Author: Matt Date: Sat Apr 11 21:32:25 2020 -0700 Update .gitignore commit a31f64fb65d42a81d1ecf0b4a8ebf03869c4ba03 Author: Matt Date: Sat Apr 11 21:31:17 2020 -0700 Refactor calibration into `common.calibration` commit eb91324a263025afdd7fd59a5001c9e95f3d3e6c Author: Matt Date: Sat Apr 11 16:48:52 2020 -0700 run spotless commit a6e3f0dda55964c8fb1819ad9386bb21de58cdf9 Author: Matt Date: Sat Apr 11 16:48:28 2020 -0700 Fix JSON mat bug and lifecam default calibration for tests, fix 3d drawing commit f2d5caea43747c80c9d7417b4ffda32dad95cd71 Author: Matt Date: Sat Apr 11 15:47:12 2020 -0700 Implement solvePNP, bounding box top and bottom commit bbf1ea445d72876d5e3c53ae3f41be68adf401ca Author: Matt Date: Sat Apr 11 15:46:32 2020 -0700 Add lifecam calibration data commit 82684920224e5f5ba2924106cbe09beb3d30d42e Author: Matt Date: Sat Apr 11 15:46:20 2020 -0700 Update geometry classes commit cab6a68184b74055ae088d59660ecd7ddff58400 Author: Matt Date: Sat Apr 11 12:23:04 2020 -0700 Clean up draw 3d, fix convex hull bug in corner detection commit bb3c247e398b72e93c77ece8c97929982edded3e Author: Matt Date: Sat Apr 11 11:44:57 2020 -0700 Update Contour.java commit 94c9b1dd342b17ceed5dca1020f1253cd6f59f62 Author: Matt Date: Sat Apr 11 11:41:05 2020 -0700 Make Draw2dContours pipe respect showMultiple commit 0f231888accb19d4018ee2a2c1ccee5e28b48f69 Author: Matt Date: Sat Apr 11 11:40:38 2020 -0700 Add point detection, fix convex hull calculation in Contour commit ef360ba7f814fad74b2f07025c08634f09eacf97 Author: Banks Troutman Date: Fri Apr 10 04:51:50 2020 -0400 Add ContourShape class for approxPolyDp Start on ColoredShape tracking commit a4e9dd3d4221085e76425e99496bc7d03536a689 Author: Matt Date: Thu Apr 9 20:53:14 2020 -0700 Create CornerDetectionPipe based on old solvePNPPipe commit 88cae18d63f8045a77e5c394af6b3f4d2fab98db Author: Banks Troutman Date: Thu Apr 9 20:19:57 2020 -0400 Add perimeter, MatOfPoint2f getters to Contour commit 525cf52ec4ac204f7996310fa3d12ad127362bf7 Author: Matt Date: Tue Mar 31 10:56:57 2020 -0700 Add slv4j logger to replace the current debugLogger I'm waiting on stuff to be less skeletoned to add more commit 50e70b907317e630ee66fb12b31d0b216e331156 Author: Matt Date: Tue Mar 31 10:40:42 2020 -0700 Add logback commit b739ba287829c07a13079434751214e4ad4e769f Author: Banks Troutman Date: Thu Apr 9 00:51:22 2020 -0400 various cleanups, add DummyFrameConsumer commit 91c36cb60ee4d40ac3715e3c7f4249e172b15e2b Author: Banks Troutman Date: Thu Apr 9 00:48:42 2020 -0400 Add CVMat, ReflectionUtils to help track rogue Mats commit 7999c9ee935052436d44ff51d64d60a266aad1fd Author: Matt Date: Wed Apr 8 21:48:02 2020 -0700 Apply spotless commit c39d9dc6fd13aedd2f684d8b2654f8a277ccf587 Author: Matt Date: Wed Apr 8 21:45:19 2020 -0700 Resolve memory leaks due to unnecessary instantiation of Points commit 7b507da3c860c2a68d70d968224648819695c489 Author: Matt Date: Wed Apr 8 21:29:53 2020 -0700 Fix ConcurrentModificationException bug in group contours pipe with potential targets commit d5c7b26f73f9c74bb2b29723f9987c3f66f8c288 Author: Banks Troutman Date: Wed Apr 8 03:49:10 2020 -0400 Refactor CVPipeline, add ReflectivePipelineTest commit 2e6a64862cc9c27faf30545cbf89053a4e9a9ed7 Author: Banks Troutman Date: Wed Apr 8 03:48:50 2020 -0400 add TestUtils class, move testimages commit 6653eb981224f4851cb2ad1219c6ebe08ca70b8f Author: Banks Troutman Date: Wed Apr 8 03:45:11 2020 -0400 add Releasable interface, implement in classes commit ef1e3024a1ef8fdfee58d4a8ef00b8aa96146721 Author: Banks Troutman Date: Tue Apr 7 19:53:41 2020 -0400 Move test images commit e25e736741d7432fca42a32f707099e62a1e5a14 Author: Banks Troutman Date: Tue Apr 7 18:42:43 2020 -0400 Apply Spotless commit ff5cee953f80b59b938df5c1a6e1bef4e9fb6573 Author: Banks Troutman Date: Tue Apr 7 01:52:06 2020 -0400 Finish ReflectivePipeline, various tweaks commit 7e6e65127a54ec784d048914fb49ca23b6ee4d29 Author: Banks Troutman Date: Tue Apr 7 01:49:14 2020 -0400 Add FrameStaticProperties as member in Frame Add FrameStaticProperties as member in Frame commit 09bf8cb500d89b8f3e11afe5e4d2c56d42ce18f9 Author: Banks Troutman Date: Mon Apr 6 17:45:25 2020 -0400 Add crosshair to DriverMode, cleanups commit 570531afb28e422f1e866454cfb8f5b745979ec9 Author: Banks Troutman Date: Mon Apr 6 14:10:39 2020 -0400 Add DriverPipeline classes, apply spotless commit 0376bdbdcb6bb9e468d353fff24d4c2ac1a0684a Author: ori agranat Date: Mon Apr 6 11:48:36 2020 +0300 updated Largest ContourSortMode and added centermost commit 378ba923c06b9a68f4db4972ef5f2b643d8051fd Author: Banks Troutman Date: Sun Apr 5 23:29:51 2020 -0400 Add pipeline classes, settings, separate enums commit 3b41afe125d8cf34e71bcd9c30d85f4f7dac30a8 Author: Banks Troutman Date: Sun Apr 5 23:29:32 2020 -0400 Refactor package structure, various cleanups * Apply spotless, attempt fix for SolvePNPTest * Fix SolvePNP Draw, fix 2019 PNP * Add active mat count to CVMat prints, fix NPE in contour release * Change providers/consumers to extend Provider/Supplier classes * ModuleManager bringup, spotless applied * Pipeline Test fixes and cleanups * Add getName to FrameProvider, set thread name in VisionRunner * VisionModuleManager test * God * Apply Spotless * Fix SolvePNPTest merge issue Co-authored-by: ori agranat Co-authored-by: Matt --- .circleci/config.yml | 36 +++++- .../common/util/TestUtils.java | 43 ++++--- .../frame/provider/FileFrameProvider.java | 3 +- .../vision/target/TargetCalculations.java | 78 ++++++++++++ .../common/vision/target/TrackedTarget.java | 119 +++--------------- .../frame/provider/FileFrameProviderTest.java | 96 ++++++++++++++ .../common/vision/opencv/ContourTest.java | 54 ++++++++ .../pipeline/ReflectivePipelineTest.java | 27 ++-- .../common/vision/pipeline/SolvePNPTest.java | 51 ++++---- .../vision/target/TargetCalculationsTest.java | 39 ++++++ .../vision/target/TrackedTargetTest.java | 54 ++++++++ 11 files changed, 432 insertions(+), 168 deletions(-) create mode 100644 chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetCalculations.java create mode 100644 chameleon-server/src/test/java/com/chameleonvision/common/vision/frame/provider/FileFrameProviderTest.java create mode 100644 chameleon-server/src/test/java/com/chameleonvision/common/vision/opencv/ContourTest.java create mode 100644 chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TargetCalculationsTest.java create mode 100644 chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TrackedTargetTest.java diff --git a/.circleci/config.yml b/.circleci/config.yml index 1ac8a1966..c7b635ffb 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -18,6 +18,37 @@ jobs: - dist + run_tests: + working_directory: ~/project/chameleon-server + docker: + - image: gradle:jdk12 + steps: + - checkout: + path: ~/project + #- restore_cache: + # key: v1-gradle-wrapper-{{ checksum "gradle/wrapper/gradle-wrapper.properties" }} + #- restore_cache: + # key: v1-gradle-cache-{{ checksum "build.gradle" }} + - run: + name: list + command: ls + - run: + name: Run tests + command: ./gradlew test + #- save_cache: + # paths: + # - .gradle/wrapper + # key: v1-gradle-wrapper-{{ checksum "gradle/wrapper/gradle-wrapper.properties" }} + #- save_cache: + # paths: + # - .gradle/caches + # key: v1-gradle-cache-{{ checksum "build.gradle" }} + - store_test_results: + path: build/test-results/test + - store_artifacts: # Upload test results for display in Artifacts: https://circleci.com/docs/2.0/artifacts/ + path: build/test-results/test + when: always + build_jar: working_directory: ~/project/chameleon-server docker: @@ -50,7 +81,6 @@ jobs: - run: gradle spotlessApply - run: git --no-pager diff --exit-code HEAD - deploy: docker: - image: cibuilds/github:0.10 @@ -68,13 +98,15 @@ workflows: release: jobs: - format_diff_against_git + - run_tests - build_ui - build_jar: requires: + - format_diff_against_git + - run_tests - build_ui - deploy: requires: - - format_diff_against_git - build_jar filters: branches: 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 index 5c5e4db4f..4c607a6da 100644 --- a/chameleon-server/src/main/java/com/chameleonvision/common/util/TestUtils.java +++ b/chameleon-server/src/main/java/com/chameleonvision/common/util/TestUtils.java @@ -1,15 +1,15 @@ package com.chameleonvision.common.util; import edu.wpi.cscore.CameraServerCvJNI; -import java.io.File; +import java.awt.*; 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 { + @SuppressWarnings("unused") public enum WPI2019Image { kCargoAngledDark48in(1.2192), kCargoSideStraightDark36in(0.9144), @@ -26,11 +26,11 @@ public class TestUtils { public static double FOV = 68.5; public final double distanceMeters; - public final String path; + public final Path path; - String getPath() { + Path getPath() { var filename = this.toString().substring(1); - return "\\2019\\WPI\\" + filename + ".jpg"; + return Path.of("2019", "WPI", filename + ".jpg"); } WPI2019Image(double distanceMeters) { @@ -39,6 +39,7 @@ public class TestUtils { } } + @SuppressWarnings("unused") public enum WPI2020Image { kBlueGoal_060in_Center(1.524), kBlueGoal_084in_Center(2.1336), @@ -62,11 +63,11 @@ public class TestUtils { public static double FOV = 68.5; public final double distanceMeters; - public final String path; + public final Path path; - String getPath() { + Path getPath() { var filename = this.toString().substring(1).replace('_', '-'); - return "\\2020\\WPI\\" + filename + ".jpg"; + return Path.of("2020", "WPI", filename + ".jpg"); } WPI2020Image(double distanceMeters) { @@ -75,22 +76,24 @@ public class TestUtils { } } - private static Path getTestImagesPath() { - var folder = TestUtils.class.getClassLoader().getResource("testimages"); - return Optional.ofNullable(folder).map(url -> new File(url.getFile()).toPath()).orElse(null); + private static Path getResourcesFolderPath() { + return Path.of("src", "test", "resources").toAbsolutePath(); + } + + public static Path getTestImagesPath() { + return getResourcesFolderPath().resolve("testimages"); } public static Path getCalibrationPath() { - var folder = TestUtils.class.getClassLoader().getResource("calibration"); - return Optional.ofNullable(folder).map(url -> new File(url.getFile()).toPath()).orElse(null); + return getResourcesFolderPath().resolve("calibration"); } public static Path getWPIImagePath(WPI2020Image image) { - return Path.of(getTestImagesPath().toString(), image.path); + return getTestImagesPath().resolve(image.path); } public static Path getWPIImagePath(WPI2019Image image) { - return Path.of(getTestImagesPath().toString(), image.path); + return getTestImagesPath().resolve(image.path); } public static void loadLibraries() { @@ -104,9 +107,13 @@ public class TestUtils { private static int DefaultTimeoutMillis = 5000; public static void showImage(Mat frame, String title, int timeoutMs) { - HighGui.imshow(title, frame); - HighGui.waitKey(timeoutMs); - HighGui.destroyAllWindows(); + try { + HighGui.imshow(title, frame); + HighGui.waitKey(timeoutMs); + HighGui.destroyAllWindows(); + } catch (HeadlessException ignored) { + + } } public static void showImage(Mat frame, int timeoutMs) { 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 d3be0a7ae..83e74bdb8 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 @@ -32,7 +32,8 @@ public class FileFrameProvider implements FrameProvider { * @param fov The fov of the image. */ public FileFrameProvider(Path path, double fov) { - if (!Files.exists(path)) throw new RuntimeException("Invalid path for image!"); + if (!Files.exists(path)) + throw new RuntimeException("Invalid path for image: " + path.toAbsolutePath().toString()); m_path = path; m_fov = fov; diff --git a/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetCalculations.java b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetCalculations.java new file mode 100644 index 000000000..d42accf11 --- /dev/null +++ b/chameleon-server/src/main/java/com/chameleonvision/common/vision/target/TargetCalculations.java @@ -0,0 +1,78 @@ +package com.chameleonvision.common.vision.target; + +import com.chameleonvision.common.util.numbers.DoubleCouple; +import org.apache.commons.math3.util.FastMath; +import org.opencv.core.Point; +import org.opencv.core.RotatedRect; + +public class TargetCalculations { + public static double calculateYaw( + double offsetCenterX, double targetCenterX, double horizontalFocalLength) { + return FastMath.toDegrees(FastMath.atan(offsetCenterX - targetCenterX) / horizontalFocalLength); + } + + public static double calculatePitch( + double offsetCenterY, double targetCenterY, double verticalFocalLength) { + return -FastMath.toDegrees(FastMath.atan(offsetCenterY - targetCenterY) / verticalFocalLength); + } + + public static Point calculateTargetOffsetPoint( + boolean isLandscape, TargetOffsetPointEdge offsetRegion, RotatedRect minAreaRect) { + Point[] vertices = new Point[4]; + + minAreaRect.points(vertices); + + Point bl = getMiddle(vertices[0], vertices[1]); + Point tl = getMiddle(vertices[1], vertices[2]); + Point tr = getMiddle(vertices[2], vertices[3]); + Point br = getMiddle(vertices[3], vertices[0]); + boolean orientation; + if (isLandscape) { + orientation = minAreaRect.size.width > minAreaRect.size.height; + } else { + orientation = minAreaRect.size.width < minAreaRect.size.height; + } + + switch (offsetRegion) { + case Top: + return orientation ? tl : tr; + case Bottom: + return orientation ? br : bl; + case Left: + return orientation ? bl : tl; + case Right: + return orientation ? tr : br; + default: + return minAreaRect.center; + } + } + + private static Point getMiddle(Point p1, Point p2) { + return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2)); + } + + public static Point calculateRobotOffsetPoint( + Point offsetPoint, + Point camCenterPoint, + DoubleCouple offsetEquationValues, + RobotOffsetPointMode offsetMode) { + switch (offsetMode) { + case None: + default: + return camCenterPoint; + case Single: + if (offsetPoint.x == 0 && offsetPoint.y == 0) { + return camCenterPoint; + } else { + return offsetPoint; + } + case Dual: + Point resultPoint = new Point(); + resultPoint.x = + (offsetPoint.x - offsetEquationValues.getFirst()) / offsetEquationValues.getSecond(); + resultPoint.y = + (offsetPoint.y * offsetEquationValues.getSecond()) + offsetEquationValues.getFirst(); + return resultPoint; + } + } +} 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 c4bfdfce7..7d73787e6 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 @@ -5,7 +5,6 @@ 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; @@ -75,112 +74,26 @@ public class TrackedTarget implements Releasable { return m_approximateBoundingPolygon; } - private void calculateTargetOffsetPoint(boolean isLandscape, TargetOffsetPointEdge offsetRegion) { - Point[] vertices = new Point[4]; - - var minAreaRect = getMinAreaRect(); - minAreaRect.points(vertices); - - Point bl = getMiddle(vertices[0], vertices[1]); - Point tl = getMiddle(vertices[1], vertices[2]); - Point tr = getMiddle(vertices[2], vertices[3]); - Point br = getMiddle(vertices[3], vertices[0]); - boolean orientation; - if (isLandscape) { - orientation = minAreaRect.size.width > minAreaRect.size.height; - } else { - orientation = minAreaRect.size.width < minAreaRect.size.height; - } - - Point resultPoint = minAreaRect.center; - switch (offsetRegion) { - case Top: - { - resultPoint = orientation ? tl : tr; - break; - } - case Bottom: - { - resultPoint = orientation ? br : bl; - break; - } - case Left: - { - resultPoint = orientation ? bl : tl; - break; - } - case Right: - { - resultPoint = orientation ? tr : br; - break; - } - } - m_targetOffsetPoint = resultPoint; - } - - private void calculateRobotOffsetPoint( - Point offsetPoint, - Point camCenterPoint, - DoubleCouple offsetEquationValues, - RobotOffsetPointMode offsetMode) { - Point resultPoint = new Point(); - switch (offsetMode) { - case None: - resultPoint = camCenterPoint; - break; - case Single: - if (offsetPoint.x == 0 && offsetPoint.y == 0) { - resultPoint = camCenterPoint; - } else { - resultPoint = offsetPoint; - } - break; - case Dual: - resultPoint.x = - (offsetPoint.x - offsetEquationValues.getFirst()) / offsetEquationValues.getSecond(); - resultPoint.y = - (offsetPoint.y * offsetEquationValues.getSecond()) + offsetEquationValues.getFirst(); - break; - } - - m_robotOffsetPoint = resultPoint; - } - - private void calculatePitch(double verticalFocalLength) { - double contourCenterY = m_mainContour.getCenterPoint().y; - double targetCenterY = m_targetOffsetPoint.y; - m_pitch = - -FastMath.toDegrees(FastMath.atan((contourCenterY - targetCenterY) / verticalFocalLength)); - } - - private void calculateYaw(double horizontalFocalLength) { - double contourCenterX = m_mainContour.getCenterPoint().x; - double targetCenterX = m_targetOffsetPoint.x; - m_yaw = - FastMath.toDegrees(FastMath.atan((contourCenterX - targetCenterX) / horizontalFocalLength)); - } - - private void calculateArea(double imageArea) { - m_area = m_mainContour.getMinAreaRect().size.area() / imageArea; - } - - private Point getMiddle(Point p1, Point p2) { - return new Point(((p1.x + p2.x) / 2), ((p1.y + p2.y) / 2)); - } - public void calculateValues(TargetCalculationParameters params) { // this MUST happen in this exact order! - calculateTargetOffsetPoint(params.isLandscape, params.targetOffsetPointEdge); - calculateRobotOffsetPoint( - m_targetOffsetPoint, - params.cameraCenterPoint, - params.offsetEquationValues, - params.robotOffsetPointMode); + m_targetOffsetPoint = + TargetCalculations.calculateTargetOffsetPoint( + params.isLandscape, params.targetOffsetPointEdge, getMinAreaRect()); + m_robotOffsetPoint = + TargetCalculations.calculateRobotOffsetPoint( + m_targetOffsetPoint, + params.cameraCenterPoint, + params.offsetEquationValues, + params.robotOffsetPointMode); // order of this stuff doesnt matter though - calculatePitch(params.verticalFocalLength); - calculateYaw(params.horizontalFocalLength); - calculateArea(params.imageArea); + m_pitch = + TargetCalculations.calculatePitch( + m_targetOffsetPoint.y, m_robotOffsetPoint.y, params.verticalFocalLength); + m_yaw = + TargetCalculations.calculateYaw( + m_targetOffsetPoint.x, m_robotOffsetPoint.x, params.horizontalFocalLength); + m_area = m_mainContour.getMinAreaRect().size.area() / params.imageArea; } @Override diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/frame/provider/FileFrameProviderTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/frame/provider/FileFrameProviderTest.java new file mode 100644 index 000000000..6b8787505 --- /dev/null +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/frame/provider/FileFrameProviderTest.java @@ -0,0 +1,96 @@ +package com.chameleonvision.common.vision.frame.provider; + +import static org.junit.jupiter.api.Assertions.*; + +import com.chameleonvision.common.util.TestUtils; +import com.chameleonvision.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); + + // TODO: find a way to skip this if a flag isn't set + 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); + + // TODO: find a way to skip this if a flag isn't set + 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); + } +} diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/opencv/ContourTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/opencv/ContourTest.java new file mode 100644 index 000000000..3a2490e46 --- /dev/null +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/opencv/ContourTest.java @@ -0,0 +1,54 @@ +package com.chameleonvision.common.vision.opencv; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.chameleonvision.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); + } +} 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 index 9a9eb6e6f..f59f9ae31 100644 --- 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 @@ -32,15 +32,13 @@ public class ReflectivePipelineTest { 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; + 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( @@ -55,7 +53,7 @@ public class ReflectivePipelineTest { printTestResults(pipelineResult); Assertions.assertTrue(pipelineResult.hasTargets()); - Assertions.assertEquals(2, pipelineResult.targets.size()); + Assertions.assertEquals(2, pipelineResult.targets.size(), "Target count wrong!"); TestUtils.showImage(pipelineResult.outputFrame.image.getMat(), "Pipeline output"); } @@ -65,11 +63,10 @@ public class ReflectivePipelineTest { 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; + 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( 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 index 56ec0f877..4229643b2 100644 --- 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 @@ -81,21 +81,17 @@ public class SolvePNPTest { public void test2019() { 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; - - settings.targetModel = TargetModel.get2019Target(); - settings.cameraCalibration = getCoeffs(LIFECAM_240P_CAL_FILE); - - pipeline.settings = settings; + 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( @@ -120,20 +116,16 @@ public class SolvePNPTest { public void test2020() { 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 = getCoeffs(LIFECAM_480P_CAL_FILE); - - settings.targetModel = TargetModel.get2020Target(36); - settings.cameraPitch = Rotation2d.fromDegrees(0.0); - - pipeline.settings = settings; + 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( @@ -175,6 +167,7 @@ public class SolvePNPTest { private static void continuouslyRunPipeline(Frame frame, ReflectivePipelineSettings settings) { var pipeline = new ReflectivePipeline(); pipeline.settings = settings; + while (true) { CVPipelineResult pipelineResult = pipeline.run(frame); printTestResults(pipelineResult); diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TargetCalculationsTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TargetCalculationsTest.java new file mode 100644 index 000000000..67d1a69eb --- /dev/null +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TargetCalculationsTest.java @@ -0,0 +1,39 @@ +package com.chameleonvision.common.vision.target; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.junit.jupiter.api.Test; +import org.opencv.core.Point; +import org.opencv.core.Size; + +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; + + @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"); + } +} diff --git a/chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TrackedTargetTest.java b/chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TrackedTargetTest.java new file mode 100644 index 000000000..5269c27e0 --- /dev/null +++ b/chameleon-server/src/test/java/com/chameleonvision/common/vision/target/TrackedTargetTest.java @@ -0,0 +1,54 @@ +package com.chameleonvision.common.vision.target; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import com.chameleonvision.common.util.TestUtils; +import com.chameleonvision.common.util.numbers.DoubleCouple; +import com.chameleonvision.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"); + } +}