Modules/ImageProcessor/VLCImageProcessor/VLCGoalRecognizer.h

Go to the documentation of this file.

/**
* @file VLCGoalRecognizer.h
*
* @author <a href="mailto:oberlies@sim.tu-darmstadt.de">Tobias Oberlies</a>
*/

#ifndef VLCGoalRecognizer_h_
#define VLCGoalRecognizer_h_

#include "Modules/ImageProcessor/ImageProcessor.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ImageProcessorUtilityClasses.h"
#include "Modules/ImageProcessor/ImageProcessorTools/BresenhamLineScan.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"


/**
 * Macro to handle the stupid problem, that the DEBUG_IMAGE_SET_PIXEL_xxx macros need the 
 * debug image identifier at compile time. There are two instances of this class, and one of 
 * them uses imageProcessorGoal1 and the other imageProcessorGoal2.
 * @param targetcolor A variable that contains the goal colour at runtime.
 * @param x The x-value of the pixel to be set (at runtime).
 * @param y The y-value of the pixel to be set (at runtime).
 * @param drawcolor Text suitable to complete DEBUG_IMAGE_SET_PIXEL_ at compile time.
 */
#ifndef NDEBUG
#define STUPID_DEBUG_IMAGE_SET_PIXEL(targetcolor, x, y, drawcolor) \
  if (targetcolor == yellow) \
  { \
    DEBUG_IMAGE_SET_PIXEL_##drawcolor(imageProcessorGoal1, (x), (y)); \
  } \
  else \
  { \
    DEBUG_IMAGE_SET_PIXEL_##drawcolor(imageProcessorGoal2, (x), (y)); \
  }

#define N_STUPID_DEBUG_IMAGE_SET_PIXEL(targetcolor, x, y, drawcolor) \
  if (targetcolor == yellow) \
  { \
    N_SET_COLORED_PIXEL_IN_GRAY_SCALE_IMAGE(goalRecognizerYellow, (x), (y), (drawcolor)); \
  } \
  else \
  { \
    N_SET_COLORED_PIXEL_IN_GRAY_SCALE_IMAGE(goalRecognizerBlue, (x), (y), (drawcolor)); \
  }

#else

#define STUPID_DEBUG_IMAGE_SET_PIXEL(targetcolor, x, y, drawcolor) ;
#define N_STUPID_DEBUG_IMAGE_SET_PIXEL(targetcolor, x, y, drawcolor) ;

#endif

/**
 * @class VLCGoalRecognizer
 *
 * TODO: Write Summary
 * 
 * @author <a href="mailto:oberlies@sim.tu-darmstadt.de"Tobias Oberlies</a>
 */ 
class VLCGoalRecognizer
{
private: // nested classes and enums

  /**
   * TODO: Write Summary
   */
  enum EdgeType
  {
    none = 0,
    edge,
    deviationWithoutEdge,
    imageBorder       // Must be the last one
  };

  /**
   * TODO: Write Summary
   */
  class EdgeDetector
  {
  public:
    /** Constructor. */
    EdgeDetector(colorClass target);
    EdgeDetector(colorClass target, Vector3<int> referenceColor, int referenceColorBase);

    /**
     * Investigates the next pixel and reports, wether an edge, or repeated deviation of 
     * the pixel colours (without the indications of an edge) have been detected. It is 
     * assumed that edges are quite sharp and that the color beyond the edge is 
     * significantly different to the goal colour. In case of a blurred edge or a steady
     * color gradient the method will (eventually) return deviationWithoutEdge.
     */
    inline EdgeType inspectPixel(const Vector2<int>& point, int index, const colorClass& colorCls, const Vector3<int>& color)
    {
      // Check, if pixel is in target colour class
      if (colorCls == targetColorClass)
      {
        // Store as possible edge point
        lastPointInClass = point;
        lastPointInClassIndex = index;
        connectedToClass = true;

        // Update the reference colour
        // The reference colour is an average of the colour of the last pixels. If
        // this method has been called on less than eight pixels, the average is 
        // equally weighted. Otherwise recent pixels have a higher weight (1/8 for 
        // the last one etc.)
        referenceColor = (referenceColor * referenceColorBase + color) / (referenceColorBase+1);
        if (referenceColorBase < 7)
          ++referenceColorBase;

        // Decrease deviation counter (since this pixel has no deviation)
        deviationCounter = deviationCounter<=4 ? 0 : deviationCounter-4;

        // Reset edge counter (assuming no pixels in the goal color class beyond the edge)
        edgeCounter = 0;
      }
      else if (colorCls != noColor && colorCls != ignoredClassification)
      {
        // Pixel is classified as a different colour. Take this as an indication for
        // an edge.
        ++edgeCounter;
        deviationCounter = deviationCounter + 2;

        // From now on don't use pixels with tolerable (but non-zero) pixels as 
        // lastPointInClass, because they're likely to be noise
        connectedToClass = false;
    
        // Check whether an edge or excessive deviation have been detected
        if (edgeCounter >= 3)
          return edge;
        else if (deviationCounter >= 10)
          return deviationWithoutEdge;

      }
      else
      {
        // Pixel is not classified in any color class, so calculate the distance to 
        // the reference color for further investigations
        int dist = distance(color);

        // Update deviation and edge counters
        if (dist < classThreshold)
        {
          // This pixel very likely to be part of the goal.
          if (edgeCounter > 0)
            --edgeCounter;
          ++deviationCounter;

          // Store as possible edge point if there haven't been any (strong) 
          // indications for an edge since the last pixel in the target class
          if (connectedToClass)
          {
            lastPointInClass = point;
            lastPointInClassIndex = index;
          }
        }
        else if(dist < edgeThreshold)
        {
          // This pixel is neither likely to be part of the goal, nor to be bejond
          // the edge. Since we're looking for a reasonably sharp edge, only few
          // pixels in this distance range are expected to occur.
          deviationCounter = deviationCounter + 2;
          connectedToClass = false;
        }
        else
        {
          // This pixel is likely to be beyond the edge of the goal. In this case
          // the edge counter is increased more than the deviation counter (with 
          // respect to their trigger thresholds 3 and 10). Hence in case of a 
          // clear edge, the edge will trigger before the deviation.
          ++edgeCounter;
          deviationCounter = deviationCounter + 2;
          connectedToClass = false;
        }

        // Check whether an edge or excessive deviation have been detected
        if (edgeCounter >= 3)
          return edge;
        else if (deviationCounter >= 10)
          return deviationWithoutEdge;
      }

      return none;
    }

    /**
     * Returns the last point in the target colour class. If inspectPixel return the 
     * value edge the returned point is located just before the edge. 
     */
    inline Vector2<int> getLastPointInClass()
    {
      return lastPointInClass;
    };

    /**
     * Similar to getLastPointInClass().
     */
    inline void getLastPointInClass(Vector2<int>& point)
    {
      point = lastPointInClass;
    }

    /**
     * Similar to getLastPointInClass(), but also returns the index of the 
     * lastPointInClass.
     */
    inline void getLastPointInClass(Vector2<int>& point, int& index)
    {
      point = lastPointInClass;
      index = lastPointInClassIndex;
    }

    /**
     * Similar to getLastPointInClass(), but returns the index of the lastPointInClass
     * instead of the poinst itself.
     */
    inline int getLastIndexInClass()
    {
      return lastPointInClassIndex;
    }

    /**
     * Resets the edge detection state while keeping the reference colour.
     * @param defaultLast The value returned by getLastPointInClass if none of the
     * inspected pixels is in or near the target colour class.
     * @param connectedToClass Assume that the first pixel that is inspected is 
     * connected to the target colour class.
     */
    void clear(const Vector2<int>& defaultLast, bool connectedToClass)
    {
      // Reset the counters
      deviationCounter = 0;
      edgeCounter = 0;

      // Set provided default values
      this->lastPointInClass = defaultLast;
      this->lastPointInClassIndex = -1;
      this->connectedToClass = connectedToClass;
    }

    /**
     * Reset the edge detection state while keeping the reference colour.
     */
    void clear()
    {
      // Reset the counters
      deviationCounter = 0;
      edgeCounter = 0;

      // Keep the lastPointInClass value assuming that the scan begins there (hence
      // first pixel inspected is connected to the class)
      lastPointInClassIndex = -1;
      connectedToClass = true;
    }

    /**
     * Reset the reference colour and the edge detector state.
     */
    void reset()
    {
      // Reset the reference colour
      referenceColorBase = 0;

      // Reset the counters
      deviationCounter = 0;
      edgeCounter = 0;

      // Keep the lastPointInClass value assuming that the scan begins there (hence
      // first pixel inspected is connected to the class)
      lastPointInClassIndex = -1;
      connectedToClass = false;
    }

    /**
     * Sets the reference colour.
     */
    void setReferenceColor(const Vector3<int>& color, int referenceColorBase)
    {
      this->referenceColor = color;
      this->referenceColorBase = referenceColorBase;
    };

    /**
     * Returns the reference colour.
     */
    void getReferenceColor(Vector3<int>& result)
    {
      result = this->referenceColor;
    };

  private: // methods

    /**
     * Calculates the Mahalanobis distance of the given colour to the reference colour.
     */
    inline int distance(const Vector3<int>& color)
    {
      // Calculate difference in each channel
      Vector3<int> difference = color - referenceColor;

      // Weigh with respect to usual variation
      return (invCovar * difference) * difference;  // in MATLAB syntax: difference'*invCovar*difference
    }

  private: // class members

    /** The maximum distance from the reference colour that is tolerated for a pixel to
      be considered in the target colour class. */
    static const float classThreshold;

    /** The minimum distance from the reference colour that pixels on the other side of
      an edge need to have. This value is greater than classThreshold. */
    static const float edgeThreshold;


  private: // members

    /** The colour class of the goal. */
    const colorClass targetColorClass;

    /** Colour class that is not considered as an indication for an edge. */
    const colorClass ignoredClassification;

    /** The inverse covariance matrix of the goal colour. It is used to calculate the
        Mahalanobis distance between the reference colour and the current colour for 
      pixels in no colour class (or an ignored one). */
    const Matrix3x3<int> invCovar;

    /** The reference colour for distance calculations. It is an average of recent 
        pixels that were classified in the target colour class. */
    Vector3<int> referenceColor;
    int referenceColorBase;

    /** Counter to detect the end of an area in target colour without an edge. */
    int deviationCounter;

    /** Counter to detect an edge. */
    int edgeCounter;


    /** Last pixel in the target colour class (or within tolerable distance). */
    Vector2<int> lastPointInClass;

    /** The index of the last point in/near the target colour class. By default, this
      value is -1, so it can be used to check whether any pixel was considered to be
      in/near the target colour class. */
    int lastPointInClassIndex;

    /** Current pixel is connected via to a pixel in the target colour class only via 
        pixels within tolerable distance, i.e. there hasn't been a pixel with a distance
      greater than the classDistance threshold since the last pixel classified in the
      target class (without distance). As long as this variable is true, points with
      tolerable distance will be stored as lastPointInClass. */
    bool connectedToClass;
    
  }; // class VLCGoalRecognizer::EdgeDetector

  
  /**
   * TODO: Write Summary
   */
  struct EdgePointList
  {
  private: // members
    
    // Arrays to store the edge points and their type
    enum {maxPoints = 20};
    Vector2<int> edgePoint[maxPoints];
    VLCGoalRecognizer::EdgeType edgeType[maxPoints];

    // Current number of edge points
    int numberOfPoints;

    // Number of edge points of a certain type
    int numberOfType[imageBorder+1];


  public: // methods

    /** Constructs an empty list. */
    EdgePointList()
      : numberOfPoints(0)
    {
      for (int i=0; i<=imageBorder; ++i)
        numberOfType[i] = 0;
    }

    /** Add an edge point to the list. */
    inline void add(Vector2<int>& point, VLCGoalRecognizer::EdgeType type)
    {
      // Ignore if too many points
      if (numberOfPoints>=maxPoints)
        return;

      // Store point and increase counters
      edgePoint[numberOfPoints] = point;
      edgeType[numberOfPoints] = type;
      ++numberOfPoints;
      ++numberOfType[type];
    }

    /** Returns the current size of the list. */
    inline int size()
    {
      return numberOfPoints;
    }

    /** Returns the number of edge points of a certain type. */
    inline int getCount(VLCGoalRecognizer::EdgeType type)
    {
      return numberOfType[type];
    }

    /** Returns an element of the list. */
    inline Vector2<int>& operator[](int index)
    {
      return edgePoint[index];
    }

    /** Returns the type of an edge points. */
    inline VLCGoalRecognizer::EdgeType getType(int index)
    {
      return edgeType[index];
    }
  }; // struct EdgePointList


  /**
   *
   */
  struct Goalpost
  {
    /** Constructs an empty goalpost hypothesis. */
    Goalpost() {};

    /** Top endpoints of the goalposts. Might not be aligned with the edge 
        horizontally. */
    Vector2<int> topPoint;
    Vector2<double> topPointH;
    bool topPointInImage; // FILL

    /** Bottom endpoints of the goalpost. Might not be aligned with the edge 
        horizontally. */
    Vector2<int> bottomPoint;
    Vector2<double> bottomPointH;

    /** Goalpost is standing on the green field. */
    bool onGreen;

    /** Height and height not conting the image border. */
    int height;
    int visibleHeight; // FILL

    /** A point aligned with the edge of the goalpost. */
    Vector2<int> edgePoint;
    Vector2<double> edgePointH;

    /** Number of strong and weak edge points detected. */
    int strongEdgeCount;
    int weakEdgeCount;

    /** Straightness */
    /** Slope */

    /** Scans towards the edge detected unbound goal-coloured areas. This often happens 
      when goals are dectected twice, or the scan along the cross bar failed. If this 
      value is set to true, all other values are invalid.*/
    bool nonExistant;

    /** Average colour of some pixels near the goalpost. This value is used to 
      detect the free part of goal (is done in the interpretation after the entire
      image was scanned) or straightnes checks. */
    Vector3<int> color;

  }; // struct Goalpost

  /**
   * TODO: Write Summary
   */
  struct GoalHypothesis
  {
    /** Constructs an empty GoalHypothesis with default values. */
    GoalHypothesis(){};
    
    /** The left and right goalpost. */
    Goalpost goalpost[2];

    /** Right end of cross bar. */
    Vector2<int> crossBarEndpoint;
    Vector2<double> crossBarEndpointH;

    /** Goal width. */
    int width;

    /** Goal height (the maximum of the goalpost heights). */
    int height;

    /** There is green below the goal hypthesis. */
    bool onGreen;

  }; // struct GoalHypothesis


  /**
   * TODO: Write Summary
   */
  enum ImageBorderSide
  {
    topBorder = 0,
    leftBorder = 1,
    bottomBorder = 2,
    rightBorder = 3,
    noBorder
  };
  
  enum FreeSide
  {
    noFreeSide,
    leftSide,
    rightSide,
    bothSides
  };


// class VLCGoalRecognizer

public:

  /**
   * Initializes the VLCGoadRecognizer.
   */
  VLCGoalRecognizer(colorClass color, const ImageProcessorInterfaces& interfaces, const ColorCorrector& colorCorrector, const ImageInfo& horizonInfo);
  
  /**
   * Destructor
   */
  ~VLCGoalRecognizer() {};
  
  /**
   * This method has to be called before a new image is scanned.
   */
  void notifyAboutNewImage();


  void notifyAboutFinish();



  // for VLC
  void setBestColorTable(ColorTable * newBestColorTable)
  {
    bestColorTable = newBestColorTable;
  }
  //


  inline void notifyAboutNewScanline(const Vector2<int>& scanLineStart)
  {
    // Reset counter for detection of goal candidates
    detectionCounter = 0;

    // Calculate how many pixels on this scanline should be skipped in order to prevent 
    // previously recognized goals to be found again
    calculateLockedPixels(scanLineStart);
  };


  inline void inspectPixel(const Vector2<int>& point, const colorClass& color)
  {
    // Check if this pixel is inside the area that has been analysed before
    if (--lockedPixels < 0)
    {

      // Check if pixel has target colour 
      if (color == goalColor)
      {
        // Increase counter
        ++detectionCounter;

        // When counter reaches 3, start the actual goal detection routine
        if (detectionCounter >= 4)
        {
          STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, point.x, point.y, BLUE);

          if (true)//inspectNeighbourhood(point))
          {
            // Analyze this blob
            analyzeGoal(point);

            // Recalculate the number of locked pixels on this scanline
            recalculateLockedPixels(point);

            //testDone = true;
            //testNow = false;
            detectionCounter = 0;
          }
          else
          {
            // Take into account that there were only few goal-coloured pixels in
            // the neighbourhood of the current pixel
            detectionCounter = 1;
          }
        }
        else
          STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, point.x, point.y, GRAY);      
      }
      else
      {
        // Decrease counter
        --detectionCounter;
        if (detectionCounter<0)
          detectionCounter = 0;
        STUPID_DEBUG_IMAGE_SET_PIXEL(goalColor, point.x, point.y, GRAY);
      }
    }
  };


private: // methods
  
  /** */
  bool inspectNeighbourhood(const Vector2<int>& point);

  /** 
   * point copied on purpose
   */
  void analyzeGoal(Vector2<int> startPoint);


  /** 
   * direction of edge normalized!!
   * imageBorderScanned: to prevent that one image border is scanned again, never necessary
   * in correct goal case
   */
  void analyzeGoalpost(VLCGoalRecognizer::EdgeDetector* detector,
                       const Vector2<double>& directionOfEdge, 
                       bool leftGoalpost,
                       Vector2<int>& focus,
                       VLCGoalRecognizer::Goalpost& result,
                       bool* imageBorderScanned);

  /** */
  void scanCrossBar(VLCGoalRecognizer::EdgeDetector* detector,
                    Vector2<int>& focus,
                    VLCGoalRecognizer::GoalHypothesis& result,
                    bool* imageBorderScanned);

  /** 
   * scans first from focus+scanLineOffset, using detector[0]!!
   */
  VLCGoalRecognizer::EdgeType detectEdgeTwice(VLCGoalRecognizer::EdgeDetector* detector,
                                                 Vector2<int>& focus,
                                                 BresenhamLineScan& scanLine,
                                                 const Vector2<int>& scanLineOffset,
                                                 int maxScanLength,
                                                 EdgePointList& edgePoints);


  /** */
  VLCGoalRecognizer::EdgeType detectEdge(VLCGoalRecognizer::EdgeDetector& detector, 
                                            Vector2<int>& focus, 
                                            BresenhamLineScan& direction,
                                            int maxScanLength,
                                            int& pixelsUntilEdge);
  
  /** */
  VLCGoalRecognizer::EdgeType scanAlongLine(VLCGoalRecognizer::EdgeDetector* detector,
                                               Vector2<int>& focus,
                                               BresenhamLineScan& scanLine,
                                               const Vector2<int>& scanLineOffset,
                                               int maximumScan,
                                               int& pixelsScanned);

  /** 
   * case noBorder mustn't be ignored -> scanning hasn't been done before or doesn't make sense 
   */
  VLCGoalRecognizer::ImageBorderSide scanOnImageBorder(VLCGoalRecognizer::EdgeDetector* detector,
                                                          Vector2<int>& focus,
                                                          const Vector2<double>& targetDirection,
                                                          int maxBorderDistance,
                                                          bool* scannedSides);

  /** 
   * direction of edge normalized!!
   * bottomPoint copied on purpose!!
   */
  bool detectGreenBelowGoalpost(Vector2<int> bottomPoint, 
                                const Vector2<double>& directionOfEdge, 
                                int goalpostHeight);

  /**
   * Returns true if the point is within one pixel distance to the image border.
   */
  inline bool nearImageBorder(const Vector2<int>& point, int maxDistance)
  {
    return point.x <= maxDistance || 
           point.y <= maxDistance || 
           point.x >= horizonInfo.maxImageCoordinates.x-maxDistance || 
           point.y >= horizonInfo.maxImageCoordinates.y-maxDistance;
  }

  /** */
  void mergeFragments(bool* deletedHypothesises);


  /** */
  void interpretResults(VLCGoalRecognizer::EdgeDetector* detector, bool* deletedHypothesises);

  
  /** */
  VLCGoalRecognizer::FreeSide detectFreePartOfGoal(VLCGoalRecognizer::EdgeDetector* detector, 
                                                      const VLCGoalRecognizer::GoalHypothesis& goal, 
                                                      int goalHeight, 
                                                      int& freeWidth, 
                                                      Vector2<int>& otherSide);

  /** */
  void lockArea(double x, double y, bool onGreen);

  /** */
  void calculateLockedPixels(const Vector2<int>& scanLineStart);
  void recalculateLockedPixels(const Vector2<int>& currentPoint);


private: // members

  /** The colour class of the goal to be detected by this object. */
  const colorClass goalColor;

  /** Collection of references to objects that are needed by this object, e.g. the current 
      image, objects used to store the processing results, etc. */
  const ImageProcessorInterfaces& interfaces;

  /** Object containing useful information related to the current horizon. */
  const ImageInfo& horizonInfo;

  /** Reference to the color correction object. */
  const ColorCorrector& colorCorrector;


  /** Counter to detect accumulations of goal-coloured pixels. Used by the method 
      inspectPixel. */ 
  int detectionCounter;

  /** Number of pixels that will not be inspected, because they are inside previously 
      analysed areas. */
  int lockedPixels;

  /** Stack containing the locked area. This is to prevent that goal-coloured blobs are 
      analysed twice. With inspectPixel being called on pixel from left to right and top
      to bottom, it is sufficient to store the bottom left point of the locked areas. */
  enum {lockAreaStackSize = 2};
  Vector2<double> lockAreaStack[lockAreaStackSize];
  int lockAreaCount;


  /** Detected goal hypothesises. */
  enum {maxHypothesises = 5};
  GoalHypothesis hypothesis[maxHypothesises];
  int hypothesisCount;

  /** Declare raster image which will take debug information.
      NB: due to the f**king macros, I have to declare both images, even though only need 
      one per instance. */
  DECLARE_DEBUG_IMAGE(imageProcessorGoal1);
  DECLARE_DEBUG_IMAGE(imageProcessorGoal2);

  
  
  // for VLC
  ColorTable * bestColorTable;
  //

};
#endif // VLCGoalRecognizer

---