Modules/ImageProcessor/RasterImageProcessor/RasterImageProcessor.cpp

Go to the documentation of this file.

/**
* @file RasterImageProcessor.cpp
* 
* This file contains the implementation of class RasterImageProcessor.
*
* @author <a href="mailto:sadprofessor@web.de">Bernd Schmidt</a>
*/
#include "RasterImageProcessor.h"
#include "Tools/Math/Common.h"
#include "Tools/Debugging/GenericDebugData.h"
#include "Tools/FieldDimensions.h"
#include "Tools/RingBuffer.h"
#include "Representations/Perception/ColorTable64.h"
#include "RDefaultStrategy.h"

#include <time.h>

using namespace std;


RasterImageProcessor::RasterImageProcessor(const ImageProcessorInterfaces& interfaces)
: ImageProcessor(interfaces), 
  corrector(),
  edgeDetector(10)
{ 
  for (int i=0;i < maxRasterSpecialists; i++)
    specials[i] = NULL;
    
  minX = 0;
  minY = 0;
  maxX = image.cameraInfo.resolutionWidth -1;
  maxY = image.cameraInfo.resolutionHeight -1;


  strategy = new RDefaultStrategy(*this);
  waitingStrategy = NULL;

  
}

RasterImageProcessor::~RasterImageProcessor()
{
  delete strategy;  
}

void RasterImageProcessor::execute()
{
  //INIT_DEBUG_IMAGE(imageProcessorScanLines, image);
  //Image emptyImage;
  INIT_DEBUG_IMAGE(imageProcessorBall,image);
  INIT_DEBUG_IMAGE(imageProcessorObstacles,image);
  INIT_DEBUG_IMAGE(imageProcessorFlagsAndGoals, image);
  INIT_DEBUG_IMAGE(imageProcessorGround, image);
  INIT_DEBUG_IMAGE(imageProcessorGeneral, image);

  
  
  //INIT_DEBUG_IMAGE(classificationY, emptyImage);
  //INIT_DEBUG_IMAGE(classificationU, emptyImage);
  //INIT_DEBUG_IMAGE(classificationV, emptyImage);
//  clock_t t1 = clock();

  init();

  strategy->execute();
//  clock_t t2 = clock();

  //OUTPUT(idText,text,"time: " << double(t2-t1)/CLOCKS_PER_SEC << " Sekunden");

  //SEND_DEBUG_IMAGE(imageProcessorGeneral);
  //SEND_DEBUG_IMAGE(imageProcessorBall);
  //SEND_DEBUG_IMAGE(imageProcessor_flagsAndGoals);
  //SEND_DEBUG_IMAGE(imageProcessor_obstacles);
  DEBUG_DRAWING_FINISHED(imageProcessor_general);
  DEBUG_DRAWING_FINISHED(imageProcessor_ball1);
  DEBUG_DRAWING_FINISHED(imageProcessor_ball2);
  DEBUG_DRAWING_FINISHED(imageProcessor_ball3);
  DEBUG_DRAWING_FINISHED(imageProcessor_obstacles);
  DEBUG_DRAWING_FINISHED(imageProcessor_ground);
  DEBUG_DRAWING_FINISHED(imageProcessor_flagsAndGoals);
};

bool RasterImageProcessor::handleMessage(InMessage &message)
{
  //InMessage *pMess = &message;  
  
  return false;
};

RasterSpecialist *RasterImageProcessor::getSpecialist(int type)
{
  return specials[type];
}

void RasterImageProcessor::setSpecialist(RasterSpecialist *specialist)
{
  int type = specialist->getType();
  if (specials[type]!=0) delete specials[type];
  specials[type] = specialist;

}

void RasterImageProcessor::removeSpecialist(int type)
{
  delete specials[type];
  specials[type] = NULL;
}


void RasterImageProcessor::changeStrategy(RasterStrategy &newStrategy)
{
  waitingStrategy = &newStrategy;

}

void RasterImageProcessor::init()
{ 
  if (waitingStrategy != NULL) {
    delete strategy;
    strategy = waitingStrategy;
    waitingStrategy = NULL;
  }
  
  strategy->init();

//  INIT_DEBUG_IMAGE(imageProcessorPlayers, image); 

  GENERATE_DEBUG_IMAGE(segmentedImage1,
  colorTable.generateColorClassImage(image, segmentedImage1ColorClassImage));

  SEND_DEBUG_COLOR_CLASS_IMAGE(segmentedImage1);
  //OUTPUT(idText,text,"frame: " << image.frameNumber);       
  // reset everything
  landmarksPercept.reset(image.frameNumber);
  linesPercept.reset(image.frameNumber);
  ballPercept.reset(image.frameNumber);
  playersPercept.reset(image.frameNumber);
  obstaclesPercept.reset(image.frameNumber);

  landmarksPercept.cameraOffset = cameraMatrix.translation;

  horizon = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
  
  LINE(imageProcessor_general,
    (int)(horizon.base.x - 120 * horizon.direction.x ), (int)(horizon.base.y - 120 * horizon.direction.y), 
    (int)(horizon.base.x + 120 * horizon.direction.x), (int)(horizon.base.y + 120 * horizon.direction.y), 
    1, Drawings::ps_solid, Drawings::white );
//  flagSpecialist.init(image);

}

void RasterImageProcessor::addFlag(Vector2<double>& left,Vector2<double>& right,
  Vector2<double>& top, Vector2<double>& bottom,Flag::FlagType type){
  DOT(imageProcessor_flagsAndGoals, right.x, right.y,
    Drawings::red, Drawings::black);
  DOT(imageProcessor_flagsAndGoals, left.x, left.y,
    Drawings::green, Drawings::black);

  bool topOnBorder = !isValidPoint(top);
  bool bottomOnBorder = !isValidPoint(bottom);
  bool leftOnBorder = !isValidPoint(left);
  bool rightOnBorder = !isValidPoint(right);

  double factor = image.cameraInfo.resolutionWidth / 2 / tan(image.cameraInfo.openingAngleWidth / 2);
  int flip = getPlayer().getTeamColor() == Player::blue ? -1 : 1;
  
  Vector3<double> vectorToLeft(factor,
    image.cameraInfo.resolutionWidth / 2 - left.x,
    image.cameraInfo.resolutionHeight / 2 - left.y);
    Vector3<double> vectorToRight(factor,
    image.cameraInfo.resolutionWidth / 2 - right.x,
    image.cameraInfo.resolutionHeight / 2 - right.y);
  Vector3<double> vectorToTop(factor,
    image.cameraInfo.resolutionWidth / 2 - top.x,
    image.cameraInfo.resolutionHeight / 2 - top.y);
    Vector3<double> vectorToBottom(factor,
    image.cameraInfo.resolutionWidth / 2 - bottom.x,
    image.cameraInfo.resolutionHeight / 2 - bottom.y);

    Vector3<double>
    vectorToLeftWorld = cameraMatrix.rotation * vectorToLeft,
    vectorToRightWorld = cameraMatrix.rotation * vectorToRight,
    vectorToTopWorld = cameraMatrix.rotation * vectorToTop,
    vectorToBottomWorld = cameraMatrix.rotation * vectorToBottom;
       
    double 
      leftAngle = atan2(vectorToLeftWorld.y,vectorToLeftWorld.x),
      rightAngle = atan2(vectorToRightWorld.y,vectorToRightWorld.x),
      topAngle = atan2(vectorToTopWorld.z,sqrt((double)sqr(vectorToTopWorld.x) + sqr(vectorToTopWorld.y)) ),
      bottomAngle = atan2(vectorToBottomWorld.z,sqrt((double)sqr(vectorToBottomWorld.x) + sqr(vectorToBottomWorld.y)) );


    Vector2<double>flagPosition;
    
    switch (type)
    {
    case Flag::pinkAboveYellow:
      flagPosition.x = xPosBackFlags * flip;
      flagPosition.y = yPosRightFlags * flip;
      break;
    case Flag::pinkAboveSkyblue:
      flagPosition.x = xPosFrontFlags * flip;
      flagPosition.y = yPosRightFlags * flip;
      break;
    case Flag::yellowAbovePink:
      flagPosition.x = xPosBackFlags * flip;
      flagPosition.y = yPosLeftFlags * flip;
      break;
    case Flag::skyblueAbovePink:
      flagPosition.x = xPosFrontFlags * flip;
      flagPosition.y = yPosLeftFlags * flip;
      break;
    }

  Vector2<double> centerAngle;
    ConditionalBoundary boundary;

    boundary.addX(leftAngle,leftOnBorder);
    boundary.addX(rightAngle,rightOnBorder);
    boundary.addY(topAngle,topOnBorder);
    boundary.addY(bottomAngle,bottomOnBorder);
    landmarksPercept.addFlag(type, flagPosition, boundary);
} 


/*
* Change log :
* 
* $Log: RasterImageProcessor.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.29  2004/06/09 13:05:57  schmidtb
* removed memory leaks
*
* Revision 1.28  2004/06/02 14:51:31  koh
* RDefaultStrategy2 wax not included, so temporarely commented out
*
* Revision 1.27  2004/06/02 11:25:33  deom
* now uses RDefaultStrategy2
*
* Revision 1.26  2004/04/08 17:14:26  wachter
* GT04 checkin of Microsoft-Hellounds
*
* Revision 1.26  2004/03/25 15:21:19  pg_besc
* made some changes
*
* Revision 1.25  2004/03/17 22:13:15  schmidtb
* *** empty log message ***
*
* Revision 1.24  2004/03/11 20:53:52  schmidtb
* removed errors
*
* Revision 1.23  2004/03/11 20:32:36  schmidtb
* new version of rip
*
* Revision 1.22  2004/03/08 01:39:02  roefer
* Interfaces should be const
*
* Revision 1.21  2004/03/03 12:53:21  schmidtb
* color correction integrated
*
* Revision 1.20  2004/03/02 12:00:09  koh
* included RFlexibleStrategy-File
*
* Revision 1.19  2004/03/01 15:49:31  dueffert
* temporary removed for compilability
*
* Revision 1.18  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.17  2004/02/25 15:43:56  schmidtb
* enabled resets
*
* Revision 1.16  2004/02/25 11:56:29  neubach
* - landmarksPercept.reset disabled in order to get combo to work
*
* Revision 1.15  2004/02/18 14:56:19  neubach
* new Segmentation established, code not cleared at all
*
* Revision 1.14  2004/02/04 13:12:23  nistico
* Removed unneeded references in RasterImageProcessor to ManualCalibration, which were binding it
* to ColorTable64
*
* Revision 1.13  2004/02/02 13:42:12  schmidtb
* merged sources of RIP. added som functions.
*
* Revision 1.12  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.11  2004/01/21 11:04:30  loetzsch
* removed some OUTPUTs
*
* Revision 1.10  2003/12/31 23:50:38  roefer
* Removed inclusion of RobotDimensions.h because it is not used
*
* Revision 1.9  2003/12/15 13:55:32  schmidtb
* Merged and patched new version of RasterImageProcessor.
*
* Revision 1.8  2003/12/15 11:49:08  juengel
* Introduced CameraInfo
*
* Revision 1.7  2003/12/08 15:02:55  schmidtb
* new version of RIP
*
* Revision 1.6  2003/12/04 09:51:23  schmidtb
* better BallSpecialist
*
* Revision 1.5  2003/12/02 21:59:02  schmidtb
* New version of RasterImageProcessor
*
* Revision 1.4  2003/11/28 14:50:01  dueffert
* bugs and warnings fixed
*
* Revision 1.3  2003/11/20 10:26:56  schmidtb
* Ball Detection added
*
* Revision 1.2  2003/11/13 10:41:29  schmidtb
* renewed RBallSpeciaslist and Strategy
*
* Revision 1.1  2003/11/12 13:13:20  schmidtb
* new RasterImageProcessor added
*
*
*/

---