RawCameraGenerator.ccGo 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 "Shared/ODataFormats.h"
00010 #include "OFbkImage.h"
00011
00012 #include "Shared/debuget.h"
00013
00014 #include <float.h>
00015
00016 RawCameraGenerator::RawCameraGenerator(unsigned int numRawLayers, unsigned int numCalcLayers, unsigned int mysid, EventBase::EventGeneratorID_t gid, unsigned int sid)
00017 : FilterBankGenerator("RawCameraGenerator","RawCameraGenerator",EventBase::visRawCameraEGID,mysid,gid,sid), numRealLayers(numRawLayers), layers(NULL), imageInfos(NULL)
00018 {
00019
00020
00021
00022
00023
00024 unsetAutoListen();
00025
00026 setNumImages(numCalcLayers,NUM_CHANNELS);
00027 }
00028
00029
00030
00031
00032
00033 void
00034 RawCameraGenerator::processEvent(const EventBase& event) {
00035 EventGeneratorBase::processEvent(event);
00036 if(event.getGeneratorID()!=getListenGeneratorID() || event.getSourceID()!=getListenSourceID())
00037 return;
00038 if(event.getTypeID()==EventBase::activateETID) {
00039 typedef DataEvent<const OFbkImageVectorData*> OFbkEvent;
00040 const OFbkEvent& fbkevent=dynamic_cast<const OFbkEvent& >(event);
00041 OFbkImageVectorData& fbkdat=*const_cast<OFbkImageVectorData*>(fbkevent.getData());
00042 for(unsigned int res=0; res<numRealLayers; res++) {
00043 layers[numLayers-2-res] = fbkdat.GetData(res);
00044 imageInfos[numLayers-2-res] = fbkdat.GetInfo(res);
00045 }
00046 {
00047 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[numLayers-2]), const_cast<unsigned char*>(layers[numLayers-2]), ofbkimageBAND_Y);
00048
00049 sysFrameNumber=frameNumber=*(int*)(img.Pointer()+(img.Height()-1)*(img.Skip()+img.Width()));
00050 }
00051 unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00052 bool dimchange=false;
00053 for(unsigned int res=numNotRealLayers; res<numLayers-1; res++) {
00054 if(widths[res]!=imageInfos[res]->width || heights[res]!=imageInfos[res]->height) {
00055 dimchange=true;
00056 serr->printf("WARNING: the image dimensions changed, now %dx%d\n",widths[numLayers-1],heights[numLayers-1]);
00057 widths[res] = imageInfos[res]->width;
00058 heights[res] = imageInfos[res]->height;
00059 }
00060
00061 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[res]), const_cast<unsigned char*>(layers[res]), ofbkimageBAND_Y);
00062 skips[res]=img.Skip();
00063 strides[res]=skips[res]+widths[res];
00064
00065 ASSERT(static_cast<unsigned int>(img.Width())==getWidth(res),"Widths don't agree for layer "<<res);
00066 ASSERT(static_cast<unsigned int>(img.Height())==getHeight(res),"Heights don't agree for layer "<<res);
00067 }
00068 if(widths[numLayers-2]*2!=widths[numLayers-1] || heights[numLayers-2]*2!=heights[numLayers-1]) {
00069
00070
00071
00072 serr->printf("WARNING: the image dimensions don't match values predicted by RobotInfo consts, now %dx%d\n",widths[numLayers-1],heights[numLayers-1]);
00073 freeCaches();
00074 dimchange=true;
00075 } else if(strides[numLayers-1]==0) {
00076
00077 dimchange=true;
00078 }
00079 if(dimchange)
00080 setDimensions();
00081 float testaspect=widths[numLayers-2]/(float)heights[numLayers-2];
00082 if(fabs(testaspect-config->vision.aspectRatio)>FLT_EPSILON) {
00083 serr->printf("WARNING: the image aspect ratio changed, was %g, now %g (diff %g)\n",config->vision.aspectRatio,testaspect,testaspect-config->vision.aspectRatio);
00084 config->vision.aspectRatio=testaspect;
00085 if(testaspect>1) {
00086 config->vision.x_range=1;
00087 config->vision.y_range=1/testaspect;
00088 } else {
00089 config->vision.x_range=testaspect;
00090 config->vision.y_range=1;
00091 }
00092 }
00093
00094 invalidateCaches();
00095 framesProcessed++;
00096 }
00097 erouter->postEvent(new FilterBankEvent(this,getGeneratorID(),getSourceID(),event.getTypeID()));
00098 }
00099
00100 unsigned int
00101 RawCameraGenerator::getBinSize() const {
00102 unsigned int used=FilterBankGenerator::getBinSize();
00103 used+=strlen("RawImage")+LoadSave::stringpad;
00104 used+=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00105 return used;
00106 }
00107
00108 unsigned int
00109 RawCameraGenerator::LoadBuffer(const char buf[], unsigned int len) {
00110 unsigned int origlen=len;
00111 unsigned int used;
00112 std::string tmp;
00113 if(0==(used=FilterBankGenerator::LoadBuffer(buf,len))) return 0;
00114 len-=used; buf+=used;
00115 if(0==(used=decode(tmp,buf,len))) return 0;
00116 len-=used; buf+=used;
00117 if(tmp!="RawImage") {
00118 serr->printf("Unhandled image type for RawCameraGenerator: %s",tmp.c_str());
00119 return 0;
00120 } else if(selectedSaveLayer!=numLayers-1) {
00121 serr->printf("Can't load into RawCameraGenerator layer %d!=%d",selectedSaveLayer,numLayers-1);
00122 return 0;
00123 } else {
00124 if(images[selectedSaveLayer][selectedSaveChannel]==NULL)
00125 images[selectedSaveLayer][selectedSaveChannel]=createImageCache(selectedSaveLayer,selectedSaveChannel);
00126 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00127 used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00128 ASSERTRETVAL(used<=len,"buffer too small",0);
00129 memcpy(img,buf,used);
00130 len-=used; buf+=used;
00131 imageValids[selectedSaveLayer][selectedSaveChannel]=true;
00132 return origlen-len;
00133 }
00134 }
00135
00136 unsigned int
00137 RawCameraGenerator::SaveBuffer(char buf[], unsigned int len) const {
00138 unsigned int origlen=len;
00139 unsigned int used;
00140 if(0==(used=FilterBankGenerator::SaveBuffer(buf,len))) return 0;
00141 len-=used; buf+=used;
00142 if(0==(used=encode("RawImage",buf,len))) return 0;
00143 len-=used; buf+=used;
00144
00145 if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00146 serr->printf("RawCameraGenerator::SaveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00147 return 0;
00148 }
00149 if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00150 serr->printf("RawCameraGenerator::SaveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00151 return 0;
00152 }
00153 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00154 used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00155 ASSERTRETVAL(used<=len,"buffer too small",0);
00156 unsigned int inc=getIncrement(selectedSaveLayer);
00157 if(inc==1) {
00158
00159 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00160 memcpy(buf,img,widths[selectedSaveLayer]);
00161 buf+=widths[selectedSaveLayer];
00162 img+=getStride(selectedSaveLayer);
00163 }
00164 } else {
00165
00166 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00167 unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00168 while(img!=rowend) {
00169 *buf++=*img;
00170 img+=inc;
00171 }
00172 img+=getSkip(selectedSaveLayer);
00173 }
00174 }
00175 len-=used;
00176
00177 return origlen-len;
00178 }
00179
00180 unsigned int
00181 RawCameraGenerator::SaveFileStream(FILE * f) const {
00182 unsigned int totalused=0;
00183 unsigned int used;
00184 {
00185 unsigned int sz=FilterBankGenerator::getBinSize();
00186 char * buf = new char[sz];
00187 memset(buf,0xF0,sz);
00188 if(buf==NULL) {
00189 std::cout << "*** WARNING could not allocate " << sz << " bytes for LoadFile";
00190 return 0;
00191 }
00192 unsigned int resp=FilterBankGenerator::SaveBuffer(buf,sz);
00193 if(resp==0) {
00194 std::cout << "*** WARNING SaveBuffer didn't write any data (possibly due to overflow or other error)" << std::endl;
00195 fwrite(buf,1,sz,f);
00196 } else {
00197 unsigned int wrote=fwrite(buf,1,resp,f);
00198 if(wrote!=resp)
00199 std::cout << "*** WARNING short write (wrote " << wrote << ", expected " << resp << ")" << std::endl;
00200 }
00201 delete [] buf;
00202 used=resp;
00203 }
00204 if(0==used) return 0;
00205 totalused+=used;
00206 if(0==(used=encode("RawImage",f))) return 0;
00207 totalused+=used;
00208
00209 if(images[selectedSaveLayer][selectedSaveChannel]==NULL) {
00210 serr->printf("RawCameraGenerator::SaveBuffer() failed because selected image is NULL -- call selectSaveImage first to make sure it's up to date\n");
00211 return 0;
00212 }
00213 if(!imageValids[selectedSaveLayer][selectedSaveChannel]) {
00214 serr->printf("RawCameraGenerator::SaveBuffer() failed because selected image is invalid -- call selectSaveImage first to make sure it's up to date\n");
00215 return 0;
00216 }
00217 unsigned char* img=images[selectedSaveLayer][selectedSaveChannel];
00218 used=widths[selectedSaveLayer]*heights[selectedSaveLayer];
00219 unsigned int inc=getIncrement(selectedSaveLayer);
00220 if(inc==1) {
00221
00222 sout->printf("Saving %d by %d\n",widths[selectedSaveLayer],heights[selectedSaveLayer]);
00223 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00224 if(fwrite(img,widths[selectedSaveLayer],1,f)==0) {
00225 serr->printf("short write on image data - ran out of space?\n");
00226 return 0;
00227 }
00228 img+=getStride(selectedSaveLayer);
00229 }
00230 } else {
00231
00232 for(unsigned int y=0; y<heights[selectedSaveLayer]; y++) {
00233 unsigned char* const rowend=img+widths[selectedSaveLayer]*inc;
00234 while(img!=rowend) {
00235 if(fputc(*img,f)==EOF) {
00236 serr->printf("short write on image data - ran out of space?\n");
00237 return 0;
00238 }
00239 img+=inc;
00240 }
00241 img+=getSkip(selectedSaveLayer);
00242 }
00243 }
00244 totalused+=used;
00245
00246 return totalused;
00247 }
00248
00249 void
00250 RawCameraGenerator::setDimensions() {
00251 freeCaches();
00252 unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00253 for(unsigned int res=0; res<numNotRealLayers; res++) {
00254 widths[res] = imageInfos[numNotRealLayers]->width>>(numNotRealLayers-res);
00255 heights[res] = imageInfos[numNotRealLayers]->height>>(numNotRealLayers-res);
00256 ASSERT(widths[res]*increments[res]==widths[numNotRealLayers],"widths*increments doesn't match total width");
00257 strides[res]=strides[numNotRealLayers]*increments[res];
00258 skips[res]=skips[numNotRealLayers]+strides[res]-strides[numNotRealLayers];
00259 }
00260 strides[numLayers-1]=widths[numLayers-1]=widths[numLayers-2]*2;
00261 heights[numLayers-1]=heights[numLayers-2]*2;
00262 }
00263
00264 void
00265 RawCameraGenerator::freeCaches() {
00266 for(unsigned int i=0; i<numLayers; i++) {
00267 for(unsigned int j=0; j<numChannels; j++) {
00268 images[i][j]=NULL;
00269 imageValids[i][j]=false;
00270 }
00271 }
00272 FilterBankGenerator::freeCaches();
00273 }
00274
00275 void
00276 RawCameraGenerator::setNumImages(unsigned int nLayers, unsigned int nChannels) {
00277 if(nLayers==numLayers && nChannels==numChannels)
00278 return;
00279 FilterBankGenerator::setNumImages(nLayers,nChannels);
00280 layers=new unsigned char*[numLayers];
00281 imageInfos=new const OFbkImageInfo*[numLayers];
00282 unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00283 for(unsigned int res=0; res<numLayers; res++) {
00284 layers[res]=NULL;
00285 imageInfos[res]=NULL;
00286 if(res<numNotRealLayers)
00287 increments[res]=1<<(numNotRealLayers-res);
00288 }
00289 }
00290
00291 unsigned char *
00292 RawCameraGenerator::createImageCache(unsigned int layer, unsigned int chan) const {
00293 if(layer==numLayers-1) {
00294 return const_cast<unsigned char*>(&dblRes[chan][0][0]);
00295 } else
00296 return NULL;
00297 }
00298
00299 void
00300 RawCameraGenerator::calcImage(unsigned int layer, unsigned int chan) {
00301 PROFSECTION("RawCameraGenerator::calcImage(...)",state->mainProfile);
00302 unsigned int numNotRealLayers=numLayers-1-numRealLayers;
00303 if(layer==numLayers-1) {
00304
00305 if(chan==CHAN_Y)
00306 reconstructImage();
00307 else
00308 upsampleImage(static_cast<channel_id_t>(chan));
00309 } else {
00310 if(layer>=numNotRealLayers) {
00311 unsigned int fbkdatChan=mapChannelID(static_cast<channel_id_t>(chan));
00312 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[layer]), const_cast<unsigned char*>(layers[layer]), fbkdatChan);
00313 images[layer][chan]=img.Pointer();
00314
00315
00316
00317
00318 if(config->vision.restore_image) {
00319 const unsigned int numPix=16;
00320 if(layer==numLayers-2) {
00321 unsigned char * s=images[layer][chan]+getStride(layer)*(getHeight(layer)-2);
00322 unsigned char * d=images[layer][chan]+getStride(layer)*(getHeight(layer)-1);
00323 for(unsigned int i=0; i<numPix; i++)
00324 *d++=*s++;
00325 } else {
00326 unsigned int inc=1<<(numLayers-2-layer);
00327 unsigned char * s;
00328
00329
00330 if(!imageValids[numLayers-2][chan]) {
00331 const OFbkImage simg(const_cast<OFbkImageInfo*>(imageInfos[numLayers-2]), const_cast<unsigned char*>(layers[numLayers-2]), fbkdatChan);
00332 s=simg.Pointer();
00333 } else {
00334 s=images[numLayers-2][chan];
00335 }
00336 s+=getStride(numLayers-2)*(getHeight(numLayers-2)-inc);
00337 unsigned char * d=images[layer][chan]+getStride(layer)*(getHeight(layer)-1);
00338 for(unsigned int i=0; i<numPix; i++) {
00339 *d++=*s;
00340 s+=inc;
00341 }
00342 }
00343 }
00344 } else {
00345
00346
00347 unsigned int fbkdatChan=mapChannelID(static_cast<channel_id_t>(chan));
00348 const OFbkImage img(const_cast<OFbkImageInfo*>(imageInfos[numNotRealLayers]), const_cast<unsigned char*>(layers[numNotRealLayers]), fbkdatChan);
00349 images[layer][chan]=img.Pointer();
00350 }
00351 }
00352 imageValids[layer][chan]=true;
00353 }
00354
00355 void
00356 RawCameraGenerator::destruct() {
00357 FilterBankGenerator::destruct();
00358 delete [] layers;
00359 layers=NULL;
00360 delete [] imageInfos;
00361 imageInfos=NULL;
00362 }
00363
00364 unsigned int
00365 RawCameraGenerator::mapChannelID(channel_id_t chan) {
00366 switch(chan) {
00367 case CHAN_Y:
00368 return ofbkimageBAND_Y;
00369 case CHAN_U:
00370 return ofbkimageBAND_Cr;
00371 case CHAN_V:
00372 return ofbkimageBAND_Cb;
00373 case CHAN_Y_DY:
00374 return ofbkimageBAND_Y_LH;
00375 case CHAN_Y_DX:
00376 return ofbkimageBAND_Y_HL;
00377 case CHAN_Y_DXDY:
00378 return ofbkimageBAND_Y_HH;
00379 default:
00380 std::cout << "RawCameraGenerator::mapChannelID bad channel" << std::endl;
00381 return ofbkimageBAND_Y;;
00382 }
00383 }
00384
00385 void
00386 RawCameraGenerator::upsampleImage(channel_id_t chan) {
00387 const unsigned int dblLayer=numLayers-1;
00388 const unsigned int srcLayer=dblLayer-1;
00389 const unsigned int width=widths[dblLayer];
00390 const unsigned int height=heights[dblLayer];
00391
00392 unsigned char * cur=images[dblLayer][chan];
00393 ASSERTRET(cur!=NULL,"destination layer is NULL");
00394 unsigned char * orig=getImage(srcLayer,chan);
00395 ASSERTRET(orig!=NULL,"source layer is NULL");
00396
00397 unsigned char * const imgend=cur+width*height;
00398 while(cur!=imgend) {
00399 unsigned char * const row=cur;
00400 unsigned char * const rowend=cur+width;
00401 while(cur!=rowend) {
00402 *cur++=*orig;
00403 *cur++=*orig++;
00404 }
00405 memcpy(cur,row,width);
00406 cur+=width;
00407 orig+=getSkip(srcLayer);
00408 }
00409 }
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426 void
00427 RawCameraGenerator::reconstructImage() {
00428 byte* yLLPtr = getImage(numLayers-2,CHAN_Y);
00429 byte* yLHPtr = getImage(numLayers-2,CHAN_Y_DY);
00430 byte* yHLPtr = getImage(numLayers-2,CHAN_Y_DX);
00431 byte* yHHPtr = getImage(numLayers-2,CHAN_Y_DXDY);
00432
00433 unsigned int w = getWidth(numLayers-2);
00434 unsigned int h = getWidth(numLayers-2);
00435 unsigned int skip = getSkip(numLayers-2);
00436
00437 unsigned char* img = images[numLayers-1][CHAN_Y];
00438 ASSERTRET(img!=NULL,"image destination NULL");
00439
00440 unsigned char* iptr0 = img;
00441 unsigned char* iptr1 = iptr0 + 2*w;
00442
00443 for (unsigned int y = 0; y < h; y++) {
00444 for (unsigned int x = 0; x < w; x++) {
00445
00446
00447
00448 short yLL = (short)*yLLPtr++;
00449 short yLH = (short)*yLHPtr++ - 128;
00450 short yHL = (short)*yHLPtr++ - 128;
00451 short yHH = (short)*yHHPtr++ - 128;
00452
00453 short a = yLL + yLH + yHL + yHH;
00454 short b = 2 * (yLL + yLH);
00455 short c = 2 * (yLL + yHL);
00456 short d = 2 * (yLL + yHH);
00457
00458 *iptr0++ = clipRange(d - a);
00459 *iptr0++ = clipRange(c - a);
00460 *iptr1++ = clipRange(b - a);
00461 *iptr1++ = clipRange(a);
00462 }
00463 yLLPtr += skip;
00464 yLHPtr += skip;
00465 yHLPtr += skip;
00466 yHHPtr += skip;
00467 iptr0 = iptr1;
00468 iptr1 += 2*w;
00469 }
00470 }
00471
00472
00473
00474
00475
00476
00477
00478
00479
00480
00481
00482
00483
|