void setSaturationFromValue(std_msgs::ColorRGBA& color, const double value, const double lowerValueBound,
const double upperValueBound, const double maxSaturation, const double minSaturation)
{
// Based on "changeSaturation" function by Darel Rex Finley.
const Eigen::Array3f HspFactors(.299, .587, .114); // see http://alienryderflex.com/hsp.html
float saturationChange = static_cast<float>(computeLinearMapping(value, value, upperValueBound, maxSaturation, minSaturation));
Vector3f colorVector;
getColorVectorFromColorMessage(colorVector, color);
float perceivedBrightness = sqrt((colorVector.array().square() * HspFactors).sum());
colorVector = perceivedBrightness + saturationChange * (colorVector.array() - perceivedBrightness);
colorVector = (colorVector.array().min(Array3f::Ones())).matrix();
getColorMessageFromColorVector(color, colorVector, false);
}
void setColorChannelFromValue(float& colorChannel, const double value, const double lowerValueBound,
const double upperValueBound, const bool invert, const double colorChannelLowerValue,
const double colorChannelUpperValue)
{
float tempColorChannelLowerValue = colorChannelLowerValue;
float tempColorChannelUpperValue = colorChannelUpperValue;
if (invert)
{
tempColorChannelLowerValue = colorChannelUpperValue;
tempColorChannelUpperValue = colorChannelLowerValue;
}
colorChannel = static_cast<float>(computeLinearMapping(value, lowerValueBound, upperValueBound, tempColorChannelLowerValue, tempColorChannelUpperValue));
}
void setColorFromValue(std_msgs::ColorRGBA& color, const double value, const double lowerValueBound, const double upperValueBound)
{
Vector3f hsl; // Hue: [0, 2 Pi], Saturation and Lightness: [0, 1]
Vector3f rgb;
hsl[0] = static_cast<float>(computeLinearMapping(value, lowerValueBound, upperValueBound, 0.0, 2.0 * M_PI));
hsl[1] = 1.0;
hsl[2] = 1.0;
float offset = 2.0 / 3.0 * M_PI;
Array3f rgbOffset(0, -offset, offset);
rgb = ((rgbOffset + hsl[0]).cos() + 0.5).min(Array3f::Ones()).max(Array3f::Zero()) * hsl[2];
float white = Vector3f(0.3, 0.59, 0.11).transpose() * rgb;
float saturation = 1.0 - hsl[1];
rgb = rgb + ((-rgb.array() + white) * saturation).matrix();
getColorMessageFromColorVector(color, rgb, false);
}
void setColorChannelFromValue(
float& colorChannel,
const float value,
const float lowerValueBound,
const float upperValueBound,
const bool invert,
const float colorChannelLowerValue,
const float colorChannelUpperValue)
{
float tempColorChannelLowerValue = colorChannelLowerValue;
float tempColorChannelUpperValue = colorChannelUpperValue;
if (invert)
{
tempColorChannelLowerValue = colorChannelUpperValue;
tempColorChannelUpperValue = colorChannelLowerValue;
}
colorChannel =
computeLinearMapping(
value, lowerValueBound, upperValueBound, tempColorChannelLowerValue,
tempColorChannelUpperValue);
}