edgerecognition.cc

该文件是包含了机器人足球比赛中的整个系统的代码

// Loctaes points in the image which fall on the side or field lines
void EdgeRecognition::EdgeDetection() {
	numFieldLineDetPoints = 0; // Resets the number of points found so far
	numSideLineDetPoints = 0; // Resets the number of points found so far
	centreCircleNum = 0;  // Resets the number of sets of three points found so far

	int currXFLP=0; // Indicates the current positon of X and Y that we are checking
	while(currXFLP < currentImageWidth_) {
		FindPoint(currXFLP);
		currXFLP = currXFLP + POINT_FIND_INC; // Move x further away from the central beacon and check again for another transition
	}
}

// This method is used NOT for detection of the edge of the field but detection of the lines on the field
// The search will begin at the top of the screen and search down to the bottom
inline void EdgeRecognition::FindPoint(int currX) {
	int greenPositionAbove = -1; // Set both green and white positions as being unknown
	int greenPositionBelow = -1;
	int whitePosition = -1;
	int pointsInColumn = 0;

	unsigned char* pixelPointer = NULL;
	int searchYDown = -1;
	// Calculate the max limit for the x value so that we always stay beneath the horizon and do not calculate any really terrible points due to noise above the barriers
	if (visionData->horizonLine.exists) {
		double m = visionData->horizonLine.m;
		double b = visionData->horizonLine.b;
		if (m > DBL_MAX || m < -DBL_MAX || b > DBL_MAX || b < -DBL_MAX) {
			//cout << "Threw out bad horizon line!!!" << endl << flush;
			return;
		}
		double hY = (m * currX + b) - HORIZON_ADJUSTMENT; // Note the horizon adjustment is to ensure that we don't have issues with the sideline dipping just below the horizon as it can do from a distance
		if (hY < 0) {
			pixelPointer = &(visionData->classified_[currX]);
			searchYDown = 0;
		} else if (hY >= currentImageHeight_) {
			return;
		} else {
			searchYDown = (int)hY;
			pixelPointer = &(visionData->classified_[searchYDown*currentImageWidth_+currX]);
		}
	} else {
		return;
	}

	// This component of the search locates the first white pixel and handles the situation where a green pixel is dicovered
	// first and we don't know whether it should be above or below
	while ((whitePosition == -1)) {
		// Search down while there is still image to be searched
		if (*pixelPointer == c_FIELD_GREEN) {
			greenPositionAbove = searchYDown;
		} else if (*pixelPointer == c_WHITE) {
			whitePosition = searchYDown;  // Updates white so it is always the lowest pixel
		}

		searchYDown += GWG_Y_INC;
		pixelPointer += GWG_Y_INC*currentImageWidth_; // Increment the Y value searching down
		if (searchYDown >= currentImageHeight_) {
			return;
		}
	}

	while((greenPositionBelow == -1)) {
		//Search down while there is still image to be searched
		if (*pixelPointer == c_FIELD_GREEN) {
			greenPositionBelow = searchYDown;
		} else if (*pixelPointer == c_WHITE) {
			whitePosition = searchYDown;  // Updates white so it is always the lowest pixel
		}

		searchYDown += GWG_Y_INC;
		pixelPointer += GWG_Y_INC*currentImageWidth_; // Increment the Y value searching down
		if (searchYDown >= currentImageHeight_) {
			return;
		}
	}

	if (greenPositionAbove == -1) {
		// This means that we found a white green transition but not a green white green transition and as such we have found a sideline
		if (numSideLineDetPoints < MAX_SIDELINE_POINTS) {
			sideLineDetPoints[numSideLineDetPoints].x = currX;
			sideLineDetPoints[numSideLineDetPoints++].y =  whitePosition;
		}
	} else {
		// To enter here we must have passed all three previous conditions i.e. whiteFound, greenAboveFound, greenBelowFound so we have a transition
		// Store the point which has been found

		// Store the transition into the find points array as we have found a point on a line
		if (numFieldLineDetPoints < MAX_SIDELINE_POINTS) {
			fieldLineDetPoints[numFieldLineDetPoints].x = currX;
			fieldLineDetPoints[numFieldLineDetPoints++].y = whitePosition;
			pointsInColumn++;
		}
	}

	// Reset the system after finding our first transition
	greenPositionAbove = greenPositionBelow;
	greenPositionBelow = -1;
	whitePosition = -1;

	// Continue to search down the image in case there is another transition present
	while (searchYDown < currentImageHeight_) {
		if ((whitePosition != -1) && (*pixelPointer == c_FIELD_GREEN)) {
			greenPositionBelow = searchYDown;
		} else if  (*pixelPointer == c_WHITE) {
			whitePosition = searchYDown;
		}
		if ((greenPositionBelow != -1) && (whitePosition != -1) && (greenPositionAbove != -1)) {
			// Store the transition into the find points array as we have found a point on a line
			if (numFieldLineDetPoints < MAX_SIDELINE_POINTS) {
				fieldLineDetPoints[numFieldLineDetPoints].x = currX;
				fieldLineDetPoints[numFieldLineDetPoints++].y = whitePosition;
			}
			// Stores the Y value of the second point in the column which we have found
			if (pointsInColumn == 1) {
				centreCircle[centreCircleNum].y = whitePosition;
			}
			pointsInColumn++;

			greenPositionAbove = greenPositionBelow;
			greenPositionBelow = -1;
			whitePosition = -1;
		}
		searchYDown += GWG_Y_INC;
		pixelPointer += GWG_Y_INC*currentImageWidth_; // Increment the Y value searching down
	}

	if ((pointsInColumn >= 3) && (centreCircleNum < MAX_CENTRE_CIRCLE_POINTS)){
		centreCircle[centreCircleNum].x = currX;
		centreCircleNum++;
	}
	return;
}

// Checks that the blob is sufficiently close to the horizon to be considered as the specified object
bool EdgeRecognition::PointBelowHorizon(int x, int y) {
	if (visionData->horizonLine.exists) {
		int horizonY = (int) (visionData->horizonLine.m * x + visionData->horizonLine.b);
		if (y < horizonY) { // Note this is a > as the 0,0 coordinate is in the top left hand corner and as we want our points below this line it must be >
			return false; // Return false if the distance from the horizon is excessive
		}
	}
	return true; // If the horizon line does not exist return true anyway as we cannot check the elevation
}

int EdgeRecognition::LocateStartPoint(sortedLine lineInstance){
	int cornerGuess = -1;
  double maxDist = -1;

  // The equation for the distance of a point from a line is as follows:
  // ((x3 - x1)(x2 - x1) + (y3 - y1)(y2 - y1))/SQUARE(DIST(p2-p1))
  // x = x1 + u (x2 - x1)
  // y = y1 + u (y2 - y1) 
  // x and y define the point on the line which is closest to point p3

  // Precalculate some of the components to minimise the effort
  int valueX1 = lineInstance.points[0].x;
  int valueY1 = lineInstance.points[0].y; 
  int valueX2 = lineInstance.points[lineInstance.numPoints-1].x;
  int valueY2 = lineInstance.points[lineInstance.numPoints-1].y;
//int diffX21 = valueX2 - valueX1;
//int diffY21 = valueY2 - valueY1;
  double distX21 = visionData->GetDistance(valueX1, valueY1, valueX2, valueY2);
  
	// Find the point which is most distant from these points - this becomes our guess for the corner
	for (int i = 1; i < (lineInstance.numPoints-2); i++) {
    int valueX3 = lineInstance.points[i].x;
    int valueY3 = lineInstance.points[i].y;
		// Need to find the centrepoint i.e. the minimum distance of the point to the line
    double u = ((valueX3 - valueX1)*(valueX2 - valueX1) + (valueY3 - valueY1)*(valueY2 - valueY1))/SQUARE(distX21);
      int x = (int) (valueX1 + u * (valueX2 - valueX1));
      int y = (int) (valueY1 + u * (valueY2 - valueY1));
      double distance = visionData->GetDistance(x, y, valueX3, valueY3);

      // Check if this point is closer than the other points
      if (distance > maxDist) {
        maxDist = distance;
        cornerGuess = i;
      }
	}

	// Return the value and find the local minima around this point
	return cornerGuess;
}


// This version of findcornerpoint works its way accross dividing the systems in two looking for the minimum error
int EdgeRecognition::FindCornerPoint(sortedLine lineInstance, int numMerges,bool leftSystem) {
	const int MIN_POINTS_IN_LINE = 5;

	if((lineInstance.numPoints) < (2 * MIN_POINTS_IN_LINE + 1)) // Checks to ensure that we have points with which to generate the corner
		return -1;

	// Perform a basic saniy check to make sure that we are not really dealing with a staight line
	int startPoint = 0;
	int endPoint = lineInstance.numPoints;

	line leftLine; // The left line which we are currently trying to fit
	line bestLeftLine; // The best left line which we have fitted so far
	line rightLine; // The right line we are currently trying to fit
	line bestRightLine; // The best right line that we have fitted so far

	double minError = DBL_MAX; // The minimum error at this stage is simply a very large number

	int leftSumXsq=0, leftSumYsq=0, leftSumX=0, leftSumY=0, leftSumXY=0;
	int rightSumXsq=0, rightSumYsq=0, rightSumX=0, rightSumY=0, rightSumXY=0;

	int i = 0;

  int centrePoint = MIN_POINTS_IN_LINE;
  const int INCREMENT_AMOUNT = 1;

  // Find where we believe the corner is meant to be 
	int centreGuess = LocateStartPoint(lineInstance);

	// Check if an error condition was returned and if so abort
	if ((centreGuess == -1) || (centreGuess < MIN_POINTS_IN_LINE)) {
		centrePoint = MIN_POINTS_IN_LINE;
  } else { 
    centrePoint = centreGuess - SEARCH_DISTANCE;
  }

	//for (i = startPoint; i < centrePoint; i++) {
  for (i = startPoint; i < centrePoint-INCREMENT_AMOUNT; i++) {
    int x = lineInstance.points[i].x; int y = lineInstance.points[i].y;
		leftSumXsq += x*x;
		leftSumYsq += y*y;
		leftSumXY += x*y;
		leftSumX += x;
		leftSumY += y;
	}

	//for (i = centrePoint; i < endPoint; i++) {
  for (i = centrePoint-INCREMENT_AMOUNT; i < endPoint; i++) {
		int x = lineInstance.points[i].x; int y = lineInstance.points[i].y;
		rightSumXsq += x*x;
		rightSumYsq += y*y;
		rightSumXY += x*y;
		rightSumX += x;
		rightSumY += y;
	}

  // Find the lines which best fit the system
  int startCentrePoint = centrePoint;
  for (; centrePoint < (startCentrePoint + 2 * SEARCH_DISTANCE); centrePoint+=INCREMENT_AMOUNT) {
    for (int k = 0; k < INCREMENT_AMOUNT; k++) {
      int x = lineInstance.points[centrePoint-1-k].x; int y = lineInstance.points[centrePoint-1-k].y;
			leftSumXsq += x*x;
			leftSumYsq += y*y;
			leftSumXY += x*y;
			leftSumX += x;
			leftSumY += y;
			x = lineInstance.points[centrePoint-1-k].x; y = lineInstance.points[centrePoint-1-k].y;
			rightSumXsq -= x*x;
			rightSumYsq -= y*y;
			rightSumXY -= x*y;
			rightSumX -= x;
			rightSumY -= y;
		}

		if(FitXLine(&leftLine,lineInstance.points,startPoint,centrePoint,-1, leftSumXsq,leftSumX, leftSumY, leftSumXY) < 1) { // Calls fitXLine to generate the best fit line for the x points
			if(FitYLine(&leftLine,lineInstance.points,startPoint,centrePoint,-1, leftSumYsq, leftSumX, leftSumY, leftSumXY) < 0) {
				continue;
			}
		}

	  if(FitXLine(&rightLine,lineInstance.points,centrePoint,endPoint,-1, rightSumXsq, rightSumX, rightSumY, rightSumXY) < 1) { // Calls fitXLine to generate the best fit line for the x points
		  if(FitYLine(&rightLine,lineInstance.points,centrePoint,endPoint,-1, rightSumYsq, rightSumX, rightSumY, rightSumXY) < 0) {
				continue;
			}
		}

		// Using the residual error in the system
		double leftError = CalculateResidual(leftLine,lineInstance.points, startPoint, centrePoint);
		double rightError = CalculateResidual(rightLine,lineInstance.points, centrePoint, endPoint);

		if ((leftError + rightError) < minError) {
			bestLeftLine = leftLine;
			bestRightLine = rightLine;
			minError = leftError + rightError;
		}
	}

	if (minError == DBL_MAX) {
		return -1; // As we have not found a line
	}

	double theta = 0;
	if (leftSystem) {
		theta = atan((bestRightLine.m-bestLeftLine.m)/(1 + bestRightLine.m*bestLeftLine.m));
	} else {
		theta = atan((bestLeftLine.m-bestRightLine.m)/(1 + bestLeftLine.m*bestRightLine.m));
	}

	if (ABS(RAD_TO_DEG(theta)) < 15) {
		//cout << "This is most likley a straight line" << endl;
		return -1;
	}

	if (ABS(bestRightLine.m - bestLeftLine.m) < 0.01) {
		//cout << "This is most likley a straight line" << endl;