Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

BufferedImageGenerator.cc

Go to the documentation of this file.
00001 #ifndef PLATFORM_APERIOS
00002 
00003 #include "BufferedImageGenerator.h"
00004 #include "Events/DataEvent.h"
00005 #include "Events/FilterBankEvent.h"
00006 #include "Wireless/Socket.h"
00007 #include "Events/EventRouter.h"
00008 #include "Shared/debuget.h"
00009 #include "Shared/ProjectInterface.h"
00010 
00011 using namespace std;
00012 
00013 void BufferedImageGenerator::doEvent() {
00014   if(event->getGeneratorID()!=getListenGeneratorID() || event->getSourceID()!=getListenSourceID())
00015     return;
00016   if(event->getTypeID()==EventBase::activateETID) {
00017     const DataEvent<ImageSource>* data=dynamic_cast<const DataEvent<ImageSource>*>(event);
00018     if(data==NULL) {
00019       serr->printf("Error: %s(%s) received event of the wrong type",getClassName().c_str(),getName().c_str());
00020       return;
00021     }
00022     if(imgsrc.layer!=data->getData().layer || imgsrc.channels!=data->getData().channels) {
00023       //can "quietly" switch the layer of transmission as long as the width and height were scaled properly
00024       //just need to reset the increments if a different layer is being used.
00025       unsigned int i;
00026       for(i=0; i<data->getData().layer; i++) {
00027         increments[i] = 1;
00028         strides[i]=data->getData().width>>(data->getData().layer-i);
00029         skips[i]=0;
00030       }
00031       increments[i] = data->getData().channels;
00032       strides[i]=data->getData().width*data->getData().channels;
00033       skips[i]=0;
00034       for(++i; i<numLayers; i++) {
00035         increments[i] = 1;
00036         strides[i]=data->getData().width<<(i-data->getData().layer);
00037         skips[i]=0;
00038       }
00039     }
00040     imgsrc=data->getData();
00041     sysFrameNumber=frameNumber=imgsrc.frameIndex;
00042     invalidateCaches(); //mark everything invalid
00043     if(numLayers>0) {
00044       // have images, check if the dimensions have changed
00045       if(imgsrc.width!=getWidth(imgsrc.layer) || imgsrc.height!=getHeight(imgsrc.layer)) {
00046         freeCaches();
00047         setDimensions();
00048         if(framesProcessed==0)
00049           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]);
00050         else
00051           serr->printf("WARNING: the image dimensions have changed since last frame, \"full\" layer now %dx%d\n",widths[ProjectInterface::fullLayer],heights[ProjectInterface::fullLayer]);
00052         erouter->postEvent(EventBase::cameraResolutionEGID,event->getSourceID(),EventBase::statusETID);
00053       } else if(framesProcessed==0) {
00054         // first frame, always set it anyway
00055         setDimensions();
00056       }
00057     }
00058     // -- reassign to the new buffer: --
00059     unsigned int i=imgsrc.layer;
00060     for(unsigned int j=0; j<imgsrc.channels; j++) {
00061       if(isAllocated[i][j]) { //in case the imgsrc layer changes
00062         delete [] images[i][j];
00063         images[i][j]=NULL;
00064         isAllocated[i][j]=false;
00065       }
00066       imageValids[i][j]=true;
00067     }
00068     images[i][RawCameraGenerator::CHAN_Y]=imgsrc.img+0;
00069     images[i][RawCameraGenerator::CHAN_U]=imgsrc.img+1;
00070     images[i][RawCameraGenerator::CHAN_V]=imgsrc.img+2;
00071     framesProcessed++;
00072   }
00073   erouter->postEvent(FilterBankEvent(this,getGeneratorID(),getSourceID(),event->getTypeID()));
00074 }
00075 
00076 unsigned int
00077 BufferedImageGenerator::getBinSize() const {
00078   unsigned int used=FilterBankGenerator::getBinSize();
00079   used+=getSerializedSize("RawImage");
00080   used+=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00081   return used;
00082 }
00083 
00084 unsigned int
00085 BufferedImageGenerator::loadBuffer(const char buf[], unsigned int len, const char* filename) {
00086   unsigned int origlen=len;
00087   if(!checkInc(FilterBankGenerator::loadBuffer(buf,len,filename),buf,len)) return 0;
00088   std::string tmp;
00089   if(decode(tmp,buf,len)) return 0;
00090   if(tmp!="RawImage") {
00091     serr->printf("Unhandled image type for BufferedImageGenerator: %s",tmp.c_str());
00092     return 0;
00093   } else if(selectedSaveLayer!=numLayers-1) {
00094     serr->printf("Can't load into BufferedImageGenerator layer %d!=%d",selectedSaveLayer,numLayers-1);
00095     return 0;
00096   } else {
00097     if(images[selectedSaveLayer][selectedSaveChannel]==NULL)
00098       images[selectedSaveLayer][selectedSaveChannel]=createImageCache(selectedSaveLayer,selectedSaveChannel);
00099     unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00100     unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00101     ASSERTRETVAL(used<=len,"buffer too small",0);
00102     memcpy(img,buf,used);
00103     len-=used; buf+=used;
00104     imageValids[selectedSaveLayer][selectedSaveChannel]=true;
00105     return origlen-len; 
00106   }
00107 }
00108 
00109 unsigned int
00110 BufferedImageGenerator::saveBuffer(char buf[], unsigned int len) const {
00111   unsigned int origlen=len;
00112   if(!checkInc(FilterBankGenerator::saveBuffer(buf,len),buf,len)) return 0;
00113   if(!encodeInc("RawImage",buf,len)) return 0;
00114   
00115   if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00116     serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00117     return 0;
00118   }
00119   if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00120     serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00121     return 0;
00122   }
00123   unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00124   unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00125   ASSERTRETVAL(used<=len,"buffer too small " << len << ' ' <<  widths[selectedSaveLayer] << ' ' << heights[selectedSaveLayer],0);
00126   unsigned int inc=getIncrement(selectedSaveLayer);
00127   if(inc==1) {
00128     //special case, straight copy
00129     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00130       memcpy(buf,img,widths[selectedSaveLayer]);
00131       buf+=widths[selectedSaveLayer];
00132       img+=getStride(selectedSaveLayer);
00133     }
00134   } else {
00135     //otherwise, interleaved or subsampling
00136     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00137       unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00138       while(img!=rowend) {
00139         *buf++=*img;
00140         img+=inc;
00141       }
00142       img+=getSkip(selectedSaveLayer);
00143     }
00144   }
00145   len-=used;
00146   
00147   return origlen-len;
00148 }
00149 
00150 unsigned int
00151 BufferedImageGenerator::saveFileStream(FILE * f) const {
00152   unsigned int totalused=0;
00153   unsigned int used;
00154   { //sigh, inheritance has failed me (I wouldn't want FilterBankGenerator::saveFileStream() to call the virtuals...)
00155     unsigned int sz=FilterBankGenerator::getBinSize();
00156     char * buf = new char[sz];
00157     memset(buf,0xF0,sz);
00158     if(buf==NULL) {
00159       std::cout << "*** WARNING could not allocate " << sz << " bytes for loadFile";
00160       return 0;
00161     }
00162     unsigned int resp=FilterBankGenerator::saveBuffer(buf,sz);
00163     if(resp==0) {
00164       std::cout << "*** WARNING saveBuffer didn't write any data (possibly due to overflow or other error)" << std::endl;
00165       fwrite(buf,1,sz,f);
00166     } else {
00167       unsigned int wrote=fwrite(buf,1,resp,f);
00168       if(wrote!=resp)
00169         std::cout << "*** WARNING short write (wrote " << wrote << ", expected " << resp << ")" << std::endl;
00170     }
00171     delete [] buf;
00172     used=resp;
00173   }
00174   if(0==used) return 0;
00175   totalused+=used;
00176   if(0==(used=encode("RawImage",f))) return 0;
00177   totalused+=used;
00178   
00179   if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00180     serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00181     return 0;
00182   }
00183   if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00184     serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00185     return 0;
00186   }
00187   unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00188   used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00189   unsigned int inc=getIncrement(selectedSaveLayer);
00190   if(inc==1) {
00191     //special case, straight copy
00192     sout->printf("Saving %d by %d\n",widths[selectedSaveLayer],heights[selectedSaveLayer]);
00193     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00194       if(fwrite(img,widths[selectedSaveLayer],1,f)==0) {
00195         serr->printf("short write on image data - ran out of space?\n");
00196         return 0;
00197       }
00198       img+=getStride(selectedSaveLayer);
00199     }
00200   } else {
00201     //otherwise, interleaved or subsampling
00202     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00203       unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00204       while(img!=rowend) {
00205         if(fputc(*img,f)==EOF) {
00206           serr->printf("short write on image data - ran out of space?\n");
00207           return 0;
00208         }
00209         img+=inc;
00210       }
00211       img+=getSkip(selectedSaveLayer);
00212     }
00213   }
00214   totalused+=used;
00215   
00216   return totalused;
00217 }
00218 
00219 void BufferedImageGenerator::freeCaches() {
00220   FilterBankGenerator::freeCaches();
00221   for(unsigned int i=0; i<numLayers; i++)
00222     for(unsigned int j=0; j<numChannels; j++)
00223       isAllocated[i][j]=false;
00224 }
00225 
00226 void BufferedImageGenerator::invalidateCaches() {
00227   for(unsigned int i=0; i<numLayers; i++)
00228     for(unsigned int j=0; j<numChannels; j++) {
00229       if(!isAllocated[i][j])
00230         images[i][j]=NULL;
00231       imageValids[i][j]=false;
00232     }
00233 }
00234 
00235 unsigned char * BufferedImageGenerator::createImageCache(unsigned int layer, unsigned int channel) const {
00236   if(layer!=imgsrc.layer || imgsrc.channels==1) {
00237     isAllocated[layer][channel]=true;
00238     return new unsigned char[widths[layer]*heights[layer]];
00239   } else {
00240     ASSERT(channel>=imgsrc.channels,"createImageCache for image that should come from src")
00241     // increment is set to imgsrc.channels, so we need to allocate multiple images at once for the generate channels
00242     unsigned int base=(channel/imgsrc.channels)*imgsrc.channels; // round down to nearest multiple of imgsrc.channels
00243     if(images[layer][base]==NULL)
00244       images[layer][base]=new unsigned char[widths[layer]*heights[layer]*imgsrc.channels];
00245     for(unsigned int i=base+1; i<base+imgsrc.channels; ++i) {
00246       ASSERT(!isAllocated[layer][i],"createImageCache for image already allocated!");
00247       ASSERT(images[layer][i]==NULL,"createImageCache for image already assigned!");
00248       images[layer][i]=images[layer][i-1]+1;
00249     }
00250     isAllocated[layer][base]=true;
00251     return images[layer][channel];
00252   }
00253 }
00254 void BufferedImageGenerator::calcImage(unsigned int layer, unsigned int channel) {
00255   //cout << "BufferedImageGenerator::calcImage(" << layer << ',' << channel << ')' << endl;
00256   ASSERTRET(layer!=imgsrc.layer || channel>=imgsrc.channels, "calcImage on src channel?");
00257   switch(channel) {
00258     case RawCameraGenerator::CHAN_Y:
00259       if(layer>imgsrc.layer) upsampleImage(imgsrc.layer,channel,layer);
00260       else downsampleImage(layer,channel);
00261       break;
00262     case RawCameraGenerator::CHAN_U:
00263     case RawCameraGenerator::CHAN_V:
00264       if(imgsrc.channels>1) {
00265         if(layer>imgsrc.layer) upsampleImage(imgsrc.layer,channel,layer);
00266         else downsampleImage(layer,channel);
00267       } else //grayscale image, use blank U and V channels
00268         memset(images[layer][channel],128,widths[layer]*heights[layer]);
00269       break;
00270     case RawCameraGenerator::CHAN_Y_DX:
00271       calcDx(layer);
00272       break;
00273     case RawCameraGenerator::CHAN_Y_DY:
00274       calcDy(layer);
00275       break;
00276     case RawCameraGenerator::CHAN_Y_DXDY:
00277       calcDxDy(layer);
00278       break;
00279     default:
00280       cerr << "Bad layer selection!" << endl;
00281   }
00282 }
00283 void BufferedImageGenerator::setDimensions() {
00284   if(imgsrc.img==NULL) //don't have an image to set from
00285     return;
00286   // set dimensions of layers below the input layer
00287   for(unsigned int i=0; i<=imgsrc.layer; i++) {
00288     //s is the scaling factor -- 2 means *half* size
00289     unsigned int s=(1<<(imgsrc.layer-i));
00290     //width and height are scaled down (divide by s)
00291     widths[i]=strides[i]=imgsrc.width/s;
00292     heights[i]=imgsrc.height/s;
00293     //stride is same as width (set above) -- we allocate these layers, don't skip rows
00294     skips[i]=0;
00295     increments[i]=1;
00296     //cout << "setDimensions() " << widths[i] << ' ' << skips[i] << ' ' << strides[i] << endl;
00297   }
00298   // set dimensions of the input layer (interleaved -- note increment and stride)
00299   increments[imgsrc.layer] = imgsrc.channels;
00300   widths[imgsrc.layer]=imgsrc.width;
00301   heights[imgsrc.layer]=imgsrc.height;
00302   strides[imgsrc.layer]=imgsrc.width*imgsrc.channels;
00303   skips[imgsrc.layer]=0;
00304   //set dimensions for layers above the input layer
00305   for(unsigned int i=imgsrc.layer+1; i<numLayers; i++) {
00306     //s is the scaling factor -- 2 means *double* size
00307     unsigned int s=(1<<(i-imgsrc.layer));
00308     //multiply by s
00309     widths[i]=strides[i]=imgsrc.width*s;
00310     heights[i]=imgsrc.height*s;
00311     //stride is same as width (set above) -- we allocate these layers, don't skip rows
00312     skips[i]=0;
00313     increments[i]=1;
00314   }
00315 }
00316 void BufferedImageGenerator::destruct() {
00317   FilterBankGenerator::destruct();
00318   for(unsigned int i=0; i<numLayers; i++)
00319     delete [] isAllocated[i];
00320   delete [] isAllocated;
00321   isAllocated=NULL;
00322 }
00323 void BufferedImageGenerator::setNumImages(unsigned int nLayers, unsigned int nChannels) {
00324   if(nLayers==numLayers && nChannels==numChannels)
00325     return;
00326   FilterBankGenerator::setNumImages(nLayers,nChannels);
00327   isAllocated=new bool*[numLayers];
00328   for(unsigned int i=0; i<numLayers; i++) {
00329     isAllocated[i]=new bool[numChannels];
00330     for(unsigned int j=0; j<numChannels; j++)
00331       isAllocated[i][j]=false;
00332   }
00333   setDimensions();
00334 }
00335 
00336 void BufferedImageGenerator::upsampleImage(unsigned int srcLayer, unsigned int chan, unsigned int destLayer) {
00337   ASSERTRET(destLayer!=imgsrc.layer,"upsample into source layer")
00338   unsigned char * cur=images[destLayer][chan];
00339   ASSERTRET(cur!=NULL,"destination layer is NULL");
00340   unsigned char * orig=getImage(srcLayer,chan);
00341   ASSERTRET(orig!=NULL,"source layer is NULL");
00342   unsigned int width=widths[destLayer];
00343   unsigned int height=heights[destLayer];
00344   unsigned int inc=getIncrement(srcLayer);
00345   int power=destLayer-srcLayer;
00346   ASSERTRET(power>0,"upsampleImage attempting to downsample")
00347   
00348   unsigned char * const imgend=cur+width*height;
00349   while(cur!=imgend) {
00350     unsigned char * const row=cur;
00351     unsigned char * const rowend=cur+width;
00352     //upsample pixels within one row
00353     while(cur<rowend) {
00354       for(int p=1<<power; p>0; p--)
00355         *cur++=*orig;
00356       orig+=inc;
00357     }
00358     //now replicate that row 1<<power times, doubling each time
00359     for(int p=0; p<power; p++) {
00360       unsigned int avail=width*(1<<p);
00361       memcpy(cur,row,avail);
00362       cur+=avail;
00363     }
00364     orig+=getSkip(srcLayer);
00365   }
00366   imageValids[destLayer][chan]=true;
00367 }
00368 
00369 void BufferedImageGenerator::downsampleImage(unsigned int destLayer, unsigned int chan) {
00370   ASSERTRET(destLayer!=imgsrc.layer,"downsample into source layer")
00371   // find closest available layer to source from
00372   unsigned int layer=destLayer;
00373   while(layer<numLayers && !imageValids[layer][chan])
00374     layer++;
00375   ASSERTRET(layer<numLayers,"valid layer to downsample from could not be found!");
00376   if (!(layer<numLayers))
00377     std::cout << "layer = " << layer << " numLayers = " << numLayers << std::endl;
00378   // we'll compute in-between layers as we go (easier computation and might be able to reuse them anyway)
00379   // layer is the current layer we're downsampling into (layer+1 is the one we're sampling from)
00380   for(unsigned int srcL=layer--; layer>=destLayer; srcL=layer--) {
00381     unsigned int srcInc=getIncrement(srcL); // destination increment is guaranteed to be 1, but source increment isn't
00382     unsigned char * s=getImage(srcL,chan);
00383     if(images[layer][chan]==NULL)
00384       images[layer][chan]=createImageCache(layer,chan);
00385     unsigned char * dst=images[layer][chan];
00386     unsigned char * const dstEnd=dst+widths[layer]*heights[layer];
00387     while(dst!=dstEnd) {
00388       unsigned char * const rowEnd=dst+widths[layer];
00389       while(dst!=rowEnd) {
00390         unsigned short x=*s;
00391         x+=*(s+strides[srcL]);
00392         s+=srcInc;
00393         x+=*s;
00394         x+=*(s+strides[srcL]);
00395         s+=srcInc;
00396         *dst++ = x/4;
00397       }
00398       s+=strides[srcL];
00399     }
00400     imageValids[layer][chan]=true;
00401   }
00402 }
00403 
00404 void BufferedImageGenerator::calcDx(unsigned int layer, unsigned int srcChan/*=RawCameraGenerator::CHAN_Y*/, unsigned int dstChan/*=RawCameraGenerator::CHAN_Y_DX*/) {
00405   unsigned char * s=getImage(layer,srcChan);
00406   unsigned char * dst=images[layer][dstChan];
00407   unsigned int inc=getIncrement(layer);
00408   unsigned int skip=getSkip(layer)+inc;
00409   unsigned char * const dstEnd=dst+getStride(layer)*heights[layer];
00410   unsigned int sc=2;  // i think this should be 1, but this is to provide better compatability with the OPEN-R implementation
00411   while(dst!=dstEnd) {
00412     unsigned char * const rowEnd=dst+widths[layer]*inc-inc;
00413     unsigned char left,right;
00414     while(dst!=rowEnd) {
00415       left=(*s)>>sc;
00416       s+=inc;
00417       right=(*s)>>sc;
00418       *dst=right+128-left;
00419       dst+=inc;
00420     }
00421     *dst=128; //rightmost column is always 128 to retain image dimensions
00422     dst+=skip;
00423     s+=skip;
00424   }
00425   imageValids[layer][dstChan]=true;
00426 }
00427 void BufferedImageGenerator::calcDy(unsigned int layer, unsigned int srcChan/*=RawCameraGenerator::CHAN_Y*/, unsigned int dstChan/*=RawCameraGenerator::CHAN_Y_DY*/) {
00428   unsigned char * s=getImage(layer,srcChan);
00429   unsigned char * dst=images[layer][dstChan];
00430   unsigned int inc=getIncrement(layer);
00431   unsigned int stride=getStride(layer);
00432   unsigned char * const dstEnd=dst+widths[layer]*inc;
00433   unsigned int sc=2;  // i think this should be 1, but this is to provide better compatability with the OPEN-R implementation
00434   while(dst!=dstEnd) {
00435     unsigned char * const colEnd=dst+heights[layer]*stride-stride;
00436     unsigned char top,bottom;
00437     while(dst!=colEnd) {
00438       top=(*s)>>sc;
00439       s+=stride;
00440       bottom=(*s)>>sc;
00441       *dst=bottom+128-top;
00442       dst+=stride;
00443     }
00444     *dst=128; //bottommost column is always 128 to retain image dimensions
00445     dst-=heights[layer]*stride-stride;
00446     s-=heights[layer]*stride-stride;
00447     dst+=inc;
00448     s+=inc;
00449   }
00450   imageValids[layer][dstChan]=true;
00451 }
00452 void BufferedImageGenerator::calcDxDy(unsigned int layer) {
00453   // if one of the dx or dy channel is already available, go from there
00454   if(imageValids[layer][RawCameraGenerator::CHAN_Y_DX]) {
00455     calcDy(layer,RawCameraGenerator::CHAN_Y_DX,RawCameraGenerator::CHAN_Y_DXDY);
00456     imageValids[layer][RawCameraGenerator::CHAN_Y_DXDY]=true;
00457   } else if(imageValids[layer][RawCameraGenerator::CHAN_Y_DY]) {
00458     calcDx(layer,RawCameraGenerator::CHAN_Y_DY,RawCameraGenerator::CHAN_Y_DXDY);
00459     imageValids[layer][RawCameraGenerator::CHAN_Y_DXDY]=true;
00460   } else {
00461     // if neither are available, calculate one of them (dx), and then the other from that
00462     getImage(layer,RawCameraGenerator::CHAN_Y_DX);
00463     calcDy(layer,RawCameraGenerator::CHAN_Y_DX,RawCameraGenerator::CHAN_Y_DXDY);
00464   }
00465 }
00466 
00467 
00468 /*! @file
00469  * @brief Implements BufferedImageGenerator, which receives camera frames as they are loaded by the simulator -- or eventually other sources
00470  * @author Ethan Tira-Thompson (ejt) (Creator)
00471  */
00472 
00473 #endif

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