Modules/ImageProcessor/CheckerboardDetector.cpp

Go to the documentation of this file.

/**
* @file CheckerboardDetector.cpp
* Implementation of class CheckerboardDetector.
* 
* @author Uwe Düffert
*/

#include "CheckerboardDetector.h"
#include "Platform/SystemCall.h"
#include "Tools/RobotConfiguration.h"

CheckerboardDetector::CheckerboardDetector(const ImageProcessorInterfaces& interfaces)
: ImageProcessor(interfaces),minY(0),maxY(255),maxDelta(255)
{
}

Vector2<double> CheckerboardDetector::getExactTransitionMiddle(const Geometry::PixeledLine lin, const int start, const int amount)
{
  Vector2<double> p;
  int index=start;
  int sum=0;
  int oldsum=0;
  while (abs(2*sum)<abs(amount))
  {
    oldsum = sum;
    sum = image.image[lin.getPixelY(index)][0][lin.getPixelX(index)]-
      image.image[lin.getPixelY(start-1)][0][lin.getPixelX(start-1)];
    index++;
  }
  double rightWeight=(sum-0.5*amount)/(sum-oldsum);
  p.x=(1-rightWeight)*lin.getPixelX(index-1)+rightWeight*lin.getPixelX(index);
  p.y=(1-rightWeight)*lin.getPixelY(index-1)+rightWeight*lin.getPixelY(index);
  return p;
}

void CheckerboardDetector::getTransitionsOnLine(const Geometry::PixeledLine lin, v2dArray* transPos, bArray* transWhiteBlack, int& numOfTrans)
{
  Vector2<double> p;
  int state=0;      //0=no change, 1=rise, 2=fall
  int sumDelta=0;   //only valid if state!=0
  int startIndex=0; //only valid if state!=0
  numOfTrans=0;
  for (int i=1;i<lin.getNumberOfPixels();i++)
  {
    int actDelta=image.image[lin.getPixelY(i)][0][lin.getPixelX(i)]-image.image[lin.getPixelY(i-1)][0][lin.getPixelX(i-1)];
    if (actDelta>=maxDelta/15)
    {
      if ((state==2)&&(sumDelta<=-maxDelta/2))
      {
        p=getExactTransitionMiddle(lin,startIndex,sumDelta);
        (*transPos)[numOfTrans]=p;
        (*transWhiteBlack)[numOfTrans++]=true;
        DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorGeneral, (int)p.x, (int)p.y);
      }
      if (state!=1)
      {
        startIndex=i;
        sumDelta=actDelta;
        state=1;
      }
      else
      {
        sumDelta+=actDelta;
      }
    }
    else if (actDelta<=-maxDelta/15)
    {
      if ((state==1)&&(sumDelta>=maxDelta/2))
      {
        p=getExactTransitionMiddle(lin,startIndex,sumDelta);
        (*transPos)[numOfTrans]=p;
        (*transWhiteBlack)[numOfTrans++]=false;
        DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorGeneral, (int)p.x, (int)p.y);
      }
      if (state!=2)
      {
        startIndex=i;
        sumDelta=actDelta;
        state=2;
      }
      else
      {
        sumDelta+=actDelta;
      }
    }
    else if (state!=0)
    {
      if (abs(sumDelta)>=maxDelta/2)
      {
        p=getExactTransitionMiddle(lin,startIndex,sumDelta);
        (*transPos)[numOfTrans]=p;
        (*transWhiteBlack)[numOfTrans++]=(sumDelta<0);
        if (sumDelta<0)
        {
          DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorGeneral, (int)p.x, (int)p.y);
        }
        else
        {
          DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorGeneral, (int)p.x, (int)p.y);
        }
      }
      state=0;
    }
  }
}

Vector2<double> CheckerboardDetector::getTransitionToWhite(const Geometry::PixeledLine lin)
{
  if (lin.getNumberOfPixels()>0)
  {
    //check if we are really in black, otherwise its nonsense
    if (image.image[lin.getPixelY(0)][0][lin.getPixelX(0)]<=minY+(maxY-minY)/4)
    {
      int state=0;      //0=no change, 1=rise, 2=fall
      int sumDelta=0;   //only valid if state!=0
      int startIndex=0; //only valid if state!=0
      for (int i=1;i<lin.getNumberOfPixels();i++)
      {
        int actDelta=image.image[lin.getPixelY(i)][0][lin.getPixelX(i)]-image.image[lin.getPixelY(i-1)][0][lin.getPixelX(i-1)];
        if (actDelta>=maxDelta/10)
        {
          if (state!=1)
          {
            startIndex=i;
            sumDelta=actDelta;
            state=1;
          }
          else
          {
            sumDelta+=actDelta;
          }
        }
        else if (state!=0)
        {
          if (sumDelta>=maxDelta/2)
          {
            return getExactTransitionMiddle(lin,startIndex,sumDelta);
          }
          state=0;
        }
      }
    }
  }
  //no transitionToWhite found -> return invalid
  return Vector2<double>(-1,-1);
}

Vector2<double> CheckerboardDetector::getMiddleAndLengthOfPerpendicular(const Vector2<double> t1, const Vector2<double> t2, double& len)
{
  double angle=atan2(t2.y-t1.y,t2.x-t1.x)+pi_2;
  double dx=400*cos(angle);
  double dy=400*sin(angle);
  Vector2<int> pmid((int)(t1.x+t2.x)/2,(int)(t1.y+t2.y)/2);
  Vector2<int> pend1((int)(t1.x+t2.x+dx)/2,(int)(t1.y+t2.y+dy)/2);
  Vector2<int> pend2((int)(t1.x+t2.x-dx)/2,(int)(t1.y+t2.y-dy)/2);
  const Vector2<int> botlef(0,0);
  const Vector2<int> toprig(image.cameraInfo.resolutionWidth-1,image.cameraInfo.resolutionHeight-1);
  Geometry::clipLineWithRectangleCohenSutherland(botlef,toprig,pmid,pend1);
  Geometry::clipLineWithRectangleCohenSutherland(botlef,toprig,pmid,pend2);
  Geometry::PixeledLine ray1(pmid.x,pend1.x,pmid.y,pend1.y);
  Geometry::PixeledLine ray2(pmid.x,pend2.x,pmid.y,pend2.y);
  Vector2<double> p1=getTransitionToWhite(ray1);
  Vector2<double> p2=getTransitionToWhite(ray2);
  Vector2<double> pm((p1.x+p2.x)/2,(p1.y+p2.y)/2);
  if (((p1.x==-1)&&(p1.y==-1))||((p2.x==-1)&&(p2.y==-1)))
  {
    pm.x=-1;
    pm.y=-1;
    len=0;
  }
  else
  {
    len=sqrt((p2.x-p1.x)*(p2.x-p1.x)+(p2.y-p1.y)*(p2.y-p1.y));
  }
  return pm;
}

bool CheckerboardDetector::getLineThroughPixelsCandidate(const v2dArray* points, const int numOfPoints, double& m, double& n)
{
  double sumXi=0;
  double sumYi=0;
  double sumXiXi=0;
  double sumXiXj=0;
  double sumXiYi=0;
  double sumXiYj=0;
  for (int i=0;i<numOfPoints;i++)
  {
    sumXi += (*points)[i].x;
    sumYi += (*points)[i].y;
    sumXiXi += (*points)[i].x * (*points)[i].x;
    sumXiYi += (*points)[i].x * (*points)[i].y;
    for (int j=0;j<numOfPoints;j++)
    {
      sumXiXj += (*points)[i].x * (*points)[j].x;
      sumXiYj += (*points)[i].x * (*points)[j].y;
    }
  }
  if (((numOfPoints*sumXiXi-sumXiXj)==0)||((numOfPoints*sumXiXi-sumXi*sumXi)==0))
  {
    return false;
  }
  m=(numOfPoints*sumXiYi-sumXiYj)/(numOfPoints*sumXiXi-sumXiXj);
  n=(sumXiXi*sumYi-sumXi*sumXiYi)/(numOfPoints*sumXiXi-sumXi*sumXi);
  return true;
}

bool CheckerboardDetector::getLineThroughPixels(const v2dArray* points, const int numOfPoints, Geometry::PixeledLine& lin)
{
  //f(x)=mx+n
  double m;
  double n;
  if ((numOfPoints<2)||(!getLineThroughPixelsCandidate(points,numOfPoints,m,n)))
  {
    return false;
  }
  double error2[100];
  double sumError2=0;
  int i;
  for (i=0;i<numOfPoints;i++)
  {
    error2[i] = m * (*points)[i].x + n - (*points)[i].y;
    error2[i] *= error2[i];
    sumError2 += error2[i];
  }
  v2dArray newPoints;
  int newNumOfPoints=0;
  for (i=0;i<numOfPoints;i++)
  {
    //throw out points that are farer away from line than average and at least 1 pixel away
    if ((error2[i]<=sumError2/numOfPoints)||(error2[i]<1))
    {
      newPoints[newNumOfPoints++]= (*points)[i];
    }
  }
  if ((newNumOfPoints<2)||(!getLineThroughPixelsCandidate(&newPoints,newNumOfPoints,m,n)))
  {
    return false;
  }
  Vector2<int> p1(0,(int)(n+1));
  Vector2<int> p2(1000,(int)(1000*m+n+1));
  const Vector2<int> botlef(0,0);
  const Vector2<int> toprig(image.cameraInfo.resolutionWidth-1,image.cameraInfo.resolutionHeight-1);
  Geometry::clipLineWithRectangleCohenSutherland(botlef,toprig,p1,p2);
  Geometry::PixeledLine myLin(p1,p2);
  lin=myLin;
  return true;
}

Vector2<double> CheckerboardDetector::getPositionFromAngles(const double alpha2,const double a2,const double alpha1,const double a1)
{
  //thanks to Mathematica :-)
  Vector2<double> pos;
  double divident = fabs((2*a1+a2)*sin(alpha2) - a2*sin(2*alpha1+alpha2));
  double divisor = 2 * (a1*a1 + a1*a2 + a2*a2 - a2*(a1+a2)*cos(2*alpha1) -
    a1*(a1+a2)*cos(2*alpha2) + a1*a2*cos(2*(alpha1+alpha2)));
  if (divisor>0)
  {
    double beta11=acos(divident/sqrt(divisor));
    double beta12=pi-beta11;
    double b=sin(beta11)*a1/sin(alpha1);
    double error1=fabs(a2/sin(alpha2)-b/sin(pi-alpha1-alpha2-beta11));
    double error2=fabs(a2/sin(alpha2)-b/sin(pi-alpha1-alpha2-beta12));
    double beta1=(error1<error2)?beta11:beta12;
    double c1 = sin(pi-alpha1-beta1)*a1/sin(alpha1);
    pos.x = -c1*sin(beta1);
    pos.y = -a1+c1*cos(beta1);
  }
  else
  {
    pos.x = -5000;
    pos.y = 0;
    //2do: error
  }
  return pos;
}

double CheckerboardDetector::getAngleBetweenScreenPoints(const Vector2<double>& p1, const Vector2<double>& p2)
{
  //determine the arc differences in x and y direction of these 2 points:
  
  //this is the uncorrected version useful for simulator images:
  double xarc=
    atan((p2.x-image.cameraInfo.resolutionWidth/2)*
    tan(image.cameraInfo.openingAngleWidth/2)/(image.cameraInfo.resolutionWidth/2))
    -atan((p1.x-image.cameraInfo.resolutionWidth/2)*
    tan(image.cameraInfo.openingAngleWidth/2)/(image.cameraInfo.resolutionWidth/2));
  double yarc=
    atan((p2.y-image.cameraInfo.resolutionHeight/2)*
    tan(image.cameraInfo.openingAngleHeight/2)/(image.cameraInfo.resolutionHeight/2))
    -atan((p1.y-image.cameraInfo.resolutionHeight/2)*
    tan(image.cameraInfo.openingAngleHeight/2)/(image.cameraInfo.resolutionHeight/2));
  
  //this is the distortion corrected version useful for real images
  //but it doesnt perform better, because our heuristics like finding a line trough pixels is done on uncorrected stuff...
  /*
  double k1 = image.cameraInfo.secondOrderRadialDistortion/image.cameraInfo.focalLenPow2;
  double k2 = image.cameraInfo.fourthOrderRadialDistortion/image.cameraInfo.focalLenPow4;
  
  double x1 = p1.x - image.cameraInfo.opticalCenter.x;
  double y1 = p1.y - image.cameraInfo.opticalCenter.y;
  double r1sqr = x1*x1 + y1*y1;
  double correctedX1 = p1.x + x1*(k1*r1sqr + k2*r1sqr*r1sqr) - image.cameraInfo.opticalCenter.x;
  double correctedY1 = p1.y + y1*(k1*r1sqr + k2*r1sqr*r1sqr) - image.cameraInfo.opticalCenter.y;
  
  double x2 = p2.x - image.cameraInfo.opticalCenter.x;
  double y2 = p2.y - image.cameraInfo.opticalCenter.y;
  double r2sqr = x2*x2 + y2*y2;
  double correctedX2 = p2.x + x2*(k1*r2sqr + k2*r2sqr*r2sqr) - image.cameraInfo.opticalCenter.x;
  double correctedY2 = p2.y + y2*(k1*r2sqr + k2*r2sqr*r2sqr) - image.cameraInfo.opticalCenter.y;
  
  double xarc = atan2(correctedX2,image.cameraInfo.focalLength) - atan2(correctedX1,image.cameraInfo.focalLength);
  double yarc = atan2(correctedY2,image.cameraInfo.focalLength) - atan2(correctedY1,image.cameraInfo.focalLength);
  */
  
  /*
  //assume you have a sphere with radius 1
  //determine the point on the sphere that ist (xarc,yarc) away from (1,0,0):
  double x=cos(xarc)*cos(yarc);
  double y=sin(xarc)*cos(yarc);
  double z=sin(yarc);
  //determine the distance between these 2 points:
  double d=sqrt((x-1)*(x-1)+y*y+z*z);
  //determine the resulting arc between these 2 points:
  double arc=2*atan(d/2);
  */
  //this can be simplified to:
  return 2*atan(sqrt(2-2*cos(xarc)*cos(yarc))/2);
}

double CheckerboardDetector::yPosFromTransitionIndex(int index)
{
  return 49.5 + 210*(-index/2) + (index<0?111:99)*(-index%2);
}

void CheckerboardDetector::execute()
{ 
  specialPercept.reset(image.frameNumber);
  double distancePanCenterToCamera = getRobotConfiguration().getRobotDimensions().distancePanCenterToCameraX;
  
  int i,x;
  //find minimal and maximal brigthness in scanlines
  minY=image.image[image.cameraInfo.resolutionHeight*16/35][0][0];
  maxY=minY;
  //it is NOT useful to increase that area: we only get stuff into sight we do not want to see:
  for (i=12;i<=26;i++)
  {
    int y=image.cameraInfo.resolutionHeight*i/35;
    for (x=0;x<image.cameraInfo.resolutionWidth;x++)
    {
      int actY=image.image[y][0][x];
      if (actY>maxY) maxY=actY;
      if (actY<minY) minY=actY;
    }
  }
  maxDelta=maxY-minY;
  
  INIT_DEBUG_IMAGE(imageProcessorGeneral, image);
  
  v2dArray blackBlockMiddle;
  int numOfBlackBlockMiddle=0;
  
  //find all relevant bw-changes on horizontal scan lines
  v2dArray transPos;
  bArray transWhiteBlack;
  int numOfTrans;
  for (i=12;i<=26;i++)
  {
    double dummyLen;
    numOfTrans=0;
    Geometry::PixeledLine lin(0,image.cameraInfo.resolutionWidth-1,image.cameraInfo.resolutionHeight*i/35,image.cameraInfo.resolutionHeight*i/35);
    getTransitionsOnLine(lin,&transPos,&transWhiteBlack,numOfTrans);
    //if we found a relevant pair of bw-changes or a bw-change near the border
    //mark its middle (=middle of perpendicular through black block (if it exists)) yellow
    if ((!transWhiteBlack[0])&&(transPos[0].x<image.cameraInfo.resolutionWidth/4))
    {
      Vector2<double> left(0,transPos[0].y);
      Vector2<double> m=getMiddleAndLengthOfPerpendicular(left,transPos[0],dummyLen);
      if ((m.x>=0)&&(m.y>=0))
      {
        DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorGeneral, (int)m.x, (int)m.y);
        blackBlockMiddle[numOfBlackBlockMiddle++]=m;
      }
    }
    if ((transWhiteBlack[numOfTrans-1])&&(transPos[numOfTrans-1].x>image.cameraInfo.resolutionWidth*3/4))
    {
      Vector2<double> right(image.cameraInfo.resolutionWidth-1,transPos[0].y);
      Vector2<double> m=getMiddleAndLengthOfPerpendicular(transPos[numOfTrans-1],right,dummyLen);
      if ((m.x>=0)&&(m.y>=0))
      {
        DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorGeneral, (int)m.x, (int)m.y);
        blackBlockMiddle[numOfBlackBlockMiddle++]=m;
      }
    }
    for (int t=0;t<numOfTrans-1;t++)
    {
      if ((transWhiteBlack[t])&&(!transWhiteBlack[t+1]))
      {
        Vector2<double> m=getMiddleAndLengthOfPerpendicular(transPos[t],transPos[t+1],dummyLen);
        if ((m.x>=0)&&(m.y>=0))
        {
          DEBUG_IMAGE_SET_PIXEL_YELLOW(imageProcessorGeneral, (int)m.x, (int)m.y);
          blackBlockMiddle[numOfBlackBlockMiddle++]=m;
        }
      }
    }
  }
  //2do: check blocks for useful size?
  
  //calculate a good line through the black block middles
  Geometry::PixeledLine lin(0,0,1,1);
  if (getLineThroughPixels(&blackBlockMiddle,numOfBlackBlockMiddle,lin))
  {
    for (i=0;i<lin.getNumberOfPixels();i++)
    {
      int y=image.image[lin.getPixelY(i)][0][lin.getPixelX(i)];
      if (y<=minY+(maxY-minY)/4)
      {
        DEBUG_IMAGE_SET_PIXEL_BLACK(imageProcessorGeneral, lin.getPixelX(i), lin.getPixelY(i));
      }
      else if (y>=maxY-(maxY-minY)/2)
      {
        DEBUG_IMAGE_SET_PIXEL_WHITE(imageProcessorGeneral, lin.getPixelX(i), lin.getPixelY(i));
      }
      else
      {
        DEBUG_IMAGE_SET_PIXEL_Y(imageProcessorGeneral, lin.getPixelX(i), lin.getPixelY(i), 128);
      }
    }
    
    //now we have a resulting line through the checkerboard.
    //recalibrate minY/maxY for that line
    minY=image.image[lin.getPixelY(0)][0][lin.getPixelX(0)];
    maxY=minY;
    for (i=0;i<lin.getNumberOfPixels();i++)
    {
      int actY=image.image[lin.getPixelY(i)][0][lin.getPixelX(i)];
      if (actY>maxY) maxY=actY;
      if (actY<minY) minY=actY;
    }
    maxDelta=maxY-minY;
    //Scan that line again to get all bw-changes
    v2dArray transPos;
    bArray transWhiteBlack;
    int numOfTrans;
    getTransitionsOnLine(lin,&transPos,&transWhiteBlack,numOfTrans);
    int numOfBlocks=0;
    double blockHeight[30];
    if (numOfTrans>2)
    {
      for (i=0;i<numOfTrans;i++)
      {
        DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorGeneral, (int)(transPos[i].x), (int)(transPos[i].y-2));
        DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorGeneral, (int)(transPos[i].x), (int)(transPos[i].y-1));
        DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorGeneral, (int)(transPos[i].x), (int)(transPos[i].y));
        DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorGeneral, (int)(transPos[i].x), (int)(transPos[i].y+1));
        DEBUG_IMAGE_SET_PIXEL_GREEN(imageProcessorGeneral, (int)(transPos[i].x), (int)(transPos[i].y+2));
      }

      //search for middle marker by scanning all senkrechte and take the longest
      Vector2<double> middleBlock(-1,-1);
      double longest=0;
      int middleOffset=-1;
      int middleBlockNum=-1;
      double l;
      Vector2<double> m;
      if (!transWhiteBlack[0])
      {
        Vector2<double> lBorder=Vector2<double>(lin.getPixelX(0),lin.getPixelY(0));
        m=getMiddleAndLengthOfPerpendicular(lBorder,transPos[0],l);
        if (l>0)
        {
          blockHeight[numOfBlocks++]=l;
        }
        for (int len=0;len<l;len++)
        {
          if(m.y+len <image.cameraInfo.resolutionHeight)
          {
            DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorGeneral, (int)(m.x), (int)(m.y+len));
          }
        }
      }
      for (i=0;i<numOfTrans-1;i++)
      {
        if ((transWhiteBlack[i])&&(!transWhiteBlack[i+1]))
        {
          m=getMiddleAndLengthOfPerpendicular(transPos[i],transPos[i+1],l);
          blockHeight[numOfBlocks++]=l;
          if (l>longest)
          {
            longest=l;
            middleBlock=m;
            middleOffset=i;
            middleBlockNum=numOfBlocks-1;
          }
          for (int len=0;len<l;len++)
          {
            if(m.y+len <image.cameraInfo.resolutionHeight)
            {
              DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorGeneral, (int)(m.x), (int)(m.y+len));
            }
          }
        }
      }
      if (transWhiteBlack[numOfTrans-1])
      {
        Vector2<double> rBorder=Vector2<double>(lin.getPixelX(lin.getNumberOfPixels()-1),lin.getPixelY(lin.getNumberOfPixels()-1));
        m=getMiddleAndLengthOfPerpendicular(transPos[numOfTrans-1],rBorder,l);
        if (l>0)
        {
          blockHeight[numOfBlocks++]=l;
        }
        for (int len=0;len<l;len++)
        {
          if(m.y+len <image.cameraInfo.resolutionHeight)
          {
            DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorGeneral, (int)(m.x), (int)(m.y+len));
          }
        }
      }
      
      //check here whether all the gaps have similar sizes
      double lastDist=(transPos[1]-transPos[0]).abs();
      for (i=1;i<numOfTrans-1;i++)
      {
        double dist=(transPos[i]-transPos[i-1]).abs();
        //if we have alternating bw transitions, than missing transition mean distance 3:
        if ((transWhiteBlack[i]==transWhiteBlack[i-1])||
          (dist>2.25*lastDist)||(dist*2.25<lastDist))
        {
          //this is an invalid scan line: transitions are missing or too far from each other
          middleOffset=-1;
          middleBlockNum=-1;
          break;
        }
        lastDist=dist;
      }
      
      if (middleBlockNum>=0)
      {
        //we only found a middle marker if it is significantly larger than the markers
        //left and right of it and if we dont watch angular to small block only:
        if (((middleBlockNum>0)&&(middleBlockNum<numOfBlocks-1)&&(blockHeight[middleBlockNum-1]*1.35<blockHeight[middleBlockNum])&&(blockHeight[middleBlockNum+1]*1.35<blockHeight[middleBlockNum]))||
            ((middleBlockNum==0)&&(numOfBlocks>2)&&(blockHeight[middleBlockNum+1]>0)&&(blockHeight[middleBlockNum+2])&&(blockHeight[middleBlockNum+1]/blockHeight[middleBlockNum+2]*1.35<blockHeight[middleBlockNum]/blockHeight[middleBlockNum+1]))||
            ((middleBlockNum==numOfBlocks-1)&&(numOfBlocks>2)&&(blockHeight[middleBlockNum-1]>0)&&(blockHeight[middleBlockNum-2])&&(blockHeight[middleBlockNum-1]/blockHeight[middleBlockNum-2]*1.35<blockHeight[middleBlockNum]/blockHeight[middleBlockNum-1])))
        {
          for (int len=0;len<longest;len++)
          {
            if(middleBlock.y+len <image.cameraInfo.resolutionHeight)
            {
              DEBUG_IMAGE_SET_PIXEL_BLUE(imageProcessorGeneral, (int)(middleBlock.x), (int)(middleBlock.y+len));
            }
          }
          //equally divide found black white transition into 2 halves
          int halfIndex=(numOfTrans-1)/2;
          double halfYPos=yPosFromTransitionIndex(halfIndex-middleOffset);
          double leftAngle=getAngleBetweenScreenPoints(transPos[0],transPos[halfIndex]);
          double leftLength=yPosFromTransitionIndex(0-middleOffset)-halfYPos;
          double rightAngle=getAngleBetweenScreenPoints(transPos[numOfTrans-1],transPos[halfIndex]);
          double rightLength=halfYPos-yPosFromTransitionIndex(numOfTrans-1-middleOffset);
          Vector2<double> pos = getPositionFromAngles(leftAngle,leftLength,rightAngle,rightLength);

          //direction camera looks to is arc to choosen transition + arc from this to camera middle:
          Vector3<double> front = cameraMatrix.rotation * Vector3<double>(1,0,0);
          Vector3<double> vectorToPoint(
            (double)image.cameraInfo.resolutionWidth/2/tan(image.cameraInfo.openingAngleWidth/2),
            /*image.cameraInfo.focalLength,*/
            (double)image.cameraInfo.resolutionWidth/2 - transPos[halfIndex].x,
            (double)image.cameraInfo.resolutionHeight/2 - transPos[halfIndex].y);
          Vector3<double> vectorToPointWorld = cameraMatrix.rotation * vectorToPoint;
          double dir = atan2(-pos.y,-pos.x)-
            (atan2(vectorToPointWorld.y,vectorToPointWorld.x)-
             atan2(front.y,front.x));
          
          specialPercept.checkerPose.translation = pos + Vector2<double>(-cos(dir)*distancePanCenterToCamera, -sin(dir)*distancePanCenterToCamera + halfYPos);

          specialPercept.type=SpecialPercept::checkerboard;
          specialPercept.checkerPose.rotation = dir - atan2(front.y,front.x);
        }
      }
    }
  }
  SEND_DEBUG_IMAGE(imageProcessorGeneral);
}

/*
* $Log: CheckerboardDetector.cpp,v $
* Revision 1.1.1.1  2004/05/22 17:19:26  cvsadm
* created new repository GT2004_WM
*
* Revision 1.23  2004/03/29 15:26:29  dueffert
* missdetection of near checkerboard corrected
*
* Revision 1.22  2004/03/08 01:38:56  roefer
* Interfaces should be const
*
* Revision 1.21  2004/03/05 15:48:22  dueffert
* small improvements to prevent bugs
*
* Revision 1.20  2004/02/29 17:29:15  dueffert
* finetuning to increase robustness
*
* Revision 1.19  2004/02/29 13:46:48  dueffert
* localization in critical cases improved
*
* Revision 1.18  2004/02/27 16:09:12  dueffert
* new ideas added, number of scan lines increased, beautified
*
* Revision 1.17  2004/02/23 16:42:25  dueffert
* maybe I should use this corrected camera calculation stuff
*
* Revision 1.16  2004/02/16 18:02:27  dueffert
* hypothetical crash reason removed
*
* Revision 1.15  2004/01/30 15:23:46  dueffert
* potential crash removed
*
* Revision 1.14  2004/01/19 14:57:49  dueffert
* specialPercept uses reset() just like other percepts now
*
* Revision 1.13  2004/01/13 11:56:50  dueffert
* added test for middle marker
*
* Revision 1.12  2004/01/08 18:19:28  mkunz
* distancePanCenterToCamera -> distancePanCenterToCameraX
* new: distancePanCenterToCameraZ
*
* Revision 1.11  2004/01/01 10:58:50  roefer
* RobotDimensions are in a class now
*
* Revision 1.10  2003/12/17 09:18:58  dueffert
* another variable image size bug fixed
*
* Revision 1.9  2003/12/16 18:15:48  dueffert
* copy'n'paste bug fixed
*
* Revision 1.8  2003/12/16 17:38:35  dueffert
* variable image resolution bug fixed
*
* Revision 1.7  2003/12/16 13:54:56  roefer
* cameraInfo used for offset calculation
*
* Revision 1.6  2003/12/15 11:46:14  juengel
* Introduced CameraInfo
*
* Revision 1.5  2003/12/15 10:57:23  dueffert
* calculation bug fixed
*
* Revision 1.4  2003/12/05 12:01:35  dueffert
* beautified
*
* Revision 1.3  2003/11/14 19:02:26  goehring
* frameNumber added
*
* Revision 1.2  2003/11/10 13:46:51  dueffert
* checkerboard localization improved
*
* Revision 1.1  2003/10/06 14:10:13  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.2  2003/09/01 10:20:11  juengel
* DebugDrawings clean-up 2
* DebugImages clean-up
* MessageIDs clean-up
* Stopwatch clean-up
*
* Revision 1.1  2003/07/30 14:50:02  dueffert
* CheckerboardDetector added
*
* Revision 1.4  2003/02/21 14:16:12  dueffert
* recognition of our checkerboard continued, see logs/special_localisator2.log
*
* Revision 1.3  2003/02/20 15:32:17  dueffert
* work continued
*
* Revision 1.2  2003/02/19 15:57:34  dueffert
* some checkerboard detection work
*
* Revision 1.1  2003/01/22 15:00:03  dueffert
* checkerboard stuff added
*
*
*/

---