📄 edgerecognition.cc
字号:
}
}
}
}
// Finds the interception point of the centre line with the side line
void EdgeRecognition::FindCentreLineCorner(double leftError, double rightError) {
// Check to make sure tat we can't see a goal post really close when we are finding these corners
for (int i=0; i < numVisionObjects_; i++) {
if ((visionObjects_[i].type_ == VisionObject::OT_GOAL_B) || (visionObjects_[i].type_ == VisionObject::OT_GOAL_Y)) {
return;
}
}
int numSortedFieldLines = 0;
sortedLine* sortedLinesField = NULL;
if (leftError < rightError) {
if (leftError > CENTRE_CORNER_ERROR_MAX) {
return; // As we have a large error which may indicate the presence of the centre circle of some other undesirable noisy data
}
sortedLinesField = sortedLinesLeft;
numSortedFieldLines = numSortedLinesLeft;
} else {
if (rightError > CENTRE_CORNER_ERROR_MAX) {
return; // As we have a large error which may indicate the presence of the centre circle of some other undesirable noisy data
}
sortedLinesField = sortedLinesRight;
numSortedFieldLines = numSortedLinesRight;
}
double minDistance = DBL_MAX;
int minFieldLineIndex = -1;
line minSideLine;
int minSideLineIndex = -1;
bool startPoint = true;
// For each of the lines in the sorted side line structure find distance of the line to the point at the end of each field line
for (int sideLineCounter=0; sideLineCounter < numSortedLinesSide; sideLineCounter++) {
if (sortedLinesSide[sideLineCounter].numPoints < 3) {
continue;
}
line sideLine;
if(FitXLine(&sideLine,sortedLinesSide[sideLineCounter].points,0,sortedLinesSide[sideLineCounter].numPoints,5) < 1) { // Calls fitXLine to generate the best fit line for the x points
if(FitYLine(&sideLine,sortedLinesSide[sideLineCounter].points,0,sortedLinesSide[sideLineCounter].numPoints,5) < 0) {
continue;
}
}
for (int fieldLineCounter=0; fieldLineCounter < numSortedFieldLines; fieldLineCounter++) {
if (sortedLinesField[fieldLineCounter].numPoints < 3) {
continue;
}
// Checks the first point of the field line
double distance = ABS(sortedLinesField[fieldLineCounter].points[0].x*sideLine.m - sortedLinesField[fieldLineCounter].points[0].y + sideLine.b)/sqrt(sideLine.m*sideLine.m+1);
if (distance < minDistance) {
minDistance = distance;
minFieldLineIndex = fieldLineCounter;
minSideLine = sideLine;
minSideLineIndex = sideLineCounter;
startPoint = true;
}
// Checks the last point of the field line
int pointValue = sortedLinesField[fieldLineCounter].numPoints-1;
distance = ABS(sortedLinesField[fieldLineCounter].points[pointValue].x*sideLine.m - sortedLinesField[fieldLineCounter].points[pointValue].y + sideLine.b)/sqrt(sideLine.m*sideLine.m+1);
if (distance < minDistance) {
minDistance = distance;
minFieldLineIndex = fieldLineCounter;
minSideLine = sideLine;
minSideLineIndex = sideLineCounter;
startPoint = false;
}
}
}
if ((minFieldLineIndex != -1) && (minDistance < 20)) {
line fieldLine;
if(FitXLine(&fieldLine,sortedLinesField[minFieldLineIndex].points,0,sortedLinesField[minFieldLineIndex].numPoints,5) < 1) { // Calls fitXLine to generate the best fit line for the x points
if(FitYLine(&fieldLine,sortedLinesField[minFieldLineIndex].points,0,sortedLinesField[minFieldLineIndex].numPoints,5) < 0) {
return;
}
}
double denominator = minSideLine.m - fieldLine.m;
if ((ABS(denominator) <= 0.0001) || (denominator > DBL_MAX)) {
return;
}
double dBX = (fieldLine.b - minSideLine.b)/denominator;
double dBY = minSideLine.m * dBX + minSideLine.b;
// Store the point as a vision object - note distance is unknown at this stage
if ((dBX < currentImageWidth_) && (dBY < currentImageHeight_) && (dBX > 0) && (dBY > 0)) {
int x = (int) dBX; int y = (int) dBY;
visionObjects_[numVisionObjects_].SetData(VisionObject::OT_CENTRE_LINE_CORNER, NULL, NULL, visionData->CalculateHeading(x), visionData->CalculateElevation(y), 1, x, y, 0);
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;
}
numVisionObjects_++;
}
}
}
// This method uses beacons on the field to set limits from which the side lines on the field are split and then used
// to calculate the angle
void EdgeRecognition::VerticalAndSideLineAngle() {
visionData->leftBorderLine.exists = false; // Stores the left border line
visionData->middleBorderLine.exists = false; // Stores the middle border line
visionData->rightBorderLine.exists = false; // Stores the right border line
// Locate any one or more of the four corner points
int PB = -1; int BP = -1; int YP = -1; int PY = -1; int PG = -1; int GP = -1; int B = -1; int Y = -1;
for(int i = 0; i < numVisionObjects_; i++) { // Search for the beacons which are currently being seen
switch(visionObjects_[i].type_) {
case VisionObject::OT_BEACON_PB:
PB = i; break;
case VisionObject::OT_BEACON_BP:
BP = i; break;
case VisionObject::OT_BEACON_PY:
PY = i; break;
case VisionObject::OT_BEACON_YP:
YP = i; break;
case VisionObject::OT_BEACON_PG:
PG = i; break;
case VisionObject::OT_BEACON_GP:
GP = i; break;
case VisionObject::OT_GOAL_B:
B = i; break;
case VisionObject::OT_GOAL_Y:
Y = i; break;
default:
continue;
}
}
if ((BP != -1) && (PB != -1)) {
FindPointGivenCenter(&(visionData->leftBorderLine),0, -1, visionObjects_[BP].centreX_, visionObjects_[BP].distance_,VisionLine::LT_SIDELINE_GP); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->middleBorderLine), visionObjects_[BP].centreX_, visionObjects_[BP].distance_, visionObjects_[PB].centreX_,visionObjects_[PB].distance_,VisionLine::LT_SIDELINE_B);
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[PB].centreX_, visionObjects_[PB].distance_, currentImageWidth_, -1, VisionLine::LT_SIDELINE_PG);
}
else if ((PB != -1) && (PY != -1)) {
FindPointGivenCenter(&(visionData->leftBorderLine),0, -1, visionObjects_[PB].centreX_, visionObjects_[PB].distance_, VisionLine::LT_SIDELINE_B); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->middleBorderLine), visionObjects_[PB].centreX_, visionObjects_[PB].distance_, visionObjects_[PY].centreX_,visionObjects_[PY].distance_, VisionLine::LT_SIDELINE_PG);
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[PY].centreX_, visionObjects_[PY].distance_, currentImageWidth_,-1, VisionLine::LT_SIDELINE_Y);
}
else if ((PY != -1) && (YP != -1)) {
FindPointGivenCenter(&(visionData->leftBorderLine),0, -1, visionObjects_[PY].centreX_, visionObjects_[PY].distance_, VisionLine::LT_SIDELINE_PG); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->middleBorderLine), visionObjects_[PY].centreX_, visionObjects_[PY].distance_, visionObjects_[YP].centreX_,visionObjects_[YP].distance_, VisionLine::LT_SIDELINE_Y);
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[YP].centreX_, visionObjects_[YP].distance_, currentImageWidth_, -1, VisionLine::LT_SIDELINE_GP);
}
else if ((YP != -1) && (BP != -1)) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, visionObjects_[YP].centreX_, visionObjects_[YP].distance_, VisionLine::LT_SIDELINE_Y); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->middleBorderLine), visionObjects_[YP].centreX_, visionObjects_[YP].distance_, visionObjects_[BP].centreX_, visionObjects_[BP].distance_, VisionLine::LT_SIDELINE_GP);
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[BP].centreX_, visionObjects_[BP].distance_, currentImageWidth_, -1, VisionLine::LT_SIDELINE_B);
}
else if (BP != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, visionObjects_[BP].centreX_, visionObjects_[BP].distance_, VisionLine::LT_SIDELINE_GP); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[BP].centreX_, visionObjects_[BP].distance_, currentImageWidth_, -1, VisionLine::LT_SIDELINE_B);
}
else if (PB != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, visionObjects_[PB].centreX_, visionObjects_[PB].distance_, VisionLine::LT_SIDELINE_B); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[PB].centreX_, visionObjects_[PB].distance_, currentImageWidth_, -1, VisionLine::LT_SIDELINE_PG);
}
else if (PY != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, visionObjects_[PY].centreX_, visionObjects_[PY].distance_, VisionLine::LT_SIDELINE_PG); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[PY].centreX_, visionObjects_[PY].distance_, currentImageWidth_, -1, VisionLine::LT_SIDELINE_Y);
}
else if (YP != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, visionObjects_[YP].centreX_, visionObjects_[YP].distance_, VisionLine::LT_SIDELINE_Y); // Parameters represent the start and finsh x values for which the sideline is defined
FindPointGivenCenter(&(visionData->rightBorderLine), visionObjects_[YP].centreX_, visionObjects_[YP].distance_, currentImageWidth_, -1, VisionLine::LT_SIDELINE_GP);
}
else if (PG != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, currentImageWidth_, -1, VisionLine::LT_SIDELINE_PG); // Parameters represent the start and finsh x values for which the sideline is defined
}
else if (GP != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, currentImageWidth_, -1, VisionLine::LT_SIDELINE_GP); // Parameters represent the start and finsh x values for which the sideline is defined
}
else if (B != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, currentImageWidth_, -1, VisionLine::LT_SIDELINE_B); // Parameters represent the start and finsh x values for which the sideline is defined
}
else if (Y != -1) {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, currentImageWidth_, -1, VisionLine::LT_SIDELINE_Y); // Parameters represent the start and finsh x values for which the sideline is defined
}
else {
FindPointGivenCenter(&(visionData->leftBorderLine), 0, -1, currentImageWidth_, -1, VisionLine::LT_SIDELINE_UNK); // Parameters represent the start and finsh x values for which the sideline is defined
}
}
void EdgeRecognition::FindPointGivenCenter(line* borderLine, int startXValue, double startDistance, int endXValue, double endDistance, const VisionLine::LineType type) {
// Take the x parameters for the start and end of the line and find the points in the sideline points array which correspond to being in this section
for (int sideLineCounter=0; sideLineCounter < numSortedLinesSide; sideLineCounter++) {
// Checks to ensure that we have points with which to generate the line
if (sortedLinesSide[sideLineCounter].numPoints < 10)
continue;
int startIndex = -1;
int endIndex = -1;
for (int i=0; i < sortedLinesSide[sideLineCounter].numPoints; i++) {
int x = sortedLinesSide[sideLineCounter].points[i].x;
if ((startIndex == -1) && (startXValue <= x) && (x <= endXValue)) {
startIndex = i;
endIndex = i;
} else if ((startIndex != -1) && (x <= endXValue)) {
endIndex = i;
}
}
// Checks to ensure that we have points now that the size of the array has been reduced to generate the line
if ((endIndex - startIndex) < (4 * POINT_FIND_INC))
continue;
line sideLine;
if (startIndex != -1) { // That is we've found a line which lies within the range
if(FitXLine(&sideLine,sortedLinesSide[sideLineCounter].points,startIndex,endIndex,5) < 1) { // Calls fitXLine to generate the best fit line for the x points
if(FitYLine(&sideLine,sortedLinesSide[sideLineCounter].points,startIndex,endIndex,5) < 0) {
continue;
}
}
}
// Store the details of the line into a line type object and add it to the array
int startX = sortedLinesSide[sideLineCounter].points[startIndex].x;
int startY = sortedLinesSide[sideLineCounter].points[startIndex].y;
int endX = sortedLinesSide[sideLineCounter].points[endIndex].x;
int endY = sortedLinesSide[sideLineCounter].points[endIndex].y;
if ((startX < currentImageWidth_) && (startY < currentImageHeight_) && (endX < currentImageWidth_) && (endY < currentImageHeight_) && (startX >= 0) && (startY >= 0) && (endX >= 0) && (endY >= 0)) {
visionLines_[numVisionLines_].SetData(type, startX, startY, visionData->CalculateHeading(startX), visionData->CalculateElevation(startY), 1, endX, endY, visionData->CalculateHeading(endX), visionData->CalculateElevation(endY), 1, 0, 0);
// Calculate the start distance based on whether we have knowledge of the dustance already
if (startDistance == -1) {
visionLines_[numVisionLines_].startDistance_ = visionData->GetDistanceToPoint(startX, startY,HEIGHTOFNECK);
} else {
visionLines_[numVisionLines_].startDistance_ = startDistance;
}
// Check that we don't have a negative distance and if so update it
if (visionLines_[numVisionLines_].startDistance_ < 1) {
visionLines_[numVisionLines_].startDistance_ = 1;
} else if (visionLines_[numVisionLines_].startDistance_ > 400){
visionLines_[numVisionLines_].startDistance_ = 400;
}
// Calculate the end distance based on whether we have knowledge of the dustance already
if (endDistance == -1) {
visionLines_[numVisionLines_].endDistance_ = visionData->GetDistanceToPoint(endX, endY,HEIGHTOFNECK);
} else {
visionLines_[numVisionLines_].endDistance_ = endDistance;
}
// Check that we don't have a negative distance and if so update it
if (visionLines_[numVisionLines_].endDistance_ < 1) {
visionLines_[numVisionLines_].endDistance_ = 1;
} else if (visionLines_[numVisionLines_].endDistance_ > 400){
visionLines_[numVisionLines_].endDistance_ = 400;
}
visionData->TransformPositionLine(&visionLines_[numVisionLines_]);
// Calculates the confidence that we have in the line
LineConfidence(&visionLines_[numVisionLines_]);
numVisionLines_++;
}
*borderLine = sideLine;
}
}
// Calculate the confidence of a point discovered in the image
void EdgeRecognition::PointConfidence(VisionObject* visObject) {
// Calculate the probability based on the elevation of the ball
double elevProb = 1.0 - visionData->NonLinearScaling(visObject->elevation_*180.0/PI, -0.018, 0.012);
// Store the confidence value and return
visObject->confidence_ = (int) (100 * elevProb);
return;
}
// Calculate the confidence with which we believe the object discovered is a line
void EdgeRecognition::LineConfidence(VisionLine* visLine) {
// Calculate the probability based on the elevation of the ball
double elevProbStart = 1.0 - visionData->NonLinearScaling(visLine->startElevation_*180.0/PI, -0.018, 0.012);
double elevProbEnd = 1.0 - visionData->NonLinearScaling(visLine->endElevation_*180.0/PI, -0.018, 0.012);
// Store the confidence value and return
visLine->confidence_ = (int) (100 * elevProbStart * elevProbEnd);
return;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -