Modules/ImageProcessor/RasterImageProcessor/BoxSpecialist.cpp

Go to the documentation of this file.

/**
* @file BoxSpecialist.cpp
* 
* This file contains the implementation of class BoxSpecialist.
*
* @author <a href="mailto:sadprofessor@web.de">Bernd Schmidt</a>
*/
#include "BoxSpecialist.h"
#include "Tools/ColorClasses.h"
#include "Tools/Location.h"

BoxSpecialist::SegmentInfo::SegmentInfo():
  lmValiditySkyblue(0),
  lmValidityYellow(0),
  goalValidity(0),
  pink(0),
  yellow(0),
  skyblue(0){
}

BoxSpecialist::BoxSpecialist(RasterImageProcessor &processor,RasterStrategy &strat):
  RasterSpecialist(processor),edgeScanner(processor)  {

  strategy = &strat;
  preScanNeeded = true;
  postScanNeeded = false;

  
  InBinaryFile stream1(getLocation().getFilename("flag.thr"));

  if(stream1.exists()) stream1 >> flagThreshold;
    else flagThreshold = 20;

  InBinaryFile stream2(getLocation().getFilename("goal.thr"));

  if(stream2.exists()) stream2 >> goalThreshold;
    else goalThreshold = 25;


  OUTPUT(idText,text,"goal-thr: " << goalThreshold);
  OUTPUT(idText,text,"flag-thr: " << flagThreshold);

}


BoxSpecialist::~BoxSpecialist(){}


int BoxSpecialist::getType() {
  return __RBoxSpecialist;
}

void BoxSpecialist::init() {
    segments_img.clear();
    infos_img.clear();
    lst_pts.clear();
    bottomPoints.clear();
    //OUTPUT(idText,text,"init started");
    edgeScanner.threshold = 17;
}

//TODO: could be faster
void BoxSpecialist::createBBox() {

  SVector::iterator sit;
  list<LinePair>::iterator l_it;
  
  double dist, p_dist;


  for (sit = segments_img.begin();sit != segments_img.end();sit++){
    if (sit->size() < 2) continue;
    
    //OUTPUT(idText,text,"size of Goal-Seg: "<<  sit->size());
    Geometry::Line top,bottom,left,right;
    Vector2<int> minHor,maxHor,minPHor,maxPHor;
    SegmentInfo si;
    double min_dist = 20000, max_dist = -20000, min_p_dist = 20000, max_p_dist = -20000;
    
    for (l_it = sit->begin();l_it!=sit->end();++l_it) {

      dist = getDistanceToLine(horizon,l_it->v1);
      p_dist = getDistanceToLine(pHorizon,l_it->v1);

      if (dist < min_dist) {
        min_dist = dist;
        minHor = l_it->v1;

      }
      if (dist > max_dist) {
        max_dist = dist;
        maxHor = l_it->v1;
      }

      if (p_dist < min_p_dist) {
        min_p_dist = p_dist;
        minPHor = l_it->v1;
      }
      if (p_dist > max_p_dist) {
        max_p_dist = p_dist;
        maxPHor = l_it->v1;

      }

      dist = getDistanceToLine(horizon,l_it->v2);
      p_dist = getDistanceToLine(pHorizon,l_it->v2);

      if (dist < min_dist) {
        min_dist = dist;
        minHor = l_it->v2;

      }
      if (dist > max_dist) {
        max_dist = dist;
        maxHor = l_it->v2;
      }

      if (p_dist < min_p_dist) {
        min_p_dist = p_dist;
        minPHor = l_it->v2;
      }
      if (p_dist > max_p_dist) {
        max_p_dist = p_dist;
        maxPHor = l_it->v2;
      }
    }
    top.base = Vector2<double>(
      (double)maxHor.x,
      (double)maxHor.y);
    top.direction = horizon.direction;
    
    bottom.base = Vector2<double>(
      (double)minHor.x,
      (double)minHor.y);
    bottom.direction = horizon.direction;
    
    left.base = Vector2<double>(
      (double)minPHor.x,
      (double)minPHor.y);
    left.direction = pHorizon.direction;
    
    right.base = Vector2<double>(
      (double)maxPHor.x,
      (double)maxPHor.y);
    right.direction = pHorizon.direction;
    
    Geometry::getIntersectionOfLines(top,left,si.rect.upperLeft);
    Geometry::getIntersectionOfLines(top,right,si.rect.upperRight);
    Geometry::getIntersectionOfLines(bottom,left,si.rect.bottomLeft);
    Geometry::getIntersectionOfLines(bottom,right,si.rect.bottomRight);
    si.rect.center = (si.rect.upperLeft + si.rect.bottomRight)/2;
    si.size = sit->size();
    si.segment = &(*sit);
    
    countColors(si,*(si.segment));
    si.bottomColor = getColor(minHor.x,minHor.y);
    si.topColor = getColor(maxHor.x,maxHor.y);
    createValidities(si);
    infos_img.push_back(si);
  }
}



void BoxSpecialist::invokeOnPreScan(int x, int y){

  Vector2 <int> cur_pt(x,y); //= getVecFromRaster(x,y);

  if (!strategy->insideBox) left_border = cur_pt;
  else {
    LinePair lp(left_border,cur_pt);
    lp.color = getColor(lp.v1.x,lp.v1.y);

    //search for edges to widen the pairs to the edges of the object
    edgeScanner.scanWest(lp.v1.x,lp.v2.y);
    edgeScanner.scanEast(lp.v2.x,lp.v2.y);

    if ((lp.v2.x-lp.v1.x)>1) {      
      lst_pts.push_back(lp);
        
      LINE(imageProcessor_general,
        lp.v1.x,lp.v1.y, 
        lp.v2.x,lp.v2.y, 
        0.5, Drawings::ps_solid, Drawings::red);
  
    }
  }
}

bool BoxSpecialist::missingData(bool ulc, bool urc, bool brc, bool blc, int i) {
  if (ulc && urc) return segments_img[i].size() >5;
  else if (blc && brc) return segments_img[i].size() >5;

  return false;
}

bool BoxSpecialist::searchGoal(BoxSpecialist::SegmentInfo& goal) {
  int i = 0;
  
  if ((goal.pink != 0) || (goal.yellow != 0) && (goal.skyblue != 0)) return false;
  if (!(goal.yellow > 20 || goal.skyblue > 20)) return false;

  double centerDistance = fabs(Geometry::getDistanceToLine(horizon,goal.rect.center));
  double colorCount = goal.yellow + goal.skyblue;
  
  if (centerDistance/colorCount > 2) return false;
  bool ownTeamColorIsBlue = getPlayer().getTeamColor() == Player::blue;
  bool opponentGoal = false;
  colorClass goalColor = (goal.yellow > goal.skyblue)? yellow : skyblue;
  
  if (ownTeamColorIsBlue){
    if(goalColor == yellow) opponentGoal = true;
    else opponentGoal = false;
  }
  else if (goalColor == skyblue) opponentGoal = true;
  

  Vector2<double> south = pHorizon.direction;
  Vector2<double> north(-pHorizon.direction.x,-pHorizon.direction.y);
  Vector2<double> east = horizon.direction;
  Vector2<double> west(-horizon.direction.x,-horizon.direction.y);
  
  int const cols = 12;
  int const rows = 8;
  
  vector<Vector2<int> > tops(cols);
  vector<Vector2<int> > bottoms(cols);
  vector<Vector2<int> > lefts(rows);
  vector<Vector2<int> > rights(rows);

//  double topDists[cols],bottomDists[cols],leftDists[rows],rightDists[rows];


  bool bottomOnBorder = true;
  bool topOnBorder = true;
  bool leftOnBorder = true;
  bool rightOnBorder = true;

  Vector2<double> step = (goal.rect.upperRight - goal.rect.upperLeft)/(cols+2); // looks to right
  Vector2<double> vert = (goal.rect.upperRight - goal.rect.bottomRight)/(rows+2); // looks to bottom
  //Vector2<double> stepRight = step*-1;
  //Vector2<double> vertDown = vert*-1;
  
  
  if (step.abs() < 1) return false;
  if (vert.abs() < 1) return false;
  
  for (i = 0;i<cols;i++){
    Vector2<double> top = goal.rect.upperLeft + (step*(i+1)) - vert;
    Vector2<double> bottom = goal.rect.bottomLeft + (step*(i+1)) + vert;
    tops[i].x = (int)(top.x+0.5);
    tops[i].y = (int)(top.y+0.5);
    bottoms[i].x = (int)(bottom.x + 0.5);
    bottoms[i].y = (int)(bottom.y + 0.5);
    if (edgeScanner.scan(bottoms[i].x,bottoms[i].y,south)) bottomOnBorder = false;
    if (edgeScanner.scan(tops[i].x,tops[i].y,north)) topOnBorder = false;

    DOT(imageProcessor_flagsAndGoals, tops[i].x, tops[i].y,
      Drawings::white, Drawings::red);
    DOT(imageProcessor_flagsAndGoals, bottoms[i].x, bottoms[i].y,
      Drawings::white, Drawings::red);
  }

  for (i = 0;i<rows;i++){
    Vector2<double> left = goal.rect.bottomLeft + step + (vert*(i+1));
    Vector2<double> right = goal.rect.bottomRight - step + (vert*(i+1));
    lefts[i].x = (int)(left.x + 0.5);
    lefts[i].y = (int)(left.y + 0.5);
    rights[i].x = (int)(right.x + 0.5);
    rights[i].y = (int)(right.y + 0.5);
    if(edgeScanner.scan(lefts[i].x,lefts[i].y,west)) leftOnBorder = false;
    if(edgeScanner.scan(rights[i].x,rights[i].y,east)) rightOnBorder = false;

    DOT(imageProcessor_flagsAndGoals, lefts[i].x, lefts[i].y,
      Drawings::white, Drawings::blue);
    DOT(imageProcessor_flagsAndGoals, rights[i].x, rights[i].y,
      Drawings::white, Drawings::blue);
  }

  //now finding the edges
  Geometry::Line top;
  Geometry::Line bottom;
  Geometry::Line left;
  Geometry::Line right;
  Geometry::Line t1;
  Geometry::Line t2;
  int r1;
  int r2;
  float best1;
  float best2;
  float dist1;
  float dist2;
    
  for(i=0;i<10;i++){
    r1 = rand()%cols;
    do r2 = rand()%cols; while (r1==r2);
    t1.base.x += bottoms[r1].x;
    t1.base.y += bottoms[r1].y;
    t2.base.x += tops[r1].x;
    t2.base.y += tops[r1].y;
    t1.direction.x = t1.base.x - bottoms[r2].x; 
    t1.direction.y = t1.base.y - bottoms[r2].y;
    t2.direction.x = t1.base.x - tops[r2].x; 
    t2.direction.y = t1.base.y - tops[r2].y;

    dist1 = 0;
    dist2 = 0;
    best1 = 1000;
    best2 = 1000;
    
    for(int j=0;j<cols;j++){
      dist1 += getDistanceToLine(t1,bottoms[j]);
      dist2 += getDistanceToLine(t2,tops[j]);
    }
    dist1 = fabs(dist1);
    dist2 = fabs(dist2);

    if (dist1 < best1){
      best1 = dist1;
      bottom = t1;
    }

    if (dist2 < best2){
      best2 = dist2;
      top = t2;
    }
  }

  t1.direction = pHorizon.direction;
  t2.direction = pHorizon.direction;

  for(i = 0;i<10;i++){
    r1 = rand()%rows;
    do r2 = rand()%rows; while (r1==r2);
    right.base.x += rights[r1].x;
    right.base.y += rights[r1].y;
    left.base.x += lefts[r1].x;
    left.base.y += lefts[r1].y;
    t1.direction.x = t1.base.x - bottoms[r2].x; 
        t1.direction.y = t1.base.y - bottoms[r2].y;
    t2.direction.x = t1.base.x - tops[r2].x; 
        t2.direction.y = t1.base.y - tops[r2].y;
    
    dist1 = 0;
    dist2 = 0;
    best1 = 1000;
    best2 = 1000;

    for(int j=0;j<rows;j++){
      dist1 += getDistanceToLine(t1,rights[j]);
      dist2 += getDistanceToLine(t2,lefts[j]);
    }
    dist1 = fabs(dist1);
    dist2 = fabs(dist2);

    if (dist1 < best1){
      best1 = dist1;
      bottom = t1;
    }
    if (dist2 < best2){
      best2 = dist2;
      top = t2;
    }
  }

  //directions could be corrigated

  //top.direction = horizon.direction;
  //bottom.direction = horizon.direction;
  //left.direction = pHorizon.direction;
  //right.direction = pHorizon.direction;

  Rectangle& r = goal.rect;
  
  //calculating rectangle
  Geometry::getIntersectionOfLines(bottom,left,goal.rect.bottomLeft);
  Geometry::getIntersectionOfLines(bottom,right,goal.rect.bottomRight);
  Geometry::getIntersectionOfLines(top,left,goal.rect.upperLeft);
  Geometry::getIntersectionOfLines(top,right,goal.rect.upperRight);
  goal.rect.center = (goal.rect.upperLeft + goal.rect.bottomRight)/2;

  double size = (goal.rect.bottomLeft - goal.rect.bottomRight).abs();
  size*= (goal.rect.bottomLeft - goal.rect.bottomRight).abs();

  if ( !(size > 100 && Geometry::getDistanceToLine(horizon, goal.rect.center) < 25) 
    || Geometry::getDistanceToLine(horizon, goal.rect.center) < -10 )
    return false;
  
  RasterSpecialist::Box b(1,1,imageWidth-2,imageWidth-2);
  bool ulc = !b.inside((int)r.upperLeft.x,(int)r.upperLeft.y);
  bool urc = !b.inside((int)r.upperRight.x,(int)r.upperRight.y);
  bool brc = !b.inside((int)r.bottomRight.x,(int)r.bottomRight.y);
  bool blc = !b.inside((int)r.bottomLeft.x,(int)r.bottomLeft.y);
  
  ConditionalBoundary bound; 
  
/*  
  obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::opponentGoal] = true;
  obstaclesPercept.angleToFreePartOfGoal[ObstaclesPercept::opponentGoal] = firstDir + factor * (lastDir - firstDir);
  double distance = ((first2 + last2) / 2).abs();
  obstaclesPercept.distanceToFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance;
  obstaclesPercept.widthOfFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance ? 2 * atan((first - last).abs() / 2 / distance) : pi;
*/
  Vector2<double> a[4];
  Vector2<double> leftCenter = (r.upperLeft + r. bottomLeft)/2;
  Vector2<double> rightCenter = (r.upperRight + r. bottomRight)/2;
  Vector2<double> bottomCenter = (r.bottomRight + r. bottomLeft)/2;
  Vector2<double> topCenter = (r.upperRight + r. upperLeft)/2;

  calculateAnglesForPoint(leftCenter,rip->cameraMatrix,rip->image.cameraInfo,a[0]);
  calculateAnglesForPoint(rightCenter,rip->cameraMatrix,rip->image.cameraInfo,a[1]);
  calculateAnglesForPoint(bottomCenter,rip->cameraMatrix,rip->image.cameraInfo,a[2]);
  calculateAnglesForPoint(topCenter,rip->cameraMatrix,rip->image.cameraInfo,a[3]);
  
  bound.addX(a[0].x,blc);
  bound.addX(a[1].x,brc);
  bound.addX(a[2].x,ulc);
  bound.addX(a[3].x,urc);

  bound.addY(a[0].y,blc);
  bound.addY(a[1].y,brc);
  bound.addY(a[2].y,ulc);
  bound.addY(a[3].y,urc);

  rip->landmarksPercept.addGoal(goalColor, ownTeamColorIsBlue, bound);
  
  //calculate distance
//  int distance = Geometry::getDistanceBySize(
//      goalHeight, 
//    (int)(topCenter - bottomCenter).abs(), 
//       rip->image.cameraInfo.resolutionWidth,
//       rip->image.cameraInfo.openingAngleWidth);

  //if(opponentGoal)
  //  rip->obstaclesPercept.distanceToFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance;
  //else
  //  rip->obstaclesPercept.distanceToFreePartOfGoal[ObstaclesPercept::ownGoal] = distance;
  //if(goal.skyblue != 0) {
  //  OUTPUT(idText,text,"goal recognized : skyblue");
  //}
  //else OUTPUT(idText,text,"goal recognized : yellow");
  
  goal.goalValidity = 1;
  
  LINE(imageProcessor_flagsAndGoals,
    goal.rect.upperLeft.x,goal.rect.upperLeft.y,
    goal.rect.upperRight.x,goal.rect.upperRight.y,
    1.5, Drawings::ps_solid, Drawings::white);

  LINE(imageProcessor_flagsAndGoals,
    goal.rect.bottomLeft.x,goal.rect.bottomLeft.y,
    goal.rect.bottomRight.x,goal.rect.bottomRight.y,
    1.5, Drawings::ps_solid, Drawings::white);

  LINE(imageProcessor_flagsAndGoals,
    goal.rect.upperLeft.x,goal.rect.upperLeft.y,
    goal.rect.bottomLeft.x,goal.rect.bottomLeft.y,
    1.5, Drawings::ps_solid, Drawings::white);

  LINE(imageProcessor_flagsAndGoals,
    goal.rect.upperRight.x,goal.rect.upperRight.y,
    goal.rect.bottomRight.x,goal.rect.bottomRight.y,
    1.5, Drawings::ps_solid, Drawings::white);
  
return true;

}

bool BoxSpecialist::searchLandmark(BoxSpecialist::SegmentInfo& flag){
//  const CameraInfo& cameraInfo = rip->image.cameraInfo;
//  const CameraMatrix& cameraMatrix = rip->cameraMatrix;
  if (flag.pink == 0) return false;
  if (flag.yellow == 0 && flag.skyblue == 0) return false;
  Vector2<double> south = pHorizon.direction;
  Vector2<double> north(-pHorizon.direction.x,-pHorizon.direction.y);
  Vector2<double> east = horizon.direction;
  Vector2<double> west(-horizon.direction.x,-horizon.direction.y);
  Rectangle &r = flag.rect;
  bool bottomOnBorder = false;
  bool topOnBorder = false;

  Vector2<int> borderCenter;
  Vector2<int> center((int)(r.center.x + 0.5),(int)(r.center.y + 0.5));
  Vector2<int> bottomCenter = center;
  Vector2<int> topCenter = center;
  
  DOT(imageProcessor_flagsAndGoals, center.x, center.y,
    Drawings::black, Drawings::white);
  
  colorClass col = noColor;
  colorClass bottomColor = noColor;
  colorClass topColor = noColor;
  
  DOT(imageProcessor_flagsAndGoals, bottomCenter.x, bottomCenter.y,
    Drawings::red, Drawings::black);
  DOT(imageProcessor_flagsAndGoals, topCenter.x, topCenter.y,
    Drawings::green, Drawings::black);

  int& bx = bottomCenter.x;
  int& by = bottomCenter.y;

  double space = (r.bottomLeft-r.upperLeft).abs()/4;

  if (space<3) space = 3;

  while (!bottomOnBorder){
    bottomOnBorder = !edgeScanner.scan(bx,by,south);
    if(abs(center.x - bx) < space && abs(center.y - by) < space)
      edgeScanner.skip(bx,by);
    else break;
  }

  int& tx = topCenter.x;
  int& ty = topCenter.y;

  while (!topOnBorder){
    topOnBorder = !edgeScanner.scan(tx,ty,north);
    if(abs(center.x - tx) < space && abs(center.y - ty) < space)
      edgeScanner.skip(tx,ty);
    else break;   
  }
  Vector2<int> quarter = (topCenter - bottomCenter)/4;
  if (quarter.abs() < 1) return false; //flag is to small
  Vector2<int> topStart(topCenter.x - quarter.x,topCenter.y - quarter.y);
  Vector2<int> bottomStart(bottomCenter.x + quarter.x,bottomCenter.y + quarter.y);
  bool found = false;
  
  //an good example for how to use the color-scan methods of the scanner
  //scan for top color
  Vector2<int> scan = topStart;
  edgeScanner.setDirection(west);
  while(edgeScanner.colorScan(scan.x,scan.y,west,col)){
    if (col == pink || col == yellow || col == skyblue){
      topColor = col;
      found = true;
      break;
    }
    edgeScanner.skip(scan.x,scan.y);
    if ((scan - topStart).abs() > 10) break;
  } 
  if (!found){
    //scan in other direction
    col = noColor;
    scan = topStart;
    
    while(edgeScanner.colorScan(scan.x,scan.y,east,col)){
      if (col == pink || col == yellow || col == skyblue){
        topColor = col;
        found = true;
        break;
      }
      edgeScanner.skip(scan.x,scan.y);
      if ((scan - topStart).abs() > 10) break; 
    }
  }
  if (!found) return false;//no topColor found
  else found = false;
  
  //scan for bottomColor
  scan = bottomStart; 
  while(edgeScanner.colorScan(scan.x,scan.y,west,col)){
    if (col == pink || col == yellow || col == skyblue){
      bottomColor = col;
      found = true;
      break;
    }
    edgeScanner.skip(scan.x,scan.y);
    if ((scan - bottomStart).abs() > 10) break;
  } 
  if (!found){
    //scan in other direction
    col = noColor;
    scan = bottomStart;
    
    while(edgeScanner.colorScan(scan.x,scan.y,east,col)){
      if (col == pink || col == yellow || col == skyblue){
        bottomColor = col;
        found = true;
        break;
      }
      edgeScanner.skip(scan.x,scan.y);
      if ((scan - bottomStart).abs() > 10) break; 
    }
  }
  if (!found) return false;

  Flag::FlagType ft= Flag::numberOfFlagTypes;
  if (topColor == pink) {
    switch (bottomColor) {
    case yellow :
      ft = Flag::pinkAboveYellow;
      break;
    case skyblue :
      ft = Flag::pinkAboveSkyblue;
      break;  
    default : break;
    }
  }
  else if(bottomColor == pink){
    switch (topColor) {
    case skyblue :
      ft = Flag::skyblueAbovePink;
      break;
    case yellow :
      ft = Flag::yellowAbovePink;
      break;
    default : break;
    }
  }
  
  //if (ft == Flag::numberOfFlagTypes) return false;

  DOT(imageProcessor_flagsAndGoals, bottomStart.x, bottomStart.y,
    Drawings::red, Drawings::black);
  
  DOT(imageProcessor_flagsAndGoals, topStart.x, topStart.y,
    Drawings::green, Drawings::black);
  
  Vector2<int> left1,left2,right1,right2;
  
  scan = topStart;

  edgeScanner.setDirection(west);
  edgeScanner.scan(scan.x,scan.y);
  left1 = scan;
  DOT(imageProcessor_flagsAndGoals, scan.x, scan.y,
    Drawings::green, Drawings::blue);

  scan = topStart;

  edgeScanner.setDirection(east);
  edgeScanner.scan(scan.x,scan.y);
  right1 = scan;  
  DOT(imageProcessor_flagsAndGoals, scan.x, scan.y,
    Drawings::green, Drawings::blue);

  scan = bottomStart;

  edgeScanner.setDirection(west);
  edgeScanner.scan(scan.x,scan.y);
  left2 = scan;
  DOT(imageProcessor_flagsAndGoals, scan.x, scan.y,
    Drawings::green, Drawings::blue);

  scan = bottomStart;
  right2 = scan;
  edgeScanner.setDirection(east);
  edgeScanner.scan(scan.x,scan.y);

  DOT(imageProcessor_flagsAndGoals, scan.x, scan.y,
    Drawings::green, Drawings::blue);

  /////////////////////////////////////////
  //adding Percept
  
  Vector2<double> left = (r.bottomLeft + r.upperLeft)/2;
  Vector2<double> right = (r.bottomRight + r.upperRight)/2;
  Vector2<double> top(topCenter.x,topCenter.y);
  Vector2<double> bottom(bottomCenter.x,bottomCenter.y);
  center = (bottomCenter + topCenter)/2;

  DOT(imageProcessor_flagsAndGoals, bottomCenter.x, bottomCenter.y,
    Drawings::red, Drawings::black);
  
  DOT(imageProcessor_flagsAndGoals, topCenter.x, topCenter.y,
    Drawings::green, Drawings::black);

  DOT(imageProcessor_flagsAndGoals, right.x, right.y,
    Drawings::red, Drawings::black);
  
  DOT(imageProcessor_flagsAndGoals, left.x, left.y,
    Drawings::green, Drawings::black);
  
  rip->addFlag(left,right,top,bottom,ft); 
  return true;
}

void BoxSpecialist::fitLandmark(BoxSpecialist::SegmentInfo &flag){
  Vector2<double> south = pHorizon.direction;
  Vector2<double> north(-pHorizon.direction.x,-pHorizon.direction.y);
  Vector2<double> east = horizon.direction;
  Vector2<double> west(-horizon.direction.x,-horizon.direction.y);
  
  Rectangle& r = flag.rect;
  bool bottomOnBorder = false;
  bool topOnBorder = false;
    
  Vector2<int> center((int)(r.center.x + 0.5),(int)(r.center.y + 0.5));
  Vector2<int> bottomCenter = center;
  Vector2<int> topCenter = center;

  DOT(imageProcessor_flagsAndGoals, center.x, center.y,
    Drawings::black, Drawings::white);
  
  DOT(imageProcessor_flagsAndGoals, bottomCenter.x, bottomCenter.y,
    Drawings::red, Drawings::black);
  DOT(imageProcessor_flagsAndGoals, topCenter.x, topCenter.y,
    Drawings::green, Drawings::black);

  int& bx = bottomCenter.x;
  int& by = bottomCenter.y;

  double space = (r.bottomLeft-r.upperLeft).abs()/4;

  if (space<3) space = 3;

  while (!bottomOnBorder){
    bottomOnBorder = !edgeScanner.scan(bx,by,south);
    if(abs(center.x - bx) < space && abs(center.y - by) < space)
      edgeScanner.skip(bx,by);
    else break;
  }

  int& tx = topCenter.x;
  int& ty = topCenter.y;

  while (!topOnBorder){
    topOnBorder = !edgeScanner.scan(tx,ty,north);
    if(abs(center.x - tx) < space && abs(center.y - ty) < space)
      edgeScanner.skip(tx,ty);
    else break;   
  }




}

void BoxSpecialist::invokeOnPostScan(int x,int y) {
  //invoved from the grid scan
  bottomPoints.push_back(Vector2<int>(x,y));
}

void BoxSpecialist::executePostProcessing() {

  if (lst_pts.size() < 2) return;
  
  int allowedXSpace = 2;
  int allowedYSpace = 4;
  
  horizon = rip->getHorizon();

  pHorizon.base = horizon.base;
  pHorizon.direction = horizon.direction;
  pHorizon.direction.rotateLeft();

  createSegmentsFromLines2(lst_pts,segments_img,allowedXSpace,allowedYSpace);

  sort<SVector::iterator>(segments_img.begin(),segments_img.end(),ListSizeOrder());
  createBBox();
  int size = infos_img.size();

  if (size == 0) return;
  
  edgeScanner.threshold = goalThreshold;
  
  vector<SegmentInfo>::iterator it = infos_img.end();
  int goalParts = 0;
  do{ 
    it--;
    if (searchGoal(*it)) goalParts++;
    if (goalParts == 1) break; //TODO: merge code 
  }while(it != infos_img.begin());


  
  edgeScanner.threshold = flagThreshold;
  //searchLandmark(size); 
    //paint debug-drawings
  it = infos_img.end();
  int foundLM = 0;
  do{ 
    it--;
    if (searchLandmark(*it)){
      foundLM++;
    }
    if (foundLM == 2) break;
  }while(it != infos_img.begin());
  Vector2<double> cameraOffset(
    rip->cameraMatrix.translation.x,
    rip->cameraMatrix.translation.y);

  if (foundLM>0) rip->landmarksPercept.estimateOffsetForFlags(cameraOffset);
  if (goalParts>0){
    //if (goalParts>1)fusionGoal();
    //detectFreePartOfGoal();
    rip->landmarksPercept.estimateOffsetForGoals(cameraOffset);
  };
    
  for(it = infos_img.begin();it!= infos_img.end();++it) {
    
    LINE(imageProcessor_general,
        it->rect.upperLeft.x,it->rect.upperLeft.y,it->rect.upperRight.x,it->rect.upperRight.y,
        0.6, Drawings::ps_solid, Drawings::white);
    LINE(imageProcessor_general,
        it->rect.bottomLeft.x,it->rect.bottomLeft.y,it->rect.bottomRight.x,it->rect.bottomRight.y,
        0.6, Drawings::ps_solid, Drawings::green);
    LINE(imageProcessor_general,
        it->rect.upperLeft.x,it->rect.upperLeft.y,it->rect.bottomLeft.x,it->rect.bottomLeft.y,
        0.6, Drawings::ps_solid, Drawings::yellow);
    LINE(imageProcessor_general,
        it->rect.upperRight.x,it->rect.upperRight.y,it->rect.bottomRight.x,it->rect.bottomRight.y,
        0.6, Drawings::ps_solid, Drawings::blue);
  }


}

void BoxSpecialist::mergeBoxes(BoxSpecialist::SegmentInfo& seg1,BoxSpecialist::SegmentInfo& seg2){
  Geometry::Line top,bottom,left,right;


//  seg1.segment->merge(*(seg2.segment));
  if (Geometry::getDistanceToLine(horizon,seg1.rect.upperLeft) <
    Geometry::getDistanceToLine(horizon,seg2.rect.upperLeft))
    seg1.rect.upperLeft = seg2.rect.upperLeft;

  if (Geometry::getDistanceToLine(horizon,seg1.rect.bottomRight) >
    Geometry::getDistanceToLine(horizon,seg2.rect.bottomRight))
    seg1.rect.bottomRight = seg2.rect.bottomRight;

  if (Geometry::getDistanceToLine(pHorizon,seg1.rect.upperRight) <
    Geometry::getDistanceToLine(pHorizon,seg2.rect.upperRight))
    seg1.rect.upperRight = seg2.rect.upperRight;

  if (Geometry::getDistanceToLine(pHorizon,seg1.rect.bottomLeft) >
    Geometry::getDistanceToLine(pHorizon,seg2.rect.bottomLeft))
    seg1.rect.bottomLeft = seg2.rect.bottomLeft;

  top.direction = horizon.direction;
  bottom.direction = horizon.direction;
  left.direction = pHorizon.direction;
  right.direction = pHorizon.direction;

  top.base = seg1.rect.upperLeft;
  bottom.base = seg1.rect.bottomRight;
  right.base = seg1.rect.upperRight;
  left.base = seg1.rect.bottomLeft;

  Geometry::getIntersectionOfLines(bottom,left,seg1.rect.bottomLeft);
  Geometry::getIntersectionOfLines(top,left,seg1.rect.upperLeft);
  Geometry::getIntersectionOfLines(bottom,right,seg1.rect.bottomRight);
  Geometry::getIntersectionOfLines(top,right,seg1.rect.upperRight);
  seg1.rect.center = (seg1.rect.upperLeft + seg1.rect.bottomRight)/2;
  seg1.size += seg2.pink;
  seg1.pink += seg2.pink;
  seg1.yellow += seg2.yellow;
  seg1.skyblue += seg2.skyblue;
}

void BoxSpecialist::countColors(BoxSpecialist::SegmentInfo& si, list<LinePair>& segment){
  list<LinePair>::iterator lines;
  si.pink = 0;
  si.yellow =0;
  si.skyblue = 0;
  for(lines = segment.begin();lines != segment.end();++lines){
    LinePair& line = *lines;
    switch(line.color){ 
    case pink: 
      si.pink += line.v2.x - line.v1.x;
      break;
    case yellow: 
      si.yellow += line.v2.x - line.v1.x;
      break;
    case skyblue: 
      si.skyblue += line.v2.x - line.v1.x;
      break;

    }
  }

  int max = 0;
  int min = 0;
  si.pink > si.skyblue? max = si.pink : max = si.skyblue;
  if (si.yellow > max) max = si.yellow;
  
  min = max/8;

  if (si.pink < min) si.pink = 0;
  if (si.yellow < min) si.yellow = 0;
  if (si.skyblue < min) si.skyblue = 0;
}

// not completed
void BoxSpecialist::fusionGoal(){
  //get the greatest segment in a goal-color and fusion it with other parts,
  //if the goal seems to be splitted
  vector<SegmentInfo>::iterator it;
  
  it = infos_img.begin();
  
  SegmentInfo& greatest = infos_img.back();

  int pos = infos_img.size()-1;
  
  if ((greatest.pink != 0) || (greatest.yellow != 0) && (greatest.skyblue != 0)) return;
  
  double radius = (greatest.rect.center - greatest.rect.bottomRight).abs();
  
  colorClass goalColor = greatest.yellow != 0 ? yellow : skyblue;
  searchGoal(greatest);
  //instead of circles around the boxes we could use rectangular-patterns 
  //to find the boxes that have to be merged
  
  for(++it;it != infos_img.end();++it){
    SegmentInfo& si = *it;
    if (si.pink > 0) continue;
    colorClass curColor = si.yellow > si.skyblue ? yellow : skyblue;
    if (curColor != goalColor) continue;
    //double  dist = fabs(Geometry::getDistanceToLine(horizon,si.rect.center))*0.01;
    //if(dist > 1) continue;
    double curRadius = (si.rect.center - si.rect.bottomRight).abs() + 4 /*+ 10 ) / (1-dist)*/ ;
    if ((si.rect.center - greatest.rect.center).abs() > curRadius + radius) continue;
    if (fabs(Geometry::getDistanceToLine(horizon,si.rect.center)) > 20)continue;
    mergeBoxes(greatest,si);
    si.goalValidity = 1;
  }

  if (!((greatest.yellow > 90 ||  greatest.skyblue > 90) && Geometry::getDistanceToLine(horizon, greatest.rect.center) < 20)) return;
  
  Rectangle& r = greatest.rect;
  RasterSpecialist::Box b(1,1,imageWidth-2,imageWidth-2);
  bool ulc = b.inside((int)r.upperLeft.x,(int)r.upperLeft.y);
  bool urc = b.inside((int)r.upperRight.x,(int)r.upperRight.y);
  bool brc = b.inside((int)r.bottomRight.x,(int)r.bottomRight.y);
  bool blc = b.inside((int)r.bottomLeft.x,(int)r.bottomLeft.y);

  if (missingData(ulc, urc, brc, blc,pos)) return;

  ConditionalBoundary bound; 
  bool ownTeamColorIsBlue = getPlayer().getTeamColor() == Player::blue;

  
/*  
  obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::opponentGoal] = true;
  obstaclesPercept.angleToFreePartOfGoal[ObstaclesPercept::opponentGoal] = firstDir + factor * (lastDir - firstDir);
  double distance = ((first2 + last2) / 2).abs();
  obstaclesPercept.distanceToFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance;
  obstaclesPercept.widthOfFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance ? 2 * atan((first - last).abs() / 2 / distance) : pi;
*/
  Vector2<double> a[4];
  calculateAnglesForPoint(r.bottomLeft,rip->cameraMatrix,rip->image.cameraInfo,a[0]);
  calculateAnglesForPoint(r.bottomRight,rip->cameraMatrix,rip->image.cameraInfo,a[1]);
  calculateAnglesForPoint(r.upperLeft,rip->cameraMatrix,rip->image.cameraInfo,a[2]);
  calculateAnglesForPoint(r.upperRight,rip->cameraMatrix,rip->image.cameraInfo,a[3]);
  
  bound.addX(a[0].x,blc);
  bound.addX(a[1].x,brc);
  bound.addX(a[2].x,ulc);
  bound.addX(a[3].x,urc);

  bound.addY(a[0].y,blc);
  bound.addY(a[1].y,brc);
  bound.addY(a[2].y,ulc);
  bound.addY(a[3].y,urc);

  rip->landmarksPercept.addGoal(greatest.bottomColor, ownTeamColorIsBlue, bound);
  
  //if(greatest.skyblue != 0) {
  //  OUTPUT(idText,text,"goal recognized : skyblue");
  //}
  //else OUTPUT(idText,text,"goal recognized : yellow");

  greatest.goalValidity = 1;
  
  LINE(imageProcessor_flagsAndGoals,
    greatest.rect.upperLeft.x,greatest.rect.upperLeft.y,
    greatest.rect.upperRight.x,greatest.rect.upperRight.y,
    1.5, Drawings::ps_solid, Drawings::white);

  LINE(imageProcessor_flagsAndGoals,
    greatest.rect.bottomLeft.x,greatest.rect.bottomLeft.y,
    greatest.rect.bottomRight.x,greatest.rect.bottomRight.y,
    1.5, Drawings::ps_solid, Drawings::white);

  LINE(imageProcessor_flagsAndGoals,
    greatest.rect.upperLeft.x,greatest.rect.upperLeft.y,
    greatest.rect.bottomLeft.x,greatest.rect.bottomLeft.y,
    1.5, Drawings::ps_solid, Drawings::white);

  LINE(imageProcessor_flagsAndGoals,
    greatest.rect.upperRight.x,greatest.rect.upperRight.y,
    greatest.rect.bottomRight.x,greatest.rect.bottomRight.y,
    1.5, Drawings::ps_solid, Drawings::white);
  
}

void BoxSpecialist::calculateAnglesForPoint
(
  const Vector2<double>& point, 
  const CameraMatrix& cameraMatrix, 
  const CameraInfo& cameraInfo, 
  Vector2<double>& angles
 )
{
  double factor = ((double)cameraInfo.resolutionWidth / 2.0) / tan(cameraInfo.openingAngleWidth / 2.0);
  
  Vector3<double> vectorToPoint(
    factor,
    cameraInfo.resolutionWidth / 2 - point.x,
    cameraInfo.resolutionHeight / 2 - point.y);
  
  Vector3<double> vectorToPointWorld = 
    cameraMatrix.rotation * vectorToPoint;
  
  angles.x =
    atan2(vectorToPointWorld.y,vectorToPointWorld.x);
  
  angles.y =
    atan2(
    vectorToPointWorld.z,
    sqrt(sqr(vectorToPointWorld.x) + sqr(vectorToPointWorld.y)) 
    );
}

bool BoxSpecialist::calculatePointOnField
(
 const double x,
 const double y,
 const CameraMatrix& cameraMatrix,
 const CameraInfo& cameraInfo,
 Vector2<int>& pointOnField
 )
{
#ifndef _WIN32 // don't recalculate on real robot
  static 
#endif
  double xFactor = tan(cameraInfo.openingAngleWidth / 2) / (cameraInfo.resolutionWidth / 2),
         yFactor = tan(cameraInfo.openingAngleHeight / 2) / (cameraInfo.resolutionHeight / 2);

  Vector3<double> 
    vectorToCenter(1, (cameraInfo.resolutionWidth / 2 - 0.5 - x) * xFactor, (cameraInfo.resolutionHeight / 2 - 0.5 - y) * yFactor);
  Vector3<double> 
    vectorToCenterWorld = cameraMatrix.rotation * vectorToCenter;

  //Is the point above the horizon ? - return
  if(vectorToCenterWorld.z > -5 * yFactor) return false;
  
  double a1 = cameraMatrix.translation.x,
         a2 = cameraMatrix.translation.y,
         a3 = cameraMatrix.translation.z,
         b1 = vectorToCenterWorld.x,
         b2 = vectorToCenterWorld.y,
         b3 = vectorToCenterWorld.z;
  
  pointOnField.x = int((a1 * b3 - a3 * b1) / b3 + 0.5);
  pointOnField.y = int((a2 * b3 - a3 * b2) / b3 + 0.5);

  return abs(pointOnField.x) < 10000 && abs(pointOnField.y) < 10000;
}

void BoxSpecialist::createValidities(BoxSpecialist::SegmentInfo& info){
  //only landmark validation done yet
  if (info.pink== 0 || info.yellow == 0) info.lmValidityYellow = 0;
  else info.lmValidityYellow = 1-  2 *  (fabs((double) info.yellow/(double)(info.yellow + info.pink) - 0.5)  );

  if (info.pink== 0 || info.skyblue == 0) info.lmValiditySkyblue = 0;
  else info.lmValiditySkyblue = 1-  2 *  (fabs((double) info.skyblue/(double)(info.skyblue + info.pink) - 0.5)  );  
}

/*void BoxSpecialist::detectFreePartOfGoal(SegmentInfo& goal){
  Rectangle& r = goal.rect;
  const int maxParts = 3;
  Vector2<double> south = pHorizon.direction;
  Vector2<double> north(-pHorizon.direction.x,-pHorizon.direction.y);
  Vector2<double> east = horizon.direction;
  Vector2<double> west(-horizon.direction.x,-horizon.direction.y);

  edgeScanner.threshold = 40;
  Vector2<int> bottomLeft((int) r.bottomLeft.x,(int)r.bottomLeft.y);
  Vector2<int> bottomRight((int) r.bottomRight.x,(int)r.bottomRight.y);
  Vector2<int> vecUp(
    (int)((r.upperLeft.x - r.bottomLeft.x)/4),
    (int)((r.upperLeft.y - r.bottomLeft.y)/4));

  Vector2<int> scan,finish,temp;
  Vector2<int> freeParts[maxParts][2];
  bool found;
  colorClass col = noColor;
  
  //scan for free part from the left side
  //edgeDetection and colorScanning are combined here again.
  finish = bottomRight + vecUp;
  temp = bottomLeft + vecUp;
  scan = temp;
  scan.x = (int)(scan.x + 3*east.x);
  scan.y = (int)(scan.y + 3*east.y);

  for(int i = 0;i<maxParts;i++){
    
    edgeScanner.scan(scan.x,scan.y,finish.x,finish.y);
    freeParts[i][0] = temp;
    freeParts[i][1] = scan;
    edgeScanner.skip(scan.x,scan.y);
      
    do{
      found = edgeScanner.colorScan(scan.x,scan.y,finish.x,finish.y,col);
      temp.x = scan.x;
      temp.y = scan.y;
      if (col == yellow){
        edgeScanner.scan(scan.x,scan.y,west);
        break;
      }
    }while (found && col != yellow);
  }
}*/

void BoxSpecialist::detectFreePartOfGoal(){
  vector<Vector2<int> >::iterator it = bottomPoints.begin();
  vector<Vector2<int> >::iterator lit = it;
  vector<Vector2<int> >::iterator last = it;

  vector<Vector2<int> >::iterator leftBorder = it;
  vector<Vector2<int> >::iterator rightBorder = it;

  int maxRange = 0;
  int clipRange = 5;
  int currentRange = 0;
  
  for(it++;it!=bottomPoints.end();it++){
    if ((*last - *it).abs() > clipRange){
      currentRange = (*last - *lit).abs();
      if (currentRange > maxRange){ 
        leftBorder = lit;
        rightBorder = last;
        maxRange = currentRange;
      }
      lit = it;
    }
  }
  
  //nothing found
  if (leftBorder==rightBorder)return;

  Vector2<double> leftAngle,rightAngle;

  LINE(imageProcessor_flagsAndGoals,
    leftBorder->x,leftBorder->y, 
    rightBorder->x,rightBorder->y, 
    2, Drawings::ps_solid, Drawings::green);

  Geometry::calculateAnglesForPoint(
    *leftBorder,rip->cameraMatrix,rip->image.cameraInfo,leftAngle);
  Geometry::calculateAnglesForPoint(
    *rightBorder,rip->cameraMatrix,rip->image.cameraInfo,rightAngle);

  Vector2<double> angles;
  Geometry::calculateAnglesForPoint(
    Vector2<int>(rip->image.cameraInfo.resolutionWidth / 2, rip->image.cameraInfo.resolutionHeight / 2),
    rip->cameraMatrix, rip->image.cameraInfo, angles);

  Pose2D camera(angles.x, Vector2<double>(
    rip->cameraMatrix.translation.x,
    rip->cameraMatrix.translation.y));

  Vector2<double> middleAngle = (leftAngle + rightAngle)/2;

  double midDir = atan2(middleAngle.x,middleAngle.y);
  Vector2<double> first2 = (camera + Pose2D(leftAngle)).translation,
                    last2 = (camera + Pose2D(rightAngle)).translation;

  rip->obstaclesPercept.angleToFreePartOfGoalWasDetermined[ObstaclesPercept::opponentGoal] = true;
  rip->obstaclesPercept.angleToFreePartOfGoal[ObstaclesPercept::opponentGoal] = midDir;
  double distance = ((first2 + last2) / 2).abs();
  rip->obstaclesPercept.distanceToFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance;
  rip->obstaclesPercept.widthOfFreePartOfGoal[ObstaclesPercept::opponentGoal] = distance ? 2 * atan((leftAngle - rightAngle).abs() / 2 / distance) : pi;  
}
  
/*bool RFieldSpecialist::isValid(slist<figure>& lst, lstLineOnField& lines) {

  unsigned int size_t = 0, max_t = 3;
  unsigned int i;

  slist<figure>::iterator first(lst.front());
  slist<figure>::iterator next = first;
  

  if (!(next+=step_alpha)) return false;

  do {
    
    point p1 = first.get()->toConsider();
    point p2 = next.get()->toConsider();

    double angle = theta(p1,p2);


    next = ++first;

  } while (next+=step_alpha);

 
  bool valid = false;
  for (i = 0;i<=size_t;++i) {
    //OUTPUT(idText,text, "sous-seg " << i << " droit à " << tab_nb[i] << " elts : " << tab_angle[i]);
    if (tab_nb[i] >= max_t) {
      
      valid = true;

      lines.push_front(tab_line[i]);
    } 
  }

  return valid;

}*/

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

* did further code clean up
*
* Revision 1.3  2004/09/02 07:59:29  schmidtb
* Added RasterImageProcessor to repository, because we used it for the OpenChallenge.
*
* Revision 1.50  2004/06/03 13:53:50  schmidtb
* returned to the previous version
*
* Revision 1.48  2004/05/25 13:27:33  schmidtb
* modified version of rip for open-challenge
*
* Revision 1.48  2004/05/18 10:52:14  pg_besc
* version of the last game at US-Open
*
* Revision 1.47  2004/04/15 19:09:01  pg_besc
* merged code
*
* Revision 1.46  2004/03/25 15:20:58  pg_besc
* made some changes
*
* Revision 1.45  2004/03/17 22:31:34  schmidtb
* integrated RFieldStateMachine and horizonal grid scan
*
* Revision 1.44  2004/03/15 09:25:43  rossdeutscher
* Commented out a bug.
*
* Revision 1.43  2004/03/11 20:54:14  schmidtb
* *** empty log message ***
*
* Revision 1.42  2004/03/11 20:33:18  schmidtb
* new version of rip
*
* Revision 1.41  2004/02/28 17:16:47  schmidtb
* debugged and made some changes
*
* Revision 1.40  2004/02/26 14:31:32  schmidtb
* comment some outputs
*
* Revision 1.39  2004/02/25 19:04:29  schmidtb
* removed return statement
*
* Revision 1.38  2004/02/25 15:43:25  schmidtb
* debugged search landmarks
*
* Revision 1.37  2004/02/25 13:30:52  schmidtb
* debugged search landmarks
*
* Revision 1.36  2004/02/18 16:35:46  schmidtb
* removed errors
*
* Revision 1.35  2004/02/18 14:56:19  neubach
* new Segmentation established, code not cleared at all
*
* Revision 1.32  2004/02/05 11:51:17  schmidtb
* now BoxSpecialist recognizes landmarks and goals
*
* Revision 1.31  2004/02/04 23:19:38  schmidtb
* debugged and added calculation for angles to BoxSpecialist
*
* Revision 1.30  2004/02/04 19:09:32  schmidtb
* *** empty log message ***
*
* Revision 1.29  2004/02/04 15:05:20  schmidtb
* warnings removed
*
* Revision 1.28  2004/02/04 13:00:49  schmidtb
* new version of BoxSpecialist
*
* Revision 1.27  2004/02/02 19:50:46  deom
* some modifications,  in order to use the new fonctions added
*
* Revision 1.26  2004/02/02 16:56:52  roefer
* Warnings removed
*
* Revision 1.25  2004/02/02 13:42:11  schmidtb
* merged sources of RIP. added som functions.
*
* Revision 1.24  2004/02/01 13:33:40  deom
* major improvements :
* - validity analysis added
* - bug fixed
* - fonction calls restructurated
* - no warnigs this time :)  dortmund won't suck anymore ...
*
* Revision 1.23  2004/01/29 09:16:19  schumann
* unsucked.
*
* Revision 1.22  2004/01/29 00:14:07  deom
* bug bixed
*
* Revision 1.20  2004/01/28 10:08:52  deom
* added some code
*
* Revision 1.19  2004/01/27 21:30:34  kerdels
* warnings removed...
*
* Revision 1.18  2004/01/27 21:26:34  deom
* *** empty log message ***
*
* Revision 1.16  2004/01/27 21:05:34  schumann
* added log
*
*/

---