| 60 | |
| 61 | template <int Dim, typename T> |
| 62 | MatrixT<T, Dim, Dim> InverseSquareRoot(MatrixT<T, Dim, Dim> A) |
| 63 | { |
| 64 | /** compute SVD */ |
| 65 | Eigen::SelfAdjointEigenSolver<MatrixT<T, Dim, Dim>> SAES(A); |
| 66 | |
| 67 | /** compute tolerance (idea from OKVIS) */ |
| 68 | T Tolerance = std::numeric_limits<T>::epsilon() * A.cols() * SAES.eigenvalues().array().maxCoeff(); |
| 69 | |
| 70 | /** set small eigen values to zero */ |
| 71 | VectorT<T, Dim> EigValInv = Vector((SAES.eigenvalues().array() > Tolerance).select(SAES.eigenvalues().array().inverse(), 0)); |
| 72 | |
| 73 | /** use modified eigen values to compute inverse sqrt */ |
| 74 | return SAES.eigenvectors() * EigValInv.cwiseSqrt().asDiagonal() * SAES.eigenvectors().transpose(); |
| 75 | } |
| 76 | |
| 77 | template <int Dim, typename T> |
| 78 | MatrixT<T, Dim, Dim> Inverse(MatrixT<T, Dim, Dim> A) |
no outgoing calls
no test coverage detected