TrackAPI.cpp

/********************************************************************************
*  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

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; 
}

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;
}

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;
}

int FindColor(const Range* hueRange, ParticleAnalysisReport *trackReport)
{ return FindColor(hueRange, DEFAULT_SATURATION_THRESHOLD, trackReport); }

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);
}

int FindColor(ColorMode mode, const Range* plane1Range, const Range* plane2Range, 
                const Range* plane3Range, ParticleAnalysisReport *trackReport)
{
        return FindColor(mode, plane1Range, plane2Range, plane3Range, trackReport, NULL);
}

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);
}

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; 
}


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);
}


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);
}

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);

}

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 );

}