Modules/ImageProcessor/SlamImageProcessor/SlamFlagLocator.h

Go to the documentation of this file.

/**
* @file SlamFlagLocator.h
*
* Definition of class SlamFlagLocator
* The FlagLocator localizes the position of additional landmarks
*
* @author <a href="mailto:c_rohde@web.de">Carsten Rohde</a>
*/

#include "Tools/Math/Vector2.h"
#include "Tools/Math/Pose2D.h"
#include "../ImageProcessorTools/BresenhamLineScan.h"
// #include "Tools/Debugging/Debugging.h"
#include "Tools/Debugging/DebugDrawings.h"


#ifndef SlamFlagLocator_h
#define SlamFlagLocator_h

class SlamFlagLocator
{

public:

  // embedded class newflagcontainer, ds for new flags
  class NewFlagContainer{
  public:
    enum{
      NO_OF_FLAGS = 10,
      NO_OF_CHARACTERIZATIONS = 13 // 12 + dont know
    };

    NewFlagContainer()
    {
      reset();
    }
  
    void reset()
    {
      for(int i=0;i<NO_OF_CHARACTERIZATIONS;++i)
        numberOfStoredFlags[i] = 0;
    }

    void addFlag(Vector2<int> flag, int characterization)
    {
      if (numberOfStoredFlags[characterization]>=NO_OF_FLAGS)
      {
      numberOfStoredFlags[characterization] = 0;
      }
      flagPositions[characterization][numberOfStoredFlags[characterization]] = flag;
      numberOfStoredFlags[characterization]++;

    }

    void getFlagsForCharacterization(Vector2<int>** flags,int& numberOfStored, int characterization)
    {
      (*flags) = flagPositions[characterization];
      numberOfStored = numberOfStoredFlags[characterization];
    }

  private:

    Vector2<int> flagPositions[NO_OF_CHARACTERIZATIONS][NO_OF_FLAGS];
    int numberOfStoredFlags[NO_OF_CHARACTERIZATIONS]; 
  
  };

  enum Sizes{
    xSize = 60+6,
    ySize = 40+6
  };

  SlamFlagLocator();
  ~SlamFlagLocator();

  void addFlagDirection(const Pose2D& robotPose, double bearing, int characterization);
  void finishUpdate();
  void getClosestFlagBearing(double& targetBearing, const Pose2D& robotPose, double measuredBearing);
  void draw();

  enum flagCharacteristics
  {
    blueOnThisSide=0,
    yellowOnThisSide=1,
    somethingElseOnThisSide=2
  };

  int getCharacterisationNumber(flagCharacteristics above, flagCharacteristics below, flagCharacteristics left, flagCharacteristics right)
  {
    return above + 3*below + 9*left + 18*right; // muhahahahahahaha   ;-)
  }



private:
  long** cluster[NewFlagContainer::NO_OF_CHARACTERIZATIONS];
  NewFlagContainer* container;
  bool updateFinished;
  double averageParam, maxParam;
  void calcFlags();

};

#endif

---