Modules/ImageProcessor/RasterImageProcessor/RFieldSpecialist.cpp

Go to the documentation of this file.

/**
* @file Modules/ImageProcessor/RasterImageProcessor/RFieldSpecialist.cpp
* 
* This file contains an class for image processing and object recognition 
*
* @author <A href="damd@free.fr">DEOM Damien</A>
* 
*/

#include "RFieldSpecialist.h"


RFieldSpecialist::RFieldSpecialist(RasterImageProcessor &processor,RasterStrategy &strat):
  RasterSpecialist(processor), edgeScanner(processor)
{
  strategy = &strat;

  segments_img.reserve(3);

  defaultParam = new processParam();

  lineInside = LinesPercept::numberOfLineTypes;

}

RFieldSpecialist::~RFieldSpecialist()
{
  delete defaultParam;
}

int RFieldSpecialist::getType()
{
  return __RFieldSpecialist;  
}

void RFieldSpecialist::init()
{ 
  preScanNeeded = true;
  postScanNeeded = false;

  lst_pts.clear();

  int i;
  for (i = 0; i< (int)segments_img.size(); ++i)
    segments_img[i].clear();
}

void RFieldSpecialist::invokeOnPreScan(int x,int y){
  
  if (!strategy->insideField) temp = Vector2<int>(x,y);
  else{
    
    point tmp2 = Vector2<int>(x,y);

    //search for edges to widen the pairs to the edges of the object

    edgeScanner.threshold = 20;
    edgeScanner.scanWest(temp.x,temp.y);
    edgeScanner.scanEast(tmp2.x,tmp2.y);

    if (tmp2.x - temp.x < 4) return;

    horLinePair* lp= new horLinePair(temp,tmp2);

    lst_pts.push_front(lp);

    LINE(imageProcessor_ground,
      lp->pt1().x,lp->pt1().y, lp->pt2().x,lp->pt2().y, 
      0.5, Drawings::ps_solid, Drawings::gray);
    
  }
}

int RFieldSpecialist::id(LinesPercept::LineType l) {
  switch (l) {
    case LinesPercept::field :       return 0;
    case LinesPercept::border :      return 1;
    case LinesPercept::yellowGoal :  return 2;
    case LinesPercept::skyblueGoal : return 3;
    default : return -1;
  }
}

LinesPercept::LineType RFieldSpecialist::id(int i) {
  switch (i) {
    case 0 : return LinesPercept::field ;
    case 1 : return LinesPercept::border;
    case 2 : return LinesPercept::yellowGoal;
    case 3 : return LinesPercept::skyblueGoal;
    default : return LinesPercept::field;
  }
}

void RFieldSpecialist::invokeOnPostScan(int x,int y) {}


void RFieldSpecialist::addLinePoint(LinesPercept::LineType line , point & v, int& p) {

  segments_img[id(line)].push_front(new edge(v,(unsigned short)p));

}


bool isEqual (double & a1, double & a2, float tol) {
  return (fabs(a1-a2)<=tol);
}


void RFieldSpecialist::fusionLines(lstLinePair& lst) {

  lstLinePair::iterator it, it2;
  double angle;


  for (it = lst.front();!it.end();++it) {

    it2 = lst.front();

    while(!it2.end() && it2->getNext() && it2->getNext()!= *it) {

      angle = (int)theta2(*it, it2->getNext())%180;

      Vector2<double> v = mil (**it2);

      if (angle<=8 || angle >= 172 &&
        Geometry::getDistanceToLine(makeLine(*it),v) <1) {

          point p1, p2;

          if (Geometry::distance(it->p1, it2->getNext()->p1) >
            Geometry::distance(it->p1, it2->getNext()->p2))
            p1 = it2->getNext()->p1;

          else p1 = it2->getNext()->p2;

          if (Geometry::distance(it2->getNext()->p1, it->p1) >
            Geometry::distance(it2->getNext()->p1, it->p2))
            p2 = it->p1;

          else p2 = it->p2;

          it->p1 = p1;
          it->p2 = p2;

          lst.erase(*it2);

      } else
        if(!++it2) break;

    }
  }
}

bool RFieldSpecialist::makeLines(lstFig& lst, lstLinePair& lines,
                 const processParam* prParam) {

  int size_t = -1;
  unsigned int min_size = prParam->min_size;
  unsigned int step_alpha = prParam->step_alpha;
  unsigned int tol_alpha = prParam->tol_alpha;

  unsigned int i;

  lstFig::iterator first(lst.front());
  lstFig::iterator next = first;
  

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

  unsigned c = lst.getSize(); 

  double * tab_angle = new double[c];
  unsigned int * tab_nb = new unsigned int[c];
  LinePair2** tab_line = new LinePair2*[c];


  for(i= 0;i<c;++i) {tab_line[i] = 0;tab_angle[i]=0;tab_nb[i]=0;}

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

    double angle = theta2(p1,p2);

    bool too_inclined = (size_t>=0) ?
      !isEqual(tab_angle[size_t],angle,(float)tol_alpha) : false;

    if (size_t==-1) {  // initial state

        ++size_t;
        tab_line[size_t] = new LinePair2(p1,p2);
        tab_angle[size_t] = angle;
        tab_nb[size_t] = 1;

    } else if (!too_inclined) { // if aligned to the precedent line

      ++tab_nb[size_t];
      tab_line[size_t]->p2 = p1;  

    } else {  // add a new line

      if (tab_nb[size_t]<min_size) {
        delete tab_line[size_t];
      } else {
        ++size_t;
      }
        
      tab_line[size_t] = new LinePair2(p1, p2);
      tab_angle[size_t] = angle;
      tab_nb[size_t] = 1;
    }

    next = ++first;

  } while (next+=step_alpha);

 
  bool valid = false;
  for (i = 0;i<=(unsigned)size_t;++i) {
    
    if (tab_nb[i] >= min_size) {
      
      valid = true;
      lines.push_front(tab_line[i]);
    } else {

      delete tab_line[i];

    }
  }

  delete [] tab_angle;
  delete [] tab_line;
  delete [] tab_nb;

  return valid;

}


void RFieldSpecialist::executePostProcessing(){

  if (lst_pts.getSize() < 4) return;

  createLinearSegment(lst_pts);

  lstFig::iterator it = lst_pts.front();

  lstLinePair res;

  if (!makeLines(lst_pts, res, defaultParam)) return;

  lstLinePair::iterator cur2 = res.front();
  
  while (!cur2.end()) {

    LINE(imageProcessor_ground,
    cur2->p1.x, cur2->p1.y, cur2->p2.x,cur2->p2.y,
    1, Drawings::ps_solid,
    Drawings::white);

    ++cur2;
  } 

  fusionLines(res);

  checkRamp(res);

  res.clear();
}

void RFieldSpecialist::drawCross(goal_line g, point& v) {

  switch(g) {
    case YELLOW :
      LINE(imageProcessor_ground,
        v.x-4,v.y-4,v.x+4, v.y+4, 
        2, Drawings::ps_solid, Drawings::yellow);
      LINE(imageProcessor_ground,
            v.x-4,v.y+4,v.x+4, v.y-4, 
        2, Drawings::ps_solid, Drawings::yellow);
      break;
    case SKYBLUE:
      LINE(imageProcessor_ground,
        v.x-4,v.y-4,v.x+4, v.y+4, 
        2, Drawings::ps_solid, Drawings::blue);
      LINE(imageProcessor_ground,
            v.x-4,v.y+4,v.x+4, v.y-4, 
        2, Drawings::ps_solid, Drawings::blue);
      break;
    default: 
      LINE(imageProcessor_ground,
        v.x-4,v.y-4,v.x+4, v.y+4, 
        2, Drawings::ps_solid, Drawings::black);
      LINE(imageProcessor_ground,
        v.x-4,v.y+4,v.x+4, v.y-4, 
        2, Drawings::ps_solid, Drawings::black);
  }

}
void RFieldSpecialist::drawCircle(goal_line g, point& v) {
  switch(g) {
    case YELLOW :
      CIRCLE(imageProcessor_ground,v.x, v.y, 5, 1, 
      Drawings::ps_solid, Drawings::yellow);
      break;
    case SKYBLUE:
      CIRCLE(imageProcessor_ground,v.x, v.y, 5, 1, 
      Drawings::ps_solid, Drawings::blue);
      break;
    default: 
      CIRCLE(imageProcessor_ground,v.x, v.y, 5, 1, 
      Drawings::ps_solid, Drawings::black);
  }
}

void RFieldSpecialist::drawLine(goal_line g, LinePair2& lp) {
  switch(g) {
    case YELLOW :
      LINE(imageProcessor_ground,lp.p1.x, lp.p1.y, lp.p2.x, lp.p2.y, 1,
      Drawings::ps_solid, Drawings::yellow);
      break;
    case SKYBLUE:
      LINE(imageProcessor_ground,lp.p1.x, lp.p1.y, lp.p2.x, lp.p2.y, 1,
      Drawings::ps_solid, Drawings::blue);
      break;
    default: 
      LINE(imageProcessor_ground,lp.p1.x, lp.p1.y, lp.p2.x, lp.p2.y, 1,
      Drawings::ps_solid, Drawings::black);
  }
}

unsigned int RFieldSpecialist::analyzeLines() {

  lstLinePair::iterator it;
  double angle;
  unsigned i;

  // calculate the most representative lines exept borders
  double max_length;
  LinePair2** max_lines = new LinePair2*[vecLines.size()];

  for (i = 1; i<vecLines.size();++i) {

    max_lines[i] = 0;
    max_length = 0;

    for (it = vecLines[i].front();!it.end() ;++it) {

      double length = it->getLength();

      if (length>max_length) {
        max_length = length;
        max_lines[i] = *it;
      }
    }
  }

  max_lines[0] = 0;
  angle = 180;
  if (max_lines[1])
    for (it = vecLines[0].front();!it.end() ;++it) {

      double tmp = 90 - (int)theta2(*it, max_lines[1])%180;
      OUTPUT(idText,text, "analyse mil  " << tmp);
      if (tmp<angle) {
        angle = tmp;
        max_lines[0] = *it;
      }
    }
  
  OUTPUT(idText,text, "max analyse mil  " << angle);
  goal_line seen_goal;

  if (max_lines[2]) {
    seen_goal = YELLOW;
  }
  else if (max_lines[3]) {
    seen_goal = SKYBLUE;
  } else seen_goal = NONE; 


  for (i = 0; i< vecLines.size(); ++i) {

    lstLinePair& lst = vecLines[i];

    for (it = lst.front();!it.end() && it->getNext();++it) {

      angle = (int)theta2(*it, it->getNext())%180;

      if (angle>10 && angle<170) {
    
        point v;
        Geometry::Line l1 = makeLine(*it);
        Geometry::Line l2 = makeLine(it->getNext());

        Geometry::getIntersectionOfLines(l1, l2, v);

        drawCross(seen_goal, v);
      
      }
    }
  }

  double size_fact;

  if (max_lines[1] ) {

    size_fact = (seen_goal==NONE) ? 0 : max_lines[1]->getLength()/max_lines[seen_goal==YELLOW ? 2 : 3]->getLength();

    OUTPUT(idText,text, "size fact " << size_fact);

    if (size_fact<2.6) {
      drawLine(seen_goal, (*max_lines[1]));
    } else {
      LINE(imageProcessor_ground,
      max_lines[1]->p1.x, max_lines[1]->p1.y, 
      max_lines[1]->p2.x, max_lines[1]->p2.y, 
      1, Drawings::ps_solid, Drawings::green);
    }

    if (max_lines[0]) {
      point v;

      Geometry::getIntersectionOfLines(makeLine(max_lines[0]),makeLine(max_lines[1]), v);

      drawCircle(seen_goal, v);
    }
  }


  delete [] max_lines;


  return 0;


}

void RFieldSpecialist::checkRamp(lstLinePair & ramp) {

  lstLinePair::iterator it2;

  double max_length = 0;
  LinePair2* max_line = 0;

  for (it2 = ramp.front();!it2.end() ;++it2) {

    double length = it2->getLength();

    if (length>max_length) {
      max_length = length;
      max_line = *it2;
    }
  }

  if (max_line) 
  { 
    LINE(imageProcessor_ground,
    max_line->p1.x, max_line->p1.y, max_line->p2.x,max_line->p2.y,
    3, Drawings::ps_solid,
    Drawings::green);

    OUTPUT(idText,text, "angle " << (int)theta2(max_line->p1,max_line->p2)%180);

    int px = max_line->p1.y > max_line->p2.y ? max_line->p1.x : max_line->p2.x;
    float d = (float)px/(float)rip->maxX;

    point pStart;
    point pEnd;

    if (max_line->p1.y > max_line->p2.y){
      pStart=max_line->p1;
      pEnd=max_line->p2;
    } else {
      pStart=max_line->p2;
      pEnd=max_line->p1;
    }

    Vector2<int> relativeStart;
    Geometry::calculatePointOnField(
      pStart.x,pStart.y,
      rip->cameraMatrix,
      rip->image.cameraInfo,relativeStart);

    Vector2<int> relativeEnd;

    Geometry::calculatePointOnField(
      pEnd.x,pEnd.y,
      rip->cameraMatrix,
      rip->image.cameraInfo,
      relativeEnd);


    rip->specialPercept.ocRedLine.addPercept(
      relativeStart,relativeEnd
      ,pStart,pEnd,
      (int)theta2(max_line->p1,max_line->p2)%180);

    
    OUTPUT(idText,text, "relative position : " << d);

  }

}

/*
 * Changelog:
 *
 * $Log: RFieldSpecialist.cpp,v $
 * Revision 1.8  2004/09/08 13:31:57  wachter
 * - Fixed some warnings
 *
 * Revision 1.7  2004/09/08 12:41:43  wachter
 * - Fixed some warnings
 * - Fixed some documentation in network-code
 *
 * Revision 1.6  2004/09/07 16:01:15  deom
 * Documentation corrected
 *
 * 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/02 09:31:19  deom
 * commented rfieldspecialist
 *
 * Revision 1.3  2004/09/02 07:59:29  schmidtb
 * Added RasterImageProcessor to repository, because we used it for the OpenChallenge.
 *
 * Revision 1.42  2004/06/17 14:18:22  kerdels
 * fixed bug in rfieldspecialist, changed threshold for bridge-detection
 *
 * Revision 1.41  2004/06/11 12:43:40  deom
 * no message
 *
 * Revision 1.40  2004/06/04 12:43:05  deom
 * optimized
 *
 * Revision 1.39  2004/06/03 14:22:51  deom
 * debugged
 *
 * Revision 1.38  2004/06/02 16:03:53  deom
 * merged different versions of FieldSpecialist
 *
 * Revision 1.37  2004/06/01 18:52:52  deom
 * memory leak problem solved
 *
 * Revision 1.36  2004/05/31 17:19:30  deom
 * *** empty log message ***
 *
 * Revision 1.35  2004/05/27 16:49:30  deom
 * new parameter set + some minor changes
 *
 * Revision 1.34  2004/05/26 14:40:36  schumann
 * detection of red line and special percept
 *
 * Revision 1.33  2004/05/25 13:27:34  schmidtb
 * modified version of rip for open-challenge
 *
 * Revision 1.32  2004/05/22 19:03:40  deom
 * -now recognizes the red line
 * -all the data required for the positionning are computed
 *
 * Revision 1.28  2004/05/22 16:56:39  pg_dade
 * -now recognizes the red line
 * -all the data required for the positionning are computed
 *
 * Revision 1.26  2004/04/15 19:08:34  pg_besc
 * merged code
 *
 * Revision 1.29  2004/03/30 13:51:38  schmidtb
 * integrated input stream from file
 *
 * Revision 1.28  2004/03/29 15:25:16  deom
 * - the basical segmentation tools are now fast completely debbuged.
 * - seem to work pretty good now
 * - somes parameters need to be tuned
 *
 * Revision 1.27  2004/03/23 15:50:26  deom
 * now uses two scan methods
 *
 * Revision 1.26  2004/03/21 21:59:06  deom
 * The new scan functions intergrated to RfieldSpecialist. The lines recognition works at 90%
 *
 * Revision 1.25  2004/03/19 22:06:49  deom
 * now based on  the segmentationTools class
 *
 * Revision 1.24  2004/03/15 15:47:51  roefer
 * Warnings removed
 *
 * Revision 1.23  2004/03/15 11:21:45  deom
 * optimized
 *
 * Revision 1.22  2004/03/15 01:17:35  deom
 * debugged, now recognizes all predetermined forms
 *
 * Revision 1.21  2004/03/14 12:51:58  deom
 * added an accurate scan method
 *
 * Revision 1.20  2004/03/12 14:39:12  deom
 * debugged
 *
 * Revision 1.19  2004/03/12 01:37:58  deom
 * added some functions required for the lines analysis
 *
 * Revision 1.18  2004/03/11 19:21:01  deom
 * pursued debugging and parametrisation
 *
 * Revision 1.17  2004/03/11 12:19:00  deom
 * debbuged RFieldspecialist
 *
 * Revision 1.16  2004/03/01 14:17:25  koh
 * added new strategy "RFlexibleStrategy" + new specialist "EnemyOnlySpecialist";
 * changed references to "RDefaultStrategy" to references to "RasterStrategy" in RFieldSpecialist
 * added Geometry::Line horizon to "RasterStrategy"
 *
 * Revision 1.15  2004/03/01 12:16:28  wachter
 * Bug commented out.
 *
 * Revision 1.14  2004/02/29 20:06:16  deom
 * detects complex lines ( but need to be debuged)
 *
 * Revision 1.13  2004/02/24 20:01:41  wachter
 * urgent message for damien added
 *
 * Revision 1.12  2004/02/23 12:47:07  roefer
 * Warnings removed
 *
 * Revision 1.11  2004/02/22 19:12:54  deom
 * now recognize simple lines
 *
 * Revision 1.9  2004/02/16 05:00:48  deom
 * basical version that show edges
 *
 * Revision 1.8  2004/01/26 22:21:09  schmidtb
 * bugfixed BoxSpecialist
 *
 * Revision 1.7  2004/01/26 14:50:08  wachter
 * Changelog added
 *
 *
 */

---