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
00015
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::processEvent(const EventBase & event) {
00045 if(event.getGeneratorID()==getListenGeneratorID() && event.getSourceID()==getListenSourceID()) {
00046 const FilterBankEvent& fbkevent=dynamic_cast<const FilterBankEvent& >(event);
00047 if(fbkevent.getSource()==NULL) {
00048 serr->printf("%s %s received event with null source\n",getClassName().c_str(),getName().c_str());
00049 return;
00050 }
00051 src=fbkevent.getSource();
00052 refresh();
00053 } else {
00054 EventGeneratorBase::processEvent(event);
00055 }
00056 }
00057
00058 unsigned int FilterBankGenerator::getBinSize() const {
00059 unsigned int used=0;
00060 used+=creatorSize("FbkImage");
00061 used+=sizeof(widths[selectedSaveLayer]);
00062 used+=sizeof(heights[selectedSaveLayer]);
00063 used+=sizeof(selectedSaveLayer);
00064 used+=sizeof(selectedSaveChannel);
00065 return used;
00066 }
00067
00068
00069 unsigned int FilterBankGenerator::loadBuffer(const char buf[], unsigned int len) {
00070 unsigned int origlen=len;
00071 if(!checkCreatorInc("FbkImage",buf,len,true)) return 0;
00072 if(!decodeInc(widths[selectedSaveLayer],buf,len)) return 0;
00073 if(!decodeInc(heights[selectedSaveLayer],buf,len)) return 0;
00074 if(!decodeInc(selectedSaveLayer,buf,len)) return 0;
00075 if(!decodeInc(selectedSaveChannel,buf,len)) return 0;
00076 return origlen-len;
00077 }
00078
00079 unsigned int FilterBankGenerator::saveBuffer(char buf[], unsigned int len) const {
00080 unsigned int origlen=len;
00081 if(!saveCreatorInc("FbkImage",buf,len)) return 0;
00082 if(!encodeInc(widths[selectedSaveLayer],buf,len)) return 0;
00083 if(!encodeInc(heights[selectedSaveLayer],buf,len)) return 0;
00084 if(!encodeInc(selectedSaveLayer,buf,len)) return 0;
00085 if(!encodeInc(selectedSaveChannel,buf,len)) return 0;
00086 return origlen-len;
00087 }
00088
00089 void
00090 FilterBankGenerator::setNumImages(unsigned int nLayers, unsigned int nChannels) {
00091 if(nLayers==numLayers && nChannels==numChannels)
00092 return;
00093 freeCaches();
00094 destruct();
00095 numLayers=nLayers;
00096 numChannels=nChannels;
00097
00098 widths=new unsigned int[numLayers];
00099 heights=new unsigned int[numLayers];
00100 if(numLayers>0) {
00101 for(unsigned int res=0; res<numLayers-1; res++) {
00102 widths[res] = CameraResolutionX>>(numLayers-2-res);
00103 heights[res] = CameraResolutionY>>(numLayers-2-res);
00104 }
00105 widths[numLayers-1]=CameraResolutionX*2;
00106 heights[numLayers-1]=CameraResolutionY*2;
00107 }
00108
00109 skips=new unsigned int[numLayers];
00110 strides=new unsigned int[numLayers];
00111 increments=new unsigned int[numLayers];
00112 images=new unsigned char**[numLayers];
00113 imageValids=new bool*[numLayers];
00114 for(unsigned int i=0; i<numLayers; i++) {
00115 skips[i]=strides[i]=0;
00116 increments[i]=1;
00117 images[i]=new unsigned char*[numChannels];
00118 imageValids[i]=new bool[numChannels];
00119 for(unsigned int j=0; j<numChannels; j++) {
00120 images[i][j]=NULL;
00121 imageValids[i][j]=false;
00122 }
00123 }
00124 }
00125
00126 void
00127 FilterBankGenerator::setDimensions() {
00128 if(src==NULL)
00129 return;
00130 for(unsigned int i=0; i<numLayers; i++) {
00131 widths[i]=src->getWidth(i);
00132 heights[i]=src->getHeight(i);
00133 }
00134 freeCaches();
00135 }
00136
00137 void
00138 FilterBankGenerator::destruct() {
00139 delete [] widths;
00140 widths=NULL;
00141 delete [] heights;
00142 heights=NULL;
00143 delete [] skips;
00144 skips=NULL;
00145 delete [] strides;
00146 strides=NULL;
00147 delete [] increments;
00148 increments=NULL;
00149 for(unsigned int i=0; i<numLayers; i++) {
00150 delete [] images[i];
00151 delete [] imageValids[i];
00152 }
00153 delete [] images;
00154 images=NULL;
00155 delete [] imageValids;
00156 imageValids=NULL;
00157 numLayers=numChannels=0;
00158 }
00159
00160 bool FilterBankGenerator::refresh() {
00161 if(sysFrameNumber==-1U) {
00162 serr->printf("ERROR: attempted access to camera image before any data was available\n");
00163 return false;
00164 }
00165 if(frameNumber==sysFrameNumber)
00166 return true;
00167 if(src==NULL) {
00168 serr->printf("ERROR: attempted access to camera image before stage had a known source\n");
00169 return false;
00170 }
00171 if(!src->refresh())
00172 return false;
00173 if(frameNumber==src->getFrameNumber())
00174 return true;
00175 frameNumber=src->getFrameNumber();
00176 setNumImages(src->getNumLayers(),src->getNumChannels());
00177 if(numLayers<=0 || numChannels<=0) {
00178 serr->printf("ERROR: attempted to access image in empty FilterBankGenerator\n");
00179 return false;
00180 }
00181 if(src->getWidth(numLayers-1)!=getWidth(numLayers-1) || src->getHeight(numLayers-1)!=getHeight(numLayers-1)) {
00182 setDimensions();
00183 if(framesProcessed==0)
00184 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]);
00185 else
00186 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]);
00187 } else if(framesProcessed==0) {
00188 setDimensions();
00189 }
00190 invalidateCaches();
00191 framesProcessed++;
00192 return true;
00193 }
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206