Speed up InvertPSDMatrix For matrices of size <= 4, Eigen implements an optimized matrix inverse, which is orders of magnitude faster than computing the Cholesky factorization and inverting it. This can have a significant impact on the performance of the SchurEliminator. This change implements this optimization, adds tests and benchmarks. Before this change on my MacBook Pro ----------------------------------------------------------------------------------- Benchmark Time CPU Iterations ----------------------------------------------------------------------------------- BenchmarkFixedSizedInvertPSDMatrix<1> 0 ns 0 ns 1000000000 BenchmarkFixedSizedInvertPSDMatrix<2> 102 ns 101 ns 6504851 BenchmarkFixedSizedInvertPSDMatrix<3> 164 ns 164 ns 4297669 BenchmarkFixedSizedInvertPSDMatrix<4> 200 ns 200 ns 3933623 BenchmarkFixedSizedInvertPSDMatrix<5> 353 ns 353 ns 1930454 BenchmarkFixedSizedInvertPSDMatrix<6> 428 ns 427 ns 1629074 BenchmarkFixedSizedInvertPSDMatrix<7> 559 ns 558 ns 1211639 BenchmarkFixedSizedInvertPSDMatrix<8> 527 ns 527 ns 1000000 BenchmarkFixedSizedInvertPSDMatrix<9> 873 ns 873 ns 902713 BenchmarkFixedSizedInvertPSDMatrix<10> 892 ns 892 ns 787410 BenchmarkFixedSizedInvertPSDMatrix<11> 1201 ns 1201 ns 564334 BenchmarkFixedSizedInvertPSDMatrix<12> 1081 ns 1080 ns 588359 BenchmarkDynamicallyInvertPSDMatrix/1 322 ns 322 ns 2244892 BenchmarkDynamicallyInvertPSDMatrix/2 362 ns 362 ns 1869693 BenchmarkDynamicallyInvertPSDMatrix/3 455 ns 455 ns 1604092 BenchmarkDynamicallyInvertPSDMatrix/4 443 ns 443 ns 1578062 BenchmarkDynamicallyInvertPSDMatrix/5 648 ns 647 ns 963232 BenchmarkDynamicallyInvertPSDMatrix/6 756 ns 755 ns 899766 BenchmarkDynamicallyInvertPSDMatrix/7 906 ns 905 ns 740506 BenchmarkDynamicallyInvertPSDMatrix/8 885 ns 885 ns 790657 BenchmarkDynamicallyInvertPSDMatrix/9 1219 ns 1219 ns 600503 BenchmarkDynamicallyInvertPSDMatrix/10 1267 ns 1266 ns 534555 BenchmarkDynamicallyInvertPSDMatrix/11 1580 ns 1579 ns 469591 BenchmarkDynamicallyInvertPSDMatrix/12 1366 ns 1365 ns 514513 after this change: ----------------------------------------------------------------------------------- Benchmark Time CPU Iterations ----------------------------------------------------------------------------------- BenchmarkFixedSizedInvertPSDMatrix<1> 0 ns 0 ns 1000000000 BenchmarkFixedSizedInvertPSDMatrix<2> 1 ns 1 ns 1000000000 BenchmarkFixedSizedInvertPSDMatrix<3> 1 ns 1 ns 514399512 BenchmarkFixedSizedInvertPSDMatrix<4> 2 ns 2 ns 320587683 BenchmarkFixedSizedInvertPSDMatrix<5> 372 ns 372 ns 1856986 BenchmarkFixedSizedInvertPSDMatrix<6> 446 ns 446 ns 1552502 BenchmarkFixedSizedInvertPSDMatrix<7> 571 ns 570 ns 1208021 BenchmarkFixedSizedInvertPSDMatrix<8> 586 ns 584 ns 1090988 BenchmarkFixedSizedInvertPSDMatrix<9> 1003 ns 1001 ns 753279 BenchmarkFixedSizedInvertPSDMatrix<10> 1074 ns 1070 ns 689974 BenchmarkFixedSizedInvertPSDMatrix<11> 1361 ns 1351 ns 545388 BenchmarkFixedSizedInvertPSDMatrix<12> 1160 ns 1158 ns 615742 BenchmarkDynamicallyInvertPSDMatrix/1 326 ns 326 ns 2206552 BenchmarkDynamicallyInvertPSDMatrix/2 362 ns 361 ns 1820982 BenchmarkDynamicallyInvertPSDMatrix/3 432 ns 431 ns 1696361 BenchmarkDynamicallyInvertPSDMatrix/4 473 ns 472 ns 1294115 BenchmarkDynamicallyInvertPSDMatrix/5 657 ns 656 ns 917888 BenchmarkDynamicallyInvertPSDMatrix/6 804 ns 802 ns 884050 BenchmarkDynamicallyInvertPSDMatrix/7 936 ns 935 ns 679565 BenchmarkDynamicallyInvertPSDMatrix/8 915 ns 915 ns 695548 BenchmarkDynamicallyInvertPSDMatrix/9 1299 ns 1293 ns 583256 BenchmarkDynamicallyInvertPSDMatrix/10 1300 ns 1296 ns 562959 BenchmarkDynamicallyInvertPSDMatrix/11 1617 ns 1610 ns 480393 BenchmarkDynamicallyInvertPSDMatrix/12 1380 ns 1379 ns 503934 Change-Id: Id0d3bbe6d610ee7a3004bb8e2b657b90307b9805

diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt
index 3670aa4..22b5d07 100644
--- a/internal/ceres/CMakeLists.txt
+++ b/internal/ceres/CMakeLists.txt

@@ -511,4 +511,7 @@
 
   add_executable(small_blas_gemm_benchmark small_blas_gemm_benchmark.cc)
   add_dependencies_to_benchmark(small_blas_gemm_benchmark)
+
+  add_executable(invert_psd_matrix_benchmark invert_psd_matrix_benchmark.cc)
+  add_dependencies_to_benchmark(invert_psd_matrix_benchmark)
 endif (BUILD_BENCHMARKS)

diff --git a/internal/ceres/invert_psd_matrix.h b/internal/ceres/invert_psd_matrix.h
index 2319fea..60f6a57 100644
--- a/internal/ceres/invert_psd_matrix.h
+++ b/internal/ceres/invert_psd_matrix.h

@@ -51,16 +51,22 @@
 typename EigenTypes<kSize, kSize>::Matrix InvertPSDMatrix(
     const bool assume_full_rank,
     const typename EigenTypes<kSize, kSize>::Matrix& m) {
+  using MType = typename EigenTypes<kSize, kSize>::Matrix;
   const int size = m.rows();
 
-  // If the matrix can be assumed to be full rank, then just use the
-  // Cholesky factorization to invert it.
+  // If the matrix can be assumed to be full rank, then if its small
+  // (< 5) and fixed size, use Eigen's optimized inverse()
+  // implementation.
+  //
+  // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html#title3
   if (assume_full_rank) {
-    return m.template selfadjointView<Eigen::Upper>().llt().solve(
-        Matrix::Identity(size, size));
+    if (kSize > 0 && kSize < 5) {
+        return m.inverse();
+    }
+    return m.llt().solve(MType::Identity(size, size));
   }
 
-  Eigen::JacobiSVD<Matrix> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::JacobiSVD<MType> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
   const double tolerance =
       std::numeric_limits<double>::epsilon() * size * svd.singularValues()(0);
 

diff --git a/internal/ceres/invert_psd_matrix_benchmark.cc b/internal/ceres/invert_psd_matrix_benchmark.cc
new file mode 100644
index 0000000..5aab6b6
--- /dev/null
+++ b/internal/ceres/invert_psd_matrix_benchmark.cc

@@ -0,0 +1,90 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2019 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 materils 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.
+//
+// Authors: sameeragarwal@google.com (Sameer Agarwal)
+
+#include "Eigen/Dense"
+#include "benchmark/benchmark.h"
+#include "ceres/invert_psd_matrix.h"
+
+namespace ceres {
+namespace internal {
+
+template <int kSize>
+void BenchmarkFixedSizedInvertPSDMatrix(benchmark::State& state) {
+  using MatrixType = typename EigenTypes<kSize, kSize>::Matrix;
+  MatrixType input = MatrixType::Random();
+  input += input.transpose() + MatrixType::Identity();
+
+  MatrixType output;
+  constexpr bool kAssumeFullRank = true;
+  for (auto _ : state) {
+    benchmark::DoNotOptimize(
+        output = InvertPSDMatrix<kSize>(kAssumeFullRank, input));
+  }
+}
+
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 1);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 2);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 3);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 4);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 5);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 6);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 7);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 8);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 9);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 10);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 11);
+BENCHMARK_TEMPLATE(BenchmarkFixedSizedInvertPSDMatrix, 12);
+
+
+void BenchmarkDynamicallyInvertPSDMatrix(benchmark::State& state) {
+  using MatrixType = typename EigenTypes<Eigen::Dynamic, Eigen::Dynamic>::Matrix;
+  const int size = state.range(0);
+  MatrixType input = MatrixType::Random(size, size);
+  input += input.transpose() + MatrixType::Identity(size, size);
+
+  MatrixType output;
+  constexpr bool kAssumeFullRank = true;
+  for (auto _ : state) {
+    benchmark::DoNotOptimize(
+        output = InvertPSDMatrix<Eigen::Dynamic>(kAssumeFullRank, input));
+  }
+}
+
+BENCHMARK(BenchmarkDynamicallyInvertPSDMatrix)
+    ->Apply([](benchmark::internal::Benchmark* benchmark) {
+      for (int i = 1; i < 13; ++i) {
+        benchmark->Args({i});
+      }
+    });
+
+}  // namespace internal
+}  // namespace ceres
+
+BENCHMARK_MAIN();

diff --git a/internal/ceres/invert_psd_matrix_test.cc b/internal/ceres/invert_psd_matrix_test.cc
index 5da9c11..0078e21 100644
--- a/internal/ceres/invert_psd_matrix_test.cc
+++ b/internal/ceres/invert_psd_matrix_test.cc

@@ -42,7 +42,7 @@
 template <int kSize>
 typename EigenTypes<kSize, kSize>::Matrix RandomPSDMatrixWithEigenValues(
     const typename EigenTypes<kSize>::Vector& eigenvalues) {
-  typename EigenTypes<kSize, kSize>::Matrix m;
+  typename EigenTypes<kSize, kSize>::Matrix m(eigenvalues.rows(), eigenvalues.rows());
   m.setRandom();
   Eigen::SelfAdjointEigenSolver<typename EigenTypes<kSize, kSize>::Matrix> es(
       m);
@@ -65,7 +65,7 @@
   const Matrix m = RandomPSDMatrixWithEigenValues<5>(eigenvalues);
   const Matrix inverse_m = InvertPSDMatrix<5>(kFullRank, m);
   EXPECT_NEAR((m * inverse_m - Matrix::Identity(5,5)).norm() / 5.0,  0.0,
-              std::numeric_limits<double>::epsilon());
+              10 * std::numeric_limits<double>::epsilon());
 }
 
 TEST(InvertPSDMatrix, RankDeficient5x5) {
@@ -82,5 +82,29 @@
               10 * std::numeric_limits<double>::epsilon());
 }
 
+TEST(InvertPSDMatrix, DynamicFullRank5x5) {
+  EigenTypes<Eigen::Dynamic>::Vector eigenvalues(5);
+  eigenvalues.setRandom();
+  eigenvalues = eigenvalues.array().abs().matrix();
+  const Matrix m = RandomPSDMatrixWithEigenValues<Eigen::Dynamic>(eigenvalues);
+  const Matrix inverse_m = InvertPSDMatrix<Eigen::Dynamic>(kFullRank, m);
+  EXPECT_NEAR((m * inverse_m - Matrix::Identity(5,5)).norm() / 5.0,  0.0,
+              10 * std::numeric_limits<double>::epsilon());
+}
+
+TEST(InvertPSDMatrix, DynamicRankDeficient5x5) {
+  EigenTypes<Eigen::Dynamic>::Vector eigenvalues(5);
+  eigenvalues.setRandom();
+  eigenvalues = eigenvalues.array().abs().matrix();
+  eigenvalues(3) = 0.0;
+  const Matrix m = RandomPSDMatrixWithEigenValues<Eigen::Dynamic>(eigenvalues);
+  const Matrix inverse_m = InvertPSDMatrix<Eigen::Dynamic>(kRankDeficient, m);
+  Matrix pseudo_identity = Matrix::Identity(5, 5);
+  pseudo_identity(3, 3) = 0.0;
+  EXPECT_NEAR((m * inverse_m * m - m).norm() / m.norm(),
+              0.0,
+              10 * std::numeric_limits<double>::epsilon());
+}
+
 }  // namespace internal
 }  // namespace ceres