void DepthVideoProcessor::bilateralFilter()

in lib/Processor.cpp [183:313]
101 lines of code
17 McCabe index (conditional complexity)

void DepthVideoProcessor::bilateralFilter(const Params& params) {
  const float depthSigma2 = sqr(params.depthSigma);
  const float colorSigma2 = sqr(params.colorSigma);

  const bool useDepthRange = (params.depthSigma > 0.f);
  const bool useColorRange = (params.colorSigma > 0.f);

  const DepthStream& ds = video_->depthStream(0);
  const ColorStream& cs = video_->colorStream("down");

  for (int frame : params.frameRange) {
    LOG(INFO) << "Processing frame " << frame <<
        " of range " << params.frameRange.toString() << "...";

    // This is the image we're filtering.
    const Mat1f& referenceDepth = *ds.frame(frame).depth();
    const int w = referenceDepth.cols;
    const int h = referenceDepth.rows;

    // We have a reference color image, too.
    const Mat* referenceColor = cs.frame(frame).image();
    CHECK_EQ(referenceColor->cols, w);
    CHECK_EQ(referenceColor->rows, h);

    // The output image.
    Mat1f filteredDepth(h, w);

    // Fetching a stack of (2 * frameRadius + 1) *guidance images*, temporally
    // centered around the current frame.
    const int f0 = std::max(0, frame - params.frameRadius);
    const int f1 = std::min(video_->numFrames()-1, frame + params.frameRadius);
    const int temporalWindowSize = f1 - f0 + 1;
    std::vector<const Mat1f*> guideDepthStack(temporalWindowSize);
    std::vector<const Mat3f*> guideColorStack(temporalWindowSize);
    for (int i = 0; i < temporalWindowSize; ++i) {
      guideDepthStack[i] = ds.frame(f0 + i).depth();
      CHECK_EQ(guideDepthStack[i]->cols, w);
      CHECK_EQ(guideDepthStack[i]->rows, h);
      guideColorStack[i] = cs.frame(f0 + i).image3f();
      CHECK_EQ(guideColorStack[i]->cols, w);
      CHECK_EQ(guideColorStack[i]->rows, h);
    }

    // Vector of samples for the median filter
    std::vector<std::pair<float, float>> depthWeightSamples;

    // Looping over the output image pixels.
    for (int y = 0; y < h; ++y) {
      const int y0 = std::max(0, y - params.spatialRadius);
      const int y1 = std::min(h - 1, y + params.spatialRadius);

      const float* const refDepthPtr =
          (const float* const)referenceDepth.ptr(y);
      const Vec3f* const refColorPtr =
          (const Vec3f* const)referenceColor->ptr(y);
      float* const dstPtr = (float* const)filteredDepth.ptr(y);

      for (int x = 0; x < w; ++x) {
        const int x0 = std::max(0, x - params.spatialRadius);
        const int x1 = std::min(w - 1, x + params.spatialRadius);

        const float referenceDepth = refDepthPtr[x];
        const Vec3f& referenceColor = refColorPtr[x];

        float sumDepth = 0.f;
        float sumWeight = 0.f;

        if (params.median) {
          depthWeightSamples.clear();
        }

        // "Inner loop" over the spatio-temporal kernel.
        for (int wf = 0; wf < temporalWindowSize; ++wf) {
          for (int wy = y0; wy <= y1; ++wy) {
            const float* const guideDepthPtr =
                (const float* const)guideDepthStack[wf]->ptr(wy);
            const Vec3f* const guideColorPtr =
                (const Vec3f* const)guideColorStack[wf]->ptr(wy);

            for (int wx = x0; wx <= x1; ++wx) {
              const float depth = guideDepthPtr[wx];
              float exponent = 0.f;

              if (useDepthRange) {
                const float diff2 = sqr(depth - referenceDepth);
                exponent += -diff2 / depthSigma2;
              }

              if (useColorRange) {
                const Vec3f& color = guideColorPtr[wx];
                float diff2 =
                    sqr(color(0) - referenceColor(0)) +
                    sqr(color(1) - referenceColor(1)) +
                    sqr(color(2) - referenceColor(2));
                exponent += -diff2 / colorSigma2;
              }

              const float weight = (exponent != 0.f ? expf(exponent) : 1.f);

              if (params.median) {
                depthWeightSamples.emplace_back(depth, weight);
              } else {
                sumDepth += depth * weight;
              }
              sumWeight += weight;
            }
          }
        }

        if (params.median) {
          float halfWeight = sumWeight / 2.f;
          std::sort(depthWeightSamples.begin(), depthWeightSamples.end());
          const int N = depthWeightSamples.size();
          float cumWeight = 0.f;
          for (int i = 0; i < N; ++i) {
            const auto& dw = depthWeightSamples[i];
            cumWeight += dw.second;
            if (cumWeight >= halfWeight) {
              dstPtr[x] = dw.first;
              break;
            }
          }
        } else {
          dstPtr[x] = (sumWeight > 0.f ? sumDepth / sumWeight : 0.f);
        }
      }
    }

    video_->depthFrame(params.depthStream, frame).setDepth(filteredDepth);
  }
}