CurvatureFilter.cpp
Go to the documentation of this file.
1 /*
2  * CurvatureFilter.cpp
3  *
4  * Created on: Sep 23, 2017
5  * Author: Peter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
13 
14 #include <Eigen/Dense>
15 
16 using namespace filters;
17 
18 namespace grid_map {
19 
20 template<typename T>
22 {
23 }
24 
25 template<typename T>
27 {
28 }
29 
30 template<typename T>
32 {
33  if (!FilterBase < T > ::getParam(std::string("input_layer"), inputLayer_)) {
34  ROS_ERROR("Curvature filter did not find parameter `input_layer`.");
35  return false;
36  }
37  ROS_DEBUG("Curvature filter input layer is = %s.", inputLayer_.c_str());
38 
39  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
40  ROS_ERROR("Curvature filter did not find parameter `output_layer`.");
41  return false;
42  }
43  ROS_DEBUG("Curvature filter output_layer = %s.", outputLayer_.c_str());
44 
45  return true;
46 }
47 
48 template<typename T>
49 bool CurvatureFilter<T>::update(const T& mapIn, T& mapOut)
50 {
51  if (!mapIn.isDefaultStartIndex()) throw std::runtime_error(
52  "CurvatureFilter cannot be used with grid maps that don't have a default buffer start index.");
53 
54  mapOut = mapIn;
55  mapOut.add(outputLayer_);
56  auto& input = mapOut[inputLayer_];
57  auto& curvature = mapOut[outputLayer_];
58  const float L2 = mapOut.getResolution() * mapOut.getResolution();
59 
60  for (size_t j = 0; j < input.cols(); ++j) {
61  for (size_t i = 0; i < input.rows(); ++i) {
62  // http://help.arcgis.com/en/arcgisdesktop/10.0/help/index.html#/How_Curvature_works/00q90000000t000000/
63  if (!std::isfinite(input(i, j))) continue;
64  float D = ((input(i, j==0 ? j : j-1) + input(i, j==input.cols()-1 ? j : j + 1)) / 2.0 - input(i, j)) / L2;
65  float E = ((input(i==0 ? i : i-1, j) + input(i==input.rows()-1 ? i : i + 1, j)) / 2.0 - input(i, j)) / L2;
66  if (!std::isfinite(D)) D = 0.0;
67  if (!std::isfinite(E)) E = 0.0;
68  curvature(i, j) = -2.0 * (D + E);
69  }
70  }
71 
72  return true;
73 }
74 
75 } /* namespace */
76 
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


grid_map_filters
Author(s): P├ęter Fankhauser , Martin Wermelinger
autogenerated on Fri Feb 19 2021 03:15:18