ceres之cholesky求解器

参考代码：

normal_sparse_cholesky:https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/dynamic_sparse_normal_cholesky_solver.cc

dense_cholesky:https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/dense_normal_cholesky_solver.cc

一：

normal_sparse_cholesky:

输入：A,B,D,求解（A'*A+D'*D）x = A'*B

主调函数：

DynamicSparseNormalCholeskySolver::DynamicSparseNormalCholeskySolver(
    const LinearSolver::Options& options)
    : options_(options) {}

LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImpl(
    CompressedRowSparseMatrix* A,
    const double* b,
    const LinearSolver::PerSolveOptions& per_solve_options,
    double* x) {
  const int num_cols = A->num_cols();
  VectorRef(x, num_cols).setZero();
  A->LeftMultiply(b, x);

  if (per_solve_options.D != NULL) {
    // Temporarily append a diagonal block to the A matrix, but undo
    // it before returning the matrix to the user.
    scoped_ptr<CompressedRowSparseMatrix> regularizer;
    if (!A->col_blocks().empty()) {
      regularizer.reset(CompressedRowSparseMatrix::CreateBlockDiagonalMatrix(
          per_solve_options.D, A->col_blocks()));
    } else {
      regularizer.reset(
          new CompressedRowSparseMatrix(per_solve_options.D, num_cols));
    }
    A->AppendRows(*regularizer);
  }

  LinearSolver::Summary summary;
  switch (options_.sparse_linear_algebra_library_type) {
    case SUITE_SPARSE:
      summary = SolveImplUsingSuiteSparse(A, x);
      break;
    case CX_SPARSE:
      summary = SolveImplUsingCXSparse(A, x);
      break;
    case EIGEN_SPARSE:
      summary = SolveImplUsingEigen(A, x);
      break;
    default:
      LOG(FATAL) << "Unknown sparse linear algebra library : "
                 << options_.sparse_linear_algebra_library_type;
  }

  if (per_solve_options.D != NULL) {
    A->DeleteRows(num_cols);
  }

  return summary;
}

主要步骤：1）x = A'*b:

参考代码：VectorRef(x, num_cols).setZero();
          A->LeftMultiply(b, x);

2）若D不等于0:A = A+diag(D)

3)solve(Ax=b):

以eigen求解为例：

LinearSolver::Summary DynamicSparseNormalCholeskySolver::SolveImplUsingEigen(
    CompressedRowSparseMatrix* A, double* rhs_and_solution) {
#ifndef CERES_USE_EIGEN_SPARSE

  LinearSolver::Summary summary;
  summary.num_iterations = 0;
  summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
  summary.message =
      "SPARSE_NORMAL_CHOLESKY cannot be used with EIGEN_SPARSE "
      "because Ceres was not built with support for "
      "Eigen's SimplicialLDLT decomposition. "
      "This requires enabling building with -DEIGENSPARSE=ON.";
  return summary;

#else

  EventLogger event_logger("DynamicSparseNormalCholeskySolver::Eigen::Solve");

  Eigen::MappedSparseMatrix<double, Eigen::RowMajor> a(A->num_rows(),
                                                       A->num_cols(),
                                                       A->num_nonzeros(),
                                                       A->mutable_rows(),
                                                       A->mutable_cols(),
                                                       A->mutable_values());

  Eigen::SparseMatrix<double> lhs = a.transpose() * a;
  Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver;

  LinearSolver::Summary summary;
  summary.num_iterations = 1;
  summary.termination_type = LINEAR_SOLVER_SUCCESS;
  summary.message = "Success.";

  solver.analyzePattern(lhs);
  if (VLOG_IS_ON(2)) {
    std::stringstream ss;
    solver.dumpMemory(ss);
    VLOG(2) << "Symbolic Analysis\n" << ss.str();
  }

  event_logger.AddEvent("Analyze");
  if (solver.info() != Eigen::Success) {
    summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
    summary.message = "Eigen failure. Unable to find symbolic factorization.";
    return summary;
  }

  solver.factorize(lhs);
  event_logger.AddEvent("Factorize");
  if (solver.info() != Eigen::Success) {
    summary.termination_type = LINEAR_SOLVER_FAILURE;
    summary.message = "Eigen failure. Unable to find numeric factorization.";
    return summary;
  }

  const Vector rhs = VectorRef(rhs_and_solution, lhs.cols());
  VectorRef(rhs_and_solution, lhs.cols()) = solver.solve(rhs);
  event_logger.AddEvent("Solve");
  if (solver.info() != Eigen::Success) {
    summary.termination_type = LINEAR_SOLVER_FAILURE;
    summary.message = "Eigen failure. Unable to do triangular solve.";
    return summary;
  }

  return summary;
#endif  // CERES_USE_EIGEN_SPARSE
}

（3.1）A = A'*A+D'*D

(3.2)通过eigen库内置的cholesky求解器：

Eigen::SimplicialLDLT<Eigen::SparseMatrix<double> > solver
求解Ax=B得到x值

二：

dense_cholesky：

LinearSolver::Summary DenseNormalCholeskySolver::SolveImpl(
    DenseSparseMatrix* A,
    const double* b,
    const LinearSolver::PerSolveOptions& per_solve_options,
    double* x) {
  if (options_.dense_linear_algebra_library_type == EIGEN) {
    return SolveUsingEigen(A, b, per_solve_options, x);
  } else {
    return SolveUsingLAPACK(A, b, per_solve_options, x);
  }
}

LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen(
    DenseSparseMatrix* A,
    const double* b,
    const LinearSolver::PerSolveOptions& per_solve_options,
    double* x) {
  EventLogger event_logger("DenseNormalCholeskySolver::Solve");

  const int num_rows = A->num_rows();
  const int num_cols = A->num_cols();

  ConstColMajorMatrixRef Aref = A->matrix();
  Matrix lhs(num_cols, num_cols);
  lhs.setZero();

  event_logger.AddEvent("Setup");

  //   lhs += A'A
  //
  // Using rankUpdate instead of GEMM, exposes the fact that its the
  // same matrix being multiplied with itself and that the product is
  // symmetric.
  lhs.selfadjointView<Eigen::Upper>().rankUpdate(Aref.transpose());

  //   rhs = A'b
  Vector rhs = Aref.transpose() * ConstVectorRef(b, num_rows);

  if (per_solve_options.D != NULL) {
    ConstVectorRef D(per_solve_options.D, num_cols);
    lhs += D.array().square().matrix().asDiagonal();
  }
  event_logger.AddEvent("Product");

  LinearSolver::Summary summary;
  summary.num_iterations = 1;
  summary.termination_type = LINEAR_SOLVER_SUCCESS;
  Eigen::LLT<Matrix, Eigen::Upper> llt =
      lhs.selfadjointView<Eigen::Upper>().llt();

  if (llt.info() != Eigen::Success) {
    summary.termination_type = LINEAR_SOLVER_FAILURE;
    summary.message = "Eigen LLT decomposition failed.";
  } else {
    summary.termination_type = LINEAR_SOLVER_SUCCESS;
    summary.message = "Success.";
  }

  VectorRef(x, num_cols) = llt.solve(rhs);
  event_logger.AddEvent("Solve");
  return summary;
}

跟稀疏Chloesky分解差不多，区别在于系数矩阵是不是稀疏矩阵，然后求解器变成了：

Eigen::LLT<Matrix, Eigen::Upper> llt =lhs.selfadjointView<Eigen::Upper>().llt();