MRPT  2.0.1
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-2020, 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};
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  };
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};
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);
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);
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 
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};
167 
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};
176  const CVectorFixedDouble<3> v(dat_v);
178 
181  EXPECT_EQ(R.asEigen(), A.asEigen() * S.asEigen());
182  }
183 }
184 
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};
192 
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};
201  const CVectorFixedDouble<3> v(dat_v);
203 
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  {
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 }
A namespace of pseudo-random numbers generators of diferent distributions.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
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:802
TEST(Matrices, inv_4x4_fix)
size_type cols() const
Number of columns in the matrix (always 1)
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:165
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
STL namespace.
This base provides a set of functions for maths stuff.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
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...
size_type rows() const
Number of rows in the vector.
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
return_t square(const num_t x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
Definition: CMatrixFixed.h:251
#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:186
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020