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