Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <grid_map_filters/NormalColorMapFilter.hpp>
00010
00011 #include <grid_map_core/grid_map_core.hpp>
00012 #include <pluginlib/class_list_macros.h>
00013
00014 #include <Eigen/Dense>
00015
00016 using namespace filters;
00017
00018 namespace grid_map {
00019
00020 template<typename T>
00021 NormalColorMapFilter<T>::NormalColorMapFilter()
00022 {
00023 }
00024
00025 template<typename T>
00026 NormalColorMapFilter<T>::~NormalColorMapFilter()
00027 {
00028 }
00029
00030 template<typename T>
00031 bool NormalColorMapFilter<T>::configure()
00032 {
00033 if (!FilterBase < T > ::getParam(std::string("input_layers_prefix"), inputLayersPrefix_)) {
00034 ROS_ERROR("Normal color map filter did not find parameter `input_layers_prefix`.");
00035 return false;
00036 }
00037 ROS_DEBUG("Normal color map filter input layers prefix is = %s.", inputLayersPrefix_.c_str());
00038
00039 if (!FilterBase < T > ::getParam(std::string("output_layer"), outputLayer_)) {
00040 ROS_ERROR("Normal color map filter did not find parameter `output_layer`.");
00041 return false;
00042 }
00043 ROS_DEBUG("Normal color map filter output_layer = %s.", outputLayer_.c_str());
00044 return true;
00045 }
00046
00047 template<typename T>
00048 bool NormalColorMapFilter<T>::update(const T& mapIn, T& mapOut)
00049 {
00050 const auto& normalX = mapIn[inputLayersPrefix_ + "x"];
00051 const auto& normalY = mapIn[inputLayersPrefix_ + "y"];
00052 const auto& normalZ = mapIn[inputLayersPrefix_ + "z"];
00053
00054 mapOut = mapIn;
00055 mapOut.add(outputLayer_);
00056 auto& color = mapOut[outputLayer_];
00057
00058
00059
00060
00061
00062
00063 for (size_t i = 0; i < color.size(); ++i) {
00064 const Eigen::Vector3f colorVector((normalX(i) + 1.0) / 2.0,
00065 (normalY(i) + 1.0) / 2.0,
00066 (normalZ(i) / 2.0) + 0.5);
00067 colorVectorToValue(colorVector, color(i));
00068 }
00069
00070 return true;
00071 }
00072
00073 }
00074
00075 PLUGINLIB_EXPORT_CLASS(grid_map::NormalColorMapFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)