GridMapVisualizationHelpers.cpp
Go to the documentation of this file.
1 /*
2  * GridMapVisualizationHelpers.cpp
3  *
4  * Created on: Jun 24, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
13 
14 using namespace Eigen;
15 
16 namespace grid_map_visualization {
17 
18 void getColorMessageFromColorVector(std_msgs::ColorRGBA& colorMessage, const Eigen::Vector3f& colorVector, bool resetTransparency)
19 {
20  colorMessage.r = colorVector(0);
21  colorMessage.g = colorVector(1);
22  colorMessage.b = colorVector(2);
23  if (resetTransparency) colorMessage.a = 1.0;
24 }
25 
26 void getColorVectorFromColorMessage(Eigen::Vector3f& colorVector, const std_msgs::ColorRGBA& colorMessage)
27 {
28  colorVector << colorMessage.r, colorMessage.g, colorMessage.b;
29 }
30 
31 void setColorFromColorValue(std_msgs::ColorRGBA& color, const unsigned long& colorValue, bool resetTransparency)
32 {
33  Vector3f colorVector;
34  grid_map::colorValueToVector(colorValue, colorVector);
35  getColorMessageFromColorVector(color, colorVector, resetTransparency);
36 }
37 
38 void setColorChannelFromValue(float& colorChannel, const double value, const double lowerValueBound,
39  const double upperValueBound, const bool invert, const double colorChannelLowerValue,
40  const double colorChannelUpperValue)
41 {
42  float tempColorChannelLowerValue = colorChannelLowerValue;
43  float tempColorChannelUpperValue = colorChannelUpperValue;
44 
45  if (invert)
46  {
47  tempColorChannelLowerValue = colorChannelUpperValue;
48  tempColorChannelUpperValue = colorChannelLowerValue;
49  }
50 
51  colorChannel = static_cast<float>(computeLinearMapping(value, lowerValueBound, upperValueBound, tempColorChannelLowerValue, tempColorChannelUpperValue));
52 }
53 
54 void interpolateBetweenColors(std_msgs::ColorRGBA& color, const std_msgs::ColorRGBA& colorForLowerValue,
55  const std_msgs::ColorRGBA& colorForUpperValue, const double value,
56  const double lowerValueBound, const double upperValueBound)
57 {
58  setColorChannelFromValue(color.r, value, lowerValueBound, upperValueBound, false, colorForLowerValue.r, colorForUpperValue.r);
59  setColorChannelFromValue(color.g, value, lowerValueBound, upperValueBound, false, colorForLowerValue.g, colorForUpperValue.g);
60  setColorChannelFromValue(color.b, value, lowerValueBound, upperValueBound, false, colorForLowerValue.b, colorForUpperValue.b);
61 }
62 
63 void setSaturationFromValue(std_msgs::ColorRGBA& color, const double value, const double /*lowerValueBound*/,
64  const double upperValueBound, const double maxSaturation, const double minSaturation)
65 {
66  // Based on "changeSaturation" function by Darel Rex Finley.
67  const Eigen::Array3f HspFactors(.299, .587, .114); // see http://alienryderflex.com/hsp.html
68  const float saturationChange{static_cast<float>(computeLinearMapping(value, value, upperValueBound, maxSaturation, minSaturation))};
69  Vector3f colorVector;
70  getColorVectorFromColorMessage(colorVector, color);
71  float perceivedBrightness = sqrt((colorVector.array().square() * HspFactors).sum());
72  colorVector = perceivedBrightness + saturationChange * (colorVector.array() - perceivedBrightness);
73  colorVector = (colorVector.array().min(Array3f::Ones())).matrix();
74  getColorMessageFromColorVector(color, colorVector, false);
75 }
76 
77 void setColorFromValue(std_msgs::ColorRGBA& color, const double value, const double lowerValueBound, const double upperValueBound)
78 {
79  Vector3f hsl; // Hue: [0, 2 Pi], Saturation and Lightness: [0, 1]
80  Vector3f rgb;
81 
82  hsl[0] = static_cast<float>(computeLinearMapping(value, lowerValueBound, upperValueBound, 0.0, 2.0 * M_PI));
83  hsl[1] = 1.0;
84  hsl[2] = 1.0;
85 
86  float offset = 2.0 / 3.0 * M_PI;
87  Array3f rgbOffset(0, -offset, offset);
88  rgb = ((rgbOffset + hsl[0]).cos() + 0.5).min(Array3f::Ones()).max(Array3f::Zero()) * hsl[2];
89  float white = Vector3f(0.3, 0.59, 0.11).transpose() * rgb;
90  float saturation = 1.0 - hsl[1];
91  rgb = rgb + ((-rgb.array() + white) * saturation).matrix();
92 
93  getColorMessageFromColorVector(color, rgb, false);
94 }
95 
97  const double& sourceValue, const double& sourceLowerValue, const double& sourceUpperValue,
98  const double& mapLowerValue, const double& mapUpperValue)
99 {
100  double m = (mapLowerValue - mapUpperValue) / (sourceLowerValue - sourceUpperValue);
101  double b = mapUpperValue - m * sourceUpperValue;
102  double mapValue = m * sourceValue + b;
103  if (mapLowerValue < mapUpperValue)
104  {
105  mapValue = std::max(mapValue, mapLowerValue);
106  mapValue = std::min(mapValue, mapUpperValue);
107  }
108  else
109  {
110  mapValue = std::min(mapValue, mapLowerValue);
111  mapValue = std::max(mapValue, mapUpperValue);
112  }
113  return mapValue;
114 }
115 
116 } /* namespace */
void interpolateBetweenColors(std_msgs::ColorRGBA &color, const std_msgs::ColorRGBA &colorForLowerValue, const std_msgs::ColorRGBA &colorForUpperValue, const double value, const double lowerValueBound, const double upperValueBound)
void setSaturationFromValue(std_msgs::ColorRGBA &color, const double value, const double lowerValueBound, const double upperValueBound, const double maxSaturation, const double minSaturation)
void setColorFromValue(std_msgs::ColorRGBA &color, const double value, const double lowerValueBound, const double upperValueBound)
void setColorChannelFromValue(float &colorChannel, const double value, const double lowerValueBound, const double upperValueBound, const bool invert=false, const double colorChannelLowerValue=0.0, const double colorChannelUpperValue=1.0)
void getColorVectorFromColorMessage(Eigen::Vector3f &colorVector, const std_msgs::ColorRGBA &colorMessage)
void setColorFromColorValue(std_msgs::ColorRGBA &color, const unsigned long &colorValue, bool resetTransparency=true)
bool colorValueToVector(const unsigned long &colorValue, Eigen::Vector3i &colorVector)
void getColorMessageFromColorVector(std_msgs::ColorRGBA &colorMessage, const Eigen::Vector3f &colorVector, bool resetTransparency=true)
double computeLinearMapping(const double &sourceValue, const double &sourceLowerValue, const double &sourceUpperValue, const double &mapLowerValue, const double &mapUpperValue)


grid_map_visualization
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:51