/*----------------------------------------------------------------------------*/ /* Copyright (c) FIRST 2008. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ /*----------------------------------------------------------------------------*/ package edu.wpi.first.wpilibj.trackerdemo; import edu.wpi.first.wpilibj.image.BinaryImage; import edu.wpi.first.wpilibj.image.ColorImage; import edu.wpi.first.wpilibj.image.NIVisionException; import edu.wpi.first.wpilibj.image.ParticleAnalysisReport; /** * * @author dtjones */ public class Target { private static final double FRC_PARTICLE_TO_IMAGE_PERCENT = .0001; private static final double FRC_SIZE_FACTOR = 3.0; private static final double FRC_MAX_IMAGE_SEPARATION = 20.0; private static final double FRC_ALIGNMENT_SCALE = 3.0; final ParticleAnalysisReport firstParticle; final ParticleAnalysisReport secondParticle; Target(ParticleAnalysisReport firstParticle, ParticleAnalysisReport secondParticle) { this.firstParticle = firstParticle; this.secondParticle = secondParticle; } public static class Position { public final int value; protected static final int above_val = 0; protected static final int below_val = 1; protected static final int right_val = 2; protected static final int left_val = 3; public static final Position above = new Position(above_val); public static final Position bellow = new Position(below_val); public static final Position right = new Position(right_val); public static final Position left = new Position(left_val); private Position(int value) { this.value = value; } } public static class Threshold { int plane1Low; int plane1High; int plane2Low; int plane2High; int plane3Low; int plane3High; public Threshold(int plane1Low, int plane1High, int plane2Low, int plane2High, int plane3Low, int plane3High) { this.plane1Low = plane1Low; this.plane1High = plane1High; this.plane2Low = plane2Low; this.plane2High = plane2High; this.plane3Low = plane3Low; this.plane3High = plane3High; } } private static boolean aligned(int center1, int center2, int dimension1, int dimension2) { double averageWidth = (dimension1 + dimension2) / 2.0; //scale down width averageWidth *= FRC_ALIGNMENT_SCALE; int centerDiff = Math.abs(center1 - center2); if (centerDiff < averageWidth) { return true; } //dimensions (widths or heights) should be nearly the same. If they are //different, most likely there is glare or incorrect color specification return false; } private static boolean adjacent(int value1, int value2) { if (Math.abs(value1 - value2) <= FRC_MAX_IMAGE_SEPARATION) { return true; } return false; } private static boolean sizesRelative(double area1, double area2) { if ((area2 < (area1 * FRC_SIZE_FACTOR)) && (area1 < (area2 * FRC_SIZE_FACTOR))) { return true; } return false; } /** * Search for a target in the given image of the two color ranges given and * positioned according to the given position. * @param image The image to fing the target within. * @param position The postion of the two colors realtive to eachother. * @param firstThreshold The first color range. * @param secondThreshold The second color range. * @return A Target object containing information about the target or null * if no target was found. */ public static Target getTarget(ColorImage image, Position position, Threshold firstThreshold, Threshold secondThreshold) throws NIVisionException{ BinaryImage firstColor = image.thresholdHSL( firstThreshold.plane1Low, firstThreshold.plane1High, firstThreshold.plane2Low, firstThreshold.plane2High, firstThreshold.plane3Low, firstThreshold.plane3High); BinaryImage secondColor = image.thresholdHSL( secondThreshold.plane1Low, secondThreshold.plane1High, secondThreshold.plane2Low, secondThreshold.plane2High, secondThreshold.plane3Low, secondThreshold.plane3High); ParticleAnalysisReport[] firstColorHits = firstColor.getOrderedParticleAnalysisReports(3); ParticleAnalysisReport[] secondColorHits = secondColor.getOrderedParticleAnalysisReports(3); firstColor.free(); secondColor.free(); for (int i = 0; i < firstColorHits.length; i++) { ParticleAnalysisReport firstTrackReport = firstColorHits[i]; if (firstTrackReport.particleToImagePercent < FRC_PARTICLE_TO_IMAGE_PERCENT) { break; } for (int j = 0; j < secondColorHits.length; j++) { ParticleAnalysisReport secondTrackReport = secondColorHits[j]; if (secondTrackReport.particleToImagePercent < FRC_PARTICLE_TO_IMAGE_PERCENT) { break; } switch (position.value) { case Position.above_val: if (secondTrackReport.center_mass_y < firstTrackReport.center_mass_y) { // add in the SizesRelative call if needed - // so far it does not seem necessary if (aligned(firstTrackReport.center_mass_x, secondTrackReport.center_mass_x, firstTrackReport.boundingRectWidth, secondTrackReport.boundingRectWidth) && adjacent(firstTrackReport.boundingRectTop, (secondTrackReport.boundingRectTop + secondTrackReport.boundingRectHeight)) && sizesRelative(firstTrackReport.particleArea, secondTrackReport.particleArea)) { //return the relevant track report return new Target(firstTrackReport, secondTrackReport); } } break; case Position.below_val: if (secondTrackReport.center_mass_y > firstTrackReport.center_mass_y) { if (aligned(firstTrackReport.center_mass_x, secondTrackReport.center_mass_x, firstTrackReport.boundingRectWidth, secondTrackReport.boundingRectWidth) && adjacent((firstTrackReport.boundingRectTop + firstTrackReport.boundingRectHeight), secondTrackReport.boundingRectTop)) { return new Target(firstTrackReport, secondTrackReport); } } break; case Position.left_val: if (secondTrackReport.center_mass_x < firstTrackReport.center_mass_x) { if (aligned(firstTrackReport.center_mass_y, secondTrackReport.center_mass_y, firstTrackReport.boundingRectWidth, secondTrackReport.boundingRectWidth) && adjacent(firstTrackReport.boundingRectLeft, (secondTrackReport.boundingRectLeft + secondTrackReport.boundingRectWidth))) { return new Target(firstTrackReport, secondTrackReport); } } break; case Position.right_val: if (secondTrackReport.center_mass_x > firstTrackReport.center_mass_x) { if (aligned(firstTrackReport.center_mass_y, secondTrackReport.center_mass_y, firstTrackReport.boundingRectWidth, secondTrackReport.boundingRectWidth) && adjacent((firstTrackReport.boundingRectLeft + secondTrackReport.boundingRectWidth), secondTrackReport.boundingRectLeft)) { return new Target(firstTrackReport, secondTrackReport); } } break; } } } return null; } public double getXPosition() { return (firstParticle.center_mass_x_normalized * firstParticle.particleToImagePercent + secondParticle.center_mass_x_normalized * secondParticle.particleToImagePercent) / getSize(); } public double getYPosition() { return (firstParticle.center_mass_y_normalized * firstParticle.particleToImagePercent + secondParticle.center_mass_y_normalized * secondParticle.particleToImagePercent) / getSize(); } public double getSize() { return firstParticle.particleToImagePercent + secondParticle.particleToImagePercent; } public String toString() { return "Target at ( " + getXPosition() + " , " + getYPosition() + " ) of size " + getSize(); } }