| 25 | namespace libRSF |
| 26 | { |
| 27 | void RobustSqrtAndInvSqrt(const Matrix &Mat, Matrix &MatSqrt, Matrix & MatSqrtInv) |
| 28 | { |
| 29 | /** compute SVD */ |
| 30 | Eigen::SelfAdjointEigenSolver<Matrix> SAES(Mat); |
| 31 | |
| 32 | /** compute tolerance (idea from OKVIS) */ |
| 33 | double Tolerance = std::numeric_limits<double>::epsilon() * Mat.cols() * SAES.eigenvalues().array().maxCoeff(); |
| 34 | |
| 35 | /** set small eigen values to zero */ |
| 36 | Vector EigVal = Vector((SAES.eigenvalues().array() > Tolerance).select(SAES.eigenvalues().array(), 0)); |
| 37 | Vector EigValInv = Vector((SAES.eigenvalues().array() > Tolerance).select(SAES.eigenvalues().array().inverse(), 0)); |
| 38 | |
| 39 | /** use modified eigen values to compute sqrt */ |
| 40 | MatSqrt = SAES.eigenvectors() * EigVal.cwiseSqrt().asDiagonal() * SAES.eigenvectors().transpose(); |
| 41 | MatSqrtInv = SAES.eigenvectors() * EigValInv.cwiseSqrt().asDiagonal() * SAES.eigenvectors().transpose(); |
| 42 | } |
| 43 | |
| 44 | void RemoveColumn(Matrix& Matrix, int ColToRemove) |
| 45 | { |