commit 0859fe8a57620b6dcc592741127963de1d463bbc
author Sameer Agarwal <sameeragarwal@google.com> Sun Apr 09 00:45:12 2017 -0700
committer Sameer Agarwal <sameeragarwal@google.com> Tue Apr 25 10:08:45 2017 -0700
tree dc563544452820faf7afa396d60c920f2cb8bc56
parent aaae5c5f62d112a5eb37610d9aa92cf426ae6890

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

internal/ceres/schur_eliminator_impl.h

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;
   }
 }