Laptop Vision and Converting between WPIBinaryImage and BinaryImage

[gizzerd91]

If anyone is making an effort to do vision on the laptop, let me make your life a little bit easier. Here is the (almost) working code for a vision processing widget in SmartDashboard that I found and have been tinkering with. Right now it's kind of a frankenstein of Greg's code and the stock vision code.

package edu.wpi.first.smartdashboard.extension.vision;

import edu.wpi.first.smartdashboard.camera.WPICameraExtension;
import edu.wpi.first.smartdashboard.robot.Robot;
import edu.wpi.first.wpijavacv.WPIBinaryImage;
import edu.wpi.first.wpijavacv.WPIColor;
import edu.wpi.first.wpijavacv.WPIColorImage;
import edu.wpi.first.wpijavacv.WPIContour;
import edu.wpi.first.wpijavacv.WPIImage;
import edu.wpi.first.wpijavacv.WPIPoint;
import edu.wpi.first.wpijavacv.WPIPolygon;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVision.MeasurementType;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;
import edu.wpi.first.wpilibj.image.RGBImage;
import edu.wpi.first.wpilibj.networking.NetworkTable;
import java.util.ArrayList;
import java.util.logging.Level;
import java.util.logging.Logger;

/**
 *
 * @author Greg Granito
 */
public class LaptopVisionRobot extends WPICameraExtension {
    public static final String NAME = "Robot Camera Square Tracker";

    NetworkTable table = NetworkTable.getTable("camera");
    WPIColor targetColor = new WPIColor(255, 0, 0);
    criteriaCollection cc;
    
    @Override
    public void init() {
        cc = new CriteriaCollection();      // create the criteria for the particle filter
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_WIDTH, 30, 400, false);
        cc.addCriteria(MeasurementType.IMAQ_MT_BOUNDING_RECT_HEIGHT, 40, 400, false);
    }
    
    @Override
    public WPIImage processImage(WPIColorImage rawImage) {
        WPIBinaryImage blueBin = rawImage.getBlueChannel().getThresholdInverted(100);
        WPIBinaryImage greenBin = rawImage.getGreenChannel().getThresholdInverted(100);
        WPIBinaryImage redBin = rawImage.getRedChannel().getThresholdInverted(100);

        WPIBinaryImage finalBin = blueBin.getAnd(redBin).getAnd(greenBin);
        try {
            finalBin.erode(2);
            finalBin.dilate(6);
            BinaryImage thresholdImage = finalBin;
            BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
            BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
            BinaryImage filteredImage = convexHullImage.particleFilter(cc);           // find filled in rectangles

            WPIContour[] contours = finalBin.findContours();

            ArrayList<WPIPolygon> polygons = new ArrayList<WPIPolygon>();

            for(WPIContour c : contours){
                double ratio = ((double)c.getHeight()) / ((double)c.getWidth());
                if(ratio < 1.5 && ratio> 0.75){
                    polygons.add(c.approxPolygon(45));
                }
            }

            ArrayList<WPIPolygon> possiblePolygons = new ArrayList<WPIPolygon>();

            for(WPIPolygon p : polygons){
                if(p.isConvex() && p.getNumVertices() == 4){
                    possiblePolygons.add(p);
                }else{
                    rawImage.drawPolygon(p, WPIColor.CYAN, 5);
                }
            }

            WPIPolygon square = null;
            int squareArea = 0;

            for(WPIPolygon p : possiblePolygons){
                rawImage.drawPolygon(p, WPIColor.GREEN, 5);
                for(WPIPolygon q : possiblePolygons){
                    if(p == q) continue;

                   int pCenterX = (p.getX() + (p.getWidth()/2));
                   
                   int qCenterX = q.getX() + (q.getWidth()/2);
                   
                   int pCenterY = (p.getY() + (p.getHeight()/2));

                   int qCenterY = q.getY() + (q.getHeight()/2);

                   rawImage.drawPoint(new WPIPoint(pCenterX, pCenterY), targetColor, 5);
                   rawImage.drawPoint(new WPIPoint(qCenterX, qCenterY), targetColor, 5);

                    if(Math.abs(pCenterX - qCenterX) < 20 &&
                        Math.abs(pCenterY - qCenterY) < 20){
                        int pArea = Math.abs(p.getArea());
                        int qArea = Math.abs(q.getArea());
                        if(pArea > qArea){
                            square = p;
                            squareArea = pArea;
                        }else{
                            square = q;
                            squareArea = qArea;
                        }
                        break;
                    }
                }
            }

            if(square != null){
                double x = square.getX() + (square.getWidth()/2);
                x = (2 * (x/rawImage.getWidth())) - 1;
                
                double area = ((double)squareArea) /  ((double)(rawImage.getWidth() * rawImage.getHeight()));
                
                synchronized(table) {
                table.beginTransaction();
                    table.putBoolean("found", true);
                    table.putDouble("x", x);
                    table.putDouble("area", area);
                table.endTransaction();
                }

                Robot.getTable().putBoolean("found", true);
                Robot.getTable().putDouble("X", x);
                Robot.getTable().putDouble("Area", area);
                rawImage.drawPolygon(square, targetColor, 7);
            }else{
                table.putBoolean("found", false);
                Robot.getTable().putBoolean("found", false);
            }

        } catch (NIVisionException ex) {
            Logger.getLogger(LaptopVisionRobot.class.getName()).log(Level.SEVERE, null, ex);
        }
        return finalBin;
    }
}

The major trouble I'm having, and why you'll find this doesn't compile, is the

BinaryImage thresholdImage = finalBin;

In order to do higher order image processing, I need to convert from WPIBinaryImage to BinaryImage, but there doesn't seem to be a way to do this. My only alternative at this point is to physically save a bitmap to the harddrive, then read it out, but that is sooooo slow. Anybody have any ideas?