MRPT  2.0.4
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 }
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:221
TEST
TEST(Matrices, inv_4x4_fix)
Definition: matrix_ops2_unittest.cpp:29
CMatrixFixed.h
geometry.h
mrpt::math::CVectorDynamic::cols
size_type cols() const
Number of columns in the matrix (always 1)
Definition: CVectorDynamic.h:138
EXPECT_TRUE
EXPECT_TRUE(mrpt::system::fileExists(ini_fil))
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
dat_A
const double dat_A[]
Definition: matrix_ops1_unittest.cpp:28
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
R
const float R
Definition: CKinematicChain.cpp:137
EXPECT_NEAR
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
random.h
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:186
mrpt::math::CMatrixDouble
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
Definition: CMatrixDynamic.h:589
CVectorFixed.h
mrpt::math::CMatrixFixed
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
mrpt::math::CVectorDynamic::rows
size_type rows() const
Number of rows in the vector.
Definition: CVectorDynamic.h:135
ops_matrices.h
mrpt::math::CMatrixDynamic::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixDynamic.h:540
EXPECT_EQ
EXPECT_EQ(out.image_pair_was_used.size(), NUM_IMGS)
A
Definition: is_defined_unittest.cpp:13
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::math::CVectorDynamic< double >
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
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:165
mrpt::math::CMatrixFixed::asEigen
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
Definition: CMatrixFixed.h:251
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:813
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
CHECK_AND_RET_ERROR
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)
Definition: matrix_ops2_unittest.cpp:27
mrpt::math::CMatrixDynamic::cols
size_type cols() const
Number of columns in the matrix.
Definition: CMatrixDynamic.h:340
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
ok
bool ok
Definition: chessboard_stereo_camera_calib_unittest.cpp:61
mrpt::math::CMatrixDynamic< double >
mrpt::math::CMatrixDynamic::rows
size_type rows() const
Number of rows in the matrix.
Definition: CMatrixDynamic.h:337



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020