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

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