edgerecognition.cc

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

// File: EdgeRecognition.cc
// Date: 18/02/03

#include "../Common/VisionObject.h"
#include "../Common/VisionLine.h"
#include "../Common/Common.h"
#include "EdgeRecognition.h"
#include "../Globals.h"
#include <math.h>
#include <float.h>
#include <list>

EdgeRecognition::EdgeRecognition() {
}

void EdgeRecognition::FindVanishingPoint(VisionData* vd) {
	
	visionData = vd;
	double associationError = -1;
	double leftAssociationError = -1;
	double rightAssociationError = -1;

	BoundaryDetection(); // Ball on wall detection

	// Run edge detection if it is required by one of the lower levels
	if ((vd->findSideLineCorner) || (vd->findCentreLineCorner) || (vd->findGoalCorner) || (vd->findCentreCircle) || vd->findSideLineAngle) {
		EdgeDetection(); // Find points on the field and side lines within the image
	}

	// Associate data identified as field lines if required
	bool fieldLineDataAssociated = false;
	if (vd->findGoalCorner || vd->findCentreCircle) {
		leftAssociationError = DataAssociationLeft(fieldLineDetPoints, numFieldLineDetPoints,sortedLinesLeft, &numSortedLinesLeft); // Allocates the points found into the lines to which they belong
		rightAssociationError = DataAssociationRight(fieldLineDetPoints, numFieldLineDetPoints,sortedLinesRight, &numSortedLinesRight); // Allocates the points found into the lines to which they belong

		if ((leftAssociationError > 0) && (rightAssociationError > 0) ){
			fieldLineDataAssociated = true;
		}
	}

	// Associate data identified as side lines if required
	bool sideLineDataAssociated = false;
	if (vd->findSideLineCorner || vd->findCentreLineCorner || vd->findSideLineAngle) {
		associationError = DataAssociationLeft(sideLineDetPoints, numSideLineDetPoints, sortedLinesSide, &numSortedLinesSide); // Allocates the points found into the lines to which they belong
		if (associationError > 0) {
			sideLineDataAssociated = true;
		}
	}

	// Find the coordinates of the side line corner
	if (vd->findSideLineCorner && sideLineDataAssociated) {
		//FindSideLineCorner(); // Find corners which are present in the side lines
	}

	// Find the coordinates of the centre side line corner
	if (vd->findCentreLineCorner && sideLineDataAssociated && fieldLineDataAssociated && (centreCircleNum == 0)) {
		FindCentreLineCorner(leftAssociationError,rightAssociationError);
	}

	// Find the coordinates of the goal corner
	if ((vd->findGoalCorner) && (fieldLineDataAssociated) && (centreCircleNum == 0)) {
		FindGoalCorner(leftAssociationError,rightAssociationError); // Finds the corners of the goalies box
	}

	// Find the coordinates of the centre circle
	if ((vd->findCentreCircle) && (fieldLineDataAssociated) && (centreCircleNum != 0)) {
		FindCentreCircle();
	}

	if (vd->findSideLineAngle) {
		VerticalAndSideLineAngle();
	}
}

// Locates the goal corner or the centre circle based on the information found during the edgedetection phase
// centreCircleNum indicates the number of three point transitions which we have found if this is equal to 0
// then we have not found any indication of the centre circle and need to check for a corner point
void EdgeRecognition::FindGoalCorner(double leftError, double rightError) {

	int numMerges = 0;

	if ((leftError < rightError) && (leftError > 0)) {
		numMerges = MergeLines(sortedLinesLeft, numSortedLinesLeft);
		for (int m=0; m < numSortedLinesLeft; m++) {
			if (sortedLinesLeft[m].subsumed == false) { // If the sorted line found has not been sumsumed then try and fit a corner to it
				FindCornerPoint(sortedLinesLeft[m],numMerges,true);
			}
		}
	} else {                  
		numMerges = MergeLines(sortedLinesRight, numSortedLinesRight);
		for (int m=0; m < numSortedLinesRight; m++) {
			if (sortedLinesRight[m].subsumed == false) { // If the sorted line found has not been sumsumed then try and fit a corner to it
				FindCornerPoint(sortedLinesRight[m],numMerges,false);
			}
		}
	}
}

// Handles the case when we believe that we have located a centre circle
void EdgeRecognition::FindCentreCircle() {
	// The following check provides a little tolerance as far as the algorithm is concerned
	if (centreCircleNum < 3) {
		return;
	}

	// Now that we have a couple of sets of points in the image which could belong to the centre of the circle
	int centreX = centreCircle[0].x + (int)((centreCircle[centreCircleNum].x - centreCircle[0].x)/2.0);
	line centreLine;

	if(FitXLine(&centreLine,centreCircle,0,centreCircleNum,5) < 1) // Calls fitXLine to generate the best fit line for the x points
		if(FitYLine(&centreLine,centreCircle,0,centreCircleNum,5) < 0)
			return;

	int centreY = (int) (centreLine.m*centreX + centreLine.b);

	if ((centreX < currentImageWidth_) && (centreY < currentImageHeight_) && (centreX > 0) && (centreY > 0)) {
		// Assume distance is large based on the distance to an infinite point theory
		visionObjects_[numVisionObjects_].SetData(VisionObject::OT_CENTRE_POINT, NULL, NULL, visionData->CalculateHeading(centreX), visionData->CalculateElevation(centreY), 1, centreX, centreY, 0);
		visionObjects_[numVisionObjects_].distance_ = visionData->GetDistanceToPoint(centreX, centreY, HEIGHTOFNECK);
		if (visionObjects_[numVisionObjects_].distance_ < 1) 
			visionObjects_[numVisionObjects_].distance_ = 1;
			visionData->TransformPositionObject(&visionObjects_[numVisionObjects_]);
			// 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

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

// Sort the points determined into a series of lines based on which points are closest together
double EdgeRecognition::DataAssociationLeft(point* lineDetPoints, int numLineDetPoints, sortedLine* sortedLines, int* numSortedLines) {
	if (numLineDetPoints == 0) {
		return -1;
	}

	*numSortedLines = 0; // Reset the number of lines that we have

	// Set the first point of the first line to the first point found so we have a starting point
	sortedLines[*numSortedLines].points[0] = lineDetPoints[0];
	sortedLines[*numSortedLines].numPoints = 1;
	sortedLines[*numSortedLines].subsumed = false;
	sortedLines[*numSortedLines].merged = false;
	(*numSortedLines)++;

	// Allocate data to the closest line
	for (int i=1; i < numLineDetPoints; i++) {
		AssociateData(i, lineDetPoints, numLineDetPoints ,sortedLines, numSortedLines);
	}

	// Determine the error of the fitted system
	double systemError = 0;
	line fittedLine;
	for (int j=0; j < *numSortedLines; j++) {
		if(FitXLine(&fittedLine,sortedLines[j].points,0,sortedLines[j].numPoints,-1) < 1) { // Calls fitXLine to generate the best fit line for the x points
			if (FitYLine(&fittedLine,sortedLines[j].points,0,sortedLines[j].numPoints,-1) < 0)
			continue;
		}
		systemError = systemError + CalculateResidual(fittedLine,sortedLines[j].points, 0, sortedLines[j].numPoints);
	}
	return systemError;
}

// Sort the points determined into a series of lines based on which points are closest together
double EdgeRecognition::DataAssociationRight(point* lineDetPoints, int numLineDetPoints, sortedLine* sortedLines, int* numSortedLines) {
	if (numLineDetPoints == 0) {
		return -1;
	}

	*numSortedLines = 0; // Reset the number of lines that we have

	// Set the first point of the first line to the first point found so we have a starting point
	sortedLines[*numSortedLines].points[0] = lineDetPoints[numLineDetPoints-1];
	sortedLines[*numSortedLines].numPoints = 1;
	sortedLines[*numSortedLines].subsumed = false;
	sortedLines[*numSortedLines].merged = false;
	(*numSortedLines)++;

	// Allocate data to the closest line
	for (int i=numLineDetPoints-2; i >= 0; i--) {
		AssociateData(i, lineDetPoints, numLineDetPoints ,sortedLines, numSortedLines);
	}

	// Determine the error of the fitted system
	double systemError = 0;
	line fittedLine;
	for (int j=0; j < *numSortedLines; j++) {
		if(FitXLine(&fittedLine,sortedLines[j].points,0,sortedLines[j].numPoints,-1) < 1) { // Calls fitXLine to generate the best fit line for the x points
			if (FitYLine(&fittedLine,sortedLines[j].points,0,sortedLines[j].numPoints,-1) < 0)
				continue;
		}
		systemError = systemError + CalculateResidual(fittedLine,sortedLines[j].points, 0, sortedLines[j].numPoints);
	}
	return systemError;
}

void EdgeRecognition::AssociateData(int i, point* lineDetPoints, int numLineDetPoints, sortedLine* sortedLines, int* numSortedLines) {
	int minIndex = -1;
	double minDistance = DBL_MAX;

	for (int j=0; j < *numSortedLines; j++) {
		double distance = visionData->GetDistance(lineDetPoints[i].x, lineDetPoints[i].y, sortedLines[j].points[sortedLines[j].numPoints-1].x, sortedLines[j].points[sortedLines[j].numPoints-1].y);
		if (distance < minDistance) {
			minDistance = distance;
			minIndex = j;
		}
	}
	if (minDistance > 15 ) {
		// Then start a new line
		if (*numSortedLines < MAX_SORTED_LINES) {
			sortedLines[*numSortedLines].points[0] = lineDetPoints[i];
			sortedLines[*numSortedLines].numPoints = 1;
			sortedLines[*numSortedLines].subsumed = false;
			sortedLines[*numSortedLines].merged = false;
			(*numSortedLines)++;
		} 
	} else {
		// Add the point to the line which has the smallest distance
		if (sortedLines[minIndex].numPoints < MAX_STORED_LINE_POINTS) {
			sortedLines[minIndex].points[sortedLines[minIndex].numPoints] = lineDetPoints[i];
			sortedLines[minIndex].numPoints++;
		}
	}
}

// Merge lines whose ends are relativly close together provided that the line has not already been subsumed
// Returns the number of merges performed on the system passed in
int EdgeRecognition::MergeLines(sortedLine* sortedLines, int numSortedLines) {
	int numMerges = 0;
	for (int k=0; k < numSortedLines; k++) {
		int minIndex = -1;
		double minDistance = DBL_MAX;
		for (int l=0; l < numSortedLines; l++) {
			if (k==l) continue;
			double distanceLeft = visionData->GetDistance(sortedLines[l].points[0].x, sortedLines[l].points[0].y, sortedLines[k].points[0].x, sortedLines[k].points[0].y);
			double distanceRight = visionData->GetDistance(sortedLines[l].points[sortedLines[l].numPoints-1].x, sortedLines[l].points[sortedLines[l].numPoints-1].y, sortedLines[k].points[sortedLines[k].numPoints-1].x, sortedLines[k].points[sortedLines[k].numPoints-1].y);
			if (sortedLines[l].subsumed == false) {
				if (distanceLeft < minDistance) {
					minDistance = distanceLeft;
					minIndex = l;
				}
				if (distanceRight < minDistance) {
					minDistance = distanceRight;
					minIndex = l;
				}
			}
		}

		if (minDistance < 20) {
			numMerges++;
			// if the distance check passes then the two lines need to be merged
			// we need to merge the lines in here take one and append it ... we may also may need to subsume the line
			double distanceLeft = visionData->GetDistance(sortedLines[minIndex].points[0].x, sortedLines[minIndex].points[0].y, sortedLines[k].points[0].x, sortedLines[k].points[0].y);
			double distanceRight = visionData->GetDistance(sortedLines[minIndex].points[sortedLines[minIndex].numPoints-1].x, sortedLines[minIndex].points[sortedLines[minIndex].numPoints-1].y, sortedLines[k].points[sortedLines[k].numPoints-1].x, sortedLines[k].points[sortedLines[k].numPoints-1].y);

			// Subsume the merges lines so we don't use it again
			sortedLines[minIndex].subsumed = true;

			// Make a decision on which way we need to join the data
			if (distanceLeft < distanceRight) {
				sortedLine tempSortedLine;
				tempSortedLine.numPoints = 0;
				for (int n=sortedLines[minIndex].numPoints-1; n >= 0; n--) {
					if (tempSortedLine.numPoints < MAX_STORED_LINE_POINTS) {
						tempSortedLine.points[tempSortedLine.numPoints] = sortedLines[minIndex].points[n];
						tempSortedLine.numPoints++;
					}
				}
				for (int otherLine=0; otherLine < sortedLines[k].numPoints; otherLine++) {
					if (tempSortedLine.numPoints < MAX_STORED_LINE_POINTS) {
						tempSortedLine.points[tempSortedLine.numPoints] = sortedLines[k].points[otherLine];
						tempSortedLine.numPoints++;
					}
				}
				// Store the new array back into the original position
				sortedLines[k] = tempSortedLine;
				sortedLines[k].merged = true;
			} else {
				for (int n=sortedLines[minIndex].numPoints-1; n >= 0; n--) {
					if (sortedLines[k].numPoints < MAX_STORED_LINE_POINTS) {
						sortedLines[k].points[sortedLines[k].numPoints] = sortedLines[minIndex].points[n];
						sortedLines[k].numPoints++;
					}
				}
				sortedLines[k].merged = true;
			}
		}
	}
	return numMerges;
}

// This method uses beacons on the field to set threshold limits from which lines on the field are located
void EdgeRecognition::FindSideLineCorner() {
	for (int m=0; m < numSortedLinesLeft; m++) {
		if (sortedLinesLeft[m].subsumed == false)  // If the sorted line found has not been sumsumed then try and fit a corner to it
			FindCornerPoint(sortedLinesLeft[m],0,true);
	}
}