GridMapIO.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_IO_H_
#define _MIRA_GRIDMAP_IO_H_

#include <opencv2/highgui/highgui.hpp>

#include <serialization/XMLSerializer.h>

#include <maps/GridMap.h>

namespace mira { namespace maps {


struct GridMapInfo
{
    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.member("Image", image, "Filename of the map image");
        r.member("CellSize", cellSize, "Resolution of the map in [m/cell]");
        r.member("Offset", offset, "Offset of the origin of the image [cells]", Point2i(0, 0));
    }

    std::string image;
    float cellSize;
    Point2i offset;
};

struct GridMapFileInfoWithFrame : public maps::GridMapInfo
{
    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, maps::GridMapInfo);
        r.member("Frame", frame, "Frame ID of the map", "MapFrame");
    }

    std::string frame;
};

struct GridMapFileInfoWithFrameAndChannelName : public GridMapFileInfoWithFrame
{
    template<typename Reflector>
    void reflect(Reflector& r)
    {
        MIRA_REFLECT_BASE(r, GridMapFileInfoWithFrame);
        r.member("Channel", channel, "The channel name where to publish the map");
    }

    std::string channel;
};

template<typename T, int Channels>
struct PixelTruncateHelper
{
    static void truncate(Img<T, Channels>& img, T truncateTo) {
        using Pixel = typename Img<T, Channels>::Pixel;
        foreach(Pixel& p, img) {
            for (int i = 0; i < Channels; ++i)
                p[i] = std::min(p[i], truncateTo);
        }
    }
};

template<typename T>
struct PixelTruncateHelper<T, 1>
{
    static void truncate(Img<T, 1>& img, T truncateTo) {
        foreach(T& p, img) {
            p = std::min(p, truncateTo);
        }
    }
};

template<typename T, int Channels = 1>
GridMap<T, Channels> loadGridMapFromImage(const Img<>& img,
                                          float cellSize,
                                          const Point2i& offset,
                                          T truncateTo = std::numeric_limits<T>::max())
{
    cv::Mat flipped;
    cv::flip(img.getMat(), flipped, 0);

    Img<T,Channels> mapImg = Img<T, Channels>::convertFrom(Img<>(flipped));
    PixelTruncateHelper<T, Channels>::truncate(mapImg, truncateTo);

    return GridMap<T, Channels>(mapImg, cellSize, offset);
}

template<typename T, int Channels = 1>
GridMap<T, Channels> loadGridMapFromFile(const Path& filename,
                                         float cellSize,
                                         const Point2i& offset,
                                         T truncateTo = std::numeric_limits<T>::max())
{
    Path path = resolvePath(filename);
    if ( !boost::filesystem::exists(path) )
        MIRA_THROW(XFileNotFound, "The map '" << path.string() << "' does not exist");

    Img<> data(cv::imread(path.string()));
    if ( data.isEmpty() )
        MIRA_THROW(XIO, "The map '" << path.string() << "' could not be loaded");

    return loadGridMapFromImage<T, Channels>(data, cellSize, offset, truncateTo);
}

template<typename T, int Channels = 1>
GridMap<T, Channels> loadGridMap(const GridMapInfo& info,
                                 T truncateTo = std::numeric_limits<T>::max())
{
    return loadGridMapFromFile<T, Channels>(info.image, info.cellSize, info.offset, truncateTo);
}

template<typename T, int Channels = 1>
GridMap<T, Channels> loadGridMap(const Path& descriptionFilename,
                                 T truncateTo = std::numeric_limits<T>::max())
{
    XMLDom xml;
    xml.loadFromFile(descriptionFilename);

    XMLDeserializer s(xml);
    maps::GridMapInfo info;
    s.deserialize("Map", info);

    return loadGridMap<T, Channels>(info, truncateTo);
}

template<typename T, int Channels>
void saveGridMapToFile(Path filename,
                       const GridMap<T, Channels>& map,
                       bool fullImagePath = true)
{
    if(!filename.has_extension())
        filename.replace_extension(".png");

    // save the image
    cv::Mat flipped;
    cv::flip(map, flipped, 0);
    cv::imwrite(filename.string(), flipped);

    // save the xml info
    Path xmlFilename = filename;
    xmlFilename.replace_extension(".xml");

    maps::GridMapInfo info;
    info.cellSize = map.getCellSize();
    info.offset = map.getMapOffset();
    if (fullImagePath)
        info.image = filename.string();
    else
        info.image = filename.filename().string();

    XMLDom xml;
    XMLSerializer s(xml);
    s.serialize("Map", info);
    xml.saveToFile(xmlFilename);
}


}}

#endif