Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ImageStreamDriver.cc

Go to the documentation of this file.
00001 #include "ImageStreamDriver.h"
00002 #include "Shared/get_time.h"
00003 #include "Shared/MarkScope.h"
00004 #include "Shared/RobotInfo.h"
00005 #include "Shared/Config.h"
00006 #include "Shared/ImageUtil.h"
00007 #include "Shared/debuget.h"
00008 
00009 using namespace std; 
00010 
00011 const char * ImageStreamDriver::formatNames[NUM_FORMATS+1] = { "yuv", "png", "jpeg", "tekkotsu", "" };
00012 //declare explicit instances of the NamedEnumerations we're using
00013 //(cuts down on some pretty significant binary size / debugging symbol bloat)
00014 INSTANTIATE_NAMEDENUMERATION_STATICS(ImageStreamDriver::format_t);
00015 
00016 const std::string ImageStreamDriver::autoRegisterImageStreamDriver = DeviceDriver::getRegistry().registerType<ImageStreamDriver>("ImageStream");
00017 
00018 unsigned int ImageStreamDriver::nextTimestamp() {
00019   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00020   if(comm==NULL || !comm->isReadable())
00021     return -1U;
00022   return get_time();
00023 }
00024 
00025 unsigned int ImageStreamDriver::getData(const char *& payload, unsigned int& payloadSize, unsigned int& timestamp, std::string& name) {
00026   payload=NULL; payloadSize=0; // in case of error
00027   CommPort * comm = CommPort::getRegistry().getInstance(commName);
00028   if(comm==NULL || !comm->isReadable())
00029     return frameNumber;
00030   /*unsigned int t=get_time();
00031   if(timestamp>t)
00032     usleep(static_cast<unsigned int>((timestamp-t)*1000/(getTimeScale()>0?getTimeScale():1.f)));
00033   if(thread==NULL) // in case we shut down while waiting
00034     return frameNumber;*/
00035 
00036   unsigned int layer=0;
00037   unsigned int width=CameraResolutionX;
00038   unsigned int height=CameraResolutionY;
00039   unsigned int components=3;
00040   {
00041     MarkScope autolock(*comm);
00042     //std::ostream os(&comm->getWriteStreambuf());
00043     std::istream is(&comm->getReadStreambuf());
00044     switch(*format) {
00045       case FORMAT_YUV: {
00046         unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00047         buffer.resize(remain);
00048         char * buf=&buffer.front();
00049         if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00050         if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00051         if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00052         if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00053         is.read(buf,remain);
00054         ++frameNumber;
00055       } break;
00056         
00057       case FORMAT_PNG: {
00058         size_t w,h,c;
00059         bool isFirst=(img==NULL);
00060         if(!image_util::decodePNG(is,w,h,c,img,imgSize)) {
00061           cerr << "ImageStreamDriver PNG decompression failed" << endl;
00062           return frameNumber;
00063         }
00064         width=w;
00065         height=h;
00066         components=c;
00067         
00068         ASSERT(imgSize==width*height*components,"image size doesn't match expected size");
00069         unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00070         buffer.resize(remain);
00071         char * buf=&buffer.front();
00072         if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00073         if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00074         if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00075         if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00076         if(isFirst) {
00077           memcpy(buf,img,imgSize);
00078           delete [] img;
00079           img=buf;
00080         }
00081         
00082         ++frameNumber;
00083       } break;
00084         
00085       case FORMAT_JPEG: {
00086         size_t w,h,c;
00087         bool isFirst=(img==NULL);
00088         if(!image_util::decodeJPEG(is,w,h,c,img,imgSize)) {
00089           cerr << "ImageStreamDriver JPEG decompression failed" << endl;
00090           return frameNumber;
00091         }
00092         width=w;
00093         height=h;
00094         components=c;
00095         
00096         ASSERT(imgSize==width*height*components,"image size doesn't match expected size");
00097         unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00098         buffer.resize(remain);
00099         char * buf=&buffer.front();
00100         if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00101         if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00102         if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00103         if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00104         if(isFirst) {
00105           memcpy(buf,img,imgSize);
00106           delete [] img;
00107           img=buf;
00108         }
00109         
00110         ++frameNumber;
00111       } break;
00112         
00113       case FORMAT_TEKKOTSU: {
00114         // ugh, LoadSave handles FILE* and char*, but not iostreams...
00115         // gonna be a little ugly :(
00116         
00117         // RAW CAM BEHAVIOR HEADER
00118         char tmp[256];
00119         unsigned int len;
00120         string fmt;
00121         is.read(tmp,LoadSave::getSerializedSize(len));
00122         LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00123         if(len!=13) {
00124           cerr << "Expecting Tekkotsu image format, but image header doesn't match! (len==" << len << ")" << endl;
00125           return frameNumber;
00126         }
00127         is.read(tmp,len+1);
00128         if(strncmp(tmp,"TekkotsuImage",len+1)!=0) {
00129           tmp[len+2]='\0';
00130           cerr << "Expecting Tekkotsu image format, but image header doesn't match! (" << tmp << ")" << endl;
00131           return frameNumber;
00132         }
00133         
00134         int encoding; //Config::vision_config::RawCamConfig::encoding_t
00135         is.read(tmp,LoadSave::getSerializedSize(encoding));
00136         LoadSave::decode(encoding,tmp,LoadSave::getSerializedSize(encoding));
00137         if(encoding==Config::vision_config::RawCamConfig::ENCODE_SINGLE_CHANNEL)
00138           components=1;
00139         int compression; //Config::vision_config::RawCamConfig::compression_t
00140         is.read(tmp,LoadSave::getSerializedSize(compression));
00141         LoadSave::decode(compression,tmp,LoadSave::getSerializedSize(compression));
00142         is.read(tmp,LoadSave::getSerializedSize(width));
00143         LoadSave::decode(width,tmp,LoadSave::getSerializedSize(width));
00144         is.read(tmp,LoadSave::getSerializedSize(height));
00145         LoadSave::decode(height,tmp,LoadSave::getSerializedSize(height));
00146         unsigned int remote_timestamp;
00147         is.read(tmp,LoadSave::getSerializedSize(remote_timestamp));
00148         LoadSave::decode(remote_timestamp,tmp,LoadSave::getSerializedSize(remote_timestamp));
00149         unsigned int fnum;
00150         is.read(tmp,LoadSave::getSerializedSize(fnum));
00151         LoadSave::decode(fnum,tmp,LoadSave::getSerializedSize(fnum));
00152         
00153         // have everything we need to set up output header...
00154         unsigned int remain = LoadSave::getSerializedSize<unsigned int>()*4+width*height*components;
00155         buffer.resize(remain);
00156         char * buf=&buffer.front();
00157         if(!LoadSave::encodeInc(layer,buf,remain)) return frameNumber;
00158         if(!LoadSave::encodeInc(width,buf,remain)) return frameNumber;
00159         if(!LoadSave::encodeInc(height,buf,remain)) return frameNumber;
00160         if(!LoadSave::encodeInc(components,buf,remain)) return frameNumber;
00161         
00162         for(unsigned int i=0; i<components; ++i) {
00163           // FILTER BANK GENERATOR HEADER
00164           is.read(tmp,LoadSave::getSerializedSize(len));
00165           LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00166           if(len!=8) {
00167             cerr << "Expecting FbkImage image format, but header doesn't match! (len==" << len << ")" << endl;
00168             return frameNumber;
00169           }
00170           is.read(tmp,len+1);
00171           if(strncmp(tmp,"FbkImage",len+1)!=0) {
00172             tmp[len+2]='\0';
00173             cerr << "Expecting FbkImage image format, but image header doesn't match! (" << tmp << ")" << endl;
00174             return frameNumber;
00175           }
00176           unsigned int lwidth;
00177           is.read(tmp,LoadSave::getSerializedSize(lwidth));
00178           LoadSave::decode(lwidth,tmp,LoadSave::getSerializedSize(lwidth));
00179           unsigned int lheight;
00180           is.read(tmp,LoadSave::getSerializedSize(lheight));
00181           LoadSave::decode(lheight,tmp,LoadSave::getSerializedSize(lheight));
00182           unsigned int lyr;
00183           is.read(tmp,LoadSave::getSerializedSize(lyr));
00184           LoadSave::decode(lyr,tmp,LoadSave::getSerializedSize(lyr));
00185           unsigned int chan_id;
00186           is.read(tmp,LoadSave::getSerializedSize(chan_id));
00187           LoadSave::decode(chan_id,tmp,LoadSave::getSerializedSize(chan_id));
00188           
00189           // GENERATOR SUBCLASS HEADER
00190           is.read(tmp,LoadSave::getSerializedSize(len));
00191           LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00192           is.read(tmp,len+1);
00193           if(strcmp(tmp,"blank")==0) {
00194             int useChan=(components==1)?i:chan_id;
00195             int off=useChan;
00196             for(unsigned int y=0; y<height; y++) {
00197               for(unsigned int x=0; x<width; x++) {
00198                 buf[off]=128;
00199                 off+=components;
00200               }
00201             }
00202           } else if(strcmp(tmp,"RawImage")==0) {
00203             int useChan=(components==1)?i:chan_id;
00204             vector<char> chan(lwidth*lheight);
00205             is.read(&chan.front(),chan.size());
00206             copyImage(buf,width,height,components,&chan.front(),lwidth,lheight,useChan);
00207             
00208           } else if(strcmp(tmp,"JPEGGrayscale")==0) {
00209             int useChan=(components==1)?i:chan_id;
00210             is.read(tmp,LoadSave::getSerializedSize(len));
00211             LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00212             vector<char> jpeg(len);
00213             is.read(&jpeg.front(),jpeg.size());
00214             size_t jwidth,jheight,jcomp;
00215             image_util::decodeJPEG(&jpeg.front(), jpeg.size(), jwidth, jheight, jcomp);
00216             if(jcomp!=1) {
00217               cerr << "Got color image where grayscale was expected" << endl;
00218               return frameNumber;
00219             }
00220             vector<char> chan(jwidth*jheight);
00221             size_t tsize=chan.size();
00222             char* tchan=&chan.front();
00223             if(!image_util::decodeJPEG(&jpeg.front(), jpeg.size(), jwidth, jheight, jcomp, tchan, tsize)) {
00224               cerr << "JPEG decompression failed" << endl;
00225               return frameNumber;
00226             }
00227             copyImage(buf,width,height,components,&chan.front(),lwidth,lheight,useChan);
00228             
00229           } else if(strcmp(tmp,"JPEGColor")==0) {
00230             is.read(tmp,LoadSave::getSerializedSize(len));
00231             LoadSave::decode(len,tmp,LoadSave::getSerializedSize(len));
00232             vector<char> jpeg(len);
00233             is.read(&jpeg.front(),jpeg.size());
00234             size_t jwidth,jheight,jcomp;
00235             size_t tsize=remain;
00236             if(!image_util::decodeJPEG(&jpeg.front(), jpeg.size(), jwidth, jheight, jcomp, buf, tsize)) {
00237               cerr << "JPEG decompression failed" << endl;
00238               return frameNumber;
00239             }
00240             i=components;
00241             
00242           } else {
00243             cerr << "Unknown image generator " << tmp << endl;
00244             return frameNumber;
00245           }
00246         }
00247         
00248         if(!is)
00249           return frameNumber;
00250         frameNumber=fnum;
00251       } break;
00252     }
00253   }
00254   payload = &buffer.front();
00255   payloadSize = buffer.size();
00256   timestamp=get_time();
00257   name=instanceName;
00258   return frameNumber;
00259 }
00260 
00261 void ImageStreamDriver::setDataSourceThread(LoadDataThread* th) {
00262   if(thread==NULL && th!=NULL) {
00263     CommPort * comm = CommPort::getRegistry().getInstance(commName);
00264     if(comm!=NULL)
00265       comm->open();
00266     commName.addPrimitiveListener(this);
00267   }
00268   if(thread!=NULL && th==NULL) {
00269     CommPort * comm = CommPort::getRegistry().getInstance(commName);
00270     if(comm!=NULL)
00271       comm->close();
00272     commName.removePrimitiveListener(this);
00273   }
00274   DataSource::setDataSourceThread(th);
00275 }
00276 
00277 void ImageStreamDriver::plistValueChanged(const plist::PrimitiveBase& pl) {
00278   if(&pl==&commName) {
00279     // if here, then setDataSourceThread has been called, thus when commName changes,
00280     // need to close old one and reopen new one
00281     CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00282     if(comm!=NULL)
00283       comm->close();
00284     comm = CommPort::getRegistry().getInstance(commName);
00285     if(comm!=NULL)
00286       comm->open();
00287   } else if(&pl==&format) {
00288     CommPort * comm = CommPort::getRegistry().getInstance(commName.getPreviousValue());
00289     if(comm!=NULL) {
00290       MarkScope autolock(*comm);
00291       delete [] img;
00292       img=NULL;
00293       imgSize=0;
00294     } else {
00295       delete [] img;
00296       img=NULL;
00297       imgSize=0;
00298     }
00299   } else {
00300     std::cerr << "Unhandled value change in " << getClassName() << ": " << pl.get() << std::endl;
00301   }
00302 }
00303 
00304 void ImageStreamDriver::copyImage(char * buf, unsigned int width, unsigned int height, unsigned int channels, const char * chan, unsigned int lwidth, unsigned int lheight, unsigned int c) {
00305   if(lwidth==width && lheight==height) {
00306     // same size, straight copy
00307     for(unsigned int y=0;y<height;y++) {
00308       unsigned int datarowstart=y*width*channels+c;
00309       unsigned int tmprowstart=y*lwidth;
00310       for(unsigned int x=0;x<width;x++)
00311         buf[datarowstart+x*channels]=chan[tmprowstart+x];
00312     }
00313   } else {
00314     // upsample image
00315     
00316     //we'll linearly interpolate between pixels
00317     //METHOD A:
00318     //hold edges, interpolate through middle:
00319     //  if we have 2 samples, scaling up to 4
00320     //   index: 0   1    2   3
00321     // maps to: 0  1/3  2/3  1
00322     float xsc=(lwidth-1)/(float)(width-1);
00323     float ysc=(lheight-1)/(float)(height-1);
00324     for(unsigned int y=0;y<height-1;y++) {
00325       unsigned int datarowstart=y*width*channels+c;
00326       float ty=y*ysc;
00327       unsigned int ly=(int)ty; //lower pixel index
00328       float fy=ty-ly; //upper pixel weight
00329       unsigned int tmprowstart=ly*lwidth;
00330       for(unsigned int x=0;x<width-1;x++) {
00331         float tx=x*xsc;
00332         unsigned int lx=(int)tx; //lower pixel index
00333         float fx=tx-lx; //upper pixel weight
00334         
00335         float lv=((int)chan[tmprowstart+lx]&0xFF)*(1-fx)+((int)chan[tmprowstart+lx+1]&0xFF)*fx;
00336         float uv=((int)chan[tmprowstart+lx+lwidth]&0xFF)*(1-fx)+((int)chan[tmprowstart+lx+1+lwidth]&0xFF)*fx;
00337         buf[datarowstart+x*channels]=(char)(lv*(1-fy)+uv*fy);
00338       }
00339       buf[datarowstart+(width-1)*channels]=chan[tmprowstart+lwidth-1];
00340     }
00341     unsigned int datarowstart=width*(height-1)*channels+c;
00342     unsigned int tmprowstart=lwidth*(lheight-1);
00343     for(unsigned int x=0;x<width-1;x++) {
00344       float tx=x*xsc;
00345       unsigned int lx=(int)tx; //lower pixel index
00346       float fx=tx-lx; //upper pixel weight
00347       buf[datarowstart+x*channels]=(char)(((int)chan[tmprowstart+lx]&0xFF)*(1-fx)+((int)chan[tmprowstart+lx+1]&0xFF)*fx);
00348     }
00349     buf[datarowstart+(width-1)*channels]=chan[tmprowstart+lwidth-1];
00350     
00351     //Unfortunately, pixels are simply interleaved, starting at the
00352     //top right.  So, Method A will stretch things to the bottom-right
00353     //a bit.  This method holds left edge and spacing, so it lines up
00354     //better with what's being transmitted (but the bottom right edges
00355     //wind up smeared)
00356     //METHOD B:
00357     //  if we have 2 samples, scaling up to 4
00358     //   index: 0   1    2   3
00359     // maps to: 0  1/2   1   1  <-- this last one would be 3/2, so we have to replicate 1
00360     /*float xsc=(chanW)/(float)(width);
00361     float ysc=(chanH)/(float)(height);
00362     unsigned int xgap=(unsigned int)roundf(1.0f/xsc);
00363     unsigned int ygap=(unsigned int)roundf(1.0f/ysc);
00364     for(unsigned int y=0;y<height-ygap;y++) {
00365       unsigned int datarowstart=y*width*channels+c;
00366       float ty=y*ysc;
00367       unsigned int ly=(int)ty; //lower pixel index
00368       float fy=ty-ly; //upper pixel weight
00369       unsigned int tmprowstart=ly*chanW;
00370       for(unsigned int x=0;x<width-xgap;x++) {
00371         float tx=x*xsc;
00372         unsigned int lx=(int)tx; //lower pixel index
00373         float fx=tx-lx; //upper pixel weight
00374         
00375         float lv=(chan[tmprowstart+lx]&0xFF)*(1-fx)+(chan[tmprowstart+lx+1]&0xFF)*fx;
00376         float uv=(chan[tmprowstart+lx+chanW]&0xFF)*(1-fx)+(chan[tmprowstart+lx+1+chanW]&0xFF)*fx;
00377         buf[datarowstart+x*channels]=(char)(lv*(1-fy)+uv*fy);
00378       }
00379       for(unsigned int x=width-xgap;x<width;x++) {
00380         float lv=(chan[tmprowstart+chanW-1]&0xFF);
00381         float uv=(chan[tmprowstart+chanW-1+chanW]&0xFF);
00382         buf[datarowstart+x*channels]=(char)(lv*(1-fy)+uv*fy);
00383       }
00384     }
00385     for(unsigned int y=height-ygap;y<height;y++) {
00386       unsigned int datarowstart=y*width*channels+c;
00387       unsigned int tmprowstart=chanW*(chanH-1);
00388       for(unsigned int x=0;x<width-xgap;x++) {
00389         float tx=x*xsc;
00390         unsigned int lx=(int)tx; //lower pixel index
00391         float fx=tx-lx; //upper pixel weight
00392         
00393         float lv=(chan[tmprowstart+lx]&0xFF)*(1-fx)+(chan[tmprowstart+lx+1]&0xFF)*fx;
00394         buf[datarowstart+x*channels]=(byte)(lv);
00395       }
00396       for(unsigned int x=width-xgap;x<width;x++)
00397         buf[datarowstart+x*channels]=chan[tmprowstart+chanW-1];
00398     }*/
00399   }
00400 }
00401 
00402 /*! @file
00403  * @brief 
00404  * @author Ethan Tira-Thompson (ejt) (Creator)
00405  *
00406  * $Author: ejt $
00407  * $Name: tekkotsu-4_0 $
00408  * $Revision: 1.4 $
00409  * $State: Exp $
00410  * $Date: 2007/11/05 00:20:11 $
00411  */

Tekkotsu Hardware Abstraction Layer 4.0
Generated Thu Nov 22 01:00:53 2007 by Doxygen 1.5.4