9void pinv(
const Eigen::MatrixXd& matrix_in, Eigen::MatrixXd& pseudo_inv,
10 const double& pinvtoler) {
11 Eigen::JacobiSVD<Eigen::MatrixXd> svd(
12 matrix_in, Eigen::ComputeThinU | Eigen::ComputeThinV);
13 Eigen::VectorXd singular_values;
14 Eigen::VectorXd singular_values_inv;
15 singular_values = svd.singularValues();
16 singular_values_inv.setZero(singular_values.size());
18 for (
int w = 0; w < singular_values.size(); ++w)
19 if (singular_values(w) > pinvtoler)
20 singular_values_inv(w) = 1 / singular_values(w);
21 pseudo_inv = svd.matrixV() * singular_values_inv.asDiagonal() *
22 svd.matrixU().transpose();