14 #include <Eigen/Dense> 37 ROS_ERROR(
"Color blending filter did not find parameter `background_layer`.");
43 ROS_ERROR(
"Color blending filter did not find parameter `foreground_layer`.");
48 std::string blendMode;
52 ROS_DEBUG(
"Color blending filter blend mode is = %s.", blendMode.c_str());
57 ROS_ERROR(
"Color blending filter blend mode `%s` does not exist.", blendMode.c_str());
62 ROS_ERROR(
"Color blending filter did not find parameter `opacity`.");
68 ROS_ERROR(
"Color blending filter did not find parameter `output_layer`.");
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);
92 Eigen::Array3f backgroundColor, foregroundColor, outputColor;
93 Eigen::Vector3f color;
95 backgroundColor = color.array();
97 foregroundColor = color.array();
101 outputColor = (1.0 -
opacity_) * backgroundColor +
opacity_ * foregroundColor;
105 Eigen::Array3f blendedColor;
106 if (foregroundColor.mean() < 0.5) {
107 blendedColor = 2.0 * backgroundColor * foregroundColor;
109 blendedColor = 1.0 - 2.0 * (1.0 - backgroundColor) * (1.0 - foregroundColor);
112 outputColor = blendedColor;
121 Eigen::Array3f blendedColor;
129 blendedColor = ((1.0 - 2.0 * foregroundColor) * backgroundColor.square() + 2.0 * backgroundColor * foregroundColor);
131 outputColor = blendedColor;
BlendModes blendMode_
Blend mode.
std::string backgroundLayer_
Input layers.
virtual bool update(const T &mapIn, T &mapOut)
double opacity_
Opacity of foreground layer.
virtual ~ColorBlendingFilter()
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)
std::string foregroundLayer_