00001 /* 00002 * NormalColorMapFilter.cpp 00003 * 00004 * Created on: Aug 22, 2017 00005 * Author: Peter Fankhauser 00006 * Institute: ETH Zurich, ANYbotics 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 // X: -1 to +1 : Red: 0 to 255 00059 // Y: -1 to +1 : Green: 0 to 255 00060 // Z: 0 to 1 : Blue: 128 to 255 00061 00062 // For each cell in map. 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 } /* namespace */ 00074 00075 PLUGINLIB_EXPORT_CLASS(grid_map::NormalColorMapFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)