Program Listing for File GridMapCvConverter.hpp

↰ Return to documentation for file (/tmp/ws/src/grid_map/grid_map_cv/include/grid_map_cv/GridMapCvConverter.hpp)

/*
 * 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_