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