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

Tekkotsu v2.2.2
Generated Tue Jan 4 15:43:15 2005 by Doxygen 1.4.0