GridMapCvConverter.hpp
Go to the documentation of this file.
00001 /*
00002  * GridMapCvConverter.hpp
00003  *
00004  *  Created on: Apr 14, 2016
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #pragma once
00010 
00011 #include <grid_map_core/grid_map_core.hpp>
00012 
00013 // OpenCV
00014 #include <cv_bridge/cv_bridge.h>
00015 
00016 // STD
00017 #include <iostream>
00018 
00019 namespace grid_map {
00020 
00024 class GridMapCvConverter
00025 {
00026  public:
00036   static bool initializeFromImage(const cv::Mat& image, const double resolution,
00037                                   grid_map::GridMap& gridMap, const grid_map::Position& position)
00038   {
00039     const double lengthX = resolution * image.rows;
00040     const double lengthY = resolution * image.cols;
00041     Length length(lengthX, lengthY);
00042     gridMap.setGeometry(length, resolution, position);
00043     return true;
00044   }
00045 
00058   template<typename Type_, int NChannels_>
00059   static bool addLayerFromImage(const cv::Mat& image, const std::string& layer,
00060                                 grid_map::GridMap& gridMap, const float lowerValue = 0.0,
00061                                 const float upperValue = 1.0, const double alphaThreshold = 0.5)
00062   {
00063     if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
00064       std::cerr << "Image size does not correspond to grid map size!" << std::endl;
00065       return false;
00066     }
00067 
00068     bool isColor = false;
00069     if (image.channels() >= 3) isColor = true;
00070     bool hasAlpha = false;
00071     if (image.channels() >= 4) hasAlpha = true;
00072 
00073     cv::Mat imageMono;
00074     if (isColor && !hasAlpha) {
00075       cv::cvtColor(image, imageMono, CV_BGR2GRAY);
00076     } else if (isColor && hasAlpha) {
00077       cv::cvtColor(image, imageMono, CV_BGRA2GRAY);
00078     } else if (!isColor && !hasAlpha){
00079       imageMono = image;
00080     } else {
00081       std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl;
00082       return false;
00083     }
00084 
00085     const float mapValueDifference = upperValue - lowerValue;
00086 
00087     float maxImageValue;
00088     if (std::is_same<Type_, float>::value || std::is_same<Type_, double>::value) {
00089       maxImageValue = 1.0;
00090     } else if (std::is_same<Type_, unsigned short>::value || std::is_same<Type_, unsigned char>::value) {
00091       maxImageValue = (float)std::numeric_limits<Type_>::max();
00092     } else {
00093       std::cerr << "This image type is not supported." << std::endl;
00094       return false;
00095     }
00096 
00097     const Type_ alphaTreshold = (Type_)(alphaThreshold * maxImageValue);
00098 
00099     gridMap.add(layer);
00100     grid_map::Matrix& data = gridMap[layer];
00101 
00102     for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
00103       const Index index(*iterator);
00104 
00105       // Check for alpha layer.
00106       if (hasAlpha) {
00107         const Type_ alpha = image.at<cv::Vec<Type_, NChannels_>>(index(0), index(1))[NChannels_ - 1];
00108         if (alpha < alphaTreshold) continue;
00109       }
00110 
00111       // Compute value.
00112       const Type_ imageValue = imageMono.at<Type_>(index(0), index(1));
00113       const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
00114       data(index(0), index(1)) = mapValue;
00115     }
00116 
00117     return true;
00118   };
00119 
00127   template<typename Type_, int NChannels_>
00128   static bool addColorLayerFromImage(const cv::Mat& image, const std::string& layer,
00129                                      grid_map::GridMap& gridMap)
00130   {
00131     if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) {
00132       std::cerr << "Image size does not correspond to grid map size!" << std::endl;
00133       return false;
00134     }
00135 
00136     bool hasAlpha = false;
00137     if (image.channels() >= 4) hasAlpha = true;
00138 
00139     cv::Mat imageRGB;
00140     if (hasAlpha) {
00141       cv::cvtColor(image, imageRGB, CV_BGRA2RGB);
00142     } else {
00143       imageRGB = image;
00144     }
00145 
00146     gridMap.add(layer);
00147 
00148     for (GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) {
00149       const auto& cvColor = imageRGB.at<cv::Vec<Type_, 3>>((*iterator)(0), (*iterator)(1));
00150       Eigen::Vector3i colorVector;
00151       colorVector(0) = cvColor[0];
00152       colorVector(1) = cvColor[1];
00153       colorVector(2) = cvColor[2];
00154       colorVectorToValue(colorVector, gridMap.at(layer, *iterator));
00155     }
00156 
00157     return true;
00158   }
00159 
00172   template<typename Type_, int NChannels_>
00173   static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
00174                       const int encoding, cv::Mat& image)
00175   {
00176     const float minValue = gridMap.get(layer).minCoeffOfFinites();
00177     const float maxValue = gridMap.get(layer).maxCoeffOfFinites();
00178     return toImage<Type_, NChannels_>(gridMap, layer, encoding, minValue, maxValue, image);
00179   };
00180 
00191   template<typename Type_, int NChannels_>
00192   static bool toImage(const grid_map::GridMap& gridMap, const std::string& layer,
00193                       const int encoding, const float lowerValue, const float upperValue,
00194                       cv::Mat& image)
00195   {
00196     // Initialize image.
00197     if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
00198       image = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), encoding);
00199     } else {
00200       std::cerr << "Invalid grid map?" << std::endl;
00201       return false;
00202     }
00203 
00204     // Get max image value.
00205     Type_ imageMax;
00206     if (std::is_same<Type_, float>::value || std::is_same<Type_, double>::value) {
00207       imageMax = 1.0;
00208     } else if (std::is_same<Type_, unsigned short>::value || std::is_same<Type_, unsigned char>::value) {
00209       imageMax = (Type_)std::numeric_limits<Type_>::max();
00210     } else {
00211       std::cerr << "This image type is not supported." << std::endl;
00212       return false;
00213     }
00214 
00215     // Clamp outliers.
00216     grid_map::GridMap map = gridMap;
00217     map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(lowerValue, upperValue));
00218     const grid_map::Matrix& data = map[layer];
00219 
00220     // Convert to image.
00221     bool isColor = false;
00222     if (image.channels() >= 3) isColor = true;
00223     bool hasAlpha = false;
00224     if (image.channels() >= 4) hasAlpha = true;
00225 
00226     for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
00227       const Index index(*iterator);
00228       if (std::isfinite(data(index(0), index(1)))) {
00229         const float& value = data(index(0), index(1));
00230         const Type_ imageValue = (Type_) (((value - lowerValue) / (upperValue - lowerValue)) * (float) imageMax);
00231         const Index imageIndex(iterator.getUnwrappedIndex());
00232         unsigned int channel = 0;
00233         image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[channel] = imageValue;
00234 
00235         if (isColor) {
00236           image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
00237           image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageValue;
00238         }
00239         if (hasAlpha) {
00240           image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[++channel] = imageMax;
00241         }
00242       }
00243     }
00244 
00245     return true;
00246   };
00247 
00248 };
00249 
00250 } /* namespace */


grid_map_cv
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:25