gridplotter.hpp

#ifndef GRIDPLOTTER_H_
#define GRIDPLOTTER_H_

#include <string>
#include <array>

#include <CImg.h>

#include <fast_methods/ndgridmap/fmcell.h>
#include <fast_methods/ndgridmap/ndgridmap.hpp>


using namespace cimg_library;

class GridPlotter {
    typedef typename std::array<unsigned int, 2> Coord2D;
    
    typedef typename std::array<double, 2> Point2D;
    
    typedef typename std::vector <Point2D> Path2D;
    
    typedef typename std::vector <Path2D> Paths2D;

    public:
        template<class T, size_t ndims> 
        static void plotMap
        (nDGridMap<T, ndims> & grid, std::string name = "") {
            std::array<unsigned int,2> d = grid.getDimSizes();
            CImg<bool> img(d[0],d[1],1,1,0);
            // Filling the image flipping Y dim. We want now top left to be the (0,0).
            cimg_forXY(img,x,y) {img(x,y) = !grid[img.width()*(img.height()-y-1)+x].isOccupied(); }
            // Code for not-inverted Y axis.
            //cimg_forXY(img,x,y) { if(grid[img.width()*y+x].getOccupancy() > 0.5) img(x,y) = true;
            //                      else img(x,y) = false; }
            name += " Map";
            img.display(name.c_str(), false);
        }

        template<class T, size_t ndims>
        static void plotOccupancyMap
        (nDGridMap<T, ndims> & grid, std::string name = "") {
            std::array<unsigned int,2> d = grid.getDimSizes();
            CImg<double> img(d[0],d[1],1,1,0);
            // Filling the image flipping Y dim. We want now top left to be the (0,0).
            cimg_forXY(img,x,y) { img(x,y) = grid[img.width()*(img.height()-y-1)+x].getOccupancy()*255; }
            name += " Occupancy Map";
            img.display(name.c_str(), false);
        }

        template<class T, size_t ndims = 2>
        static void plotArrivalTimes
        (nDGridMap<T, ndims> & grid, std::string name = "") {
            std::array<unsigned int,2> d = grid.getDimSizes();
            double max_val = grid.getMaxValue();
            CImg<double> img(d[0],d[1],1,1,0);
            // Filling the image flipping Y dim. We want now top left to be the (0,0).
            cimg_forXY(img,x,y) { img(x,y) = grid[img.width()*(img.height()-y-1)+x].getValue()/max_val*255; }
            img.map( CImg<double>::jet_LUT256() );
            name += " Grid values";
            img.display(name.c_str(), false);
        }

        template<class T, size_t ndims = 2>
        static void plotMapPath
        (nDGridMap<T, ndims> & grid, const Path2D & path, std::string name = "") {
            std::array<unsigned int,2> d = grid.getDimSizes();
            CImg<double> img(d[0],d[1],1,3,0);

            // Filling the image flipping Y dim. We want now top left to be the (0,0).
            cimg_forXYZC(img,x,y,z,c) { img(x,y,z,c) = (!grid[img.width()*(img.height()-y-1)+x].isOccupied())*255; }

            for (unsigned int i = 0; i< path.size(); ++i)
            {
                img(static_cast<unsigned int>(path[i][0]), (img.height()-static_cast<unsigned int>(path[i][1])-1), 0, 1) = 0;
                img(static_cast<unsigned int>(path[i][0]), (img.height()-static_cast<unsigned int>(path[i][1])-1), 0, 2) = 0;
            }

            name += " Map and Path";
            img.display(name.c_str(), false);
        }

        template<class T, size_t ndims = 2>
        static void plotOccupancyPath
        (nDGridMap<T, ndims> & grid, const Path2D & path, std::string name = "") {
            std::array<unsigned int,2> d = grid.getDimSizes();
            CImg<double> img(d[0],d[1],1,3,0);
            // Filling the image flipping Y dim. We want now top left to be the (0,0).
            cimg_forXYZC(img,x,y,z,c) { img(x,y,z,c) = grid[img.width()*(img.height()-y-1)+x].getOccupancy()*255; }

            for (unsigned int i = 0; i< path.size(); ++i)
            {
                img(static_cast<unsigned int>(path[i][0]), (img.height()-static_cast<unsigned int>(path[i][1])-1), 0, 1) = 0;
                img(static_cast<unsigned int>(path[i][0]), (img.height()-static_cast<unsigned int>(path[i][1])-1), 0, 2) = 0;
            }
            name += " Map and Path";
            img.display(name.c_str(), false);
        }

        template<class T, size_t ndims = 2>
        static void plotMapPaths
        (nDGridMap<T, ndims> & grid, const Paths2D & paths, std::string name = "") {
            std::array<unsigned int,2> d = grid.getDimSizes();
            CImg<double> img(d[0],d[1],1,3,0);

            // Filling the image flipping Y dim. We want now top left to be the (0,0).
            cimg_forXYZC(img,x,y,z,c) { img(x,y,z,c) = (!grid[img.width()*(img.height()-y-1)+x].isOccupied())*255; }

            // Draw the path using different colours
            for (unsigned int j = 0; j < paths.size(); ++j)
            {
                Path2D path = paths[j];
                for (unsigned int i = 0; i< path.size(); ++i)
                {
                    img(static_cast<unsigned int>(path[i][0]), (img.height()-static_cast<unsigned int>(path[i][1])-1), 0, j) = 0;
                    img(static_cast<unsigned int>(path[i][0]), (img.height()-static_cast<unsigned int>(path[i][1])-1), 0, j+1) = 0;
                }
            }
            name += " Map and Paths";
            img.display(name.c_str(), false);
        }

      template<class T, size_t ndims = 2>
      static void plotArrivalTimesPath
      (nDGridMap<T, ndims> & grid, const Path2D & path, std::string name = "") {
          std::array<unsigned int,2> d = grid.getDimSizes();
          double max_val = grid.getMaxValue();
          CImg<double> img(d[0],d[1],1,1,0);

          // Filling the image flipping Y dim. We want now top left to be the (0,0).
          cimg_forXY(img,x,y) { img(x,y) = grid[img.width()*(img.height()-y-1)+x].getValue()/max_val*255; }

          for (unsigned int i = 0; i< path.size(); ++i)
              img(static_cast<unsigned int>(path[i][0]), (img.height()-static_cast<unsigned int>(path[i][1])-1)) = 255;

          img.map( CImg<double>::jet_LUT256() );
          name += " Values and Path";
          img.display(name.c_str(), false);
      }

       template<class T, size_t ndims = 2>
       static void plotFMStates
       (nDGridMap<T, ndims> & grid, std::string name = "") {
           std::array<unsigned int,2> d = grid.getDimSizes();
           //double max_val = grid.getMaxValue();
           CImg<unsigned int> img(d[0],d[1],1,1,0);
           // Filling the image flipping Y dim. We want now top left to be the (0,0).
           cimg_forXY(img,x,y) {
               FMState state = grid[img.width()*(img.height()-y-1)+x].getState();
               unsigned int val = 0;
               if (state == FMState::NARROW)
                   val = 127;
               else if (state == FMState::OPEN)
                   val = 255;
               img(x,y) = val;
           }
           //img.map( CImg<double>::jet_LUT256() );
           name += "FMStates";
           img.display(name.c_str(), false);
       }

};

#endif /* GRIDPLOTTER_H_ */