From 0859fe8a57620b6dcc592741127963de1d463bbc Mon Sep 17 00:00:00 2001 From: Sameer Agarwal Date: Sun, 9 Apr 2017 00:45:12 -0700 Subject: [PATCH] Integrate InvertPSDMatrix into the SchurEliminator. SchurEliminator::Init now takes a bool that tells it whether it can assume that the diagonal blocks it is inverting can be assumed to be full rank or not. This information is then passed onto InvertPSDMatrix. Change-Id: I26037b6233f2aad5584fed245f631c3959928afe --- .../ceres/implicit_schur_complement_test.cc | 3 ++- internal/ceres/schur_complement_solver.cc | 4 +++- internal/ceres/schur_eliminator.h | 11 ++++++++++- internal/ceres/schur_eliminator_impl.h | 17 +++++++++-------- internal/ceres/schur_eliminator_test.cc | 3 ++- internal/ceres/schur_jacobi_preconditioner.cc | 4 +++- .../ceres/visibility_based_preconditioner.cc | 3 ++- 7 files changed, 31 insertions(+), 14 deletions(-) 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.