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