Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

RawCameraGenerator.cc

Go to the documentation of this file.
00001 #include "RawCameraGenerator.h"
00002 #include "Events/DataEvent.h"
00003 #include "Events/EventRouter.h"
00004 #include "Events/FilterBankEvent.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 #include <float.h>
00016 
00017 RawCameraGenerator::RawCameraGenerator(unsigned int numRawLayers, unsigned int numCalcLayers,
00018                unsigned int mysid, EventBase::EventGeneratorID_t gid, unsigned int sid)
00019   : FilterBankGenerator("RawCameraGenerator",EventBase::visRawCameraEGID,mysid,gid,sid),
00020     numRealLayers(numRawLayers), layers(NULL), imageInfos(NULL)
00021 {
00022   /* As a root stage, we need to listen to all incoming image
00023    * events, even if we don't currently have listeners of our own --
00024    * This is just in case user code directly accesses a generator
00025    * and we have to retroactively go back and dig up the previous
00026    * frame */
00027   unsetAutoListen();
00028   
00029   setNumImages(numCalcLayers,NUM_CHANNELS);
00030 }
00031 
00032 RawCameraGenerator::~RawCameraGenerator() {
00033   freeCaches();
00034   destruct();
00035 }
00036 
00037 /*! The const casts in this function are regretable but necessary
00038  *  since the OPEN-R OFbkImage constructor requires mutable
00039  *  arguments, even though it shouldn't be modifying the data
00040  */
00041 void RawCameraGenerator::doEvent() {
00042   if(event->getGeneratorID()!=getListenGeneratorID() || event->getSourceID()!=getListenSourceID())
00043     return;
00044   if(event->getTypeID()==EventBase::activateETID) {
00045     typedef DataEvent<const OFbkImageVectorData*> OFbkEvent;
00046     const OFbkEvent& fbkevent=dynamic_cast<const OFbkEvent& >(*event);
00047     OFbkImageVectorData& fbkdat=*const_cast<OFbkImageVectorData*>(fbkevent.getData());
00048     for(unsigned int res=0; res<numRealLayers; res++) {
00049       layers[numLayers-2-res] = fbkdat.GetData(res);
00050       imageInfos[numLayers-2-res] = fbkdat.GetInfo(res);
00051     }
00052     {
00053       const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[numLayers-2]), const_cast<unsigned char*>(layers[numLayers-2]), ofbkimageBAND_Y);
00054       //I have to do this crazy thing because apparently img.FieldCounter() doesn't work
00055       sysFrameNumber=frameNumber=*(int*)(img.Pointer()+(img.Height()-1)*(img.Skip()+img.Width()));
00056     }
00057     unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00058     bool dimchange=false;
00059     for(unsigned int res=numNotRealLayers; res<numLayers-1; res++) {
00060       if(widths[res]!=imageInfos[res]->width || heights[res]!=imageInfos[res]->height) {
00061   dimchange=true;
00062   serr->printf("WARNING: the image dimensions changed, now %dx%d\n",widths[numLayers-1],heights[numLayers-1]);
00063   widths[res] = imageInfos[res]->width;
00064   heights[res] = imageInfos[res]->height;
00065       }
00066 
00067       const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[res]), const_cast<unsigned char*>(layers[res]), ofbkimageBAND_Y);
00068       skips[res]=img.Skip();
00069       strides[res]=skips[res]+widths[res];
00070 
00071       ASSERT(static_cast<unsigned int>(img.Width())==getWidth(res),"Widths don't agree for layer "<<res);
00072       ASSERT(static_cast<unsigned int>(img.Height())==getHeight(res),"Heights don't agree for layer "<<res);
00073     }
00074     if(widths[numLayers-2]*2!=widths[numLayers-1] || heights[numLayers-2]*2!=heights[numLayers-1]) {
00075       //|| widths[numLayers-2-numRealLayers]*2!=widths[numNotRealLayers]
00076       //|| heights[numLayers-2-numRealLayers]*2!=heights[numNotRealLayers]) {
00077       //set the width and height of non-real layers (since they don't match what they should be)
00078       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]);
00079       freeCaches();
00080       dimchange=true;
00081     } else if(strides[numLayers-1]==0) {
00082       // first frame
00083       dimchange=true;
00084     }
00085     if(dimchange)
00086       setDimensions();
00087     float testaspect=widths[numLayers-2]/(float)heights[numLayers-2];
00088     if(fabs(testaspect-config->vision.aspectRatio)>FLT_EPSILON) {
00089       serr->printf("WARNING: the image aspect ratio changed, was %g, now %g (diff %g)\n",*config->vision.aspectRatio,testaspect,testaspect-*config->vision.aspectRatio);
00090       config->vision.aspectRatio=testaspect;
00091     }
00092     
00093     invalidateCaches(); 
00094     framesProcessed++;
00095   }
00096   erouter->postEvent(FilterBankEvent(this,getGeneratorID(),getSourceID(),event->getTypeID()));
00097 }
00098 
00099 unsigned int RawCameraGenerator::getBinSize() const {
00100   unsigned int used=FilterBankGenerator::getBinSize();
00101   used+=strlen("RawImage")+LoadSave::stringpad;
00102   used+=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00103   return used;
00104 }
00105 
00106 unsigned int RawCameraGenerator::loadBuffer(const char buf[], unsigned int len, const char* filename) {
00107   unsigned int origlen=len;
00108   std::string tmp;
00109   if(!checkInc(FilterBankGenerator::loadBuffer(buf,len,filename),buf,len)) return 0;
00110   if(!decodeInc(tmp,buf,len)) return 0;
00111   if(tmp!="RawImage") {
00112     serr->printf("Unhandled image type for RawCameraGenerator: %s",tmp.c_str());
00113     return 0;
00114   } else if(selectedSaveLayer!=numLayers-1) {
00115     serr->printf("Can't load into RawCameraGenerator layer %d!=%d",selectedSaveLayer,numLayers-1);
00116     return 0;
00117   } else {
00118     if(images[selectedSaveLayer][selectedSaveChannel]==NULL)
00119       images[selectedSaveLayer][selectedSaveChannel]=createImageCache(selectedSaveLayer,selectedSaveChannel);
00120     unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00121     unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00122     ASSERTRETVAL(used<=len,"buffer too small",0);
00123     memcpy(img,buf,used);
00124     len-=used; buf+=used;
00125     imageValids[selectedSaveLayer][selectedSaveChannel]=true;
00126     return origlen-len; 
00127   }
00128 }
00129 
00130 unsigned int RawCameraGenerator::saveBuffer(char buf[], unsigned int len) const {
00131   unsigned int origlen=len;
00132   if(!checkInc(FilterBankGenerator::saveBuffer(buf,len),buf,len)) return 0;
00133   if(!encodeInc("RawImage",buf,len)) return 0;
00134   
00135   if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00136     serr->printf("RawCameraGenerator::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00137     return 0;
00138   }
00139   if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00140     serr->printf("RawCameraGenerator::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00141     return 0;
00142   }
00143   unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00144   unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00145   ASSERTRETVAL(used<=len,"buffer too small",0);
00146   unsigned int inc=getIncrement(selectedSaveLayer);
00147   if(inc==1) {
00148     //special case, straight copy
00149     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00150       memcpy(buf,img,widths[selectedSaveLayer]);
00151       buf+=widths[selectedSaveLayer];
00152       img+=getStride(selectedSaveLayer);
00153     }
00154   } else {
00155     //otherwise, interleaved or subsampling
00156     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00157       unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00158       while(img!=rowend) {
00159   *buf++=*img;
00160   img+=inc;
00161       }
00162       img+=getSkip(selectedSaveLayer);
00163     }
00164   }
00165   len-=used;
00166   
00167   return origlen-len;
00168 }
00169 
00170 unsigned int RawCameraGenerator::saveFileStream(FILE * f) const {
00171   unsigned int totalused=0;
00172   unsigned int used;
00173   { //sigh, inheritance has failed me (I wouldn't want FilterBankGenerator::saveFileStream() to call the virtuals...)
00174     unsigned int sz=FilterBankGenerator::getBinSize();
00175     char * buf = new char[sz];
00176     memset(buf,0xF0,sz);
00177     if(buf==NULL) {
00178       std::cout << "*** WARNING could not allocate " << sz << " bytes for loadFile";
00179       return 0;
00180     }
00181     unsigned int resp=FilterBankGenerator::saveBuffer(buf,sz);
00182     if(resp==0) {
00183       std::cout << "*** WARNING saveBuffer didn't write any data (possibly due to overflow or other error)" << std::endl;
00184       fwrite(buf,1,sz,f);
00185     } else {
00186       unsigned int wrote=fwrite(buf,1,resp,f);
00187       if(wrote!=resp)
00188   std::cout << "*** WARNING short write (wrote " << wrote << ", expected " << resp << ")" << std::endl;
00189     }
00190     delete [] buf;
00191     used=resp;
00192   }
00193   if(0==used) return 0;
00194   totalused+=used;
00195   if(0==(used=encode("RawImage",f))) return 0;
00196   totalused+=used;
00197   
00198   if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00199     serr->printf("RawCameraGenerator::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00200     return 0;
00201   }
00202   if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00203     serr->printf("RawCameraGenerator::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00204     return 0;
00205   }
00206   unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00207   used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00208   unsigned int inc=getIncrement(selectedSaveLayer);
00209   if(inc==1) {
00210     //special case, straight copy
00211     sout->printf("Saving %d by %d\n",widths[selectedSaveLayer],heights[selectedSaveLayer]);
00212     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00213       if(fwrite(img,widths[selectedSaveLayer],1,f)==0) {
00214   serr->printf("short write on image data - ran out of space?\n");
00215   return 0;
00216       }
00217       img+=getStride(selectedSaveLayer);
00218     }
00219   } else {
00220     //otherwise, interleaved or subsampling
00221     for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00222       unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00223       while(img!=rowend) {
00224   if(fputc(*img,f)==EOF) {
00225     serr->printf("short write on image data - ran out of space?\n");
00226     return 0;
00227   }
00228   img+=inc;
00229       }
00230       img+=getSkip(selectedSaveLayer);
00231     }
00232   }
00233   totalused+=used;
00234   
00235   return totalused;
00236 }
00237 
00238 void RawCameraGenerator::setDimensions() {
00239   freeCaches();
00240   unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00241   for(unsigned int res=0; res<numNotRealLayers; res++) {
00242     widths[res] = imageInfos[numNotRealLayers]->width>>(numNotRealLayers-res);
00243     heights[res] = imageInfos[numNotRealLayers]->height>>(numNotRealLayers-res);
00244     ASSERT(widths[res]*increments[res]==widths[numNotRealLayers],"widths*increments doesn't match total width");
00245     strides[res]=strides[numNotRealLayers]*increments[res];
00246     skips[res]=skips[numNotRealLayers]+strides[res]-strides[numNotRealLayers];
00247   }
00248   strides[numLayers-1]=widths[numLayers-1]=widths[numLayers-2]*2;
00249   heights[numLayers-1]=heights[numLayers-2]*2;
00250 }
00251 
00252 void  RawCameraGenerator::freeCaches() {
00253 #ifdef PLATFORM_APERIOS
00254   const unsigned int numRefLayers=numLayers;
00255 #else
00256   const unsigned int numRefLayers=numLayers-1;
00257 #endif
00258   for(unsigned int i=0; i<numRefLayers; i++) {
00259     for(unsigned int j=0; j<numChannels; j++) {
00260       images[i][j]=NULL;
00261       imageValids[i][j]=false;
00262     }
00263   }
00264   FilterBankGenerator::freeCaches();
00265 }
00266 
00267 void RawCameraGenerator::setNumImages(unsigned int nLayers, unsigned int nChannels) {
00268   if(nLayers==numLayers && nChannels==numChannels)
00269     return;
00270   FilterBankGenerator::setNumImages(nLayers,nChannels);
00271   layers=new unsigned char*[numLayers];
00272   imageInfos=new const OFbkImageInfo*[numLayers];
00273   unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00274   for(unsigned int res=0; res<numLayers; res++) {
00275     layers[res]=NULL;
00276     imageInfos[res]=NULL;
00277     if(res<numNotRealLayers)
00278       increments[res]=1<<(numNotRealLayers-res);
00279   }
00280 }
00281 
00282 unsigned char* RawCameraGenerator::createImageCache(unsigned int layer, unsigned int chan) const {
00283   if(layer==numLayers-1) {
00284 #ifdef PLATFORM_APERIOS
00285     return const_cast<unsigned char*>(&dblRes[chan][0][0]);
00286 #else
00287     return new unsigned char[widths[layer]*heights[layer]]; 
00288 #endif
00289   } else
00290     return NULL; // calcImage will set the cache itself
00291 }
00292 
00293 void RawCameraGenerator::calcImage(unsigned int layer, unsigned int chan) {
00294   PROFSECTION("RawCameraGenerator::calcImage(...)",*mainProfiler);
00295   unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00296   if(layer==numLayers-1) {
00297     //This is the only layer for which we calculate and store any data of our own...
00298     if(chan==CHAN_Y)
00299       reconstructImage();
00300     else
00301       upsampleImage(static_cast<channel_id_t>(chan));
00302   } else {
00303     if(layer>=numNotRealLayers) {
00304       unsigned int fbkdatChan=mapChannelID(static_cast<channel_id_t>(chan));
00305       const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[layer]), const_cast<unsigned char*>(layers[layer]), fbkdatChan);
00306       images[layer][chan]=img.Pointer();
00307 
00308       //this part restores pixels over written with the CDT table and
00309       //frame count.  Yes, we are modifying the original image passed
00310       //from the system here...
00311       if(config->vision.restore_image) {
00312   const unsigned int numPix=16;
00313   if(layer==numLayers-2) {
00314     unsigned char * s=images[layer][chan]+getStride(layer)*(getHeight(layer)-2);
00315     unsigned char * d=images[layer][chan]+getStride(layer)*(getHeight(layer)-1);
00316     for(unsigned int i=0; i<numPix; i++)
00317       *d++=*s++;
00318   } else {
00319     unsigned int inc=1<<(numLayers-2-layer);
00320     unsigned char * s;
00321     //unsigned char * s=getImage(numLayers-2,chan)+getStride(numLayers-2)*(getHeight(numLayers-2)-inc);
00322     //...or an attempt to possibly avoid a trivial amount of recomputation....
00323     if(!imageValids[numLayers-2][chan]) {
00324       const OFbkImage simg(const_cast<OFbkImageInfo*>(imageInfos[numLayers-2]), const_cast<unsigned char*>(layers[numLayers-2]), fbkdatChan);
00325       s=simg.Pointer();
00326     } else {
00327       s=images[numLayers-2][chan];
00328     }
00329     s+=getStride(numLayers-2)*(getHeight(numLayers-2)-inc);
00330     unsigned char * d=images[layer][chan]+getStride(layer)*(getHeight(layer)-1);
00331     for(unsigned int i=0; i<numPix; i++) {
00332       *d++=*s;
00333       s+=inc;
00334     }
00335   }
00336       }
00337     } else {
00338       //we don't need to do the restoration in the previous section
00339       //here because these layers skip the last row
00340       unsigned int fbkdatChan=mapChannelID(static_cast<channel_id_t>(chan));
00341       const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[numNotRealLayers]), const_cast<unsigned char*>(layers[numNotRealLayers]), fbkdatChan);
00342       images[layer][chan]=img.Pointer();
00343     }
00344   }
00345   imageValids[layer][chan]=true;
00346 }
00347 
00348 void RawCameraGenerator::destruct() {
00349   FilterBankGenerator::destruct();
00350   delete [] layers;
00351   layers=NULL;
00352   delete [] imageInfos;
00353   imageInfos=NULL;
00354 }
00355 
00356 unsigned int RawCameraGenerator::mapChannelID(channel_id_t chan) {
00357   switch(chan) {
00358   case CHAN_Y:
00359     return ofbkimageBAND_Y;
00360   case CHAN_U:
00361     return ofbkimageBAND_Cb;
00362   case CHAN_V:
00363     return ofbkimageBAND_Cr;
00364   case CHAN_Y_DY:
00365     return ofbkimageBAND_Y_LH;
00366   case CHAN_Y_DX:
00367     return ofbkimageBAND_Y_HL;
00368   case CHAN_Y_DXDY:
00369     return ofbkimageBAND_Y_HH;
00370   default:
00371     std::cout << "RawCameraGenerator::mapChannelID bad channel" << std::endl;
00372     return ofbkimageBAND_Y;;
00373   }
00374 }
00375 
00376 void RawCameraGenerator::upsampleImage(channel_id_t chan) {
00377   const unsigned int dblLayer=numLayers-1;
00378   const unsigned int srcLayer=dblLayer-1;
00379   const unsigned int width=widths[dblLayer];
00380   const unsigned int height=heights[dblLayer];
00381 
00382   unsigned char * cur=images[dblLayer][chan];
00383   ASSERTRET(cur!=NULL,"destination layer is NULL");
00384   unsigned char * orig=getImage(srcLayer,chan);
00385   ASSERTRET(orig!=NULL,"source layer is NULL");
00386 
00387   unsigned char * const imgend=cur+width*height;
00388   while(cur!=imgend) {
00389     unsigned char * const row=cur;
00390     unsigned char * const rowend=cur+width;
00391     while(cur!=rowend) {
00392       *cur++=*orig;
00393       *cur++=*orig++;
00394     }
00395     memcpy(cur,row,width);
00396     cur+=width;
00397     orig+=getSkip(srcLayer);
00398   }
00399 }
00400 
00401 
00402 
00403 /*! This function is lifted from Sony's ImageObserver sample code.
00404   Here's Sony's original license for the file (ImageObserver.cc) that contained this function:
00405   <pre>
00406   Copyright 2002,2003 Sony Corporation 
00407     
00408   Permission to use, copy, modify, and redistribute this software for
00409   non-commercial use is hereby granted.
00410     
00411   This software is provided "as is" without warranty of any kind,
00412   either expressed or implied, including but not limited to the
00413   implied warranties of fitness for a particular purpose.
00414   </pre>
00415 */
00416 void RawCameraGenerator::reconstructImage() {
00417   byte* yLLPtr = getImage(numLayers-2,CHAN_Y);
00418   byte* yLHPtr = getImage(numLayers-2,CHAN_Y_DY);
00419   byte* yHLPtr = getImage(numLayers-2,CHAN_Y_DX);
00420   byte* yHHPtr = getImage(numLayers-2,CHAN_Y_DXDY);
00421   
00422   unsigned int w = getWidth(numLayers-2);
00423   unsigned int h = getWidth(numLayers-2);
00424   unsigned int skip = getSkip(numLayers-2);
00425   
00426   unsigned char* img = images[numLayers-1][CHAN_Y];
00427   ASSERTRET(img!=NULL,"image destination NULL");
00428 
00429   unsigned char* iptr0 = img;
00430   unsigned char* iptr1 = iptr0 + 2*w;
00431     
00432   for (unsigned int y = 0; y < h; y++) {
00433     for (unsigned int x = 0; x < w; x++) {
00434       //
00435       // yLH, yHL, yHH : offset binary [0, 255] -> signed int [-128, 127]
00436       //
00437       short yLL = (short)*yLLPtr++;
00438       short yLH = (short)*yLHPtr++ - 128;
00439       short yHL = (short)*yHLPtr++ - 128;
00440       short yHH = (short)*yHHPtr++ - 128;
00441 
00442       short a = yLL + yLH + yHL + yHH; // ypix11
00443       short b = 2 * (yLL + yLH);       // ypix11 + ypix01
00444       short c = 2 * (yLL + yHL);       // ypix11 + ypix10
00445       short d = 2 * (yLL + yHH);       // ypix11 + ypix00
00446             
00447       *iptr0++ = clipRange(d - a);
00448       *iptr0++ = clipRange(c - a);
00449       *iptr1++ = clipRange(b - a);
00450       *iptr1++ = clipRange(a);
00451     }
00452     yLLPtr += skip;
00453     yLHPtr += skip;
00454     yHLPtr += skip;
00455     yHHPtr += skip;
00456     iptr0  = iptr1;
00457     iptr1  += 2*w;
00458   }
00459 }
00460 
00461 
00462 /*! @file
00463  * @brief Implements RawCameraGenerator, which generates FilterBankEvents containing raw camera images
00464  * @author ejt (Creator)
00465  */
00466 

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