LightIntensityFilter.cpp
Go to the documentation of this file.
1 /*
2  * LightIntensityFilter.cpp
3  *
4  * Created on: Sep 23, 2017
5  * Author: Peter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
13 
14 #include <Eigen/Dense>
15 
16 using namespace filters;
17 
18 namespace grid_map {
19 
20 template<typename T>
22 {
23 }
24 
25 template<typename T>
27 {
28 }
29 
30 template<typename T>
32 {
33  if (!FilterBase < T > ::getParam(std::string("input_layers_prefix"), inputLayersPrefix_)) {
34  ROS_ERROR("Light intensity filter did not find parameter `input_layers_prefix`.");
35  return false;
36  }
37  ROS_DEBUG("Light intensity filter input layers prefix is = %s.", inputLayersPrefix_.c_str());
38 
39  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
40  ROS_ERROR("Light intensity filter did not find parameter `output_layer`.");
41  return false;
42  }
43  ROS_DEBUG("Light intensity filter output_layer = %s.", outputLayer_.c_str());
44 
45  std::vector<double> lightDirection;
46  if (!FilterBase < T > ::getParam(std::string("light_direction"), lightDirection)) {
47  ROS_ERROR("Light intensity filter did not find parameter `light_direction`.");
48  return false;
49  }
50  if (lightDirection.size() != 3) {
51  ROS_ERROR("Light intensity filter parameter `light_direction` needs to be of size 3.");
52  return false;
53  }
54  lightDirection_ << lightDirection[0], lightDirection[1], lightDirection[2];
55  lightDirection_.normalize();
56  ROS_DEBUG_STREAM("Light intensity filter light_direction: " << lightDirection_.transpose() << ".");
57 
58  return true;
59 }
60 
61 template<typename T>
62 bool LightIntensityFilter<T>::update(const T& mapIn, T& mapOut)
63 {
64  const auto& normalX = mapIn[inputLayersPrefix_ + "x"];
65  const auto& normalY = mapIn[inputLayersPrefix_ + "y"];
66  const auto& normalZ = mapIn[inputLayersPrefix_ + "z"];
67 
68  mapOut = mapIn;
69  mapOut.add(outputLayer_);
70  auto& color = mapOut[outputLayer_];
71 
72  // For each cell in map.
73  for (size_t i = 0; i < color.size(); ++i) {
74  if (!std::isfinite(normalZ(i))) {
75  color(i) = NAN;
76  continue;
77  }
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;
81  colorVectorToValue(colorVector, color(i));
82  }
83 
84  return true;
85 }
86 
87 } /* namespace */
88 
bool colorVectorToValue(const Eigen::Vector3i &colorVector, unsigned long &colorValue)
#define ROS_DEBUG_STREAM(args)
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


grid_map_filters
Author(s): P├ęter Fankhauser , Martin Wermelinger
autogenerated on Fri Feb 19 2021 03:15:18