5#include <gtest/gtest.h>
13Eigen::MatrixXd matrixWithSingularValues(
const std::vector<double>& values, Eigen::Index columns)
15 const Eigen::Index rows = values.size();
16 Eigen::MatrixXd left(rows, rows);
17 Eigen::MatrixXd right(columns, rows);
18 for (Eigen::Index row = 0; row < rows; ++row)
20 for (Eigen::Index column = 0; column < rows; ++column)
21 left(row, column) = std::sin((row + 1) * (column + 2));
22 for (Eigen::Index column = 0; column < columns; ++column)
23 right(column, row) = std::cos((row + 2) * (column + 1));
25 const Eigen::MatrixXd u = left.householderQr().householderQ() * Eigen::MatrixXd::Identity(rows, rows);
26 const Eigen::MatrixXd v = right.householderQr().householderQ() * Eigen::MatrixXd::Identity(columns, rows);
27 return u * Eigen::Map<const Eigen::VectorXd>(values.data(), rows).asDiagonal() * v.transpose();
30void expectSameSolution(
const std::vector<double>& singular_values)
32 constexpr double threshold = 0.001;
33 constexpr Eigen::Index columns = 7;
34 const Eigen::MatrixXd jacobian = matrixWithSingularValues(singular_values, columns);
35 const Eigen::VectorXd
input = Eigen::VectorXd::LinSpaced(jacobian.rows(), -0.7, 0.9);
36 Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian.rows(), jacobian.cols(), Eigen::ComputeThinU | Eigen::ComputeThinV);
37 svd.setThreshold(threshold);
38 svd.compute(jacobian);
39 const Eigen::VectorXd expected = svd.solve(
input);
40 Eigen::MatrixXd jacobian_storage = Eigen::MatrixXd::Zero(jacobian.rows() + 1, jacobian.cols());
41 Eigen::VectorXd input_storage = Eigen::VectorXd::Zero(
input.size() + 1);
42 jacobian_storage.topRows(jacobian.rows()) = jacobian;
45 jacobian_storage.topRows(jacobian.rows()), input_storage.topRows(
input.size()), threshold);
46 EXPECT_LT((expected - actual).norm(), 1e-8 * std::max(1.0, expected.norm()));
47 EXPECT_NEAR((jacobian * expected -
input).norm(), (jacobian * actual -
input).norm(), 1e-9);
51TEST(Pseudoinverse, FullRank)
53 expectSameSolution({ 1.0, 0.7, 0.4, 0.2, 0.1, 0.02 });
54 expectSameSolution({ 1.0, 0.3, 0.02 });
57TEST(Pseudoinverse, RankDeficient)
59 expectSameSolution({ 1.0, 0.4, 0.1, 0.0, 0.0, 0.0 });
62TEST(Pseudoinverse, ThresholdBoundary)
64 expectSameSolution({ 1.0, 0.4, 0.1, 0.01, 0.001001, 0.0005 });
65 expectSameSolution({ 1.0, 0.4, 0.1, 0.01, 0.000999, 0.0005 });
68TEST(Pseudoinverse, IllConditioned)
70 expectSameSolution({ 1.0, 0.01, 0.0001, 1e-6, 1e-8, 0.0 });
Eigen::VectorXd solvePseudoinverse(const Eigen::Ref< const Eigen::MatrixXd > &jacobian, const Eigen::Ref< const Eigen::VectorXd > &input, double threshold)
TEST(Pseudoinverse, FullRank)