MRPT  1.9.9
matrix_ops2_unittest.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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 <gtest/gtest.h>
15 #include <mrpt/math/CMatrixFixed.h>
16 #include <mrpt/math/CVectorFixed.h>
17 #include <mrpt/math/geometry.h>
18 #include <mrpt/math/ops_matrices.h>
19 #include <mrpt/random.h>
20 #include <Eigen/Dense>
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};
38  CMatrixDouble44 A(dat_A);
39  CMatrixDouble44 C = A.inverse();
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);
48  CHECK_AND_RET_ERROR((AInv - C).sum_abs() > 1e-4, "Error in inv, 4x4 fix")
49 }
50 
51 TEST(Matrices, inv_LLt_4x4_fix)
52 {
53  const double dat_A[] = {
54  // clang-format off
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
59  // clang-format on
60  };
61  CMatrixDouble44 A(dat_A);
62  CMatrixDouble44 C = A.inverse_LLt();
63  const double dat_AInv[] = {
64  // clang-format off
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
69  // clang-format on
70  };
71  CMatrixDouble44 AInv(dat_AInv);
72  CHECK_AND_RET_ERROR((AInv - C).sum_abs() > 1e-4, "Error in inv, 4x4 fix")
73 }
74 
75 TEST(Matrices, inv_6x6_fix)
76 {
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};
90  CMatrixDouble66 A(dat_A);
91  const CMatrixDouble66 C = A.inverse();
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};
105  CMatrixDouble66 AInv(dat_AInv);
106  CHECK_AND_RET_ERROR(
107  std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
108  (AInv - C).sum_abs() > 1e-4,
109  "Error in inv, 6x6 fix")
110 }
111 
112 TEST(Matrices, inv_6x6_dyn)
113 {
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};
127  CMatrixDouble A(6, 6, dat_A);
128  CMatrixDouble C = A.inverse();
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};
142  CMatrixDouble AInv(6, 6, dat_AInv);
143  CHECK_AND_RET_ERROR(
144  std::isnan(C(0, 0)) || !std::isfinite(C(0, 0)) ||
145  (AInv - C).sum_abs() > 1e-4,
146  "Error in inv, 6x6 dyn")
147 }
148 
149 TEST(Matrices, transpose)
150 {
151  const double dat_A[] = {1, 2, 3, 4, 5, 6};
152  const double dat_At[] = {1, 4, 2, 5, 3, 6};
153  const CMatrixDouble A(2, 3, dat_A);
154  const CMatrixDouble At(3, 2, dat_At);
155 
156  EXPECT_EQ(A.transpose(), At.asEigen());
157  EXPECT_EQ(A.transpose().transpose(), A.asEigen());
158 }
159 
160 TEST(Matrices, multiply_A_skew3)
161 {
162  {
163  const double dat_A[] = {1, 2, 3, 4, 5, 6};
164  const CMatrixDouble A(2, 3, dat_A);
165  const std::vector<double> v{1.0, 2.0, 3.0};
166  const CMatrixDouble S = CMatrixDouble(mrpt::math::skew_symmetric3(v));
167 
168  CMatrixDouble R;
169  mrpt::math::multiply_A_skew3(A, v, R);
170  EXPECT_EQ(R.asEigen(), A.asEigen() * S.asEigen());
171  }
172  {
173  const double dat_A[] = {1, 2, 3, 4, 5, 6};
174  const double dat_v[] = {1, 2, 3};
175  const CMatrixFixed<double, 2, 3> A(dat_A);
176  const CVectorFixedDouble<3> v(dat_v);
177  const CMatrixFixed<double, 3, 3> S = mrpt::math::skew_symmetric3(v);
178 
179  CMatrixFixed<double, 2, 3> R;
180  mrpt::math::multiply_A_skew3(A, v, R);
181  EXPECT_EQ(R.asEigen(), A.asEigen() * S.asEigen());
182  }
183 }
184 
185 TEST(Matrices, multiply_skew3_A)
186 {
187  {
188  const double dat_A[] = {1, 2, 3, 4, 5, 6};
189  const CMatrixDouble A(3, 2, dat_A);
190  const std::vector<double> v{1.0, 2.0, 3.0};
191  const CMatrixDouble S = CMatrixDouble(mrpt::math::skew_symmetric3(v));
192 
193  CMatrixDouble R;
194  mrpt::math::multiply_skew3_A(v, A, R);
195  EXPECT_EQ(R.asEigen(), S.asEigen() * A.asEigen());
196  }
197  {
198  const double dat_A[] = {1, 2, 3, 4, 5, 6};
199  const double dat_v[] = {1, 2, 3};
200  const CMatrixFixed<double, 3, 2> A(dat_A);
201  const CVectorFixedDouble<3> v(dat_v);
202  const CMatrixFixed<double, 3, 3> S = mrpt::math::skew_symmetric3(v);
203 
204  CMatrixFixed<double, 3, 2> R;
205  mrpt::math::multiply_skew3_A(v, A, R);
206  EXPECT_EQ(R.asEigen(), S.asEigen() * A.asEigen());
207  }
208 }
209 
210 TEST(Matrices, fromMatlabStringFormat)
211 {
212  const char* mat1 = "[1 2 3;-3 -6 -5]";
213  const double vals1[] = {1, 2, 3, -3, -6, -5};
214 
215  const char* mat2 =
216  " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] "; // With tabs and
217  // spaces...
218  const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000, 7};
219 
220  const char* mat3 = "[9]";
221  const char* mat4 =
222  "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]"; // An invalid matrix
223  const char* mat5 = "[ ]"; // Empty
224  const char* mat6 = "[ -405.200 42.232 ; 1219.600 -98.696 ]"; // M1 * M2
225 
226  const char* mat13 = "[9 8 7]";
227  const char* mat31 = "[9; 8; 7]";
228 
229  CMatrixDouble M1, M2, M3, M4, M5, M6;
230 
231  if (!M1.fromMatlabStringFormat(mat1) ||
232  (CMatrixFixed<double, 2, 3>(vals1) - M1).array().abs().sum() > 1e-4)
233  GTEST_FAIL() << mat1;
234 
235  {
236  CMatrixDouble M1b;
237  if (!M1b.fromMatlabStringFormat(mat1) ||
238  (CMatrixFixed<double, 2, 3>(vals1) - M1b).array().abs().sum() >
239  1e-4)
240  GTEST_FAIL() << mat1;
241  }
242 
243  if (!M2.fromMatlabStringFormat(mat2) || M2.cols() != 2 || M2.rows() != 3 ||
244  (CMatrixFixed<double, 3, 2>(vals2) - M2).array().abs().sum() > 1e-4)
245  GTEST_FAIL() << mat2;
246 
247  {
248  CMatrixDouble M2b;
249  if (!M2b.fromMatlabStringFormat(mat2) ||
250  (CMatrixDouble(CMatrixFixed<double, 3, 2>(vals2)) - M2b).sum_abs() >
251  1e-4)
252  GTEST_FAIL() << mat2;
253  }
254 
255  if (!M3.fromMatlabStringFormat(mat3)) GTEST_FAIL() << mat3;
256 
257  {
258  CVectorDouble m;
259  if (!m.fromMatlabStringFormat(mat3) || m.size() != 1)
260  GTEST_FAIL() << "CVectorDouble:" << mat3;
261  }
262  {
263  CVectorDouble m;
264  if (!m.fromMatlabStringFormat(mat3))
265  GTEST_FAIL() << "CVectorFixedDouble<1>:" << mat3;
266  }
267 
268  {
269  CVectorDouble m;
270  if (!m.fromMatlabStringFormat(mat31) || m.size() != 3)
271  GTEST_FAIL() << "CVectorDouble:" << mat31;
272  }
273  {
274  CVectorFixedDouble<3> m;
275  if (!m.fromMatlabStringFormat(mat31))
276  GTEST_FAIL() << "CVectorFixedDouble<3>:" << mat31;
277  }
278 
279  {
280  CMatrixDouble m;
281  if (!m.fromMatlabStringFormat(mat13))
282  GTEST_FAIL() << "Matrix<double,1,3>:" << mat13;
283  }
284  {
285  CVectorDouble m;
286  bool ok = m.fromMatlabStringFormat(mat31);
287  const auto len = m.size();
288  EXPECT_EQ(m.rows(), 3) << "Matrix<double,1,Dynamic>:" << mat31;
289  EXPECT_TRUE(ok) << "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;
293  }
294 
295  // This one MUST BE detected as WRONG:
296  if (M4.fromMatlabStringFormat(mat4)) GTEST_FAIL() << mat4;
297 
298  if (!M5.fromMatlabStringFormat(mat5) || M5.rows() != 0 || M5.cols() != 0)
299  GTEST_FAIL() << mat5;
300 
301  if (!M6.fromMatlabStringFormat(mat6)) GTEST_FAIL() << mat6;
302 
303  // Check correct values loaded:
304  CMatrixDouble RES = CMatrixDouble(M1 * M2);
305 
306  EXPECT_NEAR(0, (M6 - RES).array().square().sum(), 1e-3);
307 }
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
dat_A
const double dat_A[]
Definition: matrix_ops1_unittest.cpp:28
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:841
TEST
TEST(Matrices, inv_4x4_fix)
Definition: matrix_ops2_unittest.cpp:29
mrpt::math::CVectorDynamic::cols
size_type cols() const
Number of columns in the matrix (always 1)
Definition: CVectorDynamic.h:138
mrpt::math::multiply_A_skew3
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:164
mrpt::math::CVectorDynamic::size
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CVectorDynamic.h:141
CMatrixFixed.h
ops_matrices.h
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
std
STL namespace.
len
GLenum GLsizei len
Definition: glext.h:4756
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:19
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:221
mrpt::math::MatrixVectorBase::fromMatlabStringFormat
bool fromMatlabStringFormat(const std::string &s, mrpt::optional_ref< std::ostream > dump_errors_here=std::nullopt)
Reads a matrix from a string in Matlab-like format, for example: "[1 0 2; 0 4 -1]" The string must st...
Definition: MatrixVectorBase_impl.h:24
mrpt::math::CVectorDynamic::rows
size_type rows() const
Number of rows in the vector.
Definition: CVectorDynamic.h:135
mrpt::math::CMatrixDynamic::rows
size_type rows() const
Number of rows in the matrix.
Definition: CMatrixDynamic.h:324
mrpt::math::CMatrixDynamic::cols
size_type cols() const
Number of columns in the matrix.
Definition: CMatrixDynamic.h:327
transpose
GLsizei GLboolean transpose
Definition: glext.h:4150
v
const GLdouble * v
Definition: glext.h:3684
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:27
R
const float R
Definition: CKinematicChain.cpp:138
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
CVectorFixed.h
CHECK_AND_RET_ERROR
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)
Definition: matrix_ops2_unittest.cpp:27
mrpt::math::multiply_skew3_A
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:185
mrpt::math::CMatrixDynamic::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixDynamic.h:512
mrpt::math::CMatrixDouble
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixDynamic.h:561


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019