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