bool HexGridFitting::findKb3DistortionParams()

in IsometricPatternMatcher/HexGridFitting.cpp [261:303]
40 lines of code
6 McCabe index (conditional complexity)

bool HexGridFitting::findKb3DistortionParams(
    const ceres::Solver::Options& solverOption,
    const std::vector<Eigen::Matrix2Xd>& neighbourDots,
    const std::vector<int>& sampleIndx, const double inlierThreshold,
    const Sophus::Plane3d& plane, Sophus::SE3d& T_camera_target,
    std::vector<double>& distortionParams, std::vector<int>& inliersIndx) {
  ceres::LossFunction* loss_function = nullptr;  // new ceres::CauchyLoss(1.0);
  size_t numberNeighbour = neighbourDots.size();
  ceres::Problem::Options options;
  std::vector<double*> params = {T_camera_target.data()};
  std::vector<double*> paramsDistortion = {distortionParams.data()};

  ceres::Problem problemDistortion(options);
  problemDistortion.AddParameterBlock(params[0], Sophus::SE3d::num_parameters,
                                      new LocalParamSe3);

  for (size_t j = 0; j < numberNeighbour; ++j) {
    for (const int& i : sampleIndx) {
      ceres::CostFunction* cost =
          new ceres::AutoDiffCostFunction<isometricPatternDistortionCost, 1,
                                          Sophus::SE3d::num_parameters, 4>(
              new isometricPatternDistortionCost(
                  imageDots_.col(i), neighbourDots.at(j).col(i), centerXY_,
                  focalLength_, plane));
      problemDistortion.AddResidualBlock(cost, loss_function, params[0],
                                         paramsDistortion[0]);
    }
  }
  ceres::Solver::Summary summary;
  ceres::Solve(solverOption, &problemDistortion, &summary);

  if (!summary.IsSolutionUsable()) return false;
  inliersIndx = findInliers(
      neighbourDots, T_camera_target,
      Eigen::Map<Eigen::Vector4d>(distortionParams.data()), inlierThreshold);
  if (T_camera_target.inverse().translation().z() < 0) {
    T_camera_target.translation().z() = -T_camera_target.translation().z();
  }  // make sure the camera is on top of the target (not behind the target)
  if (T_camera_target.translation().z() < 0) {
    T_camera_target *= Sophus::SE3d::rotX(M_PI);
  }  // make sure the camera is facing the target
  return true;
}