19 const Eigen::Ref<const Eigen::VectorXd>&
input,
double threshold)
21 const Eigen::MatrixXd normal = jacobian * jacobian.transpose();
22 const Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> solver(normal);
23 const Eigen::VectorXd& eigenvalues = solver.eigenvalues();
24 const Eigen::MatrixXd& eigenvectors = solver.eigenvectors();
25 const double singular_max = std::sqrt(std::max(0.0, eigenvalues.maxCoeff()));
26 const double absolute_threshold = threshold * singular_max;
27 Eigen::VectorXd projected = eigenvectors.transpose() *
input;
28 for (Eigen::Index index = 0; index < eigenvalues.size(); ++index)
30 const double singular = std::sqrt(std::max(0.0, eigenvalues(index)));
31 projected(index) = singular > absolute_threshold ? projected(index) / eigenvalues(index) : 0.0;
33 return jacobian.transpose() * (eigenvectors * projected);