Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include "grid_map_cv/InpaintFilter.hpp"
00010 #include <pluginlib/class_list_macros.h>
00011 #include <ros/ros.h>
00012
00013
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
00065 mapOut = mapIn;
00066 mapOut.add(outputLayer_);
00067
00068
00069
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 }
00098
00099 PLUGINLIB_EXPORT_CLASS(grid_map::InpaintFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)