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

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