diff --git a/README.md b/README.md index 8c67e481..51674295 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ -![FTC Vision](https://raw.githubusercontent.com/lasarobotics/ftcvision/dev/img/logo-github.png) +

+ FTC-Sees! +

-# FTC Vision Library [![Build Status](https://travis-ci.org/lasarobotics/FTCVision.svg?branch=staging)](https://travis-ci.org/lasarobotics/FTCVision) +# FTC Vision Library [![Build Status](https://travis-ci.org/lasarobotics/FTCVision.svg?branch=master)](https://travis-ci.org/lasarobotics/FTCVision) Computer Vision library for FTC based on OpenCV, featuring **beacon color and position detection**, as well as an easy-to-use `VisionOpMode` format and many additional detection features planned in the future. ## Installing from Scratch @@ -30,13 +32,19 @@ include ':ftc-visionlib' compile project(':ftc-visionlib') compile project(':opencv-java') ``` -- Copy in Vision opmodes (those that end in `VisionSample.java`) from the FTCVision directory into your opmode directory. +- Update Gradle configuration by pressing the green "Sync Project with Gradle Files" button in the header (this may take a minute) +- Copy in Vision opmodes (those that end in `VisionSample.java`, located in `[vision-root]/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes`) from the FTCVision directory into your opmode directory. - Run and test the code! Let us know if you encounter any difficulties. +- You can now write your custom `VisionOpMode`! ## Status This library is currently under insanely active development. We're in the **Beta** phase right now. If you have any questions or would like to help, send a note to `smo-key` (contact info on profile). Thank you! -![A test from 8 feet away](https://raw.githubusercontent.com/lasarobotics/ftcvision/dev/img/test3.png) +#### Accuracy Test +![A test from 8 feet away](https://raw.githubusercontent.com/lasarobotics/ftcvision/img/test2.png) + +#### Distance Test +![A test from 8 feet away](https://raw.githubusercontent.com/lasarobotics/ftcvision/img/test1.png) ## Goals - Locate the lit target (the thing with two buttons) within the camera viewfield diff --git a/ftc-cameratest/src/main/AndroidManifest.xml b/ftc-cameratest/src/main/AndroidManifest.xml index b867333b..d8c3434f 100644 --- a/ftc-cameratest/src/main/AndroidManifest.xml +++ b/ftc-cameratest/src/main/AndroidManifest.xml @@ -1,19 +1,20 @@ + xmlns:tools="http://schemas.android.com/tools" + package="com.lasarobotics.tests.camera" + android:versionCode="301" + android:versionName="3.01"> - + @@ -21,18 +22,27 @@ - + - + - - - - + + + + diff --git a/ftc-cameratest/src/main/java/com/lasarobotics/tests/camera/CameraTestActivity.java b/ftc-cameratest/src/main/java/com/lasarobotics/tests/camera/CameraTestActivity.java index 3c82aebe..21d329a5 100644 --- a/ftc-cameratest/src/main/java/com/lasarobotics/tests/camera/CameraTestActivity.java +++ b/ftc-cameratest/src/main/java/com/lasarobotics/tests/camera/CameraTestActivity.java @@ -4,16 +4,16 @@ import android.os.Bundle; import android.view.WindowManager; -import org.lasarobotics.vision.android.Camera; -import org.lasarobotics.vision.android.Cameras; -import org.lasarobotics.vision.detection.ColorBlobDetector; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.ftc.resq.Beacon; -import org.lasarobotics.vision.image.Drawing; -import org.lasarobotics.vision.util.FPS; -import org.lasarobotics.vision.util.color.ColorGRAY; -import org.lasarobotics.vision.util.color.ColorHSV; -import org.lasarobotics.vision.util.color.ColorRGBA; +import org.lasarobotics.vision.test.android.Camera; +import org.lasarobotics.vision.test.android.Cameras; +import org.lasarobotics.vision.test.detection.ColorBlobDetector; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.ftc.resq.Beacon; +import org.lasarobotics.vision.test.image.Drawing; +import org.lasarobotics.vision.test.util.FPS; +import org.lasarobotics.vision.test.util.color.ColorGRAY; +import org.lasarobotics.vision.test.util.color.ColorHSV; +import org.lasarobotics.vision.test.util.color.ColorRGBA; import org.opencv.android.BaseLoaderCallback; import org.opencv.android.CameraBridgeViewBase; import org.opencv.android.CameraBridgeViewBase.CvCameraViewFrame; @@ -24,26 +24,48 @@ import org.opencv.core.Mat; import org.opencv.core.Point; -import java.text.DecimalFormat; import java.util.List; public class CameraTestActivity extends Activity implements CvCameraViewListener2 { + private static final ColorHSV colorRadius = new ColorHSV(50, 75, 127); + private static final ColorHSV lowerBoundRed = new ColorHSV((int) (305 / 360.0 * 255.0), (int) (0.200 * 255.0), (int) (0.300 * 255.0)); + private static final ColorHSV upperBoundRed = new ColorHSV((int) ((360.0 + 5.0) / 360.0 * 255.0), 255, 255); + private static final ColorHSV lowerBoundBlue = new ColorHSV((int) (170.0 / 360.0 * 255.0), (int) (0.200 * 255.0), (int) (0.750 * 255.0)); + private static final ColorHSV upperBoundBlue = new ColorHSV((int) (227.0 / 360.0 * 255.0), 255, 255); private Mat mRgba; //RGBA scene image private Mat mGray; //Grayscale scene image private CameraBridgeViewBase mOpenCvCameraView; - private float focalLength; //Camera lens focal length - //private ObjectDetection.ObjectAnalysis objectAnalysis; private FPS fpsCounter; + private final BaseLoaderCallback mLoaderCallback = new BaseLoaderCallback(this) { + @Override + public void onManagerConnected(int status) { + switch (status) { + case LoaderCallbackInterface.SUCCESS: { + // OpenCV loaded successfully! + // Load native library AFTER OpenCV initialization + + initialize(); + mOpenCvCameraView.enableView(); + } + break; + default: { + super.onManagerConnected(status); + } + break; + } + } + }; + private ColorBlobDetector detectorRed; + private ColorBlobDetector detectorBlue; public CameraTestActivity() { } - private void initialize() - { + private void initialize() { //GET CAMERA PROPERTIES Camera cam = Cameras.PRIMARY.createCamera(); android.hardware.Camera.Parameters pam = cam.getCamera().getParameters(); @@ -79,28 +101,9 @@ private void initialize() fpsCounter = new FPS(); } - private BaseLoaderCallback mLoaderCallback = new BaseLoaderCallback(this) { - @Override - public void onManagerConnected(int status) { - switch (status) { - case LoaderCallbackInterface.SUCCESS: - { - // OpenCV loaded successfully! - // Load native library AFTER OpenCV initialization - - initialize(); - - mOpenCvCameraView.enableView(); - } break; - default: - { - super.onManagerConnected(status); - } break; - } - } - }; - - /** Called when the activity is first created. */ + /** + * Called when the activity is first created. + */ @Override public void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); @@ -113,16 +116,14 @@ public void onCreate(Bundle savedInstanceState) { } @Override - public void onPause() - { + public void onPause() { super.onPause(); if (mOpenCvCameraView != null) mOpenCvCameraView.disableView(); } @Override - public void onResume() - { + public void onResume() { super.onResume(); if (!OpenCVLoader.initDebug()) { // Internal OpenCV library not found. Using OpenCV Manager for initialization @@ -139,22 +140,12 @@ public void onDestroy() { mOpenCvCameraView.disableView(); } - private ColorBlobDetector detectorRed; - private ColorBlobDetector detectorBlue; - private static final ColorHSV colorRadius = new ColorHSV(50, 75, 127); - - private static final ColorHSV lowerBoundRed = new ColorHSV( (int)(305 / 360.0 * 255.0), (int)(0.200 * 255.0), (int)(0.300 * 255.0)); - private static final ColorHSV upperBoundRed = new ColorHSV( (int)((360.0+5.0) / 360.0 * 255.0), 255 , 255); - - private static final ColorHSV lowerBoundBlue = new ColorHSV((int)(170.0 / 360.0 * 255.0), (int)(0.200 * 255.0), (int)(0.750 * 255.0)); - private static final ColorHSV upperBoundBlue = new ColorHSV((int)(227.0 / 360.0 * 255.0), 255 , 255); - public void onCameraViewStarted(int width, int height) { mRgba = new Mat(height, width, CvType.CV_8UC4); mGray = new Mat(height, width, CvType.CV_8UC1); //Initialize all detectors here - detectorRed = new ColorBlobDetector(lowerBoundRed, upperBoundRed); + detectorRed = new ColorBlobDetector(lowerBoundRed, upperBoundRed); detectorBlue = new ColorBlobDetector(lowerBoundBlue, upperBoundBlue); } @@ -192,7 +183,7 @@ public Mat onCameraFrame(CvCameraViewFrame inputFrame) { //DEBUG confidence output - Drawing.drawText(mRgba, "Confidence: " + colorAnalysis.getConfidenceString() , + Drawing.drawText(mRgba, "Confidence: " + colorAnalysis.getConfidenceString(), new Point(0, 50), 1.0f, new ColorGRAY(255)); //Transform.enlarge(mRgba, originalSize, true); @@ -200,9 +191,7 @@ public Mat onCameraFrame(CvCameraViewFrame inputFrame) { Drawing.drawText(mRgba, colorAnalysis.getStateLeft().toString() + ", " + colorAnalysis.getStateRight().toString(), new Point(0, 8), 1.0f, new ColorGRAY(255), Drawing.Anchor.BOTTOMLEFT); - } - catch (Exception e) - { + } catch (Exception e) { Drawing.drawText(mRgba, "Analysis Error", new Point(0, 8), 1.0f, new ColorRGBA("#F44336"), Drawing.Anchor.BOTTOMLEFT); e.printStackTrace(); } diff --git a/ftc-cameratest/src/main/res/layout/activity_cameratest.xml b/ftc-cameratest/src/main/res/layout/activity_cameratest.xml index cb475494..57d872d2 100644 --- a/ftc-cameratest/src/main/res/layout/activity_cameratest.xml +++ b/ftc-cameratest/src/main/res/layout/activity_cameratest.xml @@ -1,10 +1,10 @@ + android:layout_height="match_parent"> + android:layout_height="fill_parent" /> diff --git a/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicVisionSample.java b/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicVisionSample.java index 7239cb35..cbbfc3c0 100644 --- a/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicVisionSample.java +++ b/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicVisionSample.java @@ -31,24 +31,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.ftcrobotcontroller.opmodes; -import org.lasarobotics.vision.android.Cameras; -import org.lasarobotics.vision.detection.ColorBlobDetector; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.ftc.resq.Beacon; -import org.lasarobotics.vision.image.Drawing; -import org.lasarobotics.vision.image.Transform; -import org.lasarobotics.vision.opmode.ManualVisionOpMode; -import org.lasarobotics.vision.opmode.VisionExtensions; -import org.lasarobotics.vision.opmode.VisionOpMode; -import org.lasarobotics.vision.util.color.ColorGRAY; -import org.lasarobotics.vision.util.color.ColorHSV; -import org.lasarobotics.vision.util.color.ColorRGBA; -import org.opencv.core.Mat; -import org.opencv.core.Point; +import org.lasarobotics.vision.test.android.Cameras; +import org.lasarobotics.vision.test.opmode.VisionExtensions; +import org.lasarobotics.vision.test.opmode.VisionOpMode; import org.opencv.core.Size; -import java.util.List; - /** * TeleOp Mode *

diff --git a/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/ManualVisionSample.java b/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/ManualVisionSample.java index f10dd49f..b548b539 100644 --- a/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/ManualVisionSample.java +++ b/ftc-robotcontroller/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/ManualVisionSample.java @@ -31,15 +31,15 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.ftcrobotcontroller.opmodes; -import org.lasarobotics.vision.android.Cameras; -import org.lasarobotics.vision.detection.ColorBlobDetector; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.ftc.resq.Beacon; -import org.lasarobotics.vision.image.Drawing; -import org.lasarobotics.vision.opmode.ManualVisionOpMode; -import org.lasarobotics.vision.util.color.ColorGRAY; -import org.lasarobotics.vision.util.color.ColorHSV; -import org.lasarobotics.vision.util.color.ColorRGBA; +import org.lasarobotics.vision.test.android.Cameras; +import org.lasarobotics.vision.test.detection.ColorBlobDetector; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.ftc.resq.Beacon; +import org.lasarobotics.vision.test.image.Drawing; +import org.lasarobotics.vision.test.opmode.ManualVisionOpMode; +import org.lasarobotics.vision.test.util.color.ColorGRAY; +import org.lasarobotics.vision.test.util.color.ColorHSV; +import org.lasarobotics.vision.test.util.color.ColorRGBA; import org.opencv.core.Mat; import org.opencv.core.Point; import org.opencv.core.Size; @@ -53,13 +53,13 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE */ public class ManualVisionSample extends ManualVisionOpMode { - private Beacon.BeaconAnalysis colorAnalysis = new Beacon.BeaconAnalysis(); - private ColorBlobDetector detectorRed; - private ColorBlobDetector detectorBlue; private static final ColorHSV lowerBoundRed = new ColorHSV((int) (305 / 360.0 * 255.0), (int) (0.200 * 255.0), (int) (0.300 * 255.0)); private static final ColorHSV upperBoundRed = new ColorHSV((int) ((360.0 + 5.0) / 360.0 * 255.0), 255, 255); private static final ColorHSV lowerBoundBlue = new ColorHSV((int) (170.0 / 360.0 * 255.0), (int) (0.200 * 255.0), (int) (0.750 * 255.0)); private static final ColorHSV upperBoundBlue = new ColorHSV((int) (227.0 / 360.0 * 255.0), 255, 255); + private Beacon.BeaconAnalysis colorAnalysis = new Beacon.BeaconAnalysis(); + private ColorBlobDetector detectorRed; + private ColorBlobDetector detectorBlue; private boolean noError = true; @Override diff --git a/ftc-visionlib/src/androidTest/java/org/lasarobotics/vision/ApplicationTest.java b/ftc-visionlib/src/androidTest/java/org/lasarobotics/vision/test/ApplicationTest.java similarity index 89% rename from ftc-visionlib/src/androidTest/java/org/lasarobotics/vision/ApplicationTest.java rename to ftc-visionlib/src/androidTest/java/org/lasarobotics/vision/test/ApplicationTest.java index ebc57a1e..54c32cab 100644 --- a/ftc-visionlib/src/androidTest/java/org/lasarobotics/vision/ApplicationTest.java +++ b/ftc-visionlib/src/androidTest/java/org/lasarobotics/vision/test/ApplicationTest.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision; +package org.lasarobotics.vision.test; import android.app.Application; import android.test.ApplicationTestCase; diff --git a/ftc-visionlib/src/main/AndroidManifest.xml b/ftc-visionlib/src/main/AndroidManifest.xml index 7eee3f13..58a5a9b1 100644 --- a/ftc-visionlib/src/main/AndroidManifest.xml +++ b/ftc-visionlib/src/main/AndroidManifest.xml @@ -1,9 +1,9 @@ + package="org.lasarobotics.vision.vision"> - + diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Camera.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Camera.java similarity index 69% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Camera.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Camera.java index ab727aa6..654594fc 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Camera.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Camera.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.android; +package org.lasarobotics.vision.test.android; import android.content.pm.PackageManager; @@ -15,72 +15,62 @@ public class Camera { android.hardware.Camera c; int id; - public Camera(Cameras camera) - { + public Camera(Cameras camera) { makeCamera(camera.getID()); } - public android.hardware.Camera getCamera() { return c; } + public static boolean isHardwareAvailable() { + return Util.getContext().getPackageManager().hasSystemFeature(PackageManager.FEATURE_CAMERA); + } + + public static int getCameraCount() { + return android.hardware.Camera.getNumberOfCameras(); + } + + public android.hardware.Camera getCamera() { + return c; + } - private void makeCamera(int id) - { + private void makeCamera(int id) { this.id = id; try { this.c = android.hardware.Camera.open(id); - } catch (Exception e) - { + } catch (Exception e) { this.c = null; } } - public void unlock() - { + public void unlock() { c.unlock(); } - public void lock() - { + public void lock() { c.lock(); } - public void release() - { + public void release() { c.release(); } - public int getID() - { + public int getID() { return id; } - public boolean doesExist() - { + public boolean doesExist() { return (c != null); } - public static boolean isHardwareAvailable() { - return Util.getContext().getPackageManager().hasSystemFeature(PackageManager.FEATURE_CAMERA); - } - - public static int getCameraCount() - { - return android.hardware.Camera.getNumberOfCameras(); - } - - public Size getLargestFrameSize() - { + public Size getLargestFrameSize() { android.hardware.Camera.Size s = c.getParameters().getPreviewSize(); return new Size(s.width, s.height); } - public Size getBestFrameSize() - { + public Size getBestFrameSize() { android.hardware.Camera.Size s = c.getParameters().getPreferredPreviewSizeForVideo(); return new Size(s.width, s.height); } - public List getAllFrameSizes() - { + public List getAllFrameSizes() { List l = new ArrayList<>(); for (android.hardware.Camera.Size s : c.getParameters().getSupportedPreviewSizes()) l.add(new Size(s.width, s.height)); diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Cameras.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Cameras.java similarity index 65% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Cameras.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Cameras.java index d1b046f4..4fe613c2 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Cameras.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Cameras.java @@ -1,10 +1,7 @@ -package org.lasarobotics.vision.android; +package org.lasarobotics.vision.test.android; import android.content.Context; import android.content.pm.PackageManager; -import android.hardware.camera2.CameraManager; - -import org.lasarobotics.vision.android.Camera; /** * Implements the Android camera @@ -15,24 +12,21 @@ public enum Cameras { OTHER_1(2), OTHER_2(3); - int id; + final int id; - Cameras(int id) - { + Cameras(int id) { this.id = id; } - public int getID() - { - return id; + public static boolean isHardwareAvailable(Context context) { + return context.getPackageManager().hasSystemFeature(PackageManager.FEATURE_CAMERA); } - public Camera createCamera() - { - return new Camera(this); + public int getID() { + return id; } - public static boolean isHardwareAvailable(Context context) { - return context.getPackageManager().hasSystemFeature(PackageManager.FEATURE_CAMERA); + public Camera createCamera() { + return new Camera(this); } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Util.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Util.java similarity index 98% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Util.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Util.java index 1ecbd8f6..d1870bd9 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/android/Util.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/android/Util.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.android; +package org.lasarobotics.vision.test.android; import android.app.Application; import android.content.Context; diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/CascadeObjectDetection.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/CascadeObjectDetection.java similarity index 79% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/CascadeObjectDetection.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/CascadeObjectDetection.java index 6d831699..193d3d9f 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/CascadeObjectDetection.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/CascadeObjectDetection.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.detection; +package org.lasarobotics.vision.test.detection; /** * Implements cascade object detection, a rapid way of locating features such as human faces diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/ColorBlobDetector.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/ColorBlobDetector.java similarity index 80% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/ColorBlobDetector.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/ColorBlobDetector.java index d9272c68..fa201f64 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/ColorBlobDetector.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/ColorBlobDetector.java @@ -1,10 +1,10 @@ -package org.lasarobotics.vision.detection; +package org.lasarobotics.vision.test.detection; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.image.Drawing; -import org.lasarobotics.vision.util.color.Color; -import org.lasarobotics.vision.util.color.ColorHSV; -import org.lasarobotics.vision.util.color.ColorSpace; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.image.Drawing; +import org.lasarobotics.vision.test.util.color.Color; +import org.lasarobotics.vision.test.util.color.ColorHSV; +import org.lasarobotics.vision.test.util.color.ColorSpace; import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; @@ -19,12 +19,18 @@ */ public class ColorBlobDetector { + private final List contours = new ArrayList<>(); + // Cache + private final Mat mPyrDownMat = new Mat(); + private final Mat mHsvMat = new Mat(); + private final Mat mMaskOne = new Mat(); + private final Mat mMask = new Mat(); + private final Mat mDilatedMask = new Mat(); + private final Mat mHierarchy = new Mat(); //Lower bound for range checking private ColorHSV lowerBound = new ColorHSV(0, 0, 0); //Upper bound for range checking private ColorHSV upperBound = new ColorHSV(0, 0, 0); - //Minimum contour area in percent for contours filtering - private static double minContourArea = 0.1; //Color radius for range checking private Scalar colorRadius = new Scalar(75, 75, 75, 0); //Currently selected color @@ -32,24 +38,20 @@ public class ColorBlobDetector { //True if radius is set, false if lower and upper bound is set private boolean isRadiusSet = true; - private List contours = new ArrayList<>(); - - public ColorBlobDetector(Color color) - { + public ColorBlobDetector(Color color) { setColor(color); } - public ColorBlobDetector(Color color, Color colorRadius) - { + + public ColorBlobDetector(Color color, Color colorRadius) { this.colorRadius = colorRadius.convertColorScalar(ColorSpace.HSV); setColor(color); } - public ColorBlobDetector(ColorHSV colorMinimum, ColorHSV colorMaximum) - { + + public ColorBlobDetector(ColorHSV colorMinimum, ColorHSV colorMaximum) { setColorRadius(colorMinimum, colorMaximum); } - public void setColor(Color color) - { + public void setColor(Color color) { if (color == null) throw new IllegalArgumentException("Color must not be null!"); @@ -82,8 +84,7 @@ public void setColor(Color color) upperBound = new ColorHSV(upperBoundScalar); } - public void setColorRadius(Color lowerBound, Color upperBound) - { + public void setColorRadius(Color lowerBound, Color upperBound) { isRadiusSet = false; Scalar lower = lowerBound.convertColorScalar(ColorSpace.HSV); Scalar upper = upperBound.convertColorScalar(ColorSpace.HSV); @@ -99,12 +100,6 @@ public void setColorRadius(ColorHSV radius) { setColor(color); } - public void setMinContourArea(double area) { - minContourArea = area; - //Update the bounds again - setColor(color); - } - public void process(Mat rgbaImage) { Imgproc.pyrDown(rgbaImage, mPyrDownMat); Imgproc.pyrDown(mPyrDownMat, mPyrDownMat); @@ -114,8 +109,7 @@ public void process(Mat rgbaImage) { //Test whether we need two inRange operations (only if the hue crosses over 255) if (upperBound.getScalar().val[0] <= 255) { Core.inRange(mHsvMat, lowerBound.getScalar(), upperBound.getScalar(), mMask); - } else - { + } else { //We need two operations - we're going to OR the masks together Scalar lower = lowerBound.getScalar().clone(); Scalar upper = upperBound.getScalar().clone(); @@ -143,30 +137,21 @@ public void process(Mat rgbaImage) { // Filter contours by area and resize to fit the original image size contours.clear(); - for (MatOfPoint c : contourListTemp ) { - Core.multiply(c, new Scalar(4,4), c); + for (MatOfPoint c : contourListTemp) { + Core.multiply(c, new Scalar(4, 4), c); contours.add(new Contour(c)); } } - public void drawContours(Mat img, Color color) - { + public void drawContours(Mat img, Color color) { Drawing.drawContours(img, contours, color); } - public void drawContours(Mat img, Color color, int thickness) - { + + public void drawContours(Mat img, Color color, int thickness) { Drawing.drawContours(img, contours, color, thickness); } public List getContours() { return contours; } - - // Cache - private Mat mPyrDownMat = new Mat(); - private Mat mHsvMat = new Mat(); - private Mat mMaskOne = new Mat(); - private Mat mMask = new Mat(); - private Mat mDilatedMask = new Mat(); - private Mat mHierarchy = new Mat(); } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/ObjectDetection.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/ObjectDetection.java similarity index 78% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/ObjectDetection.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/ObjectDetection.java index d8b6276e..469ed38a 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/ObjectDetection.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/ObjectDetection.java @@ -1,22 +1,22 @@ -package org.lasarobotics.vision.detection; +package org.lasarobotics.vision.test.detection; import android.util.Log; -import org.lasarobotics.vision.image.Drawing; -import org.lasarobotics.vision.image.Transform; -import org.lasarobotics.vision.util.color.ColorRGBA; +import org.lasarobotics.vision.test.image.Drawing; +import org.lasarobotics.vision.test.image.Transform; +import org.lasarobotics.vision.test.util.color.ColorRGBA; import org.opencv.calib3d.Calib3d; import org.opencv.core.Core; +import org.opencv.core.DMatch; +import org.opencv.core.KeyPoint; import org.opencv.core.Mat; import org.opencv.core.MatOfDMatch; import org.opencv.core.MatOfKeyPoint; import org.opencv.core.MatOfPoint2f; import org.opencv.core.Point; -import org.opencv.core.DMatch; import org.opencv.features2d.DescriptorExtractor; import org.opencv.features2d.DescriptorMatcher; import org.opencv.features2d.FeatureDetector; -import org.opencv.core.KeyPoint; import java.util.ArrayList; import java.util.List; @@ -25,141 +25,113 @@ * Designed to detect a single object at a time in an image */ public class ObjectDetection { - public enum FeatureDetectorType - { - FAST(1), - STAR(2), - //SIFT(3), - //SURF(4), - ORB(5), - MSER(6), - GFTT(7), - HARRIS(8), - SIMPLEBLOB(9), - DENSE(10), - BRISK(11), - FAST_DYNAMIC(1, true), - STAR_DYNAMIC(2, true), - //SIFT_DYNAMIC(3, true), - //SURF_DYNAMIC(4, true), - ORB_DYNAMIC(5, true), - MSER_DYNAMIC(6, true), - GFTT_DYNAMIC(7, true), - HARRIS_DYNAMIC(8, true), - SIMPLEBLOB_DYNAMIC(9, true), - DENSE_DYNAMIC(10, true), - BRISK_DYNAMIC(11, true); - - private int m; - FeatureDetectorType(int type) { m = type; } - FeatureDetectorType(int type, boolean dynamic) { m = type + (dynamic ? 3000 : 0); } - - public int val() { return m; } - } - - public enum DescriptorExtractorType - { - //SIFT(1), - //SURF(2), - ORB(3), - BRIEF(4), - BRISK(5), - FREAK(6), - //SIFT_OPPONENT(1, true), - //SURF_OPPONENT(2, true), - ORB_OPPONENT(3, true), - BRIEF_OPPONENT(4, true), - BRISK_OPPONENT(5, true), - FREAK_OPPONENT(6, true); - - private int m; - DescriptorExtractorType(int type) { m = type; } - DescriptorExtractorType(int type, boolean opponent) { m = type + (opponent ? 1000 : 0); } - - public int val() { return m; } - } - - public enum DescriptorMatcherType - { - FLANN(1), - BRUTEFORCE(2), - BRUTEFORCE_L1(3), - BRUTEFORCE_HAMMING(4), - BRUTEFORCE_HAMMINGLUT(5), - BRUTEFORCE_SL2(6); - - private int m; - DescriptorMatcherType(int type) { m = type; } - - public int val() { return m; } - } - FeatureDetector detector; DescriptorExtractor extractor; DescriptorMatcher matcher; - public ObjectDetection() - { + public ObjectDetection() { detector = FeatureDetector.create(FeatureDetectorType.FAST.val()); extractor = DescriptorExtractor.create(DescriptorExtractorType.BRIEF.val()); matcher = DescriptorMatcher.create(DescriptorMatcherType.BRUTEFORCE_HAMMING.val()); } - public ObjectDetection(FeatureDetectorType detector, DescriptorExtractorType extractor, DescriptorMatcherType matcher) - { + + public ObjectDetection(FeatureDetectorType detector, DescriptorExtractorType extractor, DescriptorMatcherType matcher) { this.detector = FeatureDetector.create(detector.val()); this.extractor = DescriptorExtractor.create(extractor.val()); this.matcher = DescriptorMatcher.create(matcher.val()); } - public final class ObjectAnalysis - { - MatOfKeyPoint keypoints; - Mat descriptors; - Mat object; - - ObjectAnalysis(MatOfKeyPoint keypoints, Mat descriptors, Mat object) - { - this.keypoints = keypoints; - this.descriptors = descriptors; - this.object = object; + /** + * Draw keypoints directly onto an scene image - red circles indicate keypoints + * + * @param output The scene matrix + * @param sceneAnalysis Analysis of the scene, as given by analyzeScene() + */ + public static void drawKeypoints(Mat output, SceneAnalysis sceneAnalysis) { + KeyPoint[] keypoints = sceneAnalysis.keypoints.toArray(); + for (KeyPoint kp : keypoints) { + Drawing.drawCircle(output, new Point(kp.pt.x, kp.pt.y), 4, new ColorRGBA(255, 0, 0)); } } - public final class SceneAnalysis - { - MatOfKeyPoint keypoints; - Mat descriptors; - MatOfDMatch matches; - Mat scene; + public static void drawObjectLocation(Mat output, ObjectAnalysis objectAnalysis, SceneAnalysis sceneAnalysis) { + List ptsObject = new ArrayList<>(); + List ptsScene = new ArrayList<>(); - SceneAnalysis(MatOfKeyPoint keypoints, Mat descriptors, MatOfDMatch matches, Mat scene) - { - this.keypoints = keypoints; - this.descriptors = descriptors; - this.matches = matches; - this.scene = scene; + KeyPoint[] keypointsObject = objectAnalysis.keypoints.toArray(); + KeyPoint[] keypointsScene = sceneAnalysis.keypoints.toArray(); + + DMatch[] matches = sceneAnalysis.matches.toArray(); + + for (DMatch matche : matches) { + //Get the keypoints from these matches + ptsObject.add(keypointsObject[matche.queryIdx].pt); + ptsScene.add(keypointsScene[matche.trainIdx].pt); } + + MatOfPoint2f matObject = new MatOfPoint2f(); + matObject.fromList(ptsObject); + + MatOfPoint2f matScene = new MatOfPoint2f(); + matScene.fromList(ptsScene); + + //Calculate homography of object in scene + Mat homography = Calib3d.findHomography(matObject, matScene, Calib3d.RANSAC, 5.0f); + + //Create the unscaled array of corners, representing the object size + Point cornersObject[] = new Point[4]; + cornersObject[0] = new Point(0, 0); + cornersObject[1] = new Point(objectAnalysis.object.cols(), 0); + cornersObject[2] = new Point(objectAnalysis.object.cols(), objectAnalysis.object.rows()); + cornersObject[3] = new Point(0, objectAnalysis.object.rows()); + + Point[] cornersSceneTemp = new Point[0]; + + MatOfPoint2f cornersSceneMatrix = new MatOfPoint2f(cornersSceneTemp); + MatOfPoint2f cornersObjectMatrix = new MatOfPoint2f(cornersObject); + + //Transform the object coordinates to the scene coordinates by the homography matrix + Core.perspectiveTransform(cornersObjectMatrix, cornersSceneMatrix, homography); + + //Mat transform = Imgproc.getAffineTransform(cornersObjectMatrix, cornersSceneMatrix); + + //Draw the lines of the object on the scene + Point[] cornersScene = cornersSceneMatrix.toArray(); + final ColorRGBA lineColor = new ColorRGBA("#00ff00"); + Drawing.drawLine(output, new Point(cornersScene[0].x + objectAnalysis.object.cols(), cornersScene[0].y), + new Point(cornersScene[1].x + objectAnalysis.object.cols(), cornersScene[1].y), lineColor, 5); + Drawing.drawLine(output, new Point(cornersScene[1].x + objectAnalysis.object.cols(), cornersScene[1].y), + new Point(cornersScene[2].x + objectAnalysis.object.cols(), cornersScene[2].y), lineColor, 5); + Drawing.drawLine(output, new Point(cornersScene[2].x + objectAnalysis.object.cols(), cornersScene[2].y), + new Point(cornersScene[3].x + objectAnalysis.object.cols(), cornersScene[3].y), lineColor, 5); + Drawing.drawLine(output, new Point(cornersScene[3].x + objectAnalysis.object.cols(), cornersScene[3].y), + new Point(cornersScene[0].x + objectAnalysis.object.cols(), cornersScene[0].y), lineColor, 5); + } + + public static void drawDebugInfo(Mat output, SceneAnalysis sceneAnalysis) { + Transform.flip(output, Transform.FlipType.FLIP_ACROSS_Y); + Drawing.drawText(output, "Keypoints: " + sceneAnalysis.keypoints.rows(), new Point(0, 8), 1.0f, new ColorRGBA(255, 255, 255), Drawing.Anchor.BOTTOMLEFT_UNFLIPPED_Y); + Transform.flip(output, Transform.FlipType.FLIP_ACROSS_Y); } /** * Analyzes an object in preparation to search for the object in a frame. - * + *

* This method should be called in an initialize() method. * Calling the analyzeObject method twice will overwrite the previous objectAnalysis. - * + *

* It is recommended to use a GFTT (Good Features To Track) detector for this phase. + * * @param object Object image * @return The object descriptor matrix to be piped into locateObject() later */ - public ObjectAnalysis analyzeObject(Mat object) throws IllegalArgumentException - { + public ObjectAnalysis analyzeObject(Mat object) throws IllegalArgumentException { Mat descriptors = new Mat(); MatOfKeyPoint keypoints = new MatOfKeyPoint(); Log.d("FTCVision", "Analyzing object..."); - if (object == null || object.empty()) - { + if (object == null || object.empty()) { throw new IllegalArgumentException("Object image cannot be empty!"); } @@ -174,13 +146,12 @@ public ObjectAnalysis analyzeObject(Mat object) throws IllegalArgumentException /** * Analyzes a scene for a target object. - * @param scene The scene to be analyzed as a GRAYSCALE matrix + * + * @param scene The scene to be analyzed as a GRAYSCALE matrix * @param analysis The target object's analysis from analyzeObject - * @param output The output matrix, typically the rgba form of the scene matrix * @return A complete scene analysis */ - public SceneAnalysis analyzeScene(Mat scene, ObjectAnalysis analysis, Mat output) throws IllegalArgumentException - { + public SceneAnalysis analyzeScene(Mat scene, ObjectAnalysis analysis) throws IllegalArgumentException { MatOfKeyPoint keypointsScene = new MatOfKeyPoint(); //DETECT KEYPOINTS in scene @@ -194,7 +165,7 @@ public SceneAnalysis analyzeScene(Mat scene, ObjectAnalysis analysis, Mat output throw new IllegalArgumentException("Analysis must not be null!"); } - if(analysis.descriptors.cols() != descriptorsScene.cols() || analysis.descriptors.type() != descriptorsScene.type()) { + if (analysis.descriptors.cols() != descriptorsScene.cols() || analysis.descriptors.type() != descriptorsScene.type()) { throw new IllegalArgumentException("Object and scene descriptors do not match in cols() or type()."); } @@ -213,79 +184,117 @@ public SceneAnalysis analyzeScene(Mat scene, ObjectAnalysis analysis, Mat output return new SceneAnalysis(keypointsScene, descriptorsScene, matches, scene); } - /** - * Draw keypoints directly onto an scene image - red circles indicate keypoints - * @param output The scene matrix - * @param sceneAnalysis Analysis of the scene, as given by analyzeScene() - */ - public static void drawKeypoints(Mat output, SceneAnalysis sceneAnalysis) - { - KeyPoint[] keypoints = sceneAnalysis.keypoints.toArray(); - for (KeyPoint kp : keypoints) { - Drawing.drawCircle(output, new Point(kp.pt.x, kp.pt.y), 4, new ColorRGBA(255, 0, 0)); - } - } + public enum FeatureDetectorType { + FAST(1), + STAR(2), + //SIFT(3), + //SURF(4), + ORB(5), + MSER(6), + GFTT(7), + HARRIS(8), + SIMPLEBLOB(9), + DENSE(10), + BRISK(11), + FAST_DYNAMIC(1, true), + STAR_DYNAMIC(2, true), + //SIFT_DYNAMIC(3, true), + //SURF_DYNAMIC(4, true), + ORB_DYNAMIC(5, true), + MSER_DYNAMIC(6, true), + GFTT_DYNAMIC(7, true), + HARRIS_DYNAMIC(8, true), + SIMPLEBLOB_DYNAMIC(9, true), + DENSE_DYNAMIC(10, true), + BRISK_DYNAMIC(11, true); - public static void drawObjectLocation(Mat output, ObjectAnalysis objectAnalysis, SceneAnalysis sceneAnalysis) - { - List ptsObject = new ArrayList<>(); - List ptsScene = new ArrayList<>(); + private final int m; - KeyPoint[] keypointsObject = objectAnalysis.keypoints.toArray(); - KeyPoint[] keypointsScene = sceneAnalysis.keypoints.toArray(); + FeatureDetectorType(int type) { + m = type; + } - DMatch[] matches = sceneAnalysis.matches.toArray(); + FeatureDetectorType(int type, boolean dynamic) { + m = type + (dynamic ? 3000 : 0); + } - for (DMatch matche : matches) { - //Get the keypoints from thes matches - ptsObject.add(keypointsObject[matche.queryIdx].pt); - ptsScene.add(keypointsScene[matche.trainIdx].pt); + public int val() { + return m; } + } - MatOfPoint2f matObject = new MatOfPoint2f(); - matObject.fromList(ptsObject); + public enum DescriptorExtractorType { + //SIFT(1), + //SURF(2), + ORB(3), + BRIEF(4), + BRISK(5), + FREAK(6), + //SIFT_OPPONENT(1, true), + //SURF_OPPONENT(2, true), + ORB_OPPONENT(3, true), + BRIEF_OPPONENT(4, true), + BRISK_OPPONENT(5, true), + FREAK_OPPONENT(6, true); - MatOfPoint2f matScene = new MatOfPoint2f(); - matScene.fromList(ptsScene); + private final int m; - //Calculate homography of object in scene - Mat homography = Calib3d.findHomography(matObject, matScene, Calib3d.RANSAC, 5.0f); + DescriptorExtractorType(int type) { + m = type; + } - //Create the unscaled array of corners, representing the object size - Point cornersObject[] = new Point[4]; - cornersObject[0] = new Point(0, 0); - cornersObject[1] = new Point(objectAnalysis.object.cols(), 0); - cornersObject[2] = new Point(objectAnalysis.object.cols(), objectAnalysis.object.rows()); - cornersObject[3] = new Point(0, objectAnalysis.object.rows()); + DescriptorExtractorType(int type, boolean opponent) { + m = type + (opponent ? 1000 : 0); + } - Point[] cornersSceneTemp = new Point[0]; + public int val() { + return m; + } + } - MatOfPoint2f cornersSceneMatrix = new MatOfPoint2f(cornersSceneTemp); - MatOfPoint2f cornersObjectMatrix = new MatOfPoint2f(cornersObject); + public enum DescriptorMatcherType { + FLANN(1), + BRUTEFORCE(2), + BRUTEFORCE_L1(3), + BRUTEFORCE_HAMMING(4), + BRUTEFORCE_HAMMINGLUT(5), + BRUTEFORCE_SL2(6); - //Transform the object coordinates to the scene coordinates by the homography matrix - Core.perspectiveTransform(cornersObjectMatrix, cornersSceneMatrix, homography); + private final int m; - //Mat transform = Imgproc.getAffineTransform(cornersObjectMatrix, cornersSceneMatrix); + DescriptorMatcherType(int type) { + m = type; + } - //Draw the lines of the object on the scene - Point[] cornersScene = cornersSceneMatrix.toArray(); - final ColorRGBA lineColor = new ColorRGBA("#00ff00"); - Drawing.drawLine(output, new Point(cornersScene[0].x + objectAnalysis.object.cols(), cornersScene[0].y), - new Point(cornersScene[1].x + objectAnalysis.object.cols(), cornersScene[1].y), lineColor, 5); - Drawing.drawLine(output, new Point(cornersScene[1].x + objectAnalysis.object.cols(), cornersScene[1].y), - new Point(cornersScene[2].x + objectAnalysis.object.cols(), cornersScene[2].y), lineColor, 5); - Drawing.drawLine(output, new Point(cornersScene[2].x + objectAnalysis.object.cols(), cornersScene[2].y), - new Point(cornersScene[3].x + objectAnalysis.object.cols(), cornersScene[3].y), lineColor, 5); - Drawing.drawLine(output, new Point(cornersScene[3].x + objectAnalysis.object.cols(), cornersScene[3].y), - new Point(cornersScene[0].x + objectAnalysis.object.cols(), cornersScene[0].y), lineColor, 5); + public int val() { + return m; + } } - public static void drawDebugInfo(Mat output, SceneAnalysis sceneAnalysis) - { - Transform.flip(output, Transform.FlipType.FLIP_ACROSS_Y); - Drawing.drawText(output, "Keypoints: " + sceneAnalysis.keypoints.rows(), new Point(0, 8), 1.0f, new ColorRGBA(255, 255, 255), Drawing.Anchor.BOTTOMLEFT_UNFLIPPED_Y); - Transform.flip(output, Transform.FlipType.FLIP_ACROSS_Y); + public final class ObjectAnalysis { + MatOfKeyPoint keypoints; + Mat descriptors; + Mat object; + + ObjectAnalysis(MatOfKeyPoint keypoints, Mat descriptors, Mat object) { + this.keypoints = keypoints; + this.descriptors = descriptors; + this.object = object; + } + } + + public final class SceneAnalysis { + MatOfKeyPoint keypoints; + Mat descriptors; + MatOfDMatch matches; + Mat scene; + + SceneAnalysis(MatOfKeyPoint keypoints, Mat descriptors, MatOfDMatch matches, Mat scene) { + this.keypoints = keypoints; + this.descriptors = descriptors; + this.matches = matches; + this.scene = scene; + } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/PrimitiveDetection.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/PrimitiveDetection.java similarity index 85% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/PrimitiveDetection.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/PrimitiveDetection.java index b40fb099..66f70cbc 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/PrimitiveDetection.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/PrimitiveDetection.java @@ -1,10 +1,10 @@ -package org.lasarobotics.vision.detection; +package org.lasarobotics.vision.test.detection; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.detection.objects.Ellipse; -import org.lasarobotics.vision.detection.objects.Rectangle; -import org.lasarobotics.vision.image.Filter; -import org.lasarobotics.vision.util.MathUtil; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.detection.objects.Ellipse; +import org.lasarobotics.vision.test.detection.objects.Rectangle; +import org.lasarobotics.vision.test.image.Filter; +import org.lasarobotics.vision.test.util.MathUtil; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; import org.opencv.core.MatOfPoint2f; @@ -23,32 +23,7 @@ public class PrimitiveDetection { private static final double MAX_COSINE_VALUE = 0.5; private static final double EPLISON_APPROX_TOLERANCE_FACTOR = 0.02; - public class RectangleLocationResult - { - List contours; - List ellipses; - - RectangleLocationResult(List contours, List ellipses) - { - this.contours = contours; - this.ellipses = ellipses; - } - - public List getContours() - { - return contours; - } - - public List getRectangles() - { - return ellipses; - } - } - - //TODO convert this to locatePolygons() with n sides - //TODO see http://opencv-code.com/tutorials/detecting-simple-shapes-in-an-image/ - - public RectangleLocationResult locateRectangles(Mat grayImage, Mat output) { + public RectangleLocationResult locateRectangles(Mat grayImage) { Mat gray = grayImage.clone(); //Filter out some noise @@ -106,30 +81,10 @@ public RectangleLocationResult locateRectangles(Mat grayImage, Mat output) { return new RectangleLocationResult(contours, rectangles); } - public class EllipseLocationResult - { - List contours; - List ellipses; - - EllipseLocationResult(List contours, List ellipses) - { - this.contours = contours; - this.ellipses = ellipses; - } - - public List getContours() - { - return contours; - } - - public List getEllipses() - { - return ellipses; - } - } + //TODO convert this to locatePolygons() with n sides + //TODO see http://opencv-code.com/tutorials/detecting-simple-shapes-in-an-image/ - public EllipseLocationResult locateEllipses(Mat grayImage) - { + public EllipseLocationResult locateEllipses(Mat grayImage) { Mat gray = grayImage.clone(); Filter.downsample(gray, 2); @@ -146,13 +101,13 @@ public EllipseLocationResult locateEllipses(Mat grayImage) //List contours List contours = new ArrayList<>(); - for (MatOfPoint co : contoursTemp ) { + for (MatOfPoint co : contoursTemp) { contours.add(new Contour(co)); } //Find ellipses by finding fit List ellipses = new ArrayList<>(); - for (MatOfPoint co : contoursTemp ) { + for (MatOfPoint co : contoursTemp) { contours.add(new Contour(co)); //Contour must have at least 6 points for fitEllipse if (co.toArray().length < 6) @@ -168,4 +123,40 @@ public EllipseLocationResult locateEllipses(Mat grayImage) return new EllipseLocationResult(contours, ellipses); } + + public class RectangleLocationResult { + List contours; + List ellipses; + + RectangleLocationResult(List contours, List ellipses) { + this.contours = contours; + this.ellipses = ellipses; + } + + public List getContours() { + return contours; + } + + public List getRectangles() { + return ellipses; + } + } + + public class EllipseLocationResult { + List contours; + List ellipses; + + EllipseLocationResult(List contours, List ellipses) { + this.contours = contours; + this.ellipses = ellipses; + } + + public List getContours() { + return contours; + } + + public List getEllipses() { + return ellipses; + } + } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Contour.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Contour.java similarity index 59% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Contour.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Contour.java index a998edb3..f41b3f40 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Contour.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Contour.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.detection.objects; +package org.lasarobotics.vision.test.detection.objects; import org.opencv.core.MatOfPoint; import org.opencv.core.MatOfPoint2f; @@ -14,130 +14,129 @@ public class Contour extends Detectable { MatOfPoint mat; - public Contour(MatOfPoint data) - { + public Contour(MatOfPoint data) { this.mat = data; } - public Contour(MatOfPoint2f data) - { + + public Contour(MatOfPoint2f data) { this.mat = new MatOfPoint(data.toArray()); } - public MatOfPoint getData() - { + public MatOfPoint getData() { return mat; } - public MatOfPoint2f getFloatData() - { + public MatOfPoint2f getFloatData() { return new MatOfPoint2f(mat.toArray()); } - public double area() - { + public double area() { return Imgproc.contourArea(mat); } /** * Tests if the contour is closed (convex) + * * @return True if closed (convex), false otherwise */ - public boolean isClosed() - { + public boolean isClosed() { return Imgproc.isContourConvex(mat); } - public Point center() - { + public Point center() { //TODO this is an UNWEIGHTED CENTER - which means that for unusual shapes, it can be inaccurate Point sum = new Point(0, 0); Point[] points = this.getPoints(); - for(Point p : points) + for (Point p : points) sum = new Point(sum.x + p.x, sum.y + p.y); return new Point(sum.x / points.length, sum.y / points.length); } - public double height() - { - return (int)size().height; + public double height() { + return (int) size().height; } - public double width() - { - return (int)size().width; + + public double width() { + return (int) size().width; } - public double top() - { + public double top() { return topLeft().y; } - public double bottom() - { + + public double bottom() { return topLeft().y + size().height; } - public double left() - { + + public double left() { return topLeft().x; } - public double right() - { + + public double right() { return topLeft().x + size().width; } - public Rect getBoundingRect() - { - return new Rect((int)top(), (int)left(), (int)width(), (int)height()); + + public Rect getBoundingRect() { + return new Rect((int) top(), (int) left(), (int) width(), (int) height()); } - public Point bottomRight() - { + public Point bottomRight() { return new Point(right(), bottom()); } /** * Gets the top-left corner of the contour + * * @return The top left corner of the contour */ - public Point topLeft() - { + public Point topLeft() { double minX = Double.MAX_VALUE; double minY = Double.MAX_VALUE; Point[] points = getPoints(); - for (Point p : points) - { - if (p.x < minX) { minX = p.x; } - if (p.y < minY) { minY = p.y; } + for (Point p : points) { + if (p.x < minX) { + minX = p.x; + } + if (p.y < minY) { + minY = p.y; + } } return new Point(minX, minY); } - public Size size() - { + public Size size() { double minX = Double.MAX_VALUE; double maxX = Double.MIN_VALUE; double minY = Double.MAX_VALUE; double maxY = Double.MIN_VALUE; Point[] points = getPoints(); - for (Point p : points) - { - if (p.x < minX) { minX = p.x; } - if (p.y < minY) { minY = p.y; } - if (p.x > maxX) { maxX = p.x; } - if (p.y > maxY) { maxY = p.y; } + for (Point p : points) { + if (p.x < minX) { + minX = p.x; + } + if (p.y < minY) { + minY = p.y; + } + if (p.x > maxX) { + maxX = p.x; + } + if (p.y > maxY) { + maxY = p.y; + } } return new Size(maxX - minX, maxY - minY); } - public double arcLength(boolean closed) - { + public double arcLength(boolean closed) { return Imgproc.arcLength(getFloatData(), closed); } - public Point[] getPoints() - { + public Point[] getPoints() { return mat.toArray(); } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Detectable.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Detectable.java old mode 100755 new mode 100644 similarity index 65% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Detectable.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Detectable.java index fc219b09..707e768b --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Detectable.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Detectable.java @@ -1,10 +1,8 @@ -package org.lasarobotics.vision.detection.objects; +package org.lasarobotics.vision.test.detection.objects; -import org.lasarobotics.vision.image.Drawing; -import org.lasarobotics.vision.util.MathUtil; -import org.lasarobotics.vision.util.color.Color; -import org.lasarobotics.vision.util.color.ColorRGBA; -import org.lasarobotics.vision.util.color.ColorSpace; +import org.lasarobotics.vision.test.util.MathUtil; +import org.lasarobotics.vision.test.util.color.Color; +import org.lasarobotics.vision.test.util.color.ColorSpace; import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.Point; @@ -14,30 +12,33 @@ */ public abstract class Detectable { public abstract double left(); + public abstract double right(); + public abstract double top(); + public abstract double bottom(); public abstract double width(); + public abstract double height(); - public Point topLeft() - { + public Point topLeft() { return new Point(left(), top()); } - public Point bottomRight() - { + + public Point bottomRight() { return new Point(right(), bottom()); } /** * Gets the average color of the contour - * @param img The image matrix, of any color size + * + * @param img The image matrix, of any color size * @param imgSpace The image's color space * @return The average color of the region */ - public Color averageColor(Mat img, ColorSpace imgSpace) - { + public Color averageColor(Mat img, ColorSpace imgSpace) { //Coerce values to stay within screen dimensions double leftX = MathUtil.coerce(0, img.cols() - 1, left()); double rightX = MathUtil.coerce(0, img.cols() - 1, right()); @@ -46,8 +47,8 @@ public Color averageColor(Mat img, ColorSpace imgSpace) double bottomY = MathUtil.coerce(0, img.rows() - 1, bottom()); //Input points into array for calculation - //TODO rectangular submatrix-basad calculation isn't perfectly accurate when you have ellipses or weird shapes - Mat subMat = img.submat((int)topY, (int)bottomY, (int)leftX, (int)rightX); + //TODO rectangular submatrix-based calculation isn't perfectly accurate when you have ellipses or weird shapes + Mat subMat = img.submat((int) topY, (int) bottomY, (int) leftX, (int) rightX); //Calculate average and return new color instance return Color.create(Core.mean(subMat), imgSpace); diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Ellipse.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Ellipse.java old mode 100755 new mode 100644 similarity index 74% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Ellipse.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Ellipse.java index 879ad434..686c5643 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Ellipse.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Ellipse.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.detection.objects; +package org.lasarobotics.vision.test.detection.objects; import org.jetbrains.annotations.NotNull; import org.opencv.core.Point; @@ -9,10 +9,9 @@ * Implements a single ellipse (acts like RotatedRect) with advanced measurement utilities */ public class Ellipse extends Detectable implements Comparable { - private RotatedRect rect; + private final RotatedRect rect; - public Ellipse(RotatedRect rect) - { + public Ellipse(RotatedRect rect) { this.rect = rect; } @@ -20,82 +19,82 @@ public RotatedRect getRect() { return rect; } - public Size size() - { + public Size size() { return rect.size; } - public double height() - { + + public double height() { return size().height; } - public double width() - { + + public double width() { return size().width; } - public double angle() - { + + public double angle() { return rect.angle; } - public Point center() - { + + public Point center() { return rect.center; } - public double left() - { - return center().x -( width()/2); + + public double left() { + return center().x - (width() / 2); } - public double right() - { - return center().x +( width()/2); + + public double right() { + return center().x + (width() / 2); } - public double top() - { - return center().y-(height()/2); + + public double top() { + return center().y - (height() / 2); } - public double bottom() - { - return center().y+(height()/2); + + public double bottom() { + return center().y + (height() / 2); } /** * Gets the area of the ellipse + * * @return Area = semi-major axis * semi-minor axis * PI */ - public double area() - { + public double area() { return semiMajorAxis() * semiMinorAxis() * Math.PI; } - public double majorAxis() - { + + public double majorAxis() { return Math.max(height(), width()); } - public double minorAxis() - { + + public double minorAxis() { return Math.min(height(), width()); } - public double semiMajorAxis() - { + + public double semiMajorAxis() { return majorAxis() / 2; } - public double semiMinorAxis() - { + + public double semiMinorAxis() { return minorAxis() / 2; } + /** * Gets the first flattening ratio for the ellipse + * * @return First flattening ratio = (a-b)/a, where a=semi-major axis and b=semi-minor axis) */ - public double flattening() - { + public double flattening() { return (semiMajorAxis() - semiMinorAxis()) / semiMajorAxis(); } /** * Scale this ellipse by a scaling factor about its center + * * @param factor Scaling factor, 1 for no scale, less than one to contract, greater than one to expand */ - public Ellipse scale(double factor) - { + public Ellipse scale(double factor) { RotatedRect r = rect.clone(); r.size = new Size(factor * rect.size.width, factor * rect.size.height); return new Ellipse(r); @@ -103,15 +102,16 @@ public Ellipse scale(double factor) /** * Gets the eccentricity of the ellipse, between 0 (inclusive) and 1 (exclusive) + * * @return e = sqrt(1-(b^2/a^2)), where a=semi-major axis and b=semi-minor axis */ - public double eccentricity() - { - return Math.sqrt(1 - (semiMinorAxis()*semiMinorAxis())/(semiMajorAxis()*semiMajorAxis())); + public double eccentricity() { + return Math.sqrt(1 - (semiMinorAxis() * semiMinorAxis()) / (semiMajorAxis() * semiMajorAxis())); } /** * Returns true if the ellipse is ENTIRELY inside the contour + * * @param contour The contour to test against * @return True if the ellipse is entirely inside the contour, false otherwise */ @@ -126,6 +126,7 @@ public boolean isInside(Contour contour) { /*** * Compare ellipses by area + * * @param another Another ellipse * @return 1 if this is larger, -1 if another is larger, 0 otherwise */ diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Rectangle.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Rectangle.java similarity index 58% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Rectangle.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Rectangle.java index 4e7e5f52..ec4af3db 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/detection/objects/Rectangle.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/detection/objects/Rectangle.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.detection.objects; +package org.lasarobotics.vision.test.detection.objects; import org.opencv.core.Point; import org.opencv.core.Rect; @@ -11,86 +11,91 @@ public class Rectangle extends Detectable { RotatedRect rect = new RotatedRect(); - public Rectangle(RotatedRect rect) - { + public Rectangle(RotatedRect rect) { this.rect = rect; } - public Rectangle(Rect rect) - { + + public Rectangle(Rect rect) { setRect(rect); } - public Rectangle(Point[] points) - { + + public Rectangle(Point[] points) { //Find top-left and bottom-right Point min = new Point(Double.MAX_VALUE, Double.MAX_VALUE); Point max = new Point(Double.MIN_VALUE, Double.MIN_VALUE); - for(Point p :points) - { - if (p.x < min.x) { min.x = p.x; } - if (p.y < min.y) { min.y = p.y; } - if (p.x > max.x) { max.x = p.x; } - if (p.y > max.y) { max.y = p.y; } + for (Point p : points) { + if (p.x < min.x) { + min.x = p.x; + } + if (p.y < min.y) { + min.y = p.y; + } + if (p.x > max.x) { + max.x = p.x; + } + if (p.y > max.y) { + max.y = p.y; + } } setRect(new Rect(min, max)); } - private void setRect(Rect rect) - { - this.rect = new RotatedRect(new Point(rect.tl().x + rect.size().width/2, - rect.tl().y + rect.size().height/2), rect.size(), 0.0); + private void setRect(Rect rect) { + this.rect = new RotatedRect(new Point(rect.tl().x + rect.size().width / 2, + rect.tl().y + rect.size().height / 2), rect.size(), 0.0); } public RotatedRect getRotatedRect() { return rect; } + public Rect getBoundingRect() { return rect.boundingRect(); } - public Size size() - { + public Size size() { return rect.size; } - public double height() - { + + public double height() { return size().height; } - public double width() - { + + public double width() { return size().width; } - public double angle() - { + + public double angle() { return rect.angle; } - public Point center() - { + + public Point center() { return rect.center; } - public double left() - { - return center().x -( width()/2); + + public double left() { + return center().x - (width() / 2); } - public double right() - { - return center().x +( width()/2); + + public double right() { + return center().x + (width() / 2); } - public double top() - { - return center().y-(height()/2); + + public double top() { + return center().y - (height() / 2); } - public double bottom() - { - return center().y+(height()/2); + + public double bottom() { + return center().y + (height() / 2); } - public double area() - { + public double area() { return width() * height(); } /** * Returns true if the ellipse is ENTIRELY inside the contour + * * @param contour The contour to test against * @return True if the ellipse is entirely inside the contour, false otherwise */ diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/Beacon.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/Beacon.java old mode 100755 new mode 100644 similarity index 82% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/Beacon.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/Beacon.java index 1da969a9..799ec2e2 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/Beacon.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/Beacon.java @@ -1,12 +1,11 @@ -package org.lasarobotics.vision.ftc.resq; - -import org.lasarobotics.vision.detection.PrimitiveDetection; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.detection.objects.Ellipse; -import org.lasarobotics.vision.image.Drawing; -import org.lasarobotics.vision.util.MathUtil; -import org.lasarobotics.vision.util.color.ColorGRAY; -import org.lasarobotics.vision.util.color.ColorRGBA; +package org.lasarobotics.vision.test.ftc.resq; + +import org.lasarobotics.vision.test.detection.PrimitiveDetection; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.detection.objects.Ellipse; +import org.lasarobotics.vision.test.image.Drawing; +import org.lasarobotics.vision.test.util.MathUtil; +import org.lasarobotics.vision.test.util.color.ColorRGBA; import org.opencv.core.Mat; import org.opencv.core.Point; import org.opencv.core.Size; @@ -19,135 +18,20 @@ */ public final class Beacon { - public enum BeaconColor - { - RED, - BLUE, - RED_BRIGHT, - BLUE_BRIGHT, - UNKNOWN; - - @Override - public String toString() { - switch (this) - { - case RED: - return "red"; - case BLUE: - return "blue"; - case RED_BRIGHT: - return "RED!"; - case BLUE_BRIGHT: - return "BLUE!"; - case UNKNOWN: - default: - return "???"; - } - } - } - - public static class BeaconAnalysis - { - private BeaconColor left; - private BeaconColor right; - private double confidence; - - //TODO Color and CONFIDENCE should make up the results - - //TODO add Size size, Point locationTopLeft, Distance distanceApprox - public BeaconAnalysis() - { - assert left != null; - assert right != null; - this.left = BeaconColor.UNKNOWN; - this.right = BeaconColor.UNKNOWN; - this.confidence = 0.0f; - } - - public BeaconAnalysis(BeaconColor left, BeaconColor right, double confidence) - { - assert left != null; - assert right != null; - this.left = left; - this.right = right; - this.confidence = confidence; - } - - public BeaconColor getStateLeft() - { - return left; - } - - public BeaconColor getStateRight() - { - return right; - } - - public double getConfidence() - { - return MathUtil.coerce(0, 1, confidence); - } - public String getConfidenceString() - { - final DecimalFormat format = new DecimalFormat("0.000"); - return format.format(getConfidence() * 100.0f) + "%"; - } - - public boolean isLeftKnown() - { - return left != BeaconColor.UNKNOWN; - } - public boolean isLeftBlue() - { - return (left == BeaconColor.BLUE_BRIGHT || left == BeaconColor.BLUE); - } - public boolean isLeftRed() - { - return (left == BeaconColor.RED_BRIGHT || left == BeaconColor.RED); - } - public boolean isRightKnown() - { - return right != BeaconColor.UNKNOWN; - } - public boolean isRightBlue() - { - return (right == BeaconColor.BLUE_BRIGHT || right == BeaconColor.BLUE); - } - public boolean isRightRed() - { - return (right == BeaconColor.RED_BRIGHT || right == BeaconColor.RED); - } - public boolean isBeaconFound() - { - return isLeftKnown() && isRightKnown(); - } - public boolean isBeaconFullyLit() - { - return (left == BeaconColor.BLUE_BRIGHT || left == BeaconColor.RED_BRIGHT ) && - (right == BeaconColor.BLUE_BRIGHT || right == BeaconColor.RED_BRIGHT ); - } - - @Override - public String toString() { - return left.toString() + ", " + right.toString(); - } - } - - public BeaconAnalysis analyzeColor(List contoursRed, List contoursBlue, Mat img, Mat gray) - { + public BeaconAnalysis analyzeColor(List contoursRed, List contoursBlue, Mat img, Mat gray) { //The idea behind the SmartScoring algorithm is that the largest score in each contour/ellipse set will become the best //DONE First, ellipses and contours are are detected and pre-filtered to remove eccentricities //Second, ellipses, and contours are scored independently based on size and color ... higher score is better //TODO Third, comparative analysis is used on each ellipse and contour to create a score for the contours - //Ellipses within rectangles strongly increase in value - //Ellipses without nearby/contained contours are removed - //Ellipses with nearby/contained contours associate themselves with the contour - //Pairs of ellipses (those with similar size and x-position) greatly increase the associated contours' value - //Contours without nearby/contained ellipses lose value - //Contours near another contour of the opposite color increase in value - //Contours and ellipses near the expected area (if any expected area) increase in value + //Ellipses within rectangles strongly increase in value + //Ellipses without nearby/contained contours are removed + //Ellipses with nearby/contained contours associate themselves with the contour + //Pairs of ellipses (those with similar size and x-position) greatly increase the associated contours' value + //Contours without nearby/contained ellipses lose value + //Contours near another contour of the opposite color increase in value + //Contours and ellipses near the expected area (if any expected area) increase in value //Finally, a fraction of the ellipse value is added to the value of the contour - //The best ellipse is found first, then only this ellipse adds to the value + //The best ellipse is found first, then only this ellipse adds to the value //TODO The best contour from each color (if available) is selected as red and blue //TODO The two best contours are then used to calculate the location of the beacon @@ -220,7 +104,7 @@ public BeaconAnalysis analyzeColor(List contoursRed, List cont //Get beacon width on screen by extrapolating from height final double beaconActualHeight = Constants.BEACON_HEIGHT; //cm, only the lit up portion - 14.0 for entire - final double beaconActualWidth = Constants.BEACON_WIDTH; //cm + final double beaconActualWidth = Constants.BEACON_WIDTH; //cm final double beaconWidthHeightRatio = Constants.BEACON_WH_RATIO; double beaconWidth = beaconHeight * beaconWidthHeightRatio; Size beaconSize = new Size(beaconWidth, beaconHeight); @@ -247,16 +131,13 @@ else if (bestBlue == null) //Test which side is red and blue //If the distance between the sides is smaller than a value, then return unknown - final int xMinDistance = (int)(Constants.DETECTION_MIN_DISTANCE * beaconSize.width); //percent of beacon width + final int xMinDistance = (int) (Constants.DETECTION_MIN_DISTANCE * beaconSize.width); //percent of beacon width boolean leftIsRed; if (largestRedCenter.x + xMinDistance < largestBlueCenter.x) { leftIsRed = true; - } - else if (largestBlueCenter.y + xMinDistance < largestRedCenter.x) { + } else if (largestBlueCenter.y + xMinDistance < largestRedCenter.x) { leftIsRed = false; - } - else - { + } else { return new BeaconAnalysis(BeaconColor.UNKNOWN, BeaconColor.UNKNOWN, 0.0f); } @@ -278,7 +159,7 @@ else if (largestBlueCenter.y + xMinDistance < largestRedCenter.x) { Math.min(leftMostContour.top(), rightMostContour.top()); //The largest size ratio of tested over actual is the scale ratio - double scale = Math.max(widthBeacon/beaconActualWidth, heightContours / beaconActualHeight); + double scale = Math.max(widthBeacon / beaconActualWidth, heightContours / beaconActualHeight); //Define size of bounding box by scaling the actual beacon size Size beaconSizeFinal = new Size(beaconActualWidth * scale, beaconActualHeight * scale); @@ -307,4 +188,109 @@ else if (largestBlueCenter.y + xMinDistance < largestRedCenter.x) { else return new BeaconAnalysis(BeaconColor.BLUE, BeaconColor.RED, confidence); } + + public enum BeaconColor { + RED, + BLUE, + RED_BRIGHT, + BLUE_BRIGHT, + UNKNOWN; + + @Override + public String toString() { + switch (this) { + case RED: + return "red"; + case BLUE: + return "blue"; + case RED_BRIGHT: + return "RED!"; + case BLUE_BRIGHT: + return "BLUE!"; + case UNKNOWN: + default: + return "???"; + } + } + } + + public static class BeaconAnalysis { + private final double confidence; + private BeaconColor left; + private BeaconColor right; + + //TODO Color and CONFIDENCE should make up the results + + //TODO add Size size, Point locationTopLeft, Distance distanceApprox + public BeaconAnalysis() { + assert left != null; + assert right != null; + this.left = BeaconColor.UNKNOWN; + this.right = BeaconColor.UNKNOWN; + this.confidence = 0.0f; + } + + public BeaconAnalysis(BeaconColor left, BeaconColor right, double confidence) { + assert left != null; + assert right != null; + this.left = left; + this.right = right; + this.confidence = confidence; + } + + public BeaconColor getStateLeft() { + return left; + } + + public BeaconColor getStateRight() { + return right; + } + + public double getConfidence() { + return MathUtil.coerce(0, 1, confidence); + } + + public String getConfidenceString() { + final DecimalFormat format = new DecimalFormat("0.000"); + return format.format(getConfidence() * 100.0f) + "%"; + } + + public boolean isLeftKnown() { + return left != BeaconColor.UNKNOWN; + } + + public boolean isLeftBlue() { + return (left == BeaconColor.BLUE_BRIGHT || left == BeaconColor.BLUE); + } + + public boolean isLeftRed() { + return (left == BeaconColor.RED_BRIGHT || left == BeaconColor.RED); + } + + public boolean isRightKnown() { + return right != BeaconColor.UNKNOWN; + } + + public boolean isRightBlue() { + return (right == BeaconColor.BLUE_BRIGHT || right == BeaconColor.BLUE); + } + + public boolean isRightRed() { + return (right == BeaconColor.RED_BRIGHT || right == BeaconColor.RED); + } + + public boolean isBeaconFound() { + return isLeftKnown() && isRightKnown(); + } + + public boolean isBeaconFullyLit() { + return (left == BeaconColor.BLUE_BRIGHT || left == BeaconColor.RED_BRIGHT) && + (right == BeaconColor.BLUE_BRIGHT || right == BeaconColor.RED_BRIGHT); + } + + @Override + public String toString() { + return left.toString() + ", " + right.toString(); + } + } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/BeaconScoring.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/BeaconScoring.java similarity index 83% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/BeaconScoring.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/BeaconScoring.java index d77c088e..d6c1034d 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/BeaconScoring.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/BeaconScoring.java @@ -1,11 +1,11 @@ -package org.lasarobotics.vision.ftc.resq; +package org.lasarobotics.vision.test.ftc.resq; import org.jetbrains.annotations.NotNull; import org.jetbrains.annotations.Nullable; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.detection.objects.Ellipse; -import org.lasarobotics.vision.util.MathUtil; -import org.lasarobotics.vision.util.color.ColorSpace; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.detection.objects.Ellipse; +import org.lasarobotics.vision.test.util.MathUtil; +import org.lasarobotics.vision.test.util.color.ColorSpace; import org.opencv.core.Mat; import org.opencv.core.Point; import org.opencv.core.Size; @@ -22,118 +22,26 @@ class BeaconScoring { Size imgSize; - BeaconScoring(Size imgSize) - { + BeaconScoring(Size imgSize) { this.imgSize = imgSize; } - static class Scorable implements Comparable - { - double score; - - protected Scorable(double score) - { - this.score = score; - } - - public int compareTo(@NotNull Scorable another) { - //This is an inverted sort - largest first - return this.score > another.score ? -1 : this.score < another.score ? 1 : 0; - } - - public static List sort(List ellipses) - { - Scorable[] scoredEllipses = ellipses.toArray(new Scorable[ellipses.size()]); - Arrays.sort(scoredEllipses); - List finalEllipses = new ArrayList<>(); - Collections.addAll(finalEllipses, scoredEllipses); - return finalEllipses; - } - } - - static class ScoredEllipse extends Scorable - { - Ellipse ellipse; - - ScoredEllipse(Ellipse ellipse, double score) { - super(score); - this.ellipse = ellipse; - } - - public static List getList(List scored) - { - List ellipses = new ArrayList<>(); - for (ScoredEllipse e : scored) - ellipses.add(e.ellipse); - return ellipses; - } - } - - static class ScoredContour extends Scorable - { - Contour contour; - - ScoredContour(Contour contour, double score) { - super(score); - this.contour = contour; - } - - public static List getList(List scored) - { - List contours= new ArrayList<>(); - for (ScoredContour c : scored) - contours.add(c.contour); - return contours; - } - } - - static class AssociatedContour extends Scorable - { - ScoredContour contour; - List ellipses; - - AssociatedContour(ScoredContour contour, List ellipses) - { - super(score(contour, ellipses)); - this.contour = contour; - this.ellipses = ellipses; - } - - private static double score(ScoredContour contour, List ellipses) - { - //The best ellipse is found first, then only this ellipse adds to the value - double ellipseBest = 0.0f; - for (ScoredEllipse ellipse : ellipses) - ellipseBest = Math.max(ellipseBest, ellipse.score); - - //Finally, a fraction of the ellipse value is added to the value of the contour - return contour.score + (Constants.ASSOCIATION_ELLIPSE_SCORE_MULTIPLIER * ellipseBest); - } - - double updateScore() - { - this.score = score(contour, ellipses); - return score; - } - } - - /** * Create a normalized subscore around a current and expected value, using the normalized Normal CDF. - * + *

* This function is derived from the statistical probability of achieving the optimal value. Values that * are less than the best value will return the best subscore, or the bias (unless ignoreSign = true). - * @param value The actual value - if this is < bestValue, the subscore will be maximized to the bias - * @param bestValue The optimal value - * @param variance The variance of the normal CDF function, greater than 0 - * @param bias The bias that the normalized normal CDF is to be multiplied by - the result will not be larger than the bias + * + * @param value The actual value - if this is < bestValue, the subscore will be maximized to the bias + * @param bestValue The optimal value + * @param variance The variance of the normal CDF function, greater than 0 + * @param bias The bias that the normalized normal CDF is to be multiplied by - the result will not be larger than the bias * @param ignoreSign If true, ignore the sign of value - bestValue. If false (default), the calculation of value in the * normal PDF will be performed as Math.max(value - bestValue, 0), ensuring that if value is less than * bestValue, that the PDF will return the bias. * @return The subscore based on the probability of achieving the optimal value */ - private static double createSubscore(double value, double bestValue, double variance, double bias, boolean ignoreSign) - { + private static double createSubscore(double value, double bestValue, double variance, double bias, boolean ignoreSign) { return Math.min(MathUtil.normalPDFNormalized((ignoreSign ? value - bestValue : Math.max((value - bestValue), 0)) / bestValue, variance, 0) * bias, bias); } @@ -142,12 +50,10 @@ List scoreContours(List contours, @Nullable Point estimateLocation, @Nullable Double estimateDistance, Mat rgba, - Mat gray) - { + Mat gray) { List scores = new ArrayList<>(); - for(Contour contour : contours) - { + for (Contour contour : contours) { double score = 1; //Find ratio - the closer it is to the actual ratio of the beacon, the better @@ -177,12 +83,10 @@ List scoreContours(List contours, List scoreEllipses(List ellipses, @Nullable Point estimateLocation, @Nullable Double estimateDistance, - Mat gray) - { + Mat gray) { List scores = new ArrayList<>(); - for(Ellipse ellipse : ellipses) - { + for (Ellipse ellipse : ellipses) { double score = 1; //Find the eccentricity - the closer it is to 0, the better @@ -211,21 +115,18 @@ List scoreEllipses(List ellipses, scores.add(new ScoredEllipse(ellipse, score)); } - return (List)Scorable.sort(scores); + return (List) Scorable.sort(scores); } - List associate(List contours, List ellipses) - { + List associate(List contours, List ellipses) { //Ellipses with nearby/contained contours associate themselves with the contour //Ellipses without nearby/contained contours are removed List associations = new ArrayList<>(); - for (ScoredContour contour : contours) - { + for (ScoredContour contour : contours) { AssociatedContour associatedContour = new AssociatedContour(contour, new ArrayList()); - for (ScoredEllipse ellipse : ellipses) - { + for (ScoredEllipse ellipse : ellipses) { if (ellipse.ellipse.isInside(contour.contour) || (MathUtil.distance(ellipse.ellipse.center(), contour.contour.center()) <= Constants.ASSOCIATION_MAX_DISTANCE * imgSize.width)) @@ -241,23 +142,10 @@ List associate(List contours, List redContours; - List blueContours; - - MultiAssociatedContours(List redContours, List blueContours) - { - this.redContours = redContours; - this.blueContours = blueContours; - } - } - @SuppressWarnings("unchecked") MultiAssociatedContours scoreAssociations(List contoursRed, List contoursBlue, - List ellipses) - { + List ellipses) { List associationsRed = associate(contoursRed, ellipses); List associationsBlue = associate(contoursBlue, ellipses); @@ -269,7 +157,96 @@ MultiAssociatedContours scoreAssociations(List contoursRed, //calculateContourZones() //Finally, the list is sorted by score - return new MultiAssociatedContours((List)AssociatedContour.sort(associationsRed), - (List)AssociatedContour.sort(associationsBlue)); + return new MultiAssociatedContours((List) Scorable.sort(associationsRed), + (List) Scorable.sort(associationsBlue)); + } + + static class Scorable implements Comparable { + double score; + + protected Scorable(double score) { + this.score = score; + } + + public static List sort(List ellipses) { + Scorable[] scoredEllipses = ellipses.toArray(new Scorable[ellipses.size()]); + Arrays.sort(scoredEllipses); + List finalEllipses = new ArrayList<>(); + Collections.addAll(finalEllipses, scoredEllipses); + return finalEllipses; + } + + public int compareTo(@NotNull Scorable another) { + //This is an inverted sort - largest first + return this.score > another.score ? -1 : this.score < another.score ? 1 : 0; + } + } + + static class ScoredEllipse extends Scorable { + Ellipse ellipse; + + ScoredEllipse(Ellipse ellipse, double score) { + super(score); + this.ellipse = ellipse; + } + + public static List getList(List scored) { + List ellipses = new ArrayList<>(); + for (ScoredEllipse e : scored) + ellipses.add(e.ellipse); + return ellipses; + } + } + + static class ScoredContour extends Scorable { + Contour contour; + + ScoredContour(Contour contour, double score) { + super(score); + this.contour = contour; + } + + public static List getList(List scored) { + List contours = new ArrayList<>(); + for (ScoredContour c : scored) + contours.add(c.contour); + return contours; + } + } + + static class AssociatedContour extends Scorable { + ScoredContour contour; + List ellipses; + + AssociatedContour(ScoredContour contour, List ellipses) { + super(score(contour, ellipses)); + this.contour = contour; + this.ellipses = ellipses; + } + + private static double score(ScoredContour contour, List ellipses) { + //The best ellipse is found first, then only this ellipse adds to the value + double ellipseBest = 0.0f; + for (ScoredEllipse ellipse : ellipses) + ellipseBest = Math.max(ellipseBest, ellipse.score); + + //Finally, a fraction of the ellipse value is added to the value of the contour + return contour.score + (Constants.ASSOCIATION_ELLIPSE_SCORE_MULTIPLIER * ellipseBest); + } + + double updateScore() { + this.score = score(contour, ellipses); + return score; + } + } + + static class MultiAssociatedContours { + List redContours; + List blueContours; + + MultiAssociatedContours(List redContours, List blueContours) { + this.redContours = redContours; + this.blueContours = blueContours; + } } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/Constants.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/Constants.java similarity index 92% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/Constants.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/Constants.java index d17a16c8..ced6f690 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/ftc/resq/Constants.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/ftc/resq/Constants.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.ftc.resq; +package org.lasarobotics.vision.test.ftc.resq; /** * Res-Q field and object constants @@ -6,7 +6,7 @@ public abstract class Constants { public static final double BEACON_WIDTH = 21.8; //entire beacon width public static final double BEACON_HEIGHT = 14.5; //entire beacon height - public static final double BEACON_WH_RATIO = BEACON_WIDTH/BEACON_HEIGHT; //entire beacon ratio + public static final double BEACON_WH_RATIO = BEACON_WIDTH / BEACON_HEIGHT; //entire beacon ratio static final double DETECTION_MIN_DISTANCE = 0.05; static final double CONFIDENCE_DIVISOR = 30; diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Calc.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Calc.java similarity index 66% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Calc.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Calc.java index 2bc4d4f8..f24b89ef 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Calc.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Calc.java @@ -1,8 +1,8 @@ -package org.lasarobotics.vision.image; +package org.lasarobotics.vision.test.image; /** * Allows for certain calculations to be made on the image */ public class Calc { - + } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Drawing.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Drawing.java similarity index 80% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Drawing.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Drawing.java index eb5837e4..7215405b 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Drawing.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Drawing.java @@ -1,9 +1,9 @@ -package org.lasarobotics.vision.image; +package org.lasarobotics.vision.test.image; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.detection.objects.Ellipse; -import org.lasarobotics.vision.detection.objects.Rectangle; -import org.lasarobotics.vision.util.color.Color; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.detection.objects.Ellipse; +import org.lasarobotics.vision.test.detection.objects.Rectangle; +import org.lasarobotics.vision.test.util.color.Color; import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.MatOfPoint; @@ -17,52 +17,44 @@ * Methods for drawing shapes onto images */ public class Drawing { - public static void drawCircle(Mat img, Point center, int diameter, Color color) - { + public static void drawCircle(Mat img, Point center, int diameter, Color color) { drawCircle(img, center, diameter, color, 2); } - public static void drawCircle(Mat img, Point center, int diameter, Color color, int thickness) - { + + public static void drawCircle(Mat img, Point center, int diameter, Color color, int thickness) { Imgproc.circle(img, center, diameter, color.getScalarRGBA(), thickness); } - public static void drawEllipse(Mat img, Ellipse ellipse, Color color) - { + + public static void drawEllipse(Mat img, Ellipse ellipse, Color color) { drawEllipse(img, ellipse, color, 2); } - public static void drawEllipse(Mat img, Ellipse ellipse, Color color, int thickness) - { + + public static void drawEllipse(Mat img, Ellipse ellipse, Color color, int thickness) { Imgproc.ellipse(img, ellipse.center(), ellipse.size(), ellipse.angle(), 0, 360, color.getScalarRGBA(), thickness); } - public static void drawEllipses(Mat img, List ellipses, Color color) - { + + public static void drawEllipses(Mat img, List ellipses, Color color) { drawEllipses(img, ellipses, color, 2); } - public static void drawEllipses(Mat img, List ellipses, Color color, int thickness) - { + + public static void drawEllipses(Mat img, List ellipses, Color color, int thickness) { for (Ellipse ellipse : ellipses) drawEllipse(img, ellipse, color, thickness); } - public static void drawArc(Mat img, Ellipse ellipse, Color color, double angleDegrees) - { + + public static void drawArc(Mat img, Ellipse ellipse, Color color, double angleDegrees) { drawArc(img, ellipse, color, angleDegrees, 2); } - public static void drawArc(Mat img, Ellipse ellipse, Color color, double angleDegrees, int thickness) - { + + public static void drawArc(Mat img, Ellipse ellipse, Color color, double angleDegrees, int thickness) { Imgproc.ellipse(img, ellipse.center(), ellipse.size(), ellipse.angle(), 0, angleDegrees, color.getScalarRGBA(), thickness); } - public enum Anchor - { - TOPLEFT, - BOTTOMLEFT, - BOTTOMLEFT_UNFLIPPED_Y - } - public static void drawText(Mat img, String text, Point origin, float scale, Color color) - { + public static void drawText(Mat img, String text, Point origin, float scale, Color color) { drawText(img, text, origin, scale, color, Anchor.TOPLEFT); } - public static void drawText(Mat img, String text, Point origin, float scale, Color color, Anchor locationOnImage) - { + + public static void drawText(Mat img, String text, Point origin, float scale, Color color, Anchor locationOnImage) { if (locationOnImage == Anchor.BOTTOMLEFT) Transform.flip(img, Transform.FlipType.FLIP_ACROSS_Y); Imgproc.putText(img, text, origin, Core.FONT_HERSHEY_SIMPLEX, scale, color.getScalarRGBA(), 2, Core.LINE_8, @@ -71,61 +63,64 @@ public static void drawText(Mat img, String text, Point origin, float scale, Col Transform.flip(img, Transform.FlipType.FLIP_ACROSS_Y); } - public static void drawLine(Mat img, Point point1, Point point2, Color color) - { + public static void drawLine(Mat img, Point point1, Point point2, Color color) { drawLine(img, point1, point2, color, 2); } - public static void drawLine(Mat img, Point point1, Point point2, Color color, int thickness) - { + + public static void drawLine(Mat img, Point point1, Point point2, Color color, int thickness) { Imgproc.line(img, point1, point2, color.getScalarRGBA(), thickness); } - public static void drawContour(Mat img, Contour contour, Color color) - { + public static void drawContour(Mat img, Contour contour, Color color) { drawContour(img, contour, color, 2); } - public static void drawContour(Mat img, Contour contour, Color color, int thickness) - { + + public static void drawContour(Mat img, Contour contour, Color color, int thickness) { List contoursOut = new ArrayList<>(); contoursOut.add(contour.getData()); Imgproc.drawContours(img, contoursOut, -1, color.getScalarRGBA(), thickness); } - public static void drawContours(Mat img, List contours, Color color) - { + + public static void drawContours(Mat img, List contours, Color color) { drawContours(img, contours, color, 2); } - public static void drawContours(Mat img, List contours, Color color, int thickness) - { + + public static void drawContours(Mat img, List contours, Color color, int thickness) { List contoursOut = new ArrayList<>(); for (Contour contour : contours) contoursOut.add(contour.getData()); Imgproc.drawContours(img, contoursOut, -1, color.getScalarRGBA(), thickness); } - public static void drawRectangles(Mat img, List rects, Color color) - { + public static void drawRectangles(Mat img, List rects, Color color) { for (Rectangle r : rects) drawRectangle(img, r.topLeft(), r.bottomRight(), color, 2); } - public static void drawRectangles(Mat img, List rects, Color color, int thickness) - { + + public static void drawRectangles(Mat img, List rects, Color color, int thickness) { for (Rectangle r : rects) drawRectangle(img, r.topLeft(), r.bottomRight(), color, thickness); } - public static void drawRectangle(Mat img, Rectangle rect, Color color) - { + + public static void drawRectangle(Mat img, Rectangle rect, Color color) { drawRectangle(img, rect.topLeft(), rect.bottomRight(), color, 2); } - public static void drawRectangle(Mat img, Rectangle rect, Color color, int thickness) - { + + public static void drawRectangle(Mat img, Rectangle rect, Color color, int thickness) { drawRectangle(img, rect.topLeft(), rect.bottomRight(), color, thickness); } - public static void drawRectangle(Mat img, Point topLeft, Point bottomRight, Color color) - { + + public static void drawRectangle(Mat img, Point topLeft, Point bottomRight, Color color) { drawRectangle(img, topLeft, bottomRight, color, 2); } - public static void drawRectangle(Mat img, Point topLeft, Point bottomRight, Color color, int thickness) - { + + public static void drawRectangle(Mat img, Point topLeft, Point bottomRight, Color color, int thickness) { Imgproc.rectangle(img, topLeft, bottomRight, color.getScalarRGBA(), thickness); } + + public enum Anchor { + TOPLEFT, + BOTTOMLEFT, + BOTTOMLEFT_UNFLIPPED_Y + } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Filter.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Filter.java similarity index 63% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Filter.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Filter.java index 5dc39231..186bf2c1 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Filter.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Filter.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.image; +package org.lasarobotics.vision.test.image; import org.opencv.core.Mat; import org.opencv.core.Point; @@ -9,19 +9,18 @@ * Implements image filtering algorithms */ public class Filter { - public static void blur(Mat img, int amount) - { + public static void blur(Mat img, int amount) { Imgproc.GaussianBlur(img, img, new Size(2 * amount + 1, 2 * amount + 1), 0, 0); } - public static void erode(Mat img, int amount) - { + + public static void erode(Mat img, int amount) { Mat kernel = Imgproc.getStructuringElement(Imgproc.CV_SHAPE_RECT, new Size(2 * amount + 1, 2 * amount + 1), new Point(amount, amount)); Imgproc.erode(img, img, kernel); } - public static void dilate(Mat img, int amount) - { + + public static void dilate(Mat img, int amount) { Mat kernel = Imgproc.getStructuringElement(Imgproc.CV_SHAPE_RECT, new Size(2 * amount + 1, 2 * amount + 1), new Point(amount, amount)); @@ -30,21 +29,21 @@ public static void dilate(Mat img, int amount) /** * Downsample and blur an image (using a Gaussian pyramid kernel) - * @param img The image + * + * @param img The image * @param scale The scale, a number greater than 1 */ - public static void downsample(Mat img, double scale) - { - Imgproc.pyrDown(img, img, new Size((double)img.width() / scale, (double)img.height() / scale)); + public static void downsample(Mat img, double scale) { + Imgproc.pyrDown(img, img, new Size((double) img.width() / scale, (double) img.height() / scale)); } /** * Upsample and blur an image (using a Gaussian pyramid kernel) - * @param img The image + * + * @param img The image * @param scale The scale, a number greater than 1 */ - public static void upsample(Mat img, double scale) - { - Imgproc.pyrUp(img, img, new Size((double)img.width() * scale, (double)img.height() * scale)); + public static void upsample(Mat img, double scale) { + Imgproc.pyrUp(img, img, new Size((double) img.width() * scale, (double) img.height() * scale)); } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Transform.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Transform.java similarity index 75% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Transform.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Transform.java index 43820e1f..fc7b65bb 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/image/Transform.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/image/Transform.java @@ -1,7 +1,6 @@ -package org.lasarobotics.vision.image; +package org.lasarobotics.vision.test.image; -import org.lasarobotics.vision.util.MathUtil; -import org.opencv.android.Utils; +import org.lasarobotics.vision.test.util.MathUtil; import org.opencv.core.Core; import org.opencv.core.Mat; import org.opencv.core.Point; @@ -14,11 +13,11 @@ public class Transform { /** * Rotate an image by an angle (counterclockwise) + * * @param image Transform matrix * @param angle Angle to rotate by (counterclockwise) from -360 to 360 */ - public static void rotate(Mat image, double angle) - { + public static void rotate(Mat image, double angle) { //Calculate size of new matrix double radians = Math.toRadians(angle); double sin = Math.abs(Math.sin(radians)); @@ -28,39 +27,25 @@ public static void rotate(Mat image, double angle) int newHeight = (int) (image.width() * sin + image.height() * cos); // rotating image - Point center = new Point(newWidth/2, newHeight/2); + Point center = new Point(newWidth / 2, newHeight / 2); Mat rotMatrix = Imgproc.getRotationMatrix2D(center, angle, 1.0); //1.0 means 100 % scale Size size = new Size(newWidth, newHeight); Imgproc.warpAffine(image, image, rotMatrix, image.size()); } - public enum FlipType - { - FLIP_ACROSS_Y(0), - FLIP_ACROSS_X(1), - FLIP_BOTH(-1); - - int val; - FlipType(int a) - { - this.val = a; - } - } - - public static void flip(Mat img, FlipType flipType) - { + public static void flip(Mat img, FlipType flipType) { Core.flip(img, img, flipType.val); } /** * Scale an image by a scale factor - * @param img The image to scale + * + * @param img The image to scale * @param factor The scale factor. If greater than 1, image will dilate. Otherwise, * image will constrict. */ - public static void scale(Mat img, double factor) - { + public static void scale(Mat img, double factor) { resize(img, new Size(img.size().width * factor, img.size().height * factor)); } @@ -69,28 +54,28 @@ public static void scale(Mat img, double factor) * on the x and y axis, regardless of the approxSize. * This function will always create the smallest image if the ratio of approxSize * to the image's actual size do not match. - * @param img The image + * + * @param img The image * @param approxSize The target size */ - public static void scale(Mat img, Size approxSize) - { + public static void scale(Mat img, Size approxSize) { scale(img, approxSize, false, false); } /** * Scales an image to an approximate size. The scale will always be equal * on the x and y axis, regardless of the approxSize. - * @param img The image - * @param approxSize The target size - * @param maximize If maximize is true, then if the approxSize aspect ratio - * does not match the target, then the largest possible image - * would be used. If false (default), the the smallest image - * would be used. + * + * @param img The image + * @param approxSize The target size + * @param maximize If maximize is true, then if the approxSize aspect ratio + * does not match the target, then the largest possible image + * would be used. If false (default), the the smallest image + * would be used. * @param integerScale If true (default), then only integer scale factors would be used. * Otherwise, any scale factor can be used. */ - public static void scale(Mat img, Size approxSize, boolean maximize, boolean integerScale) - { + public static void scale(Mat img, Size approxSize, boolean maximize, boolean integerScale) { double scale = makeScale(img, approxSize, maximize, integerScale); scale(img, scale); } @@ -100,11 +85,11 @@ public static void scale(Mat img, Size approxSize, boolean maximize, boolean int * on the x and y axis, regardless of the approxSize. * This function will always create the largest image if the ratio of approxSize * to the image's actual size do not match. - * @param img The image + * + * @param img The image * @param approxSize The target size */ - public static void enlarge(Mat img, Size approxSize) - { + public static void enlarge(Mat img, Size approxSize) { enlarge(img, approxSize, false); } @@ -113,11 +98,11 @@ public static void enlarge(Mat img, Size approxSize) * on the x and y axis, regardless of the approxSize. * This function will always create the smallest image if the ratio of approxSize * to the image's actual size do not match. - * @param img The image + * + * @param img The image * @param approxSize The target size */ - public static void shrink(Mat img, Size approxSize) - { + public static void shrink(Mat img, Size approxSize) { shrink(img, approxSize, false); } @@ -126,16 +111,15 @@ public static void shrink(Mat img, Size approxSize) * on the x and y axis, regardless of the approxSize. * This function will always create the largest image if the ratio of approxSize * to the image's actual size do not match. - * @param img The image - * @param approxSize The target size + * + * @param img The image + * @param approxSize The target size * @param integerScale If true (default), then only integer scale factors would be used. * Otherwise, any scale factor can be used. */ - public static void enlarge(Mat img, Size approxSize, boolean integerScale) - { + public static void enlarge(Mat img, Size approxSize, boolean integerScale) { double scale = makeScale(img, approxSize, true, integerScale); - if (MathUtil.equal(scale, 1) || scale < 1) - { + if (MathUtil.equal(scale, 1) || scale < 1) { return; } scale(img, scale); @@ -146,16 +130,15 @@ public static void enlarge(Mat img, Size approxSize, boolean integerScale) * on the x and y axis, regardless of the approxSize. * This function will always create the smallest image if the ratio of approxSize * to the image's actual size do not match. - * @param img The image - * @param approxSize The target size + * + * @param img The image + * @param approxSize The target size * @param integerScale If true (default), then only integer scale factors would be used. * Otherwise, any scale factor can be used. */ - public static void shrink(Mat img, Size approxSize, boolean integerScale) - { + public static void shrink(Mat img, Size approxSize, boolean integerScale) { double scale = makeScale(img, approxSize, false, integerScale); - if (MathUtil.equal(scale, 1) || scale > 1) - { + if (MathUtil.equal(scale, 1) || scale > 1) { return; } scale(img, scale); @@ -164,25 +147,24 @@ public static void shrink(Mat img, Size approxSize, boolean integerScale) /** * Scales an image to an approximate size. The scale will always be equal * on the x and y axis, regardless of the approxSize. - * @param img The image - * @param approxSize The target size - * @param maximize If maximize is true, then if the approxSize aspect ratio - * does not match the target, then the largest possible image - * would be used. If false (default), the the smallest image - * would be used. + * + * @param img The image + * @param approxSize The target size + * @param maximize If maximize is true, then if the approxSize aspect ratio + * does not match the target, then the largest possible image + * would be used. If false (default), the the smallest image + * would be used. * @param integerScale If true (default), then only integer scale factors would be used. * Otherwise, any scale factor can be used. */ - private static double makeScale(Mat img, Size approxSize, boolean maximize, boolean integerScale) - { + private static double makeScale(Mat img, Size approxSize, boolean maximize, boolean integerScale) { Size imageSize = img.size(); double ratioWidth = approxSize.width / imageSize.width; double ratioHeight = approxSize.height / imageSize.height; double ratio = maximize ? Math.max(ratioWidth, ratioHeight) : Math.min(ratioWidth, ratioHeight); if (MathUtil.equal(ratio, 1)) return 1; - if (integerScale) - { + if (integerScale) { //The scale factor is always greater than 1 double scale = (ratio < 1) ? 1 / ratio : ratio; //If you are actually increasing the size of the object, use ceiling() @@ -190,15 +172,12 @@ private static double makeScale(Mat img, Size approxSize, boolean maximize, bool scale = maximize ^ (ratio < 1) ? Math.ceil(scale) : Math.floor(scale); //Get the actual ratio again return (ratio < 1) ? 1 / scale : scale; - } - else - { + } else { return ratio; } } - public static void resize(Mat img, Size size) - { + public static void resize(Mat img, Size size) { int interpolation; if (MathUtil.equal(size.area(), img.size().area())) return; @@ -210,4 +189,16 @@ else if (size.width < img.size().width && size.height < img.size().height) interpolation = Imgproc.CV_INTER_LINEAR; //not entirely sure, so use safe option Imgproc.resize(img, img, size, 0, 0, interpolation); } + + public enum FlipType { + FLIP_ACROSS_Y(0), + FLIP_ACROSS_X(1), + FLIP_BOTH(-1); + + final int val; + + FlipType(int a) { + this.val = a; + } + } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/LinearVisionOpMode.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/LinearVisionOpMode.java old mode 100755 new mode 100644 similarity index 83% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/LinearVisionOpMode.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/LinearVisionOpMode.java index b3e7a424..5d492b50 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/LinearVisionOpMode.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/LinearVisionOpMode.java @@ -1,10 +1,10 @@ -package org.lasarobotics.vision.opmode; - -/** - * Linear version of the Vision OpMode - NOT YET SUPPORTED - */ -public class LinearVisionOpMode { - public LinearVisionOpMode() { - throw new RuntimeException("Linear OpModes are not yet supported. Please use VisionOpMode."); - } +package org.lasarobotics.vision.test.opmode; + +/** + * Linear version of the Vision OpMode - NOT YET SUPPORTED + */ +public class LinearVisionOpMode { + public LinearVisionOpMode() { + throw new RuntimeException("Linear OpModes are not yet supported. Please use VisionOpMode."); + } } \ No newline at end of file diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/ManualVisionOpMode.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/ManualVisionOpMode.java similarity index 63% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/ManualVisionOpMode.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/ManualVisionOpMode.java index a1c49285..b82da82d 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/ManualVisionOpMode.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/ManualVisionOpMode.java @@ -1,11 +1,11 @@ -package org.lasarobotics.vision.opmode; +package org.lasarobotics.vision.test.opmode; import org.opencv.core.Mat; public abstract class ManualVisionOpMode extends VisionOpModeCore { - public final Mat frame(Mat rgba, Mat gray, boolean ready) - { + public final Mat frame(Mat rgba, Mat gray, boolean ready) { return frame(rgba, gray); } + public abstract Mat frame(Mat rgba, Mat gray); } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionEnabledActivity.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionEnabledActivity.java similarity index 83% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionEnabledActivity.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionEnabledActivity.java index f866fca6..e4320176 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionEnabledActivity.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionEnabledActivity.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.opmode; +package org.lasarobotics.vision.test.opmode; import android.app.Activity; import android.util.Log; @@ -6,8 +6,8 @@ import android.widget.LinearLayout; import org.jetbrains.annotations.Nullable; -import org.lasarobotics.vision.android.Camera; -import org.lasarobotics.vision.android.Cameras; +import org.lasarobotics.vision.test.android.Camera; +import org.lasarobotics.vision.test.android.Cameras; import org.opencv.android.BaseLoaderCallback; import org.opencv.android.CameraBridgeViewBase; import org.opencv.android.JavaCameraView; @@ -25,15 +25,15 @@ public abstract class VisionEnabledActivity extends Activity { @Override public void onManagerConnected(int status) { switch (status) { - case LoaderCallbackInterface.SUCCESS: - { + case LoaderCallbackInterface.SUCCESS: { //Woohoo! - } break; - default: - { + } + break; + default: { super.onManagerConnected(status); - } break; + } + break; } } }; @@ -41,25 +41,22 @@ public void onManagerConnected(int status) { /** * Initialize vision * Call this method from the onCreate() method after the super() - * @param layoutID The ID of the primary layout (e.g. R.id.layout_robotcontroller) - * @param camera The camera to use in vision processing + * + * @param layoutID The ID of the primary layout (e.g. R.id.layout_robotcontroller) + * @param camera The camera to use in vision processing * @param frameSize The target frame size. If null, it will get the optimal size. */ - protected final void initializeVision(int layoutID, Cameras camera, @Nullable Size frameSize) - { + protected final void initializeVision(int layoutID, Cameras camera, @Nullable Size frameSize) { LinearLayout layout = new LinearLayout(this); layout.setOrientation(LinearLayout.VERTICAL); - if (frameSize == null) - { + if (frameSize == null) { Camera c = camera.createCamera(); if (c.doesExist()) { frameSize = c.getBestFrameSize(); c.unlock(); c.release(); - } - else - { + } else { //get some kind of default frameSize = new Size(900, 900); } @@ -83,21 +80,21 @@ protected final void initializeVision(int layoutID, Cameras camera, @Nullable Si /** * Initialize vision * Call this method from the onCreate() method after the super() + * * @param layoutID The ID of the primary layout (e.g. R.id.layout_robotcontroller) - * @param camera The camera to use in vision processing + * @param camera The camera to use in vision processing */ - protected final void initializeVision(int layoutID, Cameras camera) - { + protected final void initializeVision(int layoutID, Cameras camera) { initializeVision(layoutID, camera, null); } /** * Initialize vision * Call this method from the onCreate() method after the super() + * * @param layoutID The ID of the primary layout (e.g. R.id.layout_robotcontroller) */ - protected final void initializeVision(int layoutID) - { + protected final void initializeVision(int layoutID) { initializeVision(layoutID, Cameras.PRIMARY); } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionExtensions.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionExtensions.java similarity index 59% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionExtensions.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionExtensions.java index 2f49bdcd..21dfa1a2 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionExtensions.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionExtensions.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.opmode; +package org.lasarobotics.vision.test.opmode; /** * List of Vision Extensions for the VisionOpMode @@ -6,9 +6,9 @@ public enum VisionExtensions { BEACON_COLOR(1); - int id; - VisionExtensions(int id) - { + final int id; + + VisionExtensions(int id) { this.id = id; } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionOpMode.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionOpMode.java similarity index 76% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionOpMode.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionOpMode.java index f97ce959..4b89916b 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionOpMode.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionOpMode.java @@ -1,7 +1,7 @@ -package org.lasarobotics.vision.opmode; +package org.lasarobotics.vision.test.opmode; -import org.lasarobotics.vision.ftc.resq.Beacon; -import org.lasarobotics.vision.opmode.extensions.BeaconColorExtension; +import org.lasarobotics.vision.test.ftc.resq.Beacon; +import org.lasarobotics.vision.test.opmode.extensions.BeaconColorExtension; import org.opencv.core.Mat; /** @@ -10,20 +10,19 @@ */ public abstract class VisionOpMode extends VisionOpModeCore { + /*** + * EXTENSION-SPECIFIC CODE + ***/ + private final BeaconColorExtension beaconColorExtension = new BeaconColorExtension(); + public Beacon.BeaconAnalysis beaconColor = new Beacon.BeaconAnalysis(); private int extensions = 0; private boolean initialized = false; - protected boolean isEnabled(VisionExtensions extension) - { + protected boolean isEnabled(VisionExtensions extension) { return (extensions & extension.id) > 0; } - /*** EXTENSION-SPECIFIC CODE ***/ - private BeaconColorExtension beaconColorExtension = new BeaconColorExtension(); - public Beacon.BeaconAnalysis beaconColor = new Beacon.BeaconAnalysis(); - - protected void enableExtension(VisionExtensions extension) - { + protected void enableExtension(VisionExtensions extension) { extensions = extensions | extension.id; //Don't initialize extension if we haven't ever called init() yet @@ -34,8 +33,7 @@ protected void enableExtension(VisionExtensions extension) beaconColorExtension.init(this); } - protected void disableExtension(VisionExtensions extension) - { + protected void disableExtension(VisionExtensions extension) { extensions -= extensions & extension.id; if (extension == VisionExtensions.BEACON_COLOR) diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionOpModeCore.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionOpModeCore.java similarity index 85% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionOpModeCore.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionOpModeCore.java index 1b43c4ff..a54e6127 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/VisionOpModeCore.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/VisionOpModeCore.java @@ -1,7 +1,6 @@ -package org.lasarobotics.vision.opmode; +package org.lasarobotics.vision.test.opmode; import android.app.Activity; -import android.os.Looper; import android.util.Log; import android.view.View; import android.view.ViewGroup; @@ -9,8 +8,8 @@ import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import org.lasarobotics.vision.android.Cameras; -import org.lasarobotics.vision.util.FPS; +import org.lasarobotics.vision.test.android.Cameras; +import org.lasarobotics.vision.test.util.FPS; import org.opencv.android.BaseLoaderCallback; import org.opencv.android.CameraBridgeViewBase; import org.opencv.android.JavaCameraView; @@ -23,45 +22,41 @@ * Core OpMode class containing most OpenCV functionality */ abstract class VisionOpModeCore extends OpMode implements CameraBridgeViewBase.CvCameraViewListener2 { + private static final int initialMaxSize = 1200; private static JavaCameraView openCVCamera; - protected int width, height; private static boolean initialized = false; - protected FPS fps; - private static final int initialMaxSize = 1200; - private static boolean openCVInitialized = false; - - private BaseLoaderCallback mLoaderCallback = new BaseLoaderCallback(hardwareMap.appContext) { + private final BaseLoaderCallback mLoaderCallback = new BaseLoaderCallback(hardwareMap.appContext) { @Override public void onManagerConnected(int status) { switch (status) { - case LoaderCallbackInterface.SUCCESS: - { + case LoaderCallbackInterface.SUCCESS: { //Woohoo! Log.d("OpenCV", "OpenCV Manager connected!"); openCVInitialized = true; - } break; - default: - { + } + break; + default: { super.onManagerConnected(status); - } break; + } + break; } } }; + protected int width, height; + protected FPS fps; - public final void setCamera(Cameras camera) - { + public final void setCamera(Cameras camera) { openCVCamera.disconnectCamera(); openCVCamera.setCameraIndex(camera.getID()); openCVCamera.connectCamera(width, height); } - public final void setFrameSize(Size frameSize) - { + public final void setFrameSize(Size frameSize) { openCVCamera.setMaxFrameSize((int) frameSize.width, (int) frameSize.height); openCVCamera.disconnectCamera(); - openCVCamera.connectCamera((int)frameSize.width, (int)frameSize.height); + openCVCamera.connectCamera((int) frameSize.width, (int) frameSize.height); width = openCVCamera.getFrameWidth(); height = openCVCamera.getFrameHeight(); @@ -70,7 +65,7 @@ public final void setFrameSize(Size frameSize) @Override public void init() { //Initialize camera view - final Activity activity = (Activity)hardwareMap.appContext; + final Activity activity = (Activity) hardwareMap.appContext; final VisionOpModeCore t = this; if (!OpenCVLoader.initDebug()) { @@ -85,8 +80,7 @@ public void init() { mLoaderCallback.onManagerConnected(LoaderCallbackInterface.SUCCESS); } - while(!openCVInitialized) - { + while (!openCVInitialized) { try { Thread.sleep(50); } catch (InterruptedException e) { @@ -124,8 +118,7 @@ public void run() { } }); - while(!initialized) - { + while (!initialized) { try { Thread.sleep(50); } catch (InterruptedException e) { diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/extensions/BeaconColorExtension.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/extensions/BeaconColorExtension.java similarity index 74% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/extensions/BeaconColorExtension.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/extensions/BeaconColorExtension.java index 8a3b102f..076762b2 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/extensions/BeaconColorExtension.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/extensions/BeaconColorExtension.java @@ -1,31 +1,26 @@ -package org.lasarobotics.vision.opmode.extensions; +package org.lasarobotics.vision.test.opmode.extensions; -import org.lasarobotics.vision.android.Cameras; -import org.lasarobotics.vision.detection.ColorBlobDetector; -import org.lasarobotics.vision.detection.objects.Contour; -import org.lasarobotics.vision.ftc.resq.Beacon; -import org.lasarobotics.vision.image.Transform; -import org.lasarobotics.vision.opmode.VisionOpMode; -import org.lasarobotics.vision.util.color.ColorHSV; +import org.lasarobotics.vision.test.detection.ColorBlobDetector; +import org.lasarobotics.vision.test.detection.objects.Contour; +import org.lasarobotics.vision.test.ftc.resq.Beacon; +import org.lasarobotics.vision.test.opmode.VisionOpMode; +import org.lasarobotics.vision.test.util.color.ColorHSV; import org.opencv.core.Mat; -import org.opencv.core.Size; import java.util.List; /** * Extension that supports finding and reading beacon color data */ -public class BeaconColorExtension implements VisionExtension -{ - private ColorBlobDetector detectorRed; - private ColorBlobDetector detectorBlue; +public class BeaconColorExtension implements VisionExtension { private final ColorHSV lowerBoundRed = new ColorHSV((int) (305 / 360.0 * 255.0), (int) (0.100 * 255.0), (int) (0.300 * 255.0)); private final ColorHSV upperBoundRed = new ColorHSV((int) ((360.0 + 5.0) / 360.0 * 255.0), 255, 255); private final ColorHSV lowerBoundBlue = new ColorHSV((int) (170.0 / 360.0 * 255.0), (int) (0.100 * 255.0), (int) (0.300 * 255.0)); private final ColorHSV upperBoundBlue = new ColorHSV((int) (227.0 / 360.0 * 255.0), 255, 255); + private ColorBlobDetector detectorRed; + private ColorBlobDetector detectorBlue; - public BeaconColorExtension() - { + public BeaconColorExtension() { } @@ -55,9 +50,7 @@ public Mat frame(VisionOpMode opmode, Mat rgba, Mat gray) { //Get color analysis Beacon beacon = new Beacon(); opmode.beaconColor = beacon.analyzeColor(contoursRed, contoursBlue, rgba, gray); - } - catch (Exception e) - { + } catch (Exception e) { e.printStackTrace(); } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/extensions/VisionExtension.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/extensions/VisionExtension.java similarity index 71% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/extensions/VisionExtension.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/extensions/VisionExtension.java index b6de2b6c..740d96c0 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/opmode/extensions/VisionExtension.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/opmode/extensions/VisionExtension.java @@ -1,6 +1,6 @@ -package org.lasarobotics.vision.opmode.extensions; +package org.lasarobotics.vision.test.opmode.extensions; -import org.lasarobotics.vision.opmode.VisionOpMode; +import org.lasarobotics.vision.test.opmode.VisionOpMode; import org.opencv.core.Mat; /** @@ -8,7 +8,10 @@ */ public interface VisionExtension { void init(VisionOpMode opmode); + void loop(VisionOpMode opmode); + Mat frame(VisionOpMode opmode, Mat rgba, Mat gray); + void stop(VisionOpMode opmode); } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/FPS.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/FPS.java similarity index 78% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/FPS.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/FPS.java index 6f5abc30..8cf6ac23 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/FPS.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/FPS.java @@ -1,6 +1,4 @@ -package org.lasarobotics.vision.util; - -import org.lasarobotics.vision.util.RollingAverage; +package org.lasarobotics.vision.test.util; import java.text.DecimalFormat; @@ -11,23 +9,21 @@ public class FPS { private static final int FRAME_BUFFER_LENGTH = 5; private static final DecimalFormat FPS_FORMAT = new DecimalFormat("0.00"); - private RollingAverage rollingAverage; + private final RollingAverage rollingAverage; private long lastTime; - public FPS() - { + public FPS() { rollingAverage = new RollingAverage<>(FRAME_BUFFER_LENGTH); lastTime = -1L; } /** * Update the FPS counter. - * + *

* Call this method EVERY FRAME! */ public void update() { - if (lastTime != -1L) - { + if (lastTime != -1L) { long delta = System.nanoTime() - lastTime; rollingAverage.addValue(delta); } @@ -36,21 +32,19 @@ public void update() { /** * Get the frames per second count, as a double (prefer using getFPSString() instead). + * * @return The FPS, as a double in frames/second. */ - public double getFPS() - { + public double getFPS() { double period = rollingAverage.getAverage() / 1000000000.0; //period: s return 1.0 / period; //frequency = 1/s } - public String getFPSString() - { + public String getFPSString() { return FPS_FORMAT.format(getFPS()); } - public void pause() - { + public void pause() { //make sure we resample the FPS on next update() lastTime = -1L; } @@ -58,8 +52,7 @@ public void pause() /** * Resets the FPS counter. */ - public void reset() - { + public void reset() { rollingAverage.clear(); lastTime = -1L; } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/IO.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/IO.java similarity index 75% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/IO.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/IO.java index ba1af9bc..c5dd57a8 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/IO.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/IO.java @@ -1,17 +1,13 @@ -package org.lasarobotics.vision.util; +package org.lasarobotics.vision.test.util; import android.os.Environment; -import org.lasarobotics.vision.android.Util; +import org.lasarobotics.vision.test.android.Util; -import java.io.BufferedReader; import java.io.BufferedWriter; import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileReader; import java.io.FileWriter; import java.io.IOException; -import java.util.List; import java.util.Scanner; /** @@ -20,13 +16,13 @@ public class IO { /** * Write a text file on the device + * * @param directory Directory to write the file into - * @param filename The filename, including extension - * @param data Data to write to file + * @param filename The filename, including extension + * @param data Data to write to file * @param overwrite True to overwrite file if exists, false to create new file with appended "." and integer */ - public static void writeTextFile(String directory, String filename, String data, boolean overwrite) - { + public static void writeTextFile(String directory, String filename, String data, boolean overwrite) { try { File f; f = Util.createFileOnDevice(directory, filename, overwrite); @@ -39,13 +35,12 @@ public static void writeTextFile(String directory, String filename, String data, } } - public static String readTextFile(String directory, String filename) - { + public static String readTextFile(String directory, String filename) { File f = new File(Environment.getExternalStorageDirectory().getAbsolutePath() + directory, filename); try { String str = ""; Scanner s = new Scanner(f); - while(s.hasNextLine()) + while (s.hasNextLine()) str += s.nextLine() + "\n"; return str; } catch (Exception e) { @@ -54,8 +49,7 @@ public static String readTextFile(String directory, String filename) } } - public static String[] getLines(String data) - { + public static String[] getLines(String data) { return data.split("\r?\n"); } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/MathUtil.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/MathUtil.java old mode 100755 new mode 100644 similarity index 77% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/MathUtil.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/MathUtil.java index 3e153c5d..1d910fb6 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/MathUtil.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/MathUtil.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.util; +package org.lasarobotics.vision.test.util; import org.opencv.core.Point; @@ -6,6 +6,11 @@ * Math utilities */ public final class MathUtil { + /** + * Double equality epsilon (maximum tolerance for a double) + */ + private final static double EPSILON = 0.000001; + /** * Suppresses constructor for noninstantiability */ @@ -13,11 +18,6 @@ private MathUtil() { throw new AssertionError(); } - /** - * Double equality epsilon (maximum tolerance for a double) - */ - private final static double EPSILON = 0.000001; - /** * Gives a "deadzone" where any value less * than this would return zero. @@ -90,55 +90,54 @@ public static boolean inBounds(double min, double max, double value) { return (value < max) && (value > min); } - public static double distance(double deltaX, double deltaY) - { + public static double distance(double deltaX, double deltaY) { return Math.sqrt((deltaX * deltaX) + (deltaY * deltaY)); } - public static double distance(Point p1, Point p2) - { + + public static double distance(Point p1, Point p2) { return distance(Math.abs(p2.x - p1.x), Math.abs(p2.y - p1.y)); } /** * Calculate the angle between three points + * * @param pt1 Vector 1 * @param pt2 Vector 2 * @param pt0 Vector 0 * @return The angle (cosine) between the points */ - public static double angle( Point pt1, Point pt2, Point pt0 ) - { + public static double angle(Point pt1, Point pt2, Point pt0) { double dx1 = pt1.x - pt0.x; double dy1 = pt1.y - pt0.y; double dx2 = pt2.x - pt0.x; double dy2 = pt2.y - pt0.y; - return (dx1*dx2 + dy1*dy2)/Math.sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10); + return (dx1 * dx2 + dy1 * dy2) / Math.sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2) + 1e-10); } /** * Calculate a normal probability density function (PDF) based on a variance and mean value - * @param x The location on the normal distribution. The location at the mean would be the largest value - * @param variance The variance (stddev^2) of the normal distribution. + * + * @param x The location on the normal distribution. The location at the mean would be the largest value + * @param variance The variance (stddev^2) of the normal distribution. * @param meanValue The mean value is the x location at which the function is the largest (i.e. has median) * @return The normal distribution at the location x */ - public static double normalPDF(double x, double variance, double meanValue) - { + public static double normalPDF(double x, double variance, double meanValue) { double standardDeviation = Math.sqrt(variance); - return (1/(standardDeviation*Math.sqrt(2*Math.PI))) * Math.pow(Math.E, -((x - meanValue) * (x - meanValue)) / (2 * variance)); + return (1 / (standardDeviation * Math.sqrt(2 * Math.PI))) * Math.pow(Math.E, -((x - meanValue) * (x - meanValue)) / (2 * variance)); } /** * Calculate a normal probability density function (PDF) based on a variance and mean value in which the median value is equal to 1 - * @param x The location on the normal distribution. The location at the mean would be the largest value - * @param variance The variance (stddev^2) of the normal distribution. + * + * @param x The location on the normal distribution. The location at the mean would be the largest value + * @param variance The variance (stddev^2) of the normal distribution. * @param meanValue The mean value is the x location at which the function is the largest (i.e. has median) * @return The normal distribution at the location x */ - public static double normalPDFNormalized(double x, double variance, double meanValue) - { + public static double normalPDFNormalized(double x, double variance, double meanValue) { double standardDeviation = Math.sqrt(variance); - double normal = (1/(standardDeviation*Math.sqrt(2*Math.PI))); - return (1/(standardDeviation*Math.sqrt(2*Math.PI))/normal) * Math.pow(Math.E, -((x - meanValue) * (x - meanValue)) / (2 * variance)); + double normal = (1 / (standardDeviation * Math.sqrt(2 * Math.PI))); + return (1 / (standardDeviation * Math.sqrt(2 * Math.PI)) / normal) * Math.pow(Math.E, -((x - meanValue) * (x - meanValue)) / (2 * variance)); } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/RollingAverage.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/RollingAverage.java similarity index 93% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/RollingAverage.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/RollingAverage.java index 6e6a9631..bb422660 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/RollingAverage.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/RollingAverage.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.util; +package org.lasarobotics.vision.test.util; import java.util.LinkedList; @@ -7,7 +7,7 @@ * Uses doubles as internal structures */ public class RollingAverage { - private LinkedList list; + private final LinkedList list; private int capacity; private double total; diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/Color.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/Color.java similarity index 86% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/Color.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/Color.java index b0890b81..6968caf5 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/Color.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/Color.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.util.color; +package org.lasarobotics.vision.test.util.color; import org.opencv.core.CvType; import org.opencv.core.Mat; @@ -12,13 +12,11 @@ public abstract class Color { protected Scalar scalar; - Color(Scalar s) - { + Color(Scalar s) { setScalar(s); } - public static Color create(Scalar s, ColorSpace colorSpace) - { + public static Color create(Scalar s, ColorSpace colorSpace) { Class colorClass = colorSpace.getColorClass(); try { @@ -28,24 +26,46 @@ public static Color create(Scalar s, ColorSpace colorSpace) } } - public void setScalar(Scalar s) - { - this.scalar = parseScalar(s); + public static Mat convertColorMat(Mat in, ColorSpace spaceIn, ColorSpace spaceOut) { + if (spaceIn == spaceOut) + return in; + if (!spaceIn.canConvertTo(spaceOut)) + throw new IllegalArgumentException("Cannot convert color to the desired color space."); + + Mat output = in.clone(); + + try { + for (int i = 0; i < spaceIn.getConversionsTo(spaceOut).length; i += 3) { + int conversion = spaceIn.getConversionsTo(spaceOut)[i]; + int inputDim = spaceIn.getConversionsTo(spaceOut)[i + 1]; + int outputDim = spaceIn.getConversionsTo(spaceOut)[i + 2]; + + Imgproc.cvtColor(output, output, conversion, outputDim); + } + } catch (Exception ignored) { + throw new IllegalArgumentException("Cannot convert color to the desired color space."); + } + + return output; } - public Scalar getScalar() - { + + public Scalar getScalar() { return scalar; } - public Scalar getScalarRGBA() - { + + public void setScalar(Scalar s) { + this.scalar = parseScalar(s); + } + + public Scalar getScalarRGBA() { return convertColorScalar(ColorSpace.RGBA); } public abstract ColorSpace getColorSpace(); + protected abstract Scalar parseScalar(Scalar s); - public Color convertColor(ColorSpace to) - { + public Color convertColor(ColorSpace to) { Scalar output = convertColorScalar(to); Class colorClass = to.getColorClass(); @@ -57,8 +77,7 @@ public Color convertColor(ColorSpace to) } } - public Scalar convertColorScalar(ColorSpace to) - { + public Scalar convertColorScalar(ColorSpace to) { if (getColorSpace() == to) return getScalar(); if (!getColorSpace().canConvertTo(to)) @@ -79,33 +98,7 @@ public Scalar convertColorScalar(ColorSpace to) pointMatTo.release(); pointMatFrom.release(); } - } catch (Exception ignored) - { - throw new IllegalArgumentException("Cannot convert color to the desired color space."); - } - - return output; - } - - public static Mat convertColorMat(Mat in, ColorSpace spaceIn, ColorSpace spaceOut) - { - if (spaceIn == spaceOut) - return in; - if (!spaceIn.canConvertTo(spaceOut)) - throw new IllegalArgumentException("Cannot convert color to the desired color space."); - - Mat output = in.clone(); - - try { - for (int i = 0; i < spaceIn.getConversionsTo(spaceOut).length; i += 3) { - int conversion = spaceIn.getConversionsTo(spaceOut)[i]; - int inputDim = spaceIn.getConversionsTo(spaceOut)[i + 1]; - int outputDim = spaceIn.getConversionsTo(spaceOut)[i + 2]; - - Imgproc.cvtColor(output, output, conversion, outputDim); - } - } catch (Exception ignored) - { + } catch (Exception ignored) { throw new IllegalArgumentException("Cannot convert color to the desired color space."); } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorGRAY.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorGRAY.java similarity index 65% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorGRAY.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorGRAY.java index 67dabb90..27265a9a 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorGRAY.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorGRAY.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.util.color; +package org.lasarobotics.vision.test.util.color; import org.opencv.core.Scalar; @@ -7,13 +7,11 @@ */ public class ColorGRAY extends Color { - public ColorGRAY(Scalar s) - { + public ColorGRAY(Scalar s) { super(s); } - public ColorGRAY(int v) - { + public ColorGRAY(int v) { super(new Scalar(v)); } @@ -22,15 +20,13 @@ public ColorSpace getColorSpace() { } @Override - protected Scalar parseScalar(Scalar s) - { + protected Scalar parseScalar(Scalar s) { if (s.val.length < 1) throw new IllegalArgumentException("Scalar must have 1 dimension."); return new Scalar(s.val[0]); } - public int value() - { - return (int)scalar.val[0]; + public int value() { + return (int) scalar.val[0]; } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorHSV.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorHSV.java similarity index 66% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorHSV.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorHSV.java index 10626c3c..ea50f5b8 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorHSV.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorHSV.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.util.color; +package org.lasarobotics.vision.test.util.color; import org.opencv.core.Scalar; @@ -7,20 +7,19 @@ */ public class ColorHSV extends Color { - public ColorHSV(Scalar s) - { + public ColorHSV(Scalar s) { super(s); } /** * An HSV (hue, saturation, value) color * This is NOT an HSL color - they live in different spaces. + * * @param h Hue, from 0 to 255 * @param s Saturation, from 0 to 255 * @param v Value, from 0 to 255 */ - public ColorHSV(int h, int s, int v) - { + public ColorHSV(int h, int s, int v) { super(new Scalar(h, s, v)); } @@ -29,23 +28,21 @@ public ColorSpace getColorSpace() { } @Override - protected Scalar parseScalar(Scalar s) - { + protected Scalar parseScalar(Scalar s) { if (s.val.length < 3) throw new IllegalArgumentException("Scalar must have 3 dimensions."); return new Scalar(s.val[0], s.val[1], s.val[2]); } - public int hue() - { - return (int)scalar.val[0]; + public int hue() { + return (int) scalar.val[0]; } - public int saturation() - { - return (int)scalar.val[1]; + + public int saturation() { + return (int) scalar.val[1]; } - public int value() - { - return (int)scalar.val[2]; + + public int value() { + return (int) scalar.val[2]; } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorRGBA.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorRGBA.java similarity index 53% rename from ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorRGBA.java rename to ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorRGBA.java index 3f73aa7d..52acf3c2 100644 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorRGBA.java +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorRGBA.java @@ -1,4 +1,4 @@ -package org.lasarobotics.vision.util.color; +package org.lasarobotics.vision.test.util.color; import org.opencv.core.Scalar; @@ -7,34 +7,23 @@ */ public class ColorRGBA extends Color { - public ColorRGBA(int r, int g, int b) - { - super(new Scalar(r,g,b,255)); + public ColorRGBA(int r, int g, int b) { + super(new Scalar(r, g, b, 255)); } - public ColorRGBA(int r, int g, int b, int a) - { - super(new Scalar(r,g,b,a)); + + public ColorRGBA(int r, int g, int b, int a) { + super(new Scalar(r, g, b, a)); } - public ColorRGBA(Scalar scalar) - { + + public ColorRGBA(Scalar scalar) { super(scalar); } - public ColorRGBA(String hexCode) - { - super(parseHexCode(hexCode)); - } - @Override - protected Scalar parseScalar(Scalar s) - { - if (s.val.length < 3) - throw new IllegalArgumentException("Scalar must have 3 or 4 dimensions."); - - return new Scalar(s.val[0], s.val[1], s.val[2], (s.val.length >= 4) ? (int)s.val[3] : 255); + public ColorRGBA(String hexCode) { + super(parseHexCode(hexCode)); } - private static Scalar parseHexCode(String hexCode) - { + private static Scalar parseHexCode(String hexCode) { //remove hex key # if (!hexCode.startsWith("#")) hexCode = "#" + hexCode; @@ -50,21 +39,28 @@ private static Scalar parseHexCode(String hexCode) android.graphics.Color.alpha(color)); } - public int red() - { - return (int)scalar.val[0]; + @Override + protected Scalar parseScalar(Scalar s) { + if (s.val.length < 3) + throw new IllegalArgumentException("Scalar must have 3 or 4 dimensions."); + + return new Scalar(s.val[0], s.val[1], s.val[2], (s.val.length >= 4) ? (int) s.val[3] : 255); + } + + public int red() { + return (int) scalar.val[0]; } - public int green() - { - return (int)scalar.val[1]; + + public int green() { + return (int) scalar.val[1]; } - public int blue() - { - return (int)scalar.val[2]; + + public int blue() { + return (int) scalar.val[2]; } - public int alpha() - { - return (int)scalar.val[3]; + + public int alpha() { + return (int) scalar.val[3]; } @Override @@ -72,9 +68,8 @@ public ColorSpace getColorSpace() { return ColorSpace.RGBA; } - public int getInteger() - { - return android.graphics.Color.argb((int)scalar.val[3], (int)scalar.val[0], - (int)scalar.val[1], (int)scalar.val[2]); + public int getInteger() { + return android.graphics.Color.argb((int) scalar.val[3], (int) scalar.val[0], + (int) scalar.val[1], (int) scalar.val[2]); } } diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorSpace.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorSpace.java new file mode 100644 index 00000000..a68e8130 --- /dev/null +++ b/ftc-visionlib/src/main/java/org/lasarobotics/vision/test/util/color/ColorSpace.java @@ -0,0 +1,67 @@ +package org.lasarobotics.vision.test.util.color; + +import org.opencv.imgproc.Imgproc; + +/** + * Specifies a color space (such as RGB or HSV) and all possible conversions + */ +public enum ColorSpace { + RGBA(new int[][]{null, + new int[]{Imgproc.COLOR_RGBA2RGB, 4, 3}, + new int[]{Imgproc.COLOR_RGBA2RGB, 4, 3, Imgproc.COLOR_RGB2HSV_FULL, 3, 3}, + new int[]{Imgproc.COLOR_RGBA2GRAY, 4, 1}}, + ColorRGBA.class), + + RGB(new int[][]{new int[]{Imgproc.COLOR_RGB2RGBA, 3, 4}, + null, + new int[]{Imgproc.COLOR_RGB2HSV_FULL, 3, 3}, + new int[]{Imgproc.COLOR_RGB2GRAY, 3, 1}}, + ColorRGBA.class), + + HSV(new int[][]{new int[]{Imgproc.COLOR_HSV2RGB_FULL, 3, 3, Imgproc.COLOR_RGB2RGBA, 3, 4}, + new int[]{Imgproc.COLOR_HSV2RGB_FULL, 3, 3}, + null, + new int[]{Imgproc.COLOR_HSV2RGB_FULL, 3, 3, Imgproc.COLOR_RGB2GRAY, 3, 1}}, + ColorHSV.class), + + GRAY(new int[][]{new int[]{Imgproc.COLOR_GRAY2RGBA, 1, 4}, + new int[]{Imgproc.COLOR_GRAY2RGB, 1, 3}, + new int[]{Imgproc.COLOR_GRAY2RGB, 1, 3, Imgproc.COLOR_RGB2HSV_FULL, 3, 3}, + null}, + ColorGRAY.class); + + /** + * Each conversions array contains a list of int[], one for each other ColorSpace to convert to. + * Each int[] contains a list of operations and sizes, as such: + * { operation, input scalar dimension, output scalar dimension, ... } + */ + private final int[][] conversions; + /** + * The class associated with a color - allows for dynamic casting to the class type + */ + private final Class colorClass; + + ColorSpace(int[][] conversions, Class colorClass) { + this.conversions = conversions; + this.colorClass = colorClass; + } + + int[] getConversionsTo(ColorSpace to) { + return conversions[to.ordinal()]; + } + + Class getColorClass() { + return colorClass; + } + + /** + * Tests whether the current color space can be converted to another + * + * @param to The color space to convert to + * @return True if convertable, false otherwise + */ + @SuppressWarnings("BooleanMethodIsAlwaysInverted") + public boolean canConvertTo(ColorSpace to) { + return (to == this) || (getConversionsTo(to) != null); + } +} \ No newline at end of file diff --git a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorSpace.java b/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorSpace.java deleted file mode 100644 index 2634c2ed..00000000 --- a/ftc-visionlib/src/main/java/org/lasarobotics/vision/util/color/ColorSpace.java +++ /dev/null @@ -1,70 +0,0 @@ -package org.lasarobotics.vision.util.color; - -import org.opencv.imgproc.Imgproc; - -/** - * Specifies a color space (such as RGB or HSV) and all possible conversions - */ -public enum ColorSpace -{ - RGBA(new int[][] { null, - new int[] { Imgproc.COLOR_RGBA2RGB, 4, 3 }, - new int[] { Imgproc.COLOR_RGBA2RGB, 4, 3, Imgproc.COLOR_RGB2HSV_FULL, 3, 3 }, - new int[] { Imgproc.COLOR_RGBA2GRAY, 4, 1 } }, - ColorRGBA.class), - - RGB(new int[][] { new int[] { Imgproc.COLOR_RGB2RGBA, 3, 4 }, - null, - new int[] { Imgproc.COLOR_RGB2HSV_FULL, 3, 3 }, - new int[] { Imgproc.COLOR_RGB2GRAY, 3, 1 } }, - ColorRGBA.class), - - HSV(new int[][] { new int[] { Imgproc.COLOR_HSV2RGB_FULL, 3, 3, Imgproc.COLOR_RGB2RGBA, 3, 4 }, - new int[] { Imgproc.COLOR_HSV2RGB_FULL, 3, 3}, - null, - new int[] { Imgproc.COLOR_HSV2RGB_FULL, 3, 3, Imgproc.COLOR_RGB2GRAY, 3, 1 } }, - ColorHSV.class), - - GRAY(new int[][] { new int[] { Imgproc.COLOR_GRAY2RGBA, 1, 4 }, - new int[] { Imgproc.COLOR_GRAY2RGB, 1, 3 }, - new int[] { Imgproc.COLOR_GRAY2RGB, 1, 3, Imgproc.COLOR_RGB2HSV_FULL, 3, 3 }, - null }, - ColorGRAY.class); - - /** - * Each conversions array contains a list of int[], one for each other ColorSpace to convert to. - * Each int[] contains a list of operations and sizes, as such: - * { operation, input scalar dimension, output scalar dimension, ... } - */ - private int[][] conversions; - /** - * The class associated with a color - allows for dynamic casting to the class type - */ - private Class colorClass; - - ColorSpace(int[][] conversions, Class colorClass) - { - this.conversions = conversions; - this.colorClass = colorClass; - } - - int[] getConversionsTo(ColorSpace to) - { - return conversions[to.ordinal()]; - } - - Class getColorClass() - { - return colorClass; - } - - /** - * Tests whether the current color space can be converted to another - * @param to The color space to convert to - * @return True if convertable, false otherwise - */ - public boolean canConvertTo(ColorSpace to) - { - return (to == this) || (getConversionsTo(to) != null); - } -} \ No newline at end of file diff --git a/img/logo-github.png b/img/logo-github.png deleted file mode 100644 index 5f5560b0..00000000 Binary files a/img/logo-github.png and /dev/null differ diff --git a/img/test3.png b/img/test3.png deleted file mode 100644 index a1bd27ed..00000000 Binary files a/img/test3.png and /dev/null differ