Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

MapBuilder.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 #ifndef _MapBuilder_h_
00003 #define _MapBuilder_h_
00004 
00005 #include <queue>
00006 
00007 #include "Crew/MapBuilderRequest.h"
00008 #include "Shared/fmatSpatial.h"
00009 
00010 #include "DualCoding/Point.h"
00011 
00012 #include "DualCoding/BlobData.h"
00013 #include "DualCoding/EllipseData.h"
00014 #include "DualCoding/LineData.h"
00015 #include "DualCoding/SphereData.h"
00016 #include "DualCoding/CylinderData.h"
00017 #include "DualCoding/TargetData.h"
00018 #include "DualCoding/MarkerData.h"
00019 
00020 #include "DualCoding/VRmixin.h"
00021 #include "DualCoding/SketchTypes.h"
00022 #include "DualCoding/ShapeSpace.h"
00023 #include "DualCoding/PolygonData.h"
00024 
00025 // Note: these are NOT in the DualCoding namespace
00026 class LookoutSketchEvent;
00027 class SiftTekkotsu;
00028 
00029 namespace DualCoding {
00030 
00031 class SketchSpace;
00032 
00033 class MapBuilder : public BehaviorBase {
00034 protected:
00035   SketchSpace &camSkS;
00036   ShapeSpace &camShS, &groundShS;
00037   SketchSpace &localSkS;
00038   ShapeSpace &localShS;
00039   SketchSpace &worldSkS;
00040   ShapeSpace &worldShS;
00041 
00042   const int xres, yres; //!< width and height of camera frame
00043 
00044   PlaneEquation ground_plane; //!< ground plane to which shapes are projected
00045 
00046   static bool retain; //!< if true, VRmixin::stopCrew will not clear MapBuilder structures
00047 
00048 public:
00049   //! Control whether static structures (mapbuilder, sketchGUI sockets, etc.) are retained
00050   static void setRetain(bool r) { retain = r; }
00051   static bool isRetained() { return retain; }
00052 
00053   typedef unsigned int MapBuilderVerbosity_t;
00054   static const MapBuilderVerbosity_t MBVstart = 1<<0;
00055   static const MapBuilderVerbosity_t MBVevents = 1<<1;
00056   static const MapBuilderVerbosity_t MBVexecute = 1<<2;
00057   static const MapBuilderVerbosity_t MBVcomplete = 1<<3;
00058   static const MapBuilderVerbosity_t MBVdefineGazePoints = 1<<4;
00059   static const MapBuilderVerbosity_t MBVgazePointQueue = 1<<5;
00060   static const MapBuilderVerbosity_t MBVnextGazePoint = 1<<6;
00061   static const MapBuilderVerbosity_t MBVshapeSearch = 1<<7;
00062   static const MapBuilderVerbosity_t MBVshapesFound = 1<<8;
00063   static const MapBuilderVerbosity_t MBVgroundPlane = 1<<9;
00064   static const MapBuilderVerbosity_t MBVprojectionFailed = 1<<10;
00065   static const MapBuilderVerbosity_t MBVimportShapes = 1<<11;
00066   static const MapBuilderVerbosity_t MBVnotAdmissible = 1<<12;
00067   static const MapBuilderVerbosity_t MBVshapeMatch = 1<<13;
00068   static const MapBuilderVerbosity_t MBVshapesMerge = 1<<14;
00069   static const MapBuilderVerbosity_t MBVshouldSee = 1<<15;
00070   static const MapBuilderVerbosity_t MBVdeleteShape = 1<<16;
00071   static const MapBuilderVerbosity_t MBVsetAgent = 1<<17;
00072   static const MapBuilderVerbosity_t MBVbadGazePoint = 1<<18;
00073   static const MapBuilderVerbosity_t MBVskipShape = 1<<19;
00074 
00075 private:
00076   static MapBuilderVerbosity_t verbosity;
00077 public:
00078   static void setVerbosity(MapBuilderVerbosity_t v) { verbosity = v; }
00079 
00080 protected:
00081   friend class Lookout;
00082   friend class BiColorMarkerData;   // needs to call getCamBlobs()
00083   friend class Pilot;
00084   friend class LineData;
00085   friend class EllipseData;
00086 
00087   Shape<AgentData> &theAgent; //!< Agent in the world frame
00088    //!@name Transformation matrices between local and world frames
00089   //@{
00090  public:
00091   fmat::Transform localToWorldMatrix, worldToLocalMatrix;
00092   //@}
00093 
00094 protected:
00095   std::vector<Point> badGazePoints; //!<  gaze points for which HeadPointerMC.lookAtPoint() returned false
00096 
00097   std::queue<MapBuilderRequest*> requests;
00098   MapBuilderRequest *curReq;
00099   unsigned int idCounter;
00100   float maxDistSq; //!< square of current request's max distance parameter
00101   std::map<string,SiftTekkotsu*> siftMatchers;
00102 
00103   unsigned int pointAtID, scanID; //!< ID's for lookout requests
00104   Point nextGazePoint;
00105 
00106   //! Triggers action to execute the request at the front of the queue
00107   void executeRequest();
00108   //! calls exitTest of current request if there is one and returns the result
00109   bool requestExitTest();
00110   //! posts completion event and deletes current request, executes next request if there is one
00111   void requestComplete(); 
00112 
00113 #ifdef TGT_IS_AIBO
00114 public:
00115 #endif
00116   //! Sets agent location and heading, and recomputes local-to-world transformation matrices.  Called by the Pilot.
00117   void setAgent(const Point &worldLocation, const AngTwoPi worldHeading, bool quiet=false);
00118 
00119 public:
00120   MapBuilder(); //!< Constructor
00121   virtual ~MapBuilder() {}   //!< Destructor
00122   virtual void preStart();
00123   virtual void stop(); 
00124   virtual std::string getDescription() const { return "MapBuilder"; }
00125   void printShS(ShapeSpace&) const;
00126   unsigned int executeRequest(const MapBuilderRequest&, unsigned int *req_id=NULL); //!< Execute a MapBuilder request, and optionally store the request id in a variable; the id will be returned in any case
00127   void executeRequest(BehaviorBase* requestingBehavior, const MapBuilderRequest &req); //!< Execture a MapBuilder request and store the address of the requesting behavior to use as the source ID in a mapbuilder status event
00128 
00129   MapBuilderRequest* getCurrentRequest() { return curReq; }
00130 
00131   virtual void doEvent();
00132   void processImage(const LookoutSketchEvent&);
00133 
00134   // Returns true if a ground shape should be seen in the current camera frame
00135   static bool isPointVisible(const Point &pt, const fmat::Transform& baseToCam, float maxDistanceSq) ;
00136   static bool isLineVisible(const LineData& ln, const fmat::Transform& baseToCam);
00137   static bool isShapeVisible(const ShapeRoot &ground_shape, const fmat::Transform& baseToCam, float maxDistanceSq);
00138   
00139   
00140   //! utility functions which may be used by MapBuilderRequest's exit condition and others
00141   //@{
00142   const Shape<AgentData>& getAgent() const { return theAgent; }
00143   
00144   //! updates the agent location and heading after a relative move
00145   void moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, coordinate_t const local_dz, AngSignPi dtheta);
00146   //@}
00147   
00148   std::vector<ShapeRoot> getShapes(const ShapeSpace& ShS, int minConf=2) const {
00149     const std::vector<ShapeRoot> allShapes = ShS.allShapes();
00150     if (&ShS == &camShS || &ShS == &groundShS || minConf <= 0) 
00151       return allShapes;
00152     std::vector<ShapeRoot> nonNoiseShapes;
00153     for (std::vector<ShapeRoot>::const_iterator it = allShapes.begin();
00154    it != allShapes.end(); it++)
00155       if ((*it)->getConfidence() >= minConf)
00156   nonNoiseShapes.push_back(*it);
00157     return nonNoiseShapes;
00158   }
00159 
00160   void importLocalToWorld();
00161 
00162   ShapeRoot importLocalShapeToWorld(const ShapeRoot &localShape);
00163   ShapeRoot importWorldToLocal(const ShapeRoot &worldShape);
00164   template<class T> Shape<T> importWorldToLocal(const Shape<T> &worldShape);
00165 
00166 protected:
00167   //!@name Shape extraction functions
00168   //@{
00169   void getCameraShapes(const Sketch<uchar>& camFrame);
00170 
00171   std::vector<Shape<LineData> > 
00172   getCamLines(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00173         const std::set<color_index>& occluderColors) const;
00174 
00175   std::vector<Shape<EllipseData> > 
00176   getCamEllipses(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00177      const std::set<color_index>& occluderColors) const;
00178 
00179   void getCamPolygons(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00180           const std::set<color_index>& occluderColors) const;
00181 
00182   std::vector<Shape<LineData> >  
00183   getCamWalls(const Sketch<uchar>&, unsigned int) const;
00184 
00185   void getCamSpheres(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00186          const std::set<color_index>& occluderColors) const;
00187 
00188   void getCamCylinders(const Sketch<uchar>& camFrame,
00189            const std::set<color_index>& colors,
00190            const std::map<color_index,coordinate_t>& assumedHeights,
00191            const std::map<color_index,int>& minCylinderAreas);
00192 
00193   void getCamBlobs(const Sketch<uchar>& camFrame,
00194        const std::set<color_index>& colors,
00195        const std::map<color_index,int>& minBlobAreas,
00196        const std::map<color_index, BlobData::BlobOrientation_t>& blobOrientations,
00197        const std::map<color_index,coordinate_t>& assumedBlobHeights);
00198   void getCamBlobs(const set<color_index>& colors, int defMinBlobArea=0);
00199   
00200   void getCamTargets(const Sketch<uchar> &camFrame, const std::set<color_index>& objectColors,
00201          const std::set<color_index>& occluderColors) const;
00202   
00203   std::vector<Shape<MarkerData> > 
00204   getCamMarkers(const Sketch<uchar> &camFrame, const std::set<color_index>& objectColors,
00205     const std::set<color_index>& occluderColors, const std::set<MarkerType_t>& markerTypes) const;
00206 
00207   void getCamSiftObjects(const Sketch<uchar> &rawY, const std::string &siftDatabasePath, 
00208        const std::set<std::string> &siftObjectNames);
00209 
00210   void getCamAprilTags(const Sketch<uchar> &rawY);
00211 
00212   void getCamDominoes(const Sketch<uchar> &camFrame, const std::set<color_index>& objectColors,
00213                      const std::set<color_index>& secondColors);
00214 
00215   void getCamNaughts(const Sketch<uchar> &camFrame, const std::set<color_index>& objectColors, const fmat::Column<3>& dimensions) const;
00216 
00217   void getCamCrosses(const Sketch<uchar> &camFrame, const std::set<color_index>& objectColors) const;
00218 
00219   void getCamAgents(const Sketch<uchar> &camFrame, const Sketch<yuv> &camFrameYUV,
00220                     const std::set<color_index>& objectColors) const;
00221 
00222   //@}
00223 
00224 public:
00225   void newSiftMatcher(const std::string &siftDatabasePath);
00226 
00227   void saveSiftDatabase(const std::string &siftDatabasePath);
00228 
00229   void trainSiftObject(const std::string &siftDatabasePath,
00230            const std::string &objectName, const std::string &modelName="model1");
00231 
00232   void trainSiftObject(const std::string &siftDatabasePath, const Sketch<uchar> &sketch,
00233            const std::string &objectName, const std::string &modelName="model1");
00234 
00235 public: // ** debug **
00236   // matching shapes between two spaces.
00237   void matchSrcToDst(ShapeSpace &src, ShapeSpace &dst, std::set<color_index> polygonEdgeColors=std::set<color_index>(),
00238           bool mergeSrc=true, bool mergeDst=true);
00239 
00240   //!@name Functions to make requests to the Lookout
00241   //@{
00242   void storeImage(bool useNextGazePoint);
00243   void grabCameraImageAndGo();
00244   void scanForGazePts();
00245   //@}
00246 
00247   //! set up initial gazePts either virtually or by scan
00248   void setInitialGazePts();
00249   
00250   void extendLocal(const fmat::Transform& baseToCam);
00251   void extendWorld(const fmat::Transform& baseToCam);
00252 
00253   //! decrement confidence of shapes which should have been seen according to the baseToCam matrix
00254   void removeNoise(ShapeSpace&, const fmat::Transform& baseToCam);
00255   //! erase gaze points which should have been seen according to the baseToCam matrix
00256   void removeGazePts(std::vector<GazePoint>&, const fmat::Transform& baseToCam);
00257   
00258   //! Returns true if it has set up a valid next gaze point in nextGazePoint
00259   bool determineNextGazePoint();
00260   //! Returns true if there is a shape which needs be looked at again and is reachable; sets it up as nextGazePoint
00261   bool determineNextGazePoint(const std::vector<ShapeRoot>&);
00262   // Returns true if an element of gazePts can be looked at; sets it up as nextGazePoint
00263   bool determineNextGazePoint(std::vector<GazePoint> &gazePts);
00264   //! Starts robot moving to the next gaze point
00265   void moveToNextGazePoint(const bool manualOverride=false);
00266   void doNextSearch();
00267   void doNextSearch2();
00268 
00269   // operations in ground shape space 
00270   bool isBadGazePoint(const Point&) const ;
00271   void projectToGround(const fmat::Transform& camToBase);
00272   ShapeRoot projectToLocal(ShapeRoot &shape);
00273   void filterGroundShapes(const fmat::Transform& baseToCam);
00274 
00275   // calculates ground plane based on ground plane assumption type
00276   void calculateGroundPlane();
00277 
00278 private:
00279   MapBuilder(const MapBuilder&); //!< never call this
00280   MapBuilder& operator=(const MapBuilder&);  //!< never call this
00281 };
00282 
00283 template<class T> Shape<T> MapBuilder::importWorldToLocal(const Shape<T> &worldShape) {
00284   ShapeRoot temp(localShS.importShape(worldShape));
00285   Shape<T> localShape(ShapeRootType(temp,T));
00286   localShape->applyTransform(worldToLocalMatrix);
00287   return localShape;
00288 }
00289 
00290 //! Utility function for deleting queues of pointers to Lookout or MapBuilder requests
00291 template<typename T> void deleteAll(std::queue<T*> &q) {
00292   while ( ! q.empty() ) {
00293     delete q.front();
00294     q.pop();
00295   }
00296 }
00297 
00298 } // namespace
00299 
00300 #endif
00301 

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