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

# Modules/ImageProcessor/SlamImageProcessor/SlamEdgeSpecialist.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file SlamEdgeSpecialist.cpp
    00003 * This file contains a class for Image Processing.
    00004 * @author Dirk Thomas
    00005 */
    00006
    00007 #include "SlamEdgeSpecialist.h"
    00008 #include "Representations/Perception/Image.h"
    00009 #include "Representations/Perception/EdgesPercept.h"
    00010 #include "SlamImageProcessorTools.h"
    00011 #include <list>
    00012 #include "Tools/Math/Common.h"
    00013
    00014 SlamEdgeSpecialist::SlamEdgeSpecialist
    00015 (
    00016 )
    00017 {
    00018   // used in checkPoint
    00019   greenBefore = false;
    00020   whiteBefore = false;
    00021   numberOfContinuousNoColor = 0;
    00022
    00023   // used in addCandidate
    00024   gradientThreshold = 8;
    00025   double c = cos(3*pi/4);
    00026   double s = sin(3*pi/4);
    00027   const Matrix2x2<double> rotation(
    00028     Vector2<double>(c,s),
    00029     Vector2<double>(-s,c)
    00030   );
    00031   const Matrix2x2<double> invertY(
    00032     Vector2<double>(1,0),
    00033     Vector2<double>(0,-1)
    00034   );
    00035   referenceChange = invertY*rotation;
    00036   // used in getEdgesPercept
    00037   normDistance = 1.0;
    00038   normProjection = 0.98;
    00039   multipleAverageDistance = 2.0;
    00040 }
    00041
    00042 void SlamEdgeSpecialist::reset()
    00043 {
    00044   numOfEdgePoints = 0;
    00045 }
    00046
    00047 void SlamEdgeSpecialist::resetLine()
    00048 {
    00049   greenBefore = false;
    00050   whiteBefore = false;
    00051   numberOfContinuousNoColor = 0;
    00052 }
    00053
    00054 void SlamEdgeSpecialist::checkPoint(const Vector2<int> point, const colorClass color, const CameraMatrix& cameraMatrix, const CameraMatrix& prevCameraMatrix, const Image& image)
    00055 {
    00056   Vector2<int> pointOnField; // position on field, relative to robot
    00057   if(Geometry::calculatePointOnField(point.x, point.y, cameraMatrix, image.cameraInfo, pointOnField))
    00058   {
    00059     switch(color)
    00060     {
    00061       case green:
    00062         lastGreen = point;
    00063         lastGreenField = pointOnField;
    00064         if(whiteBefore)
    00065         {
    00066           // trigger specialist (white2green)
    00067           addCandidate(lastGreen, image);
    00068         }
    00069         greenBefore = true;
    00070         whiteBefore = false;
    00071         numberOfContinuousNoColor = 0;
    00072         break;
    00073       case white:
    00074         lastWhite = pointOnField;
    00075         if(greenBefore)
    00076         {
    00077           // trigger specialist (green2white)
    00078           addCandidate(lastGreen, image);
    00079         }
    00080         greenBefore = false;
    00081         whiteBefore = true;
    00082         numberOfContinuousNoColor = 0;
    00083         break;
    00084       case noColor:
    00085         numberOfContinuousNoColor++;
    00086         break;
    00087       default:
    00088         greenBefore = false;
    00089         whiteBefore = false;
    00090         numberOfContinuousNoColor = 0;
    00091         break;
    00092     } // end switch
    00093   } // end if
    00094
    00095 }
    00096
    00097 void SlamEdgeSpecialist::addCandidate(const Vector2<int> point, const Image& image)
    00098 {
    00099   gradientThreshold = 3;
    00100
    00101   // 2x2 pixel gradient
    00102   Vector2<double> grad = Vector2<double>(image.image[point.y][0][point.x] - image.image[(point.y)-1][0][(point.x)-1] ,
    00103     image.image[(point.y)-1][0][point.x] - image.image[point.y][0][(point.x)-1]);
    00104   /*
    00105   // 3x3 pixel gradient - not better than 2x2
    00106   double grad = Vector2<double>(
    00107     image.image[point.y-1][0][point.x-1]
    00108     + 2*image.image[point.y][0][point.x-1]
    00109     + image.image[point.y+1][0][point.x-1]
    00110     - image.image[point.y-1][0][point.x+1]
    00111     - 2*image.image[point.y][0][point.x+1]
    00112     - image.image[point.y+1][0][point.x+1],
    00113     image.image[point.y-1][0][point.x-1]
    00114     + 2*image.image[point.y-1][0][point.x]
    00115     + image.image[point.y-1][0][point.x+1]
    00116     - image.image[point.y+1][0][point.x-1]
    00117     - 2*image.image[point.y+1][0][point.x]
    00118     - image.image[point.y+1][0][point.x+1]
    00119   );
    00120   gradientThreshold *= 2;
    00121   */
    00122
    00123   if (grad.abs() > gradientThreshold)
    00124   {
    00125     if (numOfEdgePoints < maxNumberOfEdgePoints)
    00126     {
    00127       edgePoints[numOfEdgePoints].offset = point;
    00128       Vector2<double> gradRotated = referenceChange * grad;
    00129       edgePoints[numOfEdgePoints].line = Geometry::Line(point, gradRotated.normalize());
    00130       edgePoints[numOfEdgePoints].weight = 0;
    00131       edgePoints[numOfEdgePoints].belongsToLineNo = -1;
    00132       DOT(imageProcessor_edges, point.x, point.y, 0, 1);
    00133       numOfEdgePoints++;
    00134     }
    00135     else
    00136     {
    00137       OUTPUT(idText,text,"EdgeSpecialist: candidate-point drop (raise maxNumberOfEdgePoints)");
    00138     }
    00139   }
    00140 }
    00141
    00142 void SlamEdgeSpecialist::getEdgesPercept(EdgesPercept& percept, const CameraMatrix& cameraMatrix, const CameraMatrix& prevCameraMatrix, const Image& image)
    00143 {
    00144   int i, j;
    00145
    00146   // for all lines, calculate how many other lines are similar/near in sense of this special norm
    00147   bool similar[maxNumberOfEdgePoints][maxNumberOfEdgePoints];
    00148   for(i = 0; i < numOfEdgePoints; i++)
    00149   {
    00150     int lineHeightI = Geometry::calculateLineSize(edgePoints[i].offset, cameraMatrix, image.cameraInfo);
    00151     for(j = i + 1; j < numOfEdgePoints; j++)
    00152     {
    00153       // similar/near if distance is small and normal is in the same direction
    00154       bool sim = false;
    00155       // test projection first - to remove unneeded calculations of "expensive" distance
    00156       double projection = edgePoints[i].line.direction * edgePoints[j].line.direction;
    00157       if(projection > normProjection)
    00158       {
    00159         // distance point to line:
    00160         // http://mo.mathematik.uni-stuttgart.de/kurse/kurs8/seite44.html
    00161         // NOTE: this is because in fact, the line.direction is here the *normal* to the real line
    00162         double distance1 = fabs((edgePoints[j].line.base - edgePoints[i].line.base) * edgePoints[i].line.direction);
    00163         int lineHeightJ = Geometry::calculateLineSize(edgePoints[j].offset, cameraMatrix, image.cameraInfo);
    00164         double lineHeight = sqrt(double(min(lineHeightI, lineHeightJ)));
    00165         if(distance1 < normDistance * lineHeight)
    00166         {
    00167           double distance2 = fabs((edgePoints[i].line.base - edgePoints[j].line.base) * edgePoints[j].line.direction);
    00168           sim = (distance2 < normDistance * lineHeight);
    00169         }
    00170       }
    00171       similar[i][j] = sim;
    00172       similar[j][i] = sim;
    00173       // draw similarity-lines
    00174       /*if(sim) LINE(imageProcessor_edges,
    00175         edgePoints[i].offset.x,
    00176         edgePoints[i].offset.y,
    00177         edgePoints[j].offset.x,
    00178         edgePoints[j].offset.y,
    00179         0, 0, 2
    00180       );*/
    00181     }
    00182   }
    00183
    00184   // analyse detected points and extract edges
    00185   int maxWeight = 0;
    00186   int colorArrow = 0;
    00187   int colorEdge = 0;
    00188   for(int m = 0; m < 10; m++)
    00189   {
    00190     int edgePointWithHighestWeight = -1;
    00191     maxWeight = 0;
    00192
    00193     // for all points, calculate how many other are similar
    00194     for(i = 0; i < numOfEdgePoints; i++)
    00195     {
    00196       if(edgePoints[i].belongsToLineNo == -1) // only if point is not yet matched to an edge
    00197       for (j = i + 1; j < numOfEdgePoints; j++)
    00198       {
    00199         if(edgePoints[j].belongsToLineNo == -1) // only if point is not yet matched to an edge
    00200         if(similar[i][j])
    00201         {
    00202           // weight added to both points because of reflexivity of distance-norm
    00203           edgePoints[i].weight++;
    00204           edgePoints[j].weight++;
    00205
    00206           if(edgePoints[i].weight > maxWeight)
    00207           {
    00208             maxWeight = edgePoints[i].weight;
    00209             edgePointWithHighestWeight = i;
    00210           }
    00211           if(edgePoints[j].weight > maxWeight)
    00212           {
    00213             maxWeight = edgePoints[j].weight;
    00214             edgePointWithHighestWeight = j;
    00215           }
    00216         }
    00217       }
    00218     }
    00219
    00220     // break detection if no more highest is found
    00221     if(maxWeight<1) break;
    00222
    00223     // draw point with highest weight
    00224     DOT(imageProcessor_edges, edgePoints[edgePointWithHighestWeight].offset.x, edgePoints[edgePointWithHighestWeight].offset.y, colorArrow, colorArrow);
    00225
    00226     // store points which belong to the current line
    00227     Vector2<int>start, end;
    00228     int numberOfEdges = 0;
    00229     double averageDistance;
    00230     struct Edge
    00231     {
    00232       //int numberOfEdgePoints;
    00233       std::list <int> pointIndices;
    00234     };
    00235     std::list <int>::iterator it;
    00236     Edge edges[30];
    00237     for(i = 0; i < 30; i++) edges[i].pointIndices.clear();
    00238
    00239     // show lines that belong to the line with highest weight
    00240     for(i = 0; i < numOfEdgePoints; i++)
    00241     {
    00242       if(edgePoints[i].belongsToLineNo == -1)
    00243       {
    00244         if(similar[i][edgePointWithHighestWeight])
    00245         {
    00246           ARROW(imageProcessor_edges,
    00247             edgePoints[i].line.base.x,
    00248             edgePoints[i].line.base.y,
    00249             edgePoints[i].line.base.x + edgePoints[i].line.direction.x * 10,
    00250             edgePoints[i].line.base.y + edgePoints[i].line.direction.y * 10, 1, 1, colorArrow
    00251           );
    00252           // store points in ordered collection based on x- or y-values
    00253           for(it = edges[numberOfEdges].pointIndices.begin( ); it != edges[numberOfEdges].pointIndices.end( ); it++)
    00254           {
    00255             if(edgePoints[*it].offset.x > edgePoints[i].offset.x) break;
    00256           }
    00257           edges[numberOfEdges].pointIndices.insert(it, i);
    00258         }
    00259       }
    00260     }
    00261     colorArrow++;
    00262     numberOfEdges++;
    00263
    00264     // check points on the single edge and filter outsiders and/or split in multiple parts
    00265
    00266     // compute avg-distance
    00267     if(edges[0].pointIndices.size() <= 1) continue;
    00268     start = edgePoints[edges[0].pointIndices.front()].offset;
    00269     end = edgePoints[edges[0].pointIndices.back()].offset;
    00270     //if(edges[0].numberOfEdgePoints > 0) averageDistance = Geometry::distance(start, end) / edges[0].numberOfEdgePoints;
    00271     averageDistance = Geometry::distance(start, end) / edges[0].pointIndices.size();
    00272     // split edge in multple parts if distance of two neighboured points is much bigger than avg-distance
    00273     int current, last, size;
    00274
    00275     it = edges[numberOfEdges-1].pointIndices.begin();
    00276     bool finished = (it != edges[numberOfEdges-1].pointIndices.end());
    00277     while(finished)
    00278     {
    00279       size = edges[numberOfEdges-1].pointIndices.size();
    00280       last = *it;
    00281       it++;
    00282       if(it != edges[numberOfEdges-1].pointIndices.end())
    00283       {
    00284         current = *it;
    00285         int x1 = edgePoints[last].offset.x;
    00286         int x2 = edgePoints[current].offset.x;
    00287         if(x1 + multipleAverageDistance * averageDistance < x2)
    00288         {
    00289           // split of *it and following points into next edge
    00290           edges[numberOfEdges].pointIndices.splice(
    00291             edges[numberOfEdges].pointIndices.begin(),
    00292             edges[numberOfEdges-1].pointIndices,
    00293             it,
    00294             edges[numberOfEdges-1].pointIndices.end()
    00295           );
    00296           it = std::list <int>::iterator(edges[numberOfEdges].pointIndices.begin());
    00297           numberOfEdges++;
    00298         }
    00299       }
    00300       finished = (it != edges[numberOfEdges-1].pointIndices.end());
    00301     }
    00302
    00303
    00304     // for each resulting edge check size, draw and generate percept
    00305     for(int j = 0; j < numberOfEdges; j++)
    00306     {
    00307       // draw edge (and generate edge-percept) if based on a couple of points
    00308       if(edges[j].pointIndices.size() > 4)
    00309       {
    00310         // generate edge-point-percept, "disable" all points for any further computation
    00311         for(it = edges[j].pointIndices.begin(); it != edges[j].pointIndices.end(); it++)
    00312         {
    00313           i = *it;
    00314           Vector2<int> pointOnField; //position on field, relative to robot
    00315           if(Geometry::calculatePointOnField(edgePoints[i].offset.x, edgePoints[i].offset.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, pointOnField))
    00316           {
    00317             edgePoints[i].belongsToLineNo = edgePointWithHighestWeight;
    00318             edgePoints[i].weight = 0;
    00319           }
    00320         }
    00321         start = edgePoints[edges[j].pointIndices.front()].offset;
    00322         end = edgePoints[edges[j].pointIndices.back()].offset;
    00323         LINE(imageProcessor_edges,
    00324           start.x,
    00325           start.y,
    00326           end.x,
    00327           end.y,
    00328           1, 1, colorEdge
    00329         );
    00330         Vector2<double> point1OnField, point2OnField;
    00331         Geometry::calculatePointOnField(start.x, start.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, point1OnField);
    00332         Geometry::calculatePointOnField(end.x, end.y, cameraMatrix, prevCameraMatrix, image.cameraInfo, point2OnField);
    00333         Vector2<double> grad = edgePoints[edges[j].pointIndices.front()].line.direction;
    00334         Vector2<double> dir(grad.y, -grad.x);
    00335         if ((edgePoints[edges[j].pointIndices.back()].line.base - edgePoints[edges[j].pointIndices.front()].line.base) * dir > 0)
    00336           percept.add(point2OnField, point1OnField, end, start);
    00337         else
    00338           percept.add(point1OnField, point2OnField, start, end);
    00339       }
    00340     }
    00341     colorEdge++;
    00342     if(edgePoints[edgePointWithHighestWeight].belongsToLineNo == -1) edgePoints[edgePointWithHighestWeight].belongsToLineNo = -2;
    00343   }
    00344 }