.. _program_listing_file__tmp_ws_src_grid_map_grid_map_cv_include_grid_map_cv_GridMapCvConverter.hpp:

Program Listing for File GridMapCvConverter.hpp
===============================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_grid_map_grid_map_cv_include_grid_map_cv_GridMapCvConverter.hpp>` (``/tmp/ws/src/grid_map/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * GridMapCvConverter.hpp
    *
    *  Created on: Apr 14, 2016
    *      Author: Péter Fankhauser
    *   Institute: ETH Zurich, ANYbotics
    */
   
   #ifndef GRID_MAP_CV__GRIDMAPCVCONVERTER_HPP_
   #define GRID_MAP_CV__GRIDMAPCVCONVERTER_HPP_
   
   #include <grid_map_core/grid_map_core.hpp>
   
   // OpenCV
   #include <cv_bridge/cv_bridge.h>
   
   // STD
   #include <iostream>
   
   #include <string>
   #include <limits>
   
   namespace grid_map
   {
   
   class GridMapCvConverter
   {
   public:
     static bool initializeFromImage(
       const cv::Mat & image, const double resolution,
       grid_map::GridMap & gridMap, const grid_map::Position & position)
     {
       const double lengthX = resolution * image.rows;
       const double lengthY = resolution * image.cols;
       Length length(lengthX, lengthY);
       gridMap.setGeometry(length, resolution, position);
       return true;
     }
   
     template<typename Type_, int NChannels_>
     static bool addLayerFromImage(
       const cv::Mat & image, const std::string & layer,
       grid_map::GridMap & gridMap, const float lowerValue = 0.0,
       const float upperValue = 1.0, const double alphaThreshold = 0.5)
     {
       if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
         std::cerr << "Image size does not correspond to grid map size!" << std::endl;
         return false;
       }
   
       bool isColor = false;
       if (image.channels() >= 3) {isColor = true;}
       bool hasAlpha = false;
       if (image.channels() >= 4) {hasAlpha = true;}
   
       cv::Mat imageMono;
       if (isColor && !hasAlpha) {
         cv::cvtColor(image, imageMono, CV_BGR2GRAY);
       } else if (isColor && hasAlpha) {
         cv::cvtColor(image, imageMono, CV_BGRA2GRAY);
       } else if (!isColor && !hasAlpha) {
         imageMono = image;
       } else {
         std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl;
         return false;
       }
   
       const float mapValueDifference = upperValue - lowerValue;
   
       float maxImageValue;
       if (std::is_same<Type_, float>::value || std::is_same<Type_, double>::value) {
         maxImageValue = 1.0;
       } else if (std::is_same<Type_, uint16_t>::value || std::is_same<Type_, unsigned char>::value) {
         maxImageValue = static_cast<float>(std::numeric_limits<Type_>::max());
       } else {
         std::cerr << "This image type is not supported." << std::endl;
         return false;
       }
   
       const Type_ alphaTreshold = (Type_)(alphaThreshold * maxImageValue);
   
       gridMap.add(layer);
       grid_map::Matrix & data = gridMap[layer];
   
       for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
         const Index index(*iterator);
   
         // Check for alpha layer.
         if (hasAlpha) {
           const Type_ alpha =
             image.at<cv::Vec<Type_, NChannels_>>(index(0), index(1))[NChannels_ - 1];
           if (alpha < alphaTreshold) {continue;}
         }
   
         // Compute value.
         const Type_ imageValue = imageMono.at<Type_>(index(0), index(1));
         const float mapValue = lowerValue + mapValueDifference *
           (static_cast<float>(imageValue) / maxImageValue);
         data(index(0), index(1)) = mapValue;
       }
   
       return true;
     }
   
     template<typename Type_, int NChannels_>
     static bool addColorLayerFromImage(
       const cv::Mat & image, const std::string & layer,
       grid_map::GridMap & gridMap)
     {
       if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
         std::cerr << "Image size does not correspond to grid map size!" << std::endl;
         return false;
       }
   
       bool hasAlpha = false;
       if (image.channels() >= 4) {hasAlpha = true;}
   
       cv::Mat imageRGB;
       if (hasAlpha) {
         cv::cvtColor(image, imageRGB, CV_BGRA2RGB);
       } else {
         imageRGB = image;
       }
   
       gridMap.add(layer);
   
       for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
         const auto & cvColor = imageRGB.at<cv::Vec<Type_, 3>>((*iterator)(0), (*iterator)(1));
         Eigen::Vector3i colorVector;
         colorVector(0) = cvColor[0];
         colorVector(1) = cvColor[1];
         colorVector(2) = cvColor[2];
         colorVectorToValue(colorVector, gridMap.at(layer, *iterator));
       }
   
       return true;
     }
   
     template<typename Type_, int NChannels_>
     static bool toImage(
       const grid_map::GridMap & gridMap, const std::string & layer,
       const int encoding, cv::Mat & image)
     {
       const float minValue = gridMap.get(layer).minCoeffOfFinites();
       const float maxValue = gridMap.get(layer).maxCoeffOfFinites();
       return toImage<Type_, NChannels_>(gridMap, layer, encoding, minValue, maxValue, image);
     }
   
     template<typename Type_, int NChannels_>
     static bool toImage(
       const grid_map::GridMap & gridMap, const std::string & layer,
       const int encoding, const float lowerValue, const float upperValue,
       cv::Mat & image)
     {
       // Initialize image.
       if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
         image = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding);
       } else {
         std::cerr << "Invalid grid map?" << std::endl;
         return false;
       }
   
       // Get max image value.
       Type_ imageMax;
       if (std::is_same<Type_, float>::value || std::is_same<Type_, double>::value) {
         imageMax = 1.0;
       } else if (std::is_same<Type_, uint16_t>::value || std::is_same<Type_, unsigned char>::value) {
         imageMax = (Type_)std::numeric_limits<Type_>::max();
       } else {
         std::cerr << "This image type is not supported." << std::endl;
         return false;
       }
   
       // Clamp outliers.
       grid_map::GridMap map = gridMap;
       map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(lowerValue, upperValue));
       const grid_map::Matrix & data = map[layer];
   
       // Convert to image.
       bool isColor = false;
       if (image.channels() >= 3) {isColor = true;}
       bool hasAlpha = false;
       if (image.channels() >= 4) {hasAlpha = true;}
   
       for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
         const Index index(*iterator);
         if (std::isfinite(data(index(0), index(1)))) {
           const float & value = data(index(0), index(1));
           const Type_ imageValue =
             (Type_) (((value - lowerValue) / (upperValue - lowerValue)) *
             static_cast<float>(imageMax));
           const Index imageIndex(iterator.getUnwrappedIndex());
           unsigned int channel = 0;
           image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[channel] = imageValue;
   
           if (isColor) {
             image.at<cv::Vec<Type_, NChannels_>>(
               imageIndex(0),
               imageIndex(1))[++channel] = imageValue;
             image.at<cv::Vec<Type_, NChannels_>>(
               imageIndex(0),
               imageIndex(1))[++channel] = imageValue;
           }
           if (hasAlpha) {
             image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageMax;
           }
         }
       }
       return true;
     }
   };
   
   }  // namespace grid_map
   #endif  // GRID_MAP_CV__GRIDMAPCVCONVERTER_HPP_