bool HexGridFitting::findT_camera_target()

in IsometricPatternMatcher/HexGridFitting.cpp [195:259]

• 59 lines of code
• 8 McCabe index (conditional complexity)

bool HexGridFitting::findT_camera_target(
    const ceres::Solver::Options& solverOption,
    const std::vector<Eigen::Matrix2Xd>&
        neighbourDots,  // each Matrix2Xd is the neighbours of a detected dot (#
                        // neighbours = numNeighboursForPoseEst_)
    const std::vector<int>& sampleIndx,  // sampleIndx stores the indices of
                                         // detected dots for the patch
    const double inlierThreshold, const Sophus::Plane3d& plane,
    Sophus::SE3d& T_camera_target, std::vector<int>& inliersIndx) {
  ceres::Problem::Options options;
  ceres::Problem problem(options);

  std::vector<double*> params = {T_camera_target.data()};
  LocalParamSe3* localParamSe3 = new LocalParamSe3;
  problem.AddParameterBlock(params[0], Sophus::SE3d::num_parameters,
                            localParamSe3);
  ceres::LossFunction* loss_function = nullptr;
  Eigen::VectorXd cameraModelParams;
  if (ifDistort_) {
    // KB3 camera model
    cameraModelParams.resize(KannalaBrandtK3Projection::kNumParams, 1);
    cameraModelParams << focalLength_, focalLength_, centerXY_(0), centerXY_(1),
        0.0, 0.0, 0.0, 0.0;
  } else {
    // Linear camera model
    cameraModelParams.resize(PinholeProjection::kNumParams, 1);
    cameraModelParams << focalLength_, focalLength_, centerXY_(0), centerXY_(1);
  }
  for (auto& i : sampleIndx) {
    for (auto const& neighbourMatrix : neighbourDots) {
      Ray3d rayInCamera;
      Ray3d rayNeighbourInCamera;
      if (ifDistort_) {
        rayInCamera = KannalaBrandtK3Projection::unproject(imageDots_.col(i),
                                                           cameraModelParams);
        rayNeighbourInCamera = KannalaBrandtK3Projection::unproject(
            neighbourMatrix.col(i), cameraModelParams);
      } else {
        rayInCamera =
            PinholeProjection::unproject(imageDots_.col(i), cameraModelParams);
        rayNeighbourInCamera = PinholeProjection::unproject(
            neighbourMatrix.col(i), cameraModelParams);
      }
      ceres::CostFunction* cost =
          new ceres::AutoDiffCostFunction<isometricPatternPoseCost, 1,
                                          Sophus::SE3d::num_parameters>(
              new isometricPatternPoseCost(rayInCamera, rayNeighbourInCamera,
                                           plane, spacing_));
      problem.AddResidualBlock(cost, loss_function, params[0]);
    }
  }
  ceres::Solver::Summary summary;
  ceres::Solve(solverOption, &problem, &summary);
  if (!summary.IsSolutionUsable()) return false;

  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
  inliersIndx = findInliers(neighbourDots, T_camera_target, distortionParams_,
                            inlierThreshold);
  return true;
}