Modules/ImageProcessor/GT2005ImageProcessor/GuideDogRobotSpecialist.cpp

Go to the documentation of this file.

/**
* @file GuideDogRobotSpecialist.cpp
*
* Implementation of class GuideDogRobotSpecialist
*/

#include "Tools/Player.h"
#include "Tools/FieldDimensions.h"
#include "Tools/RangeArray.h"
#include "Tools/Debugging/View.h"
#include "Representations/Perception/ColorTable.h"
#include "Representations/Perception/PlayersPercept.h"
#include "Representations/Perception/CameraMatrix.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ColorTableReferenceColor.h"
#include "Tools/Debugging/DebugImages.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ImageProcessorUtilityClasses.h"
#include "GuideDogRobotSpecialist.h"
#include "Modules/ImageProcessor/GT2005ImageProcessor/GT2005ImageProcessorTools.h"

Image imageProcessorRobotDetectionImage;

GuideDogRobotSpecialist::GuideDogRobotSpecialist(
  const Image& image, 
  const CameraMatrix& cameraMatrix, 
  const CameraMatrix& prevCameraMatrix, 
  const ColorTable& colorTable,
  const ColorCorrector& colorCorrector,
  PlayersPercept& playersPercept
  ):
image(image),
cameraMatrix(cameraMatrix),
prevCameraMatrix(cameraMatrix),
colorTable(colorTable),
colorCorrector(colorCorrector),
playersPercept(playersPercept)
{
}


GuideDogRobotSpecialist::~GuideDogRobotSpecialist()
{
}

void GuideDogRobotSpecialist::execute()
{
  //OUTPUT(idText,text,"robotSpecialist");
  //LINE(imageProcessor_robot_detection, 50, -50, 100, 100, 2, Drawings::ps_solid, Drawings::red);

  INIT_DEBUG_IMAGE(imageProcessorRobotDetection, image);

  horizonLine = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
  verticalLine.base = horizonLine.base;
  verticalLine.direction.x = - horizonLine.direction.y;
  verticalLine.direction.y = horizonLine.direction.x;

  calculateScanLinesParallelToHorizon();
  scanParallelToHorizon();

  SEND_DEBUG_IMAGE(imageProcessorRobotDetection);
}

void GuideDogRobotSpecialist::calculateScanLinesParallelToHorizon()
{
  numberOfHorizontalScanLines = 0;

  Geometry::Line lineAboveHorizon;
  lineAboveHorizon.direction = horizonLine.direction;

  Vector2<int> bottomLeft(0,0);
  Vector2<int> topRight(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3);

  for(int i = maxNumberOfHorizontalScanLines; i >= 0; i--)
  {
    lineAboveHorizon.base = horizonLine.base + verticalLine.direction * ( (i * 3.5) - 5 );

    if(Geometry::getIntersectionPointsOfLineAndRectangle(
      bottomLeft, topRight, lineAboveHorizon,
      leftPoint[numberOfHorizontalScanLines], 
      rightPoint[numberOfHorizontalScanLines])
      )
    {
      numberOfHorizontalScanLines++;
    }
  }
}

void GuideDogRobotSpecialist::scanParallelToHorizon()
{
  int x,y;
  int top, left, bottom, right;
  top = image.cameraInfo.resolutionHeight;
  left = image.cameraInfo.resolutionWidth;
  bottom = right = 0;
  int numberOfRedPixels = 0;

  //skip first scanline, first scanline is not straight (don't ask!)
  for(int i = 1; i < numberOfHorizontalScanLines; i++)
  {
    Geometry::PixeledLine line(leftPoint[i], rightPoint[i]);

    if(line.getNumberOfPixels() > 3)
    {
      for(int z = 0; z < line.getNumberOfPixels(); z++)
      {
        x = line.getPixelX(z);
        y = line.getPixelY(z);

        if (isRed(x,y)) 
        {
          DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorRobotDetection, x, y);
          if (y < top) top = y;
          if (y > bottom) bottom = y;
          if (x < left) left = x;
          if (x > right) right = x;
          numberOfRedPixels++;
        } else {
          DEBUG_IMAGE_SET_PIXEL_GRAY(imageProcessorRobotDetection, x, y);
        }
      }
    }
  }

  if (numberOfRedPixels > 10)
  {
    //draw red rectangle for percept
    //top line
    LINE(imageProcessor_robot_detection, left, top, right, top, 2, Drawings::ps_solid, Drawings::red);
    //bottom line
    LINE(imageProcessor_robot_detection, left, bottom, right, bottom, 2, Drawings::ps_solid, Drawings::red);
    //left line
    LINE(imageProcessor_robot_detection, left, top, left, bottom, 2, Drawings::ps_solid, Drawings::red);
    //right line
    LINE(imageProcessor_robot_detection, right, top, right, bottom, 2, Drawings::ps_solid, Drawings::red);   

    SinglePlayerPercept percept;
    Vector2<int> middle,
      pointOnField;

    middle.x = (int) (left + right) / 2;
    middle.y = bottom; 
    if(Geometry::calculatePointOnField(middle.x, middle.y, cameraMatrix, 
      prevCameraMatrix, image.cameraInfo, pointOnField)){

        // orientation of robot must be decided after motion of robot (in ours plan)
        percept.offsetOnField.x = (double) pointOnField.x;
        percept.offsetOnField.y = (double) pointOnField.y;
        percept.directionOnField = atan2(percept.offsetOnField.y, percept.offsetOnField.x); 
        percept.validity = 1;

        playersPercept.addRedPlayer(percept);
        OUTPUT(idText, text, "new offset: " << percept.offsetOnField.abs() << "; Direction: " << percept.directionOnField);
      }
  }
}

bool inline GuideDogRobotSpecialist::isRed(int x, int y) {
  //simple test, may be updated
  return (CORRECTED_COLOR_CLASS(x,y,image.image[y][0][x],image.image[y][1][x],image.image[y][2][x], colorTable, colorCorrector) == red);
}

IMAGE_VIEW(robotDetection, imageProcessorRobotDetection)
Drawings::imageProcessor_robot_detection
END_VIEW(robotDetection)

---