00001 #ifndef PLATFORM_APERIOS
00002
00003 #include "BufferedImageGenerator.h"
00004 #include "Events/DataEvent.h"
00005 #include "Events/FilterBankEvent.h"
00006 #include "Wireless/Socket.h"
00007 #include "Events/EventRouter.h"
00008 #include "Shared/debuget.h"
00009 #include "Shared/ProjectInterface.h"
00010
00011 using namespace std;
00012
00013 void BufferedImageGenerator::doEvent() {
00014 if(event->getGeneratorID()!=getListenGeneratorID() || event->getSourceID()!=getListenSourceID())
00015 return;
00016 if(event->getTypeID()==EventBase::activateETID) {
00017 const DataEvent<ImageSource>* data=dynamic_cast<const DataEvent<ImageSource>*>(event);
00018 if(data==NULL) {
00019 serr->printf("Error: %s(%s) received event of the wrong type",getClassName().c_str(),getName().c_str());
00020 return;
00021 }
00022 if(imgsrc.layer!=data->getData().layer || imgsrc.channels!=data->getData().channels) {
00023
00024
00025 unsigned int i;
00026 for(i=0; i<data->getData().layer; i++) {
00027 increments[i] = 1;
00028 strides[i]=data->getData().width>>(data->getData().layer-i);
00029 skips[i]=0;
00030 }
00031 increments[i] = data->getData().channels;
00032 strides[i]=data->getData().width*data->getData().channels;
00033 skips[i]=0;
00034 for(++i; i<numLayers; i++) {
00035 increments[i] = 1;
00036 strides[i]=data->getData().width<<(i-data->getData().layer);
00037 skips[i]=0;
00038 }
00039 }
00040 imgsrc=data->getData();
00041 sysFrameNumber=frameNumber=imgsrc.frameIndex;
00042 invalidateCaches();
00043 if(numLayers>0) {
00044
00045 if(imgsrc.width!=getWidth(imgsrc.layer) || imgsrc.height!=getHeight(imgsrc.layer)) {
00046 freeCaches();
00047 setDimensions();
00048 if(framesProcessed==0)
00049 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]);
00050 else
00051 serr->printf("WARNING: the image dimensions have changed since last frame, \"full\" layer now %dx%d\n",widths[ProjectInterface::fullLayer],heights[ProjectInterface::fullLayer]);
00052 erouter->postEvent(EventBase::cameraResolutionEGID,event->getSourceID(),EventBase::statusETID);
00053 } else if(framesProcessed==0) {
00054
00055 setDimensions();
00056 }
00057 }
00058
00059 unsigned int i=imgsrc.layer;
00060 for(unsigned int j=0; j<imgsrc.channels; j++) {
00061 if(isAllocated[i][j]) {
00062 delete [] images[i][j];
00063 images[i][j]=NULL;
00064 isAllocated[i][j]=false;
00065 }
00066 imageValids[i][j]=true;
00067 }
00068 images[i][RawCameraGenerator::CHAN_Y]=imgsrc.img+0;
00069 images[i][RawCameraGenerator::CHAN_U]=imgsrc.img+1;
00070 images[i][RawCameraGenerator::CHAN_V]=imgsrc.img+2;
00071 framesProcessed++;
00072 }
00073 erouter->postEvent(FilterBankEvent(this,getGeneratorID(),getSourceID(),event->getTypeID()));
00074 }
00075
00076 unsigned int
00077 BufferedImageGenerator::getBinSize() const {
00078 unsigned int used=FilterBankGenerator::getBinSize();
00079 used+=getSerializedSize("RawImage");
00080 used+=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00081 return used;
00082 }
00083
00084 unsigned int
00085 BufferedImageGenerator::loadBuffer(const char buf[], unsigned int len, const char* filename) {
00086 unsigned int origlen=len;
00087 if(!checkInc(FilterBankGenerator::loadBuffer(buf,len,filename),buf,len)) return 0;
00088 std::string tmp;
00089 if(decode(tmp,buf,len)) return 0;
00090 if(tmp!="RawImage") {
00091 serr->printf("Unhandled image type for BufferedImageGenerator: %s",tmp.c_str());
00092 return 0;
00093 } else if(selectedSaveLayer!=numLayers-1) {
00094 serr->printf("Can't load into BufferedImageGenerator layer %d!=%d",selectedSaveLayer,numLayers-1);
00095 return 0;
00096 } else {
00097 if(images[selectedSaveLayer][selectedSaveChannel]==NULL)
00098 images[selectedSaveLayer][selectedSaveChannel]=createImageCache(selectedSaveLayer,selectedSaveChannel);
00099 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00100 unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00101 ASSERTRETVAL(used<=len,"buffer too small",0);
00102 memcpy(img,buf,used);
00103 len-=used; buf+=used;
00104 imageValids[selectedSaveLayer][selectedSaveChannel]=true;
00105 return origlen-len;
00106 }
00107 }
00108
00109 unsigned int
00110 BufferedImageGenerator::saveBuffer(char buf[], unsigned int len) const {
00111 unsigned int origlen=len;
00112 if(!checkInc(FilterBankGenerator::saveBuffer(buf,len),buf,len)) return 0;
00113 if(!encodeInc("RawImage",buf,len)) return 0;
00114
00115 if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00116 serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00117 return 0;
00118 }
00119 if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00120 serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00121 return 0;
00122 }
00123 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00124 unsigned int used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00125 ASSERTRETVAL(used<=len,"buffer too small " << len << ' ' << widths[selectedSaveLayer] << ' ' << heights[selectedSaveLayer],0);
00126 unsigned int inc=getIncrement(selectedSaveLayer);
00127 if(inc==1) {
00128
00129 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00130 memcpy(buf,img,widths[selectedSaveLayer]);
00131 buf+=widths[selectedSaveLayer];
00132 img+=getStride(selectedSaveLayer);
00133 }
00134 } else {
00135
00136 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00137 unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00138 while(img!=rowend) {
00139 *buf++=*img;
00140 img+=inc;
00141 }
00142 img+=getSkip(selectedSaveLayer);
00143 }
00144 }
00145 len-=used;
00146
00147 return origlen-len;
00148 }
00149
00150 unsigned int
00151 BufferedImageGenerator::saveFileStream(FILE * f) const {
00152 unsigned int totalused=0;
00153 unsigned int used;
00154 {
00155 unsigned int sz=FilterBankGenerator::getBinSize();
00156 char * buf = new char[sz];
00157 memset(buf,0xF0,sz);
00158 if(buf==NULL) {
00159 std::cout << "*** WARNING could not allocate " << sz << " bytes for loadFile";
00160 return 0;
00161 }
00162 unsigned int resp=FilterBankGenerator::saveBuffer(buf,sz);
00163 if(resp==0) {
00164 std::cout << "*** WARNING saveBuffer didn't write any data (possibly due to overflow or other error)" << std::endl;
00165 fwrite(buf,1,sz,f);
00166 } else {
00167 unsigned int wrote=fwrite(buf,1,resp,f);
00168 if(wrote!=resp)
00169 std::cout << "*** WARNING short write (wrote " << wrote << ", expected " << resp << ")" << std::endl;
00170 }
00171 delete [] buf;
00172 used=resp;
00173 }
00174 if(0==used) return 0;
00175 totalused+=used;
00176 if(0==(used=encode("RawImage",f))) return 0;
00177 totalused+=used;
00178
00179 if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00180 serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00181 return 0;
00182 }
00183 if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00184 serr->printf("BufferedImageGenerator::saveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00185 return 0;
00186 }
00187 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00188 used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00189 unsigned int inc=getIncrement(selectedSaveLayer);
00190 if(inc==1) {
00191
00192 sout->printf("Saving %d by %d\n",widths[selectedSaveLayer],heights[selectedSaveLayer]);
00193 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00194 if(fwrite(img,widths[selectedSaveLayer],1,f)==0) {
00195 serr->printf("short write on image data - ran out of space?\n");
00196 return 0;
00197 }
00198 img+=getStride(selectedSaveLayer);
00199 }
00200 } else {
00201
00202 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00203 unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00204 while(img!=rowend) {
00205 if(fputc(*img,f)==EOF) {
00206 serr->printf("short write on image data - ran out of space?\n");
00207 return 0;
00208 }
00209 img+=inc;
00210 }
00211 img+=getSkip(selectedSaveLayer);
00212 }
00213 }
00214 totalused+=used;
00215
00216 return totalused;
00217 }
00218
00219 void BufferedImageGenerator::freeCaches() {
00220 FilterBankGenerator::freeCaches();
00221 for(unsigned int i=0; i<numLayers; i++)
00222 for(unsigned int j=0; j<numChannels; j++)
00223 isAllocated[i][j]=false;
00224 }
00225
00226 void BufferedImageGenerator::invalidateCaches() {
00227 for(unsigned int i=0; i<numLayers; i++)
00228 for(unsigned int j=0; j<numChannels; j++) {
00229 if(!isAllocated[i][j])
00230 images[i][j]=NULL;
00231 imageValids[i][j]=false;
00232 }
00233 }
00234
00235 unsigned char * BufferedImageGenerator::createImageCache(unsigned int layer, unsigned int channel) const {
00236 if(layer!=imgsrc.layer || imgsrc.channels==1) {
00237 isAllocated[layer][channel]=true;
00238 return new unsigned char[widths[layer]*heights[layer]];
00239 } else {
00240 ASSERT(channel>=imgsrc.channels,"createImageCache for image that should come from src")
00241
00242 unsigned int base=(channel/imgsrc.channels)*imgsrc.channels;
00243 if(images[layer][base]==NULL)
00244 images[layer][base]=new unsigned char[widths[layer]*heights[layer]*imgsrc.channels];
00245 for(unsigned int i=base+1; i<base+imgsrc.channels; ++i) {
00246 ASSERT(!isAllocated[layer][i],"createImageCache for image already allocated!");
00247 ASSERT(images[layer][i]==NULL,"createImageCache for image already assigned!");
00248 images[layer][i]=images[layer][i-1]+1;
00249 }
00250 isAllocated[layer][base]=true;
00251 return images[layer][channel];
00252 }
00253 }
00254 void BufferedImageGenerator::calcImage(unsigned int layer, unsigned int channel) {
00255
00256 ASSERTRET(layer!=imgsrc.layer || channel>=imgsrc.channels, "calcImage on src channel?");
00257 switch(channel) {
00258 case RawCameraGenerator::CHAN_Y:
00259 if(layer>imgsrc.layer) upsampleImage(imgsrc.layer,channel,layer);
00260 else downsampleImage(layer,channel);
00261 break;
00262 case RawCameraGenerator::CHAN_U:
00263 case RawCameraGenerator::CHAN_V:
00264 if(imgsrc.channels>1) {
00265 if(layer>imgsrc.layer) upsampleImage(imgsrc.layer,channel,layer);
00266 else downsampleImage(layer,channel);
00267 } else
00268 memset(images[layer][channel],128,widths[layer]*heights[layer]);
00269 break;
00270 case RawCameraGenerator::CHAN_Y_DX:
00271 calcDx(layer);
00272 break;
00273 case RawCameraGenerator::CHAN_Y_DY:
00274 calcDy(layer);
00275 break;
00276 case RawCameraGenerator::CHAN_Y_DXDY:
00277 calcDxDy(layer);
00278 break;
00279 default:
00280 cerr << "Bad layer selection!" << endl;
00281 }
00282 }
00283 void BufferedImageGenerator::setDimensions() {
00284 if(imgsrc.img==NULL)
00285 return;
00286
00287 for(unsigned int i=0; i<=imgsrc.layer; i++) {
00288
00289 unsigned int s=(1<<(imgsrc.layer-i));
00290
00291 widths[i]=strides[i]=imgsrc.width/s;
00292 heights[i]=imgsrc.height/s;
00293
00294 skips[i]=0;
00295 increments[i]=1;
00296
00297 }
00298
00299 increments[imgsrc.layer] = imgsrc.channels;
00300 widths[imgsrc.layer]=imgsrc.width;
00301 heights[imgsrc.layer]=imgsrc.height;
00302 strides[imgsrc.layer]=imgsrc.width*imgsrc.channels;
00303 skips[imgsrc.layer]=0;
00304
00305 for(unsigned int i=imgsrc.layer+1; i<numLayers; i++) {
00306
00307 unsigned int s=(1<<(i-imgsrc.layer));
00308
00309 widths[i]=strides[i]=imgsrc.width*s;
00310 heights[i]=imgsrc.height*s;
00311
00312 skips[i]=0;
00313 increments[i]=1;
00314 }
00315 }
00316 void BufferedImageGenerator::destruct() {
00317 FilterBankGenerator::destruct();
00318 for(unsigned int i=0; i<numLayers; i++)
00319 delete [] isAllocated[i];
00320 delete [] isAllocated;
00321 isAllocated=NULL;
00322 }
00323 void BufferedImageGenerator::setNumImages(unsigned int nLayers, unsigned int nChannels) {
00324 if(nLayers==numLayers && nChannels==numChannels)
00325 return;
00326 FilterBankGenerator::setNumImages(nLayers,nChannels);
00327 isAllocated=new bool*[numLayers];
00328 for(unsigned int i=0; i<numLayers; i++) {
00329 isAllocated[i]=new bool[numChannels];
00330 for(unsigned int j=0; j<numChannels; j++)
00331 isAllocated[i][j]=false;
00332 }
00333 setDimensions();
00334 }
00335
00336 void BufferedImageGenerator::upsampleImage(unsigned int srcLayer, unsigned int chan, unsigned int destLayer) {
00337 ASSERTRET(destLayer!=imgsrc.layer,"upsample into source layer")
00338 unsigned char * cur=images[destLayer][chan];
00339 ASSERTRET(cur!=NULL,"destination layer is NULL");
00340 unsigned char * orig=getImage(srcLayer,chan);
00341 ASSERTRET(orig!=NULL,"source layer is NULL");
00342 unsigned int width=widths[destLayer];
00343 unsigned int height=heights[destLayer];
00344 unsigned int inc=getIncrement(srcLayer);
00345 int power=destLayer-srcLayer;
00346 ASSERTRET(power>0,"upsampleImage attempting to downsample")
00347
00348 unsigned char * const imgend=cur+width*height;
00349 while(cur!=imgend) {
00350 unsigned char * const row=cur;
00351 unsigned char * const rowend=cur+width;
00352
00353 while(cur<rowend) {
00354 for(int p=1<<power; p>0; p--)
00355 *cur++=*orig;
00356 orig+=inc;
00357 }
00358
00359 for(int p=0; p<power; p++) {
00360 unsigned int avail=width*(1<<p);
00361 memcpy(cur,row,avail);
00362 cur+=avail;
00363 }
00364 orig+=getSkip(srcLayer);
00365 }
00366 imageValids[destLayer][chan]=true;
00367 }
00368
00369 void BufferedImageGenerator::downsampleImage(unsigned int destLayer, unsigned int chan) {
00370 ASSERTRET(destLayer!=imgsrc.layer,"downsample into source layer")
00371
00372 unsigned int layer=destLayer;
00373 while(layer<numLayers && !imageValids[layer][chan])
00374 layer++;
00375 ASSERTRET(layer<numLayers,"valid layer to downsample from could not be found!");
00376 if (!(layer<numLayers))
00377 std::cout << "layer = " << layer << " numLayers = " << numLayers << std::endl;
00378
00379
00380 for(unsigned int srcL=layer--; layer>=destLayer; srcL=layer--) {
00381 unsigned int srcInc=getIncrement(srcL);
00382 unsigned char * s=getImage(srcL,chan);
00383 if(images[layer][chan]==NULL)
00384 images[layer][chan]=createImageCache(layer,chan);
00385 unsigned char * dst=images[layer][chan];
00386 unsigned char * const dstEnd=dst+widths[layer]*heights[layer];
00387 while(dst!=dstEnd) {
00388 unsigned char * const rowEnd=dst+widths[layer];
00389 while(dst!=rowEnd) {
00390 unsigned short x=*s;
00391 x+=*(s+strides[srcL]);
00392 s+=srcInc;
00393 x+=*s;
00394 x+=*(s+strides[srcL]);
00395 s+=srcInc;
00396 *dst++ = x/4;
00397 }
00398 s+=strides[srcL];
00399 }
00400 imageValids[layer][chan]=true;
00401 }
00402 }
00403
00404 void BufferedImageGenerator::calcDx(unsigned int layer, unsigned int srcChan, unsigned int dstChan) {
00405 unsigned char * s=getImage(layer,srcChan);
00406 unsigned char * dst=images[layer][dstChan];
00407 unsigned int inc=getIncrement(layer);
00408 unsigned int skip=getSkip(layer)+inc;
00409 unsigned char * const dstEnd=dst+getStride(layer)*heights[layer];
00410 unsigned int sc=2;
00411 while(dst!=dstEnd) {
00412 unsigned char * const rowEnd=dst+widths[layer]*inc-inc;
00413 unsigned char left,right;
00414 while(dst!=rowEnd) {
00415 left=(*s)>>sc;
00416 s+=inc;
00417 right=(*s)>>sc;
00418 *dst=right+128-left;
00419 dst+=inc;
00420 }
00421 *dst=128;
00422 dst+=skip;
00423 s+=skip;
00424 }
00425 imageValids[layer][dstChan]=true;
00426 }
00427 void BufferedImageGenerator::calcDy(unsigned int layer, unsigned int srcChan, unsigned int dstChan) {
00428 unsigned char * s=getImage(layer,srcChan);
00429 unsigned char * dst=images[layer][dstChan];
00430 unsigned int inc=getIncrement(layer);
00431 unsigned int stride=getStride(layer);
00432 unsigned char * const dstEnd=dst+widths[layer]*inc;
00433 unsigned int sc=2;
00434 while(dst!=dstEnd) {
00435 unsigned char * const colEnd=dst+heights[layer]*stride-stride;
00436 unsigned char top,bottom;
00437 while(dst!=colEnd) {
00438 top=(*s)>>sc;
00439 s+=stride;
00440 bottom=(*s)>>sc;
00441 *dst=bottom+128-top;
00442 dst+=stride;
00443 }
00444 *dst=128;
00445 dst-=heights[layer]*stride-stride;
00446 s-=heights[layer]*stride-stride;
00447 dst+=inc;
00448 s+=inc;
00449 }
00450 imageValids[layer][dstChan]=true;
00451 }
00452 void BufferedImageGenerator::calcDxDy(unsigned int layer) {
00453
00454 if(imageValids[layer][RawCameraGenerator::CHAN_Y_DX]) {
00455 calcDy(layer,RawCameraGenerator::CHAN_Y_DX,RawCameraGenerator::CHAN_Y_DXDY);
00456 imageValids[layer][RawCameraGenerator::CHAN_Y_DXDY]=true;
00457 } else if(imageValids[layer][RawCameraGenerator::CHAN_Y_DY]) {
00458 calcDx(layer,RawCameraGenerator::CHAN_Y_DY,RawCameraGenerator::CHAN_Y_DXDY);
00459 imageValids[layer][RawCameraGenerator::CHAN_Y_DXDY]=true;
00460 } else {
00461
00462 getImage(layer,RawCameraGenerator::CHAN_Y_DX);
00463 calcDy(layer,RawCameraGenerator::CHAN_Y_DX,RawCameraGenerator::CHAN_Y_DXDY);
00464 }
00465 }
00466
00467
00468
00469
00470
00471
00472
00473 #endif