Modules/ImageProcessor/SlamImageProcessor/SlamImageProcessor.h

Go to the documentation of this file.

/**
* @file SlamImageProcessor.h
*
* Definition of class SlamImageProcessor
*
* @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
* @author <a href="mailto:roefer@tzi.de">Thomas Röfer</a>
*/

#ifndef __SlamImageProcessor_h_
#define __SlamImageProcessor_h_

// Include those first to get the right macros
#include "../GT2005ImageProcessor/GT2005BallSpecialist.h"
#include "../GT2005ImageProcessor/GT2005Clustering.h"
#include "../GT2005ImageProcessor/GT2005LineFinder_DeterministicApproach.h"
//#include "../MSHImageProcessor/MSHCenterCircleFinder.h"

#include "Modules/ImageProcessor/ImageProcessor.h"
#include "Tools/Debugging/DebugDrawings.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Debugging/DebugImages.h"
#include "Tools/RingBuffer.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ColorCorrector.h"
#include "SlamImageProcessorTools.h"
#include "SlamGoalRecognizer.h"
#include "SlamBeaconDetector.h"
#include "SlamEdgeSpecialist.h"
#include "SlamBorderFinder.h"
#include "Tools/Actorics/RobotDimensions.h"
#include "Tools/RobotConfiguration.h"
#include "Tools/Math/Common.h"

struct PinkCaracteristic
{
  double  pixel;
  int   count;
};

#define NUM_PINK  16

/**
* @class SlamImageProcessor
*
* The lines image processor recognizes characteristic lines in the image.
* Four types of lines are distinguished:
* edges between the skyblue goal and the field, edges between the yellow goal 
* and the field, edges between the border and the field, and edges between the
* field lines and the field.
*
* The module scans vertical and horizontal lines in the image from top to bottom
* and from left to right. As the green of the field is very dark, all edges are
* characterized by a big difference of the y-channel of adjacent pixels. An
* increase in the y-channel followed by a decrease is an indication for an edge.
*
* The projection of the pixels on the field plane is used to determine their
* relative position to the robot.
*
* @author <a href="mailto:juengel@informatik.hu-berlin.de">Matthias Juengel</a>
* @author <a href="mailto:roefer@tzi.de">Thomas Röfer</a>
*/ 
class SlamImageProcessor : public ImageProcessor
{
public:
  /** 
  * Constructor.
  * @param interfaces The paramters of the SlamImageProcessor module.
  */
  SlamImageProcessor(const ImageProcessorInterfaces& interfaces);

  /** Executes the module */
  virtual void execute();

  /** Handles an incoming message
  * @param message The message
  */
  virtual bool handleMessage(InMessage& message);

private:
  PinkCaracteristic pinkCaracteristic[NUM_PINK];

  double filterWeight;

  double averageY;
  double averageU;
  double averageV;

  int averageCount;

  double xFactor, /**< Factor to convert the pixel coordinate space to the anglular coordinate space. */
         yFactor; /**< Factor to convert the pixel coordinate space to the anglular coordinate space. */
  int yThreshold; /**< Brightness increase threshold. */
  int vThreshold; /**< Brightness decrease threshold. */
  int orangeCount,  /**< Number of columns with ball points. */
      noOrangeCount, /**< Number of columns without a ball point. */
      noRedCount, /**< Number of columns without a red robot point. */
      noBlueCount, /**< Number of columns without a blue robot point. */
      noGoalCount, /**< Number of columns without a opponent goal seen. */
      closestBottom; /**< Closest bottom point on the grid. */
  Vector2<int> firstRed, /**< First red robot point in a cluster. */
               closestRed, /**< Closest red robot point in a cluster. */
               lastRed, /**< Last red robot point in a cluster. */
               firstBlue, /**< First blue robot point in a cluster. */
               closestBlue, /**< Closest blue robot point in a cluster. */
               lastBlue, /**< Last blue robot point in a cluster. */
               firstFlag, /**< First flag point in a cluster. */
               lastFlag; /**< Last flag point in a cluster. */
  bool goalAtBorder; /**< Is the first goal point at the image border? */
  int longestBallRun;
  Vector2<int> ballCandidate;
  GT2005Clustering ballClustering;
  GT2005LineFinder_DeterministicApproach lineFinder;
//  MSHCenterCircleFinder circleFinder;

  ImageInfo imageInfo; /**< Additional information about the current image */

  CameraMatrix cmTricot, /**< Camera matrix without tricot height. */
               prevCmTricot; /**< The tricot matrix of the previous image. */

  ColorCorrector colorCorrector; /**< The color correction tool. */

  SlamBeaconDetector beaconDetector; /**< The beacon detector */

  SlamGoalRecognizer goalRecognizer; /**< The goal recognizer. */

  GT2005BallSpecialist ballSpecialist; /**< The ball specialist. */

  SlamEdgeSpecialist edgeSpecialist; /**< The edge specialist. */

  SlamBorderFinder borderFinder;


  int count;

  /**
  * The function scans columns for line points.
  */
  void scanColumns();

  /** 
  * The function scans rows for line points. 
  */
  void scanRows();

  /** 
  * The function scans a line for line points. 
  * @param start The start point of the line.
  * @param end The end point of the line.
  * @param vertical Vertical lines are scanned for more information.
  * @param noLines Should the line not be scanned for points on field lines or borders?
  */
  void scan(const Vector2<int>& start, const Vector2<int>& end,
            bool vertical, bool noLines);
  
  /** 
  * The function clusters points of red and blue robots.
  * @param bottomPoint The bottom point of the current scan column.
  * @param redFound Has a red robot point been found? In that case, the last
  *                 entry in the lines percept is that point.
  * @param blueFound Has a blue robot point been found? In that case, the last
  *                  entry in the lines percept is that point.
  */
  void clusterRobots(const unsigned char* bottomPoint, bool redFound, bool blueFound);

  /** 
  * The function clusters flag points.
  * @param flagType The type of the flag.
  * @param point The center of the pink area.
  */
  //void clusterFlags(Flag::FlagType flagType, const Vector2<int>& point);

  /**
  * The function filters the percepts, i.e. it removes potential misreadings.
  */
  void filterPercepts();

  /**
  * The function filters the line-percepts, i.e. it removes potential misreadings, for the given line-type.
  */
  void filterLinesPercept(LinesPercept& percept, int type, const CameraMatrix& cameraMatrix, const CameraMatrix& prevCameraMatrix, const Image& image);

  /**
   * The function calculates the angle of an edge at an edge point.
   * @param angleInImage The angle in image coordinates to be determined.
   * @param angleOnField The angle in relative robot field coordinates to be determined.
   * @param pointInImage The edge point in image coordinates.
   * @param pointOnField The edge point in field coordinates.
   * @param scanAngle The angle of the scan line the point was found on.
   * @param pixelBuffer The pixels on the scan line around the edge point.
   * @param againstScanline The flag if bright to dark is detected against the direction of the scanline.
   * @param borderOrLine The flag if this edge belongs to a border or a line (white to green edge).
   * @param channel The channel the gradient is calculated in.
   * @return Whether the angle in field coordinates could be determined
   */
  bool calcEdgeAngle(
    double& angleInImage,
    double& angleOnField,
    const Vector2<int>& pointInImage, 
    const Vector2<int>& pointOnField,
    double scanAngle,
    const RingBuffer<const unsigned char*,7>& pixelBuffer,
    const bool againstScanline = false,
    const bool borderOrLine = true,
    int channel = 0);

 /** 
  * The function converts an address to pixel coordinates.
  * @param p An address in image.image.
  * @return The x- and y-coordinates of the corresponding pixel.
  */
  Vector2<int> getCoords(const unsigned char* p) const
  {
    const int diff(p - &image.image[0][0][0]);
    return Vector2<int>(diff % cameraResolutionWidth_ERS7, diff / (cameraResolutionWidth_ERS7 * 6));
  }

  //!@name Helpers for grid drawing
  //!@{
  const unsigned char* last;
  Drawings::Color lineColor;
  void plot(const unsigned char* p,Drawings::Color color);
  //!@}

  double angleBetweenDirectionOfViewAndGround;

  int numberOfScannedPixels;
  bool prevCameraMatrixUninitialized;

  Matrix2x2<double> rotation2x2;

  DECLARE_DEBUG_IMAGE(imageProcessorPlayers);
  DECLARE_DEBUG_IMAGE(imageProcessorGeneral);
  DECLARE_DEBUG_COLOR_CLASS_IMAGE(segmentedImage1);
  DECLARE_DEBUG_IMAGE(imageProcessorBall);
  DECLARE_DEBUG_IMAGE(imageProcessorGradients);

  N_DECLARE_DEBUG_IMAGE(colorCorrected);
  N_DECLARE_DEBUG_IMAGE(colorCorrectedAsJpeg);
  N_DECLARE_DEBUG_COLOR_CLASS_IMAGE(segmented);
  N_DECLARE_DEBUG_IMAGE(ball);
  N_DECLARE_DEBUG_IMAGE(edgeDetection);
  N_DECLARE_DEBUG_GRAY_SCALE_IMAGE(scanLines);
};

#endif// __SlamImageProcessor_h_

---