Auto merge of #5 - lasarobotics:feature-blob, r=smo-key

Add blob detection and start on the FTC Resq beacon implementation

More tweaks and improvements to come - the camera tester app is now fully functional!

Author: homu

ftc-cameratest/src/main/java/com/lasarobotics/tests/camera/CameraTestActivity.java

@@ -7,11 +7,16 @@
 
 import org.lasarobotics.vision.android.Camera;
 import org.lasarobotics.vision.android.Cameras;
-import org.lasarobotics.vision.image.Drawing;
-import org.lasarobotics.vision.util.FPS;
 import org.lasarobotics.vision.android.Util;
+import org.lasarobotics.vision.detection.ColorBlobDetector;
+import org.lasarobotics.vision.detection.Contour;
 import org.lasarobotics.vision.detection.ObjectDetection;
-import org.lasarobotics.vision.util.Color;
+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.opencv.android.BaseLoaderCallback;
 import org.opencv.android.CameraBridgeViewBase;
 import org.opencv.android.CameraBridgeViewBase.CvCameraViewFrame;
@@ -24,17 +29,17 @@
 import org.opencv.highgui.Highgui;
 
 import java.io.File;
+import java.util.List;
 
 public class CameraTestActivity extends Activity implements CvCameraViewListener2 {
 
     private Mat mRgba; //RGBA scene image
     private Mat mGray; //Grayscale scene image
+    private CameraBridgeViewBase mOpenCvCameraView;
 
     private float focalLength; //Camera lens focal length
 
-    private CameraBridgeViewBase mOpenCvCameraView;
-
-    private ObjectDetection.ObjectAnalysis objectAnalysis;
+    //private ObjectDetection.ObjectAnalysis objectAnalysis;
     private FPS fpsCounter;
 
     private void initialize()
@@ -48,7 +53,7 @@ private void initialize()
 
         //GET OBJECT IMAGE
         //Read the target image file
-        String dir = Util.getDCIMDirectory();
+        /*String dir = Util.getDCIMDirectory();
         File file = new File(dir + "/beacon.png");
 
         if (!file.exists())
@@ -63,13 +68,13 @@ private void initialize()
             // print error and abort execution
             Log.e("CameraTester", "FAILED TO LOAD IMAGE FILE!");
             System.exit(1);
-        }
+        }*/
 
         //ANALYZE OBJECT
-        ObjectDetection detection = new ObjectDetection(ObjectDetection.FeatureDetectorType.GFTT,
-                ObjectDetection.DescriptorExtractorType.BRIEF,
-                ObjectDetection.DescriptorMatcherType.BRUTEFORCE_HAMMING);
-        objectAnalysis = detection.analyzeObject(mTarget);
+        //ObjectDetection detection = new ObjectDetection(ObjectDetection.FeatureDetectorType.GFTT,
+        //        ObjectDetection.DescriptorExtractorType.ORB,
+        //        ObjectDetection.DescriptorMatcherType.BRUTEFORCE_HAMMING);
+        //objectAnalysis = detection.analyzeObject(mTarget);
 
         //UPDATE COUNTER
         fpsCounter = new FPS();
@@ -83,7 +88,7 @@ public void onManagerConnected(int status) {
                 {
                     // OpenCV loaded successfully!
                     // Load native library AFTER OpenCV initialization
-                    
+
                     initialize();
 
                     mOpenCvCameraView.enableView();
@@ -135,9 +140,22 @@ 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)(187.0       / 360.0 * 255.0), (int)(0.750 * 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);
+
+        detectorRed  = new ColorBlobDetector(lowerBoundRed, upperBoundRed);
+        detectorBlue = new ColorBlobDetector(lowerBoundBlue, upperBoundBlue);
     }
 
     public void onCameraViewStopped() {
@@ -146,31 +164,46 @@ public void onCameraViewStopped() {
     }
 
     public Mat onCameraFrame(CvCameraViewFrame inputFrame) {
-        // input frame has RBGA format
+        // input frame has RGBA format
         mRgba = inputFrame.rgba();
         mGray = inputFrame.gray();
 
         fpsCounter.update();
 
-        ObjectDetection detection = new ObjectDetection(ObjectDetection.FeatureDetectorType.FAST_DYNAMIC,
-                ObjectDetection.DescriptorExtractorType.BRIEF,
-                ObjectDetection.DescriptorMatcherType.BRUTEFORCE_HAMMING);
+        //ObjectDetection detection = new ObjectDetection(ObjectDetection.FeatureDetectorType.ORB,
+        //        ObjectDetection.DescriptorExtractorType.ORB,
+        //        ObjectDetection.DescriptorMatcherType.BRUTEFORCE_HAMMING);
 
         try {
-            ObjectDetection.SceneAnalysis sceneAnalysis = detection.analyzeScene(mGray, objectAnalysis, mRgba);
-            ObjectDetection.drawKeypoints(mRgba, sceneAnalysis);
-            ObjectDetection.drawDebugInfo(mRgba, sceneAnalysis);
-            ObjectDetection.drawObjectLocation(mRgba, objectAnalysis, sceneAnalysis);
+            //ObjectDetection.SceneAnalysis sceneAnalysis = detection.analyzeScene(mGray, objectAnalysis, mRgba);
+            //ObjectDetection.drawKeypoints(mRgba, sceneAnalysis);
+            //ObjectDetection.drawDebugInfo(mRgba, sceneAnalysis);
+            //ObjectDetection.drawObjectLocation(mRgba, objectAnalysis, sceneAnalysis);
+
+            mRgba = inputFrame.rgba();
+
+            //Process the frame for the color blobs
+            detectorRed.process(mRgba);
+            detectorBlue.process(mRgba);
+
+            //Get the list of contours
+            List<Contour> contoursRed = detectorRed.getContours();
+            List<Contour> contoursBlue = detectorBlue.getContours();
+
+            Beacon.BeaconColorAnalysis colorAnalysis = Beacon.analyzeColor(contoursRed, contoursBlue, mRgba);
+
+            Drawing.drawContours(mRgba, contoursRed, new ColorRGBA(255, 0, 0), 3);
+            Drawing.drawContours(mRgba, contoursBlue, new ColorRGBA(0, 0, 255), 3);
+            Drawing.drawText(mRgba, colorAnalysis.getStateLeft().toString() + ", " + colorAnalysis.getStateRight().toString(),
+                    new Point(0, 8), 1.0f, new ColorGRAY(255), Drawing.Anchor.BOTTOMLEFT);
         }
         catch (Exception e)
         {
-            Drawing.drawText(mRgba, "Analysis Error", new Point(0, 8), 1.0f, new Color("#F44336"), Drawing.Anchor.BOTTOMLEFT);
+            Drawing.drawText(mRgba, "Analysis Error", new Point(0, 8), 1.0f, new ColorRGBA("#F44336"), Drawing.Anchor.BOTTOMLEFT);
             e.printStackTrace();
         }
 
-        Drawing.drawText(mRgba, "FPS: " + fpsCounter.getFPSString(), new Point(0, 24), 1.0f, new Color("#2196F3"));
-
-        //Features.highlightFeatures(mGray.getNativeObjAddr(), mRgba.getNativeObjAddr());
+        Drawing.drawText(mRgba, "FPS: " + fpsCounter.getFPSString(), new Point(0, 24), 1.0f, new ColorRGBA("#2196F3"));
 
         return mRgba;
     }

ftc-visionlib-java/src/main/java/org/lasarobotics/vision/detection/BlobDetection.java

@@ -0,0 +1,10 @@
+package org.lasarobotics.vision.detection;
+
+/**
+ * Created by arthur on 10/9/15.
+ */
+public class CascadeObjectDetection {
+    public CascadeObjectDetection() {
+
+    }
+}

ftc-visionlib-java/src/main/java/org/lasarobotics/vision/detection/CascadeObjectDetection.java

@@ -0,0 +1,192 @@
+package org.lasarobotics.vision.detection;
+
+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.opencv.core.Core;
+import org.opencv.core.Mat;
+import org.opencv.core.MatOfPoint;
+import org.opencv.core.Scalar;
+import org.opencv.imgproc.Imgproc;
+
+import java.util.ArrayList;
+import java.util.Iterator;
+import java.util.List;
+
+/**
+ * Implements blob (regional) detection based on color
+ */
+public class ColorBlobDetector {
+
+    //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
+    private Color color;
+    //True if radius is set, false if lower and upper bound is set
+    private boolean isRadiusSet = true;
+
+    private List<Contour> contours = new ArrayList<>();
+    private double maxArea;
+
+    public ColorBlobDetector(Color color)
+    {
+        setColor(color);
+    }
+    public ColorBlobDetector(Color color, Color colorRadius)
+    {
+        this.colorRadius = colorRadius.convertColorScalar(ColorSpace.HSV);
+        setColor(color);
+    }
+    public ColorBlobDetector(ColorHSV colorMinimum, ColorHSV colorMaximum)
+    {
+        setColorRadius(colorMinimum, colorMaximum);
+    }
+
+    public void setColor(Color color)
+    {
+        if (color == null)
+            throw new IllegalArgumentException("Color must not be null!");
+
+        if (isRadiusSet)
+            return;
+
+        this.color = color;
+        Scalar hsvColor = color.convertColorScalar(ColorSpace.HSV);
+
+        //calculate min and max hues
+        double minH = (hsvColor.val[0] >= colorRadius.val[0]) ? hsvColor.val[0] - colorRadius.val[0] : 0;
+        double maxH = (hsvColor.val[0] + colorRadius.val[0] <= 255) ? hsvColor.val[0] + colorRadius.val[0] : 255;
+
+        Scalar lowerBoundScalar = lowerBound.getScalar();
+        Scalar upperBoundScalar = upperBound.getScalar();
+
+        lowerBoundScalar.val[0] = minH;
+        upperBoundScalar.val[0] = maxH;
+
+        lowerBoundScalar.val[1] = hsvColor.val[1] - colorRadius.val[1];
+        upperBoundScalar.val[1] = hsvColor.val[1] + colorRadius.val[1];
+
+        lowerBoundScalar.val[2] = hsvColor.val[2] - colorRadius.val[2];
+        upperBoundScalar.val[2] = hsvColor.val[2] + colorRadius.val[2];
+
+        lowerBoundScalar.val[3] = 0;
+        upperBoundScalar.val[3] = 255;
+
+        lowerBound = new ColorHSV(lowerBoundScalar);
+        upperBound = new ColorHSV(upperBoundScalar);
+    }
+
+    //TODO test this method - set a color radius in the contructor and solve for the true min and max bound
+    public void setColorRadius(Color lowerBound, Color upperBound)
+    {
+        isRadiusSet = false;
+        Scalar lower = lowerBound.convertColorScalar(ColorSpace.HSV);
+        Scalar upper = upperBound.convertColorScalar(ColorSpace.HSV);
+
+        this.lowerBound = new ColorHSV(lower);
+        this.upperBound = new ColorHSV(upper);
+    }
+
+    public void setColorRadius(ColorHSV radius) {
+        isRadiusSet = true;
+        this.colorRadius = radius.getScalar();
+        //Update the bounds again
+        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);
+
+        Imgproc.cvtColor(mPyrDownMat, mHsvMat, Imgproc.COLOR_RGB2HSV_FULL);
+
+        //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
+        {
+            //We need two operations - we're going to OR the masks together
+            Scalar lower = lowerBound.getScalar().clone();
+            Scalar upper = upperBound.getScalar().clone();
+            while (upper.val[0] > 255)
+                upper.val[0] -= 255;
+            double tmp = lower.val[0];
+            lower.val[0] = 0;
+            //Mask 1 - from 0 to n
+            Core.inRange(mHsvMat, lower, upper, mMaskOne);
+            //Mask 2 - from 255-n to 255
+            lower.val[0] = tmp;
+            upper.val[0] = 255;
+            Core.inRange(mHsvMat, lower, upper, mMask);
+            //OR the two masks
+            Core.bitwise_or(mMaskOne, mMask, mMask);
+        }
+
+        //Dilate (blur) the mask to decrease processing power
+        Imgproc.dilate(mMask, mDilatedMask, new Mat());
+
+        List<MatOfPoint> contourListTemp = new ArrayList<>();
+
+        Imgproc.findContours(mDilatedMask, contourListTemp, mHierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);
+
+        // Find max contour area
+        double maxArea = 0;
+        List<Contour> contourList = new ArrayList<>();
+        for (MatOfPoint c : contourListTemp) {
+            Contour con = new Contour(c);
+            contourList.add(con);
+            double area = con.area();
+            if (area > maxArea)
+                maxArea = area;
+        }
+        this.maxArea = maxArea;
+
+        // Filter contours by area and resize to fit the original image size
+        contours.clear();
+        for (Contour c : contourList ) {
+            if (Imgproc.contourArea(c) > minContourArea*maxArea) {
+                Core.multiply(c, new Scalar(4,4), c);
+                contours.add(new Contour(c));
+            }
+        }
+    }
+
+    public double getContourMaxArea()
+    {
+        return maxArea;
+    }
+
+    public void drawContours(Mat img, Color color)
+    {
+        Drawing.drawContours(img, contours, color);
+    }
+    public void drawContours(Mat img, Color color, int thickness)
+    {
+        Drawing.drawContours(img, contours, color, thickness);
+    }
+
+    public List<Contour> 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();
+}