Thanks
The team members of FRC Team 2220 would like to give a big thanks to all of the Sponsors, Mentors and Parents that make having this team possible.
Thank you!
Team Login
| TrackAPI.cpp |
 |
 |
 |
| Written by TarinB |
| Saturday, February 13 2010 16:49 |
|
/******************************************************************************** * Project : FIRST Motor Controller * File Name : TrackAPI.cpp * Contributors : ELF, DWD * Creation Date : August 10, 2008 * Revision History : Source code & revision history maintained at sourceforge.WPI.edu * File Description : Tracking Routines for FIRST Vision API */ /*----------------------------------------------------------------------------*/ /* 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 $(WIND_BASE)/WPILib. */ /*----------------------------------------------------------------------------*/
#include "string.h" #include "vxWorks.h"
#include "AxisCamera.h" #include "FrcError.h" #include "TrackAPI.h" #include "Utility.h" #include "VisionAPI.h"
int TrackAPI_debugFlag = 0; #define DPRINTF if(TrackAPI_debugFlag)dprintf
/** image quality requirement: particle must be .0025 of pixels */ #define PARTICLE_TO_IMAGE_PERCENT 0.25
/** * @brief Find the largest particle that meets a criteria * @param binaryImage Image to inspect * @param rect area to search * @return 0 = error */ bool InArea(Image* binaryImage, int particleIndex, Rect rect) { char funcName[]="InArea"; double position;
imaqMeasureParticle(binaryImage, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_LEFT, &position); if ( position < (rect.left ) ) return false; // outside left of rectangle?
imaqMeasureParticle(binaryImage, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_TOP, &position); if ( position < (rect.top ) ) return false; // outside top of rectangle ?
imaqMeasureParticle(binaryImage, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_RIGHT, &position); if (position > (rect.left + rect.width) ) return false; // outside right of rectangle ?
imaqMeasureParticle(binaryImage, particleIndex, 0, IMAQ_MT_BOUNDING_RECT_BOTTOM, &position); if (position > (rect.top + rect.height) ) return false; // outside bottom of rectangle ?
DPRINTF(LOG_INFO, "particle %i is in (%i %i) height %i width %i\n", particleIndex, rect.left, rect.top, rect.height, rect.width); return true; }
/** * @brief Find the largest particle that meets a criteria * @param binaryImage Image to inspect * @param largestParticleIndex Index of the largest particle * @param rect area to search * @return 0 = error */ int GetLargestParticle(Image* binaryImage, int* largestParticleIndex) { return GetLargestParticle(binaryImage, largestParticleIndex, IMAQ_NO_RECT); }
int GetLargestParticle(Image* binaryImage, int* largestParticleIndex, Rect rect) { *largestParticleIndex = 0; // points to caller-provided variable
/* determine number of particles in thresholded image */ int numParticles; int success = frcCountParticles(binaryImage, &numParticles); if ( !success ) { return success; }
/* if no particles found we can quit here */ if (numParticles == 0) { return 0; } // unsuccessful if zero particles found
// find the largest particle double largestParticleArea = 0; double particleArea; for (int i = 0; i < numParticles; ++i) { success = imaqMeasureParticle(binaryImage, i, 0, IMAQ_MT_AREA, &particleArea); if ( !success ) { return success; } if (particleArea > largestParticleArea) { // see if is in the right area if ( InArea(binaryImage, i, rect) ) { largestParticleArea = particleArea; *largestParticleIndex = i; // return index to caller } } }
return success; }
/** * @brief Search for a color. Supports IMAQ_IMAGE_HSL. * @param color Definition for the hue range * @param trackReport Values for tracking: center of particle, particle size, color * @return 0 = error */ int FindColor(FrcHue color, ParticleAnalysisReport* trackReport) { char funcName[]="FindColor"; int success = 0; // return: 0 = error
/* track color */ // use ACTIVE_LIGHT or WHITE_LIGHT for brightly lit objects TrackingThreshold td = GetTrackingData(color, PASSIVE_LIGHT);
success = FindColor(IMAQ_HSL, &td.hue, &td.saturation, &td.luminance, trackReport); if ( !success ) { DPRINTF (LOG_INFO, "did not find color - errorCode= %i",GetLastVisionError()); return success; }
//PrintReport(par);
/* set an image quality restriction */ if (trackReport->particleToImagePercent < PARTICLE_TO_IMAGE_PERCENT) { imaqSetError(ERR_PARTICLE_TOO_SMALL, funcName); success = 0; } return success; }
/** * @brief Search for a color. Supports IMAQ_IMAGE_HSL. * @param hueRange The range for the first plane * @param trackReport Values for tracking: center of particle, particle size, color * @return 0 = error */ int FindColor(const Range* hueRange, ParticleAnalysisReport *trackReport) { return FindColor(hueRange, DEFAULT_SATURATION_THRESHOLD, trackReport); }
/** * @brief Search for a color. Supports IMAQ_IMAGE_HSL. * @param hueRange The range for the first plane * @param minSaturation The lower range saturation * @param trackReport Values for tracking: center of particle, particle size, color * @return 0 = error */ int FindColor(const Range* hueRange, int minSaturation, ParticleAnalysisReport *trackReport) { Range satRange; satRange.minValue = minSaturation; satRange.maxValue = 255; Range lumRange; lumRange.minValue = 0; lumRange.maxValue = 255; ColorMode cmode = IMAQ_HSL; return FindColor(cmode, hueRange, &satRange, &lumRange, trackReport); }
/** * @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB. * @param mode Color mode, either IMAQ_HSL or IMAQ_RGB * @param plane1Range The range for the first plane (hue or red) * @param plane2Range The range for the second plane (saturation or green) * @param plane3Range The range for the third plane (luminance or blue) * @param trackReport Values for tracking: center of particle, particle size, etc * @return 0 = error */ int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range, const Range* plane3Range, ParticleAnalysisReport *trackReport) { return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport, NULL); }
/** * @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB. * @param mode Color mode, either IMAQ_HSL or IMAQ_RGB * @param plane1Range The range for the first plane (hue or red) * @param plane2Range The range for the second plane (saturation or green) * @param plane3Range The range for the third plane (luminance or blue) * @param trackReport Values for tracking: center of particle, particle size, etc * @param colorReport Color charactaristics of the particle * @return 0 = error */ int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range, const Range* plane3Range, ParticleAnalysisReport *trackReport, ColorReport *colorReport) { return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport, NULL, IMAQ_NO_RECT); }
/** * @brief Search for a color. Supports IMAQ_IMAGE_HSL and IMAQ_IMAGE_RGB. * @param mode Color mode, either IMAQ_HSL or IMAQ_RGB * @param plane1Range The range for the first plane (hue or red) * @param plane2Range The range for the second plane (saturation or green) * @param plane3Range The range for the third plane (luminance or blue) * @param trackReport Values for tracking: center of particle, particle size, etc * @param colorReport Color charactaristics of the particle * @param rect Rectangle to confine search to * @return 0 = error */ int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range, const Range* plane3Range, ParticleAnalysisReport *trackReport, ColorReport *colorReport, Rect rect) { char funcName[]="FindColor"; int errorCode = 0; int success = 0;
/* create an image object */ Image* cameraImage = frcCreateImage(IMAQ_IMAGE_HSL); if (!cameraImage) { return success; }
/* get image from camera - if the camera has not finished initializing, * this will fail */ success = GetImage(cameraImage,NULL); if (!success){ DPRINTF(LOG_INFO, "No camera Image available Error = %i %s", errorCode, GetVisionErrorText(errorCode)); frcDispose(cameraImage); imaqSetError(errorCode, funcName); //reset error code for the caller return success; }
/* save a copy of the image to another image for color thresholding later */ Image* histImage = frcCreateImage(IMAQ_IMAGE_HSL); if (!histImage) { frcDispose(cameraImage); return success; } success = frcCopyImage(histImage,cameraImage); if ( !success ) { errorCode = GetLastVisionError(); frcDispose(funcName,cameraImage,histImage,NULL); return success; }
/* Color threshold the image */ success = frcColorThreshold(cameraImage, cameraImage, mode, plane1Range, plane2Range, plane3Range); if ( !success ) { errorCode = GetLastVisionError(); DPRINTF (LOG_DEBUG, "Error = %i %s ", errorCode, GetVisionErrorText(errorCode)); frcDispose(funcName,cameraImage,histImage,NULL); return success; }
int largestParticleIndex = 0; success = GetLargestParticle(cameraImage, &largestParticleIndex, rect ); if ( !success ) { errorCode = GetLastVisionError(); DPRINTF (LOG_DEBUG, "Error after GetLargestParticle = %i %s ", errorCode, GetVisionErrorText(errorCode)); frcDispose(funcName,cameraImage,histImage,NULL); imaqSetError(ERR_COLOR_NOT_FOUND, funcName); return success; } DPRINTF(LOG_INFO, "largestParticleIndex = %i\n", largestParticleIndex);
/* Particles were found */ /* * Fill in report information for largest particle found */ success = frcParticleAnalysis(cameraImage, largestParticleIndex, trackReport); trackReport->imageTimestamp = GetTime();
/* clean up */ if (!success) {frcDispose(funcName,cameraImage,histImage,NULL); return success;}
/* particle color statistics */ /* only if a color report requested */ if (colorReport != NULL) { /* first filter out the other particles */ ParticleFilterCriteria2 criteria; ParticleFilterOptions* options = NULL; Rect rect; int numParticles; success = frcParticleFilter(cameraImage, cameraImage, &criteria, 1, options, rect, &numParticles); if ( !success ) { DPRINTF(LOG_INFO, "frcParticleFilter errorCode %i", GetLastVisionError()); }
/* histogram the original image using the thresholded image as a mask */ int numClasses = 10; //how many classes? ColorHistogramReport* chrep = imaqColorHistogram2(histImage, numClasses, IMAQ_HSL, NULL, cameraImage); if (chrep == NULL) { DPRINTF(LOG_INFO, "NULL Color Histogram"); errorCode = GetLastVisionError(); } else { colorReport->particleHueMax = chrep->plane1.max; colorReport->particleHueMin = chrep->plane1.min; colorReport->particleHueMean = chrep->plane1.mean; colorReport->particleSatMax = chrep->plane2.max; colorReport->particleSatMin = chrep->plane2.min; colorReport->particleSatMean = chrep->plane2.mean; colorReport->particleLumMax = chrep->plane3.max; colorReport->particleLumMin = chrep->plane3.min; colorReport->particleLumMean = chrep->plane3.mean; colorReport->numberParticlesFound = numParticles; frcDispose(chrep); } }
/* clean up */ frcDispose(funcName,cameraImage,histImage,NULL);
return success; }
/** * @brief Search for two colors in relation to each other in an Image. * Supports IMAQ_IMAGE_HSL color mode. * @param td1 Tracking data for the first color, including hue, sat, lum * @param td2 Tracking data for the second color, including hue, sat, lum * @param position Position of second color in relation to the first: * ABOVE, BELOW, RIGHT or LEFT * @param trackReport Particle analysis report with averaged positional data (x, y) * @return 0 = error */ int FindTwoColors(TrackingThreshold td1, TrackingThreshold td2, SecondColorPosition position, ParticleAnalysisReport *trackReport) { char funcName[]="FindTwoColors"; ParticleAnalysisReport secondTrackReport;
// search for the first color int success = FindColor(IMAQ_HSL, &td1.hue, &td1.saturation, &td1.luminance, trackReport, NULL); if (!success) return success; DPRINTF(LOG_DEBUG, "found first color: %s", td1.name);
// found the first color, now look for the second color success = FindColor(IMAQ_HSL, &td2.hue, &td2.saturation, &td2.luminance, &secondTrackReport, NULL); if (!success) return success;
// found both colors. check position. switch (position) { case ABOVE: //is second color above first color? if (secondTrackReport.center_mass_y < trackReport->center_mass_y) { return true; } break; case BELOW: //is second color below first color? if (secondTrackReport.center_mass_y > trackReport->center_mass_y) { return true; } break; case RIGHT: //is second color to the right of first color? if (secondTrackReport.center_mass_x > trackReport->center_mass_x) { return true; } break; case LEFT: //is second color to the left of first color? if (secondTrackReport.center_mass_x < trackReport->center_mass_x) { return true; } break; default: DPRINTF(LOG_ERROR, "invalid position parameter"); }
DPRINTF(LOG_DEBUG, "%s and %s NOT IN CORRECT POSITION", td1.name, td2.name);
return false; }
/** * Data functions for tracking */
/** * @brief Get default HSL tracking parameters * Note these parameters are not fully characterized at this point * Get these default values and modify them as needed for your environment * @param hue tasked color * @param light saturation/luminance */ TrackingThreshold GetTrackingData(FrcHue hue, FrcLight light) { //char funcName[]="GetTrackingData"; TrackingThreshold trackingData;
//set saturation & luminance switch (light) { default: case FLUORESCENT: trackingData.saturation.minValue = 100; trackingData.saturation.maxValue = 255; trackingData.luminance.minValue = 40; trackingData.luminance.maxValue = 255; if (hue == GREEN) trackingData.luminance.minValue = 100; if (hue == PINK) trackingData.saturation.minValue = 80; if (hue == PINK) trackingData.luminance.minValue = 60; if (hue == PINK) trackingData.luminance.maxValue = 155; break; case PASSIVE_LIGHT: trackingData.saturation.minValue = 50; trackingData.saturation.maxValue = 255; trackingData.luminance.minValue = 20; trackingData.luminance.maxValue = 255; break; case BRIGHT_LIGHT: trackingData.saturation.minValue = 0; trackingData.saturation.maxValue = 100; trackingData.luminance.minValue = 100; trackingData.luminance.maxValue = 255; break; case ACTIVE_LIGHT: trackingData.saturation.minValue = 0; trackingData.saturation.maxValue = 50; trackingData.luminance.minValue = 150; trackingData.luminance.maxValue = 255; break; case WHITE_LIGHT: trackingData.saturation.minValue = 0; trackingData.saturation.maxValue = 20; trackingData.luminance.minValue = 200; trackingData.luminance.maxValue = 255; break; case MARIUCCI_PINK_LIGHT: trackingData.saturation.minValue = 75; trackingData.saturation.maxValue = 255; trackingData.luminance.minValue = 85; trackingData.luminance.maxValue = 255; break; case MARIUCCI_GREEN_LIGHT: trackingData.saturation.minValue = 23; trackingData.saturation.maxValue = 255; trackingData.luminance.minValue = 92; trackingData.luminance.maxValue = 255; break; }
//set hue switch (hue){ default: case WHITE: strcpy (trackingData.name, "WHITE"); trackingData.hue.minValue = 0; trackingData.hue.maxValue = 255; break; case ORANGE: strcpy (trackingData.name, "ORANGE"); trackingData.hue.minValue = 5; trackingData.hue.maxValue = 25; break; case YELLOW: strcpy (trackingData.name, "YELLOW"); trackingData.hue.minValue = 30; trackingData.hue.maxValue = 50; break; case GREEN: strcpy (trackingData.name, "GREEN"); if (light == FLUORESCENT) { trackingData.hue.minValue = 60; trackingData.hue.maxValue = 110; } else { trackingData.hue.minValue = 90; trackingData.hue.maxValue = 125; } break; case BLUE: strcpy (trackingData.name, "BLUE"); trackingData.hue.minValue = 140; trackingData.hue.maxValue = 170; break; case PURPLE: strcpy (trackingData.name, "PURPLE"); trackingData.hue.minValue = 180; trackingData.hue.maxValue = 200; break; case PINK: strcpy (trackingData.name, "PINK"); trackingData.hue.minValue = 210; trackingData.hue.maxValue = 250; break; case RED: strcpy (trackingData.name, "RED"); trackingData.hue.minValue = 240; trackingData.hue.maxValue = 255; break; case MARIUCCI_PINK: strcpy (trackingData.name, "MARIUCCI_PINK"); trackingData.hue.minValue = 220; trackingData.hue.maxValue = 255; break; case MARIUCCI_GREEN: strcpy (trackingData.name, "MARIUCCI_GREEN"); trackingData.hue.minValue = 55; trackingData.hue.maxValue = 125; break; } return(trackingData); }
/** * Print particle analysis report * @param myReport Report to print */ void PrintReport(ParticleAnalysisReport* myReport) { char funcName[]="PrintReport"; dprintf(LOG_INFO, "particle analysis:\n %s%i %s%i\n %s%d\n %s%i %s%i\n %s%g %s%g\n %s%g\n %s%i %s%i\n %s%i %s%i\n", "imageHeight = ", myReport->imageHeight, "imageWidth = ", myReport->imageWidth, "imageTimestamp = ", myReport->imageTimestamp, "center_mass_x = ", myReport->center_mass_x, "center_mass_y = ", myReport->center_mass_y, "center_mass_x_normalized = ", myReport->center_mass_x_normalized, "center_mass_y_normalized = ", myReport->center_mass_y_normalized, "particleArea = ", myReport->particleArea, "boundingRectangleTop = ", myReport->boundingRect.top, "boundingRectangleLeft = ", myReport->boundingRect.left, "boundingRectangleHeight = ", myReport->boundingRect.height, "boundingRectangleWidth = ", myReport->boundingRect.width);
dprintf(LOG_INFO, "quality statistics: \n %s%g %s%g \n", "particleToImagePercent = ", myReport->particleToImagePercent, "particleQuality = ", myReport->particleQuality); }
/** * Print color report * @param myReport Report to print */ void PrintReport(ColorReport* myReport) { char funcName[]="PrintReport"; dprintf(LOG_INFO, "particle ranges for %i particles:", "numberParticlesFound = ", myReport->numberParticlesFound); ; dprintf(LOG_INFO, "\n %s%f %s%f %s%f\n %s%f %s%f %s%f\n %s%f %s%f %s%f\n -------", "particleHueMax = ", myReport->particleHueMax, "particleHueMin = ", myReport->particleHueMin, "particleHueMean = ", myReport->particleHueMean, "particleSatMax = ", myReport->particleSatMax, "particleSatMin = ", myReport->particleSatMin, "particleSatMean = ", myReport->particleSatMean, "particleLumMax = ", myReport->particleLumMax, "particleLumMin = ", myReport->particleLumMin, "particleLumMean = ", myReport->particleLumMean);
}
/** * Print color report * @param myReport Report to print */ void PrintReport(TrackingThreshold* myReport) { char funcName[]="PrintReport"; dprintf(LOG_INFO, "name of color: %s", myReport->name);
dprintf(LOG_INFO, "\n %s%i %s%i\n %s%i %s%i\n %s%i %s%i\n -------", "hueMin = ", myReport->hue.minValue, "hueMax = ", myReport->hue.maxValue, "satMin = ", myReport->saturation.minValue, "satMax = ", myReport->saturation.maxValue, "lumMin = ", myReport->luminance.minValue, "lumMax = ", myReport->luminance.maxValue );
}
|
