| 32 | ****************************/ |
| 33 | |
| 34 | void* FrameFilter::filterThreadMethod(void) |
| 35 | { |
| 36 | unsigned int lastInputFrameVersion=0; |
| 37 | |
| 38 | while(true) |
| 39 | { |
| 40 | Kinect::FrameBuffer frame; |
| 41 | { |
| 42 | Threads::MutexCond::Lock inputLock(inputCond); |
| 43 | |
| 44 | /* Wait until a new frame arrives or the program shuts down: */ |
| 45 | while(runFilterThread&&lastInputFrameVersion==inputFrameVersion) |
| 46 | inputCond.wait(inputLock); |
| 47 | |
| 48 | /* Bail out if the program is shutting down: */ |
| 49 | if(!runFilterThread) |
| 50 | break; |
| 51 | |
| 52 | /* Work on the new frame: */ |
| 53 | frame=inputFrame; |
| 54 | lastInputFrameVersion=inputFrameVersion; |
| 55 | } |
| 56 | |
| 57 | /* Prepare a new output frame: */ |
| 58 | Kinect::FrameBuffer& newOutputFrame=outputFrames.startNewValue(); |
| 59 | |
| 60 | /* Enter the new frame into the averaging buffer and calculate the output frame's pixel values: */ |
| 61 | const RawDepth* ifPtr=inputFrame.getData<RawDepth>(); |
| 62 | RawDepth* abPtr=averagingBuffer+averagingSlotIndex*size[1]*size[0]; |
| 63 | unsigned int* sPtr=statBuffer; |
| 64 | float* ofPtr=validBuffer; |
| 65 | float* nofPtr=newOutputFrame.getData<float>(); |
| 66 | const PixelDepthCorrection* pdcPtr=pixelDepthCorrection; |
| 67 | for(unsigned int y=0;y<size[1];++y) |
| 68 | { |
| 69 | float py=float(y)+0.5f; |
| 70 | for(unsigned int x=0;x<size[0];++x,++ifPtr,++pdcPtr,++abPtr,sPtr+=3,++ofPtr,++nofPtr) |
| 71 | { |
| 72 | float px=float(x)+0.5f; |
| 73 | |
| 74 | unsigned int oldVal=*abPtr; |
| 75 | unsigned int newVal=*ifPtr; |
| 76 | |
| 77 | /* Depth-correct the new value: */ |
| 78 | float newCVal=pdcPtr->correct(newVal); |
| 79 | |
| 80 | /* Plug the depth-corrected new value into the minimum and maximum plane equations to determine its validity: */ |
| 81 | float minD=minPlane[0]*px+minPlane[1]*py+minPlane[2]*newCVal+minPlane[3]; |
| 82 | float maxD=maxPlane[0]*px+maxPlane[1]*py+maxPlane[2]*newCVal+maxPlane[3]; |
| 83 | if(minD>=0.0f&&maxD<=0.0f) |
| 84 | { |
| 85 | /* Store the new input value: */ |
| 86 | *abPtr=newVal; |
| 87 | |
| 88 | /* Update the pixel's statistics: */ |
| 89 | ++sPtr[0]; // Number of valid samples |
| 90 | sPtr[1]+=newVal; // Sum of valid samples |
| 91 | sPtr[2]+=newVal*newVal; // Sum of squares of valid samples |
nothing calls this directly
no outgoing calls
no test coverage detected