C++ BinaryImage::GetOrderedParticleAnalysisReports方法代码示例

Camera* Camera::calculate(void)
{
	AxisCamera &cam = AxisCamera::GetInstance("10.8.54.11");
	if (!cam.IsFreshImage())
				return this;
			
	take = false;
			
	int i = cam.GetImage(&ci);
	if (i) cerr << "image worked" << endl;
	else cerr << "image didn't work" << endl;
			
	//ci.Write("/colorimage.jpg");
		
	BinaryImage *bi = ci.ThresholdHSL(200, 55,
	                                  0, 255,
	                                  0, 200);
	//BinaryImage *bi = ci.ThresholdHSL(Constants::hueMin, Constants::hueMax, 
	//										  Constants::satMin, Constants::satMax, 
	//									      Constants::lumMin, Constants::lumMax);
	//BinaryImage *biggerObjects = bi->RemoveSmallObjects(false, 2);
	vector <ParticleAnalysisReport> *particleAnalysisList = bi->GetOrderedParticleAnalysisReports();
	Rect biggestRectangle = particleAnalysisList->at(0).boundingRect;
	
	int x = biggestRectangle.left + biggestRectangle.width/2;
	int y = biggestRectangle.top + biggestRectangle.height/2;
	
	cerr << "T: (" << x << "," << y << ")" << endl;
	
	bi->Write("/image.bmp");
	//delete bi;
	//delete particleAnalysisList;
	return this;
}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void)
	{
		HSLImage *Himage;
		Threshold targetThreshold(247, 255, 60, 140, 10, 50);
		BinaryImage *matchingPixels;
		vector<ParticleAnalysisReport> *pReport;
		
		//myRobot->SetSafetyEnabled(true);
		Saftey->SetEnabled(false);
		AxisCamera &mycam = AxisCamera::GetInstance("10.15.10.11");
		
		mycam.WriteResolution(AxisCamera::kResolution_640x480);
		mycam.WriteCompression(20);
		mycam.WriteBrightness(25);
		Wait(3.0);
         
		dsLCD = DriverStationLCD::GetInstance();
		dsLCD->Clear();
		
		float X[2];
		float Y[2];
		float Z[2];
		
		while(IsOperatorControl())
		{
			X[1] = Stick1->GetX();
			X[2] = Stick2->GetX();
			Y[1] = Stick1->GetY();
			Y[2] = Stick2->GetY();
			Z[1] = Stick1->GetZ();
			Z[2] = Stick2->GetZ();
			
			Jaguar1->Set(Y[1]);
			Jaguar2->Set(Y[2]);
			
			Wait(0.005);
			if (mycam.IsFreshImage())
						{
							Himage = mycam.GetImage();
							
							matchingPixels = Himage->ThresholdHSL(targetThreshold);
							pReport = matchingPixels->GetOrderedParticleAnalysisReports();
							
							for (unsigned int i = 0; i < pReport->size(); i++)
							{
								printf("Index: %d X Center: %d Y Center: %d \n", i, (*pReport)[i].center_mass_x, (*pReport)[i].center_mass_y);
								
							}
							
							delete Himage;
							delete matchingPixels;
							delete pReport;
						}
			
		}
		
			
			//myRobot->ArcadeDrive(stick); // drive with arcade style (use right stick)
			//Wait(0.005);				// wait for a motor update time
	}

	double CameraHandler::GetDistanceToBall ()
    {
		unsigned x;

		int ballNum;
		double objAngle;
		double largestArea;
		double sizeRatio;

		BinaryImage* binImg;
		vector<ParticleAnalysisReport>* particles;

		// ----- Get Image -----
		camera->GetImage(img);

		// ----- Filter out background -----
		if (m_ds->GetAlliance() == DriverStation::kBlue)
					binImg = img->ThresholdHSV(160, 184, 120, 255, 14, 233);
				else
					binImg = img->ThresholdRGB(88, 255, 0, 74, 0, 31);

		// Make picture clear
		frcMorphology(binImg->GetImaqImage(),binImg->GetImaqImage(),IMAQ_PCLOSE);
		frcMorphology(binImg->GetImaqImage(),binImg->GetImaqImage(),IMAQ_ERODE);

		particles = binImg->GetOrderedParticleAnalysisReports();

		SmartDashboard::PutNumber("DEBUG Particle size: ", particles->size());

		if (particles->size() > 0 && particles->size() < 30)
		{
			sort(particles->begin(),particles->end(),particleSort);

			//Find ball

			largestArea = 25.0;
			ballNum = -1;

			for (x = 0; ((x < particles->size()) && x < 5); x++)
			{
				sizeRatio = (double)(*particles)[x].boundingRect.height/(*particles)[x].boundingRect.width;

				if (((*particles)[x].particleArea > largestArea) && (sizeRatio > 0.75 && sizeRatio < 1.25))
				{
					largestArea = (*particles)[x].particleArea;
					ballNum = x;
				}
			}
		}

		if (ballNum >= 0)
		{
            objAngle = 0.5*((*particles)[ballNum].boundingRect.width)*(CAMERA_ANGLE/(*particles)[ballNum].imageWidth);

            return 1/tan(objAngle);
		}
		else
			return -1.0;
    }

	CameraHandler::state_t CameraHandler::getHotGoal ()
	{
		unsigned x;

		int largestWidth;		// Index of Particle
		int largestHeight;		// Index of Particle
		int largestWidthVal;	// Actual Width
		int largestHeightVal;	// Actual Height

		BinaryImage* binImg;
		ColorImage* colImg;
		vector<ParticleAnalysisReport>* particles;

		// Get Camera Image
		camera->GetImage(img);
		img->Write("bobnormal.bmp");
		// Filter out Background
		binImg = img->ThresholdHSL(103, 156, 252, 255, 33, 109);
		binImg->Write("bobbin.bmp");

		// Make picture clear
		frcMorphology(binImg->GetImaqImage(),binImg->GetImaqImage(),IMAQ_PCLOSE);
		frcMorphology(binImg->GetImaqImage(),binImg->GetImaqImage(),IMAQ_DILATE);

		// Get Particle Analysis
		particles = binImg->GetOrderedParticleAnalysisReports();
		SmartDashboard::PutNumber("Num of Particles: ",particles->size());
		if (particles->size() == 1) {
			// Find Only One Particle
			return CameraHandler::kNone;
		} else if (particles->size() > 0 && particles->size() < 30) {
			// Sort by size
			sort(particles->begin(), particles->end(), particleSort);

			// Initialize
			largestWidth = 0;
			largestHeight = 0;
			largestWidthVal = 0;
			largestHeightVal = 0;

			for (x=0; (x < 3 &&  x < particles->size()); x++) {
				// Find tallest
				if ((*particles)[x].boundingRect.height > largestHeightVal) {
					largestHeight = x;
					largestHeightVal = (*particles)[x].boundingRect.height;
				}

				// Find Fattest
				if ((*particles)[x].boundingRect.width > largestWidthVal) {
					largestWidth = x;
					largestWidthVal = (*particles)[x].boundingRect.width;
				}
			}

			if ((*particles)[largestWidth].center_mass_x < (*particles)[largestHeight].center_mass_x) {
				return kLeft;
			}
			else if ((*particles)[largestWidth].center_mass_x > (*particles)[largestHeight].center_mass_x) {
				return kRight;
			}
			else {
				return kNone;
			}

		} else {
			// Find Too Many Particles or None
			return kError;
		}
	}

	double CameraHandler::getCenter()
	{
		unsigned x,y;

		int largestWidth;
		int largestHeight[2];
		int largestWidthVal, largestHeightVal;

		BinaryImage* binImg;
		vector<ParticleAnalysisReport>* particles;

		//m_dsLCD->Printf(DriverStationLCD::kUser_Line1, 1, "Test");
		//m_dsLCD->UpdateLCD();
		//Get new camera image
		camera->GetImage(img);

		//img.Write("bob2.jpg");  //Cannot work with non-RGB images

		//int Wid = img->GetWidth();  //Sanity Check: Check width of image
		//Prints width of image from camera
		//m_dsLCD->Printf(DriverStationLCD::kUser_Line1, 1,"Width of Image: %d",Wid);

		//Perform HSLThreshold to pull out only blue LED reflection of targets into BinaryImage
		//BinaryImage* binImg = img->ThresholdHSL(202, 255, 55, 255, 55, 129);      //RED LED WORKS TERRRIBLY!!!!
		binImg = img->ThresholdHSL(52, 255, 71, 188, 76, 219);      //RED LED WORKS TERRRIBLY!!!!
		//BinaryImage* binImg = img->ThresholdHSL(57, 255, 79, 255, 51, 255);  //BLUE LED
		//BinaryImage* binImg = img->ThresholdHSL(159, 255, 0, 255, 71, 255);  //RED LED
		//Perform Morphology on image to remove noise/unwanted particles.  Also fills in incomplete particles.
		frcMorphology(binImg->GetImaqImage(),binImg->GetImaqImage(),IMAQ_PCLOSE);

		frcMorphology(binImg->GetImaqImage(),binImg->GetImaqImage(),IMAQ_DILATE);
		//Perform particle analysis on BinaryImage, creates vector with all particles found
		particles = binImg->GetOrderedParticleAnalysisReports();

		printf("Particles found: %d",(int)particles->size());

		//Print numbers of particles found to driver station
		//m_dsLCD->Printf(DriverStationLCD::kUser_Line4, 1, "# Parts:%d    ",particles->size());
		//m_dsLCD->UpdateLCD();
		if(particles->size() > 1 || particles->size() < 30)
		{// Sort by size
			sort(particles->begin(), particles->end(), particleSort);

			// Initialize
			y=0;

			largestWidth = 0;
			largestHeight[0] = -1;
			largestHeight[1] = -1;
			largestHeightVal = 0;
			largestWidthVal = 0;
			largestHeightVal = 10;

			for (x=0; (x < 3 &&  x < particles->size()); x++) {
				// Find tallest
				if ((*particles)[x].boundingRect.height > largestHeightVal) {
					largestHeight[y] = x;
					largestHeightVal = (*particles)[x].boundingRect.height;
					y++;
				}

				// Find Fattest
				if ((*particles)[x].boundingRect.width > largestWidthVal) {
					largestWidth = x;
					largestWidthVal = (*particles)[x].boundingRect.width;
				}
			}

			// Detect Which Is Hot And Return Normalized Value of X (-1.0 - 1.0)
			if (fabs((*particles)[largestHeight[0]].center_mass_x - (*particles)[largestWidth].center_mass_x) < fabs((*particles)[largestHeight[1]].center_mass_x - (*particles)[largestWidth].center_mass_x)) {
				return (*particles)[largestHeight[0]].center_mass_x_normalized;
			} else {
				return (*particles)[largestHeight[1]].center_mass_x_normalized;
			}
		}
		else{
			return 0;
		}
	}

	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);
//.........这里部分代码省略.........

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);
					}
//.........这里部分代码省略.........

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);
			
//.........这里部分代码省略.........

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 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);
//.........这里部分代码省略.........

int Vision::particleAnalysis()
{
	printf("Vision::particleAnalysis\n");
	// Get an instance of the Axis Camera
	AxisCamera &camera = AxisCamera::GetInstance(CAMERA_IP);
	
	TracePrint(TRACE_VISION, "check fresh\n");
	// check if there is a new image
	if (camera.IsFreshImage())
	{
		TracePrint(TRACE_VISION, "get image\n");
		// Get the Image
		ColorImage *colorImage = camera.GetImage();
		TracePrint(TRACE_VISION, "colorImage is %s\n", colorImage ? "not null" : "null");
		BinaryImage *binImage = colorImage->ThresholdRGB(COLOR_THRESHOLD);
		TracePrint(TRACE_VISION, "binImage is %s\n", binImage ? "not null" : "null");
		
		if(!colorImage || !binImage)
			return -1;
		
		TracePrint(TRACE_VISION, "Getting Particle Analysis Report.");
		if (particles)
		{
			delete particles;
		}
		particles = binImage->GetOrderedParticleAnalysisReports();
		if(!particles)
		{
			TracePrint(TRACE_VISION, "NULL PARTICLES");
			return -1;
		}
		if(!particles->empty())
		{
			TracePrint(TRACE_VISION, "Stepping through particle report to remove particles with area too small (total %d particles).\n", particles->size());

			int maxHeight = 0, maxIndex = -1;

			// Step through the particles and elimate any that are too small
			for (int i = 0; i<(int)particles->size(); i++) 
			{
				TracePrint(TRACE_VISION, "Particle %d:\n", i);
				TracePrint(TRACE_VISION, "area %.4lf\n", particles->at(i).particleArea);
				
				if(particles->at(i).particleArea<MIN_PARTICLE_AREA)
				{
					TracePrint(TRACE_VISION, "Particle too small, erasing... ");
					
					// Erase the current particle from view
					particles->erase(particles->begin()+i);
					
					// Because erasing an element actually adjusts all elements
					// after the current one, we need to bump <tt>i</tt> down one
					i--;
					TracePrint(TRACE_VISION, "... erased.\n");
				}
				else
				{
					TracePrint(TRACE_VISION, "Checking height...");
					if (particles->at(i).center_mass_y>maxHeight) {
						maxIndex = i;
						maxHeight = particles->at(i).center_mass_y;
					}
					TracePrint(TRACE_VISION, "... checked\n");
				}
			}
			if (maxIndex!=-1)
				targetParticle = particles->at(maxIndex);
		}
		else 
		{
			targetParticle.center_mass_x_normalized = 0;
		}
		
		if (colorImage)
		{
			printDebug("Deleting colorImages.");
			delete colorImage;
		}
		if (binImage)
		{
			printDebug("Deleting binImages.");
			delete binImage;
		}
		
		printDebug("Done Deleting Images.");
	}
	
	printDebug("Done processing image.");
	
	return particles->size();
}