void HexGridFitting::findPoseAndCamModel()

in IsometricPatternMatcher/HexGridFitting.cpp [377:427]
44 lines of code
6 McCabe index (conditional complexity)

void HexGridFitting::findPoseAndCamModel(
    const ceres::Solver::Options& solverOption, int selectIndx,
    const Sophus::SE3d& initT_camera_target) {
  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_);
  // per block pose estimation
  std::vector<int> bestIndxs = calculateSubregionPosesAndBestIndex(
      solverOption, plane, neighbourDots, initT_camera_target,
      Ts_camera_targetForSubregions, inliersIndx);
  int bestIndx = bestIndxs.back();
  if (selectIndx != -1) {
    bestIndx = selectIndx;
  }
  // global re-estimate
  T_camera_target_ = Ts_camera_targetForSubregions[bestIndx];
  std::vector<int> inliersPoseIndx;
  bool ifFindTCameraTarget =
      findT_camera_target(solverOption, neighbourDots, inliersIndx[bestIndx],
                          0.3, plane, T_camera_target_, inliersPoseIndx);

  CHECK(ifFindTCameraTarget) << "Cannot find T_camera_target";
  CHECK_GT(inliersPoseIndx.size(), 0) << "No inliers for T_camera_target. You "
                                         "can try with different focal length.";

  inlierPose.resize(2, inliersPoseIndx.size());
  for (size_t i = 0; i < inliersPoseIndx.size(); ++i) {
    inlierPose.col(i) = imageDots_.col(inliersPoseIndx[i]);
  }
  if (ifDistort_) {
    std::vector<int> inliersDistortIndx;
    std::vector<double> distortionParams = {0.0, 0.0, 0.0, 0.0};
    if (findKb3DistortionParams(solverOption, neighbourDots, inliersPoseIndx,
                                0.3, plane, T_camera_target_, distortionParams,
                                inliersDistortIndx)) {
      distortionParams_ = Eigen::Map<Eigen::Vector4d>(distortionParams.data());

      // fmt::print("Distortion parameters: {} \n",
      // distortionParams_.transpose());
      inlierDistortion.resize(2, inliersDistortIndx.size());
      for (size_t i = 0; i < inliersDistortIndx.size(); ++i) {
        inlierDistortion.col(i) = imageDots_.col(inliersDistortIndx[i]);
      }
    } else {
      LOG(INFO) << "Warning: Cannot find distortion parameters. Please try "
                   "with different focal length";
    }
  }
}