00001 #include "SegmentedColorGenerator.h"
00002 #include "Events/EventRouter.h"
00003 #include "Events/SegmentedColorFilterBankEvent.h"
00004 #include "Wireless/Wireless.h"
00005 #include "Shared/Profiler.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 SegmentedColorFilterBankEvent segev(this,getGeneratorID(),getSourceID(),EventBase::activateETID,this,getNumColors(),getColors(),&colorNames);
00048 erouter->postEvent(segev);
00049 segev.setTypeID(EventBase::statusETID);
00050 erouter->postEvent(segev);
00051 segev.setTypeID(EventBase::deactivateETID);
00052 erouter->postEvent(segev);
00053 }
00054 }
00055
00056 unsigned int
00057 SegmentedColorGenerator::loadThresholdMap(const std::string& tm_file) {
00058 const unsigned int size = 1 << (BITS_Y + BITS_U + BITS_V);
00059 cmap_t * tmap = new cmap_t[size];
00060 if(!CMVision::LoadThresholdFile(tmap,NUM_Y,NUM_U,NUM_V,config->portPath(tm_file).c_str())) {
00061 serr->printf(" ERROR: Could not load threshold file '%s'.\n",tm_file.c_str());
00062 delete [] tmap;
00063 return -1U;
00064 }
00065 if(numColors!=0) {
00066
00067 int remapped=CMVision::CheckTMapColors(tmap,NUM_Y,NUM_U,NUM_V,numColors,0);
00068 if(remapped>0)
00069 serr->printf("remapped %d colors in threshold file '%s'\n",remapped, tm_file.c_str());
00070 }
00071
00072 tmapNames.push_back(tm_file);
00073 tmaps.push_back(tmap);
00074 setNumImages(numLayers,tmaps.size());
00075 return tmaps.size()-1;
00076 }
00077
00078 bool
00079 SegmentedColorGenerator::loadColorInfo(const std::string& col_file) {
00080
00081
00082
00083
00084
00085 colorNames.clear();
00086
00087 numColors=CMVision::LoadColorInformation(colors,MAX_COLORS,config->portPath(col_file).c_str(),colorNames);
00088 if(numColors > 0){
00089 sout->printf(" Loaded %d colors.\n",numColors);
00090 } else {
00091 serr->printf(" ERROR: Could not load colors file:%s\n", col_file.c_str());
00092 return false;
00093 }
00094
00095
00096 for(unsigned int i=0; i<tmaps.size(); i++) {
00097 int remapped=CMVision::CheckTMapColors(tmaps[i],NUM_Y,NUM_U,NUM_V,numColors,0);
00098 if(remapped>0)
00099 serr->printf("remapped %d colors in threshold file '%s'\n",remapped, tmapNames[i].c_str());
00100 }
00101 return true;
00102 }
00103
00104
00105 unsigned int
00106 SegmentedColorGenerator::getBinSize() const {
00107 unsigned int used=FilterBankGenerator::getBinSize();
00108 used+=creatorSize("SegColorImage");
00109 used+=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00110 return used;
00111 }
00112
00113 unsigned int
00114 SegmentedColorGenerator::loadBuffer(const char buf[], unsigned int len) {
00115 unsigned int origlen=len;
00116 if(!checkInc(FilterBankGenerator::loadBuffer(buf,len),buf,len)) return 0;
00117 if(!checkCreatorInc("SegColorImage",buf,len)) {
00118 serr->printf("Unhandled image type for SegmentedColorGenerator: %s",buf+getSerializedSize<unsigned int>());
00119 return 0;
00120 } else {
00121
00122 unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00123 if(used>len)
00124 return 0;
00125 if(images[selectedSaveLayer][selectedSaveChannel]==NULL)
00126 images[selectedSaveLayer][selectedSaveChannel]=createImageCache(selectedSaveLayer,selectedSaveChannel);
00127 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00128 if(img==NULL)
00129 return 0;
00130 memcpy(img,buf,used);
00131 len-=used; buf+=used;
00132
00133
00134 if(!decodeColorsInc(buf,len)) return 0;
00135
00136 imageValids[selectedSaveLayer][selectedSaveChannel]=true;
00137 return origlen-len;
00138 }
00139 }
00140
00141 unsigned int
00142 SegmentedColorGenerator::saveBuffer(char buf[], unsigned int len) const {
00143 unsigned int origlen=len;
00144 if(!checkInc(FilterBankGenerator::saveBuffer(buf,len),buf,len)) return 0;
00145 if(!saveCreatorInc("SegColorImage",buf,len)) return 0;
00146
00147
00148 unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00149 if(used>len)
00150 return 0;
00151 if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00152 serr->printf("SegmentedColorGenerator::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00153 return 0;
00154 }
00155 if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00156 serr->printf("SegmentedColorGenerator::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00157 return 0;
00158 }
00159 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00160 if(img==NULL)
00161 return 0;
00162 memcpy(buf,img,used);
00163 len-=used; buf+=used;
00164
00165
00166 if(!encodeColorsInc(buf,len)) return 0;
00167
00168 return origlen-len;
00169 }
00170
00171 bool
00172 SegmentedColorGenerator::encodeColorsInc(char*& buf, unsigned int& len) const {
00173 if(!encodeInc(numColors,buf,len)) return false;
00174 for(unsigned int i=0; i<numColors; i++) {
00175 if(!encodeInc(colors[i].color.red,buf,len)) return false;
00176 if(!encodeInc(colors[i].color.green,buf,len)) return false;
00177 if(!encodeInc(colors[i].color.blue,buf,len)) return false;
00178 }
00179 return true;
00180 }
00181
00182 bool
00183 SegmentedColorGenerator::decodeColorsInc(const char*& buf, unsigned int& len) {
00184 if(!decodeInc(numColors,buf,len)) return false;
00185 for(unsigned int i=0; i<numColors; i++) {
00186 if(!decodeInc(colors[i].color.red,buf,len)) return false;
00187 if(!decodeInc(colors[i].color.green,buf,len)) return false;
00188 if(!decodeInc(colors[i].color.blue,buf,len)) return false;
00189 }
00190 return true;
00191 }
00192
00193 void
00194 SegmentedColorGenerator::setDimensions() {
00195 FilterBankGenerator::setDimensions();
00196 for(unsigned int i=0; i<numLayers; i++)
00197 strides[i]=widths[i];
00198 }
00199
00200 void
00201 SegmentedColorGenerator::setNumImages(unsigned int nLayers, unsigned int ) {
00202 FilterBankGenerator::setNumImages(nLayers,tmaps.size());
00203 }
00204
00205 unsigned char *
00206 SegmentedColorGenerator::createImageCache(unsigned int layer, unsigned int ) const {
00207
00208
00209
00210 return new unsigned char[widths[layer]*heights[layer]+1];
00211 }
00212
00213 void
00214 SegmentedColorGenerator::calcImage(unsigned int layer, unsigned int chan) {
00215 if(tmaps.size()==0)
00216 throw NoThresholdException();
00217 PROFSECTION("SegmentedColorGenerator::calcImage(...)",*mainProfiler);
00218 CMVision::image_yuv<const cmap_t> img;
00219 img.buf_y=src->getImage(layer,srcYChan);
00220 img.buf_u=src->getImage(layer,srcUChan);
00221 img.buf_v=src->getImage(layer,srcVChan);
00222 img.width=getWidth(layer);
00223 img.height=getHeight(layer);
00224 img.row_stride=src->getStride(layer);
00225 img.col_stride=src->getIncrement(layer);
00226
00227 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]);
00228 imageValids[layer][chan]=true;
00229 }
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242