Add test for new cutoff threshold

test/pseudoInverse.cpp

@@ -133,6 +133,25 @@ int main()
     fullRankCholeskyTolerance(double_scale);
     TEST(static_cast<double>(float_scale) > double_scale);
 
+    // Real-world test case
+    const float real_alloc[5][6] = {{ 0.794079,  0.794079,  0.794079,  0.794079,  0.0000,  0.0000},
+                                    { 0.607814,  0.607814,  0.607814,  0.607814,  1.0000,  1.0000},
+                                    {-0.672516,  0.915642, -0.915642,  0.672516,  0.0000,  0.0000},
+                                    { 0.159704,  0.159704,  0.159704,  0.159704, -0.2500, -0.2500},
+                                    { 0.607814, -0.607814,  0.607814, -0.607814,  1.0000,  1.0000}};
+    Matrix<float, 5, 6> real ( real_alloc);
+    Matrix<float, 6, 5> real_pinv = geninv(real);
+
+    // from SVD-based inverse
+    const float real_pinv_expected_alloc[6][5] = {{ 2.096205,  -2.722267,   2.056547,   1.503279,   3.098087},
+                                                  { 1.612621,  -1.992694,   2.056547,   1.131090,   2.275467},
+                                                  {-1.062688,   2.043479,  -2.056547,  -0.927950,  -2.275467},
+                                                  {-1.546273,   2.773052,  -2.056547,  -1.300139,  -3.098087},
+                                                  {-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000},
+                                                  {-0.293930,   0.443445,   0.000000,  -0.226222,   0.000000}};
+    Matrix<float, 6, 5> real_pinv_expected(real_pinv_expected_alloc);
+    TEST((real_pinv - real_pinv_expected).abs().max() < 1e-4);
+
     return 0;
 }