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;