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