Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

FilterBankGenerator.cc

Go to the documentation of this file.
00001 #include "FilterBankGenerator.h"
00002 #include "Events/FilterBankEvent.h"
00003 #include "Events/EventRouter.h"
00004 #include "Wireless/Socket.h"
00005 #include "Shared/RobotInfo.h"
00006 #include "Shared/ProjectInterface.h"
00007 
00008 using namespace std;
00009 
00010 unsigned int FilterBankGenerator::sysFrameNumber=-1U;
00011 
00012 unsigned char *
00013 FilterBankGenerator::getImage(unsigned int layer, unsigned int channel) {
00014   //for(int i=0;i<1000;i++)
00015   //std::cout << i << " FBG::getImage(" << layer<<'/'<<numLayers  << ',' << channel<<'/'<<numChannels << ");" << std::endl;
00016   if(!refresh())
00017     return NULL;
00018   if(!imageValids[layer][channel]) {
00019     if(images[layer][channel]==NULL)
00020       images[layer][channel]=createImageCache(layer,channel);
00021     calcImage(layer,channel);
00022   }
00023   return images[layer][channel];
00024 }
00025 
00026 void 
00027 FilterBankGenerator::freeCaches() {
00028   invalidateCaches();
00029   for(unsigned int i=0; i<numLayers; i++)
00030     for(unsigned int j=0; j<numChannels; j++) {
00031       delete [] images[i][j];
00032       images[i][j]=NULL;
00033     }
00034 }
00035 
00036 void
00037 FilterBankGenerator::invalidateCaches() {
00038   for(unsigned int i=0; i<numLayers; i++)
00039     for(unsigned int j=0; j<numChannels; j++)
00040       imageValids[i][j]=false;
00041 }
00042 
00043 void
00044 FilterBankGenerator::doEvent() {
00045   // This code is overriden by BufferedImageGenerator, except in APERIOS
00046   if(event->getGeneratorID()==getListenGeneratorID() && event->getSourceID()==getListenSourceID()) {
00047     const FilterBankEvent& fbkevent=dynamic_cast<const FilterBankEvent& >(*event);
00048     if(fbkevent.getSource()==NULL) {
00049       serr->printf("%s %s received event with null source\n",getClassName().c_str(),getName().c_str());
00050       return;
00051     }
00052     src=fbkevent.getSource();
00053     refresh();
00054   } else {
00055     EventGeneratorBase::doEvent();
00056   }
00057 }
00058 
00059 unsigned int FilterBankGenerator::getBinSize() const {
00060   unsigned int used=0;
00061   used+=creatorSize("FbkImage");
00062   used+=sizeof(widths[selectedSaveLayer]);
00063   used+=sizeof(heights[selectedSaveLayer]);
00064   used+=sizeof(selectedSaveLayer);
00065   used+=sizeof(selectedSaveChannel);
00066   return used;
00067 }
00068 
00069 /*! The loadBuffer() functions of the included subclasses aren't tested, so don't assume they'll work without a little debugging... */
00070 unsigned int FilterBankGenerator::loadBuffer(const char buf[], unsigned int len, const char* filename) {
00071   unsigned int origlen=len;
00072   if(!checkCreatorInc("FbkImage",buf,len,true)) return 0;
00073   if(!decodeInc(widths[selectedSaveLayer],buf,len)) return 0;
00074   if(!decodeInc(heights[selectedSaveLayer],buf,len)) return 0;
00075   if(!decodeInc(selectedSaveLayer,buf,len)) return 0;
00076   if(!decodeInc(selectedSaveChannel,buf,len)) return 0;
00077   return origlen-len; 
00078 }
00079 
00080 unsigned int FilterBankGenerator::saveBuffer(char buf[], unsigned int len) const {
00081   unsigned int origlen=len;
00082   if(!saveCreatorInc("FbkImage",buf,len)) return 0;
00083   if(!encodeInc(widths[selectedSaveLayer],buf,len)) return 0;
00084   if(!encodeInc(heights[selectedSaveLayer],buf,len)) return 0;
00085   if(!encodeInc(selectedSaveLayer,buf,len)) return 0;
00086   if(!encodeInc(selectedSaveChannel,buf,len)) return 0;
00087   return origlen-len;
00088 }
00089 
00090 void
00091 FilterBankGenerator::setNumImages(unsigned int nLayers, unsigned int nChannels) {
00092   if(nLayers==numLayers && nChannels==numChannels)
00093     return;
00094   freeCaches();
00095   destruct();
00096   numLayers=nLayers;
00097   numChannels=nChannels;
00098   
00099   widths=new unsigned int[numLayers];
00100   heights=new unsigned int[numLayers];
00101   if(numLayers>0) {
00102     for(unsigned int res=0; res<numLayers-1; res++) {
00103       widths[res] = CameraResolutionX>>(numLayers-2-res);
00104       heights[res] = CameraResolutionY>>(numLayers-2-res);
00105     }
00106     widths[numLayers-1]=CameraResolutionX*2;
00107     heights[numLayers-1]=CameraResolutionY*2;
00108   }
00109 
00110   skips=new unsigned int[numLayers];
00111   strides=new unsigned int[numLayers];
00112   increments=new unsigned int[numLayers];
00113   images=new unsigned char**[numLayers];
00114   imageValids=new bool*[numLayers];
00115   for(unsigned int i=0; i<numLayers; i++) {
00116     skips[i]=strides[i]=0;
00117     increments[i]=1;
00118     images[i]=new unsigned char*[numChannels];
00119     imageValids[i]=new bool[numChannels];
00120     for(unsigned int j=0; j<numChannels; j++) {
00121       images[i][j]=NULL;
00122       imageValids[i][j]=false;
00123     }
00124   }
00125 }
00126 
00127 void
00128 FilterBankGenerator::setDimensions() {
00129   if(src==NULL)
00130     return;
00131   for(unsigned int i=0; i<numLayers; i++) {
00132     widths[i]=src->getWidth(i);
00133     heights[i]=src->getHeight(i);
00134   }
00135   freeCaches();
00136 }
00137 
00138 void
00139 FilterBankGenerator::destruct() {
00140   delete [] widths;
00141   widths=NULL;
00142   delete [] heights;
00143   heights=NULL;
00144   delete [] skips;
00145   skips=NULL;
00146   delete [] strides;
00147   strides=NULL;
00148   delete [] increments;
00149   increments=NULL;
00150   for(unsigned int i=0; i<numLayers; i++) {
00151     delete [] images[i];
00152     delete [] imageValids[i];
00153   }
00154   delete [] images;
00155   images=NULL;
00156   delete [] imageValids;
00157   imageValids=NULL;
00158   numLayers=numChannels=0;
00159 }
00160 
00161 bool FilterBankGenerator::refresh() {
00162   if(sysFrameNumber==-1U) {
00163     serr->printf("ERROR: attempted access to camera image before any data was available\n");
00164     return false;
00165   }
00166   if(frameNumber==sysFrameNumber)
00167     return true;
00168   if(src==NULL) {
00169     // BufferedImageGenerator leaves src at 0.  The frameNumber might not match
00170     // sysFrameNumber if we have depth images and camera images arriving asynchronously.
00171     if ( frameNumber > 0 )
00172       return true; // still have good data
00173     serr->printf("ERROR: attempted access to camera image before stage had a known source\n");
00174     return false;
00175   }
00176   if(!src->refresh())
00177     return false;
00178   if(frameNumber==src->getFrameNumber())
00179     return true;
00180   frameNumber=src->getFrameNumber();
00181   setNumImages(src->getNumLayers(),src->getNumChannels());
00182   if(numLayers<=0 || numChannels<=0) {
00183     serr->printf("ERROR: attempted to access image in empty FilterBankGenerator\n");
00184     return false;
00185   }
00186   if(src->getWidth(numLayers-1)!=getWidth(numLayers-1) || src->getHeight(numLayers-1)!=getHeight(numLayers-1)) {
00187     setDimensions();
00188     if(framesProcessed==0)
00189       serr->printf("WARNING: %s image dimensions don't match values predicted by RobotInfo consts, \"full\" layer now %dx%d\n",getName().c_str(),widths[ProjectInterface::fullLayer],heights[ProjectInterface::fullLayer]);
00190     else
00191       serr->printf("WARNING: %s image dimensions have changed since last frame, \"full\" layer now %dx%d\n",getName().c_str(),widths[ProjectInterface::fullLayer],heights[ProjectInterface::fullLayer]);
00192   } else if(framesProcessed==0) {
00193     setDimensions();
00194   }
00195   invalidateCaches();
00196   framesProcessed++;
00197   return true;
00198 }
00199 
00200 
00201 /*! @file
00202  * @brief Implements abstract base class for generators of FilterBankEvent's
00203  * @author ejt (Creator)
00204  */
00205 

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:38 2016 by Doxygen 1.6.3