BinaryImage *BinaryImage::ConvexHull(bool connectivity8)
{
BinaryImage *result = new BinaryImage();
int success = imaqConvexHull(result->GetImaqImage(), m_imaqImage, connectivity8);
wpi_setImaqErrorWithContext(success, "Error in convex hull operation");
return result;
}
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;
}
}
bool Target::FindParticles()
{
printf("Target::FindParticles\n");
long then = (long) GetFPGATime();
// extract the blue plane
if (!imaqExtractColorPlanes(m_cameraImage.GetImaqImage(), IMAQ_RGB,
NULL, NULL, m_monoImage.GetImaqImage()))
{
printf("%s: imaqExtractColorPlanes FAILED\n", __FUNCTION__);
return false;
}
// select interesting particles
if (!imaqThreshold(m_threshold.GetImaqImage(), m_monoImage.GetImaqImage(),
/*rangeMin*/ THRESHOLD, /*rangeMax*/ 255, /*useNewValue*/ 1, /*newValue */ 1))
{
printf("%s: imaqThreshold FAILED\n", __FUNCTION__);
return false;
}
if (!imaqConvexHull(m_convexHull.GetImaqImage(), m_threshold.GetImaqImage(),
/*connectivity8*/ 1))
{
printf("%s: imaqConvexHull FAILED\n", __FUNCTION__);
return false;
}
if (!imaqSizeFilter(m_filtered.GetImaqImage(), m_convexHull.GetImaqImage(),
/*connectivity8*/ 1, /*erosions*/ 2, /*keepSize*/ IMAQ_KEEP_LARGE,
/*structuringElement*/ NULL))
{
printf("%s: imaqSizeFilter FAILED\n", __FUNCTION__);
return false;
}
int particleCount = 0;
if (!imaqCountParticles(m_filtered.GetImaqImage(), 1, &particleCount))
{
printf("%s: imaqCountParticles FAILED\n", __FUNCTION__);
return false;
}
// select the four largest particles (insertion sort)
// for now, keep track of only the particle number (index) and size
memset((void *)m_particles, 0, sizeof m_particles);
for (int i = 0; i < particleCount; i++) {
double size;
if (!imaqMeasureParticle(m_filtered.GetImaqImage(), i, FALSE,
IMAQ_MT_PARTICLE_AND_HOLES_AREA, &size))
{
printf("%s: imaqMeasureParticle %d FAILED\n", __FUNCTION__, i);
break;
}
for (int j = 0; j < 4; j++) {
if (size > m_particles[j].size) {
for (int k = 3; k > j; k--) {
m_particles[k].index = m_particles[k-1].index;
m_particles[k].size = m_particles[k-1].size;
}
m_particles[j].index = i;
m_particles[j].size = size;
break;
}
}
}
// fill in the rest of the measured data
for (m_numParticles = 0;
m_numParticles < 4 && m_particles[m_numParticles].size > 0;
m_numParticles++)
{
Particle* p = &m_particles[m_numParticles];
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_CENTER_OF_MASS_X, &(p->xCenter));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_CENTER_OF_MASS_Y, &(p->yCenter));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_LEFT, &(p->leftBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_RIGHT, &(p->rightBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_TOP, &(p->topBound));
imaqMeasureParticle(m_filtered.GetImaqImage(), p->index, FALSE,
IMAQ_MT_BOUNDING_RECT_BOTTOM, &(p->bottomBound));
// calculate height/width from bounding box
p->height = p->bottomBound - p->topBound;
p->width = p->rightBound - p->leftBound;
}
long now = (long) GetFPGATime();
printf("%s: particle detection took %ld microseconds\n", __FUNCTION__, (now - then));
printf("%s: found %d particles\n", __FUNCTION__, particleCount);
printf("%s: returning %d particles\n", __FUNCTION__, m_numParticles);
for (int i = 0; i < m_numParticles; i++) {
Particle *p = &m_particles[i];
printf(" particle %d index %d top %g bottom %g left %g right %g size %g x %g y %g\n",
i, p->index, p->topBound, p->bottomBound, p->leftBound, p->rightBound,
p->size, p->xCenter, p->yCenter);
}
return true;
}
void OperatorControl(void)
{
myRobot.SetSafetyEnabled(false);
AxisCamera &camera = AxisCamera::GetInstance("10.28.53.11");
camera.WriteResolution(AxisCamera::kResolution_320x240);
camera.WriteRotation(AxisCamera::kRotation_180);//Flip image upside-down.
camera.WriteCompression(80);//Compresses the image(?)
camera.WriteBrightness(50);//Sets the brightness to 80 on a scale from 0-100
DriverStationLCD *screen = DriverStationLCD::GetInstance();
int count = 0;
while(IsOperatorControl())
{
screen->UpdateLCD();
count++;
HSLImage* imgpointer; //declares a new hue saturation lum image
imgpointer = camera.GetImage(); //grabs an image to initialize that image
//imaqCreateImage(IMAQ_IMAGE_U8,)
//imaqCast(NULL, imgpointer, IMAQ_IMAGE_U8, NULL, -1);
BinaryImage* binImg = NULL; //declares a new binary image
//ThresholdHSL changes our regular image into a binary image.
/*hueLow Low value for hue
hueHigh High value for hue
saturationLow Low value for saturation
saturationHigh High value for saturation
luminenceLow Low value for luminence
luminenceHigh High value for luminence
*/
binImg = imgpointer->ThresholdHSL(1, 255, 1, 255, 230, 255);
//Saves the image to a temp directory
binImg->Write("/tmp/thresh.jpg");
//Writes the binary image to disk
imaqWriteFile(binImg->GetImaqImage(), "/tmp/thresh2.jpg", NULL);
delete imgpointer;
Image* Convex = imaqCreateImage(IMAQ_IMAGE_U8, 0);
int returnvalue;
returnvalue = imaqConvexHull(Convex, binImg->GetImaqImage(), TRUE);
// imaqWriteFile(Convex, "/tmp/convex.jpg", NULL);
delete binImg;
// screen->PrintfLine(DriverStationLCD::kUser_Line4,"convex is %d",returnvalue);
screen->UpdateLCD();
float lookuptable[256];
lookuptable[0] = 0;
lookuptable[1] = 65535;
//Converts image to 16 bit, then back to 8 bit.
Image* cast = imaqCreateImage(IMAQ_IMAGE_U16, 0);
imaqCast(cast, Convex, IMAQ_IMAGE_U16, lookuptable, 0);
imaqDispose(Convex);
Image* bitcast = imaqCreateImage(IMAQ_IMAGE_U8, 0);
imaqCast(bitcast, cast, IMAQ_IMAGE_U8, NULL, 0);
imaqDispose(cast);
// screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());//Notifies the user
imaqWriteFile(bitcast, "/tmp/bitcast.jpg", NULL);
//Image* SuperSize = im
//imaqCreateImage(IMAQ_IMAGE_U8, 2240);
//int returnvalue2;
//returnvalue2 = imaqSizeFilter(SuperSize, Convex, TRUE, 1, IMAQ_KEEP_LARGE, NULL);
//screen->UpdateLCD();
//imaqDispose (Convex);
// ROI *roi;
// Rect rectangle;
// rectangle.top = 0;
// rectangle.left = 0;
// rectangle.width = 320;
// rectangle.height = 120;
// imaqAddRectContour(roi,rectangle);
static RectangleDescriptor rectangleDescriptor =
{
30, // minWidth (All values are estimated)
200, // maxWidth
20, // minHeight
200 // maxHeight
};
static CurveOptions curveOptions = //extraction mode specifies what the VI identifies curves in the image. curve options are all the
{
IMAQ_NORMAL_IMAGE, // extractionMode
75, // threshold
IMAQ_NORMAL, // filterSize
25, // minLength
15, // rowStepSize
15, // columnStepSize
10, // maxEndPointGap
FALSE, // onlyClosed
FALSE // subpixelAccuracy
};
static ShapeDetectionOptions shapeOptions =
{
IMAQ_GEOMETRIC_MATCH_ROTATION_INVARIANT, // mode
NULL, // angle ranges
0, // num angle ranges
{75, 125}, // scale range
300 // minMatchScore
};
int matches = 0;
//double highscore = 0;
//int highestindex = -1;
float difference = 0;
float time = 0;
float average = 0;
double y = 0;
//The big important line of code that does important stuff
RectangleMatch* recmatch = imaqDetectRectangles(bitcast, &rectangleDescriptor, &curveOptions, &shapeOptions, NULL, &matches);
// DashboardDataSender *dds;
// dds = new DashboardDataSender;
//screen->PrintfLine(DriverStationLCD::kUser_Line3,"det %d", imaqGetLastError());
//screen->PrintfLine(DriverStationLCD::kUser_Line4,"Matches: %d",matches);//Notifies the user
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Matches: %i",matches);//Notifies the user
screen->UpdateLCD();
/*for(int i = 0; i < matches; i++)
{
//screen->PrintfLine((DriverStationLCD::Line)(i+1),"%i,%i,%i",recmatch[i].height,recmatch[i].width,recmatch[i].score);
if(recmatch[i].score > highscore)
{
highscore = recmatch[i].score;
highestindex = i;
}
screen->PrintfLine(DriverStationLCD::kUser_Line1,"score %i, i %i", recmatch[i].score, i);
screen->UpdateLCD();
}*/
average = (recmatch->corner[1].x + recmatch->corner[3].x)/2;
y = (472/(recmatch->height-4));
screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f",average);
screen->PrintfLine(DriverStationLCD::kUser_Line3," %f",distance(recmatch->height));
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%f,%f",recmatch->height, y);
screen->PrintfLine(DriverStationLCD::kUser_Line6,"%f,%f",ceil(recmatch->corner[1].x),ceil(recmatch->corner[3].x));
screen->UpdateLCD();
//screen->PrintfLine(DriverStationLCD::kUser_Line6,"%d, %d, %d, %d",recmatch->corner[1].x,recmatch->corner[2].x,recmatch->corner[3].x,recmatch->corner[4].x);
//screen->PrintfLine(DriverStationLCD::kUser_Line5,"%d, %d",recmatch->height,recmatch->width);
if(average == 0)
{
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Image Not Found");
}
else if(average < 145)
{
difference = (160 - average);
time = difference*0.0062;
vic.Set(0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Less!");
screen->UpdateLCD();
}
else if (average > 175)
{
difference = (average - 160);
time = difference*0.0062;
vic.Set(-0.1);
Wait(time);
vic.Set(0.0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"More!");
screen->UpdateLCD();
}
else
{
vic.Set(0);
screen->PrintfLine(DriverStationLCD::kUser_Line1,"Perfection!");
screen->UpdateLCD();
}
screen->PrintfLine(DriverStationLCD::kUser_Line2,"Time: %f",time);
screen->PrintfLine(DriverStationLCD::kUser_Line4,"difference %f",difference);
// screen->PrintfLine(DriverStationLCD::kUser_Line2,"Score %i, Index %i", highscore, highestindex);
/* if(matches > 0)
{
int counter = 0;
while (counter < 20)
{
average = (recmatch[highestindex].corner[1].x + recmatch[highestindex].corner[2].x + recmatch[highestindex].corner[3].x +recmatch[highestindex].corner[4].x)/4;
average2 = (recmatch[highestindex].corner[1].y + recmatch[highestindex].corner[2].y + recmatch[highestindex].corner[3].y +recmatch[highestindex].corner[4].y)/4;
// screen->PrintfLine(DriverStationLCD::kUser_Line5,"Center = %f, %f",average, average2);
screen->UpdateLCD();
if (average > 170)
{
vic.Set(-0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Left %f",vic.Get());
//screen->UpdateLCD();
}
else if (average < 150)
{
vic.Set(0.1);
//screen->PrintfLine(DriverStationLCD::kUser_Line1,"Right %f",vic.Get());
//screen->UpdateLCD();
}
else
{
counter = 20;
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}
counter++a;
}
*/
// imaqWriteFile(cast, "/tmp/linedetect.jpg", NULL);
// }
/* else
{
vic.Set(0);
// screen->PrintfLine(DriverStationLCD::kUser_Line6,"Not Mallory! %f",vic.Get());
screen->UpdateLCD();
}*/
imaqDispose(bitcast);
imaqDispose(recmatch);
//imaqDispose(roi);
vic.Set(0);
}
}
