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

# Modules/ImageProcessor/SlamImageProcessor/SlamBorderFinder.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file SlamBorderFinder.h
    00003 *
    00004 * Implementation of class SlamBorderFinder
    00005 *
    00006 * @author <a href="mailto:stefan.czarnetzki@uni-dortmund.de">Stefan Czarnetzki</a>
    00007 */
    00008
    00009
    00010 #include "SlamBorderFinder.h"
    00011
    00012 SlamBorderFinder::SlamBorderFinder(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
    00020 SlamBorderFinder::~SlamBorderFinder(void)
    00021 {
    00022 }
    00023
    00024 void SlamBorderFinder::reset()
    00025 {
    00026   numberOfLineFragments = 0;
    00027   numberOfLines = 0;
    00028   numberOfLinePoints = 0;
    00029 }
    00030
    00031 void SlamBorderFinder::considerLinePoint(Vector2<int> & pointOnLine, Vector2<double> & normalToLine)
    00032 {
    00033   // just store the point, if there is still a free space in the array
    00034   if (numberOfLinePoints < maxNumberOfLinePoints)
    00035   {
    00036     linePoints[numberOfLinePoints].pointOnLine = pointOnLine;
    00037     linePoints[numberOfLinePoints].normalToLine = normalToLine;
    00038     linePoints[numberOfLinePoints].belongsToLineNo = -1;
    00039     numberOfLinePoints++;
    00040   }
    00041   else
    00042   {
    00043     //OUTPUT(idText,text,"LineFinder: linePoint drop (raise maxNumberOfLinePoints)");
    00044   }
    00045 }
    00046
    00047 void SlamBorderFinder::execute(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo, const RobotPose & robotPose)
    00048 {
    00049   NDECLARE_DEBUGDRAWING ("SlamIMP:borderDirection", "drawingOnField", "shows the direction away from the border");
    00050
    00051
    00052   lines = new LineFragment[maxNumberOfLines];
    00053   lineFragments = new LineFragment[maxNumberOfLines*2];
    00054
    00055   findLineFragments();
    00056   findLines(linesPercept, cameraMatrix, image, imageInfo);
    00057   findIntersections(linesPercept, cameraMatrix, image, robotPose, imageInfo);
    00058   calculateDirectionAwayFromBorder(linesPercept,image,imageInfo,cameraMatrix,robotPose);
    00059
    00060   delete[] lines;
    00061   delete[] lineFragments;
    00062
    00063   DEBUG_DRAWING_FINISHED(lineCrossingsField);
    00064   DEBUG_DRAWING_FINISHED(imageProcessor_lines);
    00065   DEBUG_DRAWING_FINISHED(imageProcessor_linefragments);
    00066 }
    00067
    00068 void SlamBorderFinder::findLineFragments()
    00069 {
    00070   int i, j;
    00071   Vector2<double> temp;
    00072
    00073   // for all lines, calculate how many other lines are similar/near in sense of this special norm
    00074   bool similar[maxNumberOfLinePoints][maxNumberOfLinePoints];
    00075   for(i = 0; i < numberOfLinePoints; i++)
    00076   {
    00077     similar[i][i] = true;
    00078     for(j = i + 1; j < numberOfLinePoints; j++)
    00079     {
    00080       // similar/near if distance is small and normal is in the same direction
    00081       bool sim = false;
    00082       double projection = linePoints[i].normalToLine * linePoints[j].normalToLine; // projection := cos(angle)
    00083       if(projection > normProjection)
    00084       {
    00085         temp.x = (double)(linePoints[j].pointOnLine.x - linePoints[i].pointOnLine.x);
    00086         temp.y = (double)(linePoints[j].pointOnLine.y - linePoints[i].pointOnLine.y);
    00087         double distance1 = fabs(temp * linePoints[i].normalToLine);
    00088         if(distance1 < normDistance)
    00089         {
    00090           double distance2 = fabs(temp * linePoints[j].normalToLine);
    00091           sim = (distance2 < normDistance);
    00092         }
    00093       }
    00094       similar[i][j] = sim;
    00095       similar[j][i] = sim;
    00096     }
    00097   }
    00098
    00099   // analyse detected points and extract line fragments
    00100   int maxWeight;
    00101   int linePointWithHighestWeight;
    00102
    00103
    00104   for(int n=0; n<maxNumberOfLines*2; n++)
    00105   {
    00106     maxWeight = 0;
    00107     linePointWithHighestWeight = -1;
    00108
    00109     // find point with most similar points
    00110     int simCount[maxNumberOfLinePoints];
    00111     for(i=0; i<numberOfLinePoints; i++)
    00112     {
    00113       simCount[i]=1; // the point itself
    00114     }
    00115     for(i=0; i < numberOfLinePoints; i++)
    00116     {
    00117       if(linePoints[i].belongsToLineNo == -1) // only if point is not yet matched to a line fragment
    00118       {
    00119         for (j=i+1; j < numberOfLinePoints; j++)
    00120         {
    00121           if(linePoints[j].belongsToLineNo == -1) // only if point is not yet matched to a line fragment
    00122           {
    00123             if(similar[i][j])
    00124             {
    00125               // weight added to both points because of reflexivity
    00126               simCount[i]++;
    00127               simCount[j]++;
    00128
    00129               if(simCount[i] > maxWeight)
    00130               {
    00131                 maxWeight = simCount[i];
    00132                 linePointWithHighestWeight = i;
    00133               }
    00134               if(simCount[j] > maxWeight)
    00135               {
    00136                 maxWeight = simCount[j];
    00137                 linePointWithHighestWeight = j;
    00138               }
    00139             }
    00140           }
    00141         }
    00142       }
    00143     }
    00144     if (maxWeight<pointsNeededForLine)
    00145     {
    00146       break;
    00147     }
    00148
    00149     // build line from point with highest weight
    00150     Vector2<int> start(300,300);
    00151     Vector2<int> end(-1,-1);
    00152     // 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)
    00153     double angle = atan2(linePoints[linePointWithHighestWeight].normalToLine.y,linePoints[linePointWithHighestWeight].normalToLine.x);
    00154     if (angle <= pi_4 && angle >= -1*pi_4) // line is vertical, select start and end by y-Value
    00155     {
    00156       for (i=0; i<numberOfLinePoints; i++)
    00157       {
    00158         if (similar[i][linePointWithHighestWeight] && linePoints[i].belongsToLineNo==-1)
    00159         {
    00160           linePoints[i].belongsToLineNo=numberOfLineFragments;
    00161           if (linePoints[i].pointOnLine.y < start.y)
    00162           {
    00163             start = linePoints[i].pointOnLine;
    00164           }
    00165           if (linePoints[i].pointOnLine.y > end.y)
    00166           {
    00167             end = linePoints[i].pointOnLine;
    00168           }
    00169         }
    00170       }
    00171     }
    00172     else  // line is horizontal, select start and end by x-Value
    00173     {
    00174       for (i=0; i<numberOfLinePoints; i++)
    00175       {
    00176         if (similar[i][linePointWithHighestWeight] && linePoints[i].belongsToLineNo==-1)
    00177         {
    00178           linePoints[i].belongsToLineNo=numberOfLineFragments;
    00179           if (linePoints[i].pointOnLine.x < start.x)
    00180           {
    00181             start = linePoints[i].pointOnLine;
    00182           }
    00183           if (linePoints[i].pointOnLine.x > end.x)
    00184           {
    00185             end = linePoints[i].pointOnLine;
    00186           }
    00187         }
    00188       }
    00189     }
    00190
    00191     // for fragments with only few points, remove the long ones, because they may be caused by random noise linePoints
    00192     if (maxWeight==pointsNeededForLine)
    00193     {
    00194       Vector2<int> length = end - start;
    00195       if ( length.abs() > 70 )
    00196       { // skip this fragment and mark points as "are not and will not be used"
    00197         for (i=0; i<numberOfLinePoints; i++)
    00198         {
    00199           if (linePoints[i].belongsToLineNo==numberOfLineFragments)
    00200           {
    00201             linePoints[i].belongsToLineNo=-2;
    00202           }
    00203         }
    00204         continue;
    00205       }
    00206     }
    00207
    00208     // accept the line fragment
    00209     Vector2<double> normalToLine((double)(end.x-start.x), (double)(end.y-start.y));
    00210     normalToLine.rotateLeft();
    00211     // let the normal point in the same direction like all the normals of the points
    00212     if (normalToLine*linePoints[linePointWithHighestWeight].normalToLine < 0)
    00213     {
    00214       normalToLine *= -1;
    00215     }
    00216     normalToLine.normalize();
    00217     Vector2<int> middle((start.x+end.x)/2, (start.y+end.y)/2);
    00218     lineFragments[numberOfLineFragments].base   = middle;
    00219     lineFragments[numberOfLineFragments].normal = normalToLine;
    00220     lineFragments[numberOfLineFragments].start  = start;
    00221     lineFragments[numberOfLineFragments].end    = end;
    00222     lineFragments[numberOfLineFragments].lineFragmentAlreadyConsidered = false;
    00223     lineFragments[numberOfLineFragments].averageStep = (end-start).abs() / maxWeight;
    00224     //LINE(imageProcessor_linefragments,start.x,start.y,end.x,end.y,1,1,numberOfLineFragments);
    00225     numberOfLineFragments++;
    00226   }
    00227 }
    00228
    00229 void SlamBorderFinder::findLines(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const ImageInfo& imageInfo)
    00230 {
    00231   int i, j;
    00232   Vector2<double> temp, temp2;
    00233   Vector2<double> horizonDirection = imageInfo.horizon.direction;
    00234   horizonDirection.normalize();
    00235
    00236
    00237   // if both sides or more than one fragment of a field line are detected, then filter these out and take their middle instead
    00238   for (i=0; i<numberOfLineFragments && numberOfLines<maxNumberOfLines; i++)
    00239   {
    00240     if (lineFragments[i].lineFragmentAlreadyConsidered)
    00241     {
    00242       continue;
    00243     }
    00244     lines[numberOfLines].base = lineFragments[i].base;
    00245     lines[numberOfLines].normal = lineFragments[i].normal;
    00246     lines[numberOfLines].start = lineFragments[i].start;
    00247     lines[numberOfLines].end = lineFragments[i].end;
    00248     lines[numberOfLines].averageStep = lineFragments[i].averageStep;
    00249     int numberOfLineFragmentsUsed = 1;
    00250     lineFragments[i].lineFragmentAlreadyConsidered = true;
    00251
    00252     double angle = atan2(lines[numberOfLines].normal.y,lines[numberOfLines].normal.x);
    00253     bool sortByY = angle <= pi_4 && angle >= -1*pi_4;
    00254
    00255     for (j=i+1; j<numberOfLineFragments; j++)
    00256     {
    00257       if (lineFragments[j].lineFragmentAlreadyConsidered)
    00258       {
    00259         continue;
    00260       }
    00261       double p = lineFragments[i].normal * lineFragments[j].normal;
    00262       Vector2<double> nij;
    00263       if (p>0)
    00264       {
    00265         nij = lineFragments[i].normal + lineFragments[j].normal;
    00266       }
    00267       else
    00268       {
    00269         nij = lineFragments[i].normal - lineFragments[j].normal;
    00270       }
    00271       nij.normalize();
    00272       bool perpendicularToHorizon = fabs(horizonDirection * nij) > 0.93;
    00273
    00274       if (     (perpendicularToHorizon || p>normProjection || p<-1*normProjection) // if not perpendicular, than decide using normProjection
    00275             && (!perpendicularToHorizon || p>normProjectionPerpendicularToHorizon || p<-1*normProjectionPerpendicularToHorizon)) // else, use normProjectionPerpendicularToHorizon
    00276       {
    00277         temp.x = (double)(lineFragments[i].base.x-lineFragments[j].base.x);
    00278         temp.y = (double)(lineFragments[i].base.y-lineFragments[j].base.y);
    00279         int lineHeight = max( Geometry::calculateLineSize(lineFragments[i].base, cameraMatrix, image.cameraInfo),
    00280                               Geometry::calculateLineSize(lineFragments[j].base, cameraMatrix, image.cameraInfo));
    00281         lineHeight = max( lineHeight, 2 );
    00282
    00283         if ( (fabs(temp * lineFragments[i].normal) < 1.5 * lineHeight) && (fabs(temp * lineFragments[j].normal) < 1.5 * lineHeight) )
    00284         {
    00285
    00286           if (p<0) // wrong orientation to sum it up, so invert it
    00287           {
    00288             lineFragments[j].normal *= -1.0;
    00289           }
    00290           lines[numberOfLines].base    += lineFragments[j].base;
    00291           lines[numberOfLines].normal  += lineFragments[j].normal;
    00292           lines[numberOfLines].averageStep += lineFragments[i].averageStep;
    00293
    00294           if (sortByY) // line is vertical, select start and end by y-Value
    00295           {
    00296             if (lineFragments[j].start.y < lines[numberOfLines].start.y)
    00297             {
    00298               lines[numberOfLines].start = lineFragments[j].start;
    00299             }
    00300             if (lineFragments[j].end.y > lines[numberOfLines].end.y)
    00301             {
    00302               lines[numberOfLines].end = lineFragments[j].end;
    00303             }
    00304           }
    00305           else  // line is horizontal, select start and end by x-Value
    00306           {
    00307             if (lineFragments[j].start.x < lines[numberOfLines].start.x)
    00308             {
    00309               lines[numberOfLines].start = lineFragments[j].start;
    00310             }
    00311             if (lineFragments[j].end.x > lines[numberOfLines].end.x)
    00312             {
    00313               lines[numberOfLines].end = lineFragments[j].end;
    00314             }
    00315           }
    00316
    00317           numberOfLineFragmentsUsed++;
    00318           lineFragments[j].lineFragmentAlreadyConsidered = true;
    00319         }
    00320       }
    00321     }
    00322     // the resulting line is a middle of its fragments
    00323     lines[numberOfLines].base /= numberOfLineFragmentsUsed;
    00324     lines[numberOfLines].averageStep /= numberOfLineFragmentsUsed;
    00325     lines[numberOfLines].normal.normalize();
    00326
    00327     numberOfLines++;
    00328   }
    00329
    00330   /*
    00331   // create new fieldLine-percepts based on the found lines (if at least one line was found)
    00332   double angle;
    00333   Vector2<int> temp3;
    00334   if (numberOfLines > 0)
    00335   {
    00336     linesPercept.numberOfPoints[LinesPercept::field] = 0;
    00337     for (i=0; i<numberOfLines; i++)
    00338     {
    00339       calculateLineOnField(i,temp,temp2,cameraMatrix,image);
    00340       angle = normalize(temp2.angle() + pi_2);
    00341       // the following if's will always work, because all these points are already evaluated to lie on the field, but better save than sorry
    00342       if (Geometry::calculatePointOnField(lines[i].base.x,lines[i].base.y,cameraMatrix,image.cameraInfo,temp3))
    00343       {
    00344         linesPercept.add(LinesPercept::field,temp3,angle);
    00345       }
    00346       if (Geometry::calculatePointOnField(lines[i].start.x,lines[i].start.y,cameraMatrix,image.cameraInfo,temp3))
    00347       {
    00348         linesPercept.add(LinesPercept::field,temp3,angle);
    00349       }
    00350       if (Geometry::calculatePointOnField(lines[i].end.x,lines[i].end.y,cameraMatrix,image.cameraInfo,temp3))
    00351       {
    00352         linesPercept.add(LinesPercept::field,temp3,angle);
    00353       }
    00354     }
    00355   }
    00356   */
    00357
    00358   // draw all lines
    00359   Vector2<int> r1(0,0);
    00360   Vector2<int> r2(image.cameraInfo.resolutionWidth-1,image.cameraInfo.resolutionHeight-1);
    00361   for (i=0; i<numberOfLines; i++)
    00362   {
    00363     // in the image
    00364     Vector2<int> pointOnLine;
    00365     Vector2<double> direction;
    00366     getLine(i, pointOnLine, direction);
    00367     direction.rotateLeft();
    00368     Geometry::Line a(pointOnLine, direction);
    00369     Vector2<int> point1, point2;
    00370     Geometry::getIntersectionPointsOfLineAndRectangle(r1,r2,a,point1,point2);
    00371     /*LINE(
    00372       imageProcessor_lines,
    00373       point1.x,
    00374       point1.y,
    00375       point2.x,
    00376       point2.y,
    00377       1,
    00378       Drawings::ps_solid,
    00379       Drawings::blue
    00380       );*/
    00381   }
    00382 }
    00383
    00384 void SlamBorderFinder::findIntersections(LinesPercept & linesPercept, const CameraMatrix& cameraMatrix, const Image& image, const RobotPose & robotPose, const ImageInfo& imageInfo)
    00385 {
    00386   int i,j;
    00387   // calculate line crossings
    00388   Geometry::Line lines2[maxNumberOfLines];
    00389   Vector2<double> intersection;
    00390   Vector2<int> intersectionOnField;
    00391   for (i=0; i<numberOfLines; i++)
    00392   {
    00393     Vector2<double> direction(lines[i].normal);
    00394     direction.rotateLeft();
    00395     direction.normalize();
    00396     lines2[i] = Geometry::Line(lines[i].base,direction);
    00397   }
    00398   for (i=0; i<numberOfLines; i++)
    00399   {
    00400     for (j=i+1; j<numberOfLines; j++)
    00401     {
    00402       if (linesPerpendicularOnField(i, j, cameraMatrix, image))
    00403       {
    00404         Geometry::getIntersectionOfLines(lines2[i],lines2[j],intersection);
    00405         if (Geometry::calculatePointOnField((int)intersection.x,(int)intersection.y,cameraMatrix,image.cameraInfo,intersectionOnField))
    00406         {
    00407           if (intersectionOnField.abs() < 3600)
    00408           {
    00409             //addCrossingsPercept(intersection, intersectionOnField, i, j, linesPercept, image, imageInfo, cameraMatrix, robotPose);
    00410           }
    00411         }
    00412       }
    00413     }
    00414   }
    00415
    00416   // draw lines on field
    00417   for (i=0; i<numberOfLines; i++)
    00418   {
    00419     Vector2<int> pointOnField1, pointOnField2, directionOnField;
    00420     Geometry::calculatePointOnField(lines[i].base.x, lines[i].base.y, cameraMatrix, image.cameraInfo, pointOnField1);
    00421     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);
    00422     directionOnField = pointOnField2-pointOnField1;
    00423     pointOnField1 = pointOnField1 - directionOnField * 10;
    00424     pointOnField2 = pointOnField2 + directionOnField * 10;
    00425     Vector2<double> point1OnFieldGlobal, point2OnFieldGlobal;
    00426     point1OnFieldGlobal = Geometry::relative2FieldCoord(robotPose, (double)pointOnField1.x, (double)pointOnField1.y);
    00427     point2OnFieldGlobal = Geometry::relative2FieldCoord(robotPose, (double)pointOnField2.x, (double)pointOnField2.y);
    00428     //LINE(lineCrossingsField, point1OnFieldGlobal.x, point1OnFieldGlobal.y, point2OnFieldGlobal.x, point2OnFieldGlobal.y, 3, Drawings::ps_solid, Drawings::blue);
    00429   }
    00430 }
    00431
    00432 void SlamBorderFinder::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)
    00433 {
    00434   // prepare stuff and calculate bisection on field
    00435   Vector2<double> temp1,temp2, directionOnField1,directionOnField2;
    00436   bool firstLineIsLeft;
    00437   Vector2<double> directionOfLine1 = lines[lineNumber1].normal;
    00438   directionOfLine1.rotateRight();
    00439   Vector2<double> directionOfLine2 = lines[lineNumber2].normal;
    00440   directionOfLine2.rotateRight();
    00441   if (  !calculateLineOnField(lineNumber1,temp1,directionOnField1,cameraMatrix,image)
    00442     ||  !calculateLineOnField(lineNumber2,temp1,directionOnField2,cameraMatrix,image) )
    00443   {
    00444     // something went wrong and the calculation failed
    00445     return;
    00446   }
    00447   Vector2<double> bisectionOnField = directionOnField1 + directionOnField2;
    00448   bisectionOnField.normalize();
    00449   double angleOnField = atan2(bisectionOnField.y,bisectionOnField.x) + pi_4; // this is more stable than just taking the angle of one of the lines
    00450
    00451
    00452   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
    00453   angleBisection.normalize();
    00454   // decide, which line is lying on which side of the bisection
    00455   double angleOfBisection = atan2(angleBisection.y,angleBisection.x);
    00456   double angleOfLine1 = atan2(directionOfLine1.y,directionOfLine1.x);
    00457   double angleOfLine2 = atan2(directionOfLine2.y,directionOfLine2.x);
    00458   // 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
    00459   if ( ( angleOfLine1 < angleOfBisection + pi_2 && angleOfLine1 > angleOfBisection )
    00460     || ( angleOfLine1 + pi2 < angleOfBisection + pi_2 && angleOfLine1 + pi2 > angleOfBisection )
    00461     || ( angleOfLine1 - pi2 < angleOfBisection + pi_2 && angleOfLine1 - pi2 > angleOfBisection ) )
    00462   { // line1 lies direcly right of the bisection
    00463     firstLineIsLeft=false;
    00464   }
    00465   else
    00466   {
    00467     firstLineIsLeft=true;
    00468   }
    00469
    00470
    00471
    00472   bool bothSidesOfLine1,bothSidesOfLine2;
    00473
    00474   temp1.x = (double)lines[lineNumber1].start.x - intersectionInImage.x;
    00475   temp1.y = (double)lines[lineNumber1].start.y - intersectionInImage.y;
    00476   temp2.x = (double)lines[lineNumber1].end.x - intersectionInImage.x;
    00477   temp2.y = (double)lines[lineNumber1].end.y - intersectionInImage.y;
    00478   if ( temp1*temp2 < 0 && temp1.abs() > 10 && temp2.abs() > 10 ) // points lie on different sides and with enough distance to the crossing point
    00479   {
    00480     bothSidesOfLine1=true;
    00481   }
    00482   else
    00483   {
    00484     bothSidesOfLine1=false; // or at least not decidable without additional scanning
    00485   }
    00486
    00487   temp1.x = (double)lines[lineNumber2].start.x - intersectionInImage.x;
    00488   temp1.y = (double)lines[lineNumber2].start.y - intersectionInImage.y;
    00489   temp2.x = (double)lines[lineNumber2].end.x - intersectionInImage.x;
    00490   temp2.y = (double)lines[lineNumber2].end.y - intersectionInImage.y;
    00491   if ( temp1*temp2 < 0 && temp1.abs() > 10 && temp2.abs() > 10 ) // points lie on different sides and with enough distance to the crossing point
    00492   {
    00493     bothSidesOfLine2=true;
    00494   }
    00495   else
    00496   {
    00497     bothSidesOfLine2=false; // or at least not decidable without additional scanning
    00498   }
    00499
    00500
    00501   LinesPercept::CrossingCharacteristic side1 = LinesPercept::dontKnow;
    00502   LinesPercept::CrossingCharacteristic side2 = LinesPercept::dontKnow;
    00503   LinesPercept::CrossingCharacteristic side3 = LinesPercept::dontKnow;
    00504   LinesPercept::CrossingCharacteristic side4 = LinesPercept::dontKnow;
    00505
    00506   if ( intersectionInImage.x < 0 || intersectionInImage.y < 0 || intersectionInImage.x >= image.cameraInfo.resolutionWidth || intersectionInImage.y >= image.cameraInfo.resolutionHeight )
    00507   { // intersection is outside the image
    00508     if (linesPercept.numberOfLineCrossings<linesPercept.maxNumberOfLineCrossings)
    00509     {
    00510       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationOnField = intersectionOnField;
    00511       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationInImage = Vector2<int>((int)intersectionInImage.x,(int)intersectionInImage.y);
    00512       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleOnField = angleOnField;
    00513       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage1 = angleOfLine1;
    00514       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage2 = angleOfLine2;
    00515       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side1 = side1;
    00516       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side2 = side2;
    00517       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side3 = side3;
    00518       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side4 = side4;
    00519       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].outOfImage = true;
    00520       linesPercept.numberOfLineCrossings++;
    00521     }
    00522   }
    00523   else
    00524   { // intersection is inside the image
    00525
    00526     // do some additional scanning to get characteristics of this crossing...
    00527     Vector2<int> temp((int)intersectionInImage.x,(int)intersectionInImage.y);
    00528     int lineSize = Geometry::calculateLineSize(temp, cameraMatrix, image.cameraInfo);
    00529
    00530     if (firstLineIsLeft)
    00531     {
    00532       if (bothSidesOfLine1)
    00533       {
    00534         side1 = LinesPercept::lineOnThisSide;
    00535         side3 = LinesPercept::lineOnThisSide;
    00536       }
    00537       else
    00538       {
    00539         doVerificationScan(intersectionInImage, directionOfLine1, directionOfLine2, lineSize, side1, cameraMatrix, image);
    00540         doVerificationScan(intersectionInImage, directionOfLine1 * -1.0, directionOfLine2, lineSize, side3, cameraMatrix, image);
    00541       }
    00542       if (bothSidesOfLine2)
    00543       {
    00544         side2 = LinesPercept::lineOnThisSide;
    00545         side4 = LinesPercept::lineOnThisSide;
    00546       }
    00547       else
    00548       {
    00549         doVerificationScan(intersectionInImage, directionOfLine2 * -1.0, directionOfLine1, lineSize, side2, cameraMatrix, image);
    00550         doVerificationScan(intersectionInImage, directionOfLine2, directionOfLine1, lineSize, side4, cameraMatrix, image);
    00551       }
    00552     }
    00553     else
    00554     {
    00555
    00556       if (bothSidesOfLine1)
    00557       {
    00558         side2 = LinesPercept::lineOnThisSide;
    00559         side4 = LinesPercept::lineOnThisSide;
    00560       }
    00561       else
    00562       {
    00563         doVerificationScan(intersectionInImage, directionOfLine1 * -1.0, directionOfLine2, lineSize, side2, cameraMatrix, image);
    00564         doVerificationScan(intersectionInImage, directionOfLine1, directionOfLine2, lineSize, side4, cameraMatrix, image);
    00565       }
    00566       if (bothSidesOfLine2)
    00567       {
    00568         side1 = LinesPercept::lineOnThisSide;
    00569         side3 = LinesPercept::lineOnThisSide;
    00570       }
    00571       else
    00572       {
    00573         doVerificationScan(intersectionInImage, directionOfLine2, directionOfLine1, lineSize, side1, cameraMatrix, image);
    00574         doVerificationScan(intersectionInImage, directionOfLine2 * -1.0, directionOfLine1, lineSize, side3, cameraMatrix, image);
    00575       }
    00576     }
    00577
    00578     /*
    00579     // don't take crossings with stupid characteristics
    00580     if (
    00581         (  side1 == LinesPercept::lineOnThisSide
    00582         && side2 == LinesPercept::lineOnThisSide
    00583         && side3 == LinesPercept::lineOnThisSide
    00584         && side4 == LinesPercept::lineOnThisSide)
    00585         ||
    00586         (  side1 == LinesPercept::noLineOnThisSide
    00587         && side2 == LinesPercept::noLineOnThisSide
    00588         && side3 == LinesPercept::noLineOnThisSide
    00589         && side4 == LinesPercept::noLineOnThisSide)
    00590        )
    00591     {
    00592       return;
    00593     }
    00594     */
    00595
    00596     // add percept
    00597     if (linesPercept.numberOfLineCrossings<linesPercept.maxNumberOfLineCrossings)
    00598     {
    00599       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationOnField = intersectionOnField;
    00600       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].locationInImage = Vector2<int>((int)intersectionInImage.x,(int)intersectionInImage.y);
    00601       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleOnField = angleOnField;
    00602       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage1 = angleOfLine1;
    00603       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].angleInImage2 = angleOfLine2;
    00604       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side1 = side1;
    00605       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side2 = side2;
    00606       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side3 = side3;
    00607       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].side4 = side4;
    00608       linesPercept.lineCrossings[linesPercept.numberOfLineCrossings].outOfImage = false;
    00609       linesPercept.numberOfLineCrossings++;
    00610     }
    00611   }
    00612
    00613   // add drawings...
    00614 #ifndef NODEBUGDRAWINGS
    00615   Vector2<double> intersectionOnFieldGlobal = Geometry::relative2FieldCoord(robotPose,intersectionOnField.x,intersectionOnField.y);
    00616   //CIRCLE(lineCrossingsField,intersectionOnFieldGlobal.x,intersectionOnFieldGlobal.y,60,3,Drawings::ps_solid,Drawings::red);
    00617   //OUTPUT(idText,text,intersectionOnField.x << " " << intersectionOnField.y << " " << intersectionOnFieldGlobal.x << " " << intersectionOnFieldGlobal.y);
    00618   //double angleOnFieldGlobal = angleOnField + robotPose.rotation;
    00619
    00620   // draw characteristics of the sides: white->lineOnThisSide, black->noLineOnThisSide, light_gray->dontKnow
    00621   /*
    00622   LINE( // side1
    00623     lineCrossingsField,
    00624     intersectionOnFieldGlobal.x,
    00625     intersectionOnFieldGlobal.y,
    00626     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal)),
    00627     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal)),
    00628     5,
    00629     Drawings::ps_solid,
    00630     (side1==LinesPercept::lineOnThisSide) ? Drawings::white : ((side1==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00631   );
    00632   LINE( // side2
    00633     lineCrossingsField,
    00634     intersectionOnFieldGlobal.x,
    00635     intersectionOnFieldGlobal.y,
    00636     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi_2)),
    00637     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi_2)),
    00638     5,
    00639     Drawings::ps_solid,
    00640     (side2==LinesPercept::lineOnThisSide) ? Drawings::white : ((side2==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00641   );
    00642   LINE( // side3
    00643     lineCrossingsField,
    00644     intersectionOnFieldGlobal.x,
    00645     intersectionOnFieldGlobal.y,
    00646     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi)),
    00647     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi)),
    00648     5,
    00649     Drawings::ps_solid,
    00650     (side3==LinesPercept::lineOnThisSide) ? Drawings::white : ((side3==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00651   );
    00652   LINE( // side4
    00653     lineCrossingsField,
    00654     intersectionOnFieldGlobal.x,
    00655     intersectionOnFieldGlobal.y,
    00656     intersectionOnFieldGlobal.x + (int)(200.0*cos(angleOnFieldGlobal+pi3_2)),
    00657     intersectionOnFieldGlobal.y + (int)(200.0*sin(angleOnFieldGlobal+pi3_2)),
    00658     5,
    00659     Drawings::ps_solid,
    00660     (side4==LinesPercept::lineOnThisSide) ? Drawings::white : ((side4==LinesPercept::noLineOnThisSide) ? Drawings::black : Drawings::light_gray)
    00661   );
    00662   */
    00663 #endif
    00664   // end of drawing
    00665 }
    00666
    00667 void SlamBorderFinder::calculateDirectionAwayFromBorder(
    00668     LinesPercept & linesPercept,
    00669     const Image& image,
    00670     const ImageInfo& imageInfo,
    00671     const CameraMatrix & cameraMatrix,
    00672     const RobotPose & robotPose)
    00673 {
    00674
    00675
    00676   int i;
    00677   Vector2<double> temp1,temp2;
    00678
    00679   // calculate it from the points directly
    00680   Vector2<double> fromPoints;
    00681   Vector2<double> nearestPoint;
    00682   double nearestDistance = 100000;
    00683   for (i=0; i<numberOfLinePoints; i++)
    00684   {
    00685     if (calculateLineOnField(linePoints[i].pointOnLine,linePoints[i].normalToLine,temp1,temp2,cameraMatrix,image))
    00686     {
    00687       fromPoints += temp2;
    00688       double distance = temp1.abs();
    00689       if (distance<nearestDistance)
    00690       {
    00691         nearestDistance=distance;
    00692         nearestPoint = temp1;
    00693       }
    00694     }
    00695   }
    00696   nearestPoint = Geometry::relative2FieldCoord(robotPose,nearestPoint.x,nearestPoint.y);
    00697   fromPoints.rotateRight();
    00698   fromPoints.normalize();
    00699   NARROW(
    00700     "SlamIMP:borderDirection",
    00701     nearestPoint.x,
    00702     nearestPoint.y,
    00703     (nearestPoint.x + 300*fromPoints.x),
    00704     (nearestPoint.y + 300*fromPoints.y),
    00705     20,
    00706     Drawings::ps_solid,
    00707     Drawings::light_gray
    00708   );
    00709   NARROW(
    00710     "SlamIMP:borderDirection",
    00711   robotPose.translation.x,
    00712     robotPose.translation.y,
    00713     (robotPose.translation.x + 300*fromPoints.x),
    00714     (robotPose.translation.y + 300*fromPoints.y),
    00715     20,
    00716     Drawings::ps_solid,
    00717     Drawings::light_gray
    00718   );
    00719
    00720   if (numberOfLineFragments>0)
    00721   {
    00722   // calculate it from the fragments
    00723   Vector2<double> fromFragments;
    00724   Vector2<double> nearestFragment;
    00725   Vector2<double> nearestFragmentsNormal;
    00726   nearestDistance = 100000;
    00727   for (i=0; i<numberOfLineFragments; i++)
    00728   {
    00729     if (calculateLineOnField(lineFragments[i].base,lineFragments[i].normal,temp1,temp2,cameraMatrix,image))
    00730     {
    00731     fromFragments += temp2;
    00732     double distance = temp1.abs();
    00733     if (distance<nearestDistance)
    00734     {
    00735       nearestDistance=distance;
    00736       nearestFragment = temp1;
    00737       nearestFragmentsNormal = temp2;
    00738     }
    00739     }
    00740   }
    00741   nearestFragment = Geometry::relative2FieldCoord(robotPose,nearestFragment.x,nearestFragment.y);
    00742   fromFragments.normalize();
    00743   fromFragments.normalize();
    00744   NARROW(
    00745     "SlamIMP:borderDirection",
    00746     nearestFragment.x,
    00747     nearestFragment.y,
    00748     (nearestFragment.x + 300*fromFragments.x),
    00749     (nearestFragment.y + 300*fromFragments.y),
    00750     20,
    00751     Drawings::ps_solid,
    00752     Drawings::blue
    00753   );
    00754   NARROW(
    00755     "SlamIMP:borderDirection",
    00756     nearestFragment.x,
    00757     nearestFragment.y,
    00758     (nearestFragment.x + 200*nearestFragmentsNormal.x),
    00759     (nearestFragment.y + 200*nearestFragmentsNormal.y),
    00760     20,
    00761     Drawings::ps_solid,
    00762     Drawings::skyblue
    00763   );
    00764   }
    00765 }
    00766
    00767 int SlamBorderFinder::getNumberOfLines()
    00768 {
    00769   return numberOfLines;
    00770 }
    00771
    00772 void SlamBorderFinder::getLine(int number, Vector2<int> & pointOnLine, Vector2<double> & normalToLine)
    00773 {
    00774   pointOnLine.x = lines[number].base.x;
    00775   pointOnLine.y = lines[number].base.y;
    00776   normalToLine.x = lines[number].normal.x;
    00777   normalToLine.y = lines[number].normal.y;
    00778 }
    00779
    00780
    00781 bool SlamBorderFinder::linesPerpendicularOnField(int lineNumber1, int lineNumber2, const CameraMatrix& cameraMatrix, const Image& image)
    00782 {
    00783   Vector2<double> justAPoint, direction1, direction2;
    00784   if ( calculateLineOnField(lineNumber1, justAPoint, direction1, cameraMatrix, image)
    00785     && calculateLineOnField(lineNumber2, justAPoint, direction2, cameraMatrix, image) )
    00786   {
    00787     if (fabs(direction1*direction2) < 0.5) // directions include an angle of approximately 90° (60°..120°)
    00788     {
    00789       return true;
    00790     }
    00791   }
    00792   return false;
    00793 }
    00794
    00795 bool SlamBorderFinder::calculateLineOnField(int lineNumber, Vector2<double> & base, Vector2<double> & direction, const CameraMatrix& cameraMatrix, const Image& image)
    00796 {
    00797   Vector2<int> pointOnField1, pointOnField2, directionOnField;
    00798   if ( Geometry::calculatePointOnField(lines[lineNumber].base.x, lines[lineNumber].base.y, cameraMatrix, image.cameraInfo, pointOnField1)
    00799     && 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))
    00800   {
    00801     directionOnField = pointOnField2-pointOnField1;
    00802     base.x = (double)pointOnField1.x;
    00803     base.y = (double)pointOnField1.y;
    00804     direction.x = (double)directionOnField.x;
    00805     direction.y = (double)directionOnField.y;
    00806     direction.normalize();
    00807     return true;
    00808   }
    00809   return false;
    00810 }
    00811
    00812
    00813 bool SlamBorderFinder::calculateLineOnField(const Vector2<int> & baseInImage, const Vector2<double> & normalInImage, Vector2<double> & baseOnField, Vector2<double> & directionOnField, const CameraMatrix& cameraMatrix, const Image& image)
    00814 {
    00815   Vector2<int> pointOnField1, pointOnField2, directionOnFieldTemp;
    00816   if ( Geometry::calculatePointOnField(baseInImage.x, baseInImage.y, cameraMatrix, image.cameraInfo, pointOnField1)
    00817     && Geometry::calculatePointOnField((int)(baseInImage.x+50*normalInImage.y), (int)(baseInImage.y-50*normalInImage.x), cameraMatrix, image.cameraInfo, pointOnField2))
    00818   {
    00819     directionOnFieldTemp = pointOnField2-pointOnField1;
    00820     baseOnField.x = (double)pointOnField1.x;
    00821     baseOnField.y = (double)pointOnField1.y;
    00822     directionOnField.x = (double)directionOnFieldTemp.x;
    00823     directionOnField.y = (double)directionOnFieldTemp.y;
    00824     directionOnField.normalize();
    00825     return true;
    00826   }
    00827   return false;
    00828 }
    00829
    00830 void SlamBorderFinder::doVerificationScan(const Vector2<double> & crossingPoint, const Vector2<double> & directionToScanAt, const Vector2<double> & scanningDirection, int lineSize, LinesPercept::CrossingCharacteristic & result, const CameraMatrix& cameraMatrix, const Image& image)
    00831 {
    00832   // preparations
    00833   int i,j;
    00834   double scanningDistance = max(6.0, lineSize*1.5);
    00835   double projection = fabs( sin( atan2(directionToScanAt.y,directionToScanAt.x) - atan2(scanningDirection.y,scanningDirection.x) ) );
    00836   if (projection > 0) // should never be otherwise
    00837   {
    00838     scanningDistance /= projection;
    00839   }
    00840   double directionAngle = atan2(scanningDirection.y,scanningDirection.x);
    00841
    00842   int numberOfScanLines = 2;
    00843   double distanceBetweenScanLines = 2.0;
    00844   bool greenFound[2]; // numberOfScanLines
    00845   bool whiteFound[2]; // numberOfScanLines
    00846   bool robotColorFound[2]; // numberOfScanLines
    00847   Vector2<int> actual;
    00848   unsigned char y,u,v;
    00849   colorClass color;
    00850   Vector2<double> scanningStart;
    00851   Vector2<int> scanningStart2;
    00852
    00853   for (i=0; i<numberOfScanLines; i++)
    00854   {
    00855     scanningStart = crossingPoint + directionToScanAt * (scanningDistance + i*distanceBetweenScanLines) - scanningDirection * scanningDistance;
    00856     scanningStart2.x = (int)scanningStart.x;
    00857     scanningStart2.y = (int)scanningStart.y;
    00858     BresenhamLineScan bls(scanningStart2, directionAngle, image.cameraInfo);
    00859     actual = scanningStart2;
    00860
    00861     greenFound[i]=false;
    00862     whiteFound[i]=false;
    00863     robotColorFound[i]=false;
    00864
    00865     // begin scanning
    00866     for (j=0; j<= (int)(2.0*scanningDistance); j++)
    00867     {
    00868       if ( actual.x < 0 || actual.y < 0 || actual.x >= image.cameraInfo.resolutionWidth || actual.y >= image.cameraInfo.resolutionHeight )
    00869       {
    00870         break;
    00871       }
    00872       y = colorCorrector.correct(actual.x, actual.y, 0, image.image[actual.y][0][actual.x]);
    00873       u = colorCorrector.correct(actual.x, actual.y, 1, image.image[actual.y][1][actual.x]);
    00874       v = colorCorrector.correct(actual.x, actual.y, 2, image.image[actual.y][2][actual.x]);
    00875       color = COLOR_CLASS(y, u, v);
    00876
    00877       switch (color)
    00878       {
    00879         case green:
    00880           greenFound[i]=true;
    00881           break;
    00882         case white:
    00883           whiteFound[i]=true;
    00884           break;
    00885         case red:
    00886         case blue:
    00887           robotColorFound[i]=true;
    00888         default:
    00889           break;
    00890       }
    00891
    00892       bls.getNext(actual);
    00893     }
    00894     //LINE(imageProcessor_lines,scanningStart2.x,scanningStart2.y,actual.x,actual.y,1,Drawings::ps_solid,Drawings::gray);
    00895   }
    00896
    00897   // analyse scanning results (perhaps later in a more sophisticated way)
    00898   bool robotInTheWay = false;
    00899   bool greenSeen = false;
    00900   bool whiteSeen = false;
    00901   bool whiteAlwaysSeen = true;
    00902   for (i=0; i<numberOfScanLines; i++)
    00903   {
    00904     robotInTheWay = robotInTheWay || robotColorFound[i];
    00905     greenSeen = greenSeen || greenFound[i];
    00906     whiteSeen = whiteSeen || whiteFound[i];
    00907     whiteAlwaysSeen = whiteAlwaysSeen && whiteFound[i];
    00908   }
    00909   if (robotInTheWay)
    00910   {
    00911     result = LinesPercept::dontKnow;
    00912   }
    00913   else
    00914   {
    00915     if ( lineSize > 4 ) // there should be a quite big line, so most scan lines should have seen white
    00916     {
    00917       if (!whiteAlwaysSeen)
    00918       {
    00919         if (greenSeen)
    00920         {
    00921           result = LinesPercept::noLineOnThisSide;
    00922         }
    00923         else
    00924         {
    00925           result = LinesPercept::dontKnow;
    00926         }
    00927       }
    00928       else // white was seen
    00929       {
    00930         if (greenSeen)
    00931         {
    00932           result = LinesPercept::lineOnThisSide;
    00933         }
    00934         else
    00935         {
    00936           result = LinesPercept::dontKnow;
    00937         }
    00938       }
    00939     }
    00940     else // the line might be quite small, so a scan line might not see white just by chance/bad colortable/blur
    00941     {
    00942       if (!whiteSeen)
    00943       {
    00944         if (greenSeen)
    00945         {
    00946           result = LinesPercept::noLineOnThisSide;
    00947         }
    00948         else
    00949         {
    00950           result = LinesPercept::dontKnow;
    00951         }
    00952       }
    00953       else // white was seen
    00954       {
    00955         if (greenSeen)
    00956         {
    00957           result = LinesPercept::lineOnThisSide;
    00958         }
    00959         else
    00960         {
    00961           result = LinesPercept::dontKnow;
    00962         }
    00963       }
    00964     }
    00965   }
    00966 }
    00967
    00968
    00969