| /******************************************************************************** | |
| * 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 "VisionAPI.h" | |
| int TrackAPI_debugFlag = 0; | |
| #define DPRINTF if(TrackAPI_debugFlag)dprintf | |
| /** | |
| * @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) | |
| { | |
| 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) | |
| { | |
| 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, __FUNCTION__); | |
| 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) | |
| { | |
| 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 | |
| */ | |
| double imageTime; | |
| success = GetImage(cameraImage, &imageTime); | |
| if (!success){ | |
| DPRINTF(LOG_INFO, "No camera Image available Error = %i %s", | |
| errorCode, GetVisionErrorText(errorCode)); | |
| frcDispose(cameraImage); | |
| imaqSetError(errorCode, __FUNCTION__); //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(__FUNCTION__,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(__FUNCTION__,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(__FUNCTION__,cameraImage,histImage,NULL); | |
| imaqSetError(ERR_COLOR_NOT_FOUND, __FUNCTION__); | |
| 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 = imageTime; | |
| /* clean up */ | |
| if (!success) {frcDispose(__FUNCTION__,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(__FUNCTION__,cameraImage,histImage,NULL); | |
| return success; | |
| } | |
| /** | |
| * 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) | |
| { | |
| 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; | |
| } | |
| //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; | |
| } | |
| return(trackingData); | |
| } | |
| /** | |
| * Print particle analysis report | |
| * @param myReport Report to print | |
| */ | |
| void PrintReport(ParticleAnalysisReport* myReport) | |
| { | |
| dprintf(LOG_INFO, "particle analysis:\n %s%i %s%i\n %s%lf\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) | |
| { | |
| 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) | |
| { | |
| 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 ); | |
| } | |