Pseudo-inverse of a statically sized matrix in Eigen

pinv.hpp

/// 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();
}