void pingPongRectangle()

in source/depth_estimation/Derp.cpp [402:477]
64 lines of code
13 McCabe index (conditional complexity)

void pingPongRectangle(
    cv::Mat_<float>& dispRes,
    cv::Mat_<float>& costsRes,
    cv::Mat_<float>& confidencesRes,
    const cv::Mat_<bool>& changed,
    const cv::Mat_<cv::Vec3b>& labImage,
    const PyramidLevel<PixelType>& pyramidLevel,
    const int dstIdx,
    const int xBegin,
    const int yBegin,
    const int xEnd,
    const int yEnd) {
  const cv::Mat_<float>& disp = pyramidLevel.dstDisparity(dstIdx);
  const cv::Mat_<float>& confidences = pyramidLevel.dstConfidence(dstIdx);
  const cv::Mat_<bool>& maskFov = pyramidLevel.dstFovMask(dstIdx);
  const cv::Mat_<float>& dispBackground = pyramidLevel.dstBackgroundDisparity(dstIdx);
  const cv::Mat_<float>& variance = pyramidLevel.dstVariance(dstIdx);

  for (int y = yBegin; y < yEnd; ++y) {
    for (int x = xBegin; x < xEnd; ++x) {
      if (!maskFov(y, x)) {
        // Keep value from previous frame
        continue;
      }

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

      // Ignore locations with low variance
      if (variance(y, x) < pyramidLevel.varNoiseFloor) {
        continue;
      }

      float bestCost = INFINITY;
      float bestDisparity = disp(y, x);
      float bestConfidence = confidences(y, x);

      std::vector<std::array<int, 2>> candidateNeighborOffsets;
      if (kDoColorPruning) {
        const std::array<int, 2>& startPoint = {{x, y}};
        candidateNeighborOffsets = prunePingPongCandidates(
            candidateTemplateOriginal, labImage, startPoint, kColorPruningNumNeighbors);
      } else {
        candidateNeighborOffsets = candidateTemplateOriginal;
      }

      const float backgroundDisparity =
          pyramidLevel.hasForegroundMasks ? pyramidLevel.dstBackgroundDisparity(dstIdx)(y, x) : 0;

      for (const auto& candidateNeighborOffset : candidateNeighborOffsets) {
        const int xx = math_util::clamp(x + candidateNeighborOffset[0], 0, disp.cols - 1);
        const int yy = math_util::clamp(y + candidateNeighborOffset[1], 0, disp.rows - 1);
        if (maskFov(yy, xx)) { // inside FOV
          const float d = disp(yy, xx);

          // When using background disparity, foreground pixels must be closer than background
          if (d >= backgroundDisparity && changed(yy, xx)) {
            const auto costValues = computeCost(pyramidLevel, dstIdx, d, x, y);
            const float cost = std::get<0>(costValues);
            if (cost < bestCost) {
              bestCost = cost;
              bestDisparity = d;
              bestConfidence = std::get<1>(costValues);
            }
          }
        }
      }
      dispRes(y, x) = bestDisparity;
      costsRes(y, x) = bestCost;
      confidencesRes(y, x) = bestConfidence;
    }
  }
}