Main MRPT website > C++ reference for 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-2017, 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 
14 #include <mrpt/math/CMatrixFixedNumeric.h>
15 #include <mrpt/math/ops_matrices.h>
16 #include <mrpt/math/utils.h>
17 #include <mrpt/math/geometry.h>
18 #include <mrpt/utils/metaprogramming.h>
19 #include <mrpt/utils/CMemoryStream.h>
20 #include <mrpt/random.h>
21 #include <gtest/gtest.h>
22 
23 using namespace mrpt;
24 using namespace mrpt::utils;
25 using namespace mrpt::math;
26 using namespace mrpt::random;
27 using namespace mrpt::utils::metaprogramming;
28 using namespace std;
29 
30 #define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;
31 
32 TEST(Matrices, inv_4x4_fix)
33 {
34  const double dat_A[] = {
35  -0.710681653571291, 0.734469323344333, -0.656414638791893,
36  0.818771495864303, 1.044946492154568, 1.163592359608108,
37  -1.069421407670914, 0.307916381104872, 0.185595851677470,
38  0.116899590868673, 0.507691343481809, -3.217842384231890,
39  -0.214383515646621, -0.161495561253269, 1.303923696836841,
40  0.261535721431038};
41  CMatrixDouble44 A(dat_A);
42  CMatrixDouble44 C = A.inv();
43  const double dat_AInv[] = {
44  -0.741952742824035, 0.493481687552705, -0.134764164880760,
45  0.083693424291000, 0.638324207063440, 0.519344439204238,
46  0.264483337145361, 0.644307267615193, -0.037800456163779,
47  0.131794126194075, 0.070338431705792, 0.828591793299072,
48  -0.025568212209135, 0.068123300450057, -0.297834184749986,
49  0.158964059763645};
50  CMatrixDouble44 AInv(dat_AInv);
51  CHECK_AND_RET_ERROR(
52  (AInv - C).array().abs().sum() > 1e-4, "Error in inv, 4x4 fix")
53 }
54 
55 TEST(Matrices, inv_6x6_fix)
56 {
57  const double dat_A[] = {
58  363.769989013671875, 0.000000000000000, 316.429992675781250,
59  0.000000000000000, 87.266998291015625, 0.000000000000000,
60  101.540000915527344, 0.000000000000000, 478.709991455078125,
61  0.000000000000000, 504.540008544921875, 0.000000000000000,
62  1.000000000000000, 0.000000000000000, 1.000000000000000,
63  0.000000000000000, 1.000000000000000, 0.000000000000000,
64  0.000000000000000, 363.769989013671875, 0.000000000000000,
65  316.429992675781250, 0.000000000000000, 87.266998291015625,
66  0.000000000000000, 101.540000915527344, 0.000000000000000,
67  478.709991455078125, 0.000000000000000, 504.540008544921875,
68  0.000000000000000, 1.000000000000000, 0.000000000000000,
69  1.000000000000000, 0.000000000000000, 1.000000000000000};
70  CMatrixDouble66 A(dat_A);
71  CMatrixDouble66 C;
72  A.inv(C);
73  const double dat_AInv[] = {
74  -0.000303131460181, -0.002689371550382, 1.383348917627708,
75  0.000000000000000, 0.000000000000000, 0.000000000000000,
76  0.000000000000000, 0.000000000000000, 0.000000000000000,
77  -0.000303131460181, -0.002689371550382, 1.383348917627708,
78  0.004729457992255, 0.003244936115630, -2.049925698035195,
79  0.000000000000000, 0.000000000000000, 0.000000000000000,
80  0.000000000000000, 0.000000000000000, 0.000000000000000,
81  0.004729457992255, 0.003244936115630, -2.049925698035195,
82  -0.004426326532074, -0.000555564565248, 1.666576780407488,
83  0.000000000000000, 0.000000000000000, 0.000000000000000,
84  0.000000000000000, 0.000000000000000, 0.000000000000000,
85  -0.004426326532074, -0.000555564565248, 1.666576780407488};
86  CMatrixDouble66 AInv(dat_AInv);
87  CHECK_AND_RET_ERROR(
88  std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
89  (AInv - C).array().abs().sum() > 1e-4,
90  "Error in inv, 6x6 fix")
91 }
92 
93 TEST(Matrices, inv_6x6_dyn)
94 {
95  const double dat_A[] = {
96  363.769989013671875, 0.000000000000000, 316.429992675781250,
97  0.000000000000000, 87.266998291015625, 0.000000000000000,
98  101.540000915527344, 0.000000000000000, 478.709991455078125,
99  0.000000000000000, 504.540008544921875, 0.000000000000000,
100  1.000000000000000, 0.000000000000000, 1.000000000000000,
101  0.000000000000000, 1.000000000000000, 0.000000000000000,
102  0.000000000000000, 363.769989013671875, 0.000000000000000,
103  316.429992675781250, 0.000000000000000, 87.266998291015625,
104  0.000000000000000, 101.540000915527344, 0.000000000000000,
105  478.709991455078125, 0.000000000000000, 504.540008544921875,
106  0.000000000000000, 1.000000000000000, 0.000000000000000,
107  1.000000000000000, 0.000000000000000, 1.000000000000000};
108  CMatrixDouble A(6, 6, dat_A);
109  CMatrixDouble C = A.inv();
110  const double dat_AInv[] = {
111  -0.000303131460181, -0.002689371550382, 1.383348917627708,
112  0.000000000000000, 0.000000000000000, 0.000000000000000,
113  0.000000000000000, 0.000000000000000, 0.000000000000000,
114  -0.000303131460181, -0.002689371550382, 1.383348917627708,
115  0.004729457992255, 0.003244936115630, -2.049925698035195,
116  0.000000000000000, 0.000000000000000, 0.000000000000000,
117  0.000000000000000, 0.000000000000000, 0.000000000000000,
118  0.004729457992255, 0.003244936115630, -2.049925698035195,
119  -0.004426326532074, -0.000555564565248, 1.666576780407488,
120  0.000000000000000, 0.000000000000000, 0.000000000000000,
121  0.000000000000000, 0.000000000000000, 0.000000000000000,
122  -0.004426326532074, -0.000555564565248, 1.666576780407488};
123  CMatrixDouble AInv(6, 6, dat_AInv);
124  CHECK_AND_RET_ERROR(
125  std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
126  (AInv - C).array().abs().sum() > 1e-4,
127  "Error in inv, 6x6 dyn")
128 }
129 
130 TEST(Matrices, transpose)
131 {
132  const double dat_A[] = {1, 2, 3, 4, 5, 6};
133  const double dat_At[] = {1, 4, 2, 5, 3, 6};
134  const CMatrixDouble A(2, 3, dat_A);
135  const CMatrixDouble At(3, 2, dat_At);
136 
137  EXPECT_EQ(A.t(), At);
138  EXPECT_EQ(~A, At);
139  EXPECT_EQ(A.t().t(), A);
140 }
141 
142 TEST(Matrices, multiply_A_skew3)
143 {
144  {
145  const double dat_A[] = {1, 2, 3, 4, 5, 6};
146  const CMatrixDouble A(2, 3, dat_A);
147  const std::vector<double> v{1.0, 2.0, 3.0};
148  const CMatrixDouble S = CMatrixDouble(mrpt::math::skew_symmetric3(v));
149 
150  CMatrixDouble R;
151  R.multiply_A_skew3(A, v);
152  EXPECT_EQ(R, (A * S).eval());
153  }
154  {
155  const double dat_A[] = {1, 2, 3, 4, 5, 6};
156  const double dat_v[] = {1, 2, 3};
157  const CMatrixFixedNumeric<double, 2, 3> A(dat_A);
158  const CArrayDouble<3> v(dat_v);
159  const CMatrixFixedNumeric<double, 3, 3> S =
160  mrpt::math::skew_symmetric3(v);
161 
162  CMatrixFixedNumeric<double, 2, 3> R;
163  R.multiply_A_skew3(A, v);
164  EXPECT_TRUE(R == A * S);
165  }
166 }
167 
168 TEST(Matrices, multiply_skew3_A)
169 {
170  {
171  const double dat_A[] = {1, 2, 3, 4, 5, 6};
172  const CMatrixDouble A(3, 2, dat_A);
173  const std::vector<double> v{1.0, 2.0, 3.0};
174  const CMatrixDouble S = CMatrixDouble(mrpt::math::skew_symmetric3(v));
175 
176  CMatrixDouble R;
177  R.multiply_skew3_A(v, A);
178  EXPECT_TRUE(R == S * A);
179  }
180  {
181  const double dat_A[] = {1, 2, 3, 4, 5, 6};
182  const double dat_v[] = {1, 2, 3};
183  const CMatrixFixedNumeric<double, 3, 2> A(dat_A);
184  const CArrayDouble<3> v(dat_v);
185  const CMatrixFixedNumeric<double, 3, 3> S =
186  mrpt::math::skew_symmetric3(v);
187 
188  CMatrixFixedNumeric<double, 3, 2> R;
189  R.multiply_skew3_A(v, A);
190  EXPECT_TRUE(R == S * A);
191  }
192 }
193 
194 TEST(Matrices, fromMatlabStringFormat)
195 {
196  const char* mat1 = "[1 2 3;-3 -6 -5]";
197  const double vals1[] = {1, 2, 3, -3, -6, -5};
198 
199  const char* mat2 =
200  " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] "; // With tabs and
201  // spaces...
202  const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000, 7};
203 
204  const char* mat3 = "[9]";
205  const char* mat4 =
206  "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]"; // An invalid matrix
207  const char* mat5 = "[ ]"; // Empty
208  const char* mat6 = "[ -405.200 42.232 ; 1219.600 -98.696 ]"; // M1 * M2
209 
210  const char* mat13 = "[9 8 7]";
211  const char* mat31 = "[9; 8; 7]";
212 
213  CMatrixDouble M1, M2, M3, M4, M5, M6;
214 
215  if (!M1.fromMatlabStringFormat(mat1) ||
216  (CMatrixFixedNumeric<double, 2, 3>(vals1) - M1).array().abs().sum() >
217  1e-4)
218  GTEST_FAIL() << mat1;
219 
220  {
221  CMatrixFixedNumeric<double, 2, 3> M1b;
222  if (!M1b.fromMatlabStringFormat(mat1) ||
223  (CMatrixFixedNumeric<double, 2, 3>(vals1) - M1b)
224  .array()
225  .abs()
226  .sum() > 1e-4)
227  GTEST_FAIL() << mat1;
228  }
229 
230  if (!M2.fromMatlabStringFormat(mat2) || M2.cols() != 2 || M2.rows() != 3 ||
231  (CMatrixFixedNumeric<double, 3, 2>(vals2) - M2).array().abs().sum() >
232  1e-4)
233  GTEST_FAIL() << mat2;
234 
235  {
236  CMatrixFixedNumeric<double, 3, 2> M2b;
237  if (!M2b.fromMatlabStringFormat(mat2) ||
238  (CMatrixFixedNumeric<double, 3, 2>(vals2) - M2b)
239  .array()
240  .abs()
241  .sum() > 1e-4)
242  GTEST_FAIL() << mat2;
243  }
244 
245  if (!M3.fromMatlabStringFormat(mat3)) GTEST_FAIL() << mat3;
246 
247  {
248  CVectorDouble m;
249  if (!m.fromMatlabStringFormat(mat3) || m.size() != 1)
250  GTEST_FAIL() << "CVectorDouble:" << mat3;
251  }
252  {
253  CArrayDouble<1> m;
254  if (!m.fromMatlabStringFormat(mat3))
255  GTEST_FAIL() << "CArrayDouble<1>:" << mat3;
256  }
257 
258  {
259  CVectorDouble m;
260  if (!m.fromMatlabStringFormat(mat31) || m.size() != 3)
261  GTEST_FAIL() << "CVectorDouble:" << mat31;
262  }
263  {
264  CArrayDouble<3> m;
265  if (!m.fromMatlabStringFormat(mat31))
266  GTEST_FAIL() << "CArrayDouble<3>:" << mat31;
267  }
268 
269  {
270  Eigen::Matrix<double, 1, 3> m;
271  if (!m.fromMatlabStringFormat(mat13))
272  GTEST_FAIL() << "Matrix<double,1,3>:" << mat13;
273  }
274  {
275  Eigen::Matrix<double, 1, Eigen::Dynamic> m;
276  if (!m.fromMatlabStringFormat(mat13) || m.size() != 3)
277  GTEST_FAIL() << "Matrix<double,1,Dynamic>:" << mat13;
278  }
279 
280  // This one MUST BE detected as WRONG:
281  if (M4.fromMatlabStringFormat(mat4, nullptr /*dont dump errors to cerr*/))
282  GTEST_FAIL() << mat4;
283 
284  if (!M5.fromMatlabStringFormat(mat5) || size(M5, 1) != 0 ||
285  size(M5, 2) != 0)
286  GTEST_FAIL() << mat5;
287 
288  if (!M6.fromMatlabStringFormat(mat6)) GTEST_FAIL() << mat6;
289 
290  // Check correct values loaded:
291  CMatrixDouble RES = M1 * M2;
292 
293  EXPECT_NEAR(0, (M6 - M1 * M2).array().square().sum(), 1e-3);
294 }
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:25
CMatrixFixedNumeric.h
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: CParticleFilter.h:223
dat_A
const double dat_A[]
Definition: matrix_ops1_unittest.cpp:30
mrpt::math::skew_symmetric3
void skew_symmetric3(const VECTOR &v, MATRIX &M)
Computes the 3x3 skew symmetric matrix from a 3-vector or 3-array: .
Definition: geometry.h:853
TEST
TEST(Matrices, inv_4x4_fix)
Definition: matrix_ops2_unittest.cpp:32
multiply_skew3_A
void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A)
Definition: eigen_plugins.h:767
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:42
ops_matrices.h
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
std
STL namespace.
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:55
multiply_A_skew3
void multiply_A_skew3(const MAT_A &A, const SKEW_3VECTOR &v)
Definition: eigen_plugins.h:759
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:39
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::math::CMatrixDouble
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixTemplateNumeric.h:147
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:211
mrpt::utils::metaprogramming
A set of utility objects for metaprogramming with STL algorithms.
Definition: metaprogramming.h:24
fromMatlabStringFormat
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...
transpose
GLsizei GLboolean transpose
Definition: glext.h:4133
v
const GLdouble * v
Definition: glext.h:3678
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
R
const float R
Definition: CKinematicChain.cpp:149
CHECK_AND_RET_ERROR
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)
Definition: matrix_ops2_unittest.cpp:30
size
GLsizeiptr size
Definition: glext.h:3923
CMemoryStream.h


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019