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

# Modules/ImageProcessor/VLCImageProcessor/VLCCenterCircleFinder.cpp

Go to the documentation of this file.

    00001
    00002 #include "VLCCenterCircleFinder.h"
    00003
    00004 VLCCenterCircleFinder::VLCCenterCircleFinder(const CameraMatrix& cMatrix, const Image& im):cameraMatrix(cMatrix), image(im)
    00005 {
    00006   circleFound = false;
    00007   numberOfCandidates = 0;
    00008   houghStepX = areaOfInterestX/houghDimensionX;
    00009   houghStepY = areaOfInterestY/houghDimensionY;
    00010
    00011   for(int i = 0; i < maxNumber; i++)
    00012   {
    00013     circlePoints[i] = true;
    00014   }
    00015
    00016   for(int i = 0; i < houghDimensionY; i++)
    00017   {
    00018     for(int j = 0; j < houghDimensionX; j++)
    00019       houghSpace[i][j] = 0;
    00020     flagY[i] = false;
    00021   }
    00022 }
    00023
    00024 void VLCCenterCircleFinder::reset()
    00025 {
    00026   circleFound = false;
    00027   numberOfCandidates = 0;
    00028
    00029   for(int i = 0; i < maxNumber; i++)
    00030   {
    00031     circlePoints[i] = true;
    00032   }
    00033
    00034   for(int i = 0; i < houghDimensionY; i++)
    00035   {
    00036     for(int j = 0; j < houghDimensionX; j++)
    00037       houghSpace[i][j] = 0;
    00038     flagY[i] = false;
    00039   }
    00040 }
    00041
    00042 void VLCCenterCircleFinder::addCandidate(Vector2<int>& candiOnField, Vector2<int>& candiOfImage)
    00043 {
    00044   // area of interest:
    00045   /*LINE(circlePoints_Field,areaOfInterestX/2,areaOfInterestY/2,-areaOfInterestX/2, areaOfInterestY/2,1,1,Drawings::gray);
    00046   LINE(circlePoints_Field,areaOfInterestX/2,areaOfInterestY/2, areaOfInterestX/2, -areaOfInterestY/2,1,1,Drawings::gray);
    00047   LINE(circlePoints_Field,areaOfInterestX/2,-areaOfInterestY/2,-areaOfInterestX/2, -areaOfInterestY/2,1,1,Drawings::gray);
    00048   LINE(circlePoints_Field,-areaOfInterestX/2,-areaOfInterestY/2,-areaOfInterestX/2, areaOfInterestY/2,1,1,Drawings::gray);
    00049   */
    00050
    00051    //only candidates, which are inside the area of interest, will be added to the array candidatePoints
    00052   if((candiOnField.x < areaOfInterestX/2) && (candiOnField.x > -areaOfInterestX/2) &&
    00053      (candiOnField.y < areaOfInterestY/2) && (candiOnField.y > -areaOfInterestY/2))
    00054   {
    00055    if(numberOfCandidates < maxNumber)
    00056    {
    00057     /*if the point is "isolated", which means there is no other point at a distance of mergeDistance, then this point will be added as the next
    00058      *entry at position numberOfCandidates. Otherwise the average of this point and other points will be taken.
    00059      */
    00060      bool setAverage = false;
    00061      for(int i = 0; i < numberOfCandidates; i++)
    00062      {
    00063        double distance;
    00064        distance = sqrt(sqr(candidatePoints[i].pointOnField.x - candiOnField.x) + sqr(candidatePoints[i].pointOnField.y - candiOnField.y));
    00065        if(distance < mergeDistance)
    00066        {
    00067         double x,y;
    00068         x = (candidatePoints[i].pointOfImage.x + candiOfImage.x)/2;
    00069         y = (candidatePoints[i].pointOfImage.y + candiOfImage.y)/2;
    00070         candidatePoints[i].pointOfImage.x = int(x);
    00071         candidatePoints[i].pointOfImage.y = int(y);
    00072
    00073         x = (candidatePoints[i].pointOnField.x + candiOnField.x)/2;
    00074         y = (candidatePoints[i].pointOnField.y + candiOnField.y)/2;
    00075         candidatePoints[i].pointOnField.x = x;
    00076         candidatePoints[i].pointOnField.y = y;
    00077         setAverage = true;
    00078        }
    00079      }
    00080      if(setAverage == false)
    00081      {
    00082        Vector2<double> nullVector(0,0);
    00083        candidatePoints[numberOfCandidates].pointOnField.x = double(candiOnField.x);
    00084        candidatePoints[numberOfCandidates].pointOnField.y = double(candiOnField.y);
    00085        candidatePoints[numberOfCandidates].pointOfImage = candiOfImage;
    00086        numberOfCandidates++;
    00087      }
    00088    }//if(numberOfCandidates < maxNumber)
    00089    else
    00090    {
    00091      //OUTPUT(idText,text,"candidate-points dropped");
    00092    }
    00093   }/*if((candiOnField.x < areaOfInterstX/2) && (candiOnField.x > -areaOfInterestX/2) &&
    00094         (candiOnField.y < areaOfInterstY/2) && (candiOnField.y > -areaOfInterestY/2) */
    00095 }
    00096
    00097
    00098
    00099 void VLCCenterCircleFinder::transformArrayToField(int arrayIndex, bool xDirection, int& fieldPosition)
    00100 {
    00101   int border;
    00102   int step;
    00103   int dimension;
    00104   if(xDirection)
    00105   {
    00106     border = areaOfInterestX;
    00107     step = houghStepX;
    00108     dimension = houghDimensionX;
    00109   }
    00110   else
    00111   {
    00112     border = areaOfInterestY;
    00113     step = houghStepY;
    00114     dimension = houghDimensionY;
    00115   }
    00116   /* the two borders of the area of interest is x/y = border/2 and x/y= -(border/2). itīs origin (houghSpace[0][0]) is
    00117    * positioned at x/y =  border/2.
    00118    * if the arrayIndex is a number between 0 and dimension/2, the field-coordinate is positiv and will be calculated as follows:
    00119    * first, the quantized value for the arrayIndex 0 will be calculated (border/2 - step/2). from that value on, the other quantized value
    00120    * can be calculated through substracting a factor of step
    00121    *
    00122    * if the arrayIndex is a number between dimension/2 and dimension, the field-coordinate is negativ:
    00123    * starting from the middle of the area of interest, stepping to the outer negative border, the quantized value for arrayIndex dimension/2
    00124    * is -step/2. other quantized values through substracting a factor of step. since one start from the middle of the area of interest,
    00125    * dimension/2 has to be substracted from arrayIndex
    00126    */
    00127   if(arrayIndex < (dimension/2))
    00128   {
    00129     fieldPosition = border/2 - step/2 - arrayIndex * step;
    00130   }
    00131   else if((arrayIndex >= (dimension/2)) && (arrayIndex < dimension))
    00132   {
    00133     fieldPosition = - step/2 - (arrayIndex-dimension/2) * step;
    00134   }
    00135 }
    00136
    00137
    00138 void VLCCenterCircleFinder::transformFieldToArray(double fieldPos, bool xDirection, int& arrayIndex)
    00139 {
    00140   int border;
    00141   int step;
    00142   int dimension;
    00143   if(xDirection)
    00144   {
    00145     border = areaOfInterestX;
    00146     step = houghStepX;
    00147     dimension = houghDimensionX;
    00148   }
    00149   else
    00150   {
    00151     border = areaOfInterestY;
    00152     step = houghStepY;
    00153     dimension = houghDimensionY;
    00154   }
    00155
    00156  /* the two borders of the area of interest is x/y = border/2 and x/y= -(border/2). itīs origin (houghSpace[0][0]) is
    00157   * positioned at x/y =  border/2.
    00158   * if the field-coordinate is a number between 0 and border/2, then the arrayIndex will be a number between 0 and dimension/2:
    00159   * first of all the number (field-coord div step) will be calculated. if the number equals to dimension/2-1, then the corresponding array-index
    00160   * is 0 and if if the number equals to 0, then the corresponding array-index is dimension/2-1. so (field-coord div step) has to be
    00161   * substracted from (dimension/2-1)
    00162   *
    00163   * if the field-coordinate is a number between 0 and -border/2, then the arrayIndex will be number between dimension/2 and dimension
    00164   * same as above, the number (field-coord div step) will be calculated. if the number equals to dimension/2-1, then the corresponding
    00165   * array-index is dimension-1, and if the number equals to 0, then the corresponding array-index is dimension/2. so dimension/2 has to be
    00166   * added to (field-coord div step).
    00167   */
    00168   if((fieldPos >= 0) && (fieldPos < (border/2)))
    00169   {
    00170     arrayIndex = (dimension/2 - 1) - (int(fieldPos) / step);
    00171   }
    00172   else if((fieldPos < 0) && (fieldPos > -(border/2)))
    00173   {
    00174     fieldPos *= -1;
    00175     arrayIndex = (int(fieldPos) / step) + (dimension/2);
    00176   }
    00177
    00178 }
    00179
    00180  void VLCCenterCircleFinder::determineCirclePoint()
    00181  {
    00182    //Hough Transformation,
    00183    //(x - xc)^2 + (y - yc)^2 = r^2
    00184    int xc = 0;
    00185    int xc1 = 0; //needed, because the sqrt of a value has got two results
    00186    int yc = 0;
    00187
    00188    //fill HoughSpace(2-dimensional)
    00189    for(int i = 0; i < numberOfCandidates; i++)
    00190    {
    00191      if(circlePoints[i] != false)
    00192      {
    00193        double help = 0;
    00194        for(int y = 0; y < houghDimensionY; y++)
    00195        {
    00196          transformArrayToField(y,false,yc);
    00197          help = double(radius - sqr(candidatePoints[i].pointOnField.y - yc));
    00198          if(help > 0)
    00199          {
    00200           help = sqrt(help);
    00201           xc = int(candidatePoints[i].pointOnField.x - help);
    00202           xc1 = int(help + candidatePoints[i].pointOnField.x);
    00203           int arrayIndex;
    00204           if((xc < (areaOfInterestX/2)) && (xc > (-areaOfInterestX/2)))
    00205           {
    00206             transformFieldToArray(xc,true,arrayIndex),
    00207             houghSpace[arrayIndex][y] += 1;
    00208             flagY[y] = true;
    00209           }
    00210           if((xc1 < (areaOfInterestX/2)) && (xc1 > (-areaOfInterestX/2)))
    00211           {
    00212             transformFieldToArray(xc1,true,arrayIndex),
    00213             houghSpace[arrayIndex][y] += 1;
    00214             flagY[y] = true;
    00215           }
    00216
    00217          }
    00218
    00219        }//for(int y = 0; y < houghDimension; y++)
    00220
    00221      }//if(circlePoints[i] != maxSimilar)
    00222    }//for(int i = 0; i < numberOfCandidates; i++)
    00223
    00224   //determine maximum entry
    00225    int maxEntry = 0;
    00226    int absMaxEntry = 0;
    00227    bool existMaxEntry = false;
    00228    xc = 0;
    00229    yc = 0;
    00230    for(int y = 0; y < houghDimensionY; y++)
    00231    {
    00232      if(flagY[y] == true)
    00233      {
    00234        for(int x = 0; x < houghDimensionX; x++)
    00235        {
    00236          if(houghSpace[x][y] >= maxEntry)
    00237          {
    00238            if(houghSpace[x][y] > absMaxEntry)
    00239            {
    00240              existMaxEntry = true;
    00241              absMaxEntry = houghSpace[x][y];
    00242              transformArrayToField(x,true,xc);
    00243              transformArrayToField(y,false,yc);
    00244            }
    00245            else if(houghSpace[x][y] == absMaxEntry)
    00246            {
    00247              existMaxEntry = false;
    00248              maxEntry = houghSpace[x][y];
    00249            }
    00250            //OUTPUT(idText,text,"maxEntry, abs: " << maxEntry << ", " << absMaxEntry << ", " << existMaxEntry);
    00251          }//if(houghSpace[x][y] > maxEntry)
    00252        }//for(int x = 0; x < houghDimension; x++)
    00253      }//if(flagY[y] == true)
    00254    }//for(int y = 0; y < houghDimension; y++)
    00255
    00256    if((maxEntry >= mEntry) || existMaxEntry)
    00257    {
    00258      if((existMaxEntry)&&(absMaxEntry >= aMaxEntry))
    00259      {
    00260        circleFound = true;
    00261        midPoint.x = xc;
    00262        midPoint.y = yc;
    00263        CIRCLE(circlePoints_Field, xc, yc, 180, 30,Drawings::ps_dash, Drawings::blue);
    00264        Vector2<int> midpointOnField;
    00265        midpointOnField.x = int(xc);
    00266        midpointOnField.y = int(yc);
    00267        Vector2<int> midpointInImage;
    00268        Geometry::calculatePointInImage(midpointOnField,cameraMatrix,image.cameraInfo,midpointInImage);
    00269        DOT(circlePoints_image, midpointInImage.x, midpointInImage.y, Drawings::black, Drawings:: blue);
    00270      }
    00271      else if(maxEntry > mEntry)
    00272      {
    00273       for(int y = 0; y < houghDimensionY; y++)
    00274       {
    00275         if(flagY[y] == true)
    00276         {
    00277           for(int x = 0; x < houghDimensionX; x++)
    00278           {
    00279             if(houghSpace[x][y] == maxEntry)
    00280             {
    00281               transformArrayToField(x,true,xc);
    00282               transformArrayToField(y,false,yc);
    00283               CIRCLE(circlePoints_Field, xc, yc, 30, 30,Drawings::ps_dash, Drawings::green);
    00284               Vector2<int> midpointOnField;
    00285               midpointOnField.x = int(xc);
    00286               midpointOnField.y = int(yc);
    00287               Vector2<int> midpointInImage;
    00288               Geometry::calculatePointInImage(midpointOnField,cameraMatrix,image.cameraInfo,midpointInImage);
    00289               DOT(circlePoints_image, midpointInImage.x, midpointInImage.y, Drawings::black, Drawings::green);
    00290             }
    00291           }
    00292         }
    00293       }
    00294      }//else
    00295   }//if(maxEntry > 2)
    00296
    00297   //OUTPUT(idText,text,"Candidates: " << numberOfCandidates);
    00298   for(int i = 0; i < numberOfCandidates; i++)
    00299   {
    00300     if(circlePoints[i] != false)
    00301     {
    00302
    00303       DOT(circlePoints_image, candidatePoints[i].pointOfImage.x, candidatePoints[i].pointOfImage.y, Drawings::black,Drawings::red);
    00304       CIRCLE(circlePoints_Field, candidatePoints[i].pointOnField.x, candidatePoints[i].pointOnField.y, 30, 30,Drawings::ps_dash, Drawings::red);
    00305
    00306     }
    00307   }
    00308  }
    00309
    00310
    00311  bool VLCCenterCircleFinder::getCircle(Vector2<double>& mid)
    00312  {
    00313    if(circleFound)
    00314    {
    00315      mid = midPoint;
    00316      return(true);
    00317    }
    00318    else
    00319      return(false);
    00320  }
    00321
    00322
    00323
    00324
    00325
    00326