From 5742b7d0f14d2d170054623ccfee09ea214b8ed9 Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Wed, 26 Aug 2015 09:24:33 -0700 Subject: [PATCH] Improve performance of SPARSE_NORMAL_CHOLESKY + dynamic_sparsity The outer product computation logic in SparseNormalCholeskySolver does not work well with dynamic sparsity. The overhead of computing the sparsity pattern of the normal equations is only amortized if the sparsity is constant. If the sparsity can change from call to call SparseNormalCholeskySolver will actually be more expensive. For Eigen and for CXSparse we now explicitly compute the normal equations using their respective matrix-matrix product routines and solve. Change-Id: Ifbd8ed78987cdf71640e66ed69500442526a23d4 --- docs/source/version_history.rst | 5 +- .../ceres/sparse_normal_cholesky_solver.cc | 137 ++++++++++++------ .../ceres/sparse_normal_cholesky_solver.h | 6 +- 3 files changed, 97 insertions(+), 51 deletions(-) diff --git a/docs/source/version_history.rst b/docs/source/version_history.rst index fe0ee7be6..664826c96 100644 --- a/docs/source/version_history.rst +++ b/docs/source/version_history.rst @@ -20,7 +20,10 @@ New Features can be constructed as a cartesian product of other local parameterization. #. Add DynamicCostFunctionToFunctor. (David Gossow) -#. Optionally export Ceres build directory into local CMake package registry. +#. Optionally export Ceres build directory into local CMake package + registry. +#. Faster ``SPARSE_NORMAL_CHOLESKY`` in the presence of dynamic + sparsity. Bug Fixes & Minor Changes ------------------------- diff --git a/internal/ceres/sparse_normal_cholesky_solver.cc b/internal/ceres/sparse_normal_cholesky_solver.cc index ef9bcc7b1..ed00879b4 100644 --- a/internal/ceres/sparse_normal_cholesky_solver.cc +++ b/internal/ceres/sparse_normal_cholesky_solver.cc @@ -57,9 +57,9 @@ namespace { // 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 +template LinearSolver::Summary SimplicialLDLTSolve( - Eigen::MappedSparseMatrix& lhs, + const SparseMatrixType& lhs, const bool do_symbolic_analysis, SimplicialCholeskySolver* solver, double* rhs_and_solution, @@ -103,6 +103,53 @@ LinearSolver::Summary SimplicialLDLTSolve( #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( @@ -155,13 +202,13 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( LinearSolver::Summary summary; switch (options_.sparse_linear_algebra_library_type) { case SUITE_SPARSE: - summary = SolveImplUsingSuiteSparse(A, per_solve_options, x); + summary = SolveImplUsingSuiteSparse(A, x); break; case CX_SPARSE: - summary = SolveImplUsingCXSparse(A, per_solve_options, x); + summary = SolveImplUsingCXSparse(A, x); break; case EIGEN_SPARSE: - summary = SolveImplUsingEigen(A, per_solve_options, x); + summary = SolveImplUsingEigen(A, x); break; default: LOG(FATAL) << "Unknown sparse linear algebra library : " @@ -177,7 +224,6 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImpl( LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& per_solve_options, double * rhs_and_solution) { #ifndef CERES_USE_EIGEN_SPARSE @@ -200,10 +246,29 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( // 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. - // - // TODO(sameeragarwal): See note about how this maybe a bad idea for - // dynamic sparsity. - if (outer_product_.get() == NULL || options_.dynamic_sparsity) { + + 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 a( + A->num_rows(), + A->num_cols(), + A->num_nonzeros(), + A->mutable_rows(), + A->mutable_cols(), + A->mutable_values()); + + Eigen::SparseMatrix lhs = a.transpose() * a; + Eigen::SimplicialLDLT > solver; + return SimplicialLDLTSolve(lhs, + true, + &solver, + rhs_and_solution, + &event_logger); + } + + if (outer_product_.get() == NULL) { outer_product_.reset( CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( *A, &pattern_)); @@ -217,7 +282,7 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( // 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 AtA( + Eigen::MappedSparseMatrix lhs( outer_product_->num_rows(), outer_product_->num_rows(), outer_product_->num_nonzeros(), @@ -225,27 +290,19 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( outer_product_->mutable_cols(), outer_product_->mutable_values()); - // Dynamic sparsity means that we cannot depend on a static analysis - // of sparsity structure of the jacobian, so we compute a new - // symbolic factorization every time. - if (options_.dynamic_sparsity) { - amd_ldlt_.reset(NULL); - } - bool do_symbolic_analysis = false; - // If using post ordering or dynamic sparsity, or an old version of - // Eigen, we cannot depend on a preordered jacobian, so we work with - // a SimplicialLDLT decomposition with AMD ordering. + // 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 || - options_.dynamic_sparsity || !EIGEN_VERSION_AT_LEAST(3, 2, 2)) { if (amd_ldlt_.get() == NULL) { amd_ldlt_.reset(new SimplicialLDLTWithAMDOrdering); do_symbolic_analysis = true; } - return SimplicialLDLTSolve(AtA, + return SimplicialLDLTSolve(lhs, do_symbolic_analysis, amd_ldlt_.get(), rhs_and_solution, @@ -259,7 +316,7 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( do_symbolic_analysis = true; } - return SimplicialLDLTSolve(AtA, + return SimplicialLDLTSolve(lhs, do_symbolic_analysis, natural_ldlt_.get(), rhs_and_solution, @@ -269,11 +326,8 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingEigen( #endif // EIGEN_USE_EIGEN_SPARSE } - - LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& per_solve_options, double * rhs_and_solution) { #ifdef CERES_NO_CXSPARSE @@ -290,6 +344,11 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( #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; @@ -302,11 +361,7 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( // 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. - // - // TODO(sameeragarwal): If dynamic sparsity is enabled, then this is - // not a good idea performance wise, since the jacobian has far too - // many entries and the program will go crazy with memory. - if (outer_product_.get() == NULL || options_.dynamic_sparsity) { + if (outer_product_.get() == NULL) { outer_product_.reset( CompressedRowSparseMatrix::CreateOuterProductMatrixAndProgram( *A, &pattern_)); @@ -314,27 +369,19 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( CompressedRowSparseMatrix::ComputeOuterProduct( *A, pattern_, outer_product_.get()); - cs_di AtA_view = + cs_di lhs = cxsparse_.CreateSparseMatrixTransposeView(outer_product_.get()); - cs_di* AtA = &AtA_view; event_logger.AddEvent("Setup"); // Compute symbolic factorization if not available. - if (options_.dynamic_sparsity) { - FreeFactorization(); - } if (cxsparse_factor_ == NULL) { if (options_.use_postordering) { - cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(AtA, + cxsparse_factor_ = cxsparse_.BlockAnalyzeCholesky(&lhs, A->col_blocks(), A->col_blocks()); } else { - if (options_.dynamic_sparsity) { - cxsparse_factor_ = cxsparse_.AnalyzeCholesky(AtA); - } else { - cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(AtA); - } + cxsparse_factor_ = cxsparse_.AnalyzeCholeskyWithNaturalOrdering(&lhs); } } event_logger.AddEvent("Analysis"); @@ -343,7 +390,7 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( summary.termination_type = LINEAR_SOLVER_FATAL_ERROR; summary.message = "CXSparse failure. Unable to find symbolic factorization."; - } else if (!cxsparse_.SolveCholesky(AtA, + } else if (!cxsparse_.SolveCholesky(&lhs, cxsparse_factor_, rhs_and_solution)) { summary.termination_type = LINEAR_SOLVER_FAILURE; @@ -357,7 +404,6 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingCXSparse( LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& per_solve_options, double * rhs_and_solution) { #ifdef CERES_NO_SUITESPARSE @@ -385,6 +431,7 @@ LinearSolver::Summary SparseNormalCholeskySolver::SolveImplUsingSuiteSparse( if (options_.dynamic_sparsity) { FreeFactorization(); } + if (factor_ == NULL) { if (options_.use_postordering) { factor_ = ss_.BlockAnalyzeCholesky(&lhs, diff --git a/internal/ceres/sparse_normal_cholesky_solver.h b/internal/ceres/sparse_normal_cholesky_solver.h index d847002f2..2a93bc56d 100644 --- a/internal/ceres/sparse_normal_cholesky_solver.h +++ b/internal/ceres/sparse_normal_cholesky_solver.h @@ -69,19 +69,15 @@ class SparseNormalCholeskySolver : public CompressedRowSparseMatrixSolver { LinearSolver::Summary SolveImplUsingSuiteSparse( CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& options, double* rhs_and_solution); - // Crashes if CSparse is not installed. + LinearSolver::Summary SolveImplUsingCXSparse( CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& options, double* rhs_and_solution); - // Crashes if CERES_USE_EIGEN_SPARSE is not defined. LinearSolver::Summary SolveImplUsingEigen( CompressedRowSparseMatrix* A, - const LinearSolver::PerSolveOptions& options, double* rhs_and_solution); void FreeFactorization();