diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Color_src/Robot.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Color_src/Robot.cpp new file mode 100644 index 0000000000..7818d65354 --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Color_src/Robot.cpp @@ -0,0 +1,235 @@ +#include "WPILib.h" +#include +#include + +/** + * Example of finding yellow totes based on color. + * This example utilizes an image file, which you need to copy to the roboRIO + * To use a camera you will have to integrate the appropriate camera details with this example. + * To use a USB camera instead, see the IntermediateVision example for details + * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on + * using an Axis Camera. + */ +class VisionColor2015Sample : public SampleRobot +{ + //A structure to hold measurements of a particle + struct ParticleReport { + double PercentAreaToImageArea; + double Area; + double ConvexHullArea; + double BoundingRectLeft; + double BoundingRectTop; + double BoundingRectRight; + double BoundingRectBottom; + }; + + //Structure to represent the scores for the various tests used for target identification + struct Scores { + double Trapezoid; + double LongAspect; + double ShortAspect; + double AreaToConvexHullArea; + }; + + //Images + Image *frame; + Image *binaryFrame; + int imaqError; + + //Constants + Range TOTE_HUE_RANGE = {24, 49}; //Default hue range for yellow tote + Range TOTE_SAT_RANGE = {67, 255}; //Default saturation range for yellow tote + Range TOTE_VAL_RANGE = {49, 255}; //Default value range for yellow tote + double AREA_MINIMUM = 2; //Default Area minimum for particle as a percentage of total image area + double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22 + double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4 + double SCORE_MIN = 75.0; //Minimum score to be considered a tote + double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480 + ParticleFilterCriteria2 criteria[1]; + ParticleFilterOptions2 filterOptions = {0,0,1,1}; + Scores scores; + + +public: + void RobotInit() override { + // create images + frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0); + binaryFrame = imaqCreateImage(IMAQ_IMAGE_U8, 0); + + //Put default values to SmartDashboard so fields will appear + SmartDashboard::PutNumber("Tote hue min", TOTE_HUE_RANGE.minValue); + SmartDashboard::PutNumber("Tote hue max", TOTE_HUE_RANGE.maxValue); + SmartDashboard::PutNumber("Tote sat min", TOTE_SAT_RANGE.minValue); + SmartDashboard::PutNumber("Tote sat max", TOTE_SAT_RANGE.maxValue); + SmartDashboard::PutNumber("Tote val min", TOTE_VAL_RANGE.minValue); + SmartDashboard::PutNumber("Tote val max", TOTE_VAL_RANGE.maxValue); + SmartDashboard::PutNumber("Area min %", AREA_MINIMUM); + } + + void Autonomous() override { + while (IsAutonomous() && IsEnabled()) + { + //read file in from disk. For this example to run you need to copy image20.jpg from the SampleImages folder in this project to the + //directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp + imaqError = imaqReadFile(frame, "//home//lvuser//SampleImages//image20.jpg", NULL, NULL); + + //Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished. + TOTE_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", TOTE_HUE_RANGE.minValue); + TOTE_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", TOTE_HUE_RANGE.maxValue); + TOTE_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", TOTE_SAT_RANGE.minValue); + TOTE_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", TOTE_SAT_RANGE.maxValue); + TOTE_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", TOTE_VAL_RANGE.minValue); + TOTE_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", TOTE_VAL_RANGE.maxValue); + + //Threshold the image looking for yellow (tote color) + imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &TOTE_HUE_RANGE, &TOTE_SAT_RANGE, &TOTE_VAL_RANGE); + + //Send particle count to dashboard + int numParticles = 0; + imaqError = imaqCountParticles(binaryFrame, 1, &numParticles); + SmartDashboard::PutNumber("Masked particles", numParticles); + + //Send masked image to dashboard to assist in tweaking mask. + SendToDashboard(binaryFrame, imaqError); + + //filter out small particles + float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM); + criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false}; + imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL); + + //Send particle count after filtering to dashboard + imaqError = imaqCountParticles(binaryFrame, 1, &numParticles); + SmartDashboard::PutNumber("Filtered particles", numParticles); + + if(numParticles > 0) + { + //Measure particles and sort by particle size + std::vector particles; + for(int particleIndex = 0; particleIndex < numParticles; particleIndex++) + { + ParticleReport par; + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_CONVEX_HULL_AREA, &(par.ConvexHullArea)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight)); + particles.push_back(*par); + } + sort(particles->begin(), particles->end(), CompareParticleSizes); + + //This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise + //for the reader. Note that the long and short side scores expect a single tote and will not work for a stack of 2 or more totes. + //Modification of the code to accommodate 2 or more stacked totes is left as an exercise for the reader. + scores.Trapezoid = TrapezoidScore(particles.at(0)); + SmartDashboard::PutNumber("Trapezoid", scores.Trapezoid); + scores.LongAspect = LongSideScore(particles.at(0)); + SmartDashboard::PutNumber("Long Aspect", scores.LongAspect); + scores.ShortAspect = ShortSideScore(particles.at(0)); + SmartDashboard::PutNumber("Short Aspect", scores.ShortAspect); + scores.AreaToConvexHullArea = ConvexHullAreaScore(particles.at(0)); + SmartDashboard::PutNumber("Convex Hull Area", scores.AreaToConvexHullArea); + bool isTote = scores.Trapezoid > SCORE_MIN && (scores.LongAspect > SCORE_MIN || scores.ShortAspect > SCORE_MIN) && scores.AreaToConvexHullArea > SCORE_MIN; + bool isLong = scores.LongAspect > scores.ShortAspect; + + //Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote + SmartDashboard::PutBoolean("IsTote", isTote); + SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0), isLong)); + } else { + SmartDashboard::PutBoolean("IsTote", false); + } + + Wait(0.005); // wait for a motor update time + } + } + + void OperatorControl() override { + while(IsOperatorControl() && IsEnabled()) { + Wait(0.005); // wait for a motor update time + } + } + + + //Send image to dashboard if IMAQ has not thrown an error + void SendToDashboard(Image *image, int error) + { + if(error < ERR_SUCCESS) { + DriverStation::ReportError("Send To Dashboard error: " + std::to_string((long)imaqError) + "\n"); + } else { + CameraServer::GetInstance()->SetImage(binaryFrame); + } + } + + //Comparator function for sorting particles. Returns true if particle 1 is larger + static bool CompareParticleSizes(ParticleReport particle1, ParticleReport particle2) + { + //we want descending sort order + return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea; + } + + /** + * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise + * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2 + */ + double ratioToScore(double ratio) + { + return (fmax(0, fmin(100*(1-fabs(1-ratio)), 100))); + } + + /** + * Method to score convex hull area. This scores how "complete" the particle is. Particles with large holes will score worse than a filled in shape + */ + double ConvexHullAreaScore(ParticleReport report) + { + return ratioToScore((report.Area/report.ConvexHullArea)*1.18); + } + + /** + * Method to score if the particle appears to be a trapezoid. Compares the convex hull (filled in) area to the area of the bounding box. + * The expectation is that the convex hull area is about 95.4% of the bounding box area for an ideal tote. + */ + double TrapezoidScore(ParticleReport report) + { + return ratioToScore(report.ConvexHullArea/((report.BoundingRectRight-report.BoundingRectLeft)*(report.BoundingRectBottom-report.BoundingRectTop)*.954)); + } + + /** + * Method to score if the aspect ratio of the particle appears to match the long side of a tote. + */ + double LongSideScore(ParticleReport report) + { + return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))/LONG_RATIO); + } + + /** + * Method to score if the aspect ratio of the particle appears to match the short side of a tote. + */ + double ShortSideScore(ParticleReport report){ + return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))/SHORT_RATIO); + } + + /** + * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics + * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation. + * + * @param image The image to use for measuring the particle estimated rectangle + * @param report The Particle Analysis Report for the particle + * @param isLong Boolean indicating if the target is believed to be the long side of a tote + * @return The estimated distance to the target in feet. + */ + double computeDistance (Image *image, ParticleReport report, bool isLong) { + double normalizedWidth, targetWidth; + int xRes, yRes; + + imaqGetImageSize(image, &xRes, &yRes); + normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/xRes; + SmartDashboard::PutNumber("Width", normalizedWidth); + targetWidth = isLong ? 26.9 : 16.9; + + return targetWidth/(normalizedWidth*12*tan(VIEW_ANGLE*M_PI/(180*2))); + } +}; + +START_ROBOT_CLASS(VisionColor2015Sample); + diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Retro_src/Robot.cpp b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Retro_src/Robot.cpp new file mode 100644 index 0000000000..a2e915a5ff --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/Retro_src/Robot.cpp @@ -0,0 +1,208 @@ +#include "WPILib.h" +#include +#include + +/** + * Example of finding yellow totes based on retroreflective target. + * This example utilizes an image file, which you need to copy to the roboRIO + * To use a camera you will have to integrate the appropriate camera details with this example. + * To use a USB camera instead, see the IntermediateVision example for details + * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on + * using an Axis Camera. + */ +class VisionRetro2015Sample : public SampleRobot +{ + //A structure to hold measurements of a particle + struct ParticleReport { + double PercentAreaToImageArea; + double Area; + double BoundingRectLeft; + double BoundingRectTop; + double BoundingRectRight; + double BoundingRectBottom; + }; + + //Structure to represent the scores for the various tests used for target identification + struct Scores { + double Area; + double Aspect; + }; + + //Images + Image *frame; + Image *binaryFrame; + int imaqError; + + //Constants + Range RING_HUE_RANGE = {101, 64}; //Default hue range for ring light + Range RING_SAT_RANGE = {88, 255}; //Default saturation range for ring light + Range RING_VAL_RANGE = {134, 255}; //Default value range for ring light + double AREA_MINIMUM = 0.5; //Default Area minimum for particle as a percentage of total image area + double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22 + double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4 + double SCORE_MIN = 75.0; //Minimum score to be considered a tote + double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480 + ParticleFilterCriteria2 criteria[1]; + ParticleFilterOptions2 filterOptions = {0,0,1,1}; + Scores scores; + + +public: + void RobotInit() override { + // create images + frame = imaqCreateImage(IMAQ_IMAGE_RGB, 0); + binaryFrame = imaqCreateImage(IMAQ_IMAGE_U8, 0); + + //Put default values to SmartDashboard so fields will appear + SmartDashboard::PutNumber("Tote hue min", RING_HUE_RANGE.minValue); + SmartDashboard::PutNumber("Tote hue max", RING_HUE_RANGE.maxValue); + SmartDashboard::PutNumber("Tote sat min", RING_SAT_RANGE.minValue); + SmartDashboard::PutNumber("Tote sat max", RING_SAT_RANGE.maxValue); + SmartDashboard::PutNumber("Tote val min", RING_VAL_RANGE.minValue); + SmartDashboard::PutNumber("Tote val max", RING_VAL_RANGE.maxValue); + SmartDashboard::PutNumber("Area min %", AREA_MINIMUM); + } + + void Autonomous() override { + while (IsAutonomous() && IsEnabled()) + { + //read file in from disk. For this example to run you need to copy image.jpg from the SampleImages folder in this project to the + //directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp + imaqError = imaqReadFile(frame, "//home//lvuser//SampleImages//image.jpg", NULL, NULL); + + //Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished. + RING_HUE_RANGE.minValue = SmartDashboard::GetNumber("Tote hue min", RING_HUE_RANGE.minValue); + RING_HUE_RANGE.maxValue = SmartDashboard::GetNumber("Tote hue max", RING_HUE_RANGE.maxValue); + RING_SAT_RANGE.minValue = SmartDashboard::GetNumber("Tote sat min", RING_SAT_RANGE.minValue); + RING_SAT_RANGE.maxValue = SmartDashboard::GetNumber("Tote sat max", RING_SAT_RANGE.maxValue); + RING_VAL_RANGE.minValue = SmartDashboard::GetNumber("Tote val min", RING_VAL_RANGE.minValue); + RING_VAL_RANGE.maxValue = SmartDashboard::GetNumber("Tote val max", RING_VAL_RANGE.maxValue); + + //Threshold the image looking for ring light color + imaqError = imaqColorThreshold(binaryFrame, frame, 255, IMAQ_HSV, &RING_HUE_RANGE, &RING_SAT_RANGE, &RING_VAL_RANGE); + + //Send particle count to dashboard + int numParticles = 0; + imaqError = imaqCountParticles(binaryFrame, 1, &numParticles); + SmartDashboard::PutNumber("Masked particles", numParticles); + + //Send masked image to dashboard to assist in tweaking mask. + SendToDashboard(binaryFrame, imaqError); + + //filter out small particles + float areaMin = SmartDashboard::GetNumber("Area min %", AREA_MINIMUM); + criteria[0] = {IMAQ_MT_AREA_BY_IMAGE_AREA, areaMin, 100, false, false}; + imaqError = imaqParticleFilter4(binaryFrame, binaryFrame, criteria, 1, &filterOptions, NULL, NULL); + + //Send particle count after filtering to dashboard + imaqError = imaqCountParticles(binaryFrame, 1, &numParticles); + SmartDashboard::PutNumber("Filtered particles", numParticles); + + if(numParticles > 0) { + //Measure particles and sort by particle size + std::vector particles; + for(int particleIndex = 0; particleIndex < numParticles; particleIndex++) + { + ParticleReport par; + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA_BY_IMAGE_AREA, &(par.PercentAreaToImageArea)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_AREA, &(par.Area)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &(par.BoundingRectTop)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &(par.BoundingRectLeft)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &(par.BoundingRectBottom)); + imaqMeasureParticle(binaryFrame, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &(par.BoundingRectRight)); + particles.push_back(par); + } + sort(particles.begin(), particles.end(), CompareParticleSizes); + + //This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise + //for the reader. Note that this scores and reports information about a single particle (single L shaped target). To get accurate information + //about the location of the tote (not just the distance) you will need to correlate two adjacent targets in order to find the true center of the tote. + scores.Aspect = AspectScore(particles.at(0)); + SmartDashboard::PutNumber("Aspect", scores.Aspect); + scores.Area = AreaScore(particles.at(0)); + SmartDashboard::PutNumber("Area", scores.Area); + bool isTarget = scores.Area > SCORE_MIN && scores.Aspect > SCORE_MIN; + + //Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote + SmartDashboard::PutBoolean("IsTarget", isTarget); + SmartDashboard::PutNumber("Distance", computeDistance(binaryFrame, particles.at(0))); + } else { + SmartDashboard::PutBoolean("IsTarget", false); + } + + Wait(0.005); // wait for a motor update time + } + } + + void OperatorControl() override { + while(IsOperatorControl() && IsEnabled()) { + Wait(0.005); // wait for a motor update time + } + } + + + //Send image to dashboard if IMAQ has not thrown an error + void SendToDashboard(Image *image, int error) + { + if(error < ERR_SUCCESS) { + DriverStation::ReportError("Send To Dashboard error: " + std::to_string((long)imaqError) + "\n"); + } else { + CameraServer::GetInstance()->SetImage(binaryFrame); + } + } + + //Comparator function for sorting particles. Returns true if particle 1 is larger + static bool CompareParticleSizes(ParticleReport particle1, ParticleReport particle2) + { + //we want descending sort order + return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea; + } + + /** + * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise + * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2 + */ + double ratioToScore(double ratio) + { + return (fmax(0, fmin(100*(1-fabs(1-ratio)), 100))); + } + + + double AreaScore(ParticleReport report) + { + double boundingArea = (report.BoundingRectBottom - report.BoundingRectTop) * (report.BoundingRectRight - report.BoundingRectLeft); + //Tape is 7" edge so 49" bounding rect. With 2" wide tape it covers 24" of the rect. + return ratioToScore((49/24)*report.Area/boundingArea); + } + + /** + * Method to score if the aspect ratio of the particle appears to match the retro-reflective target. Target is 7"x7" so aspect should be 1 + */ + double AspectScore(ParticleReport report) + { + return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))); + } + + + /** + * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics + * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation. + * + * @param image The image to use for measuring the particle estimated rectangle + * @param report The Particle Analysis Report for the particle + * @return The estimated distance to the target in feet. + */ + double computeDistance (Image *image, ParticleReport report) { + double normalizedWidth, targetWidth; + int xRes, yRes; + + imaqGetImageSize(image, &xRes, &yRes); + normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/xRes; + SmartDashboard::PutNumber("Width", normalizedWidth); + targetWidth = 7; + + return targetWidth/(normalizedWidth*12*tan(VIEW_ANGLE*M_PI/(180*2))); + } +}; + +START_ROBOT_CLASS(VisionRetro2015Sample); diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/IMG_1800.png b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/IMG_1800.png new file mode 100644 index 0000000000..d80ed46e1e Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/IMG_1800.png differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image.jpg new file mode 100644 index 0000000000..a3e6f00cf2 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image10.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image10.jpg new file mode 100644 index 0000000000..a1ee41cbc9 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image10.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image11.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image11.jpg new file mode 100644 index 0000000000..860269a55b Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image11.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image12.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image12.jpg new file mode 100644 index 0000000000..148d3e5e18 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image12.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image13.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image13.jpg new file mode 100644 index 0000000000..f1588ed916 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image13.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image14.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image14.jpg new file mode 100644 index 0000000000..0fc233c40d Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image14.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image15.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image15.jpg new file mode 100644 index 0000000000..8cb8961c5d Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image15.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image16.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image16.jpg new file mode 100644 index 0000000000..d793ebbded Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image16.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image17.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image17.jpg new file mode 100644 index 0000000000..b5837d6d1c Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image17.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image18.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image18.jpg new file mode 100644 index 0000000000..346e1343a7 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image18.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image19.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image19.jpg new file mode 100644 index 0000000000..9908dd875e Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image19.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image2.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image2.jpg new file mode 100644 index 0000000000..6747e9ef5a Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image2.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image20.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image20.jpg new file mode 100644 index 0000000000..2ee1c5fdca Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image20.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image21.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image21.jpg new file mode 100644 index 0000000000..0a8c238599 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image21.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image3.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image3.jpg new file mode 100644 index 0000000000..713604e943 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image3.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image4.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image4.jpg new file mode 100644 index 0000000000..fc31585056 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image4.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image5.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image5.jpg new file mode 100644 index 0000000000..37bfd87153 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image5.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image6.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image6.jpg new file mode 100644 index 0000000000..c727371440 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image6.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image7.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image7.jpg new file mode 100644 index 0000000000..703f09b778 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image7.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image8.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image8.jpg new file mode 100644 index 0000000000..3d8d836f31 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image8.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image9.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image9.jpg new file mode 100644 index 0000000000..7a8524c561 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/2015Vision/SampleImages/image9.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml index 1013b57ed3..2cea463cb6 100644 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.cpp/resources/templates/examples/examples.xml @@ -335,7 +335,132 @@ destination="src/Robot.cpp"> + + + 2015 Vision Color Sample + An example program that demonstrates image processing to locate Yellow totes by color. + This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing. + To use this code with a camera, you must integrate the code for image acquisition from the appropriate + camera example; + + + Vision + Complete List + + + src + SampleImages + + + + + + + + + + + + + + + + + + + + + + + + + + + + 2015 Vision Retro Sample + An example program that demonstrates image processing to locate Yellow totes by the retroreflective target. + This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing. + To use this code with a camera, you must integrate the code for image acquisition from the appropriate + camera example; + + + Vision + Complete List + + + src + SampleImages + + + + + + + + + + + + + + + + + + + + + + + + + + GearsBot diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Color_src/Robot.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Color_src/Robot.java new file mode 100644 index 0000000000..df1f2c44a9 --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Color_src/Robot.java @@ -0,0 +1,212 @@ +package $package; + +import java.lang.Math; +import java.util.Comparator; +import java.util.Vector; + +import com.ni.vision.NIVision; +import com.ni.vision.NIVision.Image; +import com.ni.vision.NIVision.ImageType; + +import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.SampleRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Example of finding yellow totes based on color. + * This example utilizes an image file, which you need to copy to the roboRIO + * To use a camera you will have to integrate the appropriate camera details with this example. + * To use a USB camera instead, see the SimpelVision and AdvancedVision examples for details + * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on + * using an Axis Camera. + */ +public class Robot extends SampleRobot { + //A structure to hold measurements of a particle + public class ParticleReport implements Comparator, Comparable{ + double PercentAreaToImageArea; + double Area; + double BoundingRectLeft; + double BoundingRectTop; + double BoundingRectRight; + double BoundingRectBottom; + + public int compareTo(ParticleReport r) + { + return (int)(r.Area - this.Area); + } + + public int compare(ParticleReport r1, ParticleReport r2) + { + return (int)(r1.Area - r2.Area); + } + }; + + //Structure to represent the scores for the various tests used for target identification + public class Scores { + double Area; + double Aspect; + }; + + //Images + Image frame; + Image binaryFrame; + int imaqError; + + //Constants + NIVision.Range TOTE_HUE_RANGE = new NIVision.Range(101, 64); //Default hue range for yellow tote + NIVision.Range TOTE_SAT_RANGE = new NIVision.Range(88, 255); //Default saturation range for yellow tote + NIVision.Range TOTE_VAL_RANGE = new NIVision.Range(134, 255); //Default value range for yellow tote + double AREA_MINIMUM = 0.5; //Default Area minimum for particle as a percentage of total image area + double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22 + double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4 + double SCORE_MIN = 75.0; //Minimum score to be considered a tote + double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480 + NIVision.ParticleFilterCriteria2 criteria[] = new NIVision.ParticleFilterCriteria2[1]; + NIVision.ParticleFilterOptions2 filterOptions = new NIVision.ParticleFilterOptions2(0,0,1,1); + Scores scores = new Scores(); + + public void robotInit() { + // create images + frame = NIVision.imaqCreateImage(ImageType.IMAGE_RGB, 0); + binaryFrame = NIVision.imaqCreateImage(ImageType.IMAGE_U8, 0); + criteria[0] = new NIVision.ParticleFilterCriteria2(NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA, AREA_MINIMUM, 100.0, 0, 0); + + //Put default values to SmartDashboard so fields will appear + SmartDashboard.putNumber("Tote hue min", TOTE_HUE_RANGE.minValue); + SmartDashboard.putNumber("Tote hue max", TOTE_HUE_RANGE.maxValue); + SmartDashboard.putNumber("Tote sat min", TOTE_SAT_RANGE.minValue); + SmartDashboard.putNumber("Tote sat max", TOTE_SAT_RANGE.maxValue); + SmartDashboard.putNumber("Tote val min", TOTE_VAL_RANGE.minValue); + SmartDashboard.putNumber("Tote val max", TOTE_VAL_RANGE.maxValue); + SmartDashboard.putNumber("Area min %", AREA_MINIMUM); + } + + public void autonomous() { + while (isAutonomous() && isEnabled()) + { + //read file in from disk. For this example to run you need to copy image20.jpg from the SampleImages folder in this project to the + //directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp + NIVision.imaqReadFile(frame, "/home/lvuser/SampleImages/image20.jpg"); + + //Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished. + TOTE_HUE_RANGE.minValue = (int)SmartDashboard.getNumber("Tote hue min", TOTE_HUE_RANGE.minValue); + TOTE_HUE_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote hue max", TOTE_HUE_RANGE.maxValue); + TOTE_SAT_RANGE.minValue = (int)SmartDashboard.getNumber("Tote sat min", TOTE_SAT_RANGE.minValue); + TOTE_SAT_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote sat max", TOTE_SAT_RANGE.maxValue); + TOTE_VAL_RANGE.minValue = (int)SmartDashboard.getNumber("Tote val min", TOTE_VAL_RANGE.minValue); + TOTE_VAL_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote val max", TOTE_VAL_RANGE.maxValue); + + //Threshold the image looking for yellow (tote color) + NIVision.imaqColorThreshold(binaryFrame, frame, 255, NIVision.ColorMode.HSV, TOTE_HUE_RANGE, TOTE_SAT_RANGE, TOTE_VAL_RANGE); + + //Send particle count to dashboard + int numParticles = NIVision.imaqCountParticles(binaryFrame, 1); + SmartDashboard.putNumber("Masked particles", numParticles); + + //Send masked image to dashboard to assist in tweaking mask. + CameraServer.getInstance().setImage(binaryFrame); + + //filter out small particles + float areaMin = (float)SmartDashboard.getNumber("Area min %", AREA_MINIMUM); + criteria[0].lower = areaMin; + imaqError = NIVision.imaqParticleFilter4(binaryFrame, binaryFrame, criteria, filterOptions, null); + + //Send particle count after filtering to dashboard + numParticles = NIVision.imaqCountParticles(binaryFrame, 1); + SmartDashboard.putNumber("Filtered particles", numParticles); + + if(numParticles > 0) + { + //Measure particles and sort by particle size + Vector particles = new Vector(); + for(int particleIndex = 0; particleIndex < numParticles; particleIndex++) + { + ParticleReport par = new ParticleReport(); + par.PercentAreaToImageArea = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA); + par.Area = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA); + par.BoundingRectTop = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_TOP); + par.BoundingRectLeft = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_LEFT); + par.BoundingRectBottom = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_BOTTOM); + par.BoundingRectRight = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_RIGHT); + particles.add(par); + } + particles.sort(null); + + //This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise + //for the reader. Note that the long and short side scores expect a single tote and will not work for a stack of 2 or more totes. + //Modification of the code to accommodate 2 or more stacked totes is left as an exercise for the reader. + scores.Aspect = AspectScore(particles.elementAt(0)); + SmartDashboard.putNumber("Aspect", scores.Aspect); + scores.Area = AreaScore(particles.elementAt(0)); + SmartDashboard.putNumber("Area", scores.Area); + boolean isTote = scores.Aspect > SCORE_MIN && scores.Area > SCORE_MIN; + + //Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote + SmartDashboard.putBoolean("IsTote", isTote); + SmartDashboard.putNumber("Distance", computeDistance(binaryFrame, particles.elementAt(0))); + } else { + SmartDashboard.putBoolean("IsTote", false); + } + + Timer.delay(0.005); // wait for a motor update time + } + } + + public void operatorControl() { + while(isOperatorControl() && isEnabled()) { + Timer.delay(0.005); // wait for a motor update time + } + } + + //Comparator function for sorting particles. Returns true if particle 1 is larger + static boolean CompareParticleSizes(ParticleReport particle1, ParticleReport particle2) + { + //we want descending sort order + return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea; + } + + /** + * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise + * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2 + */ + double ratioToScore(double ratio) + { + return (Math.max(0, Math.min(100*(1-Math.abs(1-ratio)), 100))); + } + + double AreaScore(ParticleReport report) + { + double boundingArea = (report.BoundingRectBottom - report.BoundingRectTop) * (report.BoundingRectRight - report.BoundingRectLeft); + //Tape is 7" edge so 49" bounding rect. With 2" wide tape it covers 24" of the rect. + return ratioToScore((49/24)*report.Area/boundingArea); + } + + /** + * Method to score if the aspect ratio of the particle appears to match the retro-reflective target. Target is 7"x7" so aspect should be 1 + */ + double AspectScore(ParticleReport report) + { + return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))); + } + + /** + * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics + * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation. + * + * @param image The image to use for measuring the particle estimated rectangle + * @param report The Particle Analysis Report for the particle + * @param isLong Boolean indicating if the target is believed to be the long side of a tote + * @return The estimated distance to the target in feet. + */ + double computeDistance (Image image, ParticleReport report) { + double normalizedWidth, targetWidth; + NIVision.GetImageSizeResult size; + + size = NIVision.imaqGetImageSize(image); + normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/size.width; + targetWidth = 7; + + return targetWidth/(normalizedWidth*12*Math.tan(VIEW_ANGLE*Math.PI/(180*2))); + } +} diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Retro_src/Robot.java b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Retro_src/Robot.java new file mode 100644 index 0000000000..47d83727c3 --- /dev/null +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/Retro_src/Robot.java @@ -0,0 +1,212 @@ +package $package; + +import java.lang.Math; +import java.util.Comparator; +import java.util.Vector; + +import com.ni.vision.NIVision; +import com.ni.vision.NIVision.Image; +import com.ni.vision.NIVision.ImageType; + +import edu.wpi.first.wpilibj.CameraServer; +import edu.wpi.first.wpilibj.SampleRobot; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Example of finding yellow totes based on retroreflective target. + * This example utilizes an image file, which you need to copy to the roboRIO + * To use a camera you will have to integrate the appropriate camera details with this example. + * To use a USB camera instead, see the SimpelVision and AdvancedVision examples for details + * on using the USB camera. To use an Axis Camera, see the AxisCamera example for details on + * using an Axis Camera. + */ +public class Robot extends SampleRobot { + //A structure to hold measurements of a particle + public class ParticleReport implements Comparator, Comparable{ + double PercentAreaToImageArea; + double Area; + double BoundingRectLeft; + double BoundingRectTop; + double BoundingRectRight; + double BoundingRectBottom; + + public int compareTo(ParticleReport r) + { + return (int)(r.Area - this.Area); + } + + public int compare(ParticleReport r1, ParticleReport r2) + { + return (int)(r1.Area - r2.Area); + } + }; + + //Structure to represent the scores for the various tests used for target identification + public class Scores { + double Area; + double Aspect; + }; + + //Images + Image frame; + Image binaryFrame; + int imaqError; + + //Constants + NIVision.Range TOTE_HUE_RANGE = new NIVision.Range(101, 64); //Default hue range for yellow tote + NIVision.Range TOTE_SAT_RANGE = new NIVision.Range(88, 255); //Default saturation range for yellow tote + NIVision.Range TOTE_VAL_RANGE = new NIVision.Range(134, 255); //Default value range for yellow tote + double AREA_MINIMUM = 0.5; //Default Area minimum for particle as a percentage of total image area + double LONG_RATIO = 2.22; //Tote long side = 26.9 / Tote height = 12.1 = 2.22 + double SHORT_RATIO = 1.4; //Tote short side = 16.9 / Tote height = 12.1 = 1.4 + double SCORE_MIN = 75.0; //Minimum score to be considered a tote + double VIEW_ANGLE = 49.4; //View angle fo camera, set to Axis m1011 by default, 64 for m1013, 51.7 for 206, 52 for HD3000 square, 60 for HD3000 640x480 + NIVision.ParticleFilterCriteria2 criteria[] = new NIVision.ParticleFilterCriteria2[1]; + NIVision.ParticleFilterOptions2 filterOptions = new NIVision.ParticleFilterOptions2(0,0,1,1); + Scores scores = new Scores(); + + public void robotInit() { + // create images + frame = NIVision.imaqCreateImage(ImageType.IMAGE_RGB, 0); + binaryFrame = NIVision.imaqCreateImage(ImageType.IMAGE_U8, 0); + criteria[0] = new NIVision.ParticleFilterCriteria2(NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA, AREA_MINIMUM, 100.0, 0, 0); + + //Put default values to SmartDashboard so fields will appear + SmartDashboard.putNumber("Tote hue min", TOTE_HUE_RANGE.minValue); + SmartDashboard.putNumber("Tote hue max", TOTE_HUE_RANGE.maxValue); + SmartDashboard.putNumber("Tote sat min", TOTE_SAT_RANGE.minValue); + SmartDashboard.putNumber("Tote sat max", TOTE_SAT_RANGE.maxValue); + SmartDashboard.putNumber("Tote val min", TOTE_VAL_RANGE.minValue); + SmartDashboard.putNumber("Tote val max", TOTE_VAL_RANGE.maxValue); + SmartDashboard.putNumber("Area min %", AREA_MINIMUM); + } + + public void autonomous() { + while (isAutonomous() && isEnabled()) + { + //read file in from disk. For this example to run you need to copy image.jpg from the SampleImages folder in this project to the + //directory shown below using FTP or SFTP: http://wpilib.screenstepslive.com/s/4485/m/24166/l/282299-roborio-ftp + NIVision.imaqReadFile(frame, "/home/lvuser/SampleImages/image.jpg"); + + //Update threshold values from SmartDashboard. For performance reasons it is recommended to remove this after calibration is finished. + TOTE_HUE_RANGE.minValue = (int)SmartDashboard.getNumber("Tote hue min", TOTE_HUE_RANGE.minValue); + TOTE_HUE_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote hue max", TOTE_HUE_RANGE.maxValue); + TOTE_SAT_RANGE.minValue = (int)SmartDashboard.getNumber("Tote sat min", TOTE_SAT_RANGE.minValue); + TOTE_SAT_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote sat max", TOTE_SAT_RANGE.maxValue); + TOTE_VAL_RANGE.minValue = (int)SmartDashboard.getNumber("Tote val min", TOTE_VAL_RANGE.minValue); + TOTE_VAL_RANGE.maxValue = (int)SmartDashboard.getNumber("Tote val max", TOTE_VAL_RANGE.maxValue); + + //Threshold the image looking for yellow (tote color) + NIVision.imaqColorThreshold(binaryFrame, frame, 255, NIVision.ColorMode.HSV, TOTE_HUE_RANGE, TOTE_SAT_RANGE, TOTE_VAL_RANGE); + + //Send particle count to dashboard + int numParticles = NIVision.imaqCountParticles(binaryFrame, 1); + SmartDashboard.putNumber("Masked particles", numParticles); + + //Send masked image to dashboard to assist in tweaking mask. + CameraServer.getInstance().setImage(binaryFrame); + + //filter out small particles + float areaMin = (float)SmartDashboard.getNumber("Area min %", AREA_MINIMUM); + criteria[0].lower = areaMin; + imaqError = NIVision.imaqParticleFilter4(binaryFrame, binaryFrame, criteria, filterOptions, null); + + //Send particle count after filtering to dashboard + numParticles = NIVision.imaqCountParticles(binaryFrame, 1); + SmartDashboard.putNumber("Filtered particles", numParticles); + + if(numParticles > 0) + { + //Measure particles and sort by particle size + Vector particles = new Vector(); + for(int particleIndex = 0; particleIndex < numParticles; particleIndex++) + { + ParticleReport par = new ParticleReport(); + par.PercentAreaToImageArea = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA_BY_IMAGE_AREA); + par.Area = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_AREA); + par.BoundingRectTop = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_TOP); + par.BoundingRectLeft = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_LEFT); + par.BoundingRectBottom = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_BOTTOM); + par.BoundingRectRight = NIVision.imaqMeasureParticle(binaryFrame, particleIndex, 0, NIVision.MeasurementType.MT_BOUNDING_RECT_RIGHT); + particles.add(par); + } + particles.sort(null); + + //This example only scores the largest particle. Extending to score all particles and choosing the desired one is left as an exercise + //for the reader. Note that this scores and reports information about a single particle (single L shaped target). To get accurate information + //about the location of the tote (not just the distance) you will need to correlate two adjacent targets in order to find the true center of the tote. + scores.Aspect = AspectScore(particles.elementAt(0)); + SmartDashboard.putNumber("Aspect", scores.Aspect); + scores.Area = AreaScore(particles.elementAt(0)); + SmartDashboard.putNumber("Area", scores.Area); + boolean isTote = scores.Aspect > SCORE_MIN && scores.Area > SCORE_MIN; + + //Send distance and tote status to dashboard. The bounding rect, particularly the horizontal center (left - right) may be useful for rotating/driving towards a tote + SmartDashboard.putBoolean("IsTote", isTote); + SmartDashboard.putNumber("Distance", computeDistance(binaryFrame, particles.elementAt(0))); + } else { + SmartDashboard.putBoolean("IsTote", false); + } + + Timer.delay(0.005); // wait for a motor update time + } + } + + public void operatorControl() { + while(isOperatorControl() && isEnabled()) { + Timer.delay(0.005); // wait for a motor update time + } + } + + //Comparator function for sorting particles. Returns true if particle 1 is larger + static boolean CompareParticleSizes(ParticleReport particle1, ParticleReport particle2) + { + //we want descending sort order + return particle1.PercentAreaToImageArea > particle2.PercentAreaToImageArea; + } + + /** + * Converts a ratio with ideal value of 1 to a score. The resulting function is piecewise + * linear going from (0,0) to (1,100) to (2,0) and is 0 for all inputs outside the range 0-2 + */ + double ratioToScore(double ratio) + { + return (Math.max(0, Math.min(100*(1-Math.abs(1-ratio)), 100))); + } + + double AreaScore(ParticleReport report) + { + double boundingArea = (report.BoundingRectBottom - report.BoundingRectTop) * (report.BoundingRectRight - report.BoundingRectLeft); + //Tape is 7" edge so 49" bounding rect. With 2" wide tape it covers 24" of the rect. + return ratioToScore((49/24)*report.Area/boundingArea); + } + + /** + * Method to score if the aspect ratio of the particle appears to match the retro-reflective target. Target is 7"x7" so aspect should be 1 + */ + double AspectScore(ParticleReport report) + { + return ratioToScore(((report.BoundingRectRight-report.BoundingRectLeft)/(report.BoundingRectBottom-report.BoundingRectTop))); + } + + /** + * Computes the estimated distance to a target using the width of the particle in the image. For more information and graphics + * showing the math behind this approach see the Vision Processing section of the ScreenStepsLive documentation. + * + * @param image The image to use for measuring the particle estimated rectangle + * @param report The Particle Analysis Report for the particle + * @param isLong Boolean indicating if the target is believed to be the long side of a tote + * @return The estimated distance to the target in feet. + */ + double computeDistance (Image image, ParticleReport report) { + double normalizedWidth, targetWidth; + NIVision.GetImageSizeResult size; + + size = NIVision.imaqGetImageSize(image); + normalizedWidth = 2*(report.BoundingRectRight - report.BoundingRectLeft)/size.width; + targetWidth = 7; + + return targetWidth/(normalizedWidth*12*Math.tan(VIEW_ANGLE*Math.PI/(180*2))); + } +} diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/IMG_1800.png b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/IMG_1800.png new file mode 100644 index 0000000000..d80ed46e1e Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/IMG_1800.png differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image.jpg new file mode 100644 index 0000000000..a3e6f00cf2 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image10.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image10.jpg new file mode 100644 index 0000000000..a1ee41cbc9 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image10.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image11.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image11.jpg new file mode 100644 index 0000000000..860269a55b Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image11.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image12.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image12.jpg new file mode 100644 index 0000000000..148d3e5e18 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image12.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image13.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image13.jpg new file mode 100644 index 0000000000..f1588ed916 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image13.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image14.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image14.jpg new file mode 100644 index 0000000000..0fc233c40d Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image14.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image15.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image15.jpg new file mode 100644 index 0000000000..8cb8961c5d Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image15.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image16.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image16.jpg new file mode 100644 index 0000000000..d793ebbded Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image16.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image17.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image17.jpg new file mode 100644 index 0000000000..b5837d6d1c Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image17.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image18.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image18.jpg new file mode 100644 index 0000000000..346e1343a7 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image18.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image19.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image19.jpg new file mode 100644 index 0000000000..9908dd875e Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image19.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image2.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image2.jpg new file mode 100644 index 0000000000..6747e9ef5a Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image2.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image20.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image20.jpg new file mode 100644 index 0000000000..2ee1c5fdca Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image20.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image21.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image21.jpg new file mode 100644 index 0000000000..0a8c238599 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image21.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image3.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image3.jpg new file mode 100644 index 0000000000..713604e943 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image3.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image4.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image4.jpg new file mode 100644 index 0000000000..fc31585056 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image4.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image5.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image5.jpg new file mode 100644 index 0000000000..37bfd87153 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image5.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image6.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image6.jpg new file mode 100644 index 0000000000..c727371440 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image6.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image7.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image7.jpg new file mode 100644 index 0000000000..703f09b778 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image7.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image8.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image8.jpg new file mode 100644 index 0000000000..3d8d836f31 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image8.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image9.jpg b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image9.jpg new file mode 100644 index 0000000000..7a8524c561 Binary files /dev/null and b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/2015Vision/SampleImages/image9.jpg differ diff --git a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml index af3c4c6917..a06e070bb9 100755 --- a/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml +++ b/eclipse-plugins/edu.wpi.first.wpilib.plugins.java/resources/templates/examples/examples.xml @@ -325,6 +325,132 @@ destination="src/$package-dir/Robot.java"> + + + 2015 Vision Color Sample + An example program that demonstrates image processing to locate Yellow totes by color. + This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing. + To use this code with a camera, you must integrate the code for image acquisition from the appropriate + camera example; + + + Vision + Complete List + + + src/$package-dir + SampleImages + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 2015 Vision Retro Sample + An example program that demonstrates image processing to locate Yellow totes by the retroreflective target. + This example uses a file which must be copied over to the roboRIO via FTP to demonstrate processing. + To use this code with a camera, you must integrate the code for image acquisition from the appropriate + camera example; + + + Vision + Complete List + + + src/$package-dir + SampleImages + + + + + + + + + + + + + + + + + + + + + + + + + + Axis Camera Sample