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

# Modules/ImageProcessor/VLCImageProcessor/VLCLineFinder_DeterministicApproach.cpp

Go to the documentation of this file.

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