| 44 | /** math with vector/matrix class */ |
| 45 | template <int Dim, typename T> |
| 46 | MatrixT<T, Dim, Dim> SquareRoot(MatrixT<T, Dim, Dim> A) |
| 47 | { |
| 48 | /** compute SVD */ |
| 49 | Eigen::SelfAdjointEigenSolver<MatrixT<T, Dim, Dim>> SAES(A); |
| 50 | |
| 51 | /** compute tolerance (idea from OKVIS) */ |
| 52 | T Tolerance = std::numeric_limits<T>::epsilon() * A.cols() * SAES.eigenvalues().array().maxCoeff(); |
| 53 | |
| 54 | /** set small eigen values to zero */ |
| 55 | VectorT<T, Dim> EigVal = VectorT<T, Dim>((SAES.eigenvalues().array() > Tolerance).select(SAES.eigenvalues().array(), 0)); |
| 56 | |
| 57 | /** use modified eigen values to compute sqrt */ |
| 58 | return SAES.eigenvectors() * EigVal.cwiseSqrt().asDiagonal() * SAES.eigenvectors().transpose(); |
| 59 | } |
| 60 | |
| 61 | template <int Dim, typename T> |
| 62 | MatrixT<T, Dim, Dim> InverseSquareRoot(MatrixT<T, Dim, Dim> A) |
no outgoing calls
no test coverage detected