Modules/ImageProcessor/VLCImageProcessor/VLCLineFinder_DeterministicApproach.h

Go to the documentation of this file.

#ifndef __VLCLineFinder_DeterministicApproach_h_
#define __VLCLineFinder_DeterministicApproach_h_


#include "Representations/Perception/Image.h"
#include "Representations/Perception/LinesPercept.h"
#include "Tools/Math/Geometry.h"
#include "Tools/Math/Matrix2x2.h"
#include "VLCImageProcessorTools.h"
#include "Modules/ImageProcessor/ImageProcessorTools/ImageProcessorUtilityClasses.h"
#include "Tools/Math/Common.h"
#include "Modules/ImageProcessor/ImageProcessorTools/BresenhamLineScan.h"
#include "Tools/ColorClasses.h"


class VLCLineFinder_DeterministicApproach
{
public:
  VLCLineFinder_DeterministicApproach(const ColorCorrector& colorCorrector, const ColorTable& colorTable);
  ~VLCLineFinder_DeterministicApproach(void);

  /**
  * Add a new LinePont for later consideration.
  * @param pointOnLine Image coordinates of the point.
  * @param normalToLine Normal vector to the line (indicated by the gradient).
  */ 
  void considerLinePoint(
    Vector2<int> & pointOnLine,
    Vector2<double> & normalToLine);
      
  /**
  * Calculates lines and crossings out of the given LinePoints.
  * @param linesPercept The LinesPercept, where the found information is stored.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @param imageInfo The ImageInfo.
  * @param robotPose The RobotPose.
  */ 
  void execute(
    LinesPercept & linesPercept,
    const CameraMatrix& cameraMatrix,
    const Image& image,
    const ImageInfo& imageInfo,
    const RobotPose & robotPose);

    /**
  * Calculates lines and crossings out of the given LinePoints considering given information about the center circle.
  * @param linesPercept The LinesPercept, where the found information is stored.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @param imageInfo The ImageInfo.
  * @param robotPose The RobotPose.
  * @param circleFound True if the circle was found.
  * @param circleOnField The Coordinates of the center circle on the field relative to the robot.
  */ 
  void execute(
    LinesPercept & linesPercept,
    const CameraMatrix& cameraMatrix,
    const Image& image,
    const ImageInfo& imageInfo,
    const RobotPose & robotPose,
    const bool circleFound,
    const Vector2<double> & circleOnField);

  /**
  * Resets the lineFinder.
  */
  void reset();
  
    
  /**
  * Returns the number of found lines.
  */
  int getNumberOfLines();
    
  /**
  * Returns the line by number.
  * @param number The number of the line to return.
  * @param pointOnLine The base of the line.
  * @param normalToLine The normal of the line.
  */
  void getLine(
    int number,
    Vector2<int> & pointOnLine,
    Vector2<double> & normalToLine);
  
  enum {maxNumberOfLines = 6, maxNumberOfLinePoints = 60, pointsNeededForLine = 3};


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

  
private:
  /**
  * Calculates line fragments out of the given LinePoints.
  */ 
  void findLineFragments();
  
  /**
  * Calculates lines out of the previously calculated line fragments.
  * @param linesPercept The LinesPercept.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @param imageInfo The ImageInfo.
  */ 
  void findLines(
    LinesPercept & linesPercept,
    const CameraMatrix& cameraMatrix,
    const Image& image,
    const ImageInfo& imageInfo);
    
  /**
  * Calculates crossings out of the previously calculated lines.
  * @param linesPercept The LinesPercept.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @param robotPose The RobotPose.
  * @param imageInfo The ImageInfo.
  */
  void findIntersections(
    LinesPercept & linesPercept,
    const CameraMatrix& cameraMatrix,
    const Image& image,
    const RobotPose & robotPose,
    const ImageInfo& imageInfo);

    /**
  * Adds the crossing to the linesPercept including some characteristics found by additional scanning.
  * @param intersectionInImage The intersection in the image.
  * @param intersectionOnField The intersection on the field.
  * @param int The first line involved in this crossing.
  * @param int The second line involved in this crossing.
  * @param linesPercept The LinesPercept.
  * @param image The Image.
  * @param imageInfo The ImageInfo.
  * @param cameraMatrix The CameraMatrix.
  * @param robotPose The RobotPose.
  */
  void addCrossingsPercept(
    const Vector2<double> & intersectionInImage,
    const Vector2<int> & intersectionOnField, 
    int lineNumber1,
    int lineNumber2,
    LinesPercept & linesPercept,
    const Image& image,
    const ImageInfo& imageInfo,
    const CameraMatrix & cameraMatrix,
    const RobotPose & robotPose);

  /**
  * Filters out some line fragments, that may be caused by a center circle.
  * Do only use when no center detection available.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @param robotPose The RobotPose.
  * @return true if circle found.
  */ 
  bool handleCenterCircle(
    const CameraMatrix& cameraMatrix,
    const Image& image,
    const RobotPose & robotPose);

  /**
  * Filters out line points belonging to the center circle.
  * @param centerOnField The centerCircle found by the circleFinder.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @return true.
  */ 
  bool handleCenterCircle(
    const Vector2<double> & centerOnField,
    const CameraMatrix& cameraMatrix,
    const Image& image);

  /**
  * Writes the centerCircle (if found by one of the attempts) to the LinesPercept.
  * @param linesPercept The LinesPercept.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  */ 
  void addCenterCirclePercept(
    LinesPercept & linesPercept,
    const CameraMatrix& cameraMatrix,
    const Image& image);

  /**
  * Very rough calculation if lines are perpendicular on the field (used for validating crossings).
  * @param lineNumber1 Number of the 1st line.
  * @param lineNumber2 Number of the 2nd line.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @return True if the lines are perpendicular.
  */ 
  bool linesPerpendicularOnField(
    int lineNumber1,
    int lineNumber2,
    const CameraMatrix& cameraMatrix,
    const Image& image);

  /**
  * Calculates the projected line on the field.
  * @param lineNumber Number of the line to be projected on the field.
  * @param base The resulting base.
  * @param direction The resulting direction.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @return true if calculation was successful.
  */ 
  bool calculateLineOnField(
    int lineNumber,
    Vector2<double> & base,
    Vector2<double> & direction,
    const CameraMatrix& cameraMatrix,
    const Image& image);

  /**
  * Calculates the projected line on the field.
  * @param baseInImage The base in the image.
  * @param directionInImage The direction in the image.
  * @param baseOnField The resulting base.
  * @param directionOnField The resulting direction.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @return true if calculation was successful.
  */ 
  bool calculateLineOnField(
    const Vector2<int> & baseInImage,
    const Vector2<double> & directionInImage,
    Vector2<double> & baseOnField,
    Vector2<double> & directionOnField,
    const CameraMatrix& cameraMatrix,
    const Image& image);
  
  /**
  * Calculates the projected line on the field.
  * @param crossingPoint The crossingPoint in the image.
  * @param directionToScanAt The direction in the image, on which side of the crossingPoint should be scanned.
  * @param scanningDirection The direction of the scan line (not always perpendicular because of perspectival distortion).
  * @param lineSize The size of a line at this position in the image.
  * @param result The result of the scan.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  */ 
  void doVerificationScan(
    const Vector2<double> & crossingPoint,
    const Vector2<double> & directionToScanAt,
    const Vector2<double> & scanningDirection,
    int lineSize,
    LinesPercept::CrossingCharacteristic & result,
    const CameraMatrix& cameraMatrix,
    const Image& image);
  
  /**
  * Verifies if the found circle really is one.
  * @param circleOnFieldRelative The coordinates of the circle on the field.
  * @param orientation The orientation of the middle line through the circle.
  * @param cameraMatrix The CameraMatrix.
  * @param image The Image.
  * @return true if nothing is found that proves the candidate not to be a circle.
  */ 
  bool doVerificationScanForCircle(
    const Vector2<int> & circleOnFieldRelative,
    double orientation,
    const CameraMatrix& cameraMatrix,
    const Image& image);

  struct LinePoint
  {
    Vector2<int> pointOnLine;
    Vector2<double> normalToLine;
    int belongsToLineNo;
  };

  struct LineFragment
  {
    Vector2<int> base;
    Vector2<double> normal;
    bool lineFragmentAlreadyConsidered;    
    Vector2<int> start;
    Vector2<int> end;
    double averageStep;
    int numberOfPoints;
  };
  
  
  // for the resulting lines (fused from line fragmens)
  int numberOfLines;
  //LineFragment lines[maxNumberOfLines];
  LineFragment * lines;
  //


  // for line fragments
  int numberOfLineFragments;
  bool lineFragmentAlreadyConsidered[maxNumberOfLines*2];
  //LineFragment lineFragments[maxNumberOfLines*2];
  LineFragment * lineFragments;
  //

  // for the centerCircle
  Vector2<int> fromCircleFinder;
  bool foundByCircleFinder;
  Vector2<int> fromOwnHandling;
  bool foundByOwnHandling;
  double centerCircleOrientation;
  Vector2<double> lastSeenCircleGlobal;
  double lastSeenCircleOrientationGlobal;
  int framesSinceLastSeen;
  int frameCount, circleCount;
  //

  const ColorCorrector& colorCorrector;
  const ColorTable& colorTable;
  LinePoint linePoints[maxNumberOfLinePoints];
  int numberOfLinePoints;
  double normDistance;
  double normProjection;
  double normProjectionPerpendicularToHorizon; // used for fusing parallel line fragments, that are nearly perpendicular to the horizon

  
  // for VLC
  ColorTable * bestColorTable;
  //
};


#endif // __VLCLineFinder_DeterministicApproach_h_

---