pseudoInverse.cpp
5.07 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
#include "test_macros.hpp"
#include <matrix/PseudoInverse.hpp>
using namespace matrix;
static const size_t n_large = 20;
int main()
{
// 3x4 Matrix test
float data0[12] = {
0.f, 1.f, 2.f, 3.f,
4.f, 5.f, 6.f, 7.f,
8.f, 9.f, 10.f, 11.f
};
float data0_check[12] = {
-0.3375f, -0.1f, 0.1375f,
-0.13333333f, -0.03333333f, 0.06666667f,
0.07083333f, 0.03333333f, -0.00416667f,
0.275f, 0.1f, -0.075f
};
Matrix<float, 3, 4> A0(data0);
Matrix<float, 4, 3> A0_I = geninv(A0);
Matrix<float, 4, 3> A0_I_check(data0_check);
TEST((A0_I - A0_I_check).abs().max() < 1e-5);
// 4x3 Matrix test
float data1[12] = {
0.f, 4.f, 8.f,
1.f, 5.f, 9.f,
2.f, 6.f, 10.f,
3.f, 7.f, 11.f
};
float data1_check[12] = {
-0.3375f, -0.13333333f, 0.07083333f, 0.275f,
-0.1f, -0.03333333f, 0.03333333f, 0.1f,
0.1375f, 0.06666667f, -0.00416667f, -0.075f
};
Matrix<float, 4, 3> A1(data1);
Matrix<float, 3, 4> A1_I = geninv(A1);
Matrix<float, 3, 4> A1_I_check(data1_check);
TEST((A1_I - A1_I_check).abs().max() < 1e-5);
// Stess test
Matrix<float, n_large, n_large - 1> A_large;
A_large.setIdentity();
Matrix<float, n_large - 1, n_large> A_large_I;
for (size_t i = 0; i < n_large; i++) {
A_large_I = geninv(A_large);
TEST(isEqual(A_large, A_large_I.T()));
}
// Square matrix test
float data2[9] = {0, 2, 3,
4, 5, 6,
7, 8, 10
};
float data2_check[9] = {
-0.4f, -0.8f, 0.6f,
-0.4f, 4.2f, -2.4f,
0.6f, -2.8f, 1.6f
};
SquareMatrix<float, 3> A2(data2);
SquareMatrix<float, 3> A2_I = geninv(A2);
SquareMatrix<float, 3> A2_I_check(data2_check);
TEST((A2_I - A2_I_check).abs().max() < 1e-3);
// Null matrix test
Matrix<float, 6, 16> A3;
Matrix<float, 16, 6> A3_I = geninv(A3);
Matrix<float, 16, 6> A3_I_check;
TEST((A3_I - A3_I_check).abs().max() < 1e-5);
// Mock-up effectiveness matrix
const float B_quad_w[6][16] = {
{-0.5717536f, 0.43756646f, 0.5717536f, -0.43756646f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.35355328f, -0.35355328f, 0.35355328f, -0.35355328f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.28323701f, 0.28323701f, -0.28323701f, -0.28323701f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{ 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f},
{-0.25f, -0.25f, -0.25f, -0.25f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f}
};
Matrix<float, 6, 16> B = Matrix<float, 6, 16>(B_quad_w);
const float A_quad_w[16][6] = {
{ -0.495383f, 0.707107f, 0.765306f, 0.0f, 0.0f, -1.000000f },
{ 0.495383f, -0.707107f, 1.000000f, 0.0f, 0.0f, -1.000000f },
{ 0.495383f, 0.707107f, -0.765306f, 0.0f, 0.0f, -1.000000f },
{ -0.495383f, -0.707107f, -1.000000f, 0.0f, 0.0f, -1.000000f },
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}
};
Matrix<float, 16, 6> A_check = Matrix<float, 16, 6>(A_quad_w);
Matrix<float, 16, 6> A = geninv(B);
TEST((A - A_check).abs().max() < 1e-5);
// 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;
}
/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */