Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ShapeLandmarks.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef LOADED_ShapeLandmarks_h
00003 #define LOADED_ShapeLandmarks_h
00004 
00005 #include <vector>
00006 #include <string>
00007 
00008 #include "Vision/colors.h"
00009 
00010 #include "Shared/Measures.h"     // coordinate_t
00011 #include "DualCoding/ShapeRoot.h"
00012 #include "DualCoding/ShapeAprilTag.h"
00013 #include "DualCoding/ShapeCylinder.h"
00014 #include "DualCoding/ShapeMarker.h"
00015 
00016 using namespace DualCoding;
00017 
00018 //! Root class for the particle filter landmark classes.
00019 class PfRoot {
00020  public:
00021   int type;
00022   int id;
00023   rgb color;
00024   bool mobile;
00025   coordinate_t x, y;
00026   const ShapeRoot* link;
00027 
00028   PfRoot(int _type, int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y) :
00029     type(_type), id(_id), color(_color), mobile (_mobile), x(_x), y(_y),link(0) {}
00030     
00031   virtual ~PfRoot() {} //!< destructor, doesn't delete #link
00032 
00033   virtual void print(std::ostream &os) const = 0;
00034 
00035   void printRootInfo(std::ostream &os) const;
00036 
00037   static void loadLms(const std::vector<ShapeRoot> &lms, bool isWorld, std::vector<PfRoot*>& landmarks);
00038 
00039   static void deleteLms(std::vector<PfRoot*>& vec);
00040 
00041   static void findBounds(const std::vector<PfRoot*> &map, 
00042        coordinate_t &xmin, coordinate_t &ymin,
00043        coordinate_t &xmax, coordinate_t &ymax);
00044   
00045   static void printLms(const std::vector<PfRoot*> &lmvec);
00046 
00047 private:
00048   PfRoot(const PfRoot&); //!< don't call this
00049   PfRoot& operator=(const PfRoot&); //!< don't call this
00050 };
00051 
00052 //! A line landmark; world lines will have two of these, with the endpoints switched
00053 class PfLine : public PfRoot {
00054 public:
00055   coordinate_t x2, y2;
00056   bool valid1, valid2;
00057   AngPi orientation;
00058   float length;
00059 
00060   PfLine(int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y, 
00061    coordinate_t _x2, coordinate_t _y2, bool _v1, bool _v2) :
00062     PfRoot(lineDataType, _id, _color, _mobile, _x, _y),
00063     x2(_x2), y2(_y2), valid1(_v1), valid2(_v2), orientation(0), length(0) {}
00064 
00065   virtual void print(std::ostream &os) const;
00066 };
00067 
00068 //! An ellipse landmark
00069 class PfEllipse : public PfRoot {
00070 public:
00071   PfEllipse(int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y) :
00072     PfRoot(ellipseDataType, _id, _color, _mobile, _x, _y) {}
00073 
00074   virtual void print(std::ostream &os) const;
00075 };
00076 
00077 //! A point landmark
00078 class PfPoint : public PfRoot {
00079 public:
00080   PfPoint(int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y) :
00081     PfRoot(pointDataType, _id, _color, _mobile, _x, _y) {}
00082 
00083   virtual void print(std::ostream &os) const;
00084 };
00085 
00086 //! A blob landmark
00087 class PfBlob : public PfRoot {
00088 public:
00089   PfBlob(int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y) :
00090     PfRoot(blobDataType, _id, _color, _mobile, _x, _y) {}
00091 
00092   virtual void print(std::ostream &os) const;
00093 };
00094 
00095 //! A marker landmark
00096 class PfMarker : public PfRoot {
00097 public:
00098   Shape<MarkerData> data;
00099 
00100   PfMarker(int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y, const Shape<MarkerData>& _data) :
00101     PfRoot(markerDataType, _id, _color, _mobile, _x, _y), data(_data) {}
00102 
00103   virtual void print(std::ostream &os) const;
00104 };
00105 
00106 //! An apriltag landmark
00107 class PfAprilTag : public PfRoot {
00108 public:
00109   Shape<AprilTagData> data;
00110 
00111   PfAprilTag(int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y, const Shape<AprilTagData>& _data) :
00112     PfRoot(aprilTagDataType, _id, _color, _mobile, _x, _y), data(_data) {}
00113 
00114   virtual void print(std::ostream &os) const;
00115 };
00116 
00117 //! A cylinder landmark
00118 class PfCylinder : public PfRoot {
00119 public:
00120   PfCylinder(int _id, rgb _color, bool _mobile, coordinate_t _x, coordinate_t _y, const Shape<CylinderData>& cyl) :
00121     PfRoot(cylinderDataType, _id, _color, _mobile, _x, _y), radius(cyl->getRadius()), height(cyl->getHeight()) {}
00122 
00123   virtual void print(std::ostream &os) const;
00124 
00125   float radius, height;
00126 };
00127 
00128 std::ostream& operator<<(std::ostream &os, const PfRoot &x);
00129 
00130 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:50 2016 by Doxygen 1.6.3