int ImageProcessing::IVA_ProcessImage(Image *image)
{
int success = 1;
if(!processingOn) return success;
IVA_Data *ivaData;
ImageType imageType;
int pParameter[1] = {35};
float plower[1] = {0};
float pUpper[1] = {50};
int pCalibrated[1] = {0};
int pExclude[1] = {0};
ImageType imageType1;
// Initializes internal data (buffers and array of points for caliper measurements)
VisionErrChk(ivaData = IVA_InitData(5, 0));
VisionErrChk(IVA_CLRThreshold(image, 0, 99, 144, 255, 102, 255, IMAQ_RGB));
//-------------------------------------------------------------------//
// Lookup Table: Equalize //
//-------------------------------------------------------------------//
// Calculates the histogram of the image and redistributes pixel values across
// the desired range to maintain the same pixel value distribution.
VisionErrChk(imaqGetImageType(image, &imageType));
VisionErrChk(imaqEqualize(image, image, 0, (imageType == IMAQ_IMAGE_U8 ? 255 : 0), NULL));
VisionErrChk(IVA_ParticleFilter(image, pParameter, plower, pUpper,
pCalibrated, pExclude, 1, TRUE, TRUE));
//-------------------------------------------------------------------//
// Lookup Table: Equalize //
//-------------------------------------------------------------------//
// Calculates the histogram of the image and redistributes pixel values across
// the desired range to maintain the same pixel value distribution.
VisionErrChk(imaqGetImageType(image, &imageType1));
VisionErrChk(imaqEqualize(image, image, 0, (imageType1 == IMAQ_IMAGE_U8 ? 255 : 0), NULL));
// Releases the memory allocated in the IVA_Data structure.
IVA_DisposeData(ivaData);
Error:
return success;
}
bool tracking (bool use_alternate_score) //camera tracking function
{
//takes one camera frame and turns towards tallest target
//returns true if target is within deadzone, returns false otherwise
Threshold tapeThreshold(0, 255, 0, 90, 220, 255); //red hsl as of 20110303, this is the hue, saturation and luminosicity ranges that we want
BinaryImage *tapePixels;//
Image *convexHull;
BinaryImage *convexHullBinaryImage;
ParticleAnalysisReport par;//analyzed blob (pre convex hull)
ParticleAnalysisReport convexpar;// ONE filled-in blob
vector<ParticleAnalysisReport>* pars;//where many analyzed blob goes (pre)
vector<ParticleAnalysisReport>* convexpars; //where MANY filled-in blobs go
bool foundAnything = false;
double best_score = 120;
double best_speed;
double particle_score;
ImageType t;
int bs;
img = cam->GetImage();
printf("cam->GetImage() returned frame %d x %d\n",img->GetWidth(),img->GetHeight());
tapePixels = img->ThresholdHSL(tapeThreshold);
imaqGetBorderSize(tapePixels->GetImaqImage(),&bs);
imaqGetImageType(tapePixels->GetImaqImage(),&t);
convexHull = imaqCreateImage(t,bs);
convexHullBinaryImage = new BinaryImageWrapper(convexHull);
convexHullBinaryImage->GetOrderedParticleAnalysisReports();
//tapePixels = img->ThresholdHSL(int 0,int 50,int -100,int -50,int luminenceLow,int luminanceHigh);
pars = tapePixels->GetOrderedParticleAnalysisReports();
imaqConvexHull(convexHull,tapePixels->GetImaqImage(),true);
convexHullBinaryImage = new BinaryImageWrapper(convexHull);
convexpars = convexHullBinaryImage->GetOrderedParticleAnalysisReports();
//imaqGetParticleInfo()
//convexpars = convexHull->GetOrderedParticleAnalysisReports();
for (int i=0;i < convexHullBinaryImage->GetNumberParticles();i++)
{
//par = (*pars)[0];
//convexpar = (*convexpars)[i];
convexpar = convexHullBinaryImage->GetParticleAnalysisReport(i);
par = tapePixels->GetParticleAnalysisReport(i);
if((convexpar.boundingRect.width < 10) || (convexpar.boundingRect.height < 7))
{
continue;
}
// printf("%d par:%f convex:%f particle area\n",i,par.particleArea,convexpar.particleArea);
if ((par.particleArea/convexpar.particleArea > 0.4))
{
printf("%d skip max fillness ratio\n",i);
continue;
}
if ((par.particleArea/convexpar.particleArea < 0.10))
{
printf("%d skip min fillness ratio\n",i);
continue;
}
if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)>1.8)
{
printf("%d skip max aspect ratio\n",i);
continue;
}
if((double)(convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)<.8)
{
printf("%d skip min aspect ratio\n",i);
continue;
}
//printf("%f center of mass x\n",par.center_mass_x_normalized);
//printf("%f center of mass y\n",par.center_mass_y_normalized);
distanceInInches = (18.0*179.3)/(convexpar.boundingRect.height);
double pwidth = convexpar.boundingRect.width;
double mwidth = ((double)convexpar.boundingRect.left+(double)convexpar.boundingRect.width*0.5);
double angle = ((180.0/3.14159)*acos (pwidth * distanceInInches/179.3/24.0) );
if(angle != angle) angle = 0.0; // if angle is NaN, set to zero
printf("%f distance in inches\n",distanceInInches);
//printf("%f angle\n",(180.0/3.14159)*acos (pwidth * distanceInInches/415.0/24.0) );
printf("%d BBctrX:%f CMX:%f\n", i, (double)convexpar.boundingRect.left + (double)convexpar.boundingRect.width*0.5, (double)par.center_mass_x);
//printf("%f angle2\n",(((pwidth * distanceInInches)/415.0)/24.0));
//printf("%f center of mass x\n",par.center_mass_x_normalized);
printf("%d %f %f center of mass x\n",i,convexpar.center_mass_x_normalized,par.center_mass_x_normalized);
printf("%d %f %f center of mass y\n",i,convexpar.center_mass_y_normalized,par.center_mass_y_normalized);
printf("%d %f %f rectangle score\n",i,(convexpar.particleArea)/((convexpar.boundingRect.width)*(convexpar.boundingRect.height))*(100),(par.particleArea)/((par.boundingRect.width)*(par.boundingRect.height))*(100));
printf("%d %f fillness ratio\n",i,par.particleArea/convexpar.particleArea);
printf("%d %d %d width and height\n",i,(convexpar.boundingRect.width),(convexpar.boundingRect.height));
printf("%d %f aspect ratio\n",i,((convexpar.boundingRect.width)/(double)(convexpar.boundingRect.height)));
if ((double)(par.center_mass_x)>mwidth)
{
angle=angle*(-1.0);
}
printf("%f true angle\n",angle);
//Wait(1.0);
double aiming_target_offset = 0.0;
//aiming_target_offset = pwidth * angle * (-0.5 / 45.0); numbers are iffy -> NaN
double speed = trackingFeedbackFunction(mwidth + aiming_target_offset - 80.0);
printf("%f aiming_target_offset due to %f degree angle\n", aiming_target_offset, angle);
printf("%f x offset \n",mwidth + aiming_target_offset - 80.0);
printf("%f speed \n", speed);
foundAnything = true;
if (use_alternate_score == false){
particle_score = convexpar.center_mass_y;
}
else{
particle_score = 2.0*fabs((double)convexpar.center_mass_y - 60.0) + fabs((double)convexpar.center_mass_x - 80.0);
}
// keep track of the *lowest* score
if (best_score > particle_score)
{
best_score = particle_score;
best_speed = speed;
}
}
if(foundAnything == false) {
myRobot->TankDrive(0.0, 0.0);
}
else
{
myRobot->TankDrive(-best_speed,best_speed);
}
delete img;
delete tapePixels;
delete pars;
delete convexHullBinaryImage;
delete convexpars;
//imaqDispose(convexHull);
if (foundAnything && best_speed == 0.0){
return true;
}
else
{
return false;
}
}
