Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/ImageProcessor/GT2005ImageProcessor/GT2005LineFinder_DeterministicApproach.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GT2005LineFinder_DeterministicApproach.cpp
    00003 *
    00004 * Implementation of class GT2005LineFinder_DeterministicApproach
    00005 *
    00006 * @author <a href="mailto:stefan.czarnetzki@uni-dortmund.de">Stefan Czarnetzki</a>
    00007 */
    00008
    00009
    00010 #include "GT2005LineFinder_DeterministicApproach.h"
    00011
    00012 GT2005LineFinder_DeterministicApproach::GT2005LineFinder_DeterministicApproach(const ColorCorrector& colorCorrector, const ColorTable& colorTable)
    00013   :colorCorrector(colorCorrector), colorTable(colorTable)
    00014 {
    00015   normDistance = 3.0;
    00016   normProjection = 0.9925; // arccos(7°)
    00017   normProjectionPerpendicularToHorizon = 0.97; // arccos(14°)
    00018
    00019   framesSinceLastSeen=1000;
    00020   frameCount = 0;
    00021   circleCount = 0;
    00022 }
    00023
    00024 GT2005LineFinder_DeterministicApproach::~GT2005LineFinder_DeterministicApproach(void)
    00025 {
    00026 }
    00027
    00028 void GT2005LineFinder_DeterministicApproach::reset()
    00029 {
    00030   numberOfLineFragments = 0;
    00031   numberOfLines = 0;
    00032   numberOfLinePoints = 0;
    00033   foundByCircleFinder = false;
    00034   foundByOwnHandling = false;
    00035   framesSinceLastSeen++;
    00036   frameCount++;
    00037   if (frameCount>200)
    00038   {
    00039     if (circleCount>0)
    00040     {
    00041       // OUTPUT(idText,text, "MSHLineFinder: " << "Circle recognition rate:" << (((double)circleCount)/((double)frameCount)) );
    00042     }
    00043     frameCount = 0;
    00044     circleCount = 0;
    00045   }
    00046 }
    00047
    00048 void GT2005LineFinder_DeterministicApproach::considerLinePoint(Vector2<int> & pointOnLine, Vector2<double> & normalToLine)
    00049 {
    00050   // just store the point, if there is still a free space in the array
    00051   if (numberOfLinePoints < maxNumberOfLinePoints)
    00052   {
    00053     linePoints[numberOfLinePoints].pointOnLine = pointOnLine;
    00054     linePoints[numberOfLinePoints].normalToLine = normalToLine;
    00055     linePoints[numberOfLinePoints].belongsToLineNo = -1;
    00056     numberOfLinePoints++;
    00057   }
    00058   else
    00059   {
    00060     //OUTPUT(idText,text,"LineFinder: linePoint drop (raise maxNumberOfLinePoints)");
    00061   }
    00062 }
    00063
    00064 void GT2005LineFinder_DeterministicApproach::execute(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo, const RobotPose & robotPose)
    00065 {
    00066
    00067   lines = new LineFragment[maxNumberOfLines];
    00068   lineFragments = new LineFragment[maxNumberOfLines*2];
    00069
    00070   foundByCircleFinder = false;
    00071
    00072   findLineFragments();
    00073   handleCenterCircle(cameraMatrix, image, robotPose);
    00074   findLines(linesPercept, cameraMatrix, image, imageInfo);
    00075   findIntersections(linesPercept, cameraMatrix, image, robotPose, imageInfo);
    00076   addCenterCirclePercept(linesPercept, cameraMatrix, image);
    00077
    00078   delete[] lines;
    00079   delete[] lineFragments;
    00080
    00081   DEBUG_DRAWING_FINISHED(lineCrossingsField);
    00082   DEBUG_DRAWING_FINISHED(imageProcessor_lines);
    00083   DEBUG_DRAWING_FINISHED(imageProcessor_linefragments);
    00084 }
    00085
    00086 void GT2005LineFinder_DeterministicApproach::execute(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo, const RobotPose & robotPose, const bool circleFound, const Vector2<double> & circleOnField)
    00087 {
    00088   NDECLARE_DEBUGDRAWING( "gt05-imageprocessor:lineCrossingsField", "drawingOnField", "");
    00089   NDECLARE_DEBUGDRAWING( "gt05-imageprocessor:imageProcessor_lines", "drawingOnImage", "");
    00090   NDECLARE_DEBUGDRAWING( "gt05-imageprocessor:imageProcessor_linefragments", "drawingOnImage", "");
    00091
    00092   lines = new LineFragment[maxNumberOfLines];
    00093   lineFragments = new LineFragment[maxNumberOfLines*2];
    00094
    00095   foundByCircleFinder = false; // circleFound;
    00096   fromCircleFinder.x = (int)circleOnField.x;
    00097   fromCircleFinder.y = (int)circleOnField.y;
    00098
    00099   if (foundByCircleFinder)
    00100   {
    00101     handleCenterCircle(circleOnField, cameraMatrix, image);
    00102
    00103     // for validity tests
    00104     /*
    00105     Vector2<double> circleOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,circleOnField.x,circleOnField.y);
    00106     OUTPUT(idText,text, robotPose.translation.x << " " << robotPose.translation.y << " " << circleOnFieldGlobal.x << " " << circleOnFieldGlobal.y);
    00107     CIRCLE(circlePoints_Field,circleOnFieldGlobal.x,circleOnFieldGlobal.y,180,3,Drawings::ps_solid,Drawings::red);
    00108     */
    00109   }
    00110   findLineFragments();
    00111   if (!foundByCircleFinder)
    00112   {
    00113     handleCenterCircle(cameraMatrix, image, robotPose);
    00114   }
    00115   findLines(linesPercept, cameraMatrix, image, imageInfo);
    00116   findIntersections(linesPercept, cameraMatrix, image, robotPose, imageInfo);
    00117   addCenterCirclePercept(linesPercept, cameraMatrix, image);
    00118
    00119   delete[] lines;
    00120   delete[] lineFragments;
    00121
    00122   DEBUG_DRAWING_FINISHED(lineCrossingsField);
    00123   DEBUG_DRAWING_FINISHED(imageProcessor_lines);
    00124   DEBUG_DRAWING_FINISHED(imageProcessor_linefragments);
    00125 }
    00126
    00127 void GT2005LineFinder_DeterministicApproach::findLineFragments()
    00128 {
    00129   int i, j;
    00130   Vector2<double> temp;
    00131
    00132   // for all lines, calculate how many other lines are similar/near in sense of this special norm
    00133   bool similar[maxNumberOfLinePoints][maxNumberOfLinePoints];
    00134   for(i = 0; i < numberOfLinePoints; i++)
    00135   {
    00136     similar[i][i] = true;
    00137     for(j = i + 1; j < numberOfLinePoints; j++)
    00138     {
    00139       // similar/near if distance is small and normal is in the same direction
    00140       bool sim = false;
    00141       double projection = linePoints[i].normalToLine * linePoints[j].normalToLine; // projection := cos(angle)
    00142       if(projection > normProjection)
    00143       {
    00144         temp.x = (double)(linePoints[j].pointOnLine.x - linePoints[i].pointOnLine.x);
    00145         temp.y = (double)(linePoints[j].pointOnLine.y - linePoints[i].pointOnLine.y);
    00146         double distance1 = fabs(temp * linePoints[i].normalToLine);
    00147         if(distance1 < normDistance)
    00148         {
    00149           double distance2 = fabs(temp * linePoints[j].normalToLine);
    00150           sim = (distance2 < normDistance);
    00151         }
    00152       }
    00153       similar[i][j] = sim;
    00154       similar[j][i] = sim;
    00155     }
    00156   }
    00157
    00158   // analyse detected points and extract line fragments
    00159   int maxWeight;
    00160   int linePointWithHighestWeight;
    00161
    00162
    00163   for(int n=0; n<maxNumberOfLines*2; n++)
    00164   {
    00165     maxWeight = 0;
    00166     linePointWithHighestWeight = -1;
    00167
    00168     // find point with most similar points
    00169     int simCount[maxNumberOfLinePoints];
    00170     for(i=0; i<numberOfLinePoints; i++)
    00171     {
    00172       simCount[i]=1; // the point itself
    00173     }
    00174     for(i=0; i < numberOfLinePoints; i++)
    00175     {
    00176       if(linePoints[i].belongsToLineNo == -1) // only if point is not yet matched to a line fragment
    00177       {
    00178         for (j=i+1; j < numberOfLinePoints; j++)
    00179         {
    00180           if(linePoints[j].belongsToLineNo == -1) // only if point is not yet matched to a line fragment
    00181           {
    00182             if(similar[i][j])
    00183             {
    00184               // weight added to both points because of reflexivity
    00185               simCount[i]++;
    00186               simCount[j]++;
    00187
    00188               if(simCount[i] > maxWeight)
    00189               {
    00190                 maxWeight = simCount[i];
    00191                 linePointWithHighestWeight = i;
    00192               }
    00193               if(simCount[j] > maxWeight)
    00194               {
    00195                 maxWeight = simCount[j];
    00196                 linePointWithHighestWeight = j;
    00197               }
    00198             }
    00199           }
    00200         }
    00201       }
    00202     }
    00203     if (maxWeight<pointsNeededForLine)
    00204     {
    00205       break;
    00206     }
    00207
    00208     // build line from point with highest weight
    00209     Vector2<int> start(300,300);
    00210     Vector2<int> end(-1,-1);
    00211     // here the angle to the image border is used to determine, by which criteria start and end are selected (angle has no meaning in relation to the horizon)
    00212     double angle = atan2(linePoints[linePointWithHighestWeight].normalToLine.y,linePoints[linePointWithHighestWeight].normalToLine.x);
    00213     if (angle <= pi_4 && angle >= -1*pi_4) // line is vertical, select start and end by y-Value
    00214     {
    00215       for (i=0; i<numberOfLinePoints; i++)
    00216       {
    00217         if (similar[i][linePointWithHighestWeight] && linePoints[i].belongsToLineNo==-1)
    00218         {
    00219           linePoints[i].belongsToLineNo=numberOfLineFragments;
    00220           if (linePoints[i].pointOnLine.y < start.y)
    00221           {
    00222             start = linePoints[i].pointOnLine;
    00223           }
    00224           if (linePoints[i].pointOnLine.y > end.y)
    00225           {
    00226             end = linePoints[i].pointOnLine;
    00227           }
    00228         }
    00229       }
    00230     }
    00231     else  // line is horizontal, select start and end by x-Value
    00232     {
    00233       for (i=0; i<numberOfLinePoints; i++)
    00234       {
    00235         if (similar[i][linePointWithHighestWeight] && linePoints[i].belongsToLineNo==-1)
    00236         {
    00237           linePoints[i].belongsToLineNo=numberOfLineFragments;
    00238           if (linePoints[i].pointOnLine.x < start.x)
    00239           {
    00240             start = linePoints[i].pointOnLine;
    00241           }
    00242           if (linePoints[i].pointOnLine.x > end.x)
    00243           {
    00244             end = linePoints[i].pointOnLine;
    00245           }
    00246         }
    00247       }
    00248     }
    00249
    00250     // for fragments with only few points, remove the long ones, because they may be caused by random noise linePoints
    00251     if (maxWeight==pointsNeededForLine)
    00252     {
    00253       Vector2<int> length = end - start;
    00254       if ( length.abs() > 70 )
    00255       { // skip this fragment and mark points as "are not and will not be used"
    00256         for (i=0; i<numberOfLinePoints; i++)
    00257         {
    00258           if (linePoints[i].belongsToLineNo==numberOfLineFragments)
    00259           {
    00260             linePoints[i].belongsToLineNo=-2;
    00261           }
    00262         }
    00263         continue;
    00264       }
    00265     }
    00266
    00267     // accept the line fragment
    00268     Vector2<double> normalToLine((double)(end.x-start.x), (double)(end.y-start.y));
    00269     normalToLine.rotateLeft();
    00270     normalToLine.normalize();
    00271     Vector2<int> middle((start.x+end.x)/2, (start.y+end.y)/2);
    00272     lineFragments[numberOfLineFragments].base   = middle;
    00273     lineFragments[numberOfLineFragments].normal = normalToLine;
    00274     lineFragments[numberOfLineFragments].start  = start;
    00275     lineFragments[numberOfLineFragments].end    = end;
    00276     lineFragments[numberOfLineFragments].lineFragmentAlreadyConsidered = false;
    00277     lineFragments[numberOfLineFragments].averageStep = (end-start).abs() / maxWeight;
    00278     LINE(imageProcessor_linefragments,start.x,start.y,end.x,end.y,1,1,numberOfLineFragments);
    00279     NLINE( "gt05-imageprocessor:imageProcessor_linefragments",start.x,start.y,end.x,end.y,1,1,numberOfLineFragments);
    00280
    00281     numberOfLineFragments++;
    00282   }
    00283 }
    00284
    00285 void GT2005LineFinder_DeterministicApproach::findLines(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo)
    00286 {
    00287   int i, j;
    00288   Vector2<double> temp, temp2;
    00289   Vector2<double> horizonDirection = imageInfo.horizon.direction;
    00290   horizonDirection.normalize();
    00291
    00292
    00293   // if both sides or more than one fragment of a field line are detected, then filter these out and take their middle instead
    00294   for (i=0; i<numberOfLineFragments && numberOfLines<maxNumberOfLines; i++)
    00295   {
    00296     if (lineFragments[i].lineFragmentAlreadyConsidered)
    00297     {
    00298       continue;
    00299     }
    00300     lines[numberOfLines].base = lineFragments[i].base;
    00301     lines[numberOfLines].normal = lineFragments[i].normal;
    00302     lines[numberOfLines].start = lineFragments[i].start;
    00303     lines[numberOfLines].end = lineFragments[i].end;
    00304     lines[numberOfLines].averageStep = lineFragments[i].averageStep;
    00305     int numberOfLineFragmentsUsed = 1;
    00306     lineFragments[i].lineFragmentAlreadyConsidered = true;
    00307
    00308     double angle = atan2(lines[numberOfLines].normal.y,lines[numberOfLines].normal.x);
    00309     bool sortByY = angle <= pi_4 && angle >= -1*pi_4;
    00310
    00311     for (j=i+1; j<numberOfLineFragments; j++)
    00312     {
    00313       if (lineFragments[j].lineFragmentAlreadyConsidered)
    00314       {
    00315         continue;
    00316       }
    00317       double p = lineFragments[i].normal * lineFragments[j].normal;
    00318       Vector2<double> nij;
    00319       if (p>0)
    00320       {
    00321         nij = lineFragments[i].normal + lineFragments[j].normal;
    00322       }
    00323       else
    00324       {
    00325         nij = lineFragments[i].normal - lineFragments[j].normal;
    00326       }
    00327       nij.normalize();
    00328       bool perpendicularToHorizon = fabs(horizonDirection * nij) > 0.93;
    00329
    00330       if (     (perpendicularToHorizon || p>normProjection || p<-1*normProjection) // if not perpendicular, than decide using normProjection
    00331             && (!perpendicularToHorizon || p>normProjectionPerpendicularToHorizon || p<-1*normProjectionPerpendicularToHorizon)) // else, use normProjectionPerpendicularToHorizon
    00332       {
    00333         temp.x = (double)(lineFragments[i].base.x-lineFragments[j].base.x);
    00334         temp.y = (double)(lineFragments[i].base.y-lineFragments[j].base.y);
    00335         int lineHeight = max( Geometry::calculateLineSize(lineFragments[i].base, cameraMatrix, image.cameraInfo),
    00336                               Geometry::calculateLineSize(lineFragments[j].base, cameraMatrix, image.cameraInfo));
    00337         lineHeight = max( lineHeight, 2 );
    00338
    00339         if ( (fabs(temp * lineFragments[i].normal) < 1.5 * lineHeight) && (fabs(temp * lineFragments[j].normal) < 1.5 * lineHeight) )
    00340         {
    00341
    00342           if (p<0) // wrong orientation to sum it up, so invert it
    00343           {
    00344             lineFragments[j].normal *= -1.0;
    00345           }
    00346           lines[numberOfLines].base    += lineFragments[j].base;
    00347           lines[numberOfLines].normal  += lineFragments[j].normal;
    00348           lines[numberOfLines].averageStep += lineFragments[i].averageStep;
    00349
    00350           if (sortByY) // line is vertical, select start and end by y-Value
    00351           {
    00352             if (lineFragments[j].start.y < lines[numberOfLines].start.y)
    00353             {
    00354               lines[numberOfLines].start = lineFragments[j].start;
    00355             }
    00356             if (lineFragments[j].end.y > lines[numberOfLines].end.y)
    00357             {
    00358               lines[numberOfLines].end = lineFragments[j].end;
    00359             }
    00360           }
    00361           else  // line is horizontal, select start and end by x-Value
    00362           {
    00363             if (lineFragments[j].start.x < lines[numberOfLines].start.x)
    00364             {
    00365               lines[numberOfLines].start = lineFragments[j].start;
    00366             }
    00367             if (lineFragments[j].end.x > lines[numberOfLines].end.x)
    00368             {
    00369               lines[numberOfLines].end = lineFragments[j].end;
    00370             }
    00371           }
    00372
    00373           numberOfLineFragmentsUsed++;
    00374           lineFragments[j].lineFragmentAlreadyConsidered = true;
    00375         }
    00376       }
    00377     }
    00378     // the resulting line is a middle of its fragments
    00379     lines[numberOfLines].base /= numberOfLineFragmentsUsed;
    00380     lines[numberOfLines].averageStep /= numberOfLineFragmentsUsed;
    00381     lines[numberOfLines].normal.normalize();
    00382
    00383     numberOfLines++;
    00384   }
    00385
    00386   /*
    00387   // create new fieldLine-percepts based on the found lines (if at least one line was found)
    00388   double angle;
    00389   Vector2<int> temp3;
    00390   if (numberOfLines > 0)
    00391   {
    00392     linesPercept.numberOfPoints[LinesPercept::field] = 0;
    00393     for (i=0; i<numberOfLines; i++)
    00394     {
    00395       calculateLineOnField(i,temp,temp2,cameraMatrix,image);
    00396       angle = normalize(temp2.angle() + pi_2);
    00397       // the following if's will always work, because all these points are already evaluated to lie on the field, but better save than sorry
    00398       if (Geometry::calculatePointOnField(lines[i].base.x,lines[i].base.y,cameraMatrix,image.cameraInfo,temp3))
    00399       {
    00400         linesPercept.add(LinesPercept::field,temp3,angle);
    00401       }
    00402       if (Geometry::calculatePointOnField(lines[i].start.x,lines[i].start.y,cameraMatrix,image.cameraInfo,temp3))
    00403       {
    00404         linesPercept.add(LinesPercept::field,temp3,angle);
    00405       }
    00406       if (Geometry::calculatePointOnField(lines[i].end.x,lines[i].end.y,cameraMatrix,image.cameraInfo,temp3))
    00407       {
    00408         linesPercept.add(LinesPercept::field,temp3,angle);
    00409       }
    00410     }
    00411   }
    00412   */
    00413
    00414   // draw all lines
    00415   Vector2<int> r1(0,0);
    00416   Vector2<int> r2(image.cameraInfo.resolutionWidth-1,image.cameraInfo.resolutionHeight-1);
    00417   for (i=0; i<numberOfLines; i++)
    00418   {
    00419     // in the image
    00420     Vector2<int> pointOnLine;
    00421     Vector2<double> direction;
    00422     getLine(i, pointOnLine, direction);
    00423     direction.rotateLeft();
    00424     Geometry::Line a(pointOnLine, direction);
    00425     Vector2<int> point1, point2;
    00426     Geometry::getIntersectionPointsOfLineAndRectangle(r1,r2,a,point1,point2);
    00427     LINE(
    00428       imageProcessor_lines,
    00429       point1.x,
    00430       point1.y,
    00431       point2.x,
    00432       point2.y,
    00433       1,
    00434       Drawings::ps_solid,
    00435       Drawings::skyblue
    00436       );
    00437   }
    00438 }
    00439
    00440 void GT2005LineFinder_DeterministicApproach::findIntersections(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const RobotPose & robotPose, const ImageInfo& imageInfo)
    00441 {
    00442   int i,j;
    00443   // calculate line crossings
    00444   Geometry::Line lines2[maxNumberOfLines];
    00445   Vector2<double> intersection;
    00446   Vector2<int> intersectionOnField;
    00447   for (i=0; i<numberOfLines; i++)
    00448   {
    00449     Vector2<double> direction(lines[i].normal);
    00450     direction.rotateLeft();
    00451     direction.normalize();
    00452     lines2[i] = Geometry::Line(lines[i].base,direction);
    00453   }
    00454   for (i=0; i<numberOfLines; i++)
    00455   {
    00456     for (j=i+1; j<numberOfLines; j++)
    00457     {
    00458       if (linesPerpendicularOnField(i, j, cameraMatrix, image))
    00459       {
    00460         Geometry::getIntersectionOfLines(lines2[i],lines2[j],intersection);
    00461         if (Geometry::calculatePointOnField((int)intersection.x,(int)intersection.y,cameraMatrix,image.cameraInfo,intersectionOnField))
    00462         {
    00463           if (intersectionOnField.abs() < 3600)
    00464           {
    00465             addCrossingsPercept(intersection, intersectionOnField, i, j, linesPercept, image, imageInfo, cameraMatrix, robotPose);
    00466           }
    00467         }
    00468       }
    00469     }
    00470   }
    00471
    00472   // draw lines on field
    00473   for (i=0; i<numberOfLines; i++)
    00474   {
    00475     Vector2<int> pointOnField1, pointOnField2, directionOnField;
    00476     Geometry::calculatePointOnField(lines[i].base.x, lines[i].base.y, cameraMatrix, image.cameraInfo, pointOnField1);
    00477     Geometry::calculatePointOnField((int)(lines[i].base.x+50*lines[i].normal.y), (int)(lines[i].base.y-50*lines[i].normal.x), cameraMatrix, image.cameraInfo, pointOnField2);
    00478     directionOnField = pointOnField2-pointOnField1;
    00479     pointOnField1 = pointOnField1 - directionOnField * 10;
    00480     pointOnField2 = pointOnField2 + directionOnField * 10;
    00481     Vector2<double> point1OnFieldGlobal, point2OnFieldGlobal;
    00482     point1OnFieldGlobal = Geometry::relative2FieldCoord(robotPose, (double)pointOnField1.x, (double)pointOnField1.y);
    00483     point2OnFieldGlobal = Geometry::relative2FieldCoord(robotPose, (double)pointOnField2.x, (double)pointOnField2.y);
    00484     //LINE(lineCrossingsField, point1OnFieldGlobal.x, point1OnFieldGlobal.y, point2OnFieldGlobal.x, point2OnFieldGlobal.y, 3, Drawings::ps_solid, Drawings::skyblue);
    00485   }
    00486 }
    00487
    00488 void GT2005LineFinder_DeterministicApproach::addCrossingsPercept(const Vector2<double> & intersectionInImage, const Vector2<int> & intersectionOnField, int lineNumber1, int lineNumber2, LinesPercept & linesPercept, const Image& image, const ImageInfo& imageInfo, const CameraMatrix & cameraMatrix, const RobotPose & robotPose)
    00489 {
    00490   // prepare stuff and calculate bisection on field
    00491   Vector2<double> temp1,temp2, directionOnField1,directionOnField2;
    00492   bool firstLineIsLeft;
    00493   Vector2<double> directionOfLine1 = lines[lineNumber1].normal;
    00494   directionOfLine1.rotateRight();
    00495   Vector2<double> directionOfLine2 = lines[lineNumber2].normal;
    00496   directionOfLine2.rotateRight();
    00497   if (  !calculateLineOnField(lineNumber1,temp1,directionOnField1,cameraMatrix,image)
    00498     ||  !calculateLineOnField(lineNumber2,temp1,directionOnField2,cameraMatrix,image) )
    00499   {
    00500     // something went wrong and the calculation failed
    00501     return;
    00502   }
    00503   Vector2<double> bisectionOnField = directionOnField1 + directionOnField2;
    00504   bisectionOnField.normalize();
    00505   double angleOnField = atan2(bisectionOnField.y,bisectionOnField.x) + pi_4; // this is more stable than just taking the angle of one of the lines
    00506
    00507
    00508   Vector2<double> angleBisection = directionOfLine1 + directionOfLine2; // this is NOT the projection of the bisectionOnField into the image, this vector merely lies in the same quadrant, and therefore can be used here
    00509   angleBisection.normalize();
    00510   // decide, which line is lying on which side of the bisection
    00511   double angleOfBisection = atan2(angleBisection.y,angleBisection.x);
    00512   double angleOfLine1 = atan2(directionOfLine1.y,directionOfLine1.x);
    00513   double angleOfLine2 = atan2(directionOfLine2.y,directionOfLine2.x);
    00514   // BEWARE: the orientation in image coordinates and in field coordinates is invers, so sides in clockwise order in the image result in sides in contra-clockwise (i.e. mathematical positive) order on the field
    00515   if ( ( angleOfLine1 < angleOfBisection + pi_2 && angleOfLine1 > angleOfBisection )
    00516     || ( angleOfLine1 + pi2 < angleOfBisection + pi_2 && angleOfLine1 + pi2 > angleOfBisection )
    00517     || ( angleOfLine1 - pi2 < angleOfBisection + pi_2 && angleOfLine1 - pi2 > angleOfBisection ) )
    00518   { // line1 lies direcly right of the bisection
    00519     firstLineIsLeft=false;
    00520   }
    00521   else
    00522   {
    00523     firstLineIsLeft=true;
    00524   }
    00525
    00526
    00527
    00528   bool bothSidesOfLine1,bothSidesOfLine2;
    00529
    00530   temp1.x = (double)lines[lineNumber1].start.x - intersectionInImage.x;
    00531   temp1.y = (double)lines[lineNumber1].start.y - intersectionInImage.y;
    00532   temp2.x = (double)lines[lineNumber1].end.x - intersectionInImage.x;
    00533   temp2.y = (double)lines[lineNumber1].end.y - intersectionInImage.y;
    00534   if ( temp1*temp2 < 0 && temp1.abs() > 10 && temp2.abs() > 10 ) // points lie on different sides and with enough distance to the crossing point
    00535   {
    00536     bothSidesOfLine1=true;
    00537   }
    00538   else
    00539   {
    00540     bothSidesOfLine1=false; // or at least not decidable without additional scanning
    00541   }
    00542
    00543   temp1.x = (double)lines[lineNumber2].start.x - intersectionInImage.x;
    00544   temp1.y = (double)lines[lineNumber2].start.y - intersectionInImage.y;
    00545   temp2.x = (double)lines[lineNumber2].end.x - intersectionInImage.x;
    00546   temp2.y = (double)lines[lineNumber2].end.y - intersectionInImage.y;
    00547   if ( temp1*temp2 < 0 && temp1.abs() > 10 && temp2.abs() > 10 ) // points lie on different sides and with enough distance to the crossing point
    00548   {
    00549     bothSidesOfLine2=true;
    00550   }
    00551   else
    00552   {
    00553     bothSidesOfLine2=false; // or at least not decidable without additional scanning
    00554   }
    00555
    00556
    00557   LinesPercept::CrossingCharacteristic side1 = LinesPercept::dontKnow;
    00558   LinesPercept::CrossingCharacteristic side2 = LinesPercept::dontKnow;
    00559   LinesPercept::CrossingCharacteristic side3 = LinesPercept::dontKnow;
    00560   LinesPercept::CrossingCharacteristic side4 = LinesPercept::dontKnow;
    00561
    00562   if ( intersectionInImage.x < 0 || intersectionInImage.y < 0 || intersectionInImage.x >= image.cameraInfo.resolutionWidth || intersectionInImage.y >= image.cameraInfo.resolutionHeight )
    00563   { // intersection is outside the image
    00564     if (linesPercept.numberOfLineCrossings<linesPercept.maxNumberOfLineCrossings)
    00565     {
    00566       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationOnField = intersectionOnField;
    00567       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationInImage = Vector2<int>((int)intersectionInImage.x,(int)intersectionInImage.y);
    00568       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleOnField = angleOnField;
    00569       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage1 = angleOfLine1;
    00570       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage2 = angleOfLine2;
    00571       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side1 = side1;
    00572       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side2 = side2;
    00573       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side3 = side3;
    00574       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side4 = side4;
    00575       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].outOfImage = true;
    00576       linesPercept.numberOfLineCrossings++;
    00577     }
    00578   }
    00579   else
    00580   { // intersection is inside the image
    00581
    00582     // do some additional scanning to get characteristics of this crossing...
    00583     Vector2<int> temp((int)intersectionInImage.x,(int)intersectionInImage.y);
    00584     int lineSize = Geometry::calculateLineSize(temp, cameraMatrix, image.cameraInfo);
    00585
    00586     if (firstLineIsLeft)
    00587     {
    00588       if (bothSidesOfLine1)
    00589       {
    00590         side1 = LinesPercept::lineOnThisSide;
    00591         side3 = LinesPercept::lineOnThisSide;
    00592       }
    00593       else
    00594       {
    00595         doVerificationScan(intersectionInImage, directionOfLine1, directionOfLine2, lineSize, side1, cameraMatrix, image);
    00596         doVerificationScan(intersectionInImage, directionOfLine1 * -1.0, directionOfLine2, lineSize, side3, cameraMatrix, image);
    00597       }
    00598       if (bothSidesOfLine2)
    00599       {
    00600         side2 = LinesPercept::lineOnThisSide;
    00601         side4 = LinesPercept::lineOnThisSide;
    00602       }
    00603       else
    00604       {
    00605         doVerificationScan(intersectionInImage, directionOfLine2 * -1.0, directionOfLine1, lineSize, side2, cameraMatrix, image);
    00606         doVerificationScan(intersectionInImage, directionOfLine2, directionOfLine1, lineSize, side4, cameraMatrix, image);
    00607       }
    00608     }
    00609     else
    00610     {
    00611
    00612       if (bothSidesOfLine1)
    00613       {
    00614         side2 = LinesPercept::lineOnThisSide;
    00615         side4 = LinesPercept::lineOnThisSide;
    00616       }
    00617       else
    00618       {
    00619         doVerificationScan(intersectionInImage, directionOfLine1 * -1.0, directionOfLine2, lineSize, side2, cameraMatrix, image);
    00620         doVerificationScan(intersectionInImage, directionOfLine1, directionOfLine2, lineSize, side4, cameraMatrix, image);
    00621       }
    00622       if (bothSidesOfLine2)
    00623       {
    00624         side1 = LinesPercept::lineOnThisSide;
    00625         side3 = LinesPercept::lineOnThisSide;
    00626       }
    00627       else
    00628       {
    00629         doVerificationScan(intersectionInImage, directionOfLine2, directionOfLine1, lineSize, side1, cameraMatrix, image);
    00630         doVerificationScan(intersectionInImage, directionOfLine2 * -1.0, directionOfLine1, lineSize, side3, cameraMatrix, image);
    00631       }
    00632     }
    00633
    00634     /*
    00635     // don't take crossings with stupid characteristics
    00636     if (
    00637         (  side1 == LinesPercept::lineOnThisSide
    00638         && side2 == LinesPercept::lineOnThisSide
    00639         && side3 == LinesPercept::lineOnThisSide
    00640         && side4 == LinesPercept::lineOnThisSide)
    00641         ||
    00642         (  side1 == LinesPercept::noLineOnThisSide
    00643         && side2 == LinesPercept::noLineOnThisSide
    00644         && side3 == LinesPercept::noLineOnThisSide
    00645         && side4 == LinesPercept::noLineOnThisSide)
    00646        )
    00647     {
    00648       return;
    00649     }
    00650     */
    00651
    00652     // add percept
    00653     if (linesPercept.numberOfLineCrossings<linesPercept.maxNumberOfLineCrossings)
    00654     {
    00655       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationOnField = intersectionOnField;
    00656       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationInImage = Vector2<int>((int)intersectionInImage.x,(int)intersectionInImage.y);
    00657       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleOnField = angleOnField;
    00658       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage1 = angleOfLine1;
    00659       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage2 = angleOfLine2;
    00660       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side1 = side1;
    00661       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side2 = side2;
    00662       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side3 = side3;
    00663       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side4 = side4;
    00664       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].outOfImage = false;
    00665       linesPercept.numberOfLineCrossings++;
    00666     }
    00667   }
    00668
    00669   // add drawings...
    00670
    00671   COMPLEX_DRAWING( lineCrossingsField,
    00672   {
    00673   Vector2<double> intersectionOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,intersectionOnField.x,intersectionOnField.y);
    00674   CIRCLE(lineCrossingsField,intersectionOnFieldGlobal.x,intersectionOnFieldGlobal.y,60,3,Drawings::ps_solid,Drawings::red);
    00675   //OUTPUT(idText,text,intersectionOnField.x << " " << intersectionOnField.y << " " << intersectionOnFieldGlobal.x << " " << intersectionOnFieldGlobal.y);
    00676   double angleOnFieldGlobal = angleOnField + robotPose.rotation;
    00677
    00678   // draw characteristics of the sides: white->lineOnThisSide, black->noLineOnThisSide, light_gray->dontKnow
    00679   LINE( // side1
    00680     lineCrossingsField,
    00681     intersectionOnFieldGlobal.x,
    00682     intersectionOnFieldGlobal.y,
    00683     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal)),
    00684     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal)),
    00685     5,
    00686     Drawings::ps_solid,
    00687     (side1==LinesPercept::lineOnThisSide) ? Drawings::white : ((side1==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00688   );
    00689   LINE( // side2
    00690     lineCrossingsField,
    00691     intersectionOnFieldGlobal.x,
    00692     intersectionOnFieldGlobal.y,
    00693     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi_2)),
    00694     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi_2)),
    00695     5,
    00696     Drawings::ps_solid,
    00697     (side2==LinesPercept::lineOnThisSide) ? Drawings::white : ((side2==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00698   );
    00699   LINE( // side3
    00700     lineCrossingsField,
    00701     intersectionOnFieldGlobal.x,
    00702     intersectionOnFieldGlobal.y,
    00703     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi)),
    00704     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi)),
    00705     5,
    00706     Drawings::ps_solid,
    00707     (side3==LinesPercept::lineOnThisSide) ? Drawings::white : ((side3==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00708   );
    00709   LINE( // side4
    00710     lineCrossingsField,
    00711     intersectionOnFieldGlobal.x,
    00712     intersectionOnFieldGlobal.y,
    00713     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi3_2)),
    00714     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi3_2)),
    00715     5,
    00716     Drawings::ps_solid,
    00717     (side4==LinesPercept::lineOnThisSide) ? Drawings::white : ((side4==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00718   );
    00719   }
    00720   ); // end complex drawing
    00721
    00722   // new debugging
    00723   NCOMPLEX_DRAWING( "gt05-imageprocessor:lineCrossingsField",
    00724   {
    00725     Vector2<double> intersectionOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,intersectionOnField.x,intersectionOnField.y);
    00726     NCIRCLE("gt05-imageprocessor:lineCrossingsField",intersectionOnFieldGlobal.x,intersectionOnFieldGlobal.y,60,3,Drawings::ps_solid,Drawings::red);
    00727
    00728     double angleOnFieldGlobal = angleOnField + robotPose.rotation;
    00729
    00730   // draw characteristics of the sides: white->lineOnThisSide, black->noLineOnThisSide, light_gray->dontKnow
    00731   NLINE("gt05-imageprocessor:lineCrossingsField", // side1
    00732     intersectionOnFieldGlobal.x,
    00733     intersectionOnFieldGlobal.y,
    00734     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal)),
    00735     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal)),
    00736     5,
    00737     Drawings::ps_solid,
    00738     (side1==LinesPercept::lineOnThisSide) ? Drawings::white : ((side1==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00739   );
    00740   NLINE("gt05-imageprocessor:lineCrossingsField",// side2
    00741     intersectionOnFieldGlobal.x,
    00742     intersectionOnFieldGlobal.y,
    00743     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi_2)),
    00744     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi_2)),
    00745     5,
    00746     Drawings::ps_solid,
    00747     (side2==LinesPercept::lineOnThisSide) ? Drawings::white : ((side2==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00748   );
    00749   NLINE("gt05-imageprocessor:lineCrossingsField",// side3
    00750     intersectionOnFieldGlobal.x,
    00751     intersectionOnFieldGlobal.y,
    00752     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi)),
    00753     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi)),
    00754     5,
    00755     Drawings::ps_solid,
    00756     (side3==LinesPercept::lineOnThisSide) ? Drawings::white : ((side3==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00757   );
    00758   NLINE("gt05-imageprocessor:lineCrossingsField",// side4
    00759     intersectionOnFieldGlobal.x,
    00760     intersectionOnFieldGlobal.y,
    00761     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi3_2)),
    00762     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi3_2)),
    00763     5,
    00764     Drawings::ps_solid,
    00765     (side4==LinesPercept::lineOnThisSide) ? Drawings::white : ((side4==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00766   );
    00767   }
    00768   );// end complex drawing
    00769
    00770 }
    00771
    00772 bool GT2005LineFinder_DeterministicApproach::handleCenterCircle(const Vector2<double> & centerOnField, const CameraMatrix& cameraMatrix, const Image& image)
    00773 {
    00774   int i;
    00775   Vector2<int> pointOnField;
    00776   Vector2<int> center2((int)centerOnField.x,(int)centerOnField.y);
    00777   double distance;
    00778   for (i=0; i<numberOfLinePoints; i++)
    00779   {
    00780     if (Geometry::calculatePointOnField(linePoints[i].pointOnLine.x, linePoints[i].pointOnLine.y, cameraMatrix, image.cameraInfo, pointOnField))
    00781     {
    00782       distance = (pointOnField-center2).abs();
    00783       if ( distance > 80 && distance < 250 )
    00784       {
    00785         linePoints[i].belongsToLineNo = -2;
    00786       }
    00787     }
    00788   }
    00789   return true;
    00790 }
    00791
    00792 bool GT2005LineFinder_DeterministicApproach::handleCenterCircle(const CameraMatrix& cameraMatrix, const Image& image, const RobotPose & robotPose)
    00793 {
    00794   int i,j;
    00795   Vector2<double> temp1,temp2;
    00796   Vector2<int> temp3,temp4;
    00797
    00798   double orientationOfCircleCandidate1 = 0.0;
    00799   //double orientationOfCircleCandidate2 = 0.0;
    00800   bool candidateFound1 = false;
    00801   bool candidateFound2 = false;
    00802
    00803   double distanceToMiddleLine;
    00804   int possibleMiddleLine = 0;
    00805   int numberOfPointsOfPossibleMiddleLine;
    00806   double bestDistanceToPossibleMiddleLine;
    00807
    00808   bool possibleTangent[maxNumberOfLines*2];
    00809   for (i=0; i<numberOfLineFragments; i++)
    00810   {
    00811     possibleTangent[i]=false;
    00812   }
    00813   Geometry::Line linesOnField[maxNumberOfLines*2];
    00814   bool onFieldAvailable[maxNumberOfLines*2];
    00815   Vector2<double> base,direction;
    00816   for (i=0; i<numberOfLineFragments; i++)
    00817   {
    00818     onFieldAvailable[i] = calculateLineOnField(lineFragments[i].base, lineFragments[i].normal, base, direction, cameraMatrix, image);
    00819     linesOnField[i].base = base;
    00820     linesOnField[i].direction = direction.rotateLeft(); // normal (on the field) to line
    00821   }
    00822
    00823
    00824   // first test for a near circle:
    00825   // - lots of tangents are detected, and their normals on the field should cross in the center of the circle
    00826   int numberOfGoodCrossings = 0;
    00827   double deviation1 = 0.0;
    00828   double deviation2 = 0.0;
    00829   Vector2<double> intersection,centerCandidate1,centerCandidate2;
    00830   double distance1,distance2,projection;
    00831   for (i=0; i<numberOfLineFragments; i++)
    00832   {
    00833     if (!onFieldAvailable[i])
    00834     { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
    00835       continue;
    00836     }
    00837     for (j=i+1; j<numberOfLineFragments; j++)
    00838     {
    00839       if (!onFieldAvailable[j])
    00840       { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
    00841         continue;
    00842       }
    00843       Geometry::getIntersectionOfLines(linesOnField[i], linesOnField[j], intersection);
    00844       distance1 = (linesOnField[i].base-intersection).abs();
    00845       distance2 = (linesOnField[j].base-intersection).abs();
    00846
    00847       // In a situation with a real circle, one nearly never get tangents, which are perpendicular,
    00848       // but most likely some on the upper side and some on the lower,
    00849       // both only varying only by up to 50° or something.
    00850       // On the other hand, the goal arena or the outer part of the goal
    00851       // quite often provides lines, that would be nice tangents
    00852       // for a hypothetical center circle, and sometimes these are even
    00853       // verified by some linefragment that could be taken for a middle line. (Look 50 lines below.)
    00854       // BUT in these situations, the "tangents" are perpendicular on the field, and therefore easy to identify.
    00855       projection = fabs( linesOnField[i].direction * linesOnField[j].direction );
    00856
    00857       // perfect match would be both distances equal 180mm (=radius)  (or a little bit less, because actually it's only approximately tangent, and more a secant)
    00858       if ( (distance1 < 260) && (distance1 > 130) && (distance2 < 260) && (distance2 > 130) ) //&& (projection > 0.4) )
    00859       {
    00860         if (projection < 0.4)
    00861         {
    00862           continue;
    00863         }
    00864         deviation1 += fabs(distance1-180) + fabs(distance2-180);
    00865         if (numberOfGoodCrossings>0)
    00866         {
    00867           deviation2 += (centerCandidate1 - intersection).abs();
    00868         }
    00869         centerCandidate1 = (centerCandidate1 * (double)numberOfGoodCrossings + intersection) / (numberOfGoodCrossings+1.0);
    00870         numberOfGoodCrossings++;
    00871         possibleTangent[i]=true;
    00872         possibleTangent[j]=true;
    00873       }
    00874     }
    00875   }
    00876   if (numberOfGoodCrossings>0)
    00877   {
    00878     deviation1 /= numberOfGoodCrossings*2;
    00879     deviation2 /= numberOfGoodCrossings;
    00880   }
    00881
    00882   // was the first search successful?
    00883   if (numberOfGoodCrossings>=1 && deviation1<100 && deviation2<200)
    00884   {
    00885     bestDistanceToPossibleMiddleLine = 10000.0;
    00886     numberOfPointsOfPossibleMiddleLine = 0;
    00887     // verify candidate by checking, if it is crossed by the middle line
    00888     for (i=0; i<numberOfLineFragments; i++)
    00889     {
    00890       if (!onFieldAvailable[i])
    00891       { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
    00892         continue;
    00893       }
    00894       distanceToMiddleLine = fabs( (centerCandidate1-linesOnField[i].base)*linesOnField[i].direction );
    00895       if (distanceToMiddleLine < 60) // is the line very near to the center of the circle? (should be true for at least one line, because the centerline is always detected when the circle is seen)
    00896       {
    00897         // prefere lines with more points, because for these it's more likely to be a middle line
    00898         if (lineFragments[i].numberOfPoints <= numberOfPointsOfPossibleMiddleLine) // || distanceToMiddleLine >= bestDistanceToPossibleMiddleLine
    00899         {
    00900           continue;
    00901         }
    00902         if ( Geometry::calculatePointOnField(lineFragments[i].start.x,lineFragments[i].start.y,cameraMatrix,image.cameraInfo,temp3)
    00903             && Geometry::calculatePointOnField(lineFragments[i].end.x,lineFragments[i].end.y,cameraMatrix,image.cameraInfo,temp4) )
    00904         {
    00905           temp1.x = centerCandidate1.x - (double)temp3.x;
    00906           temp1.y = centerCandidate1.y - (double)temp3.y;
    00907           temp2.x = centerCandidate1.x - (double)temp4.x;
    00908           temp2.y = centerCandidate1.y - (double)temp4.y;
    00909           if (temp1 * temp2 < 0) // is the middle line detected on both sides of the center?
    00910           {
    00911             candidateFound1=true;
    00912             orientationOfCircleCandidate1 = pi_2 + atan2(linesOnField[i].direction.y,linesOnField[i].direction.x);
    00913             bestDistanceToPossibleMiddleLine = distanceToMiddleLine;
    00914             possibleMiddleLine = i;
    00915             numberOfPointsOfPossibleMiddleLine = lineFragments[i].numberOfPoints;
    00916           }
    00917         }
    00918       }
    00919       /*
    00920       else if (distanceToMiddleLine < 180 && lineFragments[i].numberOfPoints <= numberOfPointsOfPossibleMiddleLine) // would not be enough to verify a candidate, but if already verified, then project the canidate to this middle line
    00921       {
    00922         if ( Geometry::calculatePointOnField(lineFragments[i].start.x,lineFragments[i].start.y,cameraMatrix,image.cameraInfo,temp3)
    00923             && Geometry::calculatePointOnField(lineFragments[i].end.x,lineFragments[i].end.y,cameraMatrix,image.cameraInfo,temp4) )
    00924         {
    00925           temp1.x = centerCandidate1.x - (double)temp3.x;
    00926           temp1.y = centerCandidate1.y - (double)temp3.y;
    00927           temp2.x = centerCandidate1.x - (double)temp4.x;
    00928           temp2.y = centerCandidate1.y - (double)temp4.y;
    00929           if (temp1 * temp2 < 0) // is the middle line detected on both sides of the center?
    00930           {
    00931             orientationOfCircleCandidate1 = pi_2 + atan2(linesOnField[i].direction.y,linesOnField[i].direction.x);
    00932             bestDistanceToPossibleMiddleLine = distanceToMiddleLine;
    00933             possibleMiddleLine = i;
    00934             numberOfPointsOfPossibleMiddleLine = lineFragments[i].numberOfPoints;
    00935           }
    00936         }
    00937       }
    00938       */
    00939     }
    00940   }
    00941
    00942
    00943   // project the candidate to the associated middle line
    00944   if (candidateFound1 && bestDistanceToPossibleMiddleLine > 1.0) // to prevent division by zero and to much unnecessary work
    00945   {
    00946     temp1 = centerCandidate1 - linesOnField[possibleMiddleLine].base;
    00947     temp2 = linesOnField[possibleMiddleLine].direction;
    00948     centerCandidate1 -= temp2*(temp1*temp2);
    00949   }
    00950
    00951
    00952
    00953   /*
    00954   // now test for a far away circle:
    00955   // - here 2 parallel tangents can be observed, that should have a distance of 2*radius on the field
    00956   // - the center indicated by these 2 lines should additionally be crossed by another line, the centerline
    00957
    00958   int numberOfGoodPossibleCandidates = 0;
    00959   Vector2<double> possibleCandidate;
    00960   if (!candidateFound1)
    00961   {
    00962     for (i=0; i<numberOfLineFragments; i++)
    00963     {
    00964       if (!onFieldAvailable[i])
    00965       { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
    00966         continue;
    00967       }
    00968       for (j=i+1; j<numberOfLineFragments; j++)
    00969       {
    00970         if (!onFieldAvailable[j])
    00971         { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
    00972           continue;
    00973         }
    00974
    00975         // check if the 2 lines are nearly parallel and exactly 2*radius away from each other
    00976         if ( fabs( linesOnField[i].direction * linesOnField[j].direction ) > 0.9 )
    00977         {
    00978           direction = (linesOnField[i].direction + linesOnField[j].direction).normalize();
    00979           distance1 = (linesOnField[i].base - linesOnField[j].base)*direction;
    00980           if ( (distance1 < 380) && (distance1 > 300) )
    00981           {
    00982             possibleCandidate = (linesOnField[i].base + linesOnField[j].base)/2.0;
    00983             deviation1 += fabs(distance1-360);
    00984             if (numberOfGoodPossibleCandidates>0)
    00985             {
    00986               deviation2 += (centerCandidate2 - possibleCandidate).abs();
    00987             }
    00988             centerCandidate2 = (centerCandidate2 * (double)numberOfGoodPossibleCandidates + possibleCandidate) / (numberOfGoodPossibleCandidates+1.0);
    00989             numberOfGoodPossibleCandidates++;
    00990             possibleTangent[i]=true;
    00991             possibleTangent[j]=true;
    00992           }
    00993
    00994         }
    00995       }
    00996     }
    00997   }
    00998   if (numberOfGoodPossibleCandidates>0)
    00999   {
    01000     deviation1 /= numberOfGoodPossibleCandidates;
    01001     deviation2 /= numberOfGoodPossibleCandidates;
    01002   }
    01003
    01004   // was the second search successful?
    01005   if (numberOfGoodPossibleCandidates>=1 && deviation1<200 && deviation2<300)
    01006   {
    01007     // verify candidate by checking, if it is crossed by the middle line
    01008     for (i=0; i<numberOfLineFragments; i++)
    01009     {
    01010       if (!onFieldAvailable[i])
    01011       { // line could not be calculated (maybe too near to the horizon, so it isn't of interest anyway)
    01012         continue;
    01013       }
    01014       if (fabs( (centerCandidate2-linesOnField[i].base)*linesOnField[i].direction ) < 100) // is the line very near to the center of the circle? (should be true for at least one line, because the centerline is always detected when the circle is seen)
    01015       {
    01016         candidateFound2=true;
    01017         orientationOfCircleCandidate2 = pi_2 + atan2(linesOnField[i].direction.y,linesOnField[i].direction.x);
    01018       }
    01019     }
    01020   }
    01021   */
    01022
    01023
    01024   // exclude the tangents from further calculation of the lines
    01025   Vector2<int> circleCandidateInImage,temp;
    01026   Vector2<double> circleOnFieldGlobal;
    01027   if (candidateFound1)
    01028   {
    01029     for (i=0; i<numberOfLineFragments; i++)
    01030     {
    01031       distance1 = (centerCandidate1-linesOnField[i].base).abs();
    01032       if ( possibleTangent[i] || (distance1<280 && distance1>120) )
    01033       {
    01034         lineFragments[i].lineFragmentAlreadyConsidered = true;
    01035       }
    01036     }
    01037
    01038     // it is still right to exclude tangents, because the next check also filters
    01039     // out correct center circles with the wrong associated middle line.
    01040
    01041     fromOwnHandling.x = (int)centerCandidate1.x;
    01042     fromOwnHandling.y = (int)centerCandidate1.y;
    01043     centerCircleOrientation = orientationOfCircleCandidate1;
    01044
    01045     // one last check if it is really the correct center circle with the correct orientation
    01046     foundByOwnHandling = doVerificationScanForCircle(fromOwnHandling,centerCircleOrientation,cameraMatrix,image);
    01047
    01048     if (foundByOwnHandling)
    01049     {
    01050       double orientationOnField = centerCircleOrientation + robotPose.rotation;
    01051
    01052       circleOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,centerCandidate1.x,centerCandidate1.y);
    01053
    01054       lastSeenCircleGlobal = circleOnFieldGlobal;
    01055       lastSeenCircleOrientationGlobal = orientationOnField;
    01056       framesSinceLastSeen = 0;
    01057
    01058       CIRCLE(circlePoints_Field,circleOnFieldGlobal.x,circleOnFieldGlobal.y,180,3,Drawings::ps_solid,Drawings::skyblue);
    01059       LINE(circlePoints_Field,(circleOnFieldGlobal.x+180*cos(orientationOnField)),(circleOnFieldGlobal.y+180*sin(orientationOnField)),(circleOnFieldGlobal.x-180*cos(orientationOnField)),(circleOnFieldGlobal.y-180*sin(orientationOnField)),3,Drawings::ps_solid,Drawings::skyblue);
    01060       //OUTPUT(idText,text, "MSHLineFinder: " << "centerCandidate1 found!");
    01061       temp.x = (int)centerCandidate1.x;
    01062       temp.y = (int)centerCandidate1.y;
    01063       Geometry::calculatePointInImage(temp,cameraMatrix,image.cameraInfo,circleCandidateInImage);
    01064       CIRCLE(circlePoints_image,circleCandidateInImage.x,circleCandidateInImage.y,3,1,Drawings::ps_solid,Drawings::skyblue);
    01065
    01066       // for validity tests
    01067       //OUTPUT(idText,text, "MSHLineFinder: " <<  robotPose.translation.x << " " << robotPose.translation.y << " " << circleOnFieldGlobal.x << " " << circleOnFieldGlobal.y);
    01068
    01069       // test of orientation validity
    01070       // OUTPUT(idText,text, "MSHLineFinder: " <<  centerCircleOrientation + robotPose.rotation );
    01071     }
    01072
    01073   }
    01074   /*
    01075   else if(false && candidateFound2) // skip (doesn't work good enough, yet)
    01076   {
    01077     for (i=0; i<numberOfLineFragments; i++)
    01078     {
    01079       distance2 = (centerCandidate2-linesOnField[i].base).abs();
    01080       if ( possibleTangent[i] || (distance2<280 && distance2>120) )
    01081       {
    01082         lineFragments[i].lineFragmentAlreadyConsidered = true;
    01083       }
    01084     }
    01085
    01086     fromOwnHandling.x = (int)centerCandidate2.x;
    01087     fromOwnHandling.y = (int)centerCandidate2.y;
    01088     foundByOwnHandling = true;
    01089     centerCircleOrientation = orientationOfCircleCandidate2;
    01090
    01091     circleOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,centerCandidate2.x,centerCandidate2.y);
    01092
    01093     CIRCLE(circlePoints_Field,circleOnFieldGlobal.x,circleOnFieldGlobal.y,180,3,Drawings::ps_solid,Drawings::skyblue);
    01094     //OUTPUT(idText,text, "MSHLineFinder: " << "centerCandidate2 found!");
    01095     temp.x = (int)centerCandidate2.x;
    01096     temp.y = (int)centerCandidate2.y;
    01097     Geometry::calculatePointInImage(temp,cameraMatrix,image.cameraInfo,circleCandidateInImage);
    01098     CIRCLE(circlePoints_image,circleCandidateInImage.x,circleCandidateInImage.y,3,1,Drawings::ps_solid,Drawings::skyblue);
    01099   }
    01100   */
    01101
    01102
    01103   if (!candidateFound1 && framesSinceLastSeen<60)
    01104   {
    01105     Drawings::Color c;
    01106     if (framesSinceLastSeen<2)
    01107     {
    01108       c = Drawings::white;
    01109     }
    01110     else if (framesSinceLastSeen<10)
    01111     {
    01112       c = Drawings::light_gray;
    01113     }
    01114     else if (framesSinceLastSeen<30)
    01115     {
    01116       c = Drawings::gray;
    01117     }
    01118     else
    01119     {
    01120       c = Drawings::dark_gray;
    01121     }
    01122     CIRCLE(circlePoints_Field,lastSeenCircleGlobal.x,lastSeenCircleGlobal.y,180,3,Drawings::ps_solid,c);
    01123     LINE(circlePoints_Field,(lastSeenCircleGlobal.x+180*cos(lastSeenCircleOrientationGlobal)),(lastSeenCircleGlobal.y+180*sin(lastSeenCircleOrientationGlobal)),(lastSeenCircleGlobal.x-180*cos(lastSeenCircleOrientationGlobal)),(lastSeenCircleGlobal.y-180*sin(lastSeenCircleOrientationGlobal)),3,Drawings::ps_solid,c);
    01124
    01125   }
    01126
    01127   // recognition rate test
    01128   // OUTPUT(idText,text, "MSHLineFinder: " <<  (candidateFound1 ? 1 : 0) );
    01129
    01130   return (candidateFound1 || candidateFound2);
    01131 }
    01132
    01133 void GT2005LineFinder_DeterministicApproach::addCenterCirclePercept(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image)
    01134 {
    01135   int i;
    01136   if (foundByCircleFinder)
    01137   {
    01138     // dertermine orientation first
    01139     double orientation = 0.0;
    01140     bool orientationFound = false;
    01141
    01142     Vector2<double> fromCircleFinder2((double)fromCircleFinder.x, (double)fromCircleFinder.y);
    01143
    01144     double distance;
    01145     double smallestDistance = 10000.0;
    01146
    01147     Geometry::Line linesOnField[maxNumberOfLines];
    01148     bool onFieldAvailable[maxNumberOfLines];
    01149     Vector2<double> base,direction;
    01150     for (i=0; i<numberOfLines; i++)
    01151     {
    01152       onFieldAvailable[i] = calculateLineOnField(i, base, direction, cameraMatrix, image);
    01153       linesOnField[i].base = base;
    01154       linesOnField[i].direction = direction.rotateLeft(); // normal (on the field) to line
    01155     }
    01156     for (i=0; i<numberOfLines; i++)
    01157     {
    01158       distance = fabs( (fromCircleFinder2-linesOnField[i].base)*linesOnField[i].direction );
    01159       if (distance < 80 && distance < smallestDistance) // is the line very near to the center of the circle? (should be true for at least one line, because the centerline is always detected when the circle is seen, if it isn't filtered out by accident or if the centerCircle lies at the wrong location (not my fault!))   ;-)
    01160       {
    01161         orientationFound=true;
    01162         orientation = atan2(linesOnField[i].direction.y, linesOnField[i].direction.x);
    01163         orientation += pi_2; // because "direction" was the normal to the line
    01164       }
    01165     }
    01166
    01167     linesPercept.centerCircle.location = fromCircleFinder;
    01168     linesPercept.centerCircle.orientation = orientation;
    01169     linesPercept.centerCircle.orientationKnown = orientationFound;
    01170     linesPercept.centerCircleFound = true;
    01171   }
    01172   else if (foundByOwnHandling)
    01173   {
    01174     linesPercept.centerCircle.location = fromOwnHandling;
    01175     linesPercept.centerCircle.orientation = centerCircleOrientation;
    01176     linesPercept.centerCircle.orientationKnown = true;
    01177     linesPercept.centerCircleFound = true;
    01178   }
    01179 }
    01180
    01181
    01182 int GT2005LineFinder_DeterministicApproach::getNumberOfLines()
    01183 {
    01184   return numberOfLines;
    01185 }
    01186
    01187 void GT2005LineFinder_DeterministicApproach::getLine(int number, Vector2<int> & pointOnLine, Vector2<double> & normalToLine)
    01188 {
    01189   pointOnLine.x = lines[number].base.x;
    01190   pointOnLine.y = lines[number].base.y;
    01191   normalToLine.x = lines[number].normal.x;
    01192   normalToLine.y = lines[number].normal.y;
    01193 }
    01194
    01195
    01196 bool GT2005LineFinder_DeterministicApproach::linesPerpendicularOnField(int lineNumber1, int lineNumber2, const CameraMatrix& cameraMatrix, const Image& image)
    01197 {
    01198   Vector2<double> justAPoint, direction1, direction2;
    01199   if ( calculateLineOnField(lineNumber1, justAPoint, direction1, cameraMatrix, image)
    01200     && calculateLineOnField(lineNumber2, justAPoint, direction2, cameraMatrix, image) )
    01201   {
    01202     if (fabs(direction1*direction2) < 0.5) // directions include an angle of approximately 90° (60°..120°)
    01203     {
    01204       return true;
    01205     }
    01206   }
    01207   return false;
    01208 }
    01209
    01210 bool GT2005LineFinder_DeterministicApproach::calculateLineOnField(int lineNumber, Vector2<double> & base, Vector2<double> & direction, const CameraMatrix& cameraMatrix, const Image& image)
    01211 {
    01212   Vector2<int> pointOnField1, pointOnField2, directionOnField;
    01213   if ( Geometry::calculatePointOnField(lines[lineNumber].base.x, lines[lineNumber].base.y, cameraMatrix, image.cameraInfo, pointOnField1)
    01214     && Geometry::calculatePointOnField((int)(lines[lineNumber].base.x+50*lines[lineNumber].normal.y), (int)(lines[lineNumber].base.y-50*lines[lineNumber].normal.x), cameraMatrix, image.cameraInfo, pointOnField2))
    01215   {
    01216     directionOnField = pointOnField2-pointOnField1;
    01217     base.x = (double)pointOnField1.x;
    01218     base.y = (double)pointOnField1.y;
    01219     direction.x = (double)directionOnField.x;
    01220     direction.y = (double)directionOnField.y;
    01221     direction.normalize();
    01222     return true;
    01223   }
    01224   return false;
    01225 }
    01226
    01227
    01228 bool GT2005LineFinder_DeterministicApproach::calculateLineOnField(const Vector2<int> & baseInImage, const Vector2<double> & normalInImage, Vector2<double> & baseOnField, Vector2<double> & directionOnField, const CameraMatrix& cameraMatrix, const Image& image)
    01229 {
    01230   Vector2<int> pointOnField1, pointOnField2, directionOnFieldTemp;
    01231   if ( Geometry::calculatePointOnField(baseInImage.x, baseInImage.y, cameraMatrix, image.cameraInfo, pointOnField1)
    01232     && Geometry::calculatePointOnField((int)(baseInImage.x+50*normalInImage.y), (int)(baseInImage.y-50*normalInImage.x), cameraMatrix, image.cameraInfo, pointOnField2))
    01233   {
    01234     directionOnFieldTemp = pointOnField2-pointOnField1;
    01235     baseOnField.x = (double)pointOnField1.x;
    01236     baseOnField.y = (double)pointOnField1.y;
    01237     directionOnField.x = (double)directionOnFieldTemp.x;
    01238     directionOnField.y = (double)directionOnFieldTemp.y;
    01239     directionOnField.normalize();
    01240     return true;
    01241   }
    01242   return false;
    01243 }
    01244
    01245 void GT2005LineFinder_DeterministicApproach::doVerificationScan(const Vector2<double> & crossingPoint, const Vector2<double> & directionToScanAt, const Vector2<double> & scanningDirection, int lineSize, LinesPercept::CrossingCharacteristic & result, const CameraMatrix& cameraMatrix, const Image& image)
    01246 {
    01247   // preparations
    01248   int i,j;
    01249   double scanningDistance = max(6.0, lineSize*1.5);
    01250   double projection = fabs( sin( atan2(directionToScanAt.y,directionToScanAt.x) - atan2(scanningDirection.y,scanningDirection.x) ) );
    01251   if (projection > 0) // should never be otherwise
    01252   {
    01253     scanningDistance /= projection;
    01254   }
    01255   double directionAngle = atan2(scanningDirection.y,scanningDirection.x);
    01256
    01257   int numberOfScanLines = 2;
    01258   double distanceBetweenScanLines = 2.0;
    01259   bool greenFound[2]; // numberOfScanLines
    01260   bool whiteFound[2]; // numberOfScanLines
    01261   bool robotColorFound[2]; // numberOfScanLines
    01262   Vector2<int> actual;
    01263   unsigned char y,u,v;
    01264   colorClass color;
    01265   Vector2<double> scanningStart;
    01266   Vector2<int> scanningStart2;
    01267
    01268   for (i=0; i<numberOfScanLines; i++)
    01269   {
    01270     scanningStart = crossingPoint + directionToScanAt * (scanningDistance + i*distanceBetweenScanLines) - scanningDirection * scanningDistance;
    01271     scanningStart2.x = (int)scanningStart.x;
    01272     scanningStart2.y = (int)scanningStart.y;
    01273     BresenhamLineScan bls(scanningStart2, directionAngle, image.cameraInfo);
    01274     actual = scanningStart2;
    01275
    01276     greenFound[i]=false;
    01277     whiteFound[i]=false;
    01278     robotColorFound[i]=false;
    01279
    01280     // begin scanning
    01281     for (j=0; j<= (int)(2.0*scanningDistance); j++)
    01282     {
    01283       if ( actual.x < 0 || actual.y < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y >= image.cameraInfo.resolutionHeight )
    01284       {
    01285         break;
    01286       }
    01287       y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    01288       u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    01289       v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    01290       color = COLOR_CLASS(y, u, v, colorTable);
    01291
    01292       switch (color)
    01293       {
    01294         case green:
    01295           greenFound[i]=true;
    01296           break;
    01297         case white:
    01298           whiteFound[i]=true;
    01299           break;
    01300         case red:
    01301         case blue:
    01302           robotColorFound[i]=true;
    01303         default:
    01304           break;
    01305       }
    01306
    01307       bls.getNext(actual);
    01308     }
    01309     LINE(imageProcessor_lines,scanningStart2.x,scanningStart2.y,actual.x,actual.y,1,Drawings::ps_solid,Drawings::gray);
    01310     NLINE("gt05-imageprocessor:imageProcessor_lines" ,scanningStart2.x,scanningStart2.y,actual.x,actual.y,1,Drawings::ps_solid,Drawings::gray);
    01311
    01312   }
    01313
    01314   // analyse scanning results (perhaps later in a more sophisticated way)
    01315   bool robotInTheWay = false;
    01316   bool greenSeen = false;
    01317   bool whiteSeen = false;
    01318   bool whiteAlwaysSeen = true;
    01319   for (i=0; i<numberOfScanLines; i++)
    01320   {
    01321     robotInTheWay = robotInTheWay || robotColorFound[i];
    01322     greenSeen = greenSeen || greenFound[i];
    01323     whiteSeen = whiteSeen || whiteFound[i];
    01324     whiteAlwaysSeen = whiteAlwaysSeen && whiteFound[i];
    01325   }
    01326   if (robotInTheWay)
    01327   {
    01328     result = LinesPercept::dontKnow;
    01329   }
    01330   else
    01331   {
    01332     if ( lineSize > 4 ) // there should be a quite big line, so most scan lines should have seen white
    01333     {
    01334       if (!whiteAlwaysSeen)
    01335       {
    01336         if (greenSeen)
    01337         {
    01338           result = LinesPercept::noLineOnThisSide;
    01339         }
    01340         else
    01341         {
    01342           result = LinesPercept::dontKnow;
    01343         }
    01344       }
    01345       else // white was seen
    01346       {
    01347         if (greenSeen)
    01348         {
    01349           result = LinesPercept::lineOnThisSide;
    01350         }
    01351         else
    01352         {
    01353           result = LinesPercept::dontKnow;
    01354         }
    01355       }
    01356     }
    01357     else // the line might be quite small, so a scan line might not see white just by chance/bad colortable/blur
    01358     {
    01359       if (!whiteSeen)
    01360       {
    01361         if (greenSeen)
    01362         {
    01363           result = LinesPercept::noLineOnThisSide;
    01364         }
    01365         else
    01366         {
    01367           result = LinesPercept::dontKnow;
    01368         }
    01369       }
    01370       else // white was seen
    01371       {
    01372         if (greenSeen)
    01373         {
    01374           result = LinesPercept::lineOnThisSide;
    01375         }
    01376         else
    01377         {
    01378           result = LinesPercept::dontKnow;
    01379         }
    01380       }
    01381     }
    01382   }
    01383 }
    01384
    01385 bool GT2005LineFinder_DeterministicApproach::doVerificationScanForCircle(
    01386   const Vector2<int> & circleOnFieldRelative,
    01387   double orientation,
    01388   const CameraMatrix& cameraMatrix,
    01389   const Image& image)
    01390 {
    01391   int i,j;
    01392
    01393   const int numberOfScanlines = 4;
    01394
    01395   bool imageBorderFound[numberOfScanlines], circleLineFound[numberOfScanlines], robotColorFound[numberOfScanlines];
    01396
    01397   double scanAngle[numberOfScanlines];
    01398   scanAngle[0] = orientation + 0.33333 * pi;
    01399   scanAngle[1] = orientation + 0.66666 * pi;
    01400   scanAngle[2] = orientation - 0.33333 * pi;
    01401   scanAngle[3] = orientation - 0.66666 * pi;
    01402   Vector2<double> scanDirection[numberOfScanlines];
    01403   Vector2<int> scanningStart[numberOfScanlines],scanningEnd[numberOfScanlines];
    01404   Vector2<int> scanningStartOnField[numberOfScanlines],scanningEndOnField[numberOfScanlines];
    01405   double scanDistance;
    01406
    01407   for (i=0; i<numberOfScanlines; i++)
    01408   {
    01409     imageBorderFound[i] = circleLineFound[i] = robotColorFound[i] = false;
    01410
    01411     scanDirection[i].x = cos(scanAngle[i]);
    01412     scanDirection[i].y = sin(scanAngle[i]);
    01413     scanAngle[i] = normalize(scanAngle[i]);
    01414     if (scanAngle[i] > -1.0 * pi_2 && scanAngle[i] < pi_2)
    01415     {
    01416       scanDistance = 500.0; // otherwise the verification scan is too horizon dependant
    01417     }
    01418     else
    01419     {
    01420       scanDistance = 300.0;
    01421     }
    01422     scanningStartOnField[i].x = int(circleOnFieldRelative.x + 50.0*scanDirection[i].x);
    01423     scanningStartOnField[i].y = int(circleOnFieldRelative.y + 50.0*scanDirection[i].y);
    01424     scanningEndOnField[i].x = int(circleOnFieldRelative.x + scanDistance*scanDirection[i].x);
    01425     scanningEndOnField[i].y = int(circleOnFieldRelative.y + scanDistance*scanDirection[i].y);
    01426     Geometry::calculatePointInImage(scanningStartOnField[i],cameraMatrix,image.cameraInfo,scanningStart[i]);
    01427     Geometry::calculatePointInImage(scanningEndOnField[i],cameraMatrix,image.cameraInfo,scanningEnd[i]);
    01428
    01429     Vector2<int> actual = scanningStart[i];
    01430     unsigned char y,u,v;
    01431     colorClass color;
    01432
    01433     BresenhamLineScan bls(scanningStart[i], scanningEnd[i]);
    01434
    01435     bool firstGreen = false;
    01436     bool whiteFound = false;
    01437
    01438     int pixelsToScan = max(15,bls.numberOfPixels); // better for circles that are not very near
    01439     // begin scanning
    01440     for (j=0; j<=pixelsToScan; j++)
    01441     {
    01442       if ( actual.x < 0 || actual.y < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y >= image.cameraInfo.resolutionHeight )
    01443       {
    01444         imageBorderFound[i] = true;
    01445         break;
    01446       }
    01447       y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    01448       u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    01449       v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    01450       color = COLOR_CLASS(y, u, v, colorTable);
    01451
    01452       switch (color)
    01453       {
    01454         case green:
    01455           if (whiteFound)
    01456           {
    01457             // green again
    01458             circleLineFound[i] = true;
    01459           }
    01460           else
    01461           {
    01462             firstGreen = true;
    01463           }
    01464           break;
    01465         case white:
    01466           if (firstGreen)
    01467           {
    01468             whiteFound=true;
    01469           }
    01470           break;
    01471         case red:
    01472         case blue:
    01473           robotColorFound[i]=true;
    01474         default:
    01475           break;
    01476       }
    01477
    01478       bls.getNext(actual);
    01479     }
    01480     LINE(circlePoints_image,scanningStart[i].x,scanningStart[i].y,actual.x,actual.y,1,Drawings::ps_solid,Drawings::gray);
    01481   }
    01482
    01483   // put the results together
    01484   // (If a scan hit a border, than it is assumed that there may be a circle line,
    01485   // because most false positive situations are resolved with scanning 'up',
    01486   // while many circles are only seen half, and thus scans 'down' will nearly always hit the border of the image.)
    01487   // TODO: Include robotColor somehow...
    01488   bool result =    (imageBorderFound[0] || circleLineFound[0])
    01489                 && (imageBorderFound[1] || circleLineFound[1])
    01490                 && (imageBorderFound[2] || circleLineFound[2])
    01491                 && (imageBorderFound[3] || circleLineFound[3]);
    01492
    01493   //OUTPUT(idText,text, "MSHLineFinder: " << "CircleVerification returns: " << result);
    01494
    01495   if (result)
    01496   {
    01497     circleCount++;
    01498   }
    01499
    01500   return result;
    01501 }
    01502
    01503
    01504