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