|  | // Ceres Solver - A fast non-linear least squares minimizer | 
|  | // Copyright 2017 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_cholesky.h" | 
|  |  | 
|  | #include <memory> | 
|  | #include <numeric> | 
|  | #include <vector> | 
|  |  | 
|  | #include "Eigen/Dense" | 
|  | #include "Eigen/SparseCore" | 
|  | #include "ceres/block_sparse_matrix.h" | 
|  | #include "ceres/compressed_row_sparse_matrix.h" | 
|  | #include "ceres/inner_product_computer.h" | 
|  | #include "ceres/internal/eigen.h" | 
|  | #include "ceres/iterative_refiner.h" | 
|  | #include "ceres/random.h" | 
|  | #include "glog/logging.h" | 
|  | #include "gmock/gmock.h" | 
|  | #include "gtest/gtest.h" | 
|  |  | 
|  | namespace ceres { | 
|  | namespace internal { | 
|  |  | 
|  | BlockSparseMatrix* CreateRandomFullRankMatrix(const int num_col_blocks, | 
|  | const int min_col_block_size, | 
|  | const int max_col_block_size, | 
|  | const double block_density) { | 
|  | // Create a random matrix | 
|  | BlockSparseMatrix::RandomMatrixOptions options; | 
|  | options.num_col_blocks = num_col_blocks; | 
|  | options.min_col_block_size = min_col_block_size; | 
|  | options.max_col_block_size = max_col_block_size; | 
|  |  | 
|  | options.num_row_blocks = 2 * num_col_blocks; | 
|  | options.min_row_block_size = 1; | 
|  | options.max_row_block_size = max_col_block_size; | 
|  | options.block_density = block_density; | 
|  | std::unique_ptr<BlockSparseMatrix> random_matrix( | 
|  | BlockSparseMatrix::CreateRandomMatrix(options)); | 
|  |  | 
|  | // Add a diagonal block sparse matrix to make it full rank. | 
|  | Vector diagonal = Vector::Ones(random_matrix->num_cols()); | 
|  | std::unique_ptr<BlockSparseMatrix> block_diagonal( | 
|  | BlockSparseMatrix::CreateDiagonalMatrix( | 
|  | diagonal.data(), random_matrix->block_structure()->cols)); | 
|  | random_matrix->AppendRows(*block_diagonal); | 
|  | return random_matrix.release(); | 
|  | } | 
|  |  | 
|  | bool ComputeExpectedSolution(const CompressedRowSparseMatrix& lhs, | 
|  | const Vector& rhs, | 
|  | Vector* solution) { | 
|  | Matrix eigen_lhs; | 
|  | lhs.ToDenseMatrix(&eigen_lhs); | 
|  | if (lhs.storage_type() == CompressedRowSparseMatrix::UPPER_TRIANGULAR) { | 
|  | Matrix full_lhs = eigen_lhs.selfadjointView<Eigen::Upper>(); | 
|  | Eigen::LLT<Matrix, Eigen::Upper> llt = | 
|  | eigen_lhs.selfadjointView<Eigen::Upper>().llt(); | 
|  | if (llt.info() != Eigen::Success) { | 
|  | return false; | 
|  | } | 
|  | *solution = llt.solve(rhs); | 
|  | return (llt.info() == Eigen::Success); | 
|  | } | 
|  |  | 
|  | Matrix full_lhs = eigen_lhs.selfadjointView<Eigen::Lower>(); | 
|  | Eigen::LLT<Matrix, Eigen::Lower> llt = | 
|  | eigen_lhs.selfadjointView<Eigen::Lower>().llt(); | 
|  | if (llt.info() != Eigen::Success) { | 
|  | return false; | 
|  | } | 
|  | *solution = llt.solve(rhs); | 
|  | return (llt.info() == Eigen::Success); | 
|  | } | 
|  |  | 
|  | void SparseCholeskySolverUnitTest( | 
|  | const SparseLinearAlgebraLibraryType sparse_linear_algebra_library_type, | 
|  | const OrderingType ordering_type, | 
|  | const bool use_block_structure, | 
|  | const int num_blocks, | 
|  | const int min_block_size, | 
|  | const int max_block_size, | 
|  | const double block_density) { | 
|  | LinearSolver::Options sparse_cholesky_options; | 
|  | sparse_cholesky_options.sparse_linear_algebra_library_type = | 
|  | sparse_linear_algebra_library_type; | 
|  | sparse_cholesky_options.use_postordering  = (ordering_type == AMD); | 
|  | std::unique_ptr<SparseCholesky> sparse_cholesky = SparseCholesky::Create( | 
|  | sparse_cholesky_options); | 
|  | const CompressedRowSparseMatrix::StorageType storage_type = | 
|  | sparse_cholesky->StorageType(); | 
|  |  | 
|  | std::unique_ptr<BlockSparseMatrix> m(CreateRandomFullRankMatrix( | 
|  | num_blocks, min_block_size, max_block_size, block_density)); | 
|  | std::unique_ptr<InnerProductComputer> inner_product_computer( | 
|  | InnerProductComputer::Create(*m, storage_type)); | 
|  | inner_product_computer->Compute(); | 
|  | CompressedRowSparseMatrix* lhs = inner_product_computer->mutable_result(); | 
|  |  | 
|  | if (!use_block_structure) { | 
|  | lhs->mutable_row_blocks()->clear(); | 
|  | lhs->mutable_col_blocks()->clear(); | 
|  | } | 
|  |  | 
|  | Vector rhs = Vector::Random(lhs->num_rows()); | 
|  | Vector expected(lhs->num_rows()); | 
|  | Vector actual(lhs->num_rows()); | 
|  |  | 
|  | EXPECT_TRUE(ComputeExpectedSolution(*lhs, rhs, &expected)); | 
|  | std::string message; | 
|  | EXPECT_EQ(sparse_cholesky->FactorAndSolve( | 
|  | lhs, rhs.data(), actual.data(), &message), | 
|  | LINEAR_SOLVER_SUCCESS); | 
|  | Matrix eigen_lhs; | 
|  | lhs->ToDenseMatrix(&eigen_lhs); | 
|  | EXPECT_NEAR((actual - expected).norm() / actual.norm(), | 
|  | 0.0, | 
|  | std::numeric_limits<double>::epsilon() * 10) | 
|  | << "\n" | 
|  | << eigen_lhs; | 
|  | } | 
|  |  | 
|  | typedef ::testing::tuple<SparseLinearAlgebraLibraryType, OrderingType, bool> | 
|  | Param; | 
|  |  | 
|  | std::string ParamInfoToString(testing::TestParamInfo<Param> info) { | 
|  | Param param = info.param; | 
|  | std::stringstream ss; | 
|  | ss << SparseLinearAlgebraLibraryTypeToString(::testing::get<0>(param)) << "_" | 
|  | << (::testing::get<1>(param) == AMD ? "AMD" : "NATURAL") << "_" | 
|  | << (::testing::get<2>(param) ? "UseBlockStructure" : "NoBlockStructure"); | 
|  | return ss.str(); | 
|  | } | 
|  |  | 
|  | class SparseCholeskyTest : public ::testing::TestWithParam<Param> {}; | 
|  |  | 
|  | TEST_P(SparseCholeskyTest, FactorAndSolve) { | 
|  | SetRandomState(2982); | 
|  | const int kMinNumBlocks = 1; | 
|  | const int kMaxNumBlocks = 10; | 
|  | const int kNumTrials = 10; | 
|  | const int kMinBlockSize = 1; | 
|  | const int kMaxBlockSize = 5; | 
|  |  | 
|  | for (int num_blocks = kMinNumBlocks; num_blocks < kMaxNumBlocks; | 
|  | ++num_blocks) { | 
|  | for (int trial = 0; trial < kNumTrials; ++trial) { | 
|  | const double block_density = std::max(0.1, RandDouble()); | 
|  | Param param = GetParam(); | 
|  | SparseCholeskySolverUnitTest(::testing::get<0>(param), | 
|  | ::testing::get<1>(param), | 
|  | ::testing::get<2>(param), | 
|  | num_blocks, | 
|  | kMinBlockSize, | 
|  | kMaxBlockSize, | 
|  | block_density); | 
|  | } | 
|  | } | 
|  | } | 
|  |  | 
|  | #ifndef CERES_NO_SUITESPARSE | 
|  | INSTANTIATE_TEST_CASE_P(SuiteSparseCholesky, | 
|  | SparseCholeskyTest, | 
|  | ::testing::Combine(::testing::Values(SUITE_SPARSE), | 
|  | ::testing::Values(AMD, NATURAL), | 
|  | ::testing::Values(true, false)), | 
|  | ParamInfoToString); | 
|  | #endif | 
|  |  | 
|  | #ifndef CERES_NO_CXSPARSE | 
|  | INSTANTIATE_TEST_CASE_P(CXSparseCholesky, | 
|  | SparseCholeskyTest, | 
|  | ::testing::Combine(::testing::Values(CX_SPARSE), | 
|  | ::testing::Values(AMD, NATURAL), | 
|  | ::testing::Values(true, false)), | 
|  | ParamInfoToString); | 
|  | #endif | 
|  |  | 
|  | #ifdef CERES_USE_EIGEN_SPARSE | 
|  | INSTANTIATE_TEST_CASE_P(EigenSparseCholesky, | 
|  | SparseCholeskyTest, | 
|  | ::testing::Combine(::testing::Values(EIGEN_SPARSE), | 
|  | ::testing::Values(AMD, NATURAL), | 
|  | ::testing::Values(true, false)), | 
|  | ParamInfoToString); | 
|  |  | 
|  | INSTANTIATE_TEST_CASE_P(EigenSparseCholeskySingle, | 
|  | SparseCholeskyTest, | 
|  | ::testing::Combine(::testing::Values(EIGEN_SPARSE), | 
|  | ::testing::Values(AMD, NATURAL), | 
|  | ::testing::Values(true, false)), | 
|  | ParamInfoToString); | 
|  | #endif | 
|  |  | 
|  | class MockSparseCholesky : public SparseCholesky { | 
|  | public: | 
|  | MOCK_CONST_METHOD0(StorageType, CompressedRowSparseMatrix::StorageType()); | 
|  | MOCK_METHOD2(Factorize, | 
|  | LinearSolverTerminationType(CompressedRowSparseMatrix* lhs, | 
|  | std::string* message)); | 
|  | MOCK_METHOD3(Solve, | 
|  | LinearSolverTerminationType(const double* rhs, | 
|  | double* solution, | 
|  | std::string* message)); | 
|  | }; | 
|  |  | 
|  | class MockIterativeRefiner : public IterativeRefiner { | 
|  | public: | 
|  | MockIterativeRefiner() : IterativeRefiner(1) {} | 
|  | MOCK_METHOD4(Refine, | 
|  | void (const SparseMatrix& lhs, | 
|  | const double* rhs, | 
|  | SparseCholesky* sparse_cholesky, | 
|  | double* solution)); | 
|  | }; | 
|  |  | 
|  |  | 
|  | using testing::_; | 
|  | using testing::Return; | 
|  |  | 
|  | TEST(RefinedSparseCholesky, StorageType) { | 
|  | MockSparseCholesky* mock_sparse_cholesky = new MockSparseCholesky; | 
|  | MockIterativeRefiner* mock_iterative_refiner = new MockIterativeRefiner; | 
|  | EXPECT_CALL(*mock_sparse_cholesky, StorageType()) | 
|  | .Times(1) | 
|  | .WillRepeatedly(Return(CompressedRowSparseMatrix::UPPER_TRIANGULAR)); | 
|  | EXPECT_CALL(*mock_iterative_refiner, Refine(_, _, _, _)) | 
|  | .Times(0); | 
|  | std::unique_ptr<SparseCholesky> sparse_cholesky(mock_sparse_cholesky); | 
|  | std::unique_ptr<IterativeRefiner> iterative_refiner(mock_iterative_refiner); | 
|  | RefinedSparseCholesky refined_sparse_cholesky(std::move(sparse_cholesky), | 
|  | std::move(iterative_refiner)); | 
|  | EXPECT_EQ(refined_sparse_cholesky.StorageType(), | 
|  | CompressedRowSparseMatrix::UPPER_TRIANGULAR); | 
|  | }; | 
|  |  | 
|  | TEST(RefinedSparseCholesky, Factorize) { | 
|  | MockSparseCholesky* mock_sparse_cholesky = new MockSparseCholesky; | 
|  | MockIterativeRefiner* mock_iterative_refiner = new MockIterativeRefiner; | 
|  | EXPECT_CALL(*mock_sparse_cholesky, Factorize(_, _)) | 
|  | .Times(1) | 
|  | .WillRepeatedly(Return(LINEAR_SOLVER_SUCCESS)); | 
|  | EXPECT_CALL(*mock_iterative_refiner, Refine(_, _, _, _)) | 
|  | .Times(0); | 
|  | std::unique_ptr<SparseCholesky> sparse_cholesky(mock_sparse_cholesky); | 
|  | std::unique_ptr<IterativeRefiner> iterative_refiner(mock_iterative_refiner); | 
|  | RefinedSparseCholesky refined_sparse_cholesky(std::move(sparse_cholesky), | 
|  | std::move(iterative_refiner)); | 
|  | CompressedRowSparseMatrix m(1, 1, 1); | 
|  | std::string message; | 
|  | EXPECT_EQ(refined_sparse_cholesky.Factorize(&m, &message), | 
|  | LINEAR_SOLVER_SUCCESS); | 
|  | }; | 
|  |  | 
|  | TEST(RefinedSparseCholesky, FactorAndSolveWithUnsuccessfulFactorization) { | 
|  | MockSparseCholesky* mock_sparse_cholesky = new MockSparseCholesky; | 
|  | MockIterativeRefiner* mock_iterative_refiner = new MockIterativeRefiner; | 
|  | EXPECT_CALL(*mock_sparse_cholesky, Factorize(_, _)) | 
|  | .Times(1) | 
|  | .WillRepeatedly(Return(LINEAR_SOLVER_FAILURE)); | 
|  | EXPECT_CALL(*mock_sparse_cholesky, Solve(_, _, _)) | 
|  | .Times(0); | 
|  | EXPECT_CALL(*mock_iterative_refiner, Refine(_, _, _, _)) | 
|  | .Times(0); | 
|  | std::unique_ptr<SparseCholesky> sparse_cholesky(mock_sparse_cholesky); | 
|  | std::unique_ptr<IterativeRefiner> iterative_refiner(mock_iterative_refiner); | 
|  | RefinedSparseCholesky refined_sparse_cholesky(std::move(sparse_cholesky), | 
|  | std::move(iterative_refiner)); | 
|  | CompressedRowSparseMatrix m(1, 1, 1); | 
|  | std::string message; | 
|  | double rhs; | 
|  | double solution; | 
|  | EXPECT_EQ(refined_sparse_cholesky.FactorAndSolve(&m, &rhs, &solution, &message), | 
|  | LINEAR_SOLVER_FAILURE); | 
|  | }; | 
|  |  | 
|  | TEST(RefinedSparseCholesky, FactorAndSolveWithSuccess) { | 
|  | MockSparseCholesky* mock_sparse_cholesky = new MockSparseCholesky; | 
|  | std::unique_ptr<MockIterativeRefiner> mock_iterative_refiner(new MockIterativeRefiner); | 
|  | EXPECT_CALL(*mock_sparse_cholesky, Factorize(_, _)) | 
|  | .Times(1) | 
|  | .WillRepeatedly(Return(LINEAR_SOLVER_SUCCESS)); | 
|  | EXPECT_CALL(*mock_sparse_cholesky, Solve(_, _, _)) | 
|  | .Times(1) | 
|  | .WillRepeatedly(Return(LINEAR_SOLVER_SUCCESS)); | 
|  | EXPECT_CALL(*mock_iterative_refiner, Refine(_, _, _, _)) | 
|  | .Times(1); | 
|  |  | 
|  | std::unique_ptr<SparseCholesky> sparse_cholesky(mock_sparse_cholesky); | 
|  | std::unique_ptr<IterativeRefiner> iterative_refiner(std::move(mock_iterative_refiner)); | 
|  | RefinedSparseCholesky refined_sparse_cholesky(std::move(sparse_cholesky), | 
|  | std::move(iterative_refiner)); | 
|  | CompressedRowSparseMatrix m(1, 1, 1); | 
|  | std::string message; | 
|  | double rhs; | 
|  | double solution; | 
|  | EXPECT_EQ(refined_sparse_cholesky.FactorAndSolve(&m, &rhs, &solution, &message), | 
|  | LINEAR_SOLVER_SUCCESS); | 
|  | }; | 
|  |  | 
|  | }  // namespace internal | 
|  | }  // namespace ceres |