CostMap.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_COSTMAP_H_
#define _MIRA_COSTMAP_H_

#include <maps/GridMap.h>

namespace mira { namespace maps {


class CostMap : public GridMap<double>
{
public:
    typedef GridMap<double> Base;

    CostMap() {}

    CostMap(const Base& map, const std::string& iFrameID) : Base(map), frameID(iFrameID) {}

    CostMap(const Size2i& size, float cellSize,
            const Point2i& offset = Point2i(0, 0),
            const std::string& iFrameID = "") : Base(size, cellSize, offset), frameID(iFrameID) {}

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

    CostMap clone() const {
        return CostMap(Base::clone(), frameID);
    }

    Point2f getGradient(const Point2i& mapPos) const {
        Point2f value = Point2f(0.0, 0.0);
        if(mapPos.x() <= 0 || (int)mapPos.x() >= width()-1 ||
           mapPos.y() <= 0 || (int)mapPos.y() >= height()-1 )
            return value;

        value = Point2f((operator()(mapPos.x()-1,mapPos.y()) - operator()(mapPos.x()+1,mapPos.y()))/2.0,
                        (operator()(mapPos.x(),mapPos.y()-1) - operator()(mapPos.x(),mapPos.y()+1))/2.0);
        return value;
    }

public:
    std::string frameID;
};

typedef std::list<Rect2i> DirtyRegions;
typedef std::list<Rect2f> DirtyRegionsWorld;

DirtyRegions mergeRegions(DirtyRegions regions);


}}

#endif