14 #include <Eigen/Dense> 34 ROS_ERROR(
"Light intensity filter did not find parameter `input_layers_prefix`.");
37 ROS_DEBUG(
"Light intensity filter input layers prefix is = %s.", inputLayersPrefix_.c_str());
40 ROS_ERROR(
"Light intensity filter did not find parameter `output_layer`.");
43 ROS_DEBUG(
"Light intensity filter output_layer = %s.", outputLayer_.c_str());
45 std::vector<double> lightDirection;
47 ROS_ERROR(
"Light intensity filter did not find parameter `light_direction`.");
50 if (lightDirection.size() != 3) {
51 ROS_ERROR(
"Light intensity filter parameter `light_direction` needs to be of size 3.");
54 lightDirection_ << lightDirection[0], lightDirection[1], lightDirection[2];
55 lightDirection_.normalize();
56 ROS_DEBUG_STREAM(
"Light intensity filter light_direction: " << lightDirection_.transpose() <<
".");
64 const auto& normalX = mapIn[inputLayersPrefix_ +
"x"];
65 const auto& normalY = mapIn[inputLayersPrefix_ +
"y"];
66 const auto& normalZ = mapIn[inputLayersPrefix_ +
"z"];
69 mapOut.add(outputLayer_);
70 auto& color = mapOut[outputLayer_];
73 for (Eigen::Index i = 0; i < color.size(); ++i) {
74 if (!std::isfinite(normalZ(i))) {
78 const Eigen::Vector3f normal(normalX(i), normalY(i), normalZ(i));
79 const float intensity = std::max<float>(-normal.dot(lightDirection_), 0.0);
80 const Eigen::Vector3f colorVector = Eigen::Vector3f::Ones() * intensity;
bool colorVectorToValue(const Eigen::Vector3i &colorVector, unsigned long &colorValue)
#define ROS_DEBUG_STREAM(args)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)