From 6d06e9b98f9db01acbe597e3a7908c87a6c83cde Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Fri, 21 Jan 2022 18:33:16 -0800 Subject: [PATCH] Add DenseCholesky Like SparseCholesky, the DenseCholesky interface abstracts away the solution of dense linear systems using Cholesky factorization. This allows the client code to not worry about the type of dense linear algebra library being used. DenseNormalCholeskySolver and DenseSchurComplementSolver code is considerably simpler as a result. Change-Id: Ie15f09ee376d5f9a64609e6a55ad83e99c76352a --- internal/ceres/CMakeLists.txt | 2 + internal/ceres/dense_cholesky.cc | 175 ++++++++++++++++++ internal/ceres/dense_cholesky.h | 133 +++++++++++++ internal/ceres/dense_cholesky_test.cc | 107 +++++++++++ internal/ceres/dense_linear_solver_test.cc | 4 +- .../ceres/dense_normal_cholesky_solver.cc | 73 +------- internal/ceres/dense_normal_cholesky_solver.h | 14 +- internal/ceres/lapack.cc | 58 ------ internal/ceres/schur_complement_solver.cc | 31 +--- internal/ceres/schur_complement_solver.h | 12 +- internal/ceres/sparse_cholesky.h | 9 +- 11 files changed, 445 insertions(+), 173 deletions(-) create mode 100644 internal/ceres/dense_cholesky.cc create mode 100644 internal/ceres/dense_cholesky.h create mode 100644 internal/ceres/dense_cholesky_test.cc diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 56e88cbbc..018c5b41f 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -72,6 +72,7 @@ set(CERES_INTERNAL_SRC covariance.cc covariance_impl.cc cxsparse.cc + dense_cholesky.cc dense_normal_cholesky_solver.cc dense_qr_solver.cc dense_sparse_matrix.cc @@ -453,6 +454,7 @@ if (BUILD_TESTING AND GFLAGS) ceres_test(covariance) ceres_test(cubic_interpolation) ceres_test(dense_linear_solver) + ceres_test(dense_cholesky) ceres_test(dense_sparse_matrix) ceres_test(detect_structure) ceres_test(dogleg_strategy) diff --git a/internal/ceres/dense_cholesky.cc b/internal/ceres/dense_cholesky.cc new file mode 100644 index 000000000..1037ad36a --- /dev/null +++ b/internal/ceres/dense_cholesky.cc @@ -0,0 +1,175 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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/dense_cholesky.h" + +#include +#include + +#ifndef CERES_NO_LAPACK + +// C interface to the LAPACK Cholesky factorization and triangular solve. +extern "C" void dpotrf_( + const char* uplo, const int* n, double* a, const int* lda, int* info); + +extern "C" void dpotrs_(const char* uplo, + const int* n, + const int* nrhs, + const double* a, + const int* lda, + double* b, + const int* ldb, + int* info); +#endif + +namespace ceres { +namespace internal { + +std::unique_ptr DenseCholesky::Create( + const LinearSolver::Options& options) { + std::unique_ptr dense_cholesky; + + switch (options.dense_linear_algebra_library_type) { + case EIGEN: + dense_cholesky = std::make_unique(); + break; + + case LAPACK: +#ifndef CERES_NO_LAPACK + dense_cholesky = std::make_unique(); + break; +#else + LOG(FATAL) << "Ceres was compiled without support for LAPACK."; +#endif + + default: + LOG(FATAL) << "Unknown dense linear algebra library type : " + << DenseLinearAlgebraLibraryTypeToString( + options.dense_linear_algebra_library_type); + } + return dense_cholesky; +} + +LinearSolverTerminationType DenseCholesky::FactorAndSolve( + int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message) { + LinearSolverTerminationType termination_type = + Factorize(num_cols, lhs, message); + if (termination_type == LINEAR_SOLVER_SUCCESS) { + termination_type = Solve(rhs, solution, message); + } + return termination_type; +} + +LinearSolverTerminationType EigenDenseCholesky::Factorize( + int num_cols, double* lhs, std::string* message) { + Eigen::Map m(lhs, num_cols, num_cols); + llt_.reset(new LLTType(m)); + if (llt_->info() != Eigen::Success) { + *message = "Eigen failure. Unable to perform dense Cholesky factorization."; + return LINEAR_SOLVER_FAILURE; + } + + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType EigenDenseCholesky::Solve(const double* rhs, + double* solution, + std::string* message) { + if (llt_->info() != Eigen::Success) { + *message = "Eigen failure. Unable to perform dense Cholesky factorization."; + return LINEAR_SOLVER_FAILURE; + } + + VectorRef(solution, llt_->cols()) = + llt_->solve(ConstVectorRef(rhs, llt_->cols())); + *message = "Success."; + return LINEAR_SOLVER_SUCCESS; +} + +LinearSolverTerminationType LAPACKDenseCholesky::Factorize( + int num_cols, double* lhs, std::string* message) { + lhs_ = lhs; + num_cols_ = num_cols; + + const char uplo = 'L'; + int info = 0; + dpotrf_(&uplo, &num_cols_, lhs_, &num_cols_, &info); + + if (info < 0) { + termination_type_ = LINEAR_SOLVER_FATAL_ERROR; + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dpotrf fatal error." + << "Argument: " << -info << " is invalid."; + } else if (info > 0) { + termination_type_ = LINEAR_SOLVER_FAILURE; + *message = StringPrintf( + "LAPACK::dpotrf numerical failure. " + "The leading minor of order %d is not positive definite.", + info); + } else { + termination_type_ = LINEAR_SOLVER_SUCCESS; + *message = "Success."; + } + return termination_type_; +} + +LinearSolverTerminationType LAPACKDenseCholesky::Solve(const double* rhs, + double* solution, + std::string* message) { + const char uplo = 'L'; + const int nrhs = 1; + int info = 0; + + std::copy_n(rhs, num_cols_, solution); + dpotrs_( + &uplo, &num_cols_, &nrhs, lhs_, &num_cols_, solution, &num_cols_, &info); + + if (info < 0) { + termination_type_ = LINEAR_SOLVER_FATAL_ERROR; + LOG(FATAL) << "Congratulations, you found a bug in Ceres." + << "Please report it." + << "LAPACK::dpotrs fatal error." + << "Argument: " << -info << " is invalid."; + } + + *message = "Success"; + termination_type_ = LINEAR_SOLVER_SUCCESS; + + return termination_type_; +} + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/dense_cholesky.h b/internal/ceres/dense_cholesky.h new file mode 100644 index 000000000..0859c6822 --- /dev/null +++ b/internal/ceres/dense_cholesky.h @@ -0,0 +1,133 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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) + +#ifndef CERES_INTERNAL_DENSE_CHOLESKY_H_ +#define CERES_INTERNAL_DENSE_CHOLESKY_H_ + +// This include must come before any #ifndef check on Ceres compile options. +// clang-format off +#include "ceres/internal/port.h" +// clang-format on + +#include + +#include "Eigen/Dense" +#include "ceres/linear_solver.h" +#include "glog/logging.h" + +namespace ceres { +namespace internal { + +// An interface that abstracts away the internal details of various dense linear +// algebra libraries and offers a simple API for solving dense symmetric +// positive definite linear systems using a Cholesky factorization. +class CERES_EXPORT_INTERNAL DenseCholesky { + public: + static std::unique_ptr Create( + const LinearSolver::Options& options); + + virtual ~DenseCholesky() = default; + + // Computes the Cholesky factorization of the given matrix. + // + // The input matrix lhs is assumed to be a column-major num_cols x num_cols + // matrix, that is symmetric positive definite with its lower triangular part + // containing the left hand side of the linear system being solved. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the factorization succeeds or not. + // As a result it is the user's responsibility to ensure that lhs is valid + // when Solve is called. + virtual LinearSolverTerminationType Factorize(int num_cols, + double* lhs, + std::string* message) = 0; + + // Computes the solution to the equation + // + // lhs * solution = rhs + // + // Calling Solve without calling Factorize is undefined behaviour. It is the + // user's responsibility to ensure that the input matrix lhs passed to + // Factorize has not been freed/modified when Solve is called. + virtual LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) = 0; + + // Convenience method which combines a call to Factorize and Solve. Solve is + // only called if Factorize returns LINEAR_SOLVER_SUCCESS. + // + // The input matrix lhs may be modified by the implementation to store the + // factorization, irrespective of whether the method succeeds or not. It is + // the user's responsibility to ensure that lhs is valid if and when Solve is + // called again after this call. + LinearSolverTerminationType FactorAndSolve(int num_cols, + double* lhs, + const double* rhs, + double* solution, + std::string* message); +}; + +class CERES_EXPORT_INTERNAL EigenDenseCholesky : public DenseCholesky { + public: + ~EigenDenseCholesky() override = default; + + LinearSolverTerminationType Factorize(int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + using LLTType = Eigen::LLT, Eigen::Lower>; + std::unique_ptr llt_; +}; + +class CERES_EXPORT_INTERNAL LAPACKDenseCholesky : public DenseCholesky { + public: + ~LAPACKDenseCholesky() override = default; + + LinearSolverTerminationType Factorize(int num_cols, + double* lhs, + std::string* message) override; + LinearSolverTerminationType Solve(const double* rhs, + double* solution, + std::string* message) override; + + private: + double* lhs_ = nullptr; + int num_cols_ = -1; + LinearSolverTerminationType termination_type_ = LINEAR_SOLVER_FATAL_ERROR; +}; + +} // namespace internal +} // namespace ceres + +#endif // CERES_INTERNAL_DENSE_CHOLESKY_H_ diff --git a/internal/ceres/dense_cholesky_test.cc b/internal/ceres/dense_cholesky_test.cc new file mode 100644 index 000000000..1b0ee65e4 --- /dev/null +++ b/internal/ceres/dense_cholesky_test.cc @@ -0,0 +1,107 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 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/dense_cholesky.h" + +#include +#include +#include + +#include "Eigen/Dense" +#include "ceres/internal/eigen.h" +#include "ceres/linear_solver.h" +#include "glog/logging.h" +#include "gmock/gmock.h" +#include "gtest/gtest.h" + +namespace ceres { +namespace internal { + +typedef DenseLinearAlgebraLibraryType Param; + +std::string ParamInfoToString(testing::TestParamInfo info) { + return DenseLinearAlgebraLibraryTypeToString(info.param); +} + +class DenseCholeskyTest : public ::testing::TestWithParam {}; + +TEST_P(DenseCholeskyTest, FactorAndSolve) { + // TODO(sameeragarwal): Convert these tests into type parameterized tests so + // that we can test the single and double precision solvers. + + using Scalar = double; + using MatrixType = Eigen::Matrix; + using VectorType = Eigen::Matrix; + + LinearSolver::Options options; + options.dense_linear_algebra_library_type = GetParam(); + std::unique_ptr dense_cholesky = + DenseCholesky::Create(options); + + const int kNumTrials = 10; + const int kMinNumCols = 1; + const int kMaxNumCols = 10; + + for (int num_cols = kMinNumCols; num_cols < kMaxNumCols; ++num_cols) { + for (int trial = 0; trial < kNumTrials; ++trial) { + const MatrixType a = MatrixType::Random(num_cols, num_cols); + MatrixType lhs = a.transpose() * a; + lhs += VectorType::Ones(num_cols).asDiagonal(); + Vector x = VectorType::Random(num_cols); + Vector rhs = lhs * x; + Vector actual = Vector::Random(num_cols); + + LinearSolver::Summary summary; + summary.termination_type = dense_cholesky->FactorAndSolve( + num_cols, lhs.data(), rhs.data(), actual.data(), &summary.message); + EXPECT_EQ(summary.termination_type, LINEAR_SOLVER_SUCCESS); + EXPECT_NEAR((x - actual).norm() / x.norm(), + 0.0, + std::numeric_limits::epsilon() * 10) + << "\nexpected: " << x.transpose() + << "\nactual : " << actual.transpose(); + } + } +} + +#ifndef CERES_NO_LAPACK +INSTANTIATE_TEST_SUITE_P(_, + DenseCholeskyTest, + ::testing::Values(EIGEN, LAPACK), + ParamInfoToString); +#else +INSTANTIATE_TEST_SUITE_P(_, + DenseCholeskyTest, + ::testing::Values(EIGEN), + ParamInfoToString); +#endif + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/dense_linear_solver_test.cc b/internal/ceres/dense_linear_solver_test.cc index 3929a6f4a..419ec3eb0 100644 --- a/internal/ceres/dense_linear_solver_test.cc +++ b/internal/ceres/dense_linear_solver_test.cc @@ -105,7 +105,9 @@ TEST_P(DenseLinearSolverTest, _) { const double residual = (expected_normal_rhs - actual_normal_rhs).norm() / expected_normal_rhs.norm(); - EXPECT_NEAR(residual, 0.0, 10 * std::numeric_limits::epsilon()); + EXPECT_NEAR(residual, 0.0, 10 * std::numeric_limits::epsilon()) + << "\nexpected: " << expected_normal_rhs.transpose() + << "\nactual: " << actual_normal_rhs.transpose(); } namespace { diff --git a/internal/ceres/dense_normal_cholesky_solver.cc b/internal/ceres/dense_normal_cholesky_solver.cc index 51c639097..73e5bf82c 100644 --- a/internal/ceres/dense_normal_cholesky_solver.cc +++ b/internal/ceres/dense_normal_cholesky_solver.cc @@ -36,7 +36,6 @@ #include "ceres/blas.h" #include "ceres/dense_sparse_matrix.h" #include "ceres/internal/eigen.h" -#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/types.h" #include "ceres/wall_time.h" @@ -46,25 +45,13 @@ namespace internal { DenseNormalCholeskySolver::DenseNormalCholeskySolver( const LinearSolver::Options& options) - : options_(options) {} + : options_(options), cholesky_(DenseCholesky::Create(options_)) {} 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(); @@ -94,64 +81,12 @@ LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingEigen( LinearSolver::Summary summary; summary.num_iterations = 1; - summary.termination_type = LINEAR_SOLVER_SUCCESS; - Eigen::LLT llt = - lhs.selfadjointView().llt(); + summary.termination_type = cholesky_->FactorAndSolve( + num_cols, lhs.data(), rhs.data(), x, &summary.message); + event_logger.AddEvent("FactorAndSolve"); - 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; } -LinearSolver::Summary DenseNormalCholeskySolver::SolveUsingLAPACK( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x) { - EventLogger event_logger("DenseNormalCholeskySolver::Solve"); - - 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. - A->AppendDiagonal(per_solve_options.D); - } - - const int num_cols = A->num_cols(); - Matrix lhs(num_cols, num_cols); - event_logger.AddEvent("Setup"); - - // lhs = A'A - // - // Note: This is a bit delicate, it assumes that the stride on this - // matrix is the same as the number of rows. - BLAS::SymmetricRankKUpdate( - A->num_rows(), num_cols, A->values(), true, 1.0, 0.0, lhs.data()); - - if (per_solve_options.D != NULL) { - // Undo the modifications to the matrix A. - A->RemoveDiagonal(); - } - - // TODO(sameeragarwal): Replace this with a gemv call for true blasness. - // rhs = A'b - VectorRef(x, num_cols) = - A->matrix().transpose() * ConstVectorRef(b, A->num_rows()); - event_logger.AddEvent("Product"); - - LinearSolver::Summary summary; - summary.num_iterations = 1; - summary.termination_type = LAPACK::SolveInPlaceUsingCholesky( - num_cols, lhs.data(), x, &summary.message); - event_logger.AddEvent("Solve"); - return summary; -} } // namespace internal } // namespace ceres diff --git a/internal/ceres/dense_normal_cholesky_solver.h b/internal/ceres/dense_normal_cholesky_solver.h index 68ea61129..d81dee90c 100644 --- a/internal/ceres/dense_normal_cholesky_solver.h +++ b/internal/ceres/dense_normal_cholesky_solver.h @@ -34,6 +34,7 @@ #ifndef CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ #define CERES_INTERNAL_DENSE_NORMAL_CHOLESKY_SOLVER_H_ +#include "ceres/dense_cholesky.h" #include "ceres/linear_solver.h" namespace ceres { @@ -84,19 +85,8 @@ class DenseNormalCholeskySolver : public DenseSparseMatrixSolver { const LinearSolver::PerSolveOptions& per_solve_options, double* x) final; - LinearSolver::Summary SolveUsingLAPACK( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x); - - LinearSolver::Summary SolveUsingEigen( - DenseSparseMatrix* A, - const double* b, - const LinearSolver::PerSolveOptions& per_solve_options, - double* x); - const LinearSolver::Options options_; + std::unique_ptr cholesky_; }; } // namespace internal diff --git a/internal/ceres/lapack.cc b/internal/ceres/lapack.cc index a159ec706..7ce00a5c2 100644 --- a/internal/ceres/lapack.cc +++ b/internal/ceres/lapack.cc @@ -35,18 +35,6 @@ #include "glog/logging.h" #ifndef CERES_NO_LAPACK -// C interface to the LAPACK Cholesky factorization and triangular solve. -extern "C" void dpotrf_(char* uplo, int* n, double* a, int* lda, int* info); - -extern "C" void dpotrs_(char* uplo, - int* n, - int* nrhs, - double* a, - int* lda, - double* b, - int* ldb, - int* info); - extern "C" void dgels_(char* uplo, int* m, int* n, @@ -63,52 +51,6 @@ extern "C" void dgels_(char* uplo, namespace ceres { namespace internal { -LinearSolverTerminationType LAPACK::SolveInPlaceUsingCholesky( - int num_rows, - const double* in_lhs, - double* rhs_and_solution, - std::string* message) { -#ifdef CERES_NO_LAPACK - LOG(FATAL) << "Ceres was built without a BLAS library."; - return LINEAR_SOLVER_FATAL_ERROR; -#else - char uplo = 'L'; - int n = num_rows; - int info = 0; - int nrhs = 1; - double* lhs = const_cast(in_lhs); - - dpotrf_(&uplo, &n, lhs, &n, &info); - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dpotrf fatal error." - << "Argument: " << -info << " is invalid."; - return LINEAR_SOLVER_FATAL_ERROR; - } - - if (info > 0) { - *message = StringPrintf( - "LAPACK::dpotrf numerical failure. " - "The leading minor of order %d is not positive definite.", - info); - return LINEAR_SOLVER_FAILURE; - } - - dpotrs_(&uplo, &n, &nrhs, lhs, &n, rhs_and_solution, &n, &info); - if (info < 0) { - LOG(FATAL) << "Congratulations, you found a bug in Ceres." - << "Please report it." - << "LAPACK::dpotrs fatal error." - << "Argument: " << -info << " is invalid."; - return LINEAR_SOLVER_FATAL_ERROR; - } - - *message = "Success"; - return LINEAR_SOLVER_SUCCESS; -#endif -} - int LAPACK::EstimateWorkSizeForQR(int num_rows, int num_cols) { #ifdef CERES_NO_LAPACK LOG(FATAL) << "Ceres was built without a LAPACK library."; diff --git a/internal/ceres/schur_complement_solver.cc b/internal/ceres/schur_complement_solver.cc index 65e7854f9..866fd67a3 100644 --- a/internal/ceres/schur_complement_solver.cc +++ b/internal/ceres/schur_complement_solver.cc @@ -46,7 +46,6 @@ #include "ceres/conjugate_gradients_solver.h" #include "ceres/detect_structure.h" #include "ceres/internal/eigen.h" -#include "ceres/lapack.h" #include "ceres/linear_solver.h" #include "ceres/sparse_cholesky.h" #include "ceres/triplet_sparse_matrix.h" @@ -201,8 +200,8 @@ LinearSolver::Summary DenseSchurComplementSolver::SolveReducedLinearSystem( summary.termination_type = LINEAR_SOLVER_SUCCESS; summary.message = "Success."; - const BlockRandomAccessDenseMatrix* m = - down_cast(lhs()); + BlockRandomAccessDenseMatrix* m = + down_cast(mutable_lhs()); const int num_rows = m->num_rows(); // The case where there are no f blocks, and the system is block @@ -212,26 +211,8 @@ LinearSolver::Summary DenseSchurComplementSolver::SolveReducedLinearSystem( } summary.num_iterations = 1; - - if (options().dense_linear_algebra_library_type == EIGEN) { - Eigen::LLT llt = - ConstMatrixRef(m->values(), num_rows, num_rows) - .selfadjointView() - .llt(); - if (llt.info() != Eigen::Success) { - summary.termination_type = LINEAR_SOLVER_FAILURE; - summary.message = - "Eigen failure. Unable to perform dense Cholesky factorization."; - return summary; - } - - VectorRef(solution, num_rows) = llt.solve(ConstVectorRef(rhs(), num_rows)); - } else { - VectorRef(solution, num_rows) = ConstVectorRef(rhs(), num_rows); - summary.termination_type = LAPACK::SolveInPlaceUsingCholesky( - num_rows, m->values(), solution, &summary.message); - } - + summary.termination_type = cholesky_->FactorAndSolve( + num_rows, m->mutable_values(), rhs(), solution, &summary.message); return summary; } @@ -377,8 +358,8 @@ SparseSchurComplementSolver::SolveReducedLinearSystemUsingConjugateGradients( preconditioner_.reset(new BlockRandomAccessDiagonalMatrix(blocks_)); } - BlockRandomAccessSparseMatrix* sc = down_cast( - const_cast(lhs())); + BlockRandomAccessSparseMatrix* sc = + down_cast(mutable_lhs()); // Extract block diagonal from the Schur complement to construct the // schur_jacobi preconditioner. diff --git a/internal/ceres/schur_complement_solver.h b/internal/ceres/schur_complement_solver.h index 3bfa22f22..60af0434b 100644 --- a/internal/ceres/schur_complement_solver.h +++ b/internal/ceres/schur_complement_solver.h @@ -40,6 +40,7 @@ #include "ceres/block_random_access_matrix.h" #include "ceres/block_sparse_matrix.h" #include "ceres/block_structure.h" +#include "ceres/dense_cholesky.h" #include "ceres/internal/port.h" #include "ceres/linear_solver.h" #include "ceres/schur_eliminator.h" @@ -130,10 +131,12 @@ class CERES_EXPORT_INTERNAL SchurComplementSolver protected: const LinearSolver::Options& options() const { return options_; } - const BlockRandomAccessMatrix* lhs() const { return lhs_.get(); } void set_lhs(BlockRandomAccessMatrix* lhs) { lhs_.reset(lhs); } - const double* rhs() const { return rhs_.get(); } + const BlockRandomAccessMatrix* lhs() const { return lhs_.get(); } + BlockRandomAccessMatrix* mutable_lhs() { return lhs_.get(); } + void set_rhs(double* rhs) { rhs_.reset(rhs); } + const double* rhs() const { return rhs_.get(); } private: virtual void InitStorage(const CompressedRowBlockStructure* bs) = 0; @@ -152,7 +155,8 @@ class CERES_EXPORT_INTERNAL SchurComplementSolver class DenseSchurComplementSolver : public SchurComplementSolver { public: explicit DenseSchurComplementSolver(const LinearSolver::Options& options) - : SchurComplementSolver(options) {} + : SchurComplementSolver(options), + cholesky_(DenseCholesky::Create(options)) {} DenseSchurComplementSolver(const DenseSchurComplementSolver&) = delete; void operator=(const DenseSchurComplementSolver&) = delete; @@ -163,6 +167,8 @@ class DenseSchurComplementSolver : public SchurComplementSolver { LinearSolver::Summary SolveReducedLinearSystem( const LinearSolver::PerSolveOptions& per_solve_options, double* solution) final; + + std::unique_ptr cholesky_; }; // Sparse Cholesky factorization based solver. diff --git a/internal/ceres/sparse_cholesky.h b/internal/ceres/sparse_cholesky.h index a6af6b2c2..39670d5e5 100644 --- a/internal/ceres/sparse_cholesky.h +++ b/internal/ceres/sparse_cholesky.h @@ -104,11 +104,10 @@ class CERES_EXPORT_INTERNAL SparseCholesky { // Convenience method which combines a call to Factorize and // Solve. Solve is only called if Factorize returns // LINEAR_SOLVER_SUCCESS. - virtual LinearSolverTerminationType FactorAndSolve( - CompressedRowSparseMatrix* lhs, - const double* rhs, - double* solution, - std::string* message); + LinearSolverTerminationType FactorAndSolve(CompressedRowSparseMatrix* lhs, + const double* rhs, + double* solution, + std::string* message); }; class IterativeRefiner;