trackapi.cpp

good luck to everyone!

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

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