std::vector getPoints()

in source/conversion/ExportPointCloud.cpp [64:134]
60 lines of code
12 McCabe index (conditional complexity)

std::vector<WorldColor> getPoints(const Camera& cam) {
  const std::string& camId = cam.id;
  LOG(INFO) << folly::sformat("Processing camera {}...", camId);

  // Load disparity and color, and resize color to size of disparity
  const cv::Mat_<float> disparity = loadImage<float>(FLAGS_disparity, camId, FLAGS_frame);

  // Load color (resize it to match disparity)
  const cv::Mat_<cv::Vec3f> color =
      loadResizedImage<cv::Vec3f>(FLAGS_color, camId, FLAGS_frame, disparity.size());

  // Rescale camera as disparity may not be full resolution
  const int w = disparity.cols;
  const int h = disparity.rows;
  const Camera camRescale = cam.rescale({w, h});

  // Transform each pixel to world coordinates and append to result
  std::vector<WorldColor> points(w * h);
  std::vector<bool> idxToDelete(w * h, false);

  ThreadPool threadPool(FLAGS_threads);
  const int edgeX = w;
  const int edgeY = 1;
  for (int yBegin = 0; yBegin < h; yBegin += edgeY) {
    for (int xBegin = 0; xBegin < w; xBegin += edgeX) {
      const int xEnd = std::min(xBegin + edgeX, w);
      const int yEnd = std::min(yBegin + edgeY, h);
      threadPool.spawn([&, xBegin, yBegin, xEnd, yEnd] {
        for (int y = yBegin; y < yEnd; ++y) {
          for (int x = xBegin; x < xEnd; ++x) {
            const int idx = w * y + x;
            if (FLAGS_subsample > 1 && rand() % FLAGS_subsample != 0) {
              idxToDelete[idx] = true;
              continue; // only retain 1 in subsample points
            }
            Camera::Vector2 pixel = {x + 0.5, y + 0.5};
            if (camRescale.isOutsideImageCircle(pixel)) {
              idxToDelete[idx] = true;
              continue;
            }
            WorldColor worldColor;
            const double m = 1 / disparity(y, x);
            Camera::Vector3 world = camRescale.rig(pixel, m);
            const Camera::Real depth = world.norm();
            if (depth > FLAGS_max_depth) {
              if (FLAGS_clip) {
                idxToDelete[idx] = true;
                continue;
              }
              world *= FLAGS_max_depth / depth;
            }
            worldColor.head<3>() = world.cast<float>();
            const cv::Vec3f c = color(y, x);
            worldColor.tail<3>() = Eigen::Array3f(c[2], c[1], c[0]);
            points[y * w + x] = worldColor;
          }
        }
      });
    }
  }
  threadPool.join();

  std::vector<WorldColor> pointsFiltered;
  for (ssize_t i = 0; i < ssize(points); ++i) {
    if (!idxToDelete[i]) {
      pointsFiltered.push_back(points[i]);
    }
  }

  return pointsFiltered;
}