MRPT  1.9.9
matrix_ops2_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 // Note: Matrices unit tests have been split in different files since
11 // building them with eigen3 eats a lot of RAM and may be a problem while
12 // compiling in small systems.
13 
16 #include <mrpt/math/ops_matrices.h>
17 //#include <mrpt/math/utils.h>
18 #include <mrpt/math/geometry.h>
19 #include <mrpt/random.h>
20 #include <gtest/gtest.h>
21 
22 using namespace mrpt;
23 using namespace mrpt::math;
24 using namespace mrpt::random;
25 using namespace std;
26 
27 #define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;
28 
29 TEST(Matrices, inv_4x4_fix)
30 {
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,
37  0.261535721431038};
39  CMatrixDouble44 C = A.inv();
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,
46  0.158964059763645};
47  CMatrixDouble44 AInv(dat_AInv);
49  (AInv - C).array().abs().sum() > 1e-4, "Error in inv, 4x4 fix")
50 }
51 
52 TEST(Matrices, inv_6x6_fix)
53 {
54  const double dat_A[] = {
55  363.769989013671875, 0.000000000000000, 316.429992675781250,
56  0.000000000000000, 87.266998291015625, 0.000000000000000,
57  101.540000915527344, 0.000000000000000, 478.709991455078125,
58  0.000000000000000, 504.540008544921875, 0.000000000000000,
59  1.000000000000000, 0.000000000000000, 1.000000000000000,
60  0.000000000000000, 1.000000000000000, 0.000000000000000,
61  0.000000000000000, 363.769989013671875, 0.000000000000000,
62  316.429992675781250, 0.000000000000000, 87.266998291015625,
63  0.000000000000000, 101.540000915527344, 0.000000000000000,
64  478.709991455078125, 0.000000000000000, 504.540008544921875,
65  0.000000000000000, 1.000000000000000, 0.000000000000000,
66  1.000000000000000, 0.000000000000000, 1.000000000000000};
69  A.inv(C);
70  const double dat_AInv[] = {
71  -0.000303131460181, -0.002689371550382, 1.383348917627708,
72  0.000000000000000, 0.000000000000000, 0.000000000000000,
73  0.000000000000000, 0.000000000000000, 0.000000000000000,
74  -0.000303131460181, -0.002689371550382, 1.383348917627708,
75  0.004729457992255, 0.003244936115630, -2.049925698035195,
76  0.000000000000000, 0.000000000000000, 0.000000000000000,
77  0.000000000000000, 0.000000000000000, 0.000000000000000,
78  0.004729457992255, 0.003244936115630, -2.049925698035195,
79  -0.004426326532074, -0.000555564565248, 1.666576780407488,
80  0.000000000000000, 0.000000000000000, 0.000000000000000,
81  0.000000000000000, 0.000000000000000, 0.000000000000000,
82  -0.004426326532074, -0.000555564565248, 1.666576780407488};
83  CMatrixDouble66 AInv(dat_AInv);
85  std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
86  (AInv - C).array().abs().sum() > 1e-4,
87  "Error in inv, 6x6 fix")
88 }
89 
90 TEST(Matrices, inv_6x6_dyn)
91 {
92  const double dat_A[] = {
93  363.769989013671875, 0.000000000000000, 316.429992675781250,
94  0.000000000000000, 87.266998291015625, 0.000000000000000,
95  101.540000915527344, 0.000000000000000, 478.709991455078125,
96  0.000000000000000, 504.540008544921875, 0.000000000000000,
97  1.000000000000000, 0.000000000000000, 1.000000000000000,
98  0.000000000000000, 1.000000000000000, 0.000000000000000,
99  0.000000000000000, 363.769989013671875, 0.000000000000000,
100  316.429992675781250, 0.000000000000000, 87.266998291015625,
101  0.000000000000000, 101.540000915527344, 0.000000000000000,
102  478.709991455078125, 0.000000000000000, 504.540008544921875,
103  0.000000000000000, 1.000000000000000, 0.000000000000000,
104  1.000000000000000, 0.000000000000000, 1.000000000000000};
105  CMatrixDouble A(6, 6, dat_A);
106  CMatrixDouble C = A.inv();
107  const double dat_AInv[] = {
108  -0.000303131460181, -0.002689371550382, 1.383348917627708,
109  0.000000000000000, 0.000000000000000, 0.000000000000000,
110  0.000000000000000, 0.000000000000000, 0.000000000000000,
111  -0.000303131460181, -0.002689371550382, 1.383348917627708,
112  0.004729457992255, 0.003244936115630, -2.049925698035195,
113  0.000000000000000, 0.000000000000000, 0.000000000000000,
114  0.000000000000000, 0.000000000000000, 0.000000000000000,
115  0.004729457992255, 0.003244936115630, -2.049925698035195,
116  -0.004426326532074, -0.000555564565248, 1.666576780407488,
117  0.000000000000000, 0.000000000000000, 0.000000000000000,
118  0.000000000000000, 0.000000000000000, 0.000000000000000,
119  -0.004426326532074, -0.000555564565248, 1.666576780407488};
120  CMatrixDouble AInv(6, 6, dat_AInv);
122  std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
123  (AInv - C).array().abs().sum() > 1e-4,
124  "Error in inv, 6x6 dyn")
125 }
126 
127 TEST(Matrices, transpose)
128 {
129  const double dat_A[] = {1, 2, 3, 4, 5, 6};
130  const double dat_At[] = {1, 4, 2, 5, 3, 6};
131  const CMatrixDouble A(2, 3, dat_A);
132  const CMatrixDouble At(3, 2, dat_At);
133 
134  EXPECT_EQ(A.t(), At);
135  EXPECT_EQ(~A, At);
136  EXPECT_EQ(A.t().t(), A);
137 }
138 
140 {
141  {
142  const double dat_A[] = {1, 2, 3, 4, 5, 6};
143  const CMatrixDouble A(2, 3, dat_A);
144  const std::vector<double> v{1.0, 2.0, 3.0};
146 
148  R.multiply_A_skew3(A, v);
149  EXPECT_EQ(R, (A * S).eval());
150  }
151  {
152  const double dat_A[] = {1, 2, 3, 4, 5, 6};
153  const double dat_v[] = {1, 2, 3};
155  const CArrayDouble<3> v(dat_v);
158 
160  R.multiply_A_skew3(A, v);
161  EXPECT_TRUE(R == A * S);
162  }
163 }
164 
166 {
167  {
168  const double dat_A[] = {1, 2, 3, 4, 5, 6};
169  const CMatrixDouble A(3, 2, dat_A);
170  const std::vector<double> v{1.0, 2.0, 3.0};
172 
174  R.multiply_skew3_A(v, A);
175  EXPECT_TRUE(R == S * A);
176  }
177  {
178  const double dat_A[] = {1, 2, 3, 4, 5, 6};
179  const double dat_v[] = {1, 2, 3};
181  const CArrayDouble<3> v(dat_v);
184 
186  R.multiply_skew3_A(v, A);
187  EXPECT_TRUE(R == S * A);
188  }
189 }
190 
192 {
193  const char* mat1 = "[1 2 3;-3 -6 -5]";
194  const double vals1[] = {1, 2, 3, -3, -6, -5};
195 
196  const char* mat2 =
197  " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] "; // With tabs and
198  // spaces...
199  const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000, 7};
200 
201  const char* mat3 = "[9]";
202  const char* mat4 =
203  "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]"; // An invalid matrix
204  const char* mat5 = "[ ]"; // Empty
205  const char* mat6 = "[ -405.200 42.232 ; 1219.600 -98.696 ]"; // M1 * M2
206 
207  const char* mat13 = "[9 8 7]";
208  const char* mat31 = "[9; 8; 7]";
209 
210  CMatrixDouble M1, M2, M3, M4, M5, M6;
211 
212  if (!M1.fromMatlabStringFormat(mat1) ||
213  (CMatrixFixedNumeric<double, 2, 3>(vals1) - M1).array().abs().sum() >
214  1e-4)
215  GTEST_FAIL() << mat1;
216 
217  {
219  if (!M1b.fromMatlabStringFormat(mat1) ||
220  (CMatrixFixedNumeric<double, 2, 3>(vals1) - M1b)
221  .array()
222  .abs()
223  .sum() > 1e-4)
224  GTEST_FAIL() << mat1;
225  }
226 
227  if (!M2.fromMatlabStringFormat(mat2) || M2.cols() != 2 || M2.rows() != 3 ||
228  (CMatrixFixedNumeric<double, 3, 2>(vals2) - M2).array().abs().sum() >
229  1e-4)
230  GTEST_FAIL() << mat2;
231 
232  {
234  if (!M2b.fromMatlabStringFormat(mat2) ||
235  (CMatrixFixedNumeric<double, 3, 2>(vals2) - M2b)
236  .array()
237  .abs()
238  .sum() > 1e-4)
239  GTEST_FAIL() << mat2;
240  }
241 
242  if (!M3.fromMatlabStringFormat(mat3)) GTEST_FAIL() << mat3;
243 
244  {
245  CVectorDouble m;
246  if (!m.fromMatlabStringFormat(mat3) || m.size() != 1)
247  GTEST_FAIL() << "CVectorDouble:" << mat3;
248  }
249  {
250  CArrayDouble<1> m;
251  if (!m.fromMatlabStringFormat(mat3))
252  GTEST_FAIL() << "CArrayDouble<1>:" << mat3;
253  }
254 
255  {
256  CVectorDouble m;
257  if (!m.fromMatlabStringFormat(mat31) || m.size() != 3)
258  GTEST_FAIL() << "CVectorDouble:" << mat31;
259  }
260  {
261  CArrayDouble<3> m;
262  if (!m.fromMatlabStringFormat(mat31))
263  GTEST_FAIL() << "CArrayDouble<3>:" << mat31;
264  }
265 
266  {
267  Eigen::Matrix<double, 1, 3> m;
268  if (!m.fromMatlabStringFormat(mat13))
269  GTEST_FAIL() << "Matrix<double,1,3>:" << mat13;
270  }
271  {
272  Eigen::Matrix<double, 1, Eigen::Dynamic> m;
273  if (!m.fromMatlabStringFormat(mat13) || m.size() != 3)
274  GTEST_FAIL() << "Matrix<double,1,Dynamic>:" << mat13;
275  }
276 
277  // This one MUST BE detected as WRONG:
278  if (M4.fromMatlabStringFormat(mat4, nullptr /*dont dump errors to cerr*/))
279  GTEST_FAIL() << mat4;
280 
281  if (!M5.fromMatlabStringFormat(mat5) || M5.rows() != 0 || M5.cols() != 0)
282  GTEST_FAIL() << mat5;
283 
284  if (!M6.fromMatlabStringFormat(mat6)) GTEST_FAIL() << mat6;
285 
286  // Check correct values loaded:
287  CMatrixDouble RES = M1 * M2;
288 
289  EXPECT_NEAR(0, (M6 - M1 * M2).array().square().sum(), 1e-3);
290 }
A namespace of pseudo-random numbers generators of diferent distributions.
const double dat_A[]
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
Definition: geometry.h:843
TEST(Matrices, inv_4x4_fix)
void multiply_A_skew3(const MAT_A &A, const SKEW_3VECTOR &v, MAT_OUT &out)
Only for vectors/arrays "v" of length3, compute out = A * Skew(v), where Skew(v) is the skew symmetri...
Definition: ops_matrices.h:166
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually...
Definition: CArrayNumeric.h:25
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
STL namespace.
T square(const T x)
Inline function for the square of a number.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
bool fromMatlabStringFormat(const std::string &s, std::ostream *dump_errors_here=nullptr)
Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]" The string must star...
GLsizei GLboolean transpose
Definition: glext.h:4133
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)
void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A, MAT_OUT &out)
Only for vectors/arrays "v" of length3, compute out = Skew(v) * A, where Skew(v) is the skew symmetri...
Definition: ops_matrices.h:190



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020