00001 #include "wrinkled_map_alg.h" 00002 00003 /* borrar 24/2/12 00004 void init_colormap(std::vector<RGBValue>& colormap) 00005 { 00006 RGBValue a; 00007 a.Red=0; a.Green=0; a.Blue=128; colormap.push_back(a); 00008 a.Red=0; a.Green=0; a.Blue=144; colormap.push_back(a); 00009 a.Red=0; a.Green=0; a.Blue=160; colormap.push_back(a); 00010 a.Red=0; a.Green=0; a.Blue=176; colormap.push_back(a); 00011 a.Red=0; a.Green=0; a.Blue=192; colormap.push_back(a); 00012 a.Red=0; a.Green=0; a.Blue=208; colormap.push_back(a); 00013 a.Red=0; a.Green=0; a.Blue=225; colormap.push_back(a); 00014 a.Red=0; a.Green=0; a.Blue=241; colormap.push_back(a); 00015 a.Red=0; a.Green=2; a.Blue=255; colormap.push_back(a); 00016 a.Red=0; a.Green=18; a.Blue=255; colormap.push_back(a); 00017 a.Red=0; a.Green=34; a.Blue=255; colormap.push_back(a); 00018 a.Red=0; a.Green=51; a.Blue=255; colormap.push_back(a); 00019 a.Red=0; a.Green=67; a.Blue=255; colormap.push_back(a); 00020 a.Red=0; a.Green=83; a.Blue=255; colormap.push_back(a); 00021 a.Red=0; a.Green=99; a.Blue=255; colormap.push_back(a); 00022 a.Red=0; a.Green=115; a.Blue=255; colormap.push_back(a); 00023 a.Red=0; a.Green=132; a.Blue=255; colormap.push_back(a); 00024 a.Red=0; a.Green=148; a.Blue=255; colormap.push_back(a); 00025 a.Red=0; a.Green=164; a.Blue=255; colormap.push_back(a); 00026 a.Red=0; a.Green=180; a.Blue=255; colormap.push_back(a); 00027 a.Red=0; a.Green=196; a.Blue=255; colormap.push_back(a); 00028 a.Red=0; a.Green=212; a.Blue=255; colormap.push_back(a); 00029 a.Red=0; a.Green=229; a.Blue=255; colormap.push_back(a); 00030 a.Red=0; a.Green=245; a.Blue=255; colormap.push_back(a); 00031 a.Red=6; a.Green=255; a.Blue=249; colormap.push_back(a); 00032 a.Red=22; a.Green=255; a.Blue=233; colormap.push_back(a); 00033 a.Red=38; a.Green=255; a.Blue=217; colormap.push_back(a); 00034 a.Red=55; a.Green=255; a.Blue=200; colormap.push_back(a); 00035 a.Red=71; a.Green=255; a.Blue=184; colormap.push_back(a); 00036 a.Red=87; a.Green=255; a.Blue=168; colormap.push_back(a); 00037 a.Red=103; a.Green=255; a.Blue=152; colormap.push_back(a); 00038 a.Red=119; a.Green=255; a.Blue=136; colormap.push_back(a); 00039 a.Red=136; a.Green=255; a.Blue=119; colormap.push_back(a); 00040 a.Red=152; a.Green=255; a.Blue=103; colormap.push_back(a); 00041 a.Red=168; a.Green=255; a.Blue=87; colormap.push_back(a); 00042 a.Red=184; a.Green=255; a.Blue=71; colormap.push_back(a); 00043 a.Red=200; a.Green=255; a.Blue=55; colormap.push_back(a); 00044 a.Red=217; a.Green=255; a.Blue=38; colormap.push_back(a); 00045 a.Red=233; a.Green=255; a.Blue=22; colormap.push_back(a); 00046 a.Red=249; a.Green=255; a.Blue=6; colormap.push_back(a); 00047 a.Red=255; a.Green=245; a.Blue=0; colormap.push_back(a); 00048 a.Red=255; a.Green=229; a.Blue=0; colormap.push_back(a); 00049 a.Red=255; a.Green=213; a.Blue=0; colormap.push_back(a); 00050 a.Red=255; a.Green=196; a.Blue=0; colormap.push_back(a); 00051 a.Red=255; a.Green=180; a.Blue=0; colormap.push_back(a); 00052 a.Red=255; a.Green=164; a.Blue=0; colormap.push_back(a); 00053 a.Red=255; a.Green=148; a.Blue=0; colormap.push_back(a); 00054 a.Red=255; a.Green=132; a.Blue=0; colormap.push_back(a); 00055 a.Red=255; a.Green=115; a.Blue=0; colormap.push_back(a); 00056 a.Red=255; a.Green=99; a.Blue=0; colormap.push_back(a); 00057 a.Red=255; a.Green=83; a.Blue=0; colormap.push_back(a); 00058 a.Red=255; a.Green=67; a.Blue=0; colormap.push_back(a); 00059 a.Red=255; a.Green=51; a.Blue=0; colormap.push_back(a); 00060 a.Red=255; a.Green=34; a.Blue=0; colormap.push_back(a); 00061 a.Red=255; a.Green=18; a.Blue=0; colormap.push_back(a); 00062 a.Red=255; a.Green=2; a.Blue=0; colormap.push_back(a); 00063 a.Red=241; a.Green=0; a.Blue=0; colormap.push_back(a); 00064 a.Red=225; a.Green=0; a.Blue=0; colormap.push_back(a); 00065 a.Red=208; a.Green=0; a.Blue=0; colormap.push_back(a); 00066 a.Red=192; a.Green=0; a.Blue=0; colormap.push_back(a); 00067 a.Red=176; a.Green=0; a.Blue=0; colormap.push_back(a); 00068 a.Red=160; a.Green=0; a.Blue=0; colormap.push_back(a); 00069 a.Red=144; a.Green=0; a.Blue=0; colormap.push_back(a); 00070 a.Red=128; a.Green=0; a.Blue=0; colormap.push_back(a); 00071 } 00072 */ 00073 00074 WrinkledMapAlgorithm::WrinkledMapAlgorithm(void) 00075 { 00076 this->min_entropy_ = -5; 00077 } 00078 00079 WrinkledMapAlgorithm::~WrinkledMapAlgorithm(void) 00080 { 00081 } 00082 00083 void WrinkledMapAlgorithm::config_update(Config& new_cfg, uint32_t level) 00084 { 00085 this->lock(); 00086 00087 // save the current configuration 00088 this->config_=new_cfg; 00089 00090 this->unlock(); 00091 } 00092 00093 // WrinkledMapAlgorithm Public API 00094 00095 void WrinkledMapAlgorithm::compute_entropy_wrinkled_map(const normal_descriptor_node::ndesc_pc &ndesc_pc_msg, pcl::PointCloud<pcl::PointXYZRGB> &hmap) 00096 { 00097 ROS_INFO("Entering wrinkle map computer."); 00098 int idx; 00099 00100 hmap.width = ndesc_pc_msg.width; 00101 hmap.height = ndesc_pc_msg.height; 00102 hmap.points.resize(hmap.width*hmap.height); 00103 //borrar 24/2/12 hmap.points3d.resize(hmap.width*hmap.height); 00104 00105 //TODO reset vector more efficiently 00106 for(uint i=0; i<hmap.points.size(); i++) 00107 { 00108 hmap.points[i].x=std::numeric_limits<float>::quiet_NaN(); 00109 hmap.points[i].y=std::numeric_limits<float>::quiet_NaN(); 00110 hmap.points[i].z=std::numeric_limits<float>::quiet_NaN(); 00111 hmap.points[i].rgb=std::numeric_limits<float>::quiet_NaN(); 00112 } 00113 ROS_INFO("size %d %d ndesc %d", hmap.width, hmap.height, ndesc_pc_msg.descriptors.size()); 00114 for(uint i=0; i<ndesc_pc_msg.descriptors.size(); i++) 00115 { 00116 idx = ndesc_pc_msg.descriptors[i].u + ndesc_pc_msg.descriptors[i].v*hmap.width; 00117 hmap.points[idx].rgb = this->compute_entropy(ndesc_pc_msg.descriptors[i]); 00118 hmap.points[idx].x = ndesc_pc_msg.descriptors[i].point3d.x; 00119 hmap.points[idx].y = ndesc_pc_msg.descriptors[i].point3d.y; 00120 hmap.points[idx].z = ndesc_pc_msg.descriptors[i].point3d.z; 00121 } 00122 ROS_INFO("Exiting wrinkle map computer."); 00123 } 00124 00125 00126