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

Tekkotsu v2.2
Generated Tue Oct 19 14:19:15 2004 by Doxygen 1.3.9.1