00001
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
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;
00043
00044 PlaneEquation ground_plane;
00045
00046 static bool retain;
00047
00048 public:
00049
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;
00083 friend class Pilot;
00084 friend class LineData;
00085 friend class EllipseData;
00086
00087 Shape<AgentData> &theAgent;
00088
00089
00090 public:
00091 fmat::Transform localToWorldMatrix, worldToLocalMatrix;
00092
00093
00094 protected:
00095 std::vector<Point> badGazePoints;
00096
00097 std::queue<MapBuilderRequest*> requests;
00098 MapBuilderRequest *curReq;
00099 unsigned int idCounter;
00100 float maxDistSq;
00101 std::map<string,SiftTekkotsu*> siftMatchers;
00102
00103 unsigned int pointAtID, scanID;
00104 Point nextGazePoint;
00105
00106
00107 void executeRequest();
00108
00109 bool requestExitTest();
00110
00111 void requestComplete();
00112
00113 #ifdef TGT_IS_AIBO
00114 public:
00115 #endif
00116
00117 void setAgent(const Point &worldLocation, const AngTwoPi worldHeading, bool quiet=false);
00118
00119 public:
00120 MapBuilder();
00121 virtual ~MapBuilder() {}
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);
00127 void executeRequest(BehaviorBase* requestingBehavior, const MapBuilderRequest &req);
00128
00129 MapBuilderRequest* getCurrentRequest() { return curReq; }
00130
00131 virtual void doEvent();
00132 void processImage(const LookoutSketchEvent&);
00133
00134
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
00141
00142 const Shape<AgentData>& getAgent() const { return theAgent; }
00143
00144
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
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:
00236
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
00241
00242 void storeImage(bool useNextGazePoint);
00243 void grabCameraImageAndGo();
00244 void scanForGazePts();
00245
00246
00247
00248 void setInitialGazePts();
00249
00250 void extendLocal(const fmat::Transform& baseToCam);
00251 void extendWorld(const fmat::Transform& baseToCam);
00252
00253
00254 void removeNoise(ShapeSpace&, const fmat::Transform& baseToCam);
00255
00256 void removeGazePts(std::vector<GazePoint>&, const fmat::Transform& baseToCam);
00257
00258
00259 bool determineNextGazePoint();
00260
00261 bool determineNextGazePoint(const std::vector<ShapeRoot>&);
00262
00263 bool determineNextGazePoint(std::vector<GazePoint> &gazePts);
00264
00265 void moveToNextGazePoint(const bool manualOverride=false);
00266 void doNextSearch();
00267 void doNextSearch2();
00268
00269
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
00276 void calculateGroundPlane();
00277
00278 private:
00279 MapBuilder(const MapBuilder&);
00280 MapBuilder& operator=(const MapBuilder&);
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
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 }
00299
00300 #endif
00301