Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/ImageProcessor/GT2005ImageProcessor/GuideDogRobotSpecialist.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file GuideDogRobotSpecialist.cpp
    00003 *
    00004 * Implementation of class GuideDogRobotSpecialist
    00005 */
    00006
    00007 #include "Tools/Player.h"
    00008 #include "Tools/FieldDimensions.h"
    00009 #include "Tools/RangeArray.h"
    00010 #include "Tools/Debugging/View.h"
    00011 #include "Representations/Perception/ColorTable.h"
    00012 #include "Representations/Perception/PlayersPercept.h"
    00013 #include "Representations/Perception/CameraMatrix.h"
    00014 #include "Modules/ImageProcessor/ImageProcessorTools/ColorTableReferenceColor.h"
    00015 #include "Tools/Debugging/DebugImages.h"
    00016 #include "Modules/ImageProcessor/ImageProcessorTools/ImageProcessorUtilityClasses.h"
    00017 #include "GuideDogRobotSpecialist.h"
    00018 #include "Modules/ImageProcessor/GT2005ImageProcessor/GT2005ImageProcessorTools.h"
    00019
    00020 Image imageProcessorRobotDetectionImage;
    00021
    00022 GuideDogRobotSpecialist::GuideDogRobotSpecialist(
    00023   const Image& image,
    00024   const CameraMatrix& cameraMatrix,
    00025   const CameraMatrix& prevCameraMatrix,
    00026   const ColorTable& colorTable,
    00027   const ColorCorrector& colorCorrector,
    00028   PlayersPercept& playersPercept
    00029   ):
    00030 image(image),
    00031 cameraMatrix(cameraMatrix),
    00032 prevCameraMatrix(cameraMatrix),
    00033 colorTable(colorTable),
    00034 colorCorrector(colorCorrector),
    00035 playersPercept(playersPercept)
    00036 {
    00037 }
    00038
    00039
    00040 GuideDogRobotSpecialist::~GuideDogRobotSpecialist()
    00041 {
    00042 }
    00043
    00044 void GuideDogRobotSpecialist::execute()
    00045 {
    00046   //OUTPUT(idText,text,"robotSpecialist");
    00047   //LINE(imageProcessor_robot_detection, 50, -50, 100, 100, 2, Drawings::ps_solid, Drawings::red);
    00048
    00049   INIT_DEBUG_IMAGE(imageProcessorRobotDetection, image);
    00050
    00051   horizonLine = Geometry::calculateHorizon(cameraMatrix, image.cameraInfo);
    00052   verticalLine.base = horizonLine.base;
    00053   verticalLine.direction.x = - horizonLine.direction.y;
    00054   verticalLine.direction.y = horizonLine.direction.x;
    00055
    00056   calculateScanLinesParallelToHorizon();
    00057   scanParallelToHorizon();
    00058
    00059   SEND_DEBUG_IMAGE(imageProcessorRobotDetection);
    00060 }
    00061
    00062 void GuideDogRobotSpecialist::calculateScanLinesParallelToHorizon()
    00063 {
    00064   numberOfHorizontalScanLines = 0;
    00065
    00066   Geometry::Line lineAboveHorizon;
    00067   lineAboveHorizon.direction = horizonLine.direction;
    00068
    00069   Vector2<int> bottomLeft(0,0);
    00070   Vector2<int> topRight(image.cameraInfo.resolutionWidth - 1, image.cameraInfo.resolutionHeight - 3);
    00071
    00072   for(int i = maxNumberOfHorizontalScanLines; i >= 0; i--)
    00073   {
    00074     lineAboveHorizon.base = horizonLine.base + verticalLine.direction * ( (i * 3.5) - 5 );
    00075
    00076     if(Geometry::getIntersectionPointsOfLineAndRectangle(
    00077       bottomLeft, topRight, lineAboveHorizon,
    00078       leftPoint[numberOfHorizontalScanLines],
    00079       rightPoint[numberOfHorizontalScanLines])
    00080       )
    00081     {
    00082       numberOfHorizontalScanLines++;
    00083     }
    00084   }
    00085 }
    00086
    00087 void GuideDogRobotSpecialist::scanParallelToHorizon()
    00088 {
    00089   int x,y;
    00090   int top, left, bottom, right;
    00091   top = image.cameraInfo.resolutionHeight;
    00092   left = image.cameraInfo.resolutionWidth;
    00093   bottom = right = 0;
    00094   int numberOfRedPixels = 0;
    00095
    00096   //skip first scanline, first scanline is not straight (don't ask!)
    00097   for(int i = 1; i < numberOfHorizontalScanLines; i++)
    00098   {
    00099     Geometry::PixeledLine line(leftPoint[i], rightPoint[i]);
    00100
    00101     if(line.getNumberOfPixels() > 3)
    00102     {
    00103       for(int z = 0; z < line.getNumberOfPixels(); z++)
    00104       {
    00105         x = line.getPixelX(z);
    00106         y = line.getPixelY(z);
    00107
    00108         if (isRed(x,y))
    00109         {
    00110           DEBUG_IMAGE_SET_PIXEL_RED(imageProcessorRobotDetection, x, y);
    00111           if (y < top) top = y;
    00112           if (y > bottom) bottom = y;
    00113           if (x < left) left = x;
    00114           if (x > right) right = x;
    00115           numberOfRedPixels++;
    00116         } else {
    00117           DEBUG_IMAGE_SET_PIXEL_GRAY(imageProcessorRobotDetection, x, y);
    00118         }
    00119       }
    00120     }
    00121   }
    00122
    00123   if (numberOfRedPixels > 10)
    00124   {
    00125     //draw red rectangle for percept
    00126     //top line
    00127     LINE(imageProcessor_robot_detection, left, top, right, top, 2, Drawings::ps_solid, Drawings::red);
    00128     //bottom line
    00129     LINE(imageProcessor_robot_detection, left, bottom, right, bottom, 2, Drawings::ps_solid, Drawings::red);
    00130     //left line
    00131     LINE(imageProcessor_robot_detection, left, top, left, bottom, 2, Drawings::ps_solid, Drawings::red);
    00132     //right line
    00133     LINE(imageProcessor_robot_detection, right, top, right, bottom, 2, Drawings::ps_solid, Drawings::red);
    00134
    00135     SinglePlayerPercept percept;
    00136     Vector2<int> middle,
    00137       pointOnField;
    00138
    00139     middle.x = (int) (left + right) / 2;
    00140     middle.y = bottom;
    00141     if(Geometry::calculatePointOnField(middle.x, middle.y, cameraMatrix,
    00142       prevCameraMatrix, image.cameraInfo, pointOnField)){
    00143
    00144         // orientation of robot must be decided after motion of robot (in ours plan)
    00145         percept.offsetOnField.x = (double) pointOnField.x;
    00146         percept.offsetOnField.y = (double) pointOnField.y;
    00147         percept.directionOnField = atan2(percept.offsetOnField.y, percept.offsetOnField.x);
    00148         percept.validity = 1;
    00149
    00150         playersPercept.addRedPlayer(percept);
    00151         OUTPUT(idText, text, "new offset: " << percept.offsetOnField.abs() << "; Direction: " << percept.directionOnField);
    00152       }
    00153   }
    00154 }
    00155
    00156 bool inline GuideDogRobotSpecialist::isRed(int x, int y) {
    00157   //simple test, may be updated
    00158   return (CORRECTED_COLOR_CLASS(x,y,image.image[y][0][x],image.image[y][1][x],image.image[y][2][x], colorTable, colorCorrector) == red);
    00159 }
    00160
    00161 IMAGE_VIEW(robotDetection, imageProcessorRobotDetection)
    00162 Drawings::imageProcessor_robot_detection
    00163 END_VIEW(robotDetection)