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

Tekkotsu v2.0
Generated Wed Jan 21 03:20:29 2004 by Doxygen 1.3.4