SlidingWindowMathExpressionFilter.cpp
Go to the documentation of this file.
1 /*
2  * SlidingWindowMathExpressionFilter.cpp
3  *
4  * Created on: Aug 18, 2017
5  * Author: Peter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
12 
13 using namespace filters;
14 
15 namespace grid_map {
16 
17 template<typename T>
19 : windowSize_(3),
20  useWindowLength_(false),
21  windowLength_(0.0),
22  isComputeEmptyCells_(true),
23  edgeHandling_(SlidingWindowIterator::EdgeHandling::INSIDE)
24 {
25 }
26 
27 template<typename T>
29 {
30 }
31 
32 template<typename T>
34 {
35  if (!FilterBase<T>::getParam(std::string("input_layer"), inputLayer_)) {
36  ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'input_layer'.");
37  return false;
38  }
39 
40  if (!FilterBase<T>::getParam(std::string("output_layer"), outputLayer_)) {
41  ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'output_layer'.");
42  return false;
43  }
44 
45  if (!FilterBase<T>::getParam(std::string("expression"), expression_)) {
46  ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'expression'.");
47  return false;
48  }
49 
50  if (!FilterBase<T>::getParam(std::string("window_size"), windowSize_)) {
51  if (FilterBase<T>::getParam(std::string("window_length"), windowLength_)) {
52  useWindowLength_ = true;
53  }
54  }
55 
56  if (!FilterBase<T>::getParam(std::string("compute_empty_cells"), isComputeEmptyCells_)) {
57  ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'compute_empty_cells'.");
58  return false;
59  }
60 
61  std::string edgeHandlingMethod;
62  if (!FilterBase<T>::getParam(std::string("edge_handling"), edgeHandlingMethod)) {
63  ROS_ERROR("SlidingWindowMathExpressionFilter did not find parameter 'edge_handling'.");
64  return false;
65  }
66  if (edgeHandlingMethod == "inside") edgeHandling_ = SlidingWindowIterator::EdgeHandling::INSIDE;
67  else if (edgeHandlingMethod == "crop") edgeHandling_ = SlidingWindowIterator::EdgeHandling::CROP;
68  else if (edgeHandlingMethod == "empty") edgeHandling_ = SlidingWindowIterator::EdgeHandling::EMPTY;
69  else if (edgeHandlingMethod == "mean") edgeHandling_ = SlidingWindowIterator::EdgeHandling::MEAN;
70  else {
71  ROS_ERROR("SlidingWindowMathExpressionFilter did not find method '%s' for edge handling.", edgeHandlingMethod.c_str());
72  return false;
73  }
74 
75  // TODO Can we make caching work with changing shared variable?
76 // parser_.setCacheExpressions(true);
77  return true;
78 }
79 
80 template<typename T>
81 bool SlidingWindowMathExpressionFilter<T>::update(const T& mapIn, T& mapOut)
82 {
83  mapOut = mapIn;
84  mapOut.add(outputLayer_);
85  Matrix& outputData = mapOut[outputLayer_];
87  if (useWindowLength_) iterator.setWindowLength(mapIn, windowLength_);
88  for (; !iterator.isPastEnd(); ++iterator) {
89  parser_.var(inputLayer_).setLocal(iterator.getData());
91  if (result.matrix().cols() == 1 && result.matrix().rows() == 1) {
92  outputData(iterator.getLinearIndex()) = result.matrix()(0);
93  } else {
94  ROS_ERROR("SlidingWindowMathExpressionFilter could not apply filter because expression has to result in a scalar!");
95  }
96  }
97  return true;
98 }
99 
100 } /* namespace */
101 
Eigen::MatrixXf Matrix
Value< Derived > & var(const std::string &name)
Definition: EigenLab.h:163
bool isComputeEmptyCells_
If empty cells should be computed as well.
SlidingWindowIterator::EdgeHandling edgeHandling_
Edge handling method.
const Matrix getData() const
Eigen::Map< Derived > & matrix()
Definition: EigenLab.h:68
bool useWindowLength_
If window length (instead of window size) should be used.
void setWindowLength(const GridMap &gridMap, const double windowLength)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
EigenLab::Parser< Eigen::MatrixXf > parser_
EigenLab parser.
const size_t & getLinearIndex() const
Value< Derived > eval(const std::string &expression)
Definition: EigenLab.h:283


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