NormalColorMapFilter.cpp
Go to the documentation of this file.
1 /*
2  * NormalColorMapFilter.cpp
3  *
4  * Created on: Aug 22, 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("Normal color map filter did not find parameter `input_layers_prefix`.");
35  return false;
36  }
37  ROS_DEBUG("Normal color map filter input layers prefix is = %s.", inputLayersPrefix_.c_str());
38 
39  if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
40  ROS_ERROR("Normal color map filter did not find parameter `output_layer`.");
41  return false;
42  }
43  ROS_DEBUG("Normal color map filter output_layer = %s.", outputLayer_.c_str());
44  return true;
45 }
46 
47 template<typename T>
48 bool NormalColorMapFilter<T>::update(const T& mapIn, T& mapOut)
49 {
50  const auto& normalX = mapIn[inputLayersPrefix_ + "x"];
51  const auto& normalY = mapIn[inputLayersPrefix_ + "y"];
52  const auto& normalZ = mapIn[inputLayersPrefix_ + "z"];
53 
54  mapOut = mapIn;
55  mapOut.add(outputLayer_);
56  auto& color = mapOut[outputLayer_];
57 
58  // X: -1 to +1 : Red: 0 to 255
59  // Y: -1 to +1 : Green: 0 to 255
60  // Z: 0 to 1 : Blue: 128 to 255
61 
62  // For each cell in map.
63  for (size_t i = 0; i < color.size(); ++i) {
64  const Eigen::Vector3f colorVector((normalX(i) + 1.0) / 2.0,
65  (normalY(i) + 1.0) / 2.0,
66  (normalZ(i) / 2.0) + 0.5);
67  colorVectorToValue(colorVector, color(i));
68  }
69 
70  return true;
71 }
72 
73 } /* namespace */
74 
bool colorVectorToValue(const Eigen::Vector3i &colorVector, unsigned long &colorValue)
#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