FrameFilter.cpp

   1 /***********************************************************************
   2 FrameFilter - Class to filter streams of depth frames arriving from a
   3 depth camera, with code to detect unstable values in each pixel, and
   4 fill holes resulting from invalid samples.
   5 Copyright (c) 2012-2016 Oliver Kreylos
   6
   7 This file is part of the Augmented Reality Sandbox (SARndbox).
   8
   9 The Augmented Reality Sandbox is free software; you can redistribute it
  10 and/or modify it under the terms of the GNU General Public License as
  11 published by the Free Software Foundation; either version 2 of the
  12 License, or (at your option) any later version.
  13
  14 The Augmented Reality Sandbox is distributed in the hope that it will be
  15 useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17 General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License along
  20 with the Augmented Reality Sandbox; if not, write to the Free Software
  21 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  22 ***********************************************************************/
  23
  24 #include "FrameFilter.h"
  25
  26 #include <Misc/FunctionCalls.h>
  27 #include <Geometry/HVector.h>
  28 #include <Geometry/Matrix.h>
  29
  30 /****************************
  31 Methods of class FrameFilter:
  32 ****************************/
  33
  34 void* FrameFilter::filterThreadMethod(void) {
  35     unsigned int lastInputFrameVersion = 0;
  36
  37     while(true) {
  38         Kinect::FrameBuffer frame;
  39         {
  40             Threads::MutexCond::Lock inputLock(inputCond);
  41
  42             /* Wait until a new frame arrives or the program shuts down: */
  43             while(runFilterThread && lastInputFrameVersion == inputFrameVersion)
  44                 inputCond.wait(inputLock);
  45
  46             /* Bail out if the program is shutting down: */
  47             if(!runFilterThread)
  48                 break;
  49
  50             /* Work on the new frame: */
  51             frame = inputFrame;
  52             lastInputFrameVersion = inputFrameVersion;
  53         }
  54
  55         /* Prepare a new output frame: */
  56         Kinect::FrameBuffer& newOutputFrame = outputFrames.startNewValue();
  57
  58         /* Enter the new frame into the averaging buffer and calculate the output frame's pixel values: */
  59         const RawDepth* ifPtr = inputFrame.getData<RawDepth>();
  60         RawDepth* abPtr = averagingBuffer + averagingSlotIndex * size[1] * size[0];
  61         unsigned int* sPtr = statBuffer;
  62         float* ofPtr = validBuffer;
  63         float* nofPtr = newOutputFrame.getData<float>();
  64         const PixelDepthCorrection* pdcPtr = pixelDepthCorrection;
  65         for(unsigned int y = 0; y < size[1]; ++y) {
  66             float py = float(y) + 0.5f;
  67             for(unsigned int x = 0; x < size[0];
  68                     ++x, ++ifPtr, ++pdcPtr, ++abPtr, sPtr += 3, ++ofPtr, ++nofPtr) {
  69                 float px = float(x) + 0.5f;
  70
  71                 unsigned int oldVal = *abPtr;
  72                 unsigned int newVal = *ifPtr;
  73
  74                 /* Depth-correct the new value: */
  75                 float newCVal = pdcPtr->correct(newVal);
  76
  77                 /* Plug the depth-corrected new value into the minimum and maximum plane equations to determine its validity: */
  78                 float minD = minPlane[0] * px + minPlane[1] * py + minPlane[2] * newCVal + minPlane[3];
  79                 float maxD = maxPlane[0] * px + maxPlane[1] * py + maxPlane[2] * newCVal + maxPlane[3];
  80                 if(minD >= 0.0f && maxD <= 0.0f) {
  81                     /* Store the new input value: */
  82                     *abPtr = newVal;
  83
  84                     /* Update the pixel's statistics: */
  85                     ++sPtr[0]; // Number of valid samples
  86                     sPtr[1] += newVal; // Sum of valid samples
  87                     sPtr[2] += newVal * newVal; // Sum of squares of valid samples
  88
  89                     /* Check if the previous value in the averaging buffer was valid: */
  90                     if(oldVal != 2048U) {
  91                         --sPtr[0]; // Number of valid samples
  92                         sPtr[1] -= oldVal; // Sum of valid samples
  93                         sPtr[2] -= oldVal * oldVal; // Sum of squares of valid samples
  94                     }
  95                 } else if(!retainValids) {
  96                     /* Store an invalid input value: */
  97                     *abPtr = 2048U;
  98
  99                     /* Check if the previous value in the averaging buffer was valid: */
 100                     if(oldVal != 2048U) {
 101                         --sPtr[0]; // Number of valid samples
 102                         sPtr[1] -= oldVal; // Sum of valid samples
 103                         sPtr[2] -= oldVal * oldVal; // Sum of squares of valid samples
 104                     }
 105                 }
 106
 107                 /* Check if the pixel is considered "stable": */
 108                 if(sPtr[0] >= minNumSamples
 109                         && sPtr[2]*sPtr[0] <= maxVariance * sPtr[0]*sPtr[0] + sPtr[1]*sPtr[1]) {
 110                     /* Check if the new depth-corrected running mean is outside the previous value's envelope: */
 111                     float newFiltered = pdcPtr->correct(float(sPtr[1]) / float(sPtr[0]));
 112                     if(Math::abs(newFiltered - *ofPtr) >= hysteresis) {
 113                         /* Set the output pixel value to the depth-corrected running mean: */
 114                         *nofPtr = *ofPtr = newFiltered;
 115                     } else {
 116                         /* Leave the pixel at its previous value: */
 117                         *nofPtr = *ofPtr;
 118                     }
 119                 } else if(retainValids) {
 120                     /* Leave the pixel at its previous value: */
 121                     *nofPtr = *ofPtr;
 122                 } else {
 123                     /* Assign default value to instable pixels: */
 124                     *nofPtr = instableValue;
 125                 }
 126             }
 127         }
 128
 129         /* Go to the next averaging slot: */
 130         if(++averagingSlotIndex == numAveragingSlots)
 131             averagingSlotIndex = 0U;
 132
 133         /* Apply a spatial filter if requested: */
 134         if(spatialFilter) {
 135             for(int filterPass = 0; filterPass < 2; ++filterPass) {
 136                 /* Low-pass filter the entire output frame in-place: */
 137                 for(unsigned int x = 0; x < size[0]; ++x) {
 138                     /* Get a pointer to the current column: */
 139                     float* colPtr = newOutputFrame.getData<float>() + x;
 140
 141                     /* Filter the first pixel in the column: */
 142                     float lastVal = *colPtr;
 143                     *colPtr = (colPtr[0] * 2.0f + colPtr[size[0]]) / 3.0f;
 144                     colPtr += size[0];
 145
 146                     /* Filter the interior pixels in the column: */
 147                     for(unsigned int y = 1; y < size[1] - 1; ++y, colPtr += size[0]) {
 148                         /* Filter the pixel: */
 149                         float nextLastVal = *colPtr;
 150                         *colPtr = (lastVal + colPtr[0] * 2.0f + colPtr[size[0]]) * 0.25f;
 151                         lastVal = nextLastVal;
 152                     }
 153
 154                     /* Filter the last pixel in the column: */
 155                     *colPtr = (lastVal + colPtr[0] * 2.0f) / 3.0f;
 156                 }
 157                 float* rowPtr = newOutputFrame.getData<float>();
 158                 for(unsigned int y = 0; y < size[1]; ++y) {
 159                     /* Filter the first pixel in the row: */
 160                     float lastVal = *rowPtr;
 161                     *rowPtr = (rowPtr[0] * 2.0f + rowPtr[1]) / 3.0f;
 162                     ++rowPtr;
 163
 164                     /* Filter the interior pixels in the row: */
 165                     for(unsigned int x = 1; x < size[0] - 1; ++x, ++rowPtr) {
 166                         /* Filter the pixel: */
 167                         float nextLastVal = *rowPtr;
 168                         *rowPtr = (lastVal + rowPtr[0] * 2.0f + rowPtr[1]) * 0.25f;
 169                         lastVal = nextLastVal;
 170                     }
 171
 172                     /* Filter the last pixel in the row: */
 173                     *rowPtr = (lastVal + rowPtr[0] * 2.0f) / 3.0f;
 174                     ++rowPtr;
 175                 }
 176             }
 177         }
 178
 179         /* Finalize the new output frame in the output buffer: */
 180         outputFrames.postNewValue();
 181
 182         /* Pass the new output frame to the registered receiver: */
 183         if(outputFrameFunction != 0)
 184             (*outputFrameFunction)(newOutputFrame);
 185     }
 186
 187     return 0;
 188 }
 189
 190 FrameFilter::FrameFilter(const unsigned int sSize[2], unsigned int sNumAveragingSlots,
 191                          const FrameFilter::PixelDepthCorrection* sPixelDepthCorrection, const PTransform& depthProjection,
 192                          const Plane& basePlane)
 193     : pixelDepthCorrection(sPixelDepthCorrection),
 194       averagingBuffer(0),
 195       statBuffer(0),
 196       outputFrameFunction(0) {
 197     /* Remember the frame size: */
 198     for(int i = 0; i < 2; ++i)
 199         size[i] = sSize[i];
 200
 201     /* Initialize the input frame slot: */
 202     inputFrameVersion = 0;
 203
 204     /* Initialize the valid depth range: */
 205     setValidDepthInterval(0U, 2046U);
 206
 207     /* Initialize the averaging buffer: */
 208     numAveragingSlots = sNumAveragingSlots;
 209     averagingBuffer = new RawDepth[numAveragingSlots * size[1]*size[0]];
 210     RawDepth* abPtr = averagingBuffer;
 211     for(unsigned int i = 0; i < numAveragingSlots; ++i)
 212         for(unsigned int y = 0; y < size[1]; ++y)
 213             for(unsigned int x = 0; x < size[0]; ++x, ++abPtr)
 214                 *abPtr = 2048U; // Mark sample as invalid
 215     averagingSlotIndex = 0U;
 216
 217     /* Initialize the statistics buffer: */
 218     statBuffer = new unsigned int[size[1]*size[0] * 3];
 219     unsigned int* sbPtr = statBuffer;
 220     for(unsigned int y = 0; y < size[1]; ++y)
 221         for(unsigned int x = 0; x < size[0]; ++x)
 222             for(int i = 0; i < 3; ++i, ++sbPtr)
 223                 *sbPtr = 0;
 224
 225     /* Initialize the stability criterion: */
 226     minNumSamples = (numAveragingSlots + 1) / 2;
 227     maxVariance = 4;
 228     hysteresis = 0.1f;
 229     retainValids = true;
 230     instableValue = 0.0;
 231
 232     /* Enable spatial filtering: */
 233     spatialFilter = true;
 234
 235     /* Convert the base plane equation from camera space to depth-image space: */
 236     PTransform::HVector basePlaneCc(basePlane.getNormal());
 237     basePlaneCc[3] = -basePlane.getOffset();
 238     PTransform::HVector basePlaneDic(depthProjection.getMatrix().transposeMultiply(basePlaneCc));
 239     basePlaneDic /= Geometry::mag(basePlaneDic.toVector());
 240
 241     /* Initialize the valid buffer: */
 242     validBuffer = new float[size[1]*size[0]];
 243     float* vbPtr = validBuffer;
 244     for(unsigned int y = 0; y < size[1]; ++y)
 245         for(unsigned int x = 0; x < size[0]; ++x, ++vbPtr)
 246             *vbPtr = float(-((double(x) + 0.5) * basePlaneDic[0] + (double(y) + 0.5) * basePlaneDic[1] +
 247                              basePlaneDic[3]) / basePlaneDic[2]);
 248
 249     /* Initialize the output frame buffer: */
 250     for(int i = 0; i < 3; ++i)
 251         outputFrames.getBuffer(i) = Kinect::FrameBuffer(size[0], size[1],
 252                                     size[1] * size[0] * sizeof(float));
 253
 254     /* Start the filtering thread: */
 255     runFilterThread = true;
 256     filterThread.start(this, &FrameFilter::filterThreadMethod);
 257 }
 258
 259 FrameFilter::~FrameFilter(void) {
 260     /* Shut down the filtering thread: */
 261     {
 262         Threads::MutexCond::Lock inputLock(inputCond);
 263         runFilterThread = false;
 264         inputCond.signal();
 265     }
 266     filterThread.join();
 267
 268     /* Release all allocated buffers: */
 269     delete[] averagingBuffer;
 270     delete[] statBuffer;
 271     delete[] validBuffer;
 272     delete outputFrameFunction;
 273 }
 274
 275 void FrameFilter::setValidDepthInterval(unsigned int newMinDepth, unsigned int newMaxDepth) {
 276     /* Set the equations for the minimum and maximum plane in depth image space: */
 277     minPlane[0] = 0.0f;
 278     minPlane[1] = 0.0f;
 279     minPlane[2] = 1.0f;
 280     minPlane[3] = -float(newMinDepth) + 0.5f;
 281     maxPlane[0] = 0.0f;
 282     maxPlane[1] = 0.0f;
 283     maxPlane[2] = 1.0f;
 284     maxPlane[3] = -float(newMaxDepth) - 0.5f;
 285 }
 286
 287 void FrameFilter::setValidElevationInterval(const PTransform& depthProjection,
 288         const Plane& basePlane, double newMinElevation, double newMaxElevation) {
 289     /* Calculate the equations of the minimum and maximum elevation planes in camera space: */
 290     PTransform::HVector minPlaneCc(basePlane.getNormal());
 291     minPlaneCc[3] = -(basePlane.getOffset() + newMinElevation * basePlane.getNormal().mag());
 292     PTransform::HVector maxPlaneCc(basePlane.getNormal());
 293     maxPlaneCc[3] = -(basePlane.getOffset() + newMaxElevation * basePlane.getNormal().mag());
 294
 295     /* Transform the plane equations to depth image space and flip and swap the min and max planes because elevation increases opposite to raw depth: */
 296     PTransform::HVector minPlaneDic(depthProjection.getMatrix().transposeMultiply(minPlaneCc));
 297     double minPlaneScale = -1.0 / Geometry::mag(minPlaneDic.toVector());
 298     for(int i = 0; i < 4; ++i)
 299         maxPlane[i] = float(minPlaneDic[i] * minPlaneScale);
 300     PTransform::HVector maxPlaneDic(depthProjection.getMatrix().transposeMultiply(maxPlaneCc));
 301     double maxPlaneScale = -1.0 / Geometry::mag(maxPlaneDic.toVector());
 302     for(int i = 0; i < 4; ++i)
 303         minPlane[i] = float(maxPlaneDic[i] * maxPlaneScale);
 304 }
 305
 306 void FrameFilter::setStableParameters(unsigned int newMinNumSamples, unsigned int newMaxVariance) {
 307     minNumSamples = newMinNumSamples;
 308     maxVariance = newMaxVariance;
 309 }
 310
 311 void FrameFilter::setHysteresis(float newHysteresis) {
 312     hysteresis = newHysteresis;
 313 }
 314
 315 void FrameFilter::setRetainValids(bool newRetainValids) {
 316     retainValids = newRetainValids;
 317 }
 318
 319 void FrameFilter::setInstableValue(float newInstableValue) {
 320     instableValue = newInstableValue;
 321 }
 322
 323 void FrameFilter::setSpatialFilter(bool newSpatialFilter) {
 324     spatialFilter = newSpatialFilter;
 325 }
 326
 327 void FrameFilter::setOutputFrameFunction(FrameFilter::OutputFrameFunction*
 328         newOutputFrameFunction) {
 329     delete outputFrameFunction;
 330     outputFrameFunction = newOutputFrameFunction;
 331 }
 332
 333 void FrameFilter::receiveRawFrame(const Kinect::FrameBuffer& newFrame) {
 334     Threads::MutexCond::Lock inputLock(inputCond);
 335
 336     /* Store the new buffer in the input buffer: */
 337     inputFrame = newFrame;
 338     ++inputFrameVersion;
 339
 340     /* Signal the background thread: */
 341     inputCond.signal();
 342 }