void computeBruteForceDisparity()

in source/depth_estimation/Derp.cpp [263:381]
100 lines of code
22 McCabe index (conditional complexity)

void computeBruteForceDisparity(
    PyramidLevel<PixelType>& pyramidLevel,
    const int dstIdx,
    const float minDepthMeters,
    const float maxDepthMeters,
    const bool partialCoverage,
    const bool useForegroundMasks,
    const int numThreads) {
  cv::Mat_<float>& dstDisparity = pyramidLevel.dstDisparity(dstIdx);
  cv::Mat_<float>& dstCosts = pyramidLevel.dstCost(dstIdx);
  cv::Mat_<float>& dstConfidences = pyramidLevel.dstConfidence(dstIdx);

  LOG(INFO) << "Computing initial costs at " << pyramidLevel.sizeLevel
            << folly::sformat(" ({})", pyramidLevel.rigDst[dstIdx].id);

  std::vector<float> disparities(kNumDepths);
  const float minDisparity = 1.0f / maxDepthMeters;
  const float maxDisparity = 1.0f / minDepthMeters;
  for (int i = 0; i < kNumDepths; ++i) {
    const float d = probeDisparity(i, kNumDepths, minDisparity, maxDisparity);
    disparities[i] = d;
  }

  // Create a cost map for each possible disparity
  ThreadPool threadPool(numThreads);
  std::vector<cv::Mat_<float>> costs(disparities.size());
  std::vector<cv::Mat_<float>> confidences(disparities.size());
  for (int iDisparity = 0; iDisparity < int(disparities.size()); ++iDisparity) {
    costs[iDisparity].create(pyramidLevel.sizeLevel);
    costs[iDisparity].setTo(NAN);
    confidences[iDisparity].create(pyramidLevel.sizeLevel);
    confidences[iDisparity].setTo(NAN);
    threadPool.spawn(
        &computeBruteForceCosts,
        std::ref(pyramidLevel),
        dstIdx,
        disparities[iDisparity],
        std::ref(costs[iDisparity]),
        std::ref(confidences[iDisparity]));
  }
  threadPool.join();

  // Get best cost on each location
  // We have one cost per disparity at each location
  // Ignore margins if dst = src (won't be able to get entire patch)
  const int margin = kSearchWindowRadius;
  for (int y = margin; y < dstDisparity.rows - margin; ++y) {
    for (int x = margin; x < dstDisparity.cols - margin; ++x) {
      if (!pyramidLevel.dstFovMask(dstIdx)(y, x)) { // outside FOV
        dstDisparity(y, x) = NAN;
        continue;
      }

      // Use background value if we're outside the foreground mask
      if (!pyramidLevel.dstForegroundMask(dstIdx)(y, x)) {
        dstDisparity(y, x) = pyramidLevel.dstBackgroundDisparity(dstIdx)(y, x);
        continue;
      }

      float minCost = FLT_MAX;
      float minCostConfidence = 0;
      int bestDisparityIdx = -1;
      for (int i = 0; i < int(costs.size()); ++i) {
        const float cost = costs[i](y, x);
        if (cost < minCost) {
          minCost = cost;
          minCostConfidence = confidences[i](y, x);
          bestDisparityIdx = i;
        }
      }
      if (bestDisparityIdx == -1) {
        // This can only happen if we're outside the overlapping area when we
        // have partial coverage or due to noise in foreground masks
        std::string warning = folly::sformat(
            "Insufficient coverage at {} ({}, {}) ", pyramidLevel.rigDst[dstIdx].id, x, y);
        CHECK(partialCoverage || useForegroundMasks) << warning;

        const std::string partialCoverageFailure = "due to partial coverage";
        const std::string foregroundMaskFailure = "due to noisy foreground masks";

        warning += partialCoverage ? partialCoverageFailure : "";
        warning += (partialCoverage && useForegroundMasks) ? " or " : "";
        warning += useForegroundMasks ? foregroundMaskFailure : "";

        LOG(WARNING) << warning;
        dstDisparity(y, x) = minDisparity;
      } else {
        dstDisparity(y, x) = disparities[bestDisparityIdx];
      }
      dstCosts(y, x) = minCost;
      dstConfidences(y, x) = minCostConfidence;
    }
  }

  // Extend disparities to margin
  if (margin > 0) {
    for (int y = 0; y < dstDisparity.rows; ++y) {
      for (int x = 0; x < dstDisparity.cols; ++x) {
        if (x < margin || x >= dstDisparity.cols - margin || y < margin ||
            y >= dstDisparity.rows - margin) {
          // Use background value if we're outside the foreground mask
          if (!pyramidLevel.dstForegroundMask(dstIdx)(y, x)) {
            dstDisparity(y, x) = pyramidLevel.dstBackgroundDisparity(dstIdx)(y, x);
            continue;
          }
          dstDisparity(y, x) = dstDisparity(
              math_util::clamp(y, margin, dstDisparity.rows - margin - 1),
              math_util::clamp(x, margin, dstDisparity.cols - margin - 1));
          dstCosts(y, x) = dstCosts(
              math_util::clamp(y, margin, dstCosts.rows - margin - 1),
              math_util::clamp(x, margin, dstCosts.cols - margin - 1));
          dstConfidences(y, x) = dstConfidences(
              math_util::clamp(y, margin, dstConfidences.rows - margin - 1),
              math_util::clamp(x, margin, dstConfidences.cols - margin - 1));
        }
      }
    }
  }
}