Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

CDTGenerator.cc

Go to the documentation of this file.
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   /* As a root stage, we need to listen to all incoming image
00019    * events, even if we don't currently have listeners of our own --
00020    * This is just in case user code directly accesses a generator
00021    * and we have to retroactively go back and dig up the previous
00022    * frame */
00023   unsetAutoListen();
00024   
00025   setNumImages(numCalcLayers,NUM_CHANNELS);
00026 }
00027 
00028 /*! The const casts in this function are regretable but necessary
00029  *  since the OPEN-R OFbkImage constructor requires mutable
00030  *  arguments, even though it shouldn't be modifying the data
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       //I have to do this crazy thing because apparently img.FieldCounter() doesn't work
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         //|| widths[numLayers-2-numRealLayers]*2!=widths[numNotRealLayers]
00070         //|| heights[numLayers-2-numRealLayers]*2!=heights[numNotRealLayers]) {
00071         //set the width and height of non-real layers (since they don't match what they should be)
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   // todo - i wish we had some color infomation to pass here so we could use the event for CMVision's RLE, etc.
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   // todo - once we have color information - we could make this interoperable with SegmentedColorGenerator
00092   // by following the same serialization format
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 [] /*buf*/, unsigned int /*len*/) {
00100   //all our memory is in system controlled buffers - we probably shouldn't overwrite it...
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   // todo - once we have color information - we could make this interoperable with SegmentedColorGenerator
00112   // by following the same serialization format
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     //special case, straight copy
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     //otherwise, interleaved or subsampling
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 /*layer*/, unsigned int /*chan*/) const {
00197   return NULL; // calcImage will set the cache itself
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     //I'm not sure if this is needed for CDT images themselves - haven't used it yet
00210     //so this section is commented out for now just in case.
00211     /*
00212       //this part restores pixels over written with the CDT table and
00213       //frame count.  Yes, we are modifying the original image passed
00214       //from the system here...
00215       if(config->vision.restore_image) {
00216         const unsigned int numPix=16;
00217         if(layer==numLayers-2) {
00218           unsigned char * s=images[layer][chan]+getStride(layer)*(getHeight(layer)-2);
00219           unsigned char * d=images[layer][chan]+getStride(layer)*(getHeight(layer)-1);
00220           for(unsigned int i=0; i<numPix; i++)
00221             *d++=*s++;
00222         } else {
00223           unsigned int inc=1<<(numLayers-2-layer);
00224           unsigned char * s;
00225           //unsigned char * s=getImage(numLayers-2,chan)+getStride(numLayers-2)*(getHeight(numLayers-2)-inc);
00226           //...or an attempt to possibly avoid a trivial amount of recomputation....
00227           if(!imageValids[numLayers-2][chan]) {
00228             const OFbkImage simg(const_cast<OFbkImageInfo*>(imageInfos[numLayers-2]), const_cast<unsigned char*>(layers[numLayers-2]), fbkdatChan);
00229             s=simg.Pointer();
00230           } else {
00231             s=images[numLayers-2][chan];
00232           }
00233           s+=getStride(numLayers-2)*(getHeight(numLayers-2)-inc);
00234           unsigned char * d=images[layer][chan]+getStride(layer)*(getHeight(layer)-1);
00235           for(unsigned int i=0; i<numPix; i++) {
00236             *d++=*s;
00237             s+=inc;
00238           }
00239         }
00240       }
00241     */
00242   } else {
00243     //we don't need to do the restoration in the previous section
00244     //here because these layers skip the last row
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 /*! @file
00262  * @brief Implements CDTGenerator, which generates SegmentedColorFilterBankEvents with images provided from the system
00263  * @author ejt (Creator)
00264  *
00265  * $Author: ejt $
00266  * $Name: tekkotsu-4_0 $
00267  * $Revision: 1.14 $
00268  * $State: Exp $
00269  * $Date: 2007/06/05 21:56:00 $
00270  */
00271 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:51 2007 by Doxygen 1.5.4