ColorBlendingFilter.cpp
Go to the documentation of this file.
1 /*
2  * ColorBlendingFilter.cpp
3  *
4  * Created on: Sep 14, 2017
5  * Author: Peter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
13 
14 #include <Eigen/Dense>
15 #include <math.h>
16 
17 using namespace filters;
18 
19 namespace grid_map {
20 
21 template<typename T>
23  : opacity_(1.0),
24  blendMode_(BlendModes::Normal)
25 {
26 }
27 
28 template<typename T>
30 {
31 }
32 
33 template<typename T>
35 {
36  if (!FilterBase < T > ::getParam(std::string("background_layer"), backgroundLayer_)) {
37  ROS_ERROR("Color blending filter did not find parameter `background_layer`.");
38  return false;
39  }
40  ROS_DEBUG("Color blending filter background layer is = %s.", backgroundLayer_.c_str());
41 
42  if (!FilterBase < T > ::getParam(std::string("foreground_layer"), foregroundLayer_)) {
43  ROS_ERROR("Color blending filter did not find parameter `foreground_layer`.");
44  return false;
45  }
46  ROS_DEBUG("Color blending filter foreground layer is = %s.", foregroundLayer_.c_str());
47 
48  std::string blendMode;
49  if (!FilterBase < T > ::getParam(std::string("blend_mode"), blendMode)) {
50  blendMode = "normal";
51  }
52  ROS_DEBUG("Color blending filter blend mode is = %s.", blendMode.c_str());
53  if (blendMode == "normal") blendMode_ = BlendModes::Normal;
54  else if (blendMode == "hard_light") blendMode_ = BlendModes::HardLight;
55  else if (blendMode == "soft_light") blendMode_ = BlendModes::SoftLight;
56  else {
57  ROS_ERROR("Color blending filter blend mode `%s` does not exist.", blendMode.c_str());
58  return false;
59  }
60 
61  if (!FilterBase < T > ::getParam(std::string("opacity"), opacity_)) {
62  ROS_ERROR("Color blending filter did not find parameter `opacity`.");
63  return false;
64  }
65  ROS_DEBUG("Color blending filter opacity is = %f.", opacity_);
66 
67  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
68  ROS_ERROR("Color blending filter did not find parameter `output_layer`.");
69  return false;
70  }
71  ROS_DEBUG("Color blending filter output_layer = %s.", outputLayer_.c_str());
72  return true;
73 }
74 
75 template<typename T>
76 bool ColorBlendingFilter<T>::update(const T& mapIn, T& mapOut)
77 {
78  const auto& background = mapIn[backgroundLayer_];
79  const auto& foreground = mapIn[foregroundLayer_];
80 
81  mapOut = mapIn;
82  mapOut.add(outputLayer_);
83  auto& output = mapOut[outputLayer_];
84 
85  // For each cell in map.
86  for (size_t i = 0; i < output.size(); ++i) {
87  if (std::isnan(background(i))) {
88  output(i) = foreground(i);
89  } else if (std::isnan(foreground(i))) {
90  output(i) = background(i);
91  } else {
92  Eigen::Array3f backgroundColor, foregroundColor, outputColor;
93  Eigen::Vector3f color;
94  colorValueToVector(background(i), color);
95  backgroundColor = color.array();
96  colorValueToVector(foreground(i), color);
97  foregroundColor = color.array();
98 
99  switch (blendMode_) {
100  case BlendModes::Normal:
101  outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * foregroundColor;
102  break;
104  {
105  Eigen::Array3f blendedColor;
106  if (foregroundColor.mean() < 0.5) {
107  blendedColor = 2.0 * backgroundColor * foregroundColor;
108  } else {
109  blendedColor = 1.0 - 2.0 * (1.0 - backgroundColor) * (1.0 - foregroundColor);
110  }
111  if (opacity_ == 1.0) {
112  outputColor = blendedColor;
113  } else {
114  outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * blendedColor;
115  }
116 
117  break;
118  }
120  {
121  Eigen::Array3f blendedColor;
122  // Photoshop.
123 // if (foregroundColor.mean() < 0.5) {
124 // blendedColor = 2.0 * backgroundColor * foregroundColor + backgroundColor.square() * (1.0 - 2.0 * foregroundColor);
125 // } else {
126 // blendedColor = 2.0 * backgroundColor * (1.0 - foregroundColor) + backgroundColor.sqrt() * (2.0 * foregroundColor - 1.0);
127 // }
128  // Pegtop.
129  blendedColor = ((1.0 - 2.0 * foregroundColor) * backgroundColor.square() + 2.0 * backgroundColor * foregroundColor);
130  if (opacity_ == 1.0) {
131  outputColor = blendedColor;
132  } else {
133  outputColor = (1.0 - opacity_) * backgroundColor + opacity_ * blendedColor;
134  }
135 
136  break;
137  }
138  }
139 
140  colorVectorToValue(Eigen::Vector3f(outputColor), output(i));
141  }
142  }
143 
144  return true;
145 }
146 
147 } /* namespace */
148 
BlendModes blendMode_
Blend mode.
std::string backgroundLayer_
Input layers.
virtual bool update(const T &mapIn, T &mapOut)
double opacity_
Opacity of foreground layer.
std::string outputLayer_
Output layer name.
bool colorVectorToValue(const Eigen::Vector3i &colorVector, unsigned long &colorValue)
bool colorValueToVector(const unsigned long &colorValue, Eigen::Vector3i &colorVector)
#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