GridMap.h

Go to the documentation of this file.

/*
 * Copyright (C) by
 *   MetraLabs GmbH (MLAB), GERMANY
 *  and
 *   Neuroinformatics and Cognitive Robotics Labs (NICR) at TU Ilmenau, GERMANY
 * All rights reserved.
 *
 * Redistribution and modification of this code is strictly prohibited.
 *
 * IN NO EVENT SHALL "MLAB" OR "NICR" BE LIABLE TO ANY PARTY FOR DIRECT,
 * INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF
 * THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLAB" OR
 * "NICR" HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" AND "NICR" SPECIFICALLY DISCLAIM ANY WARRANTIES, INCLUDING,
 * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
 * FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
 * ON AN "AS IS" BASIS, AND "MLAB" AND "NICR" HAVE NO OBLIGATION TO
 * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_GRIDMAP_H_
#define _MIRA_GRIDMAP_H_

#include <cmath>
#include <type_traits>

#include <image/Img.h>
#include <geometry/Size.h>
#include <geometry/Rect.h>
#include <serialization/NoGenericReflect.h>
#include <serialization/IsNotMetaSerializable.h>

namespace mira { namespace maps {


template<typename T, int Channels = 1>
class GridMap : public Img<T,Channels>
{
    // the is_pod check is too hard here, but the type needs to be trivial, i.e.
    // there must exist a trivial default constructor, copy constructor and
    // trivial destructor
    // TODO: we can remove this check if arithmetic types are not sufficient,
    // but then have to make sure that the types are "trivial" (see above).
    static_assert(std::is_void<T>::value || std::is_arithmetic<T>::value, "GridMap can be used with arithmetic types only");

    typedef Img<T,Channels> Base;
public:

    typedef typename Base::Pixel CellType;

public:

    explicit GridMap(float cellSize=0.1f) : mCellSize(cellSize), mOffset(0, 0) {}

    GridMap(const Size2i& size, float cellSize, const Point2i& offset = Point2i(0, 0)) :
        Base(size), mCellSize(cellSize), mOffset(offset) {}

    GridMap(const Rect2f& region, float cellSize) :
        Base(getOffsetAndSize(region,cellSize).second),
        mCellSize(cellSize),
        mOffset(getOffsetAndSize(region,cellSize).first)
    {}


    GridMap(const Rect2i& region, float cellSize) :
        Base(region.size()),
        mCellSize(cellSize),
        mOffset(-region.minCorner)
    {}

    GridMap(const Base& data, float cellSize, const Point2i& offset = Point2i(0, 0)) :
        Base(data), mCellSize(cellSize), mOffset(offset) {}

    // use default copy constructor and assignment operator

public:

    float getCellSize() const { return mCellSize; }


    Point2i getOffset() const { return mOffset; }

    Point2i getMapOffset() const { return mOffset; }

    Point2f getWorldOffset() const {
        return Point2f(mOffset.x()*mCellSize, mOffset.y()*mCellSize);
    }


public:

    GridMap& operator=(const CellType& c) {
        Base::operator=(c);
        return *this;
    }

    GridMap clone() const {
        return GridMap(Base::clone(), mCellSize, mOffset);
    }

public:

    Rect2f getRegion() const {
        return Rect2f(-mOffset.x()*mCellSize, -mOffset.y()*mCellSize,
                       this->width()*mCellSize, this->height()*mCellSize);
    }


    Rect2i getMapRegion() const {
        return Rect2i(-mOffset.x(), -mOffset.y(),
                       this->width(), this->height());
    }

public:

    void grow(const Rect2f& region, const CellType& valueForNewCells);

    void grow(const Rect2i& region, const CellType& valueForNewCells);

    void grow(const Point2i& growLowerLeftDelta, const Point2i& growUpperRightDelta,
              const CellType& valueForNewCells);

    void clip(const Rect2i& region, const CellType& valueForNewCells);

    void clip(const Rect2f& region, const CellType& valueForNewCells);

public:

    Point2i world2map(const Point2f& p, bool roundDown = true) const {
        if (roundDown)
            return Point2i((int)std::floor(p.x()/mCellSize + mOffset.x()),
                           (int)std::floor(p.y()/mCellSize + mOffset.y()));
        else
            return Point2i((int)std::ceil(p.x()/mCellSize + mOffset.x()),
                           (int)std::ceil(p.y()/mCellSize + mOffset.y()));
    }

    Point2f world2mapf(const Point2f& p) const {
        return Point2f(p.x()/mCellSize + mOffset.x(),
                        p.y()/mCellSize + mOffset.y());
    }

    Point2f map2world(const Point2i& p) const {
        return Point2f((p.x() - mOffset.x()) * mCellSize,
                        (p.y() - mOffset.y()) * mCellSize);
    }

    Point2f map2world(const Point2f& p) const {
        return Point2f((p.x() - mOffset.x()) * mCellSize,
                        (p.y() - mOffset.y()) * mCellSize);
    }

    Rect2i world2map(const Rect2f& r, bool includeBorder = true) const {
        if(includeBorder) {
            Point2i lowerLeft (std::floor(r.x0() / mCellSize + mOffset.x()),
                               std::floor(r.y0() / mCellSize + mOffset.y()));
            Point2i upperRight(std::ceil(r.x1()  / mCellSize + mOffset.x()),
                               std::ceil(r.y1()  / mCellSize + mOffset.y()));
            return Rect2i(lowerLeft, upperRight);
        } else {
            Point2i lowerLeft (std::ceil(r.x0()  / mCellSize + mOffset.x()),
                               std::ceil(r.y0()  / mCellSize + mOffset.y()));
            Point2i upperRight(std::floor(r.x1() / mCellSize + mOffset.x()),
                               std::floor(r.y1() / mCellSize + mOffset.y()));
            return Rect2i(lowerLeft, upperRight);
        }
    }


    MIRA_NO_GENERIC_REFLECT_MEMBER(GridMap<T>)

    
    template<typename Derived>
    void reflect(BinarySerializer<Derived>& r)  {
        MIRA_REFLECT_BASE(r, Base);
        r.member("CellSize", mCellSize, "The cellsize in meter");
        r.member("Offset"  , mOffset,   "The cell index that corresponds to the origin of the map");
    }

    template<typename Derived>
    void reflect(BinaryDeserializer<Derived>& r) {
        MIRA_REFLECT_BASE(r, Base);
        r.member("CellSize", mCellSize, "The cellsize in meter");
        r.member("Offset"  , mOffset,   "The cell index that corresponds to the origin of the map");
    }

    void reflect(JSONSerializer& r) {
        MIRA_REFLECT_BASE(r, Base);
        r.member("CellSize", mCellSize, "The cellsize in meter");
        r.member("Offset"  , mOffset,   "The cell index that corresponds to the origin of the map");
    }

private:

    static std::pair<Point2i, Size2i> getOffsetAndSize(const Rect2f& region, float cellSize);

protected:
    float   mCellSize; 
    Point2i mOffset;   
};


template<typename T, int Channels>
inline void GridMap<T,Channels>::grow(const Rect2f& region, const CellType& valueForNewCells)
{
    // if our map already contains the specified region, do nothing
    if(getRegion().x0()<=region.x0() &&
       getRegion().y0()<=region.y0() &&
       getRegion().x1()>=region.x1() &&
       getRegion().y1()>=region.y1())
        return;

    // get the overall region
    Rect2f o = region | getRegion();
    Size2i oldSize = this->size();

    auto offsetSize = getOffsetAndSize(o,mCellSize);

    Point2i growLowerLeft  = offsetSize.first - mOffset;
    Point2i growUpperRight = offsetSize.second - oldSize - growLowerLeft;

    assert(growLowerLeft.x()>=0 && growLowerLeft.y()>=0);
    assert(growUpperRight.x()>=0 && growUpperRight.y()>=0);
    grow(growLowerLeft,growUpperRight, valueForNewCells);
}

template<typename T, int Channels>
inline void GridMap<T,Channels>::grow(const Rect2i& region, const CellType& valueForNewCells)
{
    // if our map already contains the specified region, do nothing
    if(getMapRegion().contains(region))
        return;

    // get the overall region
    const auto o = region | getMapRegion();

    const auto newOffset = -o.minCorner;
    const auto newSize = o.size();

    const Size2i oldSize = this->size();
    const Point2i growLowerLeft  = newOffset - mOffset;
    const Point2i growUpperRight = newSize - oldSize - growLowerLeft;

    assert(growLowerLeft.x()>=0 && growLowerLeft.y()>=0);
    assert(growUpperRight.x()>=0 && growUpperRight.y()>=0);
    grow(growLowerLeft,growUpperRight, valueForNewCells);
}

template<typename T, int Channels>
inline void GridMap<T,Channels>::grow(const Point2i& growLowerLeft, const Point2i& growUpperRight,
                                           const CellType& valueForNewCells)
{
    assert(growLowerLeft.x()>=0 && growLowerLeft.y()>=0);
    assert(growUpperRight.x()>=0 && growUpperRight.y()>=0);

    Size2i oldSize = this->size();
    Size2i newSize = oldSize + growLowerLeft + growUpperRight;

    Base& oldBuffer = *this;
    Base newBuffer(newSize);

    // copy the existing data into the new buffer (scanline by scanline)
    for(int y=0; y<oldBuffer.height(); ++y)
    {
        const CellType* src = oldBuffer[y];
        CellType* dest      = newBuffer[y+growLowerLeft.y()];
        memcpy(dest+growLowerLeft.x(), src, oldBuffer.width()*sizeof(CellType));
    }

    // now fill the new areas with the default data:

    //  newBuffer:
    //  ------------------------------
    //  |           above            |
    //  |----------------------------|
    //  |    |              |        |
    //  |    |  oldBuffer   |        |
    //  |left|              | right  |
    //  |    |              |        |
    //  |    |              |        |
    //  |----------------------------|
    //  |          below             |
    //  |----------------------------|

    // above
    for(int y=0; y<growLowerLeft.y(); ++y)
    {
        CellType* dest = newBuffer[y];
        for(int x=0; x<newBuffer.width(); ++x)
            dest[x] = valueForNewCells;
    }

    // left and right
    for(int y=growLowerLeft.y(); y<growLowerLeft.y()+oldBuffer.height(); ++y)
    {
        CellType* dest = newBuffer[y];
        // left
        for(int x=0; x<growLowerLeft.x(); ++x)
            dest[x] = valueForNewCells;
        // right
        for(int x=growLowerLeft.x()+oldBuffer.width(); x<newBuffer.width(); ++x)
            dest[x] = valueForNewCells;
    }

    // below
    for(int y=growLowerLeft.y()+oldBuffer.height(); y<newBuffer.height(); ++y)
    {
        CellType* dest = newBuffer[y];
        for(int x=0; x<newBuffer.width(); ++x)
            dest[x] = valueForNewCells;
    }


    oldBuffer = newBuffer;
    mOffset = mOffset + growLowerLeft;
}

template<typename T, int Channels>
inline void GridMap<T,Channels>::clip(const Rect2f& region, const CellType& valueForNewCells)
{
    Point2i lowerLeft (std::floor(region.x0() / mCellSize),
                       std::floor(region.y0() / mCellSize));
    Point2i upperRight(std::ceil(region.x1()  / mCellSize),
                       std::ceil(region.y1()  / mCellSize));
    Rect2i r(lowerLeft, upperRight);

    clip(r,valueForNewCells);
}

template<typename T, int Channels>
inline void GridMap<T,Channels>::clip(const Rect2i& region, const CellType& valueForNewCells)
{
    Rect2i oldRegion = getMapRegion();
    if(region==oldRegion)
        return; // nothing to do

    //  newBuffer:
    //  ------------
    //  |  new     :
    //  |    ------:-----------------|
    //  |    | copy:                 |
    //  |....|.....:    oldBuffer    |
    //       |                       |
    //       |                       |
    //       -------------------------


    Point2i growLowerLeft  = oldRegion.minCorner - region.minCorner;
    Point2i growUpperRight = region.maxCorner - oldRegion.maxCorner;

    Point2i copyRegionInNewLL(0,0);
    Point2i copyRegionInOldLL(0,0);
    if(oldRegion.minCorner.x() > region.minCorner.x())
        copyRegionInNewLL.x() = oldRegion.minCorner.x() - region.minCorner.x();
    else
        copyRegionInOldLL.x() = region.minCorner.x() - oldRegion.minCorner.x();

    if(oldRegion.minCorner.y() > region.minCorner.y())
        copyRegionInNewLL.y() = oldRegion.minCorner.y() - region.minCorner.y();
    else
        copyRegionInOldLL.y() = region.minCorner.y() - oldRegion.minCorner.y();


    Rect2i intersection = oldRegion & region;

    Base& oldBuffer = *this;
    Base newBuffer(region.size());

    if(intersection.isValid()) {

        Point2i copyRegionInNewUR = copyRegionInNewLL + intersection.size();
        Point2i copyRegionInOldUR = copyRegionInOldLL + intersection.size();

        //  detailed view for copying:
        //  ------------------------------
        //  |           above            |
        //  |----------------------------|
        //  |    |              |        |
        //  |    |  oldBuffer   |        |
        //  |left|              | right  |
        //  |    |              |        |
        //  |    |              |        |
        //  |----------------------------|
        //  |          below             |
        //  |----------------------------|


        // fill in new parts
        // above
        assert(copyRegionInNewLL.y()<=newBuffer.height());
        for(int y=0; y<copyRegionInNewLL.y(); ++y)
        {
            CellType* dest = newBuffer[y];
            for(int x=0; x<newBuffer.width(); ++x)
                dest[x] = valueForNewCells;
        }

        // left and right
        assert(copyRegionInNewLL.y()>=0 && copyRegionInNewUR.y()<=newBuffer.height());
        assert(copyRegionInNewUR.x()>=0 && copyRegionInNewLL.x()<=newBuffer.width());
        for(int y=copyRegionInNewLL.y(); y<copyRegionInNewUR.y(); ++y)
        {
            CellType* dest = newBuffer[y];
            // left
            for(int x=0; x<copyRegionInNewLL.x(); ++x)
                dest[x] = valueForNewCells;
            // right
            for(int x=copyRegionInNewUR.x(); x<newBuffer.width(); ++x)
                dest[x] = valueForNewCells;
        }

        // below
        assert(copyRegionInNewUR.y()>=0);
        for(int y=copyRegionInNewUR.y(); y<newBuffer.height(); ++y)
        {
            CellType* dest = newBuffer[y];
            for(int x=0; x<newBuffer.width(); ++x)
                dest[x] = valueForNewCells;
        }

        // copy the overlapping content (scanline by scanline)
        for(int y=0; y<intersection.height(); ++y)
        {
            const CellType* src = oldBuffer[y+copyRegionInOldLL.y()]+copyRegionInOldLL.x();
            CellType* dest      = newBuffer[y+copyRegionInNewLL.y()]+copyRegionInNewLL.x();
            memcpy(dest, src, intersection.width()*sizeof(CellType));
        }
    } else {
        // no intersection between old and new buffer, so simply fill the
        // new buffer
        newBuffer = valueForNewCells;
    }

    mOffset = -region.minCorner;
    oldBuffer = newBuffer;

}

template<typename T, int Channels>
inline std::pair<Point2i, Size2i> GridMap<T,Channels>::getOffsetAndSize(const Rect2f& region, float cellSize)
{
    Point2i lowerLeft (std::floor(region.x0() / cellSize),
                       std::floor(region.y0() / cellSize));
    Point2i upperRight(std::ceil(region.x1()  / cellSize),
                       std::ceil(region.y1()  / cellSize));

    // compute required size and offset of the map
    Size2i  size = upperRight - lowerLeft;// + Size2i(1,1);
    Point2i offset = -lowerLeft;
    return std::make_pair(offset,size);
}


} // namespace maps

template <typename TPixel, int TChannels>
class IsNotMetaSerializable<maps::GridMap<TPixel, TChannels>> : public std::true_type {};


} // namespace mira

#endif /* _MIRA_GRIDMAP_H_ */