Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

MapBuilder.h Go to the documentation of this file. //-*-c++-*- #ifndef _MapBuilder_h_ #define _MapBuilder_h_ #include <queue> #include "Behaviors/BehaviorBase.h" #include "Shared/newmat/newmat.h" #include "BlobData.h" #include "LineData.h" #include "Point.h" #include "VRmixin.h" #include "LookoutRequests.h" #include "MapBuilderRequests.h" #include "SketchTypes.h" #include "ShapeSpace.h" namespace DualCoding { class SketchSpace; class ScanRequest; class MapBuilder : public BehaviorBase { protected: SketchSpace &camSkS; ShapeSpace &camShS, &groundShS; struct maps { enum Space { local, world } space; SketchSpace &SkS; ShapeSpace &ShS; vector<Point> gazePts; vector<NEWMAT::Matrix> baseToCamMats; maps(Space _space, SketchSpace& _SkS) : space(_space), SkS(_SkS), ShS(_SkS.getDualSpace()), gazePts(), baseToCamMats() {} } local, world, *cur; public: const int xres, yres; //!< width and height of camera frame NEWMAT::ColumnVector ground_plane; //!< ground plane to which shapes are projected protected: Shape<AgentData> &theAgent; //!< agent in the world frame //! trasformation matrices between local and world frames. NEWMAT::Matrix localToWorldMatrix, worldToLocalTranslateMatrix, worldToLocalRotateMatrix; vector<Point> badGazePoints; //!< gaze points for which HeadPointerMC.lookAtPoint() returned false bool agentAtOrigin; //!< whether or not agent is at origin and orientation is zero or two pi. queue<MapBuilderRequest*> requests; MapBuilderRequest *curReq; unsigned int idCounter; unsigned int maxDistSq; //!< sqrt of current request's max distance parameter unsigned int pointAtID, scanID; //!< ID's for lookout requests Point nextGazePoint; //! posts completion event and deletes current request, executes next request if there is one void requestComplete(); //! triggers action to execute the front one in requests queue void executeRequest(); //! calls exitTest of current request if there is one and returns the result bool requestExitTest(); public: MapBuilder(); //!< Constructor virtual ~MapBuilder() {} //!< Destructor virtual void DoStart(); virtual void DoStop(); virtual void processEvent(const EventBase&); virtual std::string getDescription() const { return "MapBuilder"; } void printShS(ShapeSpace&) const; unsigned int executeRequest(const MapBuilderRequest& req); void processImage(const Sketch<uchar>&, const NEWMAT::Matrix& camToBase, const NEWMAT::Matrix& baseToCam); // returns if a ground shape should be seen in the current camera frame static bool isPointVisible(const Point &pt, const NEWMAT::Matrix& baseToCam, float maxDistanceSq) ; static bool isLineVisible(const LineData& ln, const NEWMAT::Matrix& baseToCam); static bool isShapeVisible(const ShapeRoot &ground_shape, const NEWMAT::Matrix& baseToCam, float maxDistanceSq); //!@ utility functions which may be used by MapBuilderRequests' exit condition and others const Shape<AgentData>& getAgent() const { return theAgent; } // sets the agent location and heading void setAgent(const Point &location, const AngTwoPi heading); // updates the agent location and heading void moveAgent(coordinate_t const local_dx, coordinate_t const local_dy, AngTwoPi dtheta); vector<ShapeRoot> getShapes(const ShapeSpace& ShS, int minConf=2) const { const vector<ShapeRoot> allShapes = ShS.allShapes(); if (&ShS == &camShS || &ShS == &groundShS || minConf <= 0) return allShapes; vector<ShapeRoot> nonNoiseShapes; for (vector<ShapeRoot>::const_iterator it = allShapes.begin(); it != allShapes.end(); it++) if ((*it)->getConfidence() >= minConf) nonNoiseShapes.push_back(*it); return nonNoiseShapes; } const vector<Point>& getGazePts() const { return cur->gazePts; } static bool returnTrue() { return true; } static bool returnFalse() { return false; } //}@ //!@ Shape extraction functions vector<Shape<LineData> > getCamLines(const Sketch<uchar>&, const set<int>& objectColors, const set<int>& occluderColors) const ; vector<Shape<PolygonData> > getCamPolygons(const Sketch<uchar>&, const set<int>& objectColors, const set<int>& occluderColors) const ; vector<Shape<EllipseData> > getCamEllipses(const Sketch<uchar>&, const set<int>& objectColors, const set<int>& occluderColors) const ; vector<Shape<SphereData> > getCamSpheres(const Sketch<uchar>&, const set<int>& objectColors, const set<int>& occluderColors) const ; vector<Shape<LineData> > getCamWalls(const Sketch<uchar>&, unsigned int) const ; vector<Shape<BlobData> > getCamBlobs(const Sketch<uchar>& sketch, const set<int>& objectColors) const { return BlobData::extractBlobs(sketch, objectColors); } //}@ static Shape<BlobData> formBoundingBox(coordinate_t left, coordinate_t right, coordinate_t front, coordinate_t rear) { return Shape<BlobData> (new BlobData(VRmixin::groundShS, Point(front,left), Point(front,right), Point(rear,right), Point(rear,left), fabs((left-right)*(front-rear)), vector<BlobData::run>(), BlobData::groundplane, rgb())); } void importLocalToWorld(); void importWorldToLocal(const ShapeRoot &shape); // matching shapes between two spaces. static void matchSrcToDst(ShapeSpace &src, ShapeSpace &dst, set<int> polygonEdgeColors=set<int>(), bool mergeSrc=true, bool mergeDst=true); protected: //! functions to make requests to lookout void scan(ScanRequest& req); void storeImageAt(const Point& pt); void storeImage(); //! define gazePts either virtually or by scan void defineGazePts(vector<Point>&, const ShapeRoot& area, bool doScan);// {} void defineGazePts(vector<Point>& gazePts, const vector<Point>& corners, float meshSize); void getCameraShapes(const Sketch<uchar>& camFrame); void getCamBlobs(); void extendLocal(const NEWMAT::Matrix& baseToCam); void extendWorld(const NEWMAT::Matrix& baseToCam); //! decrement confidence of shapes which should have been seen according to the baseToCam matrix void removeNoise(ShapeSpace&, const NEWMAT::Matrix& baseToCam); //! erase gaze points which should have been seen according to the baseToCam matrix void removeGazePts(vector<Point>&, const NEWMAT::Matrix& baseToCam); //! Returns true if it has set up a valid next gaze point in nextGazePoint bool determineNextGazePoint(); //! Returns true if there is a shape which needs be looked at again and is reachable; sets it up as nextGazePoint bool determineNextGazePoint(const vector<ShapeRoot>&); // Returns true if an element of gazePt can be looked at; sets it up as nextGazePoint bool determineNextGazePoint(vector<Point> &gazePts); //! Starts robot moving to the next gaze point void moveToNextGazePoint(const bool manualOverride=false); // operations in ground shape space bool isBadGazePoint(const Point&) const ; void projectToGround(const NEWMAT::Matrix& camToBase); void filterGroundShapes(const NEWMAT::Matrix& baseToCam); // calculates ground place based on ground plane assumption type void calculateGroundPlane(const MapBuilderRequest::GroundPlaneAssumption_t& gpa); private: MapBuilder(const MapBuilder&); //!< never call this MapBuilder& operator=(const MapBuilder&); //!< never call this }; /* Functions of the MapBuilder subsystem: 1. Given a series of camera views from the same body position (but varying head positions), assemble them into a local world view. This requires: a) Matching functions to establish correspondence between groundspace objects and local worldspace objects. This is tricky when lines have missing endpoints, or ellipses lie partially offscreen. Matching algorithm: do ellipses first -- they're simpler: just match color, center point Line matching: pick a starting line (longest first) in groundShS then find all colinear line segments in groundShS matching its rho/theta do the same in localworldShS now a smart algorithm can assemble line segments into a more complex match than just 1-1 but for starters can code just a 1-1 match Find nearby ellipses in ground and world space and cluster them together before matching. This will allow us to handle piles of free game pieces. Make a list of ambiguous points that we would like to examine further by taking another image, moving the head and maybe even the body. Idea: we have to deal with lots of ambiguity in matching camera frames to the local world map, and may want to confirm some things with additional camera frames if we suspect noise. But the world map should be considered to be reliable so we shouldn't need the same kind of expensive tentative matching there. However, we may want to allow for "hints" of things to be put in the world map if we're not certain of our data. For example, a far off ellipse may be of indeterminate size and orientation, so put it in the world map as a hint-of-ellipse and let the robot wander over there and get a close up view if it wants to resolve whether this is a real ellipse or noise. Can use distance from the camera as a parameter to control how we treat an object; small, far away objects should be treated as less reliable. Can keep counts on how many frames an object was seen or not seen, and delete objects as spurious if they were only seen once or twice and then not seen in a while. Unmatched object: to know that a mobile object has disappeared, we need to recognize that we should see it but we don't. How? For each camera frame, project the four corners to ground, yielding a trapezoid. As we build the local world map we have a collection of trapezoids. For any object in the global map that isn't matched by something in the local, we can check its coordinates to see if it falls within one of the trapizeoids. If so, then we looked and didn't see it, so it's gone. b) Update functions that update local worldspace object descriptions once correspondences have been determined. 2. Given a local worldspace map and a global worldspace map, and possibly an estimate of current position and orientation on the latter, determine the robot's true position and orientation in the world, and update the global worldspace map. This requires: a) A search method to find the best translation and rotation of the local worldspace map to fit the global worldspace map. A particle filter looks like a good candidate method. b) Update functions that update the global worldspace description once correspondences have been determined. c) A mechanism for recognizing when the world has changed, e.g., objects have been added, deleted, or moved. This requires: i) Designation of objects as fixed or mobile ii) Declaration of objects as unique, of a fixed number, or multitudinous. Unique fixed objects are the best landmarks. Multitudinous mobile objects may need to be monitored more frequently. 3. Given a request to self-localize, acquire the minimum necessary number of local views to perform that function. This requires: a) Designation of objects to be used as beacons or key landmarks. b) Head motion constraints. c) Body motion constraints (possibly "no body motion allowed".) Ideally, rather than directly executing motion commands, the system should pass its requests to look in certain directions to the affordance generator, which will figure out what is feasible, e.g., if we're up against a wall and can't turn in that direction, we may have to take a step sideways first. Let the affordance system worry abou that. 4. Given a prototype global world map description, visually explore the world and instantiate the prototype as an actual global world map. For example, we might define a tic-tac-toe environment as containing two sets of perpendicular lines defining the game board, plus a pool of free pieces. We may not know the exact lengths of the lines, or their orientation, or the location of the pool relative to the board; these need to be determined by observation. We may not even know the color of the lines, in which case we'll have to look for a contrast difference, but that is too advanced to worry about at present. */ } // namespace #endif

DualCoding 3.0beta

Generated Wed Oct 4 00:01:53 2006 by Doxygen 1.4.7