diff --git a/internal/ceres/implicit_schur_complement_test.cc b/internal/ceres/implicit_schur_complement_test.cc index e586ea14e..21401f70d 100644 --- a/internal/ceres/implicit_schur_complement_test.cc +++ b/internal/ceres/implicit_schur_complement_test.cc @@ -89,7 +89,8 @@ class ImplicitSchurComplementTest : public ::testing::Test { scoped_ptr eliminator( SchurEliminatorBase::Create(options)); CHECK_NOTNULL(eliminator.get()); - eliminator->Init(num_eliminate_blocks_, bs); + const bool kFullRankETE = true; + eliminator->Init(num_eliminate_blocks_, kFullRankETE, bs); lhs->resize(num_schur_rows, num_schur_rows); rhs->resize(num_schur_rows); diff --git a/internal/ceres/schur_complement_solver.cc b/internal/ceres/schur_complement_solver.cc index 65449832c..81e0d46c1 100644 --- a/internal/ceres/schur_complement_solver.cc +++ b/internal/ceres/schur_complement_solver.cc @@ -135,7 +135,9 @@ LinearSolver::Summary SchurComplementSolver::SolveImpl( &options_.e_block_size, &options_.f_block_size); eliminator_.reset(CHECK_NOTNULL(SchurEliminatorBase::Create(options_))); - eliminator_->Init(options_.elimination_groups[0], A->block_structure()); + const bool kFullRankETE = true; + eliminator_->Init( + options_.elimination_groups[0], kFullRankETE, A->block_structure()); }; std::fill(x, x + A->num_cols(), 0.0); event_logger.AddEvent("Setup"); diff --git a/internal/ceres/schur_eliminator.h b/internal/ceres/schur_eliminator.h index 761b58adc..a5462288d 100644 --- a/internal/ceres/schur_eliminator.h +++ b/internal/ceres/schur_eliminator.h @@ -171,7 +171,14 @@ class SchurEliminatorBase { // CompressedRowBlockStructure object passed to this method is the // same one (or is equivalent to) the one associated with the // BlockSparseMatrix objects below. + // + // assume_full_rank_ete controls how the eliminator inverts with the + // diagonal blocks corresponding to e blocks in A'A. If + // assume_full_rank_ete is true, then a Cholesky factorization is + // used to compute the inverse, otherwise a singular value + // decomposition is used to compute the pseudo inverse. virtual void Init(int num_eliminate_blocks, + bool assume_full_rank_ete, const CompressedRowBlockStructure* bs) = 0; // Compute the Schur complement system from the augmented linear @@ -225,6 +232,7 @@ class SchurEliminator : public SchurEliminatorBase { // SchurEliminatorBase Interface virtual ~SchurEliminator(); virtual void Init(int num_eliminate_blocks, + bool assume_full_rank_ete, const CompressedRowBlockStructure* bs); virtual void Eliminate(const BlockSparseMatrix* A, const double* b, @@ -308,7 +316,9 @@ class SchurEliminator : public SchurEliminatorBase { int row_block_index, BlockRandomAccessMatrix* lhs); + int num_threads_; int num_eliminate_blocks_; + bool assume_full_rank_ete_; // Block layout of the columns of the reduced linear system. Since // the f blocks can be of varying size, this vector stores the @@ -341,7 +351,6 @@ class SchurEliminator : public SchurEliminatorBase { scoped_array chunk_outer_product_buffer_; int buffer_size_; - int num_threads_; int uneliminated_row_begins_; // Locks for the blocks in the right hand side of the reduced linear diff --git a/internal/ceres/schur_eliminator_impl.h b/internal/ceres/schur_eliminator_impl.h index f2535880f..7ee419e72 100644 --- a/internal/ceres/schur_eliminator_impl.h +++ b/internal/ceres/schur_eliminator_impl.h @@ -60,6 +60,7 @@ #include "ceres/internal/eigen.h" #include "ceres/internal/fixed_array.h" #include "ceres/internal/scoped_ptr.h" +#include "ceres/invert_psd_matrix.h" #include "ceres/map_util.h" #include "ceres/schur_eliminator.h" #include "ceres/small_blas.h" @@ -76,14 +77,16 @@ SchurEliminator::~SchurEliminator() { } template -void -SchurEliminator:: -Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) { +void SchurEliminator::Init( + int num_eliminate_blocks, + bool assume_full_rank_ete, + const CompressedRowBlockStructure* bs) { CHECK_GT(num_eliminate_blocks, 0) << "SchurComplementSolver cannot be initialized with " << "num_eliminate_blocks = 0."; num_eliminate_blocks_ = num_eliminate_blocks; + assume_full_rank_ete_ = assume_full_rank_ete; const int num_col_blocks = bs->cols.size(); const int num_row_blocks = bs->rows.size(); @@ -268,10 +271,7 @@ Eliminate(const BlockSparseMatrix* A, // use it to multiply other matrices/vectors instead of doing a // Solve call over and over again. typename EigenTypes::Matrix inverse_ete = - ete - .template selfadjointView() - .llt() - .solve(Matrix::Identity(e_block_size, e_block_size)); + InvertPSDMatrix(assume_full_rank_ete_, ete); // For the current chunk compute and update the rhs of the reduced // linear system. @@ -360,7 +360,8 @@ BackSubstitute(const BlockSparseMatrix* A, ete.data(), 0, 0, e_block_size, e_block_size); } - ete.llt().solveInPlace(y_block); + y_block = InvertPSDMatrix(assume_full_rank_ete_, ete) + * y_block; } } diff --git a/internal/ceres/schur_eliminator_test.cc b/internal/ceres/schur_eliminator_test.cc index 85ecd8f4c..f07d102e5 100644 --- a/internal/ceres/schur_eliminator_test.cc +++ b/internal/ceres/schur_eliminator_test.cc @@ -153,7 +153,8 @@ class SchurEliminatorTest : public ::testing::Test { scoped_ptr eliminator; eliminator.reset(SchurEliminatorBase::Create(options)); - eliminator->Init(num_eliminate_blocks, A->block_structure()); + const bool kFullRankETE = true; + eliminator->Init(num_eliminate_blocks, kFullRankETE, A->block_structure()); eliminator->Eliminate(A.get(), b.get(), diagonal.data(), &lhs, rhs.data()); MatrixRef lhs_ref(lhs.mutable_values(), lhs.num_rows(), lhs.num_cols()); diff --git a/internal/ceres/schur_jacobi_preconditioner.cc b/internal/ceres/schur_jacobi_preconditioner.cc index 3e6cc90f6..c3f43a9e7 100644 --- a/internal/ceres/schur_jacobi_preconditioner.cc +++ b/internal/ceres/schur_jacobi_preconditioner.cc @@ -76,7 +76,9 @@ void SchurJacobiPreconditioner::InitEliminator( eliminator_options.f_block_size = options_.f_block_size; eliminator_options.row_block_size = options_.row_block_size; eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); - eliminator_->Init(eliminator_options.elimination_groups[0], &bs); + const bool kFullRankETE = true; + eliminator_->Init( + eliminator_options.elimination_groups[0], kFullRankETE, &bs); } // Update the values of the preconditioner matrix and factorize it. diff --git a/internal/ceres/visibility_based_preconditioner.cc b/internal/ceres/visibility_based_preconditioner.cc index b0000cdb1..429626af3 100644 --- a/internal/ceres/visibility_based_preconditioner.cc +++ b/internal/ceres/visibility_based_preconditioner.cc @@ -341,7 +341,8 @@ void VisibilityBasedPreconditioner::InitEliminator( eliminator_options.f_block_size = options_.f_block_size; eliminator_options.row_block_size = options_.row_block_size; eliminator_.reset(SchurEliminatorBase::Create(eliminator_options)); - eliminator_->Init(eliminator_options.elimination_groups[0], &bs); + const bool kFullRankETE = true; + eliminator_->Init(eliminator_options.elimination_groups[0], kFullRankETE, &bs); } // Update the values of the preconditioner matrix and factorize it.