VoxelMap.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_VOXELMAP_H_
#define _MIRA_VOXELMAP_H_

#include <cmath>
#include <type_traits>

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

namespace mira { namespace maps {


template <typename T>
class VoxelMap
{
public:
    // the is_pod check is too hard here, since user defined constructors
    // are allowed, as long there is a trivial default constructor,
    // copy constructor and trivial destructor
    static_assert(std::has_trivial_destructor<T>::value,
            "Can be used with types that have trivial destructor only");

#ifndef MIRA_WINDOWS
    // VC++2010 does not yet support the "Explicitly-defaulted and deleted
    // special member functions"-syntax, therefore we must skip these checks
    static_assert(std::has_trivial_default_constructor<T>::value,
            "Can be used with types that have a trivial default constructor only");
    static_assert(std::has_trivial_copy_constructor<T>::value,
            "Can be used with types that have a trivial copy constructor only");
#endif

public:

    typedef typename boost::multi_array<T,3> Array;
    typedef typename boost::multi_array::extent_range Range;

private:

    void init(const Size3i& size, float cellSize, const Point3i& offset)
    {
        mCellSize = cellSize;
        mOffset = offset;
        mSize = size;

        Point3i a = -mOffset;
        Point3i b = a + mSize;

        mData = createArray(a,b);
    }

    Array* createArray(const Point3i& a, const Point3i& b) {
        typename Array::extent_gen extents;
        return new Array(extents[Range(a(0),b(0))]
                                [Range(a(1),b(1))]
                                [Range(a(2),b(2))]);
    }


public:

    VoxelMap(float cellSize=0.1f) : mCellSize(cellSize), mOffset(0, 0, 0) {  mData = new Array; }

    VoxelMap(const Size3i& size, float cellSize, const Point3i& offset = Point3i(0, 0, 0)) {
        init(size, cellSize, offset);
    }

    VoxelMap(const Box3f& region, float cellSize)
    {
        auto p = getOffsetAndSize(region,cellSize);
        init(p.second, cellSize, p.first);
    }

    VoxelMap(const Box3i& region, float cellSize) {
        init(region.size(), cellSize, -region.minCorner);
    }

    ~VoxelMap() { delete mData; }

public:

    float getCellSize() const { return mCellSize; }

    Point3i getOffset() const { return mOffset; }

public:

    VoxelMap& operator=(const T& value) {

        Point3i a = -mOffset;
        Point3i b = a + mSize;
        for(int i=a(0); i<b(0); ++i)
            for(int j=a(1); j<b(1); ++j)
                for(int k=a(2); k<b(2); ++k)
                    (*mData)[i][j][k] = value;
        return *this;
    }

public:

    Box3f getRegion() const { return Box3f(-mOffset*mCellSize, mSize*mCellSize); }

    Box3i getMapRegion() const { return Box3f(-mOffset, mSize); }

public:


    Array& data() { return *mData; }
    const Array& data() const { return *mData; }

    T& operator()(const Point3i& idx) {
        return data()[idx(0)][idx(1)][idx(2)];
    }

    const T& operator()(const Point3i& idx) const {
        return data()[idx(0)][idx(1)][idx(2)];
    }


public:

    Point3i world2map(const Point3f& p) const {
        float ioff = 1.0f / mCellSize;
        return p * ioff + mOffset;
    }

    Point3f map2world(const Point3i& p) const {
        return (p - mOffset) * mCellSize;
    }


public:

    void clip(const Box3i& region, const T& valueForNewCells);

#if 0

    void grow(const Index& pMin, const Index& pMax)
    {
        if(pMin >= mMin && pMax <= mMax)
            return;

        //std::cout << "VoxelMap::grow " << endl;

        Index min;
        Index max;
        for(int i=0; i<3; ++i) {
            min(i) = std::min(mMin(i), pMin(i));
            max(i) = std::max(mMax(i), pMax(i));
        }

        typename Array::extent_gen extents;
        Array* newData = new Array(extents[Range(min(0),max(0)+1)]
                                                    [Range(min(1),max(1)+1)]
                                                    [Range(min(2),max(2)+1)]);

        // copy old content in new map
        for(int i=mMin(0); i<=mMax(0); ++i)
            for(int j=mMin(1); j<=mMax(1); ++j)
                for(int k=mMin(2); k<=mMax(2); ++k)
                    (*newData)[i][j][k] = (*mData)[i][j][k];

        Array* oldData = mData;
        mData = newData;
        mMin = min;
        mMax = max;
        delete oldData;
    }

    void grow(const MetricIndex& pMin, const MetricIndex& pMax) {
        grow(metricToCell(pMin), metricToCell(pMax));
    }


    void resize(const Index& min, const Index& max)
    {
        if(min == mMin && max == mMax)
             return;

        //std::cout << "VoxelMap::resize " << endl;

        typename Array::extent_gen extents;
        Array* newData = new Array(extents[Range(min(0),max(0)+1)]
                                                    [Range(min(1),max(1)+1)]
                                                    [Range(min(2),max(2)+1)]);

        // get the intersecting region, that needs to be copied
        Index interMin;
        Index interMax;

        for(int i=0; i<3; ++i) {
            interMin(i) = std::max(mMin(i), min(i));
            interMax(i) = std::min(mMax(i), max(i));
        }

        // copy content in intersecting region into new map
        for(int i=interMin(0); i<=interMax(0); ++i)
            for(int j=interMin(1); j<=interMax(1); ++j)
                for(int k=interMin(2); k<=interMax(2); ++k)
                    (*newData)[i][j][k] = (*mData)[i][j][k];

        Array* oldData = mData;
        mData = newData;
        mMin = min;
        mMax = max;
        delete oldData;
    }

    void resize(const MetricIndex& pMin, const MetricIndex& pMax) {
        resize(metricToCell(pMin), metricToCell(pMax));
    }

#endif


private:

    static std::pair<Point3i, Size3i> getOffsetAndSize(const Box3f& region, float cellSize);

private:

    float   mCellSize; 
    Point3i mOffset;   
    Size3i  mSize;

    Array* mData;
};


template<typename T>
inline void VoxelMap<T>::clip(const Box3i& region, const T& valueForNewCells)
{
    Box3i oldRegion = getMapRegion();
    if(region==oldRegion)
        return; // nothing to do

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


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

    Point3i copyRegionInNewLL(0,0,0);
    Point3i copyRegionInOldLL(0,0,0);

    for(int i=0; i<3; ++i)
    {
        if(oldRegion.minCorner(i) > region.minCorner(i))
            copyRegionInNewLL(i) = oldRegion.minCorner(i) - region.minCorner(i);
        else
            copyRegionInOldLL(i) = region.minCorner(i) - oldRegion.minCorner(i);
    }

    Box3i intersection = oldRegion & region;

    Array* oldBuffer = this->mData;
    Array* newBuffer = createArray(region.minCorner, region.maxCorner);

    if(intersection.isValid()) {

        Point3i copyRegionInNewUR = copyRegionInNewLL + intersection.size();
        Point3i 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>
inline std::pair<Point3i, Size3i> VoxelMap<T>::getOffsetAndSize(const Box3f& region, float cellSize)
{
    Point3i lowerLeft (std::floor(region.x0() / cellSize),
                       std::floor(region.y0() / cellSize),
                       std::floor(region.z0() / cellSize));
    Point3i upperRight(std::ceil(region.x1()  / cellSize),
                       std::ceil(region.y1()  / cellSize),
                       std::ceil(region.z1()  / cellSize));

    // compute required size and offset of the map
    Size3i  size = upperRight - lowerLeft;
    Point3i offset = -lowerLeft;
    return std::make_pair(offset,size);
}


}} // namespace

#endif /* _MIRA_GRIDMAP_H_ */