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

Tekkotsu v2.4.1
Generated Tue Aug 16 16:32:48 2005 by Doxygen 1.4.4