14 #include <Eigen/Dense> 36 ROS_ERROR(
"Color map filter did not find parameter `input_layer`.");
42 ROS_ERROR(
"Color map filter did not find parameter `output_layer`.");
48 ROS_ERROR(
"Color map filter did not find parameter `min/value`.");
52 ROS_ERROR(
"Color map filter did not find parameter `max/value`.");
56 std::vector<double> minColor;
58 ROS_ERROR(
"Color map filter did not find parameter `min/color`.");
61 if (minColor.size() != 3) {
62 ROS_ERROR(
"Color map filter parameter `min/color` needs to be of size 3.");
65 minColor_ << minColor[0], minColor[1], minColor[2];
67 std::vector<double> maxColor;
69 ROS_ERROR(
"Color map filter did not find parameter `max/color`.");
72 if (maxColor.size() != 3) {
73 ROS_ERROR(
"Color map filter parameter `max/color` needs to be of size 3.");
76 maxColor_ << maxColor[0], maxColor[1], maxColor[2];
93 for (Eigen::Index 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;
std::string outputLayer_
Output layer name.
std::string inputLayer_
Input layer name.
virtual ~ColorMapFilter()
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)
Eigen::Vector3f maxColor_
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)