InpaintFilter.cpp
Go to the documentation of this file.
00001 /*
00002  * InpaintFilter.cpp
00003  *
00004  *  Created on: May 6, 2017
00005  *      Author: Tanja Baumann, Peter Fankhauser
00006  *   Institute: ETH Zurich, Robotic Systems Lab
00007  */
00008 
00009 #include "grid_map_cv/InpaintFilter.hpp"
00010 #include <pluginlib/class_list_macros.h>
00011 #include <ros/ros.h>
00012 
00013 // Grid Map
00014 #include <grid_map_core/grid_map_core.hpp>
00015 
00016 using namespace filters;
00017 
00018 namespace grid_map {
00019 
00020 template<typename T>
00021 InpaintFilter<T>::InpaintFilter()
00022     : radius_(5.0) {
00023 
00024 }
00025 
00026 template<typename T>
00027 InpaintFilter<T>::~InpaintFilter() {
00028 
00029 }
00030 
00031 template<typename T>
00032 bool InpaintFilter<T>::configure() {
00033   if (!FilterBase < T > ::getParam(std::string("radius"), radius_)) {
00034     ROS_ERROR("InpaintRadius filter did not find param radius.");
00035     return false;
00036   }
00037 
00038   if (radius_ < 0.0) {
00039     ROS_ERROR("Radius must be greater than zero.");
00040     return false;
00041   }
00042 
00043   ROS_DEBUG("Radius = %f.", radius_);
00044 
00045   if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
00046     ROS_ERROR("Inpaint filter did not find parameter `input_layer`.");
00047     return false;
00048   }
00049 
00050   ROS_DEBUG("Inpaint input layer is = %s.", inputLayer_.c_str());
00051 
00052   if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
00053     ROS_ERROR("Inpaint filter did not find parameter `output_layer`.");
00054     return false;
00055   }
00056 
00057   ROS_DEBUG("Inpaint output layer = %s.", outputLayer_.c_str());
00058 
00059   return true;
00060 }
00061 
00062 template<typename T>
00063 bool InpaintFilter<T>::update(const T& mapIn, T& mapOut) {
00064   // Add new layer to the elevation map.
00065   mapOut = mapIn;
00066   mapOut.add(outputLayer_);
00067 
00068   //Convert elevation layer to OpenCV image to fill in holes.
00069   //Get the inpaint mask (nonzero pixels indicate where values need to be filled in).
00070   mapOut.add("inpaint_mask", 0.0);
00071 
00072   mapOut.setBasicLayers(std::vector<std::string>());
00073   for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
00074     if (!mapOut.isValid(*iterator, inputLayer_)) {
00075       mapOut.at("inpaint_mask", *iterator) = 1.0;
00076     }
00077   }
00078   cv::Mat originalImage;
00079   cv::Mat mask;
00080   cv::Mat filledImage;
00081   const float minValue = mapOut.get(inputLayer_).minCoeffOfFinites();
00082   const float maxValue = mapOut.get(inputLayer_).maxCoeffOfFinites();
00083 
00084   grid_map::GridMapCvConverter::toImage<unsigned char, 3>(mapOut, inputLayer_, CV_8UC3, minValue, maxValue,
00085                                                           originalImage);
00086   grid_map::GridMapCvConverter::toImage<unsigned char, 1>(mapOut, "inpaint_mask", CV_8UC1, mask);
00087 
00088   const double radiusInPixels = radius_ / mapIn.getResolution();
00089   cv::inpaint(originalImage, mask, filledImage, radiusInPixels, cv::INPAINT_NS);
00090 
00091   grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 3>(filledImage, outputLayer_, mapOut, minValue, maxValue);
00092   mapOut.erase("inpaint_mask");
00093 
00094   return true;
00095 }
00096 
00097 }/* namespace */
00098 
00099 PLUGINLIB_EXPORT_CLASS(grid_map::InpaintFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)


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