std::vector HexGridFitting::calculateSubregionPosesAndBestIndex()

in IsometricPatternMatcher/HexGridFitting.cpp [331:375]

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

std::vector<int> HexGridFitting::calculateSubregionPosesAndBestIndex(
    const ceres::Solver::Options& solverOption, const Sophus::Plane3d& plane,
    const std::vector<Eigen::Matrix2Xd>& neighbourDots,
    const Sophus::SE3d& initT_camera_target,
    std::vector<Sophus::SE3d>& Ts_camera_targetForSubregions,
    std::vector<std::vector<int>>& inliersIndx) {
  // blocks
  double maxX = imageDots_.row(0).maxCoeff();
  double minX = imageDots_.row(0).minCoeff();
  double maxY = imageDots_.row(1).maxCoeff();
  double minY = imageDots_.row(1).minCoeff();
  std::vector<double> segX;
  std::vector<double> segY;
  // segment X
  for (int i = 0; i <= numberSegX_; i++) {
    segX.push_back(minX + (maxX - minX) / numberSegX_ * i);
  }
  // segment Y
  for (int i = 0; i <= numberSegY_; i++) {
    segY.push_back(minY + (maxY - minY) / numberSegY_ * i);
  }
  std::vector<std::vector<int>> blockIndx(numberSegX_ * numberSegY_);
  for (size_t i = 0; i < imageDots_.cols(); ++i) {
    for (int x = 0; x < numberSegX_; x++) {
      for (int y = 0; y < numberSegY_; y++) {
        if (imageDots_(0, i) >= segX[x] && imageDots_(0, i) < segX[x + 1] &&
            imageDots_(1, i) >= segY[y] && imageDots_(1, i) < segY[y + 1])
          blockIndx[x * numberSegY_ + y].push_back(i);
      }
    }
  }
  std::vector<int> numberInliers;
  std::vector<int> indx;
  for (int i = 0; i < blockIndx.size(); ++i) {
    Ts_camera_targetForSubregions.push_back(
        initT_camera_target);  // initialization
    if (findT_camera_target(solverOption, neighbourDots, blockIndx[i], 0.2,
                            plane, Ts_camera_targetForSubregions[i],
                            inliersIndx[i])) {
      numberInliers.push_back(inliersIndx[i].size());
    }
  }
  getSortIndx(numberInliers, indx);  // ascend order
  return indx;
}