Modules/SelfLocator/GT2005SelfLocator/GT2005SampleTemplateGenerator.h

Go to the documentation of this file.

 #ifndef __GT2005SampleTemplateGenerator_h_
#define __GT2005SampleTemplateGenerator_h_

/*
* Template generation for the gt2005 SelfLocator, completely
* identical to the gt2004 SelfLocators template generation
*
* @author <A href=mailto:roefer@tzi.de>Thomas Röfer</A>
* @author <A href=mailto:c_rohde@web.de>Carsten Rohde</A>
* @author <A href=mailto:judith-winter@web.de>Judith Winter</A>
*/

#include "Representations/Perception/LandmarksPercept.h"
#include "Tools/Math/Pose2D.h"
#include "Tools/Field.h"
#include "../LinesTables2005.h"

#include "GT2005SelfLocatorSample.h"
#include "GT2005SelfLocatorParameters.h"

// remove most of the parameters of generateTemplate by adding addXXX Mehtods to
// keep datastructures inside this class updated

/**
* A Class to encapsulate the template geneation feature of the gt04 localizer
*/
class GT2005SampleTemplateGenerator : public LinesTables2005
{
public:
  /**
  * Constructor
  */
  GT2005SampleTemplateGenerator(const GT2005SelfLocatorParameters& parameters);
  
  /**
  * this method returns a generated template
  */
  GT2005SelfLocatorSample getTemplate();

  /**
  * this method generates new sample templates
  */
  void generateTemplates(const LandmarksPercept& landmarksPercept, 
               const LinesPercept& linesPercept,
                           const Pose2D& odometry);

  int getNumberOfTemplates() const;

  void draw(const Pose2D& pose, const Drawings::Color& color) const;
  void draw(const Pose2D &pose, const Drawings::Color &color,
            const Vector2<double>& mark1, const Vector2<double>& mark2, double dir1, double dir2) const;
  void draw(const Pose2D &pose, const Drawings::Color &color,
            const Vector2<double>& mark1, const Vector2<double>& mark2,
            const Vector2<double>& mark3, double dir1, double dir2, double dir3) const;

private:

  const GT2005SelfLocatorParameters& parameters;

  enum{
    TEMPLATES_MAX = 100,        /**< maximum number of samples - should be the same as max number of samples in locator */
    FLAGS_MAX = 3               /**< maximum number of flags stored in a buffer */
  };

  Flag flags[FLAGS_MAX];              /**< A buffer for previously seen flags. */

  int numberOfTemplates,              /**< The number of templates generated. */
      numberOfFlags,
      nextTemplate;               /**< The next template delivered. */
  int randomFactor;               /**< A factor that is increased if more templates are required. */
  Pose2D templates[TEMPLATES_MAX];        /**< Templates for poses replacing bad samples. */

  bool getBearing(const LandmarksPercept& landmarksPercept,int i,
          Vector2<double>& mark, double& dir,double& dist);

  int poseFromBearingsAndDistance( double dir0,double dir1,double dist,
                  const Vector2<double>& mark0,
                  const Vector2<double>& mark1,
                  const Vector2<double>& cameraOffset,
                  Pose2D& resultingPose1,
                  Pose2D& resultingPose2);

  bool poseFromBearings(double dir0,double dir1,double dir2,
            const Vector2<double>& mark0,
            const Vector2<double>& mark1,
            const Vector2<double>& mark2,
            const Vector2<double>& cameraOffset,
            Pose2D& resultingPose);

  void addFlag(const Flag& flag);

  // test if templates returned from poseFromBearings were calculated correctly
  // this should not be necessary but sometimes wrong poses are returned
  bool checkTemplatePose(const Pose2D &pose, const Vector2<double> &mark, double dir);
};
#endif

---