Created
August 14, 2024 14:48
-
-
Save pavel-kirienko/1f8e2150081cc0b15c95e133c04df06e to your computer and use it in GitHub Desktop.
Pseudo-inverse of a statically sized matrix in Eigen
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| /// Pseudo-inverse of a matrix. There may be a more efficient solution for square matrices; this is a general one. | |
| /// Tolerance less than epsilon*10 may cause numerical instability. | |
| template <auto R, auto C, typename S> | |
| [[nodiscard]] Eigen::Matrix<S, C, R> pinv(const Eigen::Matrix<S, R, C>& a, const S tolerance = std::numeric_limits<S>::epsilon() * 10) | |
| { | |
| const Eigen::JacobiSVD<Eigen::Matrix<S, R, C>> svd(a, Eigen::ComputeFullU | Eigen::ComputeFullV); | |
| Eigen::Matrix<S, C, R> si; | |
| si.setZero(); | |
| for (int i = 0; i < svd.singularValues().size(); ++i) | |
| { | |
| if (svd.singularValues()(i) > tolerance) | |
| { | |
| assert((i < R) && (i < C)); | |
| si(i, i) = static_cast<S>(1) / svd.singularValues()(i); | |
| } | |
| } | |
| return svd.matrixV() * si * svd.matrixU().transpose(); | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment