00001
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;
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;
00040
00041 NEWMAT::ColumnVector ground_plane;
00042
00043 static bool retain;
00044
00045 public:
00046
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;
00079
00080
00081 NEWMAT::Matrix localToWorldMatrix, worldToLocalTranslateMatrix, worldToLocalRotateMatrix;
00082
00083
00084 std::vector<Point> badGazePoints;
00085
00086 std::queue<MapBuilderRequest*> requests;
00087 MapBuilderRequest *curReq;
00088 unsigned int idCounter;
00089
00090 float maxDistSq;
00091 unsigned int pointAtID, scanID;
00092 Point nextGazePoint;
00093
00094 void executeRequest();
00095
00096 bool requestExitTest();
00097
00098 void requestComplete();
00099
00100 public:
00101 MapBuilder();
00102 virtual ~MapBuilder() {}
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);
00108
00109 virtual void processEvent(const EventBase&);
00110 void processImage(const LookoutSketchEvent&);
00111
00112
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
00119
00120 const Shape<AgentData>& getAgent() const { return theAgent; }
00121
00122
00123 void setAgent(const Point &worldLocation, const AngTwoPi worldHeading);
00124
00125
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
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
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
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
00195 void defineGazePts();
00196
00197 void extendLocal(const NEWMAT::Matrix& baseToCam);
00198 void extendWorld(const NEWMAT::Matrix& baseToCam);
00199
00200
00201 void removeNoise(ShapeSpace&, const NEWMAT::Matrix& baseToCam);
00202
00203 void removeGazePts(std::vector<Point>&, const NEWMAT::Matrix& baseToCam);
00204
00205
00206 bool determineNextGazePoint();
00207
00208 bool determineNextGazePoint(const std::vector<ShapeRoot>&);
00209
00210 bool determineNextGazePoint(std::vector<Point> &gazePts);
00211
00212 void moveToNextGazePoint(const bool manualOverride=false);
00213 void doNextSearch();
00214 void doNextSearch2();
00215
00216
00217 bool isBadGazePoint(const Point&) const ;
00218 void projectToGround(const NEWMAT::Matrix& camToBase);
00219 void filterGroundShapes(const NEWMAT::Matrix& baseToCam);
00220
00221
00222 void calculateGroundPlane();
00223
00224 private:
00225 MapBuilder(const MapBuilder&);
00226 MapBuilder& operator=(const MapBuilder&);
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
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 }
00246
00247 #endif
00248