internal/ceres/sparse_normal_cholesky_solver.cc - ceres-solver - Git at Google

 // Ceres Solver - A fast non-linear least squares minimizer
 // Copyright 2015 Google Inc. All rights reserved.
 // http://ceres-solver.org/
 //
 // Redistribution and use in source and binary forms, with or without
 // modification, are permitted provided that the following conditions are met:
 //
 // * Redistributions of source code must retain the above copyright notice,
 //   this list of conditions and the following disclaimer.
 // * Redistributions in binary form must reproduce the above copyright notice,
 //   this list of conditions and the following disclaimer in the documentation
 //   and/or other materials provided with the distribution.
 // * Neither the name of Google Inc. nor the names of its contributors may be
 //   used to endorse or promote products derived from this software without
 //   specific prior written permission.
 //
 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 //
 // Author: sameeragarwal@google.com (Sameer Agarwal)

 #include "ceres/sparse_normal_cholesky_solver.h"

 #include <algorithm>
 #include <cstring>
 #include <ctime>
 #include <sstream>

 #include "ceres/compressed_row_sparse_matrix.h"
 #include "ceres/cxsparse.h"
 #include "ceres/internal/eigen.h"
 #include "ceres/internal/scoped_ptr.h"
 #include "ceres/linear_solver.h"
 #include "ceres/suitesparse.h"
 #include "ceres/triplet_sparse_matrix.h"
 #include "ceres/types.h"
 #include "ceres/wall_time.h"
 #include "Eigen/SparseCore"

 #ifdef CERES_USE_EIGEN_SPARSE
 #include "Eigen/SparseCholesky"
 #endif

 namespace ceres {
 namespace internal {
 namespace {

 #ifdef CERES_USE_EIGEN_SPARSE
 // A templated factorized and solve function, which allows us to use
 // the same code independent of whether a AMD or a Natural ordering is
 // used.
 template <typename SimplicialCholeskySolver, typename SparseMatrixType>
 LinearSolver::Summary SimplicialLDLTSolve(
     const SparseMatrixType& lhs,
     const bool do_symbolic_analysis,
     SimplicialCholeskySolver* solver,
     double* rhs_and_solution,
     EventLogger* event_logger) {
   LinearSolver::Summary summary;
   summary.num_iterations = 1;
   summary.termination_type = LINEAR_SOLVER_SUCCESS;
   summary.message = "Success.";

   if (do_symbolic_analysis) {
     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

 #ifndef CERES_NO_CXSPARSE
 LinearSolver::Summary ComputeNormalEquationsAndSolveUsingCXSparse(
     CompressedRowSparseMatrix* A,
     double * rhs_and_solution,
     EventLogger* event_logger) {
   LinearSolver::Summary summary;
   summary.num_iterations = 1;
   summary.termination_type = LINEAR_SOLVER_SUCCESS;
   summary.message = "Success.";

   CXSparse cxsparse;

   // Wrap the augmented Jacobian in a compressed sparse column matrix.
   cs_di a_transpose = cxsparse.CreateSparseMatrixTransposeView(A);

   // Compute the normal equations. J'J delta = J'f and solve them
   // using a sparse Cholesky factorization. Notice that when compared
   // to SuiteSparse we have to explicitly compute the transpose of Jt,
   // and then the normal equations before they can be
   // factorized. CHOLMOD/SuiteSparse on the other hand can just work
   // off of Jt to compute the Cholesky factorization of the normal
   // equations.
   cs_di* a = cxsparse.TransposeMatrix(&a_transpose);
   cs_di* lhs = cxsparse.MatrixMatrixMultiply(&a_transpose, a);
   cxsparse.Free(a);
   event_logger->AddEvent("NormalEquations");

   cs_dis* factor = cxsparse.AnalyzeCholesky(lhs);
   event_logger->AddEvent("Analysis");

   if (factor == NULL) {
     summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
     summary.message = "CXSparse::AnalyzeCholesky failed.";
   } else if (!cxsparse.SolveCholesky(lhs, factor, rhs_and_solution)) {
     summary.termination_type = LINEAR_SOLVER_FAILURE;
     summary.message = "CXSparse::SolveCholesky failed.";
   }
   event_logger->AddEvent("Solve");

   cxsparse.Free(lhs);
   cxsparse.Free(factor);
   event_logger->AddEvent("TearDown");
   return summary;
 }

 #endif  // CERES_NO_CXSPARSE

 }  // namespace

 SparseNormalCholeskySolver::SparseNormalCholeskySolver(
     const LinearSolver::Options& options)
     : factor_(NULL),
       cxsparse_factor_(NULL),
       options_(options) {
 }

 void SparseNormalCholeskySolver::FreeFactorization() {
   if (factor_ != NULL) {
     ss_.Free(factor_);
     factor_ = NULL;
   }

   if (cxsparse_factor_ != NULL) {
     cxsparse_.Free(cxsparse_factor_);
     cxsparse_factor_ = NULL;
   }
 }

 SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {
   FreeFactorization();
 }

 LinearSolver::Summary SparseNormalCholeskySolver::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().size() > 0) {
       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;
 }

 LinearSolver::Summary SparseNormalCholeskySolver::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("SparseNormalCholeskySolver::Eigen::Solve");
   // Compute the normal equations. J'J delta = J'f and solve them
   // using a sparse Cholesky factorization. Notice that when compared
   // to SuiteSparse we have to explicitly compute the normal equations
   // before they can be factorized. CHOLMOD/SuiteSparse on the other
   // hand can just work off of Jt to compute the Cholesky
   // factorization of the normal equations.

   if (options_.dynamic_sparsity) {
     // In the case where the problem has dynamic sparsity, it is not
     // worth using the ComputeOuterProduct routine, as the setup cost
     // is not amortized over multiple calls to 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;
     return SimplicialLDLTSolve(lhs,
                                true,
                                &solver,
                                rhs_and_solution,
                                &event_logger);
   }

   if (outer_product_.get() == NULL) {
     outer_product_.reset(
         CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
             *A, &pattern_));
   }

   CompressedRowSparseMatrix::ComputeOuterProduct(
       *A, pattern_, outer_product_.get());

   // Map to an upper triangular column major matrix.
   //
   // outer_product_ is a compressed row sparse matrix and in lower
   // triangular form, when mapped to a compressed column sparse
   // matrix, it becomes an upper triangular matrix.
   Eigen::MappedSparseMatrix<double, Eigen::ColMajor> lhs(
       outer_product_->num_rows(),
       outer_product_->num_rows(),
       outer_product_->num_nonzeros(),
       outer_product_->mutable_rows(),
       outer_product_->mutable_cols(),
       outer_product_->mutable_values());

   bool do_symbolic_analysis = false;

   // If using post ordering or an old version of Eigen, we cannot
   // depend on a preordered jacobian, so we work with a SimplicialLDLT
   // decomposition with AMD ordering.
   if (options_.use_postordering ||
       !EIGEN_VERSION_AT_LEAST(3, 2, 2)) {
     if (amd_ldlt_.get() == NULL) {
       amd_ldlt_.reset(new SimplicialLDLTWithAMDOrdering);
       do_symbolic_analysis = true;
     }

     return SimplicialLDLTSolve(lhs,
                                do_symbolic_analysis,
                                amd_ldlt_.get(),
                                rhs_and_solution,
                                &event_logger);
   }

 #if EIGEN_VERSION_AT_LEAST(3,2,2)
   // The common case
   if (natural_ldlt_.get() == NULL) {
     natural_ldlt_.reset(new SimplicialLDLTWithNaturalOrdering);
     do_symbolic_analysis = true;
   }

   return SimplicialLDLTSolve(lhs,
                              do_symbolic_analysis,
                              natural_ldlt_.get(),
                              rhs_and_solution,
                              &event_logger);
 #endif

 #endif  // EIGEN_USE_EIGEN_SPARSE
 }

 LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
     CompressedRowSparseMatrix* A,
     double * rhs_and_solution) {
 #ifdef CERES_NO_CXSPARSE

   LinearSolver::Summary summary;
   summary.num_iterations = 0;
   summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
   summary.message =
       "SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE "
       "because Ceres was not built with support for CXSparse. "
       "This requires enabling building with -DCXSPARSE=ON.";

   return summary;

 #else

   EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve");
   if (options_.dynamic_sparsity) {
     return ComputeNormalEquationsAndSolveUsingCXSparse(A,
                                                        rhs_and_solution,
                                                        &event_logger);
   }

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

   // Compute the normal equations. J'J delta = J'f and solve them
   // using a sparse Cholesky factorization. Notice that when compared
   // to SuiteSparse we have to explicitly compute the normal equations
   // before they can be factorized. CHOLMOD/SuiteSparse on the other
   // hand can just work off of Jt to compute the Cholesky
   // factorization of the normal equations.
   if (outer_product_.get() == NULL) {
     outer_product_.reset(
         CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
             *A, &pattern_));
   }

   CompressedRowSparseMatrix::ComputeOuterProduct(
       *A, pattern_, outer_product_.get());
   cs_di lhs =
       cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get());

   event_logger.AddEvent("Setup");

   // Compute symbolic factorization if not available.
   if (cxsparse_factor_ == NULL) {
     if (options_.use_postordering) {
       cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(&lhs,
                                                         A->col_blocks(),
                                                         A->col_blocks());
     } else {
       cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(&lhs);
     }
   }
   event_logger.AddEvent("Analysis");

   if (cxsparse_factor_ == NULL) {
     summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
     summary.message =
         "CXSparse failure. Unable to find symbolic factorization.";
   } else if (!cxsparse_.SolveCholesky(&lhs,
                                       cxsparse_factor_,
                                       rhs_and_solution)) {
     summary.termination_type = LINEAR_SOLVER_FAILURE;
     summary.message = "CXSparse::SolveCholesky failed.";
   }
   event_logger.AddEvent("Solve");

   return summary;
 #endif
 }

 LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
     CompressedRowSparseMatrix* A,
     double * rhs_and_solution) {
 #ifdef CERES_NO_SUITESPARSE

   LinearSolver::Summary summary;
   summary.num_iterations = 0;
   summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
   summary.message =
       "SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE "
       "because Ceres was not built with support for SuiteSparse. "
       "This requires enabling building with -DSUITESPARSE=ON.";
   return summary;

 #else

   EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve");
   LinearSolver::Summary summary;
   summary.termination_type = LINEAR_SOLVER_SUCCESS;
   summary.num_iterations = 1;
   summary.message = "Success.";

   const int num_cols = A->num_cols();
   cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A);
   event_logger.AddEvent("Setup");

   if (options_.dynamic_sparsity) {
     FreeFactorization();
   }

   if (factor_ == NULL) {
     if (options_.use_postordering) {
       factor_ = ss_.BlockAnalyzeCholesky(&lhs,
                                          A->col_blocks(),
                                          A->row_blocks(),
                                          &summary.message);
     } else {
       if (options_.dynamic_sparsity) {
         factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message);
       } else {
         factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs,
                                                          &summary.message);
       }
     }
   }
   event_logger.AddEvent("Analysis");

   if (factor_ == NULL) {
     summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
     // No need to set message as it has already been set by the
     // symbolic analysis routines above.
     return summary;
   }

   summary.termination_type = ss_.Cholesky(&lhs, factor_, &summary.message);
   if (summary.termination_type != LINEAR_SOLVER_SUCCESS) {
     return summary;
   }

   cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution,
                                              num_cols,
                                              num_cols);
   cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message);
   event_logger.AddEvent("Solve");

   ss_.Free(rhs);
   if (solution != NULL) {
     memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution));
     ss_.Free(solution);
   } else {
     // No need to set message as it has already been set by the
     // numeric factorization routine above.
     summary.termination_type = LINEAR_SOLVER_FAILURE;
   }

   event_logger.AddEvent("Teardown");
   return summary;
 #endif
 }

 }   // namespace internal
 }   // namespace ceres
	// Ceres Solver - A fast non-linear least squares minimizer
	// Copyright 2015 Google Inc. All rights reserved.
	// http://ceres-solver.org/
	//
	// Redistribution and use in source and binary forms, with or without
	// modification, are permitted provided that the following conditions are met:
	//
	// * Redistributions of source code must retain the above copyright notice,
	// this list of conditions and the following disclaimer.
	// * Redistributions in binary form must reproduce the above copyright notice,
	// this list of conditions and the following disclaimer in the documentation
	// and/or other materials provided with the distribution.
	// * Neither the name of Google Inc. nor the names of its contributors may be
	// used to endorse or promote products derived from this software without
	// specific prior written permission.
	//
	// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
	// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
	// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
	// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
	// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
	// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
	// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
	// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
	// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
	// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
	// POSSIBILITY OF SUCH DAMAGE.
	//
	// Author: sameeragarwal@google.com (Sameer Agarwal)

	#include "ceres/sparse_normal_cholesky_solver.h"

	#include <algorithm>
	#include <cstring>
	#include <ctime>
	#include <sstream>

	#include "ceres/compressed_row_sparse_matrix.h"
	#include "ceres/cxsparse.h"
	#include "ceres/internal/eigen.h"
	#include "ceres/internal/scoped_ptr.h"
	#include "ceres/linear_solver.h"
	#include "ceres/suitesparse.h"
	#include "ceres/triplet_sparse_matrix.h"
	#include "ceres/types.h"
	#include "ceres/wall_time.h"
	#include "Eigen/SparseCore"

	#ifdef CERES_USE_EIGEN_SPARSE
	#include "Eigen/SparseCholesky"
	#endif

	namespace ceres {
	namespace internal {
	namespace {

	#ifdef CERES_USE_EIGEN_SPARSE
	// A templated factorized and solve function, which allows us to use
	// the same code independent of whether a AMD or a Natural ordering is
	// used.
	template <typename SimplicialCholeskySolver, typename SparseMatrixType>
	LinearSolver::Summary SimplicialLDLTSolve(
	const SparseMatrixType& lhs,
	const bool do_symbolic_analysis,
	SimplicialCholeskySolver* solver,
	double* rhs_and_solution,
	EventLogger* event_logger) {
	LinearSolver::Summary summary;
	summary.num_iterations = 1;
	summary.termination_type = LINEAR_SOLVER_SUCCESS;
	summary.message = "Success.";

	if (do_symbolic_analysis) {
	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

	#ifndef CERES_NO_CXSPARSE
	LinearSolver::Summary ComputeNormalEquationsAndSolveUsingCXSparse(
	CompressedRowSparseMatrix* A,
	double * rhs_and_solution,
	EventLogger* event_logger) {
	LinearSolver::Summary summary;
	summary.num_iterations = 1;
	summary.termination_type = LINEAR_SOLVER_SUCCESS;
	summary.message = "Success.";

	CXSparse cxsparse;

	// Wrap the augmented Jacobian in a compressed sparse column matrix.
	cs_di a_transpose = cxsparse.CreateSparseMatrixTransposeView(A);

	// Compute the normal equations. J'J delta = J'f and solve them
	// using a sparse Cholesky factorization. Notice that when compared
	// to SuiteSparse we have to explicitly compute the transpose of Jt,
	// and then the normal equations before they can be
	// factorized. CHOLMOD/SuiteSparse on the other hand can just work
	// off of Jt to compute the Cholesky factorization of the normal
	// equations.
	cs_di* a = cxsparse.TransposeMatrix(&a_transpose);
	cs_di* lhs = cxsparse.MatrixMatrixMultiply(&a_transpose, a);
	cxsparse.Free(a);
	event_logger->AddEvent("NormalEquations");

	cs_dis* factor = cxsparse.AnalyzeCholesky(lhs);
	event_logger->AddEvent("Analysis");

	if (factor == NULL) {
	summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
	summary.message = "CXSparse::AnalyzeCholesky failed.";
	} else if (!cxsparse.SolveCholesky(lhs, factor, rhs_and_solution)) {
	summary.termination_type = LINEAR_SOLVER_FAILURE;
	summary.message = "CXSparse::SolveCholesky failed.";
	}
	event_logger->AddEvent("Solve");

	cxsparse.Free(lhs);
	cxsparse.Free(factor);
	event_logger->AddEvent("TearDown");
	return summary;
	}

	#endif // CERES_NO_CXSPARSE

	} // namespace

	SparseNormalCholeskySolver::SparseNormalCholeskySolver(
	const LinearSolver::Options& options)
	: factor_(NULL),
	cxsparse_factor_(NULL),
	options_(options) {
	}

	void SparseNormalCholeskySolver::FreeFactorization() {
	if (factor_ != NULL) {
	ss_.Free(factor_);
	factor_ = NULL;
	}

	if (cxsparse_factor_ != NULL) {
	cxsparse_.Free(cxsparse_factor_);
	cxsparse_factor_ = NULL;
	}
	}

	SparseNormalCholeskySolver::~SparseNormalCholeskySolver() {
	FreeFactorization();
	}

	LinearSolver::Summary SparseNormalCholeskySolver::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().size() > 0) {
	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;
	}

	LinearSolver::Summary SparseNormalCholeskySolver::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("SparseNormalCholeskySolver::Eigen::Solve");
	// Compute the normal equations. J'J delta = J'f and solve them
	// using a sparse Cholesky factorization. Notice that when compared
	// to SuiteSparse we have to explicitly compute the normal equations
	// before they can be factorized. CHOLMOD/SuiteSparse on the other
	// hand can just work off of Jt to compute the Cholesky
	// factorization of the normal equations.

	if (options_.dynamic_sparsity) {
	// In the case where the problem has dynamic sparsity, it is not
	// worth using the ComputeOuterProduct routine, as the setup cost
	// is not amortized over multiple calls to 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;
	return SimplicialLDLTSolve(lhs,
	true,
	&solver,
	rhs_and_solution,
	&event_logger);
	}

	if (outer_product_.get() == NULL) {
	outer_product_.reset(
	CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
	*A, &pattern_));
	}

	CompressedRowSparseMatrix::ComputeOuterProduct(
	*A, pattern_, outer_product_.get());

	// Map to an upper triangular column major matrix.
	//
	// outer_product_ is a compressed row sparse matrix and in lower
	// triangular form, when mapped to a compressed column sparse
	// matrix, it becomes an upper triangular matrix.
	Eigen::MappedSparseMatrix<double, Eigen::ColMajor> lhs(
	outer_product_->num_rows(),
	outer_product_->num_rows(),
	outer_product_->num_nonzeros(),
	outer_product_->mutable_rows(),
	outer_product_->mutable_cols(),
	outer_product_->mutable_values());

	bool do_symbolic_analysis = false;

	// If using post ordering or an old version of Eigen, we cannot
	// depend on a preordered jacobian, so we work with a SimplicialLDLT
	// decomposition with AMD ordering.
	if (options_.use_postordering \|\|
	!EIGEN_VERSION_AT_LEAST(3, 2, 2)) {
	if (amd_ldlt_.get() == NULL) {
	amd_ldlt_.reset(new SimplicialLDLTWithAMDOrdering);
	do_symbolic_analysis = true;
	}

	return SimplicialLDLTSolve(lhs,
	do_symbolic_analysis,
	amd_ldlt_.get(),
	rhs_and_solution,
	&event_logger);
	}

	#if EIGEN_VERSION_AT_LEAST(3,2,2)
	// The common case
	if (natural_ldlt_.get() == NULL) {
	natural_ldlt_.reset(new SimplicialLDLTWithNaturalOrdering);
	do_symbolic_analysis = true;
	}

	return SimplicialLDLTSolve(lhs,
	do_symbolic_analysis,
	natural_ldlt_.get(),
	rhs_and_solution,
	&event_logger);
	#endif

	#endif // EIGEN_USE_EIGEN_SPARSE
	}

	LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse(
	CompressedRowSparseMatrix* A,
	double * rhs_and_solution) {
	#ifdef CERES_NO_CXSPARSE

	LinearSolver::Summary summary;
	summary.num_iterations = 0;
	summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
	summary.message =
	"SPARSE_NORMAL_CHOLESKY cannot be used with CX_SPARSE "
	"because Ceres was not built with support for CXSparse. "
	"This requires enabling building with -DCXSPARSE=ON.";

	return summary;

	#else

	EventLogger event_logger("SparseNormalCholeskySolver::CXSparse::Solve");
	if (options_.dynamic_sparsity) {
	return ComputeNormalEquationsAndSolveUsingCXSparse(A,
	rhs_and_solution,
	&event_logger);
	}

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

	// Compute the normal equations. J'J delta = J'f and solve them
	// using a sparse Cholesky factorization. Notice that when compared
	// to SuiteSparse we have to explicitly compute the normal equations
	// before they can be factorized. CHOLMOD/SuiteSparse on the other
	// hand can just work off of Jt to compute the Cholesky
	// factorization of the normal equations.
	if (outer_product_.get() == NULL) {
	outer_product_.reset(
	CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram(
	*A, &pattern_));
	}

	CompressedRowSparseMatrix::ComputeOuterProduct(
	*A, pattern_, outer_product_.get());
	cs_di lhs =
	cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get());

	event_logger.AddEvent("Setup");

	// Compute symbolic factorization if not available.
	if (cxsparse_factor_ == NULL) {
	if (options_.use_postordering) {
	cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(&lhs,
	A->col_blocks(),
	A->col_blocks());
	} else {
	cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(&lhs);
	}
	}
	event_logger.AddEvent("Analysis");

	if (cxsparse_factor_ == NULL) {
	summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
	summary.message =
	"CXSparse failure. Unable to find symbolic factorization.";
	} else if (!cxsparse_.SolveCholesky(&lhs,
	cxsparse_factor_,
	rhs_and_solution)) {
	summary.termination_type = LINEAR_SOLVER_FAILURE;
	summary.message = "CXSparse::SolveCholesky failed.";
	}
	event_logger.AddEvent("Solve");

	return summary;
	#endif
	}

	LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse(
	CompressedRowSparseMatrix* A,
	double * rhs_and_solution) {
	#ifdef CERES_NO_SUITESPARSE

	LinearSolver::Summary summary;
	summary.num_iterations = 0;
	summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
	summary.message =
	"SPARSE_NORMAL_CHOLESKY cannot be used with SUITE_SPARSE "
	"because Ceres was not built with support for SuiteSparse. "
	"This requires enabling building with -DSUITESPARSE=ON.";
	return summary;

	#else

	EventLogger event_logger("SparseNormalCholeskySolver::SuiteSparse::Solve");
	LinearSolver::Summary summary;
	summary.termination_type = LINEAR_SOLVER_SUCCESS;
	summary.num_iterations = 1;
	summary.message = "Success.";

	const int num_cols = A->num_cols();
	cholmod_sparse lhs = ss_.CreateSparseMatrixTransposeView(A);
	event_logger.AddEvent("Setup");

	if (options_.dynamic_sparsity) {
	FreeFactorization();
	}

	if (factor_ == NULL) {
	if (options_.use_postordering) {
	factor_ = ss_.BlockAnalyzeCholesky(&lhs,
	A->col_blocks(),
	A->row_blocks(),
	&summary.message);
	} else {
	if (options_.dynamic_sparsity) {
	factor_ = ss_.AnalyzeCholesky(&lhs, &summary.message);
	} else {
	factor_ = ss_.AnalyzeCholeskyWithNaturalOrdering(&lhs,
	&summary.message);
	}
	}
	}
	event_logger.AddEvent("Analysis");

	if (factor_ == NULL) {
	summary.termination_type = LINEAR_SOLVER_FATAL_ERROR;
	// No need to set message as it has already been set by the
	// symbolic analysis routines above.
	return summary;
	}

	summary.termination_type = ss_.Cholesky(&lhs, factor_, &summary.message);
	if (summary.termination_type != LINEAR_SOLVER_SUCCESS) {
	return summary;
	}

	cholmod_dense* rhs = ss_.CreateDenseVector(rhs_and_solution,
	num_cols,
	num_cols);
	cholmod_dense* solution = ss_.Solve(factor_, rhs, &summary.message);
	event_logger.AddEvent("Solve");

	ss_.Free(rhs);
	if (solution != NULL) {
	memcpy(rhs_and_solution, solution->x, num_cols * sizeof(*rhs_and_solution));
	ss_.Free(solution);
	} else {
	// No need to set message as it has already been set by the
	// numeric factorization routine above.
	summary.termination_type = LINEAR_SOLVER_FAILURE;
	}

	event_logger.AddEvent("Teardown");
	return summary;
	#endif
	}

	} // namespace internal
	} // namespace ceres