Modules/ImageProcessor/VLCImageProcessor/VLCCenterCircleFinder.cpp

Go to the documentation of this file.

#include "VLCCenterCircleFinder.h"

VLCCenterCircleFinder::VLCCenterCircleFinder(const CameraMatrix& cMatrix, const Image& im):cameraMatrix(cMatrix), image(im)
{
  circleFound = false;
  numberOfCandidates = 0;
  houghStepX = areaOfInterestX/houghDimensionX;
  houghStepY = areaOfInterestY/houghDimensionY;

  for(int i = 0; i < maxNumber; i++)
  {
    circlePoints[i] = true;
  }

  for(int i = 0; i < houghDimensionY; i++)
  {
    for(int j = 0; j < houghDimensionX; j++)
      houghSpace[i][j] = 0;
    flagY[i] = false;
  }
}

void VLCCenterCircleFinder::reset()
{
  circleFound = false;
  numberOfCandidates = 0;

  for(int i = 0; i < maxNumber; i++)
  {
    circlePoints[i] = true;
  }

  for(int i = 0; i < houghDimensionY; i++)
  {
    for(int j = 0; j < houghDimensionX; j++)
      houghSpace[i][j] = 0;
    flagY[i] = false;
  }
}

void VLCCenterCircleFinder::addCandidate(Vector2<int>& candiOnField, Vector2<int>& candiOfImage)
{
  // area of interest:
  /*LINE(circlePoints_Field,areaOfInterestX/2,areaOfInterestY/2,-areaOfInterestX/2, areaOfInterestY/2,1,1,Drawings::gray);
  LINE(circlePoints_Field,areaOfInterestX/2,areaOfInterestY/2, areaOfInterestX/2, -areaOfInterestY/2,1,1,Drawings::gray);
  LINE(circlePoints_Field,areaOfInterestX/2,-areaOfInterestY/2,-areaOfInterestX/2, -areaOfInterestY/2,1,1,Drawings::gray);
  LINE(circlePoints_Field,-areaOfInterestX/2,-areaOfInterestY/2,-areaOfInterestX/2, areaOfInterestY/2,1,1,Drawings::gray);
  */

   //only candidates, which are inside the area of interest, will be added to the array candidatePoints
  if((candiOnField.x < areaOfInterestX/2) && (candiOnField.x > -areaOfInterestX/2) && 
     (candiOnField.y < areaOfInterestY/2) && (candiOnField.y > -areaOfInterestY/2))
  {
   if(numberOfCandidates < maxNumber)
   {
    /*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
     *entry at position numberOfCandidates. Otherwise the average of this point and other points will be taken.
     */
     bool setAverage = false;
     for(int i = 0; i < numberOfCandidates; i++)
     {
       double distance;
       distance = sqrt(sqr(candidatePoints[i].pointOnField.x - candiOnField.x) + sqr(candidatePoints[i].pointOnField.y - candiOnField.y));
       if(distance < mergeDistance)
       {
        double x,y;
        x = (candidatePoints[i].pointOfImage.x + candiOfImage.x)/2;
        y = (candidatePoints[i].pointOfImage.y + candiOfImage.y)/2;
        candidatePoints[i].pointOfImage.x = int(x);
        candidatePoints[i].pointOfImage.y = int(y);

        x = (candidatePoints[i].pointOnField.x + candiOnField.x)/2;
        y = (candidatePoints[i].pointOnField.y + candiOnField.y)/2;
        candidatePoints[i].pointOnField.x = x;
        candidatePoints[i].pointOnField.y = y;
        setAverage = true;
       }
     }
     if(setAverage == false)
     {
       Vector2<double> nullVector(0,0);
       candidatePoints[numberOfCandidates].pointOnField.x = double(candiOnField.x);
       candidatePoints[numberOfCandidates].pointOnField.y = double(candiOnField.y);
       candidatePoints[numberOfCandidates].pointOfImage = candiOfImage;
       numberOfCandidates++;
     }
   }//if(numberOfCandidates < maxNumber)
   else
   {
     //OUTPUT(idText,text,"candidate-points dropped");
   }
  }/*if((candiOnField.x < areaOfInterstX/2) && (candiOnField.x > -areaOfInterestX/2) && 
        (candiOnField.y < areaOfInterstY/2) && (candiOnField.y > -areaOfInterestY/2) */
}


 
void VLCCenterCircleFinder::transformArrayToField(int arrayIndex, bool xDirection, int& fieldPosition)
{
  int border;
  int step;
  int dimension;
  if(xDirection)
  {
    border = areaOfInterestX;
    step = houghStepX;
    dimension = houghDimensionX;
  }
  else
  {
    border = areaOfInterestY;
    step = houghStepY;
    dimension = houghDimensionY;
  }
  /* 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 
   * positioned at x/y =  border/2.  
   * if the arrayIndex is a number between 0 and dimension/2, the field-coordinate is positiv and will be calculated as follows:
   * first, the quantized value for the arrayIndex 0 will be calculated (border/2 - step/2). from that value on, the other quantized value
   * can be calculated through substracting a factor of step 
   *
   * if the arrayIndex is a number between dimension/2 and dimension, the field-coordinate is negativ:
   * starting from the middle of the area of interest, stepping to the outer negative border, the quantized value for arrayIndex dimension/2
   * is -step/2. other quantized values through substracting a factor of step. since one start from the middle of the area of interest,       
   * dimension/2 has to be substracted from arrayIndex
   */
  if(arrayIndex < (dimension/2))
  {
    fieldPosition = border/2 - step/2 - arrayIndex * step; 
  }
  else if((arrayIndex >= (dimension/2)) && (arrayIndex < dimension))
  {
    fieldPosition = - step/2 - (arrayIndex-dimension/2) * step;
  }
}


void VLCCenterCircleFinder::transformFieldToArray(double fieldPos, bool xDirection, int& arrayIndex)
{
  int border;
  int step;
  int dimension;
  if(xDirection)
  {
    border = areaOfInterestX;
    step = houghStepX;
    dimension = houghDimensionX;
  }
  else
  {
    border = areaOfInterestY;
    step = houghStepY;
    dimension = houghDimensionY;
  }
  
 /* 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 
  * positioned at x/y =  border/2.
  * if the field-coordinate is a number between 0 and border/2, then the arrayIndex will be a number between 0 and dimension/2:
  * 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
  * 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 
  * substracted from (dimension/2-1)
  *
  * if the field-coordinate is a number between 0 and -border/2, then the arrayIndex will be number between dimension/2 and dimension
  * same as above, the number (field-coord div step) will be calculated. if the number equals to dimension/2-1, then the corresponding 
  * 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 
  * added to (field-coord div step).
  */
  if((fieldPos >= 0) && (fieldPos < (border/2)))
  {
    arrayIndex = (dimension/2 - 1) - (int(fieldPos) / step);
  }
  else if((fieldPos < 0) && (fieldPos > -(border/2)))
  {
    fieldPos *= -1;
    arrayIndex = (int(fieldPos) / step) + (dimension/2);
  }

}

 void VLCCenterCircleFinder::determineCirclePoint()
 {
   //Hough Transformation,
   //(x - xc)^2 + (y - yc)^2 = r^2
   int xc = 0;
   int xc1 = 0; //needed, because the sqrt of a value has got two results
   int yc = 0;

   //fill HoughSpace(2-dimensional)
   for(int i = 0; i < numberOfCandidates; i++)
   {
     if(circlePoints[i] != false)
     {
       double help = 0;
       for(int y = 0; y < houghDimensionY; y++)
       {
         transformArrayToField(y,false,yc);
         help = double(radius - sqr(candidatePoints[i].pointOnField.y - yc));
         if(help > 0)
         {
          help = sqrt(help);
          xc = int(candidatePoints[i].pointOnField.x - help);
          xc1 = int(help + candidatePoints[i].pointOnField.x);
          int arrayIndex;
          if((xc < (areaOfInterestX/2)) && (xc > (-areaOfInterestX/2)))
          {
            transformFieldToArray(xc,true,arrayIndex),
            houghSpace[arrayIndex][y] += 1;
            flagY[y] = true;
          }
          if((xc1 < (areaOfInterestX/2)) && (xc1 > (-areaOfInterestX/2)))
          {
            transformFieldToArray(xc1,true,arrayIndex),
            houghSpace[arrayIndex][y] += 1;
            flagY[y] = true;
          }
          
         }
 
       }//for(int y = 0; y < houghDimension; y++)

     }//if(circlePoints[i] != maxSimilar)
   }//for(int i = 0; i < numberOfCandidates; i++)
  
  //determine maximum entry
   int maxEntry = 0;
   int absMaxEntry = 0;
   bool existMaxEntry = false;
   xc = 0;
   yc = 0;
   for(int y = 0; y < houghDimensionY; y++)
   {
     if(flagY[y] == true)
     {
       for(int x = 0; x < houghDimensionX; x++)
       {
         if(houghSpace[x][y] >= maxEntry)
         {
           if(houghSpace[x][y] > absMaxEntry)
           {
             existMaxEntry = true;
             absMaxEntry = houghSpace[x][y];
             transformArrayToField(x,true,xc);
             transformArrayToField(y,false,yc);
           }
           else if(houghSpace[x][y] == absMaxEntry)
           {
             existMaxEntry = false;
             maxEntry = houghSpace[x][y];
           }
           //OUTPUT(idText,text,"maxEntry, abs: " << maxEntry << ", " << absMaxEntry << ", " << existMaxEntry);
         }//if(houghSpace[x][y] > maxEntry)  
       }//for(int x = 0; x < houghDimension; x++)
     }//if(flagY[y] == true)
   }//for(int y = 0; y < houghDimension; y++)

   if((maxEntry >= mEntry) || existMaxEntry)
   {
     if((existMaxEntry)&&(absMaxEntry >= aMaxEntry))
     {
       circleFound = true;
       midPoint.x = xc;
       midPoint.y = yc;
       CIRCLE(circlePoints_Field, xc, yc, 180, 30,Drawings::ps_dash, Drawings::blue);
       Vector2<int> midpointOnField;
       midpointOnField.x = int(xc);
       midpointOnField.y = int(yc);
       Vector2<int> midpointInImage;
       Geometry::calculatePointInImage(midpointOnField,cameraMatrix,image.cameraInfo,midpointInImage);
       DOT(circlePoints_image, midpointInImage.x, midpointInImage.y, Drawings::black, Drawings:: blue);
     }
     else if(maxEntry > mEntry)
     {
      for(int y = 0; y < houghDimensionY; y++)
      {
        if(flagY[y] == true)
        {
          for(int x = 0; x < houghDimensionX; x++)
          {
            if(houghSpace[x][y] == maxEntry)
            {
              transformArrayToField(x,true,xc);
              transformArrayToField(y,false,yc);
              CIRCLE(circlePoints_Field, xc, yc, 30, 30,Drawings::ps_dash, Drawings::green);
              Vector2<int> midpointOnField;
              midpointOnField.x = int(xc);
              midpointOnField.y = int(yc);
              Vector2<int> midpointInImage;
              Geometry::calculatePointInImage(midpointOnField,cameraMatrix,image.cameraInfo,midpointInImage);
              DOT(circlePoints_image, midpointInImage.x, midpointInImage.y, Drawings::black, Drawings::green);
            }
          }
        }
      }
     }//else
  }//if(maxEntry > 2)

  //OUTPUT(idText,text,"Candidates: " << numberOfCandidates);
  for(int i = 0; i < numberOfCandidates; i++)
  {
    if(circlePoints[i] != false)
    {
     
      DOT(circlePoints_image, candidatePoints[i].pointOfImage.x, candidatePoints[i].pointOfImage.y, Drawings::black,Drawings::red);
      CIRCLE(circlePoints_Field, candidatePoints[i].pointOnField.x, candidatePoints[i].pointOnField.y, 30, 30,Drawings::ps_dash, Drawings::red);

    }
  }
 }


 bool VLCCenterCircleFinder::getCircle(Vector2<double>& mid)
 {
   if(circleFound)
   {
     mid = midPoint;
     return(true);
   }
   else
     return(false);
 }

---