Modules/ImageProcessor/RasterImageProcessor/RasterSpecialist.cpp

Go to the documentation of this file.

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

using namespace std;

typedef list<list<RasterSpecialist::LinePair> > SegmentList;

RasterSpecialist::RasterSpecialist(RasterImageProcessor &processor)
: rip(&processor),
  Y_VALUE(0),
  U_VALUE(cameraResolutionHeight_ERS7),
  V_VALUE(2 * cameraResolutionHeight_ERS7),
  imageWidth(cameraResolutionWidth_ERS7),
  imageHeight(cameraResolutionHeight_ERS7) 
{ 
  postScanNeeded = false;
  preScanNeeded = false;
}

RasterSpecialist::~RasterSpecialist()
{

}

RasterSpecialist::LinePair::LinePair():
  v1(0,0),v2(0,0),segment(-1),color(noColor)
{

}

RasterSpecialist::LinePair::LinePair(Vector2<int> vec1, Vector2<int> vec2):
  v1(vec1),v2(vec2),segment(-1),color(noColor)
{

}

RasterSpecialist::LinePair::LinePair(Vector2<int> vec1,Vector2<int> vec2,colorClass c):
  v1(vec1),v2(vec2),color(c)
{

}

RasterSpecialist::Box::Box(int minX,int minY,int maxX,int maxY):
  minX(minX),maxX(maxX),minY(minY),maxY(maxY)
{
    
}
 
bool RasterSpecialist::createGridSegments(std::list<GridLP>& lines,
  std::vector<std::list<GridLP> >& segments,int xSpace, int ySpace){
if (lines.size() == 0) return false;
    int maxSegments=20;
  int segs[20];
  int finalSegs[20];
  int segCount = 0;
  int finalCount = 0;
  list<GridLP>::iterator it = 0;
  list<GridLP>::iterator rit = 0;

  for (unsigned int i=0;i<20;i++) {
     segs[i]=0;
     finalSegs[i]=0;
  }

  it=lines.begin();
  it ->segment = segCount;
  segs[segCount] = segCount;
  segCount++;


    for (it++;it!=lines.end();it++) {
    rit = it;
    int lead = -1;
    
    do{
      rit--;
      if((it->column - rit->column) > ySpace) break;
      if (!gridFit(*it,*rit,xSpace)) continue;
      if (lead == -1){
        //erstem Segment zuordnen
        lead = segs[rit->segment];
        it->segment = lead;

      }
      else{
        int newSeg = segs[rit->segment];
        if (newSeg == lead) continue;
        
        //weiteres segment gefunden
        //segmente werden vereinigt
        if (lead < newSeg)
          segs[newSeg] = lead;
        else segs[lead] = newSeg;

      }
    }while( (rit != lines.begin()));
    if (lead == -1){
      //neues Segment gefunden
      it->segment = segCount;
      segs[segCount] = segCount;
      segCount++;
      if (segCount>maxSegments-1)
        return false;
    }
    
  }

  // calculating final segment number
  for(int j=0;j<maxSegments;++j){
    if (j == segs[j]){ 
      finalSegs[j] = finalCount;
      segments.push_back(list<GridLP>());
      finalCount++;
    }
    else finalSegs[j] = finalSegs[segs[j]];
  }

  //collecting linepairs in calculated segments
  for(it = lines.begin();it != lines.end();++it)
    segments[finalSegs[it->segment]].push_back(*it);

  //OUTPUT(idText,text,"lines in segments "<<  linesAfter);
//  OUTPUT(idText,text,"found segments "<<  segments.size());

  //segmentation finished successfully
  return true;
}

bool RasterSpecialist::createSegmentsFromLines2(std::list<LinePair>& lines,
  std::vector<std::list<LinePair> >& segments,int xSpace, int ySpace){
 //   OUTPUT(idText,text,"size before "<<  lines.size());
  if (lines.size() == 0) return false;
    const int maxSegments=50;
  int segs[maxSegments];
  int finalSegs[maxSegments];
  int segCount = 0;
  int finalCount = 0;
  list<LinePair>::iterator it = 0;
  list<LinePair>::iterator rit = 0;

  for (int i=0;i<maxSegments;i++) {
     segs[i]=0;
     finalSegs[i]=0;
  }

  it=lines.begin();
  it ->segment = segCount;
  segs[segCount] = segCount;
  segCount++;


    for (it++;it!=lines.end();it++) {
    rit = it;
    int lead = -1;
    
    do{
      rit--;
      if((it->v1.y - rit->v1.y) > ySpace) break;
      if (!linePairsFit(*it,*rit,xSpace)) continue;
      if (lead == -1){
        //erstem Segment zuordnen
        lead = segs[rit->segment];
        it->segment = lead;

      }
      else{
        int newSeg = segs[rit->segment];
        if (newSeg == lead) continue;
        
        //weiteres segment gefunden
        //segmente werden vereinigt
        if (lead < newSeg)
          segs[newSeg] = lead;
        else segs[lead] = newSeg;

      }
    }while( (rit != lines.begin()));
    if (lead == -1){
      //neues Segment gefunden
      it->segment = segCount;
      segs[segCount] = segCount;
      segCount++;
      if (segCount>maxSegments-1)
        return false;
    }
    
  }

  // calculating final segment number
  for(int j=0;j<maxSegments;++j){
    if (j == segs[j]){ 
      finalSegs[j] = finalCount;
      segments.push_back(list<LinePair>());
      finalCount++;
    }
    else finalSegs[j] = finalSegs[segs[j]];
  }

  //collecting linepairs in calculated segments
  for(it = lines.begin();it != lines.end();++it)
    segments[finalSegs[it->segment]].push_back(*it);

  //OUTPUT(idText,text,"lines in segments "<<  linesAfter);
//  OUTPUT(idText,text,"found segments "<<  segments.size());

  //segmentation finished successfully
  return true;
}


/** Clusters the LinePairs and puts the segments in v */
void RasterSpecialist::createSegmentsFromLines(std::list<LinePair>& lines,
  std::vector<std::list<LinePair> >& segments,int xSpace, int ySpace){
//this algorithm runs on simulator, but seems to have bugs on the robot!
  OUTPUT(idText,text,"size before "<<  lines.size());
  if (lines.size() == 0) return;
  int allowedSpace = ySpace;
  segList.clear();
  //SegmentList segList;  
  SegmentList::iterator sit = 0;
  SegmentList::iterator sit2 = 0;
  list<LinePair>::iterator lit = lines.begin();
  list<LinePair>::iterator it = 0;
  list<LinePair>::iterator it2 = 0;
  LinePair compare;
  list<LinePair> *currentSeg;
    
  segList.push_back(list<LinePair>());
  segList.back().push_back(*lit);
  //compare = segList.front().back();
  //lines.pop_front();
  
  for(++lit;lit!=lines.end();++lit){
    
    LinePair& current = *lit;
    //lines.pop_front();
    
    list<LinePair > *lead = 0;

    for(sit = segList.begin();sit != segList.end();sit++){
      if (sit->empty()) continue;
      currentSeg = &(*sit);
      it = currentSeg->end();
      it--;
      compare = *it;
      //continue; 
      //remove segment if it's far away
      if (current.v1.y - compare.v1.y > allowedSpace){
        //if (!currentSeg->empty())
        segments.push_back(*currentSeg);

        sit2 = sit;
        sit++;
        segList.erase(sit2); //könnte Probleme machen
        sit--;
        continue;
      }

      if (linePairsFit(current,compare,xSpace)){
        currentSeg->push_back(current);
        lead = currentSeg;
        continue;
      }
      else if(it == currentSeg->begin()) continue;  

      //compare all Line Pairs of currentSeg with current
      //ist it != 0 sicher?
      do{
        it--;
        compare = *it;
        //compare all relevant LinePairs
        if(linePairsFit(current,compare,xSpace)){
          if(!lead){
          //mark lead and put the segment to it
            currentSeg->push_back(current);
            lead = currentSeg;
            break;
          }
          else{
          //merge two segments and remove the second one from segList
            lead->merge(*currentSeg);
            sit2 = sit;
            sit++;
            segList.erase(sit2); //könnte Probleme machen
            sit--;
            sit2 = 0;
            break;
          }
        }
      


      }while(it != currentSeg->begin() && current.v1.y - compare.v1.y <= allowedSpace);


    }
    //no segment found for current, so create new segment with current        
    if (!lead){
      segList.push_back(list<LinePair>());
      segList.back().push_back(current);
    }


  }
  
  //put all non-finished segments in segments-vector
  if (!segList.empty())
    for(sit = segList.begin();sit != segList.end();sit++){
    currentSeg = &(*sit);
    if (!currentSeg->empty())
      segments.push_back(*currentSeg);
  }
  
  //if no segments found return (only a bug-fix)
  if (segments.empty()) return;

  vector<list<LinePair> >::iterator iter = segments.begin();
  int linesAfter = 0;
  while(iter != segments.end()){
    currentSeg = &(*iter);
    linesAfter+= currentSeg->size();
    iter++;
  }
  OUTPUT(idText,text,"lines in segments "<<  linesAfter);
  OUTPUT(idText,text,"found segments "<<  segments.size());
  OUTPUT(idText,text,"size of segList "<<  segList.size());
}

bool RasterSpecialist::doColorSegmentation(std::vector<LinePair>& lines,
  std::vector<std::vector<LinePair> >& segments,int xSpace,int ySpace){
  
  if (lines.size() == 0) return false;
    int maxSegments=20;
  int segs[20];
  int finalSegs[20];
  int segCount = 0;
  int finalCount = 0;
  vector<LinePair>::iterator it;
  vector<LinePair>::iterator rit;

  for (unsigned int i=0;i<20;i++) {
     segs[i]=0;
     finalSegs[i]=0;
  }

  it=lines.begin();
  it ->segment = segCount;
  segs[segCount] = segCount;
  segCount++;


    for (it++;it!=lines.end();it++) {
    rit = it;
    int lead = -1;

    /*if (rit != lines.begin()) */do{
      rit--;
      if (!linesFit(*it,*rit,xSpace)) continue;
      if (lead == -1){
        //erstem Segment zuordnen
        lead = segs[rit->segment];
        it->segment = lead;

      }
      else{
        int newSeg = segs[rit->segment];
        if (newSeg == lead) continue;
        
        //weiteres segment gefunden
        //segmente werden vereinigt
        if (lead < newSeg)
          segs[newSeg] = lead;
        else segs[lead] = newSeg;

      }
    }while(((it->v1.y - rit->v1.y) <= ySpace) && (rit != lines.begin()));
    if (lead == -1){
      //neues Segment gefunden
      it->segment = segCount;
      segs[segCount] = segCount;
      segCount++;
      if (segCount>maxSegments-1)
        return false;
    }
    
  }

  // calculating final segment number
  for(int j=0;j<segCount;++j){
    if (j == segs[j]){ 
      finalSegs[j] = finalCount;
      segments.push_back(vector<LinePair>());
      finalCount++;
    }
    else finalSegs[j] = finalSegs[segs[j]];
  }

  //collecting linepairs in calculated segments
  for(it = lines.begin();it != lines.end();++it)
    segments[finalSegs[it->segment]].push_back(*it);

  //OUTPUT(idText,text,"lines in segments "<<  linesAfter);
  OUTPUT(idText,text,"found segments "<< segments.size());

  //segmentation finished successfully
  return true;
}

//TODO: debug calculation of convex hull
void RasterSpecialist::createConvexPoly(std::list<LinePair>& input,std::vector<Vector2<int> > output){
  list<LinePair>::iterator it = input.begin();
  output.push_back(it->v1);
  list<LinePair>::iterator rit = 0;
  
  double t1 = 0;
  double t2 = 0;
  int row = it->v1.y;
  while (it != input.end() && it->v1.y == row) it++;
  t1 = theta(output.back(),it->v1);
  output.push_back(it->v1);

  //finding Convex-Hull along the left side top-down
  for(;it != input.end();++it){
    if (it->v1.y == row) continue;
    
    row = it->v1.y;
    t2 = theta(output.back(),it->v1);
    
    while (t1 >= t2){
      output.pop_back();
      if (output.size() < 2){ 
        t2 = theta(output.front(),it->v1);
        break;
      }
      t1 = theta(output[output.size()-2],output.back());
      t2 = theta(output.back(),it->v1);
    }
    output.push_back(it->v1);
    t1 = t2;
  }
  
  t1 = theta(output.back(),input.back().v2);
  output.push_back(input.back().v2);  
  
  if (it == input.begin()) return;

  //finding Convex-Hull along the right side bottom-up
  do{
    it--;
    if (it->v2.y == row) continue;
    
    row = it->v2.y;
    t2 = theta(output.back(),it->v2);
    
    while (t1 >= t2){
      output.pop_back();
      if (output.size() < 2){ 
        t2 = theta(output.front(),it->v2);
        break;
      }
      t1 = theta(output[output.size()-2],output.back());
      t2 = theta(output.back(),it->v2);
    }

    output.push_back(it->v2);
    t1 = t2;
  }while(it!=input.begin());

}


/*
* Change log :
* 
* $Log: RasterSpecialist.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.28  2004/05/25 13:27:34  schmidtb
* modified version of rip for open-challenge
*
* Revision 1.30  2004/05/22 16:01:49  pg_besc
* -modified version of rip for bridge-recognition
*
* Revision 1.29  2004/04/22 16:57:26  pg_besc
* new version of RBallSpecialist2, now omidirectional scans for detection
*
* Revision 1.28  2004/04/20 07:50:27  pg_besc
* new version of pre scan
*
* Revision 1.27  2004/03/25 15:24:55  pg_besc
* made some changes
*
* Revision 1.26  2004/03/11 20:32:58  schmidtb
* new version of rip
*
* Revision 1.25  2004/03/04 09:54:50  schmidtb
* color correction integrated
*
* Revision 1.24  2004/03/03 12:53:21  schmidtb
* color correction integrated
*
* Revision 1.23  2004/02/28 17:16:47  schmidtb
* debugged and made some changes
*
* Revision 1.22  2004/02/26 14:31:32  schmidtb
* comment some outputs
*
* Revision 1.21  2004/02/19 14:22:15  neubach
* - debugged Code of RIP
*
* Revision 1.20  2004/02/18 14:56:19  neubach
* new Segmentation established, code not cleared at all
*
* Revision 1.19  2004/02/05 11:51:18  schmidtb
* now BoxSpecialist recognizes landmarks and goals
*
* Revision 1.18  2004/02/04 13:09:48  schmidtb
* errors removed
*
* Revision 1.17  2004/02/04 13:00:49  schmidtb
* new version of BoxSpecialist
*
* Revision 1.16  2004/02/02 13:42:12  schmidtb
* merged sources of RIP. added som functions.
*
* Revision 1.15  2004/01/31 11:45:02  hyung
* modified enemyValidity-calculation;
* established basical enviroment for TrikotErkennung, based on Arrays and Lists, changes will take affect only #ifdef TrikotErkennung!
*
* Revision 1.14  2004/01/26 22:21:09  schmidtb
* bugfixed BoxSpecialist
*
* Revision 1.13  2004/01/20 10:23:58  schmidtb
* fixed bugs in RGoalSpecialist
*
* Revision 1.12  2004/01/19 16:18:45  schmidtb
* GoalSpecialist recognizes Landmarks
*
* Revision 1.11  2004/01/15 23:12:34  roefer
* Warnings removed
*
* Revision 1.10  2004/01/15 13:45:02  schmidtb
* segmentation established
*
* Revision 1.8  2003/12/30 20:12:03  roefer
* Image size is now 208 x 160. Smaller images are placed in the upper left corner
*
* Revision 1.7  2003/12/15 13:55:32  schmidtb
* Merged and patched new version of RasterImageProcessor.
*
* Revision 1.6  2003/12/08 16:49:49  roefer
* signed/unsigned mismatch corrected
*
* Revision 1.5  2003/12/08 15:02:55  schmidtb
* new version of RIP
*
* Revision 1.4  2003/12/04 09:51:23  schmidtb
* better BallSpecialist
*
* Revision 1.3  2003/11/28 14:50:01  dueffert
* bugs and warnings fixed
*
* Revision 1.2  2003/11/20 10:26:56  schmidtb
* Ball Detection added
*
* Revision 1.1  2003/11/12 13:13:20  schmidtb
* new RasterImageProcessor added
*
*
*/

---