ColorMapFilter.cpp
Go to the documentation of this file.
1 /*
2  * ColorMapFilter.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  : min_(0.0),
23  max_(1.0)
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("Color map filter did not find parameter `input_layer`.");
37  return false;
38  }
39  ROS_DEBUG("Color map filter input_layer = %s.", inputLayer_.c_str());
40 
41  if (!FilterBase<T>::getParam(std::string("output_layer"), outputLayer_)) {
42  ROS_ERROR("Color map filter did not find parameter `output_layer`.");
43  return false;
44  }
45  ROS_DEBUG("Color map filter output_layer = %s.", outputLayer_.c_str());
46 
47  if (!FilterBase<T>::getParam(std::string("min/value"), min_)) {
48  ROS_ERROR("Color map filter did not find parameter `min/value`.");
49  return false;
50  }
51  if (!FilterBase<T>::getParam(std::string("max/value"), max_)) {
52  ROS_ERROR("Color map filter did not find parameter `max/value`.");
53  return false;
54  }
55 
56  std::vector<double> minColor;
57  if (!FilterBase<T>::getParam(std::string("min/color"), minColor)) {
58  ROS_ERROR("Color map filter did not find parameter `min/color`.");
59  return false;
60  }
61  if (minColor.size() != 3) {
62  ROS_ERROR("Color map filter parameter `min/color` needs to be of size 3.");
63  return false;
64  }
65  minColor_ << minColor[0], minColor[1], minColor[2];
66 
67  std::vector<double> maxColor;
68  if (!FilterBase<T>::getParam(std::string("max/color"), maxColor)) {
69  ROS_ERROR("Color map filter did not find parameter `max/color`.");
70  return false;
71  }
72  if (maxColor.size() != 3) {
73  ROS_ERROR("Color map filter parameter `max/color` needs to be of size 3.");
74  return false;
75  }
76  maxColor_ << maxColor[0], maxColor[1], maxColor[2];
77 
78  return true;
79 }
80 
81 template<typename T>
82 bool ColorMapFilter<T>::update(const T& mapIn, T& mapOut)
83 {
84  mapOut = mapIn;
85  auto& input = mapIn[inputLayer_];
86  mapOut.add(outputLayer_);
87  auto& output = mapOut[outputLayer_];
88 
89  const double range = max_ - min_;
90  const Eigen::Vector3f colorRange = maxColor_ - minColor_;
91 
92  // For each cell in map.
93  for (size_t i = 0; i < output.size(); ++i) {
94  if (!std::isfinite(input(i))) continue;
95  const double value = std::min<float>(std::max<float>(input(i), min_), max_);
96  const double factor = (value - min_) / range;
97  const Eigen::Vector3f color = minColor_ + factor * colorRange;
98  colorVectorToValue(color, output(i));
99  }
100 
101  return true;
102 }
103 
104 } /* namespace */
105 
std::string outputLayer_
Output layer name.
std::string inputLayer_
Input layer name.
Eigen::Vector3f minColor_
Min./max. colors.
bool colorVectorToValue(const Eigen::Vector3i &colorVector, unsigned long &colorValue)
double min_
Min./max. values.
virtual bool update(const T &mapIn, T &mapOut)
#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