Fix invert PSD matrix.

This CL fixes and simplifies the implementation of InvertPSDMatrix.
InvertPSDMatrix uses a JacobiSVD with thin U and V. This requires a
variable number of columns, which is not always the case, which
triggers an assert in debug mode. The type of the matrix is changed
to always use a dynamic number of columns.

In addition, instead of calculating the inverse of the matrix by hand,
the SVD solve method is used.

Change-Id: I353c741cf537b58eb5b18663902318babe1a66de

internal/ceres/invert_psd_matrix.h

diff --git a/internal/ceres/invert_psd_matrix.h b/internal/ceres/invert_psd_matrix.h
index 2a61c60..21d301a 100644
--- a/internal/ceres/invert_psd_matrix.h
+++ b/internal/ceres/invert_psd_matrix.h
@@ -67,16 +67,10 @@
         MType::Identity(size, size));
   }
 
-  Eigen::JacobiSVD<MType> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  const double tolerance =
-      std::numeric_limits<double>::epsilon() * size * svd.singularValues()(0);
-
-  return svd.matrixV() *
-         (svd.singularValues().array() > tolerance)
-             .select(svd.singularValues().array().inverse(), 0)
-             .matrix()
-             .asDiagonal() *
-         svd.matrixU().adjoint();
+  // For a thin SVD the number of columns of the matrix need to be dynamic.
+  using SVDMType = typename EigenTypes<kSize, Eigen::Dynamic>::Matrix;
+  Eigen::JacobiSVD<SVDMType> svd(m, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  return svd.solve(MType::Identity(size, size));
 }
 
 }  // namespace internal