edgerecognition.cc

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

		return -1;
	}

	// Based on the position with least error (which technically should be the corner
	int x = (int)((bestLeftLine.b - bestRightLine.b)/(bestRightLine.m - bestLeftLine.m));
	int y = (int)(bestRightLine.m * x + bestRightLine.b);

	// Store the point as a vision object - note distance is unknown at this stage
	if ((x < currentImageWidth_) && (y < currentImageHeight_) && (x > 0) && (y > 0)) {
	  // Assume distance is large based on the distance to an infinite point theory
		// Determine whether this is an outside corner or not
		bool outSideCorner = false;
		int rightX = lineInstance.points[endPoint-1].x;
		int leftX = lineInstance.points[startPoint].x;
    
    if (((leftX < x) && (rightX < x)) || ((leftX > x) && (rightX > x))) {
			outSideCorner = true;
		}

		if  (((RAD_TO_DEG(theta)) < 0)  || (outSideCorner == true) ){
// FIXME outside goal corners are now disabled (picks up sooo many false ones ... don't know where) (-CM)
      return -1;
//		visionObjects_[numVisionObjects_].SetData(VisionObject::OT_GOAL_CORNER_OUT, NULL, NULL, visionData->CalculateHeading(x), visionData->CalculateElevation(y), 10000, x, y, 0);
		} else {
			visionObjects_[numVisionObjects_].SetData(VisionObject::OT_GOAL_CORNER_IN, NULL, NULL, visionData->CalculateHeading(x), visionData->CalculateElevation(y), 10000, x, y, 0);
		}
		
    // Calculate the actual distance based on elevation - Note because this calculation assumes the height of the dogs neck it is only valid when the dog is standing
		visionObjects_[numVisionObjects_].distance_ = visionData->GetDistanceToPoint(x, y,HEIGHTOFNECK);
		
    if (visionObjects_[numVisionObjects_].distance_ < 1) 
			visionObjects_[numVisionObjects_].distance_ = 1;
		visionData->TransformPositionObject(&visionObjects_[numVisionObjects_]);

		// The confidence indicates on a scale of 1 .. 100 how confident we are that the data collected about the object is correct
    PointConfidence(&visionObjects_[numVisionObjects_]);
    if (visionObjects_[numVisionObjects_].confidence_ == 0.0) {
      return -1;
    }
    numVisionObjects_++;
	}
	return 0;
}

// Calculates the Sum Squared Residual following a least squares fit - Abs(Ax+By+C)/Sqrt(A^2+B^2)
double EdgeRecognition::CalculateResidual(line genLine, point* lineDetPoints,  int startPoint, int endPoint) {
	double m = genLine.m;
	double b = genLine.b;
	double residual = 0;
	double sumSquaredResidual = 0;
  double numPointsUsed = 0;

	// Calculates the distance to a point which in turn determines the residual
	double denominator = sqrt(m*m+1);
	for(int i = startPoint; i < endPoint; i++) {
    if (lineDetPoints[i].x != -1) {
      residual = (lineDetPoints[i].x*m - lineDetPoints[i].y + b)/denominator;
		  sumSquaredResidual = sumSquaredResidual + SQUARE(residual);
      numPointsUsed++;
    }
	}
	return (sumSquaredResidual/numPointsUsed);
}

int EdgeRecognition::FindBorderLine(line* generatedLine) {
	generatedLine->exists = false; // Indicates that we have not generated a border line
	if(numSideLineDetPoints == 0) // Checks to ensure that we have points with which to generate the line
		return -1;

	if(FitXLine(generatedLine,sideLineDetPoints,0,numSideLineDetPoints,5) < 1) // Calls fitXLine to generate the best fit line for the x points
		if(FitYLine(generatedLine,sideLineDetPoints,0,numSideLineDetPoints,5) < 0)
			return -1;

	generatedLine->exists = true; // We have now generated a border line so set the boolean to true
	return 0;
}

// The remove outliers boolean checks to see whether outliers should be removed and the line refitted
// This is excleuded when we are trying to use the line fit to determine if the corner has been found
int EdgeRecognition::FitXLine(line* generatedLine, point* lineDetPoints, int startPoint, int endPoint, const int OUTLIER_BOUND, int sumXsq, int sumX, int sumY, int sumXY) {
	int n=0;
	bool recalculateLine = false;

	// calculating first line
	n = endPoint - startPoint;
	int returnValue = LeastSquaredFit(generatedLine,sumXsq,sumX,sumY,sumXY,n); // Note the order of these parameters is important
	if (returnValue < 0) // check whether an error value has been returned
		 return -1;
	recalculateLine = false; // Indicates that the m and b values have been generated

	// if the slope is greater than 1, we need to do a Y calculation
	// Note the > and < symbols have been reversed from the original code
	if((generatedLine->m > 1) || (generatedLine->m < -1))
		return 0;

	if (OUTLIER_BOUND < 0)  {
		generatedLine->exists = true;
		return 1;
	}
	
  // Checking points for outliers and changing sums if necessary
	for(int j = startPoint; j < endPoint; j++) {
		// Checks if the point lies to far from the line - Abs(Ax+By+C)/Sqrt(A^2+B^2)
		double dist = ABS(lineDetPoints[j].x*generatedLine->m - lineDetPoints[j].y + generatedLine->b)/sqrt(SQUARE(generatedLine->m) + 1);
		if (ABS(dist) > OUTLIER_BOUND) {
			recalculateLine = true;
			// Regenerate our variables before we recalculate the line
			sumXsq -= lineDetPoints[j].x * lineDetPoints[j].x;
			sumX -= lineDetPoints[j].x;
			sumXY -= lineDetPoints[j].x * lineDetPoints[j].y;
			sumY -= lineDetPoints[j].y;
			lineDetPoints[j].x = -1;
			n--;
		}
	}
		// if we found outliers, we recalc line
	if(recalculateLine) {
		int returnValue = LeastSquaredFit(generatedLine,sumXsq,sumX,sumY,sumXY,n); // Note the order of these parameters is important
		if (returnValue < 0) // check whether an error value has been returned
			return -1;
	}
	generatedLine->exists = true;
	return 1;
} 


int EdgeRecognition::FitXLine(line* generatedLine, point* lineDetPoints, int startPoint, int endPoint, const int OUTLIER_BOUND) {
	int sumXsq=0, sumX=0, sumXY=0, sumY=0;
	for(int i = startPoint; i < endPoint; i++) {
		int x = lineDetPoints[i].x; int y = lineDetPoints[i].y;
		sumXsq += x*x;
		sumXY += x*y;
		sumX += x;
		sumY += y;
	}
	return FitXLine(generatedLine, lineDetPoints, startPoint, endPoint, OUTLIER_BOUND, sumXsq, sumX, sumY, sumXY);
}


int EdgeRecognition::FitYLine(line* generatedLine, point* lineDetPoints, int startPoint, int endPoint,const int OUTLIER_BOUND, int sumYsq, int sumX, int sumY, int sumXY)
{
	int n=0;
	bool recalculateLine = false;

	// calculating first line
	n = endPoint - startPoint;
	int returnValue = LeastSquaredFit(generatedLine,sumYsq,sumY,sumX,sumXY,n); // Note the order of these parameters is important
	if (returnValue < 0) // check whether an error value has been returned
		 return -1;
	recalculateLine = false; // Indicates that the m and b values have been generated

 if (OUTLIER_BOUND < 0)  {
		generatedLine->exists = true;
		return 1;
	}

	//  if its less than 1, we'll just keep going since its probably close to 1 (X didnt' want it)
	// checking points for outliers and changing sums if necessary
	for(int j = startPoint; j < endPoint; j++) {
		// Checks if the point lies to far from the line - Abs(Ax+By+C)/Sqrt(A^2+B^2)
		double dist = ABS(lineDetPoints[j].x*generatedLine->m - lineDetPoints[j].y + generatedLine->b)/sqrt(SQUARE(generatedLine->m) + 1);
		if (ABS(dist) > OUTLIER_BOUND) {
			recalculateLine = true;
			sumYsq -= lineDetPoints[j].y * lineDetPoints[j].y;
			sumY -= lineDetPoints[j].y;
			sumXY -= lineDetPoints[j].x * lineDetPoints[j].y;
			sumX -= lineDetPoints[j].x;
			lineDetPoints[j].x = -1;
			n--;
		}
	}

	// if we found outliers, we recalc line
	if(recalculateLine) {
		int returnValue = LeastSquaredFit(generatedLine,sumYsq,sumY,sumX,sumXY,n); // Note the order of these parameters is important
		if (returnValue < 0) // check whether an error value has been returned
			 return -1;
	}
	generatedLine->exists = true;
	return 1;
} // fitYLine


// This is the same as the x section almost - combine
int EdgeRecognition::FitYLine(line* generatedLine, point* lineDetPoints, int startPoint, int endPoint,const int OUTLIER_BOUND)
{
	int sumYsq=0, sumY=0, sumXY=0, sumX=0;
	for(int i = startPoint; i < endPoint; i++) {
		int x = lineDetPoints[i].x; int y = lineDetPoints[i].y;
		sumYsq += y*y;
		sumXY += x*y;
		sumX += x;
		sumY += y;
	}

	return FitYLine(generatedLine, lineDetPoints, startPoint, endPoint, OUTLIER_BOUND, sumYsq, sumX, sumY, sumXY);
} 

int EdgeRecognition::LeastSquaredFit(line* generatedLine, int sumValSq, int sumVal, int sumOth, int sumXY, int n) {
	double det=0.0;
	double wA=0.0, wB=0.0, wC=0.0, wD=0.0;

	// Calculate the denominator of the system
	det = (sumValSq * n) - (sumVal * sumVal);

	if(det == 0) { // Simply checks that the denominator is not 0
		return -1;
	}

	wA = n/det;
	wB = wC = (-sumVal)/det;
	wD = sumValSq/det;

	generatedLine->m = (wA * sumXY) + (wB * sumOth); // Generates the m and b values for the line of best fit
	generatedLine->b = (wC * sumXY) + (wD * sumOth);
	return 0;
}

/*

// Uses the horizon and border lines to calculate the vanishing point of the two
// This point is then stored as a vision object n vision object array
void EdgeRecognition::CalcVanishingPoint() {
	// Checks that the horizon line and the border line have been determined and if so calculates the vanishing point
	if ((visionData->horizonLine.exists && visionData->borderLine.exists) && ((visionData->horizonLine.m - visionData->borderLine.m) != 0.0 )) {
		int x = (int)((float)(visionData->borderLine.b - visionData->horizonLine.b)/(float)(visionData->horizonLine.m - visionData->borderLine.m));
		int y = (int)((float)(visionData->horizonLine.m * x) + (float)visionData->horizonLine.b);
		// Store the point as a vision object - note distance is unknown at this stage
		visionObjects_[numVisionObjects_].SetData(VisionObject::OT_VANISHING_POINT, NULL, NULL, visionData->CalculateHeading(x), visionData->CalculateElevation(y), -1, x, y, 100);
		visionData->TransformPosition(&visionObjects_[numVisionObjects_]);
		numVisionObjects_++;
	}
}
*/

// Checks if the ball is on a wall and runs special code if it is to avoid pushing the ball all the way to the other end of the field
void EdgeRecognition::BoundaryDetection() {
	int wallOnLeftCounter = 0;
	int wallOnRightCounter = 0;
	wallOnLeft_=false;
	wallOnRight_=false;
	for (int i = 0; i < numVisionObjects_; i++) { // Search for the ball
		// Locate the ball vision objects and process to see if the ball is next to a wall on the right hand side

		if (visionObjects_[i].type_ == VisionObject::OT_BALL) {
      int distanceDiff = (int) (0.5 * visionData->GetDistance(visionObjects_[i].topBlob_->minX,visionObjects_[i].topBlob_->minY,visionObjects_[i].centreX_,visionObjects_[i].centreY_));

      int wallSearchRegionLeft = WALL_SEARCH_REGION;
      int wallSearchRegionRight = WALL_SEARCH_REGION;
      int x = 0;
      int unknownCounter = 0;
			for (x=0;x<wallSearchRegionLeft; x++) {
				int xPosition = visionObjects_[i].centreX_ - distanceDiff - x;
				int y = x;
				int yPosition = (int)((visionObjects_[i].topBlob_->minY+visionObjects_[i].topBlob_->maxY)/2.0 - distanceDiff - y);
        if (yPosition < 0 || yPosition >= currentImageHeight_ || xPosition < 0 || xPosition >= currentImageWidth_) continue;

        if (visionData->classified_[currentImageWidth_ * yPosition + xPosition] == c_BALL_ORANGE) {
          wallSearchRegionLeft++;
        }
			  int searchPixel = visionData->classified_[currentImageWidth_ * yPosition + xPosition];
        if (searchPixel == c_ROBOT_RED) {
          break;
        } else if (searchPixel == c_FIELD_GREEN) {
          break;
        } else if (searchPixel == c_UNKNOWN) {
          unknownCounter++;
          if (unknownCounter >= 3) break;
        } else if (searchPixel == c_WHITE) {
				  wallOnLeftCounter++;
					if (wallOnLeftCounter >= 4) {
						wallOnLeft_ = true;
            break;
					}
				}
			}

			for (x=0;x<wallSearchRegionRight; x++) {
				int xPosition = visionObjects_[i].centreX_ + distanceDiff + x;
				int y = x;
        int unknownCounter = 0;
				int yPosition = (int)((visionObjects_[i].topBlob_->minY+visionObjects_[i].topBlob_->maxY)/2.0 - distanceDiff - y);
        if (yPosition < 0 || yPosition >= currentImageHeight_ || xPosition < 0 || xPosition >= currentImageWidth_) continue;

        if (visionData->classified_[currentImageWidth_ * yPosition + xPosition] == c_BALL_ORANGE) {
          wallSearchRegionRight++;
        }
			  int searchPixel = visionData->classified_[currentImageWidth_ * yPosition + xPosition];
        if (searchPixel == c_ROBOT_RED) {
          break;
        } else if (searchPixel == c_FIELD_GREEN) {
          break;
        } else if (searchPixel == c_UNKNOWN) {
          unknownCounter++;
          if (unknownCounter >= 3) break;
        } else if (searchPixel == c_WHITE) {
				  wallOnRightCounter++;
					if (wallOnRightCounter >= 4) {
						wallOnRight_ = true;
						break;
					}
				}