ceres之共轭梯度求解器

参考源码：https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/conjugate_gradients_solver.cc

求解目标：Ax=B,已知系数矩阵A和残差项B，求解使得Ax=B的x

求解输入：（1）A,B

      （2）控制参数：r_tolerance(残差项下限，默认为0)

                                 q_tolerance(二次项下限.默认为0)：

                                  preconditioner(预优项,用M表示)

                                  min_num_iterations，max_num_iterations（最小最大迭代次数，默认为1）

 

 

求解过程：

LinearSolver::Summary ConjugateGradientsSolver::Solve(
    LinearOperator* A,
    const double* b,
    const LinearSolver::PerSolveOptions& per_solve_options,
    double* x) {
  CHECK_NOTNULL(A);
  CHECK_NOTNULL(x);
  CHECK_NOTNULL(b);
  CHECK_EQ(A->num_rows(), A->num_cols());

  LinearSolver::Summary summary;
  summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE;
  summary.message = "Maximum number of iterations reached.";
  summary.num_iterations = 0;

  const int num_cols = A->num_cols();
  VectorRef xref(x, num_cols);
  ConstVectorRef bref(b, num_cols);

  const double norm_b = bref.norm();
  if (norm_b == 0.0) {
    xref.setZero();
    summary.termination_type = LINEAR_SOLVER_SUCCESS;
    summary.message = "Convergence. |b| = 0.";
    return summary;
  }

  Vector r(num_cols);
  Vector p(num_cols);
  Vector z(num_cols);
  Vector tmp(num_cols);

  const double tol_r = per_solve_options.r_tolerance * norm_b;

  tmp.setZero();
  A->RightMultiply(x, tmp.data());
  r = bref - tmp;
  double norm_r = r.norm();
  if (options_.min_num_iterations == 0 && norm_r <= tol_r) {
    summary.termination_type = LINEAR_SOLVER_SUCCESS;
    summary.message =
        StringPrintf("Convergence. |r| = %e <= %e.", norm_r, tol_r);
    return summary;
  }

  double rho = 1.0;

  // Initial value of the quadratic model Q = x'Ax - 2 * b'x.
  double Q0 = -1.0 * xref.dot(bref + r);

  for (summary.num_iterations = 1;; ++summary.num_iterations) {
    // Apply preconditioner
    if (per_solve_options.preconditioner != NULL) {
      z.setZero();
      per_solve_options.preconditioner->RightMultiply(r.data(), z.data());
    } else {
      z = r;
    }

    double last_rho = rho;
    rho = r.dot(z);
    if (IsZeroOrInfinity(rho)) {
      summary.termination_type = LINEAR_SOLVER_FAILURE;
      summary.message = StringPrintf("Numerical failure. rho = r'z = %e.", rho);
      break;
    }

    if (summary.num_iterations == 1) {
      p = z;
    } else {
      double beta = rho / last_rho;
      if (IsZeroOrInfinity(beta)) {
        summary.termination_type = LINEAR_SOLVER_FAILURE;
        summary.message = StringPrintf(
            "Numerical failure. beta = rho_n / rho_{n-1} = %e, "
            "rho_n = %e, rho_{n-1} = %e", beta, rho, last_rho);
        break;
      }
      p = z + beta * p;
    }

    Vector& q = z;
    q.setZero();
    A->RightMultiply(p.data(), q.data());
    const double pq = p.dot(q);
    if ((pq <= 0) || IsInfinite(pq))  {
      summary.termination_type = LINEAR_SOLVER_NO_CONVERGENCE;
      summary.message = StringPrintf(
          "Matrix is indefinite, no more progress can be made. "
          "p'q = %e. |p| = %e, |q| = %e",
          pq, p.norm(), q.norm());
      break;
    }

    const double alpha = rho / pq;
    if (IsInfinite(alpha)) {
      summary.termination_type = LINEAR_SOLVER_FAILURE;
      summary.message =
          StringPrintf("Numerical failure. alpha = rho / pq = %e, "
                       "rho = %e, pq = %e.", alpha, rho, pq);
      break;
    }

    xref = xref + alpha * p;

    // Ideally we would just use the update r = r - alpha*q to keep
    // track of the residual vector. However this estimate tends to
    // drift over time due to round off errors. Thus every
    // residual_reset_period iterations, we calculate the residual as
    // r = b - Ax. We do not do this every iteration because this
    // requires an additional matrix vector multiply which would
    // double the complexity of the CG algorithm.
    if (summary.num_iterations % options_.residual_reset_period == 0) {
      tmp.setZero();
      A->RightMultiply(x, tmp.data());
      r = bref - tmp;
    } else {
      r = r - alpha * q;
    }

    // Quadratic model based termination.
    //   Q1 = x'Ax - 2 * b' x.
    const double Q1 = -1.0 * xref.dot(bref + r);

    //Q(x) = x'Ax - 2b'x
    //i * (Q(x_i) - Q(x_i-1)) / Q(x_i) < q_tolerance.
    const double zeta = summary.num_iterations * (Q1 - Q0) / Q1;
    if (zeta < per_solve_options.q_tolerance &&
        summary.num_iterations >= options_.min_num_iterations) {
      summary.termination_type = LINEAR_SOLVER_SUCCESS;
      summary.message =
          StringPrintf("Iteration: %d Convergence: zeta = %e < %e. |r| = %e",
                       summary.num_iterations,
                       zeta,
                       per_solve_options.q_tolerance,
                       r.norm());
      break;
    }
    Q0 = Q1;

    // Residual based termination.
    norm_r = r. norm();
    if (norm_r <= tol_r &&
        summary.num_iterations >= options_.min_num_iterations) {
      summary.termination_type = LINEAR_SOLVER_SUCCESS;
      summary.message =
          StringPrintf("Iteration: %d Convergence. |r| = %e <= %e.",
                       summary.num_iterations,
                       norm_r,
                       tol_r);
      break;
    }

    if (summary.num_iterations >= options_.max_num_iterations) {
      break;
    }
  }

  return summary;
}

 

伪代码：

tol_r = r_tolerance*||b||

r = b - Ax

如果||r||<=tol_r:return

Q0 = x'*A*x-2*b'*x

rho = 1

for(int k=0;k