Modules/ImageProcessor/SlamImageProcessor/SlamBorderFinder.h

Go to the documentation of this file.

/**
* @file SlamBorderFinder.h
*
* Definition of class SlamBorderFinder
*
* @author <a href="mailto:stefan.czarnetzki@uni-dortmund.de">Stefan Czarnetzki</a>
*/

#ifndef __SlamBorderFinder_h_
#define __SlamBorderFinder_h_


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


class SlamBorderFinder
{
public:
  SlamBorderFinder(const ColorCorrector& colorCorrector, const ColorTable& colorTable);
  ~SlamBorderFinder(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);

  /**
  * 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};
  
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);

  /**
  * Calculates the direction away from the border.
  * @param linesPercept The LinesPercept.
  * @param image The Image.
  * @param imageInfo The ImageInfo.
  * @param cameraMatrix The CameraMatrix.
  * @param robotPose The RobotPose.
  */
  void calculateDirectionAwayFromBorder(
    LinesPercept & linesPercept,
    const Image& image,
    const ImageInfo& imageInfo,
    const CameraMatrix & cameraMatrix,
    const RobotPose & robotPose);

  /**
  * 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);
  
  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;
  //

  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
};


#endif // __SlamBorderFinder_h_

---