Method SolveIRLS

src/colmap/estimators/rotation_averaging_impl.cc:775–835

}

bool RotationAveragingSolver::SolveIRLS(RotationAveragingProblem& problem) {
  SparseCholeskyWithFallbackSolver solver;
  bool pattern_analyzed = false;

  const double sigma = DegToRad(options_.irls_loss_parameter_sigma);

  Eigen::SparseMatrix<double> at_weight;
  Eigen::SparseMatrix<double> at_weight_a;
  Eigen::VectorXd step(problem.NumParameters());

  int iteration = 0;
  for (iteration = 0; iteration < options_.max_num_irls_iterations;
       iteration++) {
    problem.ComputeResiduals();

    auto weights_irls = ComputeIRLSWeights(problem, sigma);
    if (!weights_irls) {
      return false;
    }

    // Optionally add a small Tikhonov ridge to stabilize poorly conditioned
    // but mathematically PD systems. When zero (default), no regularization
    // is added.
    at_weight =
        problem.ConstraintMatrix().transpose() * weights_irls->asDiagonal();
    at_weight_a = at_weight * problem.ConstraintMatrix();
    if (options_.ridge_regularization > 0) {
      for (int i = 0; i < at_weight_a.cols(); ++i) {
        at_weight_a.coeffRef(i, i) += options_.ridge_regularization;
      }
    }

    if (!pattern_analyzed) {
      solver.AnalyzePattern(at_weight_a);
      pattern_analyzed = true;
    }
    if (!solver.Factorize(at_weight_a)) {
      LOG(ERROR) << "IRLS Cholesky factorization failed (iteration "
                 << iteration << ")";
      return false;
    }

    if (!solver.Solve(at_weight * problem.Residuals(), &step) ||
        step.array().isNaN().any()) {
      LOG(ERROR) << "IRLS solve failed (iteration " << iteration << ")";
      return false;
    }
    problem.UpdateState(step);

    const double avg_step = problem.AverageStepSize(step);
    VLOG(2) << "IRLS iteration " << iteration << ", average step: " << avg_step;

    if (avg_step < options_.irls_step_convergence_threshold) {
      iteration++;
      break;
    }
  }
  VLOG(2) << "IRLS total iteration: " << iteration;