diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index 3b16beb51..92c9a525e 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -642,29 +642,7 @@ Matrix cholesky_inverse(const Matrix &A) // return BNU::prod(trans(inv), inv); } -#if 0 /* ************************************************************************* */ -// TODO, would be faster with Cholesky -Matrix inverse_square_root(const Matrix& A) { - size_t m = A.cols(), n = A.rows(); - if (m!=n) - throw invalid_argument("inverse_square_root: A must be square"); - - // Perform SVD, TODO: symmetric SVD? - Matrix U,V; - Vector S; - svd(A,U,S,V); - - // invert and sqrt diagonal of S - // We also arbitrarily choose sign to make result have positive signs - for(size_t i = 0; i inline Matrix3 skewSymmetric(const Eigen::MatrixBase& w) { return skewSymmetric(w(0),w(1),w(2));} -/** Use SVD to calculate inverse square root of a matrix */ +/** Use Cholesky to calculate inverse square root of a matrix */ GTSAM_EXPORT Matrix inverse_square_root(const Matrix& A); /** Calculate the LL^t decomposition of a S.P.D matrix */