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