std::vector ValidPointsAndTrisFromIm()

in src/Utils.cpp [44:75]

• 31 lines of code
• 8 McCabe index (conditional complexity)

std::vector<Eigen::Vector4f> ValidPointsAndTrisFromIm(
    const pangolin::Image<Eigen::Vector4f>& pixNorms,
    std::vector<Eigen::Vector4f>& tris,
    int& totalObs,
    int& wrongObs) {
  std::vector<Eigen::Vector4f> points;
  Eigen::Vector4f n;

  for (unsigned int w = 0; w < pixNorms.w; w++) {
    for (unsigned int h = 0; h < pixNorms.h; h++) {
      n = pixNorms(w, h);
      if (n[3] == 0.0f)
        continue;
      totalObs++;
      const std::size_t triInd = static_cast<std::size_t>(n[3] + 0.01f) - 1;
      Eigen::Vector4f triTrack = tris[triInd];
      if (triTrack[3] == 0.0f)
        tris[triInd] = n;
      else if (triTrack[3] > 0.0f) {
        const float dot = triTrack.head<3>().dot(n.head<3>());
        if (dot < 0.0f) {
          tris[triInd][3] = -1.0f;
          wrongObs++;
        }
      } else if (triTrack[3] < 0.0f) {
        wrongObs++;
      }
      points.push_back(n);
    }
  }
  return points;
}