Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <grid_map_filters/CurvatureFilter.hpp>
00010
00011 #include <grid_map_core/grid_map_core.hpp>
00012 #include <pluginlib/class_list_macros.h>
00013
00014 #include <Eigen/Dense>
00015
00016 using namespace filters;
00017
00018 namespace grid_map {
00019
00020 template<typename T>
00021 CurvatureFilter<T>::CurvatureFilter()
00022 {
00023 }
00024
00025 template<typename T>
00026 CurvatureFilter<T>::~CurvatureFilter()
00027 {
00028 }
00029
00030 template<typename T>
00031 bool CurvatureFilter<T>::configure()
00032 {
00033 if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
00034 ROS_ERROR("Curvature filter did not find parameter `input_layer`.");
00035 return false;
00036 }
00037 ROS_DEBUG("Curvature filter input layer is = %s.", inputLayer_.c_str());
00038
00039 if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
00040 ROS_ERROR("Curvature filter did not find parameter `output_layer`.");
00041 return false;
00042 }
00043 ROS_DEBUG("Curvature filter output_layer = %s.", outputLayer_.c_str());
00044
00045 return true;
00046 }
00047
00048 template<typename T>
00049 bool CurvatureFilter<T>::update(const T& mapIn, T& mapOut)
00050 {
00051 if (!mapIn.isDefaultStartIndex()) throw std::runtime_error(
00052 "CurvatureFilter cannot be used with grid maps that don't have a default buffer start index.");
00053
00054 mapOut = mapIn;
00055 mapOut.add(outputLayer_);
00056 auto& input = mapOut[inputLayer_];
00057 auto& curvature = mapOut[outputLayer_];
00058 const float L2 = mapOut.getResolution() * mapOut.getResolution();
00059
00060 for (size_t j = 0; j < input.cols(); ++j) {
00061 for (size_t i = 0; i < input.rows(); ++i) {
00062
00063 if (!std::isfinite(input(i, j))) continue;
00064 float D = ((input(i, j==0 ? j : j-1) + input(i, j==input.cols()-1 ? j : j + 1)) / 2.0 - input(i, j)) / L2;
00065 float E = ((input(i==0 ? i : i-1, j) + input(i==input.rows()-1 ? i : i + 1, j)) / 2.0 - input(i, j)) / L2;
00066 if (!std::isfinite(D)) D = 0.0;
00067 if (!std::isfinite(E)) E = 0.0;
00068 curvature(i, j) = -2.0 * (D + E);
00069 }
00070 }
00071
00072 return true;
00073 }
00074
00075 }
00076
00077 PLUGINLIB_EXPORT_CLASS(grid_map::CurvatureFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)