std::vector HexGridFitting::findInliers()

in IsometricPatternMatcher/HexGridFitting.cpp [166:193]

• 26 lines of code
• 6 McCabe index (conditional complexity)

std::vector<int> HexGridFitting::findInliers(
    const std::vector<Eigen::Matrix2Xd>& neighbourDots,
    const Sophus::SE3d& T_camera_target,
    const Eigen::Vector4d& distortionParams, const double inlierThreshold) {
  // if each distance of the numberNeighbour closest neighours to the dots in
  // the transferred space is around spacing, the dot is considered as an inlier
  std::vector<int> inliersIndx;
  size_t numberNeighbour = neighbourDots.size();
  std::vector<Eigen::VectorXd> distance;
  for (auto const& neighbourMatrix : neighbourDots) {
    distance.push_back(
        (reprojectDots(T_camera_target, distortionParams, imageDots_) -
         reprojectDots(T_camera_target, distortionParams, neighbourMatrix))
            .colwise()
            .norm() -
        Eigen::MatrixXd::Constant(1, imageDots_.cols(), spacing_));
  }
  for (int i = 0; i < imageDots_.cols(); ++i) {
    size_t numInlierPts = 0;
    for (const auto& neighourDist : distance) {
      if (abs(neighourDist(i)) <= inlierThreshold) {
        numInlierPts++;
      }
    }
    if (numInlierPts == numberNeighbour) inliersIndx.push_back(i);
  }
  return inliersIndx;
}