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
diff --git a/internal/ceres/implicit_schur_complement_test.cc b/internal/ceres/implicit_schur_complement_test.cc
index e586ea1..21401f7 100644
--- a/internal/ceres/implicit_schur_complement_test.cc
+++ b/internal/ceres/implicit_schur_complement_test.cc
@@ -89,7 +89,8 @@
scoped_ptr<SchurEliminatorBase> 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 6544983..81e0d46 100644
--- a/internal/ceres/schur_complement_solver.cc
+++ b/internal/ceres/schur_complement_solver.cc
@@ -135,7 +135,9 @@
&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 761b58a..a546228 100644
--- a/internal/ceres/schur_eliminator.h
+++ b/internal/ceres/schur_eliminator.h
@@ -171,7 +171,14 @@
// 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 @@
// 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 @@
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 @@
scoped_array<double> 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 f253588..7ee419e 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 @@
}
template <int kRowBlockSize, int kEBlockSize, int kFBlockSize>
-void
-SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::
-Init(int num_eliminate_blocks, const CompressedRowBlockStructure* bs) {
+void SchurEliminator<kRowBlockSize, kEBlockSize, kFBlockSize>::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 @@
// use it to multiply other matrices/vectors instead of doing a
// Solve call over and over again.
typename EigenTypes<kEBlockSize, kEBlockSize>::Matrix inverse_ete =
- ete
- .template selfadjointView<Eigen::Upper>()
- .llt()
- .solve(Matrix::Identity(e_block_size, e_block_size));
+ InvertPSDMatrix<kEBlockSize>(assume_full_rank_ete_, ete);
// For the current chunk compute and update the rhs of the reduced
// linear system.
@@ -360,7 +360,8 @@
ete.data(), 0, 0, e_block_size, e_block_size);
}
- ete.llt().solveInPlace(y_block);
+ y_block = InvertPSDMatrix<kEBlockSize>(assume_full_rank_ete_, ete)
+ * y_block;
}
}
diff --git a/internal/ceres/schur_eliminator_test.cc b/internal/ceres/schur_eliminator_test.cc
index 85ecd8f..f07d102 100644
--- a/internal/ceres/schur_eliminator_test.cc
+++ b/internal/ceres/schur_eliminator_test.cc
@@ -153,7 +153,8 @@
scoped_ptr<SchurEliminatorBase> 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 3e6cc90..c3f43a9 100644
--- a/internal/ceres/schur_jacobi_preconditioner.cc
+++ b/internal/ceres/schur_jacobi_preconditioner.cc
@@ -76,7 +76,9 @@
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 b0000cd..429626a 100644
--- a/internal/ceres/visibility_based_preconditioner.cc
+++ b/internal/ceres/visibility_based_preconditioner.cc
@@ -341,7 +341,8 @@
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.
