14 #include <gtest/gtest.h>
20 #include <Eigen/Dense>
27 #define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;
29 TEST(Matrices, inv_4x4_fix)
31 const double dat_A[] = {
32 -0.710681653571291, 0.734469323344333, -0.656414638791893,
33 0.818771495864303, 1.044946492154568, 1.163592359608108,
34 -1.069421407670914, 0.307916381104872, 0.185595851677470,
35 0.116899590868673, 0.507691343481809, -3.217842384231890,
36 -0.214383515646621, -0.161495561253269, 1.303923696836841,
40 const double dat_AInv[] = {
41 -0.741952742824035, 0.493481687552705, -0.134764164880760,
42 0.083693424291000, 0.638324207063440, 0.519344439204238,
43 0.264483337145361, 0.644307267615193, -0.037800456163779,
44 0.131794126194075, 0.070338431705792, 0.828591793299072,
45 -0.025568212209135, 0.068123300450057, -0.297834184749986,
51 TEST(Matrices, inv_LLt_4x4_fix)
53 const double dat_A[] = {
55 5.340050636150691, -5.233909093073407, -0.716270110219391, 2.459687431594893,
56 -5.233909093073407, 9.825652319628178, 5.408354798920869, -0.568364497163277,
57 -0.716270110219391, 5.408354798920869, 7.221045225242777, 1.571113904458352,
58 2.459687431594893, -0.568364497163277, 1.571113904458352, 2.339742034936232
63 const double dat_AInv[] = {
65 2.1041492047454131, 1.1944136386884630, -0.3135215860855067, -1.7113463206284218,
66 1.1944136386884630, 0.9132606154380595, -0.3988774167033647, -0.7659551448058749,
67 -0.3135215860855067, -0.3988774167033647, 0.4163286875558479, -0.0468609127642805,
68 -1.7113463206284218, -0.7659551448058749, -0.0468609127642805, 2.0718776191839314
75 TEST(Matrices, inv_6x6_fix)
77 const double dat_A[] = {
78 363.769989013671875, 0.000000000000000, 316.429992675781250,
79 0.000000000000000, 87.266998291015625, 0.000000000000000,
80 101.540000915527344, 0.000000000000000, 478.709991455078125,
81 0.000000000000000, 504.540008544921875, 0.000000000000000,
82 1.000000000000000, 0.000000000000000, 1.000000000000000,
83 0.000000000000000, 1.000000000000000, 0.000000000000000,
84 0.000000000000000, 363.769989013671875, 0.000000000000000,
85 316.429992675781250, 0.000000000000000, 87.266998291015625,
86 0.000000000000000, 101.540000915527344, 0.000000000000000,
87 478.709991455078125, 0.000000000000000, 504.540008544921875,
88 0.000000000000000, 1.000000000000000, 0.000000000000000,
89 1.000000000000000, 0.000000000000000, 1.000000000000000};
92 const double dat_AInv[] = {
93 -0.000303131460181, -0.002689371550382, 1.383348917627708,
94 0.000000000000000, 0.000000000000000, 0.000000000000000,
95 0.000000000000000, 0.000000000000000, 0.000000000000000,
96 -0.000303131460181, -0.002689371550382, 1.383348917627708,
97 0.004729457992255, 0.003244936115630, -2.049925698035195,
98 0.000000000000000, 0.000000000000000, 0.000000000000000,
99 0.000000000000000, 0.000000000000000, 0.000000000000000,
100 0.004729457992255, 0.003244936115630, -2.049925698035195,
101 -0.004426326532074, -0.000555564565248, 1.666576780407488,
102 0.000000000000000, 0.000000000000000, 0.000000000000000,
103 0.000000000000000, 0.000000000000000, 0.000000000000000,
104 -0.004426326532074, -0.000555564565248, 1.666576780407488};
107 std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
108 (AInv - C).sum_abs() > 1e-4,
109 "Error in inv, 6x6 fix")
114 const double dat_A[] = {
115 363.769989013671875, 0.000000000000000, 316.429992675781250,
116 0.000000000000000, 87.266998291015625, 0.000000000000000,
117 101.540000915527344, 0.000000000000000, 478.709991455078125,
118 0.000000000000000, 504.540008544921875, 0.000000000000000,
119 1.000000000000000, 0.000000000000000, 1.000000000000000,
120 0.000000000000000, 1.000000000000000, 0.000000000000000,
121 0.000000000000000, 363.769989013671875, 0.000000000000000,
122 316.429992675781250, 0.000000000000000, 87.266998291015625,
123 0.000000000000000, 101.540000915527344, 0.000000000000000,
124 478.709991455078125, 0.000000000000000, 504.540008544921875,
125 0.000000000000000, 1.000000000000000, 0.000000000000000,
126 1.000000000000000, 0.000000000000000, 1.000000000000000};
129 const double dat_AInv[] = {
130 -0.000303131460181, -0.002689371550382, 1.383348917627708,
131 0.000000000000000, 0.000000000000000, 0.000000000000000,
132 0.000000000000000, 0.000000000000000, 0.000000000000000,
133 -0.000303131460181, -0.002689371550382, 1.383348917627708,
134 0.004729457992255, 0.003244936115630, -2.049925698035195,
135 0.000000000000000, 0.000000000000000, 0.000000000000000,
136 0.000000000000000, 0.000000000000000, 0.000000000000000,
137 0.004729457992255, 0.003244936115630, -2.049925698035195,
138 -0.004426326532074, -0.000555564565248, 1.666576780407488,
139 0.000000000000000, 0.000000000000000, 0.000000000000000,
140 0.000000000000000, 0.000000000000000, 0.000000000000000,
141 -0.004426326532074, -0.000555564565248, 1.666576780407488};
144 std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
145 (AInv - C).sum_abs() > 1e-4,
146 "Error in inv, 6x6 dyn")
151 const double dat_A[] = {1, 2, 3, 4, 5, 6};
152 const double dat_At[] = {1, 4, 2, 5, 3, 6};
163 const double dat_A[] = {1, 2, 3, 4, 5, 6};
165 const std::vector<double> v{1.0, 2.0, 3.0};
173 const double dat_A[] = {1, 2, 3, 4, 5, 6};
174 const double dat_v[] = {1, 2, 3};
188 const double dat_A[] = {1, 2, 3, 4, 5, 6};
190 const std::vector<double> v{1.0, 2.0, 3.0};
198 const double dat_A[] = {1, 2, 3, 4, 5, 6};
199 const double dat_v[] = {1, 2, 3};
210 TEST(Matrices, fromMatlabStringFormat)
212 const char* mat1 =
"[1 2 3;-3 -6 -5]";
213 const double vals1[] = {1, 2, 3, -3, -6, -5};
216 " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] ";
218 const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000, 7};
220 const char* mat3 =
"[9]";
222 "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]";
223 const char* mat5 =
"[ ]";
224 const char* mat6 =
"[ -405.200 42.232 ; 1219.600 -98.696 ]";
226 const char* mat13 =
"[9 8 7]";
227 const char* mat31 =
"[9; 8; 7]";
233 GTEST_FAIL() << mat1;
240 GTEST_FAIL() << mat1;
245 GTEST_FAIL() << mat2;
252 GTEST_FAIL() << mat2;
260 GTEST_FAIL() <<
"CVectorDouble:" << mat3;
265 GTEST_FAIL() <<
"CVectorFixedDouble<1>:" << mat3;
271 GTEST_FAIL() <<
"CVectorDouble:" << mat31;
276 GTEST_FAIL() <<
"CVectorFixedDouble<3>:" << mat31;
282 GTEST_FAIL() <<
"Matrix<double,1,3>:" << mat13;
287 const auto len = m.
size();
288 EXPECT_EQ(m.
rows(), 3) <<
"Matrix<double,1,Dynamic>:" << mat31;
290 EXPECT_EQ(m.
rows(), 3) <<
"Matrix<double,1,Dynamic>:" << mat31;
291 EXPECT_EQ(m.
cols(), 1) <<
"Matrix<double,1,Dynamic>:" << mat31;
292 EXPECT_EQ(len, 3) <<
"Matrix<double,1,Dynamic>:" << mat31;
299 GTEST_FAIL() << mat5;