00001 #include "SegmentedColorGenerator.h"
00002 #include "Events/EventRouter.h"
00003 #include "Events/SegmentedColorFilterBankEvent.h"
00004 #include "Wireless/Wireless.h"
00005 #include "Shared/WorldState.h"
00006
00007 #include "Shared/debuget.h"
00008
00009 SegmentedColorGenerator::SegmentedColorGenerator(EventBase::EventGeneratorID_t gid, unsigned int sid, unsigned int mysid)
00010 : FilterBankGenerator("SegmentedColorGenerator",gid,sid,EventBase::visSegmentEGID,mysid), srcYChan(0), srcUChan(1), srcVChan(2), tmaps(), tmapNames(), numColors(0), colorNames()
00011 { }
00012
00013 SegmentedColorGenerator::SegmentedColorGenerator(EventBase::EventGeneratorID_t gid, unsigned int sid, unsigned int mysid, unsigned int syc, unsigned int suc, unsigned int svc)
00014 : FilterBankGenerator("SegmentedColorGenerator",gid,sid,EventBase::visSegmentEGID,mysid), srcYChan(syc), srcUChan(suc), srcVChan(svc), tmaps(), tmapNames(), numColors(0), colorNames()
00015 { }
00016
00017 SegmentedColorGenerator::~SegmentedColorGenerator() {
00018 freeCaches();
00019 destruct();
00020 for(unsigned int i=0; i<tmaps.size(); i++)
00021 delete [] tmaps[i];
00022
00023
00024
00025
00026
00027
00028 colorNames.clear();
00029 }
00030
00031 void
00032 SegmentedColorGenerator::processEvent(const EventBase& event) {
00033 FilterBankGenerator::processEvent(event);
00034 if(event.getGeneratorID()==getListenGeneratorID() && event.getSourceID()==getListenSourceID())
00035 erouter->postEvent(new SegmentedColorFilterBankEvent(this,getGeneratorID(),getSourceID(),this,getNumColors(),getColors(),&colorNames));
00036 }
00037
00038 unsigned int
00039 SegmentedColorGenerator::loadThresholdMap(const std::string& tm_file) {
00040 const unsigned int size = 1 << (BITS_Y + BITS_U + BITS_V);
00041 cmap_t * tmap = new cmap_t[size];
00042 if(!CMVision::LoadThresholdFile(tmap,NUM_Y,NUM_U,NUM_V,tm_file.c_str())) {
00043 serr->printf(" ERROR: Could not load threshold file '%s'.\n",tm_file.c_str());
00044 delete [] tmap;
00045 return -1U;
00046 }
00047 if(numColors!=0) {
00048
00049 int remapped=CMVision::CheckTMapColors(tmap,NUM_Y,NUM_U,NUM_V,numColors,0);
00050 if(remapped>0)
00051 serr->printf("remapped %d colors in threshold file '%s'\n",remapped, tm_file.c_str());
00052 }
00053
00054 tmapNames.push_back(tm_file);
00055 tmaps.push_back(tmap);
00056 setNumImages(numLayers,tmaps.size());
00057 return tmaps.size()-1;
00058 }
00059
00060 bool
00061 SegmentedColorGenerator::loadColorInfo(const std::string& col_file) {
00062
00063
00064
00065
00066
00067 colorNames.clear();
00068
00069 numColors=CMVision::LoadColorInformation(colors,MAX_COLORS,col_file.c_str(),colorNames);
00070 if(numColors > 0){
00071 sout->printf(" Loaded %d colors.\n",numColors);
00072 } else {
00073 serr->printf(" ERROR: Could not load colors file:%s\n", col_file.c_str());
00074 return false;
00075 }
00076
00077
00078 for(unsigned int i=0; i<tmaps.size(); i++) {
00079 int remapped=CMVision::CheckTMapColors(tmaps[i],NUM_Y,NUM_U,NUM_V,numColors,0);
00080 if(remapped>0)
00081 serr->printf("remapped %d colors in threshold file '%s'\n",remapped, tmapNames[i].c_str());
00082 }
00083 return true;
00084 }
00085
00086
00087 unsigned int
00088 SegmentedColorGenerator::getBinSize() const {
00089 unsigned int used=FilterBankGenerator::getBinSize();
00090 used+=strlen("SegColorImage")+LoadSave::stringpad;
00091 used+=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00092 return used;
00093 }
00094
00095 unsigned int
00096 SegmentedColorGenerator::LoadBuffer(const char buf[], unsigned int len) {
00097 unsigned int origlen=len;
00098 unsigned int used;
00099 std::string tmp;
00100 if(0==(used=FilterBankGenerator::LoadBuffer(buf,len))) return 0;
00101 len-=used; buf+=used;
00102 if(0==(used=decode(tmp,buf,len))) return 0;
00103 len-=used; buf+=used;
00104 if(tmp!="SegColorImage") {
00105 serr->printf("Unhandled image type for SegmentedColorGenerator: %s",tmp.c_str());
00106 return 0;
00107 } else {
00108
00109 used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00110 if(used>len)
00111 return 0;
00112 if(images[selectedSaveLayer][selectedSaveChannel]==NULL)
00113 images[selectedSaveLayer][selectedSaveChannel]=createImageCache(selectedSaveLayer,selectedSaveChannel);
00114 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00115 if(img==NULL)
00116 return 0;
00117 memcpy(img,buf,used);
00118 len-=used; buf+=used;
00119
00120
00121 if(0==(used=decodeColors(buf,len))) return 0;
00122 len-=used; buf+=used;
00123
00124 imageValids[selectedSaveLayer][selectedSaveChannel]=true;
00125 return origlen-len;
00126 }
00127 }
00128
00129 unsigned int
00130 SegmentedColorGenerator::SaveBuffer(char buf[], unsigned int len) const {
00131 unsigned int origlen=len;
00132 unsigned int used;
00133 if(0==(used=FilterBankGenerator::SaveBuffer(buf,len))) return 0;
00134 len-=used; buf+=used;
00135 if(0==(used=encode("SegColorImage",buf,len))) return 0;
00136 len-=used; buf+=used;
00137
00138
00139 used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00140 if(used>len)
00141 return 0;
00142 unsigned char* img=getImage(selectedSaveLayer,selectedSaveChannel);
00143 if(img==NULL)
00144 return 0;
00145 memcpy(buf,img,used);
00146 len-=used; buf+=used;
00147
00148
00149 if(0==(used=encodeColors(buf,len))) return 0;
00150 len-=used; buf+=used;
00151
00152 return origlen-len;
00153 }
00154
00155 unsigned int
00156 SegmentedColorGenerator::encodeColors(char buf[], unsigned int len) const {
00157 unsigned int origlen=len;
00158 unsigned int used;
00159 if(0==(used=encode(numColors,buf,len))) return 0;
00160 len-=used; buf+=used;
00161 for(unsigned int i=0; i<numColors; i++) {
00162 if(0==(used=encode(colors[i].color.red,buf,len))) return 0;
00163 len-=used; buf+=used;
00164 if(0==(used=encode(colors[i].color.green,buf,len))) return 0;
00165 len-=used; buf+=used;
00166 if(0==(used=encode(colors[i].color.blue,buf,len))) return 0;
00167 len-=used; buf+=used;
00168 }
00169 return origlen-len;
00170 }
00171
00172 unsigned int
00173 SegmentedColorGenerator::decodeColors(const char buf[], unsigned int len) {
00174 unsigned int origlen=len;
00175 unsigned int used;
00176 if(0==(used=decode(numColors,buf,len))) return 0;
00177 len-=used; buf+=used;
00178 for(unsigned int i=0; i<numColors; i++) {
00179 if(0==(used=decode(colors[i].color.red,buf,len))) return 0;
00180 len-=used; buf+=used;
00181 if(0==(used=decode(colors[i].color.green,buf,len))) return 0;
00182 len-=used; buf+=used;
00183 if(0==(used=decode(colors[i].color.blue,buf,len))) return 0;
00184 len-=used; buf+=used;
00185 }
00186 return origlen-len;
00187 }
00188
00189 void
00190 SegmentedColorGenerator::setDimensions() {
00191 FilterBankGenerator::setDimensions();
00192 for(unsigned int i=0; i<numLayers; i++)
00193 strides[i]=widths[i];
00194 }
00195
00196 void
00197 SegmentedColorGenerator::setNumImages(unsigned int nLayers, unsigned int ) {
00198 FilterBankGenerator::setNumImages(nLayers,tmaps.size());
00199 }
00200
00201 unsigned char *
00202 SegmentedColorGenerator::createImageCache(unsigned int layer, unsigned int ) const {
00203 return new unsigned char[widths[layer]*heights[layer]];
00204 }
00205
00206 void
00207 SegmentedColorGenerator::calcImage(unsigned int layer, unsigned int chan) const {
00208 PROFSECTION("SegmentedColorGenerator::calcImage(...)",state->mainProfile);
00209 CMVision::image_yuv<const cmap_t> img;
00210 img.buf_y=src->getImage(layer,srcYChan);
00211 img.buf_u=src->getImage(layer,srcUChan);
00212 img.buf_v=src->getImage(layer,srcVChan);
00213 img.width=getWidth(layer);
00214 img.height=getHeight(layer);
00215 img.row_stride=src->getStride(layer);
00216
00217 CMVision::ThresholdImageYUVPlanar<cmap_t,CMVision::image_yuv<const cmap_t>,const cmap_t,BITS_Y,BITS_U,BITS_V>(images[layer][chan],img,tmaps[chan]);
00218 imageValids[layer][chan]=true;
00219 }
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232