00001 #include "CDTGenerator.h"
00002 #include "Events/DataEvent.h"
00003 #include "Events/EventRouter.h"
00004 #include "Events/SegmentedColorFilterBankEvent.h"
00005 #include "Wireless/Wireless.h"
00006 #include "Shared/Config.h"
00007 #include "Shared/Profiler.h"
00008 #include "Shared/ProjectInterface.h"
00009
00010 #include "Shared/ODataFormats.h"
00011 #include "OFbkImage.h"
00012
00013 #include "Shared/debuget.h"
00014
00015 CDTGenerator::CDTGenerator(unsigned int numRawLayers, unsigned int numCalcLayers, unsigned int mysid, EventBase::EventGeneratorID_t gid, unsigned int sid)
00016 : FilterBankGenerator("CDTGenerator","CDTGenerator",EventBase::visSegmentEGID,mysid,gid,sid), numRealLayers(numRawLayers), layers(NULL), imageInfos(NULL)
00017 {
00018
00019
00020
00021
00022
00023 unsetAutoListen();
00024
00025 setNumImages(numCalcLayers,NUM_CHANNELS);
00026 }
00027
00028
00029
00030
00031
00032 void
00033 CDTGenerator::processEvent(const EventBase& event) {
00034 EventGeneratorBase::processEvent(event);
00035 if(event.getGeneratorID()!=getListenGeneratorID() || event.getSourceID()!=getListenSourceID())
00036 return;
00037 if(event.getTypeID()==EventBase::activateETID) {
00038 typedef DataEvent<const OFbkImageVectorData*> OFbkEvent;
00039 const OFbkEvent& fbkevent=dynamic_cast<const OFbkEvent& >(event);
00040 OFbkImageVectorData& fbkdat=*const_cast<OFbkImageVectorData*>(fbkevent.getData());
00041 for(unsigned int res=0; res<numRealLayers; res++) {
00042 layers[numLayers-1-res] = fbkdat.GetData(res);
00043 imageInfos[numLayers-1-res] = fbkdat.GetInfo(res);
00044 }
00045 {
00046 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[numLayers-2]), const_cast<unsigned char*>(layers[numLayers-2]), ofbkimageBAND_CDT);
00047
00048 sysFrameNumber=frameNumber=*(int*)(img.Pointer()+(img.Height()-1)*(img.Skip()+img.Width()));
00049 }
00050 unsigned int numNotRealLayers=numLayers-numRealLayers;
00051 bool dimchange=false;
00052 for(unsigned int res=numNotRealLayers; res<numLayers; res++) {
00053 if(widths[res]!=imageInfos[res]->width || heights[res]!=imageInfos[res]->height) {
00054 dimchange=true;
00055 serr->printf("WARNING: the image dimensions changed, now %dx%d\n",widths[numLayers-1],heights[numLayers-1]);
00056 widths[res] = imageInfos[res]->width;
00057 heights[res] = imageInfos[res]->height;
00058 }
00059
00060 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[res]), const_cast<unsigned char*>(layers[res]), ofbkimageBAND_CDT);
00061 skips[res]=img.Skip();
00062 strides[res]=skips[res]+widths[res];
00063
00064 ASSERT(static_cast<unsigned int>(img.Width())==getWidth(res),"Widths don't match");
00065 ASSERT(static_cast<unsigned int>(img.Height())==getHeight(res),"Heights don't match");
00066 }
00067 if(numNotRealLayers>0) {
00068 if(widths[numNotRealLayers-1]*2!=widths[numNotRealLayers] || heights[numNotRealLayers]*2!=heights[numNotRealLayers]) {
00069
00070
00071
00072 serr->printf("WARNING: the image dimensions don't match values predicted by RobotInfo consts, \"full\" layer now %dx%d\n",widths[ProjectInterface::fullLayer],heights[ProjectInterface::fullLayer]);
00073 dimchange=true;
00074 } else if(strides[0]==0) {
00075 dimchange=true;
00076 }
00077 }
00078 if(dimchange)
00079 setDimensions();
00080
00081 invalidateCaches();
00082 framesProcessed++;
00083 }
00084
00085 erouter->postEvent(SegmentedColorFilterBankEvent(this,getGeneratorID(),getSourceID(),event.getTypeID(),NULL,0,NULL,NULL));
00086 }
00087
00088 unsigned int
00089 CDTGenerator::getBinSize() const {
00090 unsigned int used=FilterBankGenerator::getBinSize();
00091
00092
00093 used+=strlen("CDTImage")+LoadSave::stringpad;
00094 used+=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00095 return used;
00096 }
00097
00098 unsigned int
00099 CDTGenerator::loadBuffer(const char [] , unsigned int ) {
00100
00101 serr->printf("Can't load into CDTGenerator");
00102 return 0;
00103 }
00104
00105 unsigned int
00106 CDTGenerator::saveBuffer(char buf[], unsigned int len) const {
00107 unsigned int origlen=len;
00108 unsigned int used;
00109 if(0==(used=FilterBankGenerator::saveBuffer(buf,len))) return 0;
00110 len-=used; buf+=used;
00111
00112
00113 if(0==(used=encode("CDTImage",buf,len))) return 0;
00114 len-=used; buf+=used;
00115
00116 if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00117 serr->printf("CDTImage::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00118 return 0;
00119 }
00120 if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00121 serr->printf("CDTImage::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00122 return 0;
00123 }
00124 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00125 used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00126 if(used>len)
00127 return 0;
00128 unsigned int inc=getIncrement(selectedSaveLayer);
00129 if(inc==1) {
00130
00131 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00132 unsigned char* const rowend=img+widths[selectedSaveLayer];
00133 while(img!=rowend)
00134 *buf++=*img++;
00135 img+=getSkip(selectedSaveLayer);
00136 }
00137 } else {
00138
00139 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00140 unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00141 while(img!=rowend) {
00142 *buf++=*img;
00143 img+=inc;
00144 }
00145 img+=getSkip(selectedSaveLayer);
00146 }
00147 }
00148 len-=used;
00149
00150 return origlen-len;
00151 }
00152
00153 void
00154 CDTGenerator::setDimensions() {
00155 freeCaches();
00156 unsigned int numNotRealLayers=numLayers-numRealLayers;
00157 for(unsigned int res=0; res<numNotRealLayers; res++) {
00158 widths[res] = imageInfos[numNotRealLayers]->width>>(numNotRealLayers-res);
00159 heights[res] = imageInfos[numNotRealLayers]->height>>(numNotRealLayers-res);
00160 ASSERT(widths[res]*increments[res]==widths[numNotRealLayers],"widths*increments doesn't match total width");
00161 strides[res]=strides[numNotRealLayers]*increments[res];
00162 skips[res]=skips[numNotRealLayers]+strides[res]-strides[numNotRealLayers];
00163 }
00164 strides[numLayers-1]=widths[numLayers-1]=widths[numLayers-2]*2;
00165 heights[numLayers-1]=heights[numLayers-2]*2;
00166 }
00167
00168 void
00169 CDTGenerator::freeCaches() {
00170 for(unsigned int i=0; i<numLayers; i++) {
00171 for(unsigned int j=0; j<numChannels; j++) {
00172 images[i][j]=NULL;
00173 imageValids[i][j]=false;
00174 }
00175 }
00176 FilterBankGenerator::freeCaches();
00177 }
00178
00179 void
00180 CDTGenerator::setNumImages(unsigned int nLayers, unsigned int nChannels) {
00181 if(nLayers==numLayers && nChannels==numChannels)
00182 return;
00183 FilterBankGenerator::setNumImages(nLayers,nChannels);
00184 layers=new unsigned char*[numLayers];
00185 imageInfos=new const OFbkImageInfo*[numLayers];
00186 unsigned int numNotRealLayers=numLayers-numRealLayers;
00187 for(unsigned int res=0; res<numLayers; res++) {
00188 layers[res]=NULL;
00189 imageInfos[res]=NULL;
00190 if(res<numNotRealLayers)
00191 increments[res]=1<<(numNotRealLayers-res);
00192 }
00193 }
00194
00195 unsigned char *
00196 CDTGenerator::createImageCache(unsigned int , unsigned int ) const {
00197 return NULL;
00198 }
00199
00200 void
00201 CDTGenerator::calcImage(unsigned int layer, unsigned int chan) {
00202 PROFSECTION("CDTGenerator::calcImage(...)",*mainProfiler);
00203 unsigned int numNotRealLayers=numLayers-numRealLayers;
00204 if(layer>=numNotRealLayers) {
00205 unsigned int fbkdatChan=ofbkimageBAND_CDT;
00206 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[layer]), const_cast<unsigned char*>(layers[layer]), fbkdatChan);
00207 images[layer][chan]=img.Pointer();
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242 } else {
00243
00244
00245 unsigned int fbkdatChan=ofbkimageBAND_CDT;
00246 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[numNotRealLayers]), const_cast<unsigned char*>(layers[numNotRealLayers]), fbkdatChan);
00247 images[layer][chan]=img.Pointer();
00248 }
00249 imageValids[layer][chan]=true;
00250 }
00251
00252 void
00253 CDTGenerator::destruct() {
00254 FilterBankGenerator::destruct();
00255 delete [] layers;
00256 layers=NULL;
00257 delete [] imageInfos;
00258 imageInfos=NULL;
00259 }
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271