MeanInRadiusFilter.cpp
Go to the documentation of this file.
00001 /*
00002  * MeanInRadiusFilter.cpp
00003  *
00004  *  Created on: May 3, 2017
00005  *      Author: Tanja Baumann, Peter Fankhauser
00006  *   Institute: ETH Zurich, Robotic Systems Lab
00007  */
00008 
00009 #include "grid_map_filters/MeanInRadiusFilter.hpp"
00010 
00011 #include <grid_map_core/grid_map_core.hpp>
00012 #include <pluginlib/class_list_macros.h>
00013 
00014 using namespace filters;
00015 
00016 namespace grid_map {
00017 
00018 template<typename T>
00019 MeanInRadiusFilter<T>::MeanInRadiusFilter()
00020     : radius_(0.0)
00021 {
00022 }
00023 
00024 template<typename T>
00025 MeanInRadiusFilter<T>::~MeanInRadiusFilter()
00026 {
00027 }
00028 
00029 template<typename T>
00030 bool MeanInRadiusFilter<T>::configure()
00031 {
00032   if (!FilterBase < T > ::getParam(std::string("radius"), radius_)) {
00033     ROS_ERROR("MeanInRadius filter did not find parameter `radius`.");
00034     return false;
00035   }
00036 
00037   if (radius_ < 0.0) {
00038     ROS_ERROR("MeanInRadius filter: Radius must be greater than zero.");
00039     return false;
00040   }
00041 
00042   ROS_DEBUG("Radius = %f.", radius_);
00043 
00044   if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
00045     ROS_ERROR("MeanInRadius filter did not find parameter `input_layer`.");
00046     return false;
00047   }
00048 
00049   ROS_DEBUG("MeanInRadius input layer is = %s.", inputLayer_.c_str());
00050 
00051   if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
00052     ROS_ERROR("MeanInRadius filter did not find parameter `output_layer`.");
00053     return false;
00054   }
00055 
00056   ROS_DEBUG("MeanInRadius output_layer = %s.", outputLayer_.c_str());
00057   return true;
00058 }
00059 
00060 template<typename T>
00061 bool MeanInRadiusFilter<T>::update(const T& mapIn, T& mapOut)
00062 {
00063   // Add new layers to the elevation map.
00064   mapOut = mapIn;
00065   mapOut.add(outputLayer_);
00066 
00067   double value;
00068 
00069   // First iteration through the elevation map.
00070   for (grid_map::GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
00071 
00072     double valueSum = 0.0;
00073     int counter = 0;
00074     // Requested position (center) of circle in map.
00075     Eigen::Vector2d center;
00076     mapOut.getPosition(*iterator, center);
00077 
00078     // Find the mean in a circle around the center
00079     for (grid_map::CircleIterator submapIterator(mapOut, center, radius_); !submapIterator.isPastEnd();
00080         ++submapIterator) {
00081       if (!mapOut.isValid(*submapIterator, inputLayer_))
00082         continue;
00083       value = mapOut.at(inputLayer_, *submapIterator);
00084       valueSum += value;
00085       counter++;
00086     }
00087 
00088     if (counter != 0)
00089       mapOut.at(outputLayer_, *iterator) = valueSum / counter;
00090   }
00091 
00092   return true;
00093 }
00094 
00095 } /* namespace */
00096 
00097 PLUGINLIB_EXPORT_CLASS(grid_map::MeanInRadiusFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)


grid_map_filters
Author(s): Péter Fankhauser , Martin Wermelinger
autogenerated on Mon Oct 9 2017 03:09:30