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