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 "Behaviors/BehaviorBase.h"
00008 #include "Shared/newmat/newmat.h"
00009 
00010 #include "Point.h"
00011 
00012 #include "BlobData.h"
00013 #include "EllipseData.h"
00014 #include "LineData.h"
00015 #include "SphereData.h"
00016 #include "TargetData.h"
00017 
00018 #include "VRmixin.h"
00019 #include "MapBuilderRequest.h"
00020 #include "SketchTypes.h"
00021 #include "ShapeSpace.h"
00022 #include "PolygonData.h"
00023 
00024 class LookoutSketchEvent;  // note: this is NOT in the DualCoding namespace
00025 
00026 namespace DualCoding {
00027 
00028 class SketchSpace;
00029 
00030 class MapBuilder : public BehaviorBase {
00031 protected:
00032   SketchSpace &camSkS;
00033   ShapeSpace &camShS, &groundShS;
00034   SketchSpace &localSkS;
00035   ShapeSpace &localShS;
00036   SketchSpace &worldSkS;
00037   ShapeSpace &worldShS;
00038 
00039   const int xres, yres; //!< width and height of camera frame
00040 
00041   NEWMAT::ColumnVector ground_plane; //!< ground plane to which shapes are projected
00042 
00043   static bool retain; //!< if true, VRmixin::stopCrew will not clear MapBuilder structures
00044 
00045 public:
00046   //! Control whether static structures (mapbuilder, sketchGUI sockets, etc.) are retained
00047   static void setRetain(bool r) { retain = r; }
00048   static bool isRetained() { return retain; }
00049 
00050   typedef unsigned int MapBuilderVerbosity_t;
00051   static const MapBuilderVerbosity_t MBVstart = 1<<0;
00052   static const MapBuilderVerbosity_t MBVevents = 1<<1;
00053   static const MapBuilderVerbosity_t MBVexecute = 1<<2;
00054   static const MapBuilderVerbosity_t MBVcomplete = 1<<3;
00055   static const MapBuilderVerbosity_t MBVdefineGazePoints = 1<<4;
00056   static const MapBuilderVerbosity_t MBVnextGazePoint = 1<<5;
00057   static const MapBuilderVerbosity_t MBVshapeSearch = 1<<6;
00058   static const MapBuilderVerbosity_t MBVshapesFound = 1<<7;
00059   static const MapBuilderVerbosity_t MBVgroundPlane = 1<<8;
00060   static const MapBuilderVerbosity_t MBVprojectionFailed = 1<<9;
00061   static const MapBuilderVerbosity_t MBVimportShapes = 1<<10;
00062   static const MapBuilderVerbosity_t MBVnotAdmissible = 1<<11;
00063   static const MapBuilderVerbosity_t MBVshapeMatch = 1<<12;
00064   static const MapBuilderVerbosity_t MBVshapesMerge = 1<<13;
00065   static const MapBuilderVerbosity_t MBVshouldSee = 1<<14;
00066   static const MapBuilderVerbosity_t MBVdeleteShape = 1<<15;
00067   static const MapBuilderVerbosity_t MBVsetAgent = 1<<16;
00068   static const MapBuilderVerbosity_t MBVbadGazePoint = 1<<17;
00069 
00070 private:
00071   static MapBuilderVerbosity_t verbosity;
00072 public:
00073   static void setVerbosity(MapBuilderVerbosity_t v) { verbosity = v; }
00074 
00075 protected:
00076   friend class Lookout;
00077 
00078   Shape<AgentData> &theAgent; //!< Agent in the world frame
00079    //!@name Transformation matrices between local and world frames
00080   //@{
00081   NEWMAT::Matrix localToWorldMatrix, worldToLocalTranslateMatrix, worldToLocalRotateMatrix;
00082   //@}
00083 
00084   std::vector<Point> badGazePoints; //!<  gaze points for which HeadPointerMC.lookAtPoint() returned false
00085 
00086   std::queue<MapBuilderRequest*> requests;
00087   MapBuilderRequest *curReq;
00088   unsigned int idCounter;
00089   
00090   float maxDistSq; //!< square of current request's max distance parameter
00091   unsigned int pointAtID, scanID; //!< ID's for lookout requests
00092   Point nextGazePoint;
00093   //! Triggers action to execute the request at the front of the queue
00094   void executeRequest();
00095   //! calls exitTest of current request if there is one and returns the result
00096   bool requestExitTest();
00097   //! posts completion event and deletes current request, executes next request if there is one
00098   void requestComplete(); 
00099 
00100 public:
00101   MapBuilder(); //!< Constructor
00102   virtual ~MapBuilder() {}   //!< Destructor
00103   virtual void DoStart();
00104   virtual void DoStop(); 
00105   virtual std::string getDescription() const { return "MapBuilder"; }
00106   void printShS(ShapeSpace&) const;
00107   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
00108 
00109   virtual void processEvent(const EventBase&);
00110   void processImage(const LookoutSketchEvent&);
00111 
00112   // Returns true if a ground shape should be seen in the current camera frame
00113   static bool isPointVisible(const Point &pt, const NEWMAT::Matrix& baseToCam, float maxDistanceSq) ;
00114   static bool isLineVisible(const LineData& ln, const NEWMAT::Matrix& baseToCam);
00115   static bool isShapeVisible(const ShapeRoot &ground_shape, const NEWMAT::Matrix& baseToCam, float maxDistanceSq);
00116   
00117   
00118   //! utility functions which may be used by MapBuilderRequest's exit condition and others
00119   //@{
00120   const Shape<AgentData>& getAgent() const { return theAgent; }
00121 
00122   // sets the agent location and heading
00123   void setAgent(const Point &worldLocation, const AngTwoPi worldHeading);
00124   
00125   // updates the agent location and heading
00126   void moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, AngTwoPi dtheta);
00127   //@}
00128   
00129   std::vector<ShapeRoot> getShapes(const ShapeSpace& ShS, int minConf=2) const {
00130     const std::vector<ShapeRoot> allShapes = ShS.allShapes();
00131     if (&ShS == &camShS || &ShS == &groundShS || minConf <= 0) 
00132       return allShapes;
00133     std::vector<ShapeRoot> nonNoiseShapes;
00134     for (std::vector<ShapeRoot>::const_iterator it = allShapes.begin();
00135    it != allShapes.end(); it++)
00136       if ((*it)->getConfidence() >= minConf)
00137   nonNoiseShapes.push_back(*it);
00138     return nonNoiseShapes;
00139   }
00140 
00141   void importLocalToWorld();
00142 
00143   ShapeRoot importWorldToLocal(const ShapeRoot &worldShape);
00144   template<class T> Shape<T> importWorldToLocal(const Shape<T> &worldShape);
00145 
00146 protected:
00147   //!@name Shape extraction functions
00148   //@{
00149   void getCameraShapes(const Sketch<uchar>& camFrame);
00150 
00151   std::vector<Shape<LineData> > 
00152   getCamLines(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00153         const std::set<color_index>& occluderColors) const;
00154 
00155   std::vector<Shape<EllipseData> > 
00156   getCamEllipses(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00157      const std::set<color_index>& occluderColors) const;
00158 
00159   std::vector<Shape<PolygonData> > 
00160   getCamPolygons(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00161      const std::set<color_index>& occluderColors) const;
00162 
00163   std::vector<Shape<LineData> >  
00164   getCamWalls(const Sketch<uchar>&, unsigned int) const;
00165 
00166   std::vector<Shape<SphereData> >  
00167   getCamSpheres(const Sketch<uchar>&, const std::set<color_index>& objectColors, 
00168     const std::set<color_index>& occluderColors) const;
00169 
00170   void getCamBlobs(const Sketch<uchar>& camFrame,
00171        const std::set<color_index>& colors,
00172        const std::map<color_index,int>& minBlobAreas,
00173        const std::map<color_index, BlobData::BlobOrientation_t>& blobOrientations,
00174        const std::map<color_index,coordinate_t>& assumedBlobHeights);
00175   void getCamBlobs();
00176   
00177   std::vector<Shape<TargetData> > 
00178   getCamTargets(const Sketch<uchar> &camFrame, const std::set<color_index>& objectColors,
00179     const std::set<color_index>& occluderColors) const;
00180   //@}
00181 
00182   // matching shapes between two spaces.
00183   static void matchSrcToDst(ShapeSpace &src, ShapeSpace &dst, std::set<color_index> polygonEdgeColors=std::set<color_index>(),
00184           bool mergeSrc=true, bool mergeDst=true);
00185 
00186   //!@name Functions to make requests to the Lookout
00187   //@{
00188   void storeImage() { storeImage(Point(),false); }
00189   void storeImage(const Point& pt, bool havepoint=true);
00190   void grabCameraImageAndGo();
00191   void scanForGazePts();
00192   //@}
00193 
00194   //! define gazePts either virtually or by scan
00195   void defineGazePts();
00196   
00197   void extendLocal(const NEWMAT::Matrix& baseToCam);
00198   void extendWorld(const NEWMAT::Matrix& baseToCam);
00199 
00200   //! decrement confidence of shapes which should have been seen according to the baseToCam matrix
00201   void removeNoise(ShapeSpace&, const NEWMAT::Matrix& baseToCam);
00202   //! erase gaze points which should have been seen according to the baseToCam matrix
00203   void removeGazePts(std::vector<Point>&, const NEWMAT::Matrix& baseToCam);
00204   
00205   //! Returns true if it has set up a valid next gaze point in nextGazePoint
00206   bool determineNextGazePoint();
00207   //! Returns true if there is a shape which needs be looked at again and is reachable; sets it up as nextGazePoint
00208   bool determineNextGazePoint(const std::vector<ShapeRoot>&);
00209   // Returns true if an element of gazePts can be looked at; sets it up as nextGazePoint
00210   bool determineNextGazePoint(std::vector<Point> &gazePts);
00211   //! Starts robot moving to the next gaze point
00212   void moveToNextGazePoint(const bool manualOverride=false);
00213   void doNextSearch();
00214   void doNextSearch2();
00215 
00216   // operations in ground shape space 
00217   bool isBadGazePoint(const Point&) const ;
00218   void projectToGround(const NEWMAT::Matrix& camToBase);
00219   void filterGroundShapes(const NEWMAT::Matrix& baseToCam);
00220 
00221   // calculates ground plane based on ground plane assumption type
00222   void calculateGroundPlane();
00223 
00224 private:
00225   MapBuilder(const MapBuilder&); //!< never call this
00226   MapBuilder& operator=(const MapBuilder&);  //!< never call this
00227 };
00228 
00229 template<class T> Shape<T> MapBuilder::importWorldToLocal(const Shape<T> &worldShape) {
00230   ShapeRoot temp(localShS.importShape(worldShape));
00231   Shape<T> localShape(ShapeRootType(temp,T));
00232   localShape->applyTransform(worldToLocalTranslateMatrix);
00233   localShape->applyTransform(worldToLocalRotateMatrix);
00234   return localShape;
00235 }
00236 
00237 //! Utility function for deleting queues of pointers to Lookout or MapBuilder requests
00238 template<typename T> void deleteAll(std::queue<T*> &q) {
00239   while ( ! q.empty() ) {
00240     delete q.front();
00241     q.pop();
00242   }
00243 }
00244 
00245 } // namespace
00246 
00247 #endif
00248 

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4