Modules/ImageProcessor/RasterImageProcessor/RBallSpecialist2.cpp

Go to the documentation of this file.

/**
* @file RBallSpecialist2.cpp
* 
* This file contains the implementation of class RBallSpecialist2.
*
* @author <a href="mailto:sadprofessor@web.de">Bernd Schmidt</a>
*/
#include "RBallSpecialist2.h"
#include <algorithm>
#include <stdlib.h>
#include <time.h>
#include <list>
#include <bitset>
#include "Tools/Location.h"
#include "Tools/RingBuffer.h"

using namespace std;

RBallSpecialist2::RBallSpecialist2(RasterImageProcessor &processor,RasterStrategy &strat):
  RasterSpecialist(processor),
//  edgeDetector(4),
  edgeScanner(processor),
  numberOfEdgePoints(0),
  points(MAX_EDGE_POINTS),
  roundness(0)
{   
  strategy = &strat;
  preScanNeeded = true;
  postScanNeeded = true;

  InBinaryFile stream(getLocation().getFilename("ball.thr"));

  if(stream.exists()){ 
    stream >> threshold;
    stream >> minEdgeThreshold;
  }
    else{ 
    threshold = 30;
    minEdgeThreshold = 30;
  }

  OUTPUT(idText,text,"ball-thr: " << threshold);
  OUTPUT(idText,text,"edge-thr: " << minEdgeThreshold);

}

RBallSpecialist2::~RBallSpecialist2()
{

}
/** Executes the post-processing. */
void RBallSpecialist2::executePostProcessing()
{   
  Geometry::Circle circle;
  double validity = 0;
  int spaceX = 2;
  int spaceY = 8;
  RasterSpecialist::Box bounds(0,0,0,0);
  //OUTPUT(idText,text,"ball post process");
  
  vector<list<LinePair> > segs;
  createSegmentsFromLines2(rows,segs,spaceX,spaceY);
  if (segs.empty()) return;

  //DEBUG-CODE
  //for (int i = 0;i<segs.size();i++){
  //  OUTPUT(idText,text,"ball: seg: " << segs[i].size());  
  //}

  sort<vector<list<LinePair> >::iterator>(segs.begin(),segs.end(),ListSizeOrder());
  edgeScanner.threshold = minEdgeThreshold;
  //TODO: algo für 1 - 3 Punkte implementieren (Spezialfälle)
  if (createBox(segs.back(),bounds)){   
    int width = bounds.maxX-bounds.minX;
    int height = bounds.minY-bounds.maxY;
    int size = width > height ? width : height;
    if (size > rip->image.cameraInfo.resolutionHeight/2)
      edgeScanner.threshold = minEdgeThreshold;
    else{
      edgeScanner.threshold = threshold;
      if (size < 20)
        edgeScanner.threshold += 15;
    }
    if (size > 4){
      validity = calculateLargeCircle(bounds,circle);
    }
    else{
      if (!filterBallPoints(segs.back())) return;   
      if (numberOfEdgePoints < 4) validity = calculateSmallCircle(circle);
      else validity = calculateCircleByEdges(circle);
    }
  }
  addBallPercept(circle,validity);  
}

void RBallSpecialist2::invokeOnPreScan(int x,int y){
  
  if (!strategy->insideBall) temp = Vector2<int>(x,y);
  else{
    //sich ein Paar merken
    LinePair lp(temp,Vector2<int>(x,y));
    //edgeScanner.ballScanWest(lp.v1.x,lp.v1.y);
    //edgeScanner.ballScanEast(lp.v2.x,lp.v2.y);
  
    rows.push_back(lp);
    //OUTPUT(idText,text,"pair accepted: " << x << " " << y);
    LINE(imageProcessor_ball1,
      lp.v1.x,lp.v1.y, 
      lp.v2.x,lp.v2.y, 
      0.5, Drawings::ps_solid, Drawings::red);    
  }
}

void RBallSpecialist2::invokeOnPostScan(int x,int y)
{
  //not needed
}
int RBallSpecialist2::getType()
{
  return __RBallSpecialist; 
}

void RBallSpecialist2::init()
{ 
  postScanNeeded = true;
  preScanNeeded = true;
  roundness = 0;
  rows.clear();
  
  edgeScanner.threshold = minEdgeThreshold;

//  OUTPUT(idText,text,"lines-size: " << lines.size());
}

void RBallSpecialist2::addBallPercept(Geometry::Circle &circle,double validity)
{ 
//  OUTPUT(idText,text,"Dirty-test: " << validity);
//  OUTPUT(idText,text,"Roundness-test: " << roundness);

  if (roundness < 0.6 || validity < 0.3) return;
  
  //putting both together and test again
  validity = (roundness + validity)/2.0f;
  if(validity < 0.5) return;


  if (Geometry::getDistanceToLine(rip->getHorizon(),circle.center) > 0) return;

  
  Vector2<int> bottomPoint((int)circle.center.x,(int)(circle.center.y + circle.radius));
  Vector2<int> pointOnField;
  if(!Geometry::calculatePointOnField(bottomPoint.x,bottomPoint.y,
    rip->cameraMatrix,rip->image.cameraInfo,pointOnField)) return;

  

  CIRCLE(imageProcessor_ball1,
    (int)circle.center.x, 
    (int)circle.center.y, 
    (int)circle.radius,
    1, Drawings::ps_solid, Drawings::blue);
    
//  OUTPUT(idText,text,"Ball-Validity: " << validity);
    Vector2<int> center((int)circle.center.x,(int)circle.center.y);

  //Ball zum Percept hinzufügen
  Vector2<double> angles;
    Geometry::calculateAnglesForPoint(
    center,
    rip->cameraMatrix, rip->image.cameraInfo, angles);


  
  
  rip->ballPercept.add(
    rip->image.cameraInfo,
    center,
    circle.radius,
    angles, 
    Geometry::pixelSizeToAngleSize(circle.radius,rip->image.cameraInfo),
    rip->cameraMatrix.translation, 
    rip->cameraMatrix.isValid);

  //OUTPUT(idText,text,"Ball-Distance: " << rip->ballPercept.getDistanceSizeBased());
}

double RBallSpecialist2::validateCircle(Geometry::Circle &circle)
{ 

  /*CIRCLE(imageProcessor_ball1,
  (int)circle.center.x, 
  (int)circle.center.y, 
  (int)circle.radius,
  0.3, Drawings::ps_solid, Drawings::white);
  */
  
  
  /*Validity using size of the radius */
  double size = 1;
  
  double maxRadius =  25  /*((double)176)/(double)7*/;
  size = circle.radius;

  if (size > maxRadius ) size = maxRadius;
  size = size/maxRadius;

  /*Validity using dirty Points*/
  double pValidity = 1;
  int points = 1;
  int validPoints = 1;
  int counts = 7;

  double r2 = circle.radius * circle.radius;
  
  int minX = (int) (circle.center.x - circle.radius);
  int minY = (int) (circle.center.y - circle.radius);
  int maxX = (int) (circle.center.x + circle.radius);
  int maxY = (int) (circle.center.y + circle.radius);

  //merging imageBounds with circle Bounds

  if (minX < 0) minX = 0;
  if (minY < 0) minY = 0;
  if (maxX > rip->image.cameraInfo.resolutionWidth) maxX = rip->image.cameraInfo.resolutionWidth;
  if (maxY > rip->image.cameraInfo.resolutionHeight) maxY = rip->image.cameraInfo.resolutionHeight;
  
  //if (maxX > 176) maxX = 176;
  //if (maxY > 144) maxY = 144;
  int margin = (int) (((maxX - minX) + (maxY - minY))/(2*counts) + (double)0.5);
  //OUTPUT(idText,text,"Margin: " << margin);
  if (margin < 2) margin = 2;

  //counting validPoints
  if (maxX > margin) maxX -= margin;
  if (maxY > margin) maxY -= margin;
  
  for (int x = minX; x < maxX; x += margin)
    for (int y = minY; y < maxY - margin; y+= margin){
    points++;
    
    colorClass color = getColor(x,y);
    
    if (Geometry::insideCircle(circle,r2,x,y)){
      if (color == orange)
        validPoints++;
      else{
        DOT(imageProcessor_ball1, x, y, Drawings::white, Drawings::red);
      }
    }
    else if (color == green || color == white || color == gray || 
         color == yellow || color == skyblue){
      validPoints++;
    }
    else{
      DOT(imageProcessor_ball1, x, y, Drawings::white, Drawings::red);
    }
    
  }

  pValidity = (double)validPoints/(double)points;
  //OUTPUT(idText,text,"validPoints: " << validPoints);
  //OUTPUT(idText,text,"allPoints: " << points);
  //OUTPUT(idText,text,"Size-Validity: " << size);
  //OUTPUT(idText,text,"DirtyPoints-Validity: " << pValidity);
  //OUTPUT(idText,text,"Ball-Validity: " << (size + pValidity*4)/5);
  //return (size + pValidity*2)/3 ;
  return pValidity;
}

bool RBallSpecialist2::filterBallPoints(std::list<LinePair>& segment){
  numberOfEdgePoints = 0;
  if (segment.empty()) return false;

  list<LinePair>::iterator it = segment.begin();
  list<LinePair>::iterator temp;
  int row = it->v1.y;

  if (isEdge(it->v1.x,it->v1.y)){
    points[numberOfEdgePoints] = it->v1;
    numberOfEdgePoints++;
    DOT(imageProcessor_ball1, it->v1.x, it->v1.y,
      Drawings::white, Drawings::green);
  }
  temp = it;

  for (it++;it!=segment.end();it++){
    if (row != it->v1.y) row = it->v1.y;
    else {
      temp = it;
      continue;
    };
    
    if (isEdge(temp->v2.x,temp->v2.y)){
      points[numberOfEdgePoints] = temp->v2;
      numberOfEdgePoints++;
      DOT(imageProcessor_ball1, it->v2.x, it->v2.y,
        Drawings::white, Drawings::green);
      if (numberOfEdgePoints == MAX_EDGE_POINTS){
          return true;
      }
    }

    if (isEdge(it->v1.x,it->v1.y)){
      points[numberOfEdgePoints] = it->v1;
      numberOfEdgePoints++;
      DOT(imageProcessor_ball1, it->v1.x, it->v1.y,
        Drawings::white, Drawings::green);
      if (numberOfEdgePoints == MAX_EDGE_POINTS){
          return true;
      }
    }
    temp = it;
  }
  it = --segment.end();
  if (isEdge(it->v1.x,it->v1.y)){
    points[numberOfEdgePoints] = it->v1;
    numberOfEdgePoints++;
    DOT(imageProcessor_ball1, it->v1.x, it->v1.y,
        Drawings::white, Drawings::green);
  }

  if (numberOfEdgePoints > 3) return true;
  else return false;
}

double RBallSpecialist2::validateEdgePoints(Geometry::Circle &circle){
  double validity = 0;
  int validPoints = 0;
  double dist = 0;
  
  for (int i = 0;i<numberOfEdgePoints;i++){
    Vector2<double> vertex(points[i].x,points[i].y);
    dist = fabs((circle.center - vertex).abs() - circle.radius);
    if (dist < MAX_CIRCLE_DIST) validPoints++;
  }
  validity = ((double)validPoints)/(double)numberOfEdgePoints;

  return validity;
}

double RBallSpecialist2::middleEdgePointDist(Geometry::Circle &circle){
  double validity = 0;
  double dist = 0;
  
  for (int i = 0;i<numberOfEdgePoints;i++){
    Vector2<double> vertex(points[i].x,points[i].y);
    dist += fabs((circle.center - vertex).abs() - circle.radius);
  }
  validity = dist/(double)numberOfEdgePoints;

  return validity;
}

double RBallSpecialist2::calculateCircleByEdges(Geometry::Circle& circle){
  Geometry::Circle current;
  int chances = 20;
  double best = 1000;
  double validity = 0;
  double distance = 0;
  int r1 = 0;
  int r2 = 0;
  int r3 = 0;

  for(int i = 0;i<chances;i++){
    r1 = rand()%numberOfEdgePoints;
    do{ r2 = rand()%numberOfEdgePoints; }while(r2 == r1);
    do{ r3 = rand()%numberOfEdgePoints; }while(r3 == r1 || r3 == r2);
    current = Geometry::getCircle(points[r1],points[r2],points[r3]);
    
    distance = middleEdgePointDist(current);
    //OUTPUT(idText,text,"dist-test: " << distance);
    if (distance > best) continue;
    best = distance;
    circle = current;
  }
  //OUTPUT(idText,text,"dist-best: " << best);
  roundness = validateEdgePoints(circle);
  validity = validateCircle(circle);
  return validity;
}

bool RBallSpecialist2::createBox(std::list<LinePair>& segment,
                 RasterSpecialist::Box& box){
  if (segment.empty()) return false;
  list<LinePair>::iterator it = segment.begin();
  list<LinePair>::iterator temp = it;
  int row = it->v1.y;

  box.minY = row;
  box.maxY = segment.back().v1.y;
  box.minX = +100000;
  box.maxX = -100000;


  for (it++;it!=segment.end();it++){
    if (row != it->v1.y) row = it->v1.y;
    else {
      temp = it;
      continue;
    };
    if (temp->v2.x > box.maxX) box.maxX = temp->v2.x;
    if (temp->v1.x < box.minX) box.minX = temp->v1.x;
    temp = it;
  }

  LINE(imageProcessor_ball1,
  box.minX,box.minY, 
  box.maxX,box.minY, 
  2, Drawings::ps_solid, Drawings::orange);
  
  LINE(imageProcessor_ball1,
  box.maxX,box.minY,  
  box.maxX,box.maxY, 
  2, Drawings::ps_solid, Drawings::orange); 

  LINE(imageProcessor_ball1,
  box.maxX,box.maxY,
  box.minX,box.maxY, 
  2, Drawings::ps_solid, Drawings::orange); 

  LINE(imageProcessor_ball1,
  box.minX,box.maxY, 
  box.minX,box.minY, 
  2, Drawings::ps_solid, Drawings::orange); 


  return true;
}

double RBallSpecialist2::calculateLargeCircle(
    const Box& input,Geometry::Circle& circle)
{ 
  numberOfEdgePoints = 0;
  int cx = (input.minX + input.maxX)/2;
  int cy = (input.minY + input.maxY)/2;

    
  Vector2<int> start,aim,scan;
  double step = pi2/MAX_EDGE_POINTS;
  start.x = cx;
  start.y = cy;
  for (int i = 0;i<MAX_EDGE_POINTS;i++)
  {
    scan = start;
    Vector2<double> dir;
    getCoordinatesByAngle((double)i*step,dir.x,dir.y);
    aim.x = (int)dir.x + start.x;
    aim.y = (int)dir.y + start.y;
    /*LINE(imageProcessor_ball1,
      aim.x,aim.y, 
      start.x,start.y, 
      1, Drawings::ps_solid, Drawings::orange);*/ 
    
    if(edgeScanner.scan(scan.x,scan.y,dir))
    {
      points[numberOfEdgePoints++] = scan;
      LINE(imageProcessor_ball1,
        scan.x,scan.y, 
        start.x,start.y, 
        1, Drawings::ps_solid, Drawings::white); 
    }
    
  }
  if (numberOfEdgePoints<3) return 0;
  return calculateCircleByEdges(circle);

}

double RBallSpecialist2::calculateSmallCircle(Geometry::Circle& circle){
  switch (numberOfEdgePoints){
  case 3: 
    circle = Geometry::getCircle(points[0],points[1],points[2]);
    if (circle.radius > 5){
      circle.radius = 5;
      roundness = 0.61f;
      return 0;
    }
    roundness = 0.61f;
    return validateCircle(circle);
  case 2: 
    circle.center.x = (points[0].x + points[1].x)/2;
    circle.center.y = (points[0].x + points[1].x)/2;
    //only guessed, the radius could be approximated whith a few
    //scanlines through the area of the two points
    circle.radius = (points[0] - points[1]).abs()/1.6;
    if (circle.radius > 5){
      circle.radius = 5;
      roundness = 0.61f;
      return 0;
    }
    return validateCircle(circle);
  case 1:
    circle.center.x = points[0].x;
    circle.center.y = points[0].y;
    circle.radius = 1.5;
    roundness = 0.61f;
    return validateCircle(circle);
  default:
    roundness = 0;
    return 0;

  }
};

void RBallSpecialist2::setThreshold(int threshold){
  this->threshold = threshold;
}

/*
* Change log :
* 
* $Log: RBallSpecialist2.cpp,v $
* Revision 1.5  2004/09/06 12:02:26  schmidtb
* commented almost all members, removed warnings in documentation

* did further code clean up
*
* Revision 1.4  2004/09/03 11:32:13  nistico
* References to MSH2004ColorCorrector removed, now uses the unified ColorCorrector
*
* Revision 1.3  2004/09/02 07:59:29  schmidtb
* Added RasterImageProcessor to repository, because we used it for the OpenChallenge.
*
* Revision 1.4  2004/05/25 13:27:33  schmidtb
* modified version of rip for open-challenge
*
* Revision 1.9  2004/05/22 16:01:49  pg_besc
* -modified version of rip for bridge-recognition
*
* Revision 1.6  2004/04/22 16:57:26  pg_besc
* new version of RBallSpecialist2, now omidirectional scans for detection
*
* Revision 1.5  2004/04/20 07:50:27  pg_besc
* new version of pre scan
*
* Revision 1.4  2004/04/15 19:09:02  pg_besc
* merged code
*
* Revision 1.3  2004/03/25 16:15:22  nistico
* Compatibility with MSH2004BallLocator restored
*
* Revision 1.2  2004/03/25 15:26:10  pg_besc
* made some changes
*
*
*
*/

---