maploader.hpp

#ifndef MAPLOADER_H_
#define MAPLOADER_H_

#include <CImg.h>

#include <fast_methods/ndgridmap/ndgridmap.hpp>


using namespace cimg_library;


class MapLoader {
    public:
        template<class T, size_t ndims>
        static void loadMapFromImg
        (const char * filename, nDGridMap<T, ndims> & grid) {
            std::vector<unsigned int> obs;
            CImg<double> img(filename);
            std::array<unsigned int, ndims> dimsize;
            dimsize[0] = img.width();
            dimsize[1] = img.height();
            grid.resize(dimsize);

            // Filling the grid flipping Y dim. We want bottom left to be the (0,0).
            cimg_forXY(img,x,y) {
                double occupancy = img(x,y)/255;
                unsigned int idx = img.width()*(img.height()-y-1)+x;
                grid[idx].setOccupancy(occupancy);
                if (grid[idx].isOccupied())
                    obs.push_back(idx);
                }
            grid.setOccupiedCells(std::move(obs));
        }

        template<class T, size_t ndims>
        static int loadMapFromText
        (const char * filename, nDGridMap<T, ndims> & grid) {
            std::ifstream file;
            std::vector<unsigned int> obs;
            file.open(filename);

            if (file.is_open())
            {
                std::string val;
                std::getline(file, val);

                double leafsize;
                size_t ndims_aux;

                file >> leafsize;
                file >> ndims_aux;

                if (ndims_aux != ndims) {
                    console::error("Number of dimensions specified does not match the loaded grid.");
                    exit(1);
                }

                std::array<unsigned int, ndims> dimsize;
                for (unsigned int &i : dimsize) {
                    file >> i;
                }

                grid.resize(dimsize);
                grid.setLeafSize(leafsize);

                double occupancy;
                for (unsigned int i = 0; i < grid.size(); ++i)
                {
                    file >> occupancy;
                    grid[i].setOccupancy(occupancy);

                    if (grid[i].isOccupied())
                        obs.push_back(i);
                }
                grid.setOccupiedCells(std::move(obs));
                return 1;
            }
            else
            {
                console::error("File not found.");
                return 0;
            }
        }
};

#endif /* MAPLOADER_H_ */