azaleasource/WPILibCProgramming/trunk/WPILib/Vision2009/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

/**
* @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 );

}