Modules/ImageProcessor/RasterImageProcessor/RBridgeSpecialist.cpp

Go to the documentation of this file.

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

#include "RBridgeSpecialist.h"
#include "Tools/ColorClasses.h"
#include "Tools/Location.h"

RBridgeSpecialist::SegmentInfo::SegmentInfo():
  lmValidityOrange(0),
  lmValidityYellow(0),
  orange(0),
  yellow(0),
  skyblue(0)
{

}



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

  lineColor=red;
  strategy = &strat;
  preScanNeeded = true;
  postScanNeeded = false;
}


RBridgeSpecialist::~RBridgeSpecialist(){}


int RBridgeSpecialist::getType() {
  return __RBridgeSpecialist;
}

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

//TODO: could be faster
void RBridgeSpecialist::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() < 1) 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));
    createValidities(si);
    infos_img.push_back(si);
  }
}



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

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

  if (!strategy->insideBridge) left_border = cur_pt;
  else {
    LinePair lp(left_border,cur_pt);
    lp.color = getColor(lp.v1.x,lp.v1.y);
    //search for accurate pixels on colortable
    lp.v1 = scanWest(lp.v1.x,lp.v1.y);
    lp.v2 = scanEast(lp.v2.x,lp.v2.y);


    if ((lp.v2.x-lp.v1.x)>1) {
      //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);
      
      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);
  
    }
  }
}

void RBridgeSpecialist::invokeOnPostScan(int x,int y) {
  //not needed
}

void RBridgeSpecialist::executePostProcessing() {
  //OUTPUT(idText,text,"size of lst "<<  lst_pts.size());
  if (lst_pts.size() < 2) return;
  
  int allowedXSpace = 2;
  int allowedYSpace = 2;
  
  horizon = rip->getHorizon();

  pHorizon.base = horizon.base;
  pHorizon.direction = horizon.direction;
  pHorizon.direction.rotateLeft();
  
  createSegmentsFromLines2(lst_pts,segments_img,allowedXSpace,allowedYSpace);
  
  //OUTPUT(idText,text,"size of lst "<<  lst_pts.size());
  //OUTPUT(idText,text,"size of segments "<<  segments_img.size());

  sort<SVector::iterator>(segments_img.begin(),segments_img.end(),ListSizeOrder());
  createBBox();
  //int size = infos_img.size();
  vector<SegmentInfo>::iterator it;
  for(it = infos_img.begin();it != infos_img.end();++it) {
    double width = 0.5;
    double boxWidth;
    findBridgeMark(*it);
    //testing for the right filter
    if ((it->lmValidityOrange > 0.3 || it->lmValidityYellow > 0.3) &&
      (boxWidth = (it->rect.bottomRight - it->rect.bottomLeft).abs()) > 5 &&
      (it->rect.upperLeft - it->rect.bottomLeft).abs() * 3 < boxWidth)
    {
      width = 1;  
      LINE(imageProcessor_general,
          it->rect.upperLeft.x,it->rect.upperLeft.y,it->rect.upperRight.x,it->rect.upperRight.y,
          width, Drawings::ps_solid, Drawings::white);
      LINE(imageProcessor_general,
          it->rect.bottomLeft.x,it->rect.bottomLeft.y,it->rect.bottomRight.x,it->rect.bottomRight.y,
          width, Drawings::ps_solid, Drawings::green);
      LINE(imageProcessor_general,
          it->rect.upperLeft.x,it->rect.upperLeft.y,it->rect.bottomLeft.x,it->rect.bottomLeft.y,
          width, Drawings::ps_solid, Drawings::yellow);
      LINE(imageProcessor_general,
          it->rect.upperRight.x,it->rect.upperRight.y,it->rect.bottomRight.x,it->rect.bottomRight.y,
          width, Drawings::ps_solid, Drawings::blue);
  
    }
  }

}

void RBridgeSpecialist::countColors(RBridgeSpecialist::SegmentInfo& si, list<LinePair>& segment){
  list<LinePair>::iterator lines;
  si.orange = 0;
  si.yellow =0;
  si.skyblue = 0;
  for(lines = segment.begin();lines != segment.end();++lines){
    LinePair& line = *lines;
    switch(line.color){ 
    case orange: 
      si.orange += 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.orange > si.skyblue? max = si.orange : max = si.skyblue;
  if (si.yellow > max) max = si.yellow;
  
  min = max/8;

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

void RBridgeSpecialist::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 RBridgeSpecialist::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 RBridgeSpecialist::createValidities(RBridgeSpecialist::SegmentInfo& info){
  //only landmark validation done yet
  if (info.skyblue== 0 || info.yellow == 0) info.lmValidityYellow = 0;
  else info.lmValidityYellow = 1-  2 *  (fabs((double) info.yellow/(double)(info.yellow + info.skyblue) - 0.5)  );

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

void RBridgeSpecialist::findBridgeMark(RBridgeSpecialist::SegmentInfo& si){
  //if (si.size < 6) return; //element to small
  double boxWidth;
  //testing for the right filter
  bool isMark = (si.lmValidityOrange > 0.3 || si.lmValidityYellow > 0.3) &&
    (boxWidth = (si.rect.bottomRight - si.rect.bottomLeft).abs()) > 5 &&
    (si.rect.upperLeft - si.rect.bottomLeft).abs() * 3 < boxWidth;

  if (!isMark) return;
  BitePoint::Position pos = getPosition(si);
  if (pos == BitePoint::numOfPositions) return; //undefined
  Vector2<int> markCenter;

  Rectangle& r = si.rect;
   
  Vector2<double> leftCenter = (r.upperLeft + r.bottomLeft)/2;
  Vector2<double> rightCenter = (r.upperRight + r.bottomRight)/2;
  Vector2<double> leftToRight = (rightCenter - leftCenter)/6;
  Vector2<int> leftStart(
    (int)(leftCenter.x + leftToRight.x),
    (int)(leftCenter.y + leftToRight.y));
  Vector2<int> rightStart(
    (int)(rightCenter.x - leftToRight.x),
    (int)(rightCenter.y - leftToRight.y));

  if (boxWidth < 60)
  {
    markCenter.x = (int)((leftCenter.x + rightCenter.x)/2);
    markCenter.y = (int)((leftCenter.y + rightCenter.y)/2);
  }
  else
  {
    colorClass c = noColor;
    Vector2<int> temp;
    edgeScanner.setDirection(leftToRight);
      DOT(imageProcessor_general, leftStart.x,leftStart.y,
      Drawings::white, Drawings::green);
      RasterSpecialist::Box b(1,1,imageWidth-2,imageWidth-2);
    while (b.inside(leftStart.x,leftStart.y)){
      c = getColor(leftStart);
      if (c == si.rightColor){
        markCenter = leftStart;
        break;
      }
      edgeScanner.skip(leftStart.x,leftStart.y);
    }
  }
  //mid of main-mark found  
  DOT(imageProcessor_general, markCenter.x,markCenter.y,
    Drawings::white, Drawings::blue);
  Vector2<int> up;
  up.x = (int)(rightCenter.y - leftCenter.y);
  up.y = (int) -(rightCenter.x - (int)leftCenter.x);
  up = up/5;
  
  
  //now searching for offset to black-line
  Vector2<int> upper(markCenter + up);
  Vector2<int> lower(markCenter - up);
  OUTPUT(idText,text,"up: " << up.x << " " << up.y);
  
  DOT(imageProcessor_general, upper.x,upper.y,
    Drawings::white, Drawings::blue); 
  DOT(imageProcessor_general, lower.x,lower.y,
    Drawings::white, Drawings::red);

  Geometry::Line perp;
  perp.base.x = (double)markCenter.x;
  perp.base.y = (double)markCenter.y;
  perp.direction = pHorizon.direction;

  Vector2<int> lScan,rScan,lineCenter;
  int leftAbs = 10000,rightAbs = 10000;
  float offset = 0;
  lScan = lower;
  rScan = lower;
  Vector2<int> temp;
  if (getColor(lScan.x,lScan.y) == lineColor)
  {
    edgeScanner.scan(lScan.x,lScan.y,-leftToRight);
    edgeScanner.scan(rScan.x,rScan.y,leftToRight);
    lineCenter = (lScan + rScan)/2;
    offset = getDistanceToLine(perp,lineCenter);
  }
  else
  { 
    colorClass c;
    edgeScanner.setDirection(-leftToRight);
    while(edgeScanner.colorScan(lScan.x,lScan.y,c)){
      if (c == lineColor)
      {
        lScan = (lScan + temp)/2;
        leftAbs = (lScan - lower).abs();
        break;
      }
      //edgeScanner.skip(lScan.x,lScan.y);
      temp = lScan;       
    }

    c = noColor;
    edgeScanner.setDirection(leftToRight);
    while(edgeScanner.colorScan(rScan.x,rScan.y,c)){
      if (c == lineColor)
      { 
        rScan = (rScan + temp)/2;
        rightAbs = (rScan - lower).abs();

        break;
      }
      //edgeScanner.skip(rScan.x,rScan.y);
      temp = rScan;       
    }
    
    if (leftAbs < rightAbs)
    {
      offset = (float)leftAbs;
      lineCenter = lScan;
    }
    else
    { 
      offset = (float)-rightAbs;
      lineCenter = rScan;
    }
  }
  
  offset = offset*100/(float)boxWidth;
  DOT(imageProcessor_general, lineCenter.x,lineCenter.y,
    Drawings::white, Drawings::pink);
  OUTPUT(idText,text,"offset: " << offset);
  double distance;
  double height = (r.upperLeft -r.bottomLeft).abs();
  if (boxWidth > 120)
    distance = Geometry::getDistanceBySize(
      rip->image.cameraInfo,133,boxWidth,markCenter.x,markCenter.y);
  else
    distance = Geometry::getDistanceBySize(
      rip->image.cameraInfo,13,height,markCenter.x,markCenter.y);
  OUTPUT(idText,text,"distance: " << distance);
  Vector2<double> angles;
  Vector2<double> inImage((double)markCenter.x,(double)markCenter.y);
  calculateAnglesForPoint(
    inImage,rip->cameraMatrix,rip->image.cameraInfo,angles);
  OUTPUT(idText,text,"angle: " << angles.x);
  rip->specialPercept.ocBridge.bitePoint[pos].addPercept(angles.x,distance,offset);
}

BitePoint::Position RBridgeSpecialist::getPosition(RBridgeSpecialist::SegmentInfo& mark){
  edgeScanner.setDirection(horizon.direction);
  colorClass c,comp;
  Vector2<int> scan;
  mark.leftColor = noColor;
  mark.rightColor = noColor;
  Vector2<double> toRight = (mark.rect.upperRight - mark.rect.upperLeft)/7;
  scan.x = (int)(mark.rect.upperLeft.x + mark.rect.bottomLeft.x)/2 + (int)toRight.x;
  scan.y = (int)(mark.rect.upperLeft.y + mark.rect.bottomLeft.y)/2 + (int)toRight.y;
  DOT(imageProcessor_general, scan.x,scan.y,
    Drawings::white, Drawings::red);
  if (mark.lmValidityOrange > mark.lmValidityYellow)
    comp = orange;
  else comp = yellow;
  c = getColor(scan.x,scan.y);
  do{
    if(c == skyblue)
    {
      mark.leftColor = skyblue;
      mark.rightColor = comp;
      break;
    }
    else if (c == comp)
    {
      mark.leftColor = comp;
      mark.rightColor = skyblue;
      break;
    }
  }while (edgeScanner.colorScan(scan.x,scan.y,c));

  if (mark.leftColor == noColor) return BitePoint::numOfPositions;
  OUTPUT(idText,text,"finished type:" << 
    ColorClasses::getColorName(mark.leftColor) << " " << 
    ColorClasses::getColorName(mark.rightColor));
  if (comp == yellow)
  {
    if(mark.leftColor == skyblue)
      return BitePoint::frontleft;
    else
      return BitePoint::behindleft;
  }
  else
  {
    if(mark.leftColor == skyblue)
      return BitePoint::behindright;
    else
      return BitePoint::frontright;
  }
}

---