inline void processTaskFunc(UINT32 hotGoalPtr...)
{
bool hotGoal = (bool *) hotGoalPtr;
Scores *scores;
TargetReport target;
int verticalTargets[MAX_PARTICLES];
int horizontalTargets[MAX_PARTICLES];
int verticalTargetCount, horizontalTargetCount;
Threshold threshold(0, 255, 0, 255, 220, 255); //HSV threshold criteria, ranges are in that order ie. Hue is 60-100
ParticleFilterCriteria2 criteria[] = {
{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
}; //Particle filter criteria, used to filter out small particles
//AxisCamera &camera = AxisCamera::GetInstance(); //To use the Axis camera uncomment this line
/**
* Do the image capture with the camera and apply the algorithm described above. This
* sample will either get images from the camera or from an image file stored in the top
* level directory in the flash memory on the cRIO. The file name in this case is "testImage.jpg"
*/
ColorImage *image;
image = new RGBImage("/testImage.jpg"); // get the sample image from the cRIO flash
//image = camera.GetImage(); //To get the images from the camera comment the line above and uncomment this one
BinaryImage *thresholdImage = image->ThresholdHSV(threshold); // get just the green target pixels
//thresholdImage->Write("/threshold.bmp");
BinaryImage *filteredImage = thresholdImage->ParticleFilter(criteria, 1); //Remove small particles
//filteredImage->Write("Filtered.bmp");
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports(); //get a particle analysis report for each particle
verticalTargetCount = horizontalTargetCount = 0;
//Iterate through each particle, scoring it and determining whether it is a target or not
if(reports->size() > 0)
{
scores = new Scores[reports->size()];
for (unsigned int i = 0; i < MAX_PARTICLES && i < reports->size(); i++) {
ParticleAnalysisReport *report = &(reports->at(i));
//Score each particle on rectangularity and aspect ratio
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioVertical = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioHorizontal = scoreAspectRatio(filteredImage, report, false);
//Check if the particle is a horizontal target, if not, check if it's a vertical target
if(scoreCompare(scores[i], false))
{
printf("particle: %d is a Horizontal Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
horizontalTargets[horizontalTargetCount++] = i; //Add particle to target array and increment count
} else if (scoreCompare(scores[i], true)) {
printf("particle: %d is a Vertical Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
verticalTargets[verticalTargetCount++] = i; //Add particle to target array and increment count
} else {
printf("particle: %d is not a Target centerX: %d centerY: %d \n", i, report->center_mass_x, report->center_mass_y);
}
printf("Scores rect: %f ARvert: %f \n", scores[i].rectangularity, scores[i].aspectRatioVertical);
printf("ARhoriz: %f \n", scores[i].aspectRatioHorizontal);
}
//Zero out scores and set verticalIndex to first target in case there are no horizontal targets
target.totalScore = target.leftScore = target.rightScore = target.tapeWidthScore = target.verticalScore = 0;
target.verticalIndex = verticalTargets[0];
for (int i = 0; i < verticalTargetCount; i++)
{
ParticleAnalysisReport *verticalReport = &(reports->at(verticalTargets[i]));
for (int j = 0; j < horizontalTargetCount; j++)
{
ParticleAnalysisReport *horizontalReport = &(reports->at(horizontalTargets[j]));
double horizWidth, horizHeight, vertWidth, leftScore, rightScore, tapeWidthScore, verticalScore, total;
//Measure equivalent rectangle sides for use in score calculation
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_LONG_SIDE, &horizWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), verticalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &vertWidth);
imaqMeasureParticle(filteredImage->GetImaqImage(), horizontalReport->particleIndex, 0, IMAQ_MT_EQUIVALENT_RECT_SHORT_SIDE, &horizHeight);
//Determine if the horizontal target is in the expected location to the left of the vertical target
leftScore = ratioToScore(1.2*(verticalReport->boundingRect.left - horizontalReport->center_mass_x)/horizWidth);
//Determine if the horizontal target is in the expected location to the right of the vertical target
rightScore = ratioToScore(1.2*(horizontalReport->center_mass_x - verticalReport->boundingRect.left - verticalReport->boundingRect.width)/horizWidth);
//Determine if the width of the tape on the two targets appears to be the same
tapeWidthScore = ratioToScore(vertWidth/horizHeight);
//Determine if the vertical location of the horizontal target appears to be correct
verticalScore = ratioToScore(1-(verticalReport->boundingRect.top - horizontalReport->center_mass_y)/(4*horizHeight));
total = leftScore > rightScore ? leftScore:rightScore;
total += tapeWidthScore + verticalScore;
//If the target is the best detected so far store the information about it
if(total > target.totalScore)
{
target.horizontalIndex = horizontalTargets[j];
target.verticalIndex = verticalTargets[i];
target.totalScore = total;
target.leftScore = leftScore;
target.rightScore = rightScore;
target.tapeWidthScore = tapeWidthScore;
target.verticalScore = verticalScore;
}
}
//Determine if the best target is a Hot target
target.Hot = hotOrNot(target);
}
if(verticalTargetCount > 0)
{
//Information about the target is contained in the "target" structure
//To get measurement information such as sizes or locations use the
//horizontal or vertical index to get the particle report as shown below
ParticleAnalysisReport *distanceReport = &(reports->at(target.verticalIndex));
double distance = computeDistance(filteredImage, distanceReport);
if(target.Hot)
{
printf("Hot target located \n");
printf("Distance: %f \n", distance);
hotGoal = true;
} else {
printf("No hot target present \n");
printf("Distance: %f \n", distance);
hotGoal = false;
}
}
}
// be sure to delete images after using them
delete filteredImage;
delete thresholdImage;
delete image;
//delete allocated reports and Scores objects also
delete scores;
delete reports;
}
int VisionControl::ProcessImage()
{
if(m_camera->IsFreshImage())
{
ColorImage *image = NULL;
m_camera->GetImage(image);
Threshold threshold(60,100,90,255,20,255);
ParticleFilterCriteria2 criteria[] = {IMAQ_MT_AREA,AREA_MINIMUM,65535,false,false};
BinaryImage *thresholdImage = image->ThresholdHSV(threshold);
BinaryImage *convexHullImage = thresholdImage->ConvexHull(false);
BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 1);
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports();
for (unsigned int i = 0; i < reports->size(); i++)
{
ParticleAnalysisReport *report = &(reports->at(i));
// first, determine if this is a particle we are looking at.
if(report->boundingRect.left > 320/2 || report->boundingRect.left + report->boundingRect.width < 320/2)
{
// particle is not lined up with center of vision
// note: may not want to do this for autonomous!
continue;
}
double aspectRatio = AspectRatio(filteredImage, report);
double difference3ptGoal = fabs(1-(aspectRatio / ((54.f+4+4)/(12.f+4+4))));
double difference2ptGoal = fabs(1-(aspectRatio / ((54.f+4+4)/(21.f+4+4))));
if(difference2ptGoal < 0.25 && difference2ptGoal < difference3ptGoal)
{
m_elevation = 0;
m_distance = ComputeDistance(thresholdImage, report, true);
m_relativeAzimuth = 0;
}
else if(difference3ptGoal < 0.25 && difference3ptGoal < difference2ptGoal)
{
m_elevation = 0;
m_distance = ComputeDistance(thresholdImage, report, false);
m_relativeAzimuth = 0;
}
else
{
// didn't sufficiently match a target!
}
}
/*
Scores *scores = new Scores[reports->size()];
//Iterate through each particle, scoring it and determining whether it is a target or not
for (unsigned i = 0; i < reports->size(); i++)
{
ParticleAnalysisReport *report = &(reports->at(i));
scores[i].rectangularity = ScoreRectangularity(report);
scores[i].aspectRatioOuter = ScoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioInner = ScoreAspectRatio(filteredImage, report, false);
scores[i].xEdge = ScoreXEdge(thresholdImage, report);
scores[i].yEdge = ScoreYEdge(thresholdImage, report);
if(ScoreCompare(scores[i], false))
{
printf("particle: %d is a High Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Distance: %f \n", ComputeDistance(thresholdImage, report, false));
}
else if (ScoreCompare(scores[i], true))
{
printf("particle: %d is a Middle Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Distance: %f \n", ComputeDistance(thresholdImage, report, true));
}
else
{
printf("particle: %d is not a goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
}
printf("rect: %f ARinner: %f \n", scores[i].rectangularity, scores[i].aspectRatioInner);
printf("ARouter: %f xEdge: %f yEdge: %f \n", scores[i].aspectRatioOuter, scores[i].xEdge, scores[i].yEdge);
}
*/
delete image;
delete thresholdImage;
delete convexHullImage;
delete filteredImage;
delete reports;
//delete scores;
return 1;
}
return 0;
}
void VisionSubsystem::ProcessImage() {
printf("Vision: Starting Vision Subsystem\n");
///////////////
// Seting Up //
///////////////
targetVisable[TOP_TARGET] = false;
targetVisable[MIDDLE_TARGET] = false;
targetVisable[BOTTOM_TARGET] = false;
targetDistances[TOP_TARGET] = 0.0;
targetDistances[MIDDLE_TARGET] = 0.0;
targetDistances[BOTTOM_TARGET] = 0.0;
targetPositionX[TOP_TARGET] = 0.0;
targetPositionY[TOP_TARGET] = 0.0;
targetPositionX[MIDDLE_TARGET] = 0.0;
targetPositionY[MIDDLE_TARGET] = 0.0;
targetPositionX[BOTTOM_TARGET] = 0.0;
targetPositionY[BOTTOM_TARGET] = 0.0;
/*
* This creates a object with the needed values for the processing
* the image later on, for a certain color.
*/
printf("Vision: Setting the clor threshold values\n");
Threshold threshold(THRESHOLD_HUE_MIN,
THRESHOLD_HUE_MAX,
THRESHOLD_SATURATION_MIN,
THRESHOLD_SATURATION_MAX,
THRESHOLD_VALUE_MIN,
THRESHOLD_VALUE_MAX);
ParticleFilterCriteria2 criteria[] = {
{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}
};
/*
* This is the function that sets up the axis camera to get images.
* To use the camera on the second port on the cRIO, uncommet the second line below
* with "192.168.0.90" in it.
*/
printf("Vision: Setting camera IP to 10.30.81.12/n");
AxisCamera &camera = AxisCamera::GetInstance("10.30.81.12");
//AxisCamera &camera = AxisCamera::GetInstance("192.168.0.90"); //
// This creates a Color image, then on the second line it fills it with the image from the camera.
printf("Vision: Creating ColorImage object image\n");
ColorImage *image;
printf("Vision: Getting the Image from the camera \n");
image = camera.GetImage();
//////////////////////////////
// Image processing section //
//////////////////////////////
//Process the image with the threshold values
printf("Vision: Filtering the image with threshold values into object thresholdImage\n");
BinaryImage *thesholdImage = image->ThresholdHSV(threshold);
//This will fill shape that is complete and fill in the inside of siad shape.
printf("Vision: Filling in the convex shapes into the object of convexHullImage\n");
BinaryImage *convexHullImage = thesholdImage->ConvexHull(false);
//This will get rid of random particles in the image that are notconcentrated enougth.
printf("Vision: Filtering image for the unwanted random particles");
BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 1);
//This creates a report that will be used later to idenify targets
printf("Vision: Creating the report of the filtered Image\n");
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports();
//This creates a data stucture that is used to score objects.
scores = new Scores[reports->size()];
for (unsigned i = 0; i < reports->size(); i++) {
ParticleAnalysisReport *report = &(reports->at(i));
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioOuter = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioInner = scoreAspectRatio(filteredImage, report, false);
scores[i].xEdge = scoreXEdge(thesholdImage, report);
scores[i].yEdge = scoreYEdge(thesholdImage, report);
if(scoreCompare(scores[i], false)) {
printf("Vision: particle: %d is High Goal centerX %f centerY: %f \n", i , report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Vision: Distance: %f \n", computeDistance(thesholdImage, report, false));
targetPositionX[TOP_TARGET] = report->center_mass_x;
targetPositionY[TOP_TARGET] = report->center_mass_y;
targetDistances[TOP_TARGET] = computeDistance(thesholdImage, report, false);
targetVisable[TOP_TARGET] = true;
targetPositionX[TOP_TARGET] = targetPosition(TOP_TARGET, true);
targetPositionY[TOP_TARGET] = targetPosition(TOP_TARGET, false);
} else if (scoreCompare(scores[i], true)){
printf("Vision: particle: %d is Middle Goal centerX %f centerY: %f \n", i , report->center_mass_x_normalized, report->center_mass_y_normalized);
printf("Vision: Distance: %f \n", computeDistance(thesholdImage, report, true));
targetPositionX[MIDDLE_TARGET] = report->center_mass_x;
targetPositionY[MIDDLE_TARGET] = report->center_mass_y;
targetDistances[MIDDLE_TARGET] = computeDistance(thesholdImage, report, true);
targetVisable[MIDDLE_TARGET] = true;
targetPositionX[MIDDLE_TARGET] = targetPosition(MIDDLE_TARGET, true);
targetPositionY[MIDDLE_TARGET] = targetPosition(MIDDLE_TARGET, false);
} else {
printf("Vision: particle %d is not a goal centerX: %f centery: %f \n" , i, report->center_mass_x_normalized, report->center_mass_y_normalized);
}
printf("Vision: rect: %f ARinner: %f \n", scores[i].rectangularity, scores[i].aspectRatioInner);
printf("Vision: ARouter: %f xEdge: %f yEdge: %f \n", scores[i].aspectRatioOuter, scores[i].xEdge, scores[i].yEdge);
}
//printf("\n");
printf("Vision: Deleting the object filtered image\n");
delete filteredImage;
printf("Vision: Deleting the objectconvexHullImage\n");
delete convexHullImage;
printf("Vision: Deleting the object thresholdimage\n");
delete thesholdImage;
printf("Vision: Deleting the object image");
delete image;
delete scores;
delete reports;
printf("Vision: Done\n");
}
void mtdCameraCode(void)
{
Threshold threshold(0, 255, 0, 255, 221, 255);
ParticleFilterCriteria2 criteria[] = {{IMAQ_MT_AREA, AREA_MINIMUM, 65535, false, false}};
AxisCamera &camera = AxisCamera::GetInstance("10.26.3.11");
camera.WriteResolution(AxisCamera::kResolution_320x240);
camera.WriteCompression(20);
camera.WriteBrightness(50);
//SmartDashboard::PutNumber("Test", 3);
if(timerCamera.Get() > 0.1)
{
ColorImage *image;
//image = new RGBImage("/HybridLine_DoubleGreenBK3.jpg"); // get the sample image from the cRIO flash
image = camera.GetImage();
//camera.GetImage(image); //To get the images from the camera comment the line above and uncomment this one
//Wait(.1);
//SmartDashboard::PutNumber("Test", 4);
BinaryImage *thresholdImage = image->ThresholdHSV(threshold); // get just the green target pixels
// thresholdImage->Write("/threshold.bmp");
//SmartDashboard::PutNumber("Test", 5);
BinaryImage *convexHullImage = thresholdImage->ConvexHull(false); // fill in partial and full rectangles
// convexHullImage->Write("ConvexHull.bmp");
//SmartDashboard::PutNumber("Test", 6);
BinaryImage *filteredImage = convexHullImage->ParticleFilter(criteria, 1); //Remove small particles
// filteredImage->Write("/Filtered.bmp");
//SmartDashboard::PutNumber("Test", 7);
vector<ParticleAnalysisReport> *reports = filteredImage->GetOrderedParticleAnalysisReports(); //get a particle analysis report for each particle
//SmartDashboard::PutNumber("Test", 8);
int size = reports->size();
scores = new Scores[size];
//SmartDashboard::PutNumber("Test", 9);
//Iterate through each particle, scoring it and determining whether it is a target or not
for (unsigned i = 0; i < reports->size(); i++)
{
//SmartDashboard::PutNumber("Test", 10);
ParticleAnalysisReport *report = &(reports->at(i));
scores[i].rectangularity = scoreRectangularity(report);
scores[i].aspectRatioOuter = scoreAspectRatio(filteredImage, report, true);
scores[i].aspectRatioInner = scoreAspectRatio(filteredImage, report, false);
scores[i].xEdge = scoreXEdge(thresholdImage, report);
scores[i].yEdge = scoreYEdge(thresholdImage, report);
if(scoreCompare(scores[i], false))
{
//We hit this!! Note to self: changethe below printf statement
//To use SmartDashboard::PutString so wecan seevalues.
//printf("particle: %d is a High Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
//string particle = ("particle: %d is a High Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
SmartDashboard::PutNumber("CenterX", report->center_mass_x);
SmartDashboard::PutNumber("CenterY", report->center_mass_y);
SmartDashboard::PutNumber("Area", report->particleArea);
SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutNumber("size", reports->size());
SmartDashboard::PutNumber("height", report->boundingRect.height);
SmartDashboard::PutNumber("Quality", report->particleQuality);
//SmartDashboard::PutNumber("Test",computeDistance(thresholdImage, report, false));
//SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutString("high goal detected", "asdf");
}
else if (scoreCompare(scores[i], true))
{
printf("particle: %d is a Middle Goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
SmartDashboard::PutNumber("Test", computeDistance(thresholdImage, report, true));
SmartDashboard::PutNumber("CenterX", report->center_mass_x);
SmartDashboard::PutNumber("CenterY", report->center_mass_y);
SmartDashboard::PutNumber("height", report->boundingRect.height);
SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutString("middle goal detected", "adsf");
}
else
{
printf("particle: %d is not a goal centerX: %f centerY: %f \n", i, report->center_mass_x_normalized, report->center_mass_y_normalized);
SmartDashboard::PutNumber("CenterX", report->center_mass_x);
SmartDashboard::PutNumber("CenterY", report->center_mass_y);
SmartDashboard::PutNumber("height", report->boundingRect.height);
SmartDashboard::PutNumber("Distance",computeDistance(thresholdImage,report, false));
SmartDashboard::PutString("we areinelse", "else");
}
if(report->center_mass_x < 85.00)
{
SmartDashboard::PutString("Pausing", "paused");
//image->Write("C:\\testimg.bmp");
//Wait(10);
}
printf("rect: %f ARinner: %f \n", scores[i].rectangularity, scores[i].aspectRatioInner);
printf("ARouter: %f xEdge: %f yEdge: %f \n", scores[i].aspectRatioOuter, scores[i].xEdge, scores[i].yEdge);
}
printf("\n");
// be sure to delete images after using them
delete filteredImage;
delete convexHullImage;
delete thresholdImage;
delete image;
//delete allocated reports and Scores objects also
delete scores;
delete reports;
}
timerCamera.Reset();
}