grid-map.h

Go to the documentation of this file.

#ifndef __GRID_MAP_H
#define __GRID_MAP_H

#include <vector>
#include <set>
#include <cassert>
#include <stdexcept>
#include <boost/tuple/tuple.hpp>
#include <Eigen/Eigen>
#include "nav_msgs/OccupancyGrid.h"

class GridMap
{
public:
        typedef signed short Value;
        typedef Eigen::Vector2f Vector;
        typedef std::set<GridMap*> Group;
        
        struct MapGroupEmpty:public std::runtime_error
        {
                MapGroupEmpty():std::runtime_error("Map group empty, use constructor providing at least resolution and defaultValue.") {}
        };
        struct MapGroupNotEmpty:public std::runtime_error
        {
                MapGroupNotEmpty():std::runtime_error("Map group not empty, use constructor taking only gridMapGroup and defaultValue as argument.") {}
        };
        struct WrongKnownMap:public std::runtime_error
        {
                WrongKnownMap():std::runtime_error("Known map for OccupancyGrid conversion is not in the same map group as the probabilistic map or map group missing.") {}
        };

protected:
        typedef std::vector<Value> Values;
        
        float resolution; // resolution, in unit per grid cell
        int startX, startY; // start of map, in grid cell
        int width, height; // size of the map, in grid cell
        Value defaultValue; // default filling value
        Values values; // map data
        Group* mapGroup; // resize group, when one particle of the group is resized, all are
        mutable unsigned rayCount; // number of rays cast on this grid

public:
        GridMap(Group* gridMapGroup, const Value defaultValue);
        GridMap(const float resolution, const Value defaultValue, Group* gridMapGroup = 0);
        GridMap(const float resolution, const float startX, const float startY, const float width, const float height, const Value defaultValue, Group* gridMapGroup = 0);
        GridMap(const std::string &pgmFileName, const float resolution, const Value defaultValue, Group* gridMapGroup = 0);
        
        GridMap(const GridMap& that);
        GridMap& operator=(const GridMap& that);
        
        ~GridMap();

public:
        // coordinate transform and bound check
        Value& atInternalCoord(const int x, const int y);
        Value atInternalCoord(const int x, const int y) const ;
        Vector fromInternalCoord(const int x, const int y) const;
        Vector fromInternalCoord(const float x, const float y) const;
        void toInternalCoord(const Vector& pos, int& internalX, int& internalY) const;
        void toInternalCoordSuperSampled(const Vector& pos, const int superSampleRes, int& internalX, int& internalY) const;
        static inline Value saturatedValueFromInt(int v) { if (v < -32768) return -32768; else if (v > 32767) return 32767; else return Value(v); }
        
        // getters
        // internal
        inline int getInternalStartX() const { return startX; }
        inline int getInternalStartY() const { return startY; }
        inline int getInternalWidth() const { return width; }
        inline int getInternalHeight() const { return height; }
        inline float getResolution() const { return resolution; }
        // external
        inline Vector getMinCoord() const { return fromInternalCoord(0, 0); }
        inline Vector getMaxCoord() const { return fromInternalCoord(width-1, height-1); }
        inline Vector getSize() const { return getMaxCoord() - getMinCoord(); }
        
        // values and bound accessors
        Value getValueNearest(const Vector& pos) const;
        Value getValue(const Vector& pos) const;
        Vector getSlope(const Vector& pos, const float limit = 65536.f) const;
        bool isWithinBoundsInternal(const int x, const int y) const;
        bool isWithinBounds(const Vector& pos) const;
        void setNearestValue(const Vector& pos, const Value value);
        void addNearestValueSaturated(const Vector& pos, const int delta);
        void extendToFit(const Vector& pos);
        
        // conversion to ROS
        nav_msgs::OccupancyGrid toOccupancyGrid(const std::string& frame_id, const GridMap* knownMap = 0) const;
        
        // local operations on map; safe, extends map when required
        template <typename F>
        void lineScan(const Vector& start, const Vector& stop, F& functor, const Value& value = 0);
        
        template <typename F>
        void lineScan(const Vector& start, const Vector& stop, F& functor, const Value texture[], const unsigned textureLength);
        
        // global operations on map
        void invert();
        void threshold(const Value threshold, const Value lowValue, const Value highValue);
        void dilate4(const unsigned amount, const unsigned delta = 0);
        void dilate8(const unsigned amount, const unsigned delta = 0);
        void erode4(const unsigned amount, const unsigned delta = 0);
        void erode8(const unsigned amount, const unsigned delta = 0);
        typedef boost::tuple<unsigned, Vector, unsigned> Label;
        typedef std::vector<Label> Labels;
        Labels labelize(const Value threshold);
        typedef std::pair<Vector, Vector> VectorPair;
        VectorPair closestPoints(const Label& area0, const Label& area1, const unsigned monteCarloSteps = 100) const;

        GridMap buildGradient(const Vector& goal, bool &isSuccess) const;
        template<typename Op>
        void unaryOperation(Op& op)
        {
                for (Values::iterator it = values.begin(); it != values.end(); ++it)
                        op(*it);
        }
        template<typename Op>
        void unaryOperation(const Op& op)
        {
                for (Values::iterator it = values.begin(); it != values.end(); ++it)
                        op(*it);
        }
        template<typename Op>
        void binaryOperation(const GridMap& that, Op& op)
        {
                assert(that.width == width);
                assert(that.height == height);
                for (size_t i = 0; i < values.size(); ++i)
                        values[i] = op(values[i], that.values[i]);
        }
        template<typename Op>
        void binaryOperation(const GridMap& that, const Op& op)
        {
                assert(that.width == width);
                assert(that.height == height);
                for (size_t i = 0; i < values.size(); ++i)
                        values[i] = op(values[i], that.values[i]);
        }
        
        // performance measurement and stats
        inline void rayCountReset() { rayCount = 0; }
        inline unsigned getRayCount() const { return rayCount; }
        float getInformationQuantity() const;
        void toPGMFile(const std::string& fileName, const int divisorToPGM = 256) const;

protected:
        void initiateMapGroup();
        bool extendMap(int xMin, int yMin, int xMax, int yMax);
        void extendMapInternal(int deltaStartX, int deltaStartY, int newWidth, int newHeight);
        void dilateN(const unsigned amount, const int lookup[][2], const size_t lookupSize, const unsigned delta);
        void erodeN(const unsigned amount, const int lookup[][2], const size_t lookupSize, const unsigned delta);
};

#endif // __GRID_MAP_H