void DepthmapPruner::Prune()

in opensfm/src/dense/src/depthmap.cc [576:636]

• 61 lines of code
• 12 McCabe index (conditional complexity)

void DepthmapPruner::Prune(std::vector<float> *merged_points,
                           std::vector<float> *merged_normals,
                           std::vector<unsigned char> *merged_colors,
                           std::vector<unsigned char> *merged_labels) {
  cv::Matx33f Rinv = Rs_[0].t();
  for (int i = 0; i < depths_[0].rows; ++i) {
    for (int j = 0; j < depths_[0].cols; ++j) {
      float depth = depths_[0].at<float>(i, j);
      if (depth <= 0) {
        continue;
      }
      cv::Vec3f normal = cv::normalize(planes_[0].at<cv::Vec3f>(i, j));
      float area = -normal(2) / depth * Ks_[0](0, 0);
      cv::Vec3f point = Backproject(j, i, depth, Ks_[0], Rs_[0], ts_[0]);
      bool keep = true;
      for (int other = 1; other < depths_.size(); ++other) {
        cv::Vec3d reprojection =
            Project(point, Ks_[other], Rs_[other], ts_[other]);
        if (reprojection(2) < z_epsilon || isnan(reprojection(2))) {
          continue;
        }
        std::int64_t iu =
            static_cast<std::int64_t>(reprojection(0) / reprojection(2) + 0.5);
        std::int64_t iv =
            static_cast<std::int64_t>(reprojection(1) / reprojection(2) + 0.5);
        double depth_of_point = reprojection(2);
        if (!IsInsideImage(depths_[other], iv, iu)) {
          continue;
        }
        float depth_at_reprojection = depths_[other].at<float>(iv, iu);
        if (depth_at_reprojection >
            (1 - same_depth_threshold_) * depth_of_point) {
          cv::Vec3f normal_at_reprojection =
              cv::normalize(planes_[other].at<cv::Vec3f>(iv, iu));
          if ((depth_at_reprojection == 0.0) ||
              (-normal_at_reprojection(2) / depth_at_reprojection *
                   Ks_[other](0, 0) >
               area)) {
            keep = false;
            break;
          }
        }
      }
      if (keep) {
        cv::Vec3f R1_normal = Rinv * normal;
        cv::Vec3b color = colors_[0].at<cv::Vec3b>(i, j);
        unsigned char label = labels_[0].at<unsigned char>(i, j);
        merged_points->push_back(point[0]);
        merged_points->push_back(point[1]);
        merged_points->push_back(point[2]);
        merged_normals->push_back(R1_normal[0]);
        merged_normals->push_back(R1_normal[1]);
        merged_normals->push_back(R1_normal[2]);
        merged_colors->push_back(color[0]);
        merged_colors->push_back(color[1]);
        merged_colors->push_back(color[2]);
        merged_labels->push_back(label);
      }
    }
  }
}