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