Eigen::VectorXi HexGridFitting::findGoodPoseIndex()

in IsometricPatternMatcher/HexGridFitting.cpp [305:329]

• 23 lines of code
• 3 McCabe index (conditional complexity)

Eigen::VectorXi HexGridFitting::findGoodPoseIndex(
    double goodPoseInlierRatio, const ceres::Solver::Options& solverOption,
    const Sophus::SE3d& initT_camera_target) {
  Eigen::VectorXi selectedPoseIdx(numberSegX_ * numberSegY_);
  selectedPoseIdx.fill(-1);
  Sophus::Plane3d plane(Sophus::Vector3d(0.0, 0.0, 1.0), 0.0);
  std::vector<Eigen::Matrix2Xd> neighbourDots =
      imageNeighbourMatrix(numNeighboursForPoseEst_);
  std::vector<Sophus::SE3d> Ts_camera_targetForSubregions;
  std::vector<std::vector<int>> inliersIndx(numberSegX_ * numberSegY_);
  std::vector<int> bestIndxs = calculateSubregionPosesAndBestIndex(
      solverOption, plane, neighbourDots, initT_camera_target,
      Ts_camera_targetForSubregions, inliersIndx);
  // build descend order selectedPoseIdx
  for (int i = 0; i < selectedPoseIdx.size(); ++i) {
    int idx = bestIndxs.size() - i - 1;
    int poseidx = bestIndxs.at(idx);
    // only select pose with enough inliers
    if (inliersIndx[poseidx].size() >=
        imageDots_.cols() * goodPoseInlierRatio) {
      selectedPoseIdx(i) = poseidx;
    }
  }
  return selectedPoseIdx;
}