grid-map.h
Go to the documentation of this file.
00001 #ifndef __GRID_MAP_H
00002 #define __GRID_MAP_H
00003 
00004 #include <vector>
00005 #include <set>
00006 #include <cassert>
00007 #include <stdexcept>
00008 #include <boost/tuple/tuple.hpp>
00009 #include <Eigen/Eigen>
00010 #include "nav_msgs/OccupancyGrid.h"
00011 
00012 class GridMap
00013 {
00014 public:
00015         typedef signed short Value;
00016         typedef Eigen::Vector2f Vector;
00017         typedef std::set<GridMap*> Group;
00018         
00019         struct MapGroupEmpty:public std::runtime_error
00020         {
00021                 MapGroupEmpty():std::runtime_error("Map group empty, use constructor providing at least resolution and defaultValue.") {}
00022         };
00023         struct MapGroupNotEmpty:public std::runtime_error
00024         {
00025                 MapGroupNotEmpty():std::runtime_error("Map group not empty, use constructor taking only gridMapGroup and defaultValue as argument.") {}
00026         };
00027         struct WrongKnownMap:public std::runtime_error
00028         {
00029                 WrongKnownMap():std::runtime_error("Known map for OccupancyGrid conversion is not in the same map group as the probabilistic map or map group missing.") {}
00030         };
00031 
00032 protected:
00033         typedef std::vector<Value> Values;
00034         
00035         float resolution; // resolution, in unit per grid cell
00036         int startX, startY; // start of map, in grid cell
00037         int width, height; // size of the map, in grid cell
00038         Value defaultValue; // default filling value
00039         Values values; // map data
00040         Group* mapGroup; // resize group, when one particle of the group is resized, all are
00041         mutable unsigned rayCount; // number of rays cast on this grid
00042 
00043 public:
00044         GridMap(Group* gridMapGroup, const Value defaultValue);
00045         GridMap(const float resolution, const Value defaultValue, Group* gridMapGroup = 0);
00046         GridMap(const float resolution, const float startX, const float startY, const float width, const float height, const Value defaultValue, Group* gridMapGroup = 0);
00047         GridMap(const std::string &pgmFileName, const float resolution, const Value defaultValue, Group* gridMapGroup = 0);
00048         
00049         GridMap(const GridMap& that);
00050         GridMap& operator=(const GridMap& that);
00051         
00052         ~GridMap();
00053 
00054 public:
00055         // coordinate transform and bound check
00056         Value& atInternalCoord(const int x, const int y);
00057         Value atInternalCoord(const int x, const int y) const ;
00058         Vector fromInternalCoord(const int x, const int y) const;
00059         Vector fromInternalCoord(const float x, const float y) const;
00060         void toInternalCoord(const Vector& pos, int& internalX, int& internalY) const;
00061         void toInternalCoordSuperSampled(const Vector& pos, const int superSampleRes, int& internalX, int& internalY) const;
00062         static inline Value saturatedValueFromInt(int v) { if (v < -32768) return -32768; else if (v > 32767) return 32767; else return Value(v); }
00063         
00064         // getters
00065         // internal
00066         inline int getInternalStartX() const { return startX; }
00067         inline int getInternalStartY() const { return startY; }
00068         inline int getInternalWidth() const { return width; }
00069         inline int getInternalHeight() const { return height; }
00070         inline float getResolution() const { return resolution; }
00071         // external
00072         inline Vector getMinCoord() const { return fromInternalCoord(0, 0); }
00073         inline Vector getMaxCoord() const { return fromInternalCoord(width-1, height-1); }
00074         inline Vector getSize() const { return getMaxCoord() - getMinCoord(); }
00075         
00076         // values and bound accessors
00078         Value getValueNearest(const Vector& pos) const;
00080         Value getValue(const Vector& pos) const;
00082         Vector getSlope(const Vector& pos, const float limit = 65536.f) const;
00083         bool isWithinBoundsInternal(const int x, const int y) const;
00084         bool isWithinBounds(const Vector& pos) const;
00086         void setNearestValue(const Vector& pos, const Value value);
00088         void addNearestValueSaturated(const Vector& pos, const int delta);
00090         void extendToFit(const Vector& pos);
00091         
00092         // conversion to ROS
00094         nav_msgs::OccupancyGrid toOccupancyGrid(const std::string& frame_id, const GridMap* knownMap = 0) const;
00095         
00096         // local operations on map; safe, extends map when required
00097         template <typename F>
00098         void lineScan(const Vector& start, const Vector& stop, F& functor, const Value& value = 0);
00099         
00100         template <typename F>
00101         void lineScan(const Vector& start, const Vector& stop, F& functor, const Value texture[], const unsigned textureLength);
00102         
00103         // global operations on map
00105         void invert();
00107         void threshold(const Value threshold, const Value lowValue, const Value highValue);
00109         void dilate4(const unsigned amount, const unsigned delta = 0);
00111         void dilate8(const unsigned amount, const unsigned delta = 0);
00113         void erode4(const unsigned amount, const unsigned delta = 0);
00115         void erode8(const unsigned amount, const unsigned delta = 0);
00117         typedef boost::tuple<unsigned, Vector, unsigned> Label;
00119         typedef std::vector<Label> Labels;
00121         Labels labelize(const Value threshold);
00123         typedef std::pair<Vector, Vector> VectorPair;
00125         VectorPair closestPoints(const Label& area0, const Label& area1, const unsigned monteCarloSteps = 100) const;
00127 
00129         GridMap buildGradient(const Vector& goal, bool &isSuccess) const;
00131         template<typename Op>
00132         void unaryOperation(Op& op)
00133         {
00134                 for (Values::iterator it = values.begin(); it != values.end(); ++it)
00135                         op(*it);
00136         }
00138         template<typename Op>
00139         void unaryOperation(const Op& op)
00140         {
00141                 for (Values::iterator it = values.begin(); it != values.end(); ++it)
00142                         op(*it);
00143         }
00145         template<typename Op>
00146         void binaryOperation(const GridMap& that, Op& op)
00147         {
00148                 assert(that.width == width);
00149                 assert(that.height == height);
00150                 for (size_t i = 0; i < values.size(); ++i)
00151                         values[i] = op(values[i], that.values[i]);
00152         }
00154         template<typename Op>
00155         void binaryOperation(const GridMap& that, const Op& op)
00156         {
00157                 assert(that.width == width);
00158                 assert(that.height == height);
00159                 for (size_t i = 0; i < values.size(); ++i)
00160                         values[i] = op(values[i], that.values[i]);
00161         }
00162         
00163         // performance measurement and stats
00164         inline void rayCountReset() { rayCount = 0; }
00165         inline unsigned getRayCount() const { return rayCount; }
00166         float getInformationQuantity() const;
00167         void toPGMFile(const std::string& fileName, const int divisorToPGM = 256) const;
00168 
00169 protected:
00170         void initiateMapGroup();
00171         bool extendMap(int xMin, int yMin, int xMax, int yMax);
00172         void extendMapInternal(int deltaStartX, int deltaStartY, int newWidth, int newHeight);
00173         void dilateN(const unsigned amount, const int lookup[][2], const size_t lookupSize, const unsigned delta);
00174         void erodeN(const unsigned amount, const int lookup[][2], const size_t lookupSize, const unsigned delta);
00175 };
00176 
00177 #endif // __GRID_MAP_H


ethzasl_gridmap_2d
Author(s): Stéphane Magnenat
autogenerated on Mon Oct 6 2014 10:27:33