Main MRPT website > C++ reference for MRPT 1.5.7
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 
31 
32 #define CHECK_AND_RET_ERROR(_COND_,_MSG_) EXPECT_FALSE(_COND_) << _MSG_;
33 
34 
35 TEST(Matrices,inv_4x4_fix)
36 {
37  const double dat_A[] = {-0.710681653571291,0.734469323344333,-0.656414638791893,0.818771495864303,1.044946492154568,1.163592359608108,-1.069421407670914,0.307916381104872,0.185595851677470,0.116899590868673,0.507691343481809,-3.217842384231890,-0.214383515646621,-0.161495561253269,1.303923696836841,0.261535721431038};
38  CMatrixDouble44 A(dat_A);
39  CMatrixDouble44 C = A.inv();
40  const double dat_AInv[] = {-0.741952742824035,0.493481687552705,-0.134764164880760,0.083693424291000,0.638324207063440,0.519344439204238,0.264483337145361,0.644307267615193,-0.037800456163779,0.131794126194075,0.070338431705792,0.828591793299072,-0.025568212209135,0.068123300450057,-0.297834184749986,0.158964059763645};
41  CMatrixDouble44 AInv(dat_AInv);
42  CHECK_AND_RET_ERROR( (AInv-C).array().abs().sum() >1e-4, "Error in inv, 4x4 fix")
43 }
44 
45 TEST(Matrices,inv_6x6_fix)
46 {
47  const double dat_A[] = {363.769989013671875,0.000000000000000,316.429992675781250,0.000000000000000,87.266998291015625,0.000000000000000,101.540000915527344,0.000000000000000,478.709991455078125,0.000000000000000,504.540008544921875,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000,0.000000000000000,0.000000000000000,363.769989013671875,0.000000000000000,316.429992675781250,0.000000000000000,87.266998291015625,0.000000000000000,101.540000915527344,0.000000000000000,478.709991455078125,0.000000000000000,504.540008544921875,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000};
48  CMatrixDouble66 A(dat_A);
49  CMatrixDouble66 C;
50  A.inv(C);
51  const double dat_AInv[] = {-0.000303131460181,-0.002689371550382,1.383348917627708,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,-0.000303131460181,-0.002689371550382,1.383348917627708,0.004729457992255,0.003244936115630,-2.049925698035195,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.004729457992255,0.003244936115630,-2.049925698035195,-0.004426326532074,-0.000555564565248,1.666576780407488,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,-0.004426326532074,-0.000555564565248,1.666576780407488};
52  CMatrixDouble66 AInv(dat_AInv);
53  CHECK_AND_RET_ERROR( isNaN(C(0,0)) || !isFinite(C(0,0)) || (AInv-C).array().abs().sum() >1e-4, "Error in inv, 6x6 fix")
54 }
55 
56 TEST(Matrices,inv_6x6_dyn)
57 {
58  const double dat_A[] = {363.769989013671875,0.000000000000000,316.429992675781250,0.000000000000000,87.266998291015625,0.000000000000000,101.540000915527344,0.000000000000000,478.709991455078125,0.000000000000000,504.540008544921875,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000,0.000000000000000,0.000000000000000,363.769989013671875,0.000000000000000,316.429992675781250,0.000000000000000,87.266998291015625,0.000000000000000,101.540000915527344,0.000000000000000,478.709991455078125,0.000000000000000,504.540008544921875,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000,0.000000000000000,1.000000000000000};
59  CMatrixDouble A(6,6,dat_A);
60  CMatrixDouble C = A.inv();
61  const double dat_AInv[] = {-0.000303131460181,-0.002689371550382,1.383348917627708,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,-0.000303131460181,-0.002689371550382,1.383348917627708,0.004729457992255,0.003244936115630,-2.049925698035195,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.004729457992255,0.003244936115630,-2.049925698035195,-0.004426326532074,-0.000555564565248,1.666576780407488,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,0.000000000000000,-0.004426326532074,-0.000555564565248,1.666576780407488};
62  CMatrixDouble AInv(6,6,dat_AInv);
63  CHECK_AND_RET_ERROR( isNaN(C(0,0)) || !isFinite(C(0,0)) || (AInv-C).array().abs().sum() >1e-4, "Error in inv, 6x6 dyn")
64 }
65 
66 TEST(Matrices,transpose)
67 {
68  const double dat_A[] = {
69  1,2,3,
70  4,5,6 };
71  const double dat_At[] = {
72  1,4,
73  2,5,
74  3,6};
75  const CMatrixDouble A(2,3,dat_A);
76  const CMatrixDouble At(3,2,dat_At);
77 
78  EXPECT_EQ(A.t(), At);
79  EXPECT_EQ(~A, At);
80  EXPECT_EQ(A.t().t(), A);
81 }
82 
83 TEST(Matrices,multiply_A_skew3)
84 {
85  {
86  const double dat_A[] = {
87  1,2,3,
88  4,5,6 };
89  const CMatrixDouble A(2,3,dat_A);
90  const std::vector<double> v = make_vector<3>(1.0,2.0,3.0);
91  const CMatrixDouble S = CMatrixDouble( mrpt::math::skew_symmetric3(v) );
92 
93  CMatrixDouble R;
94  R.multiply_A_skew3(A,v);
95  EXPECT_EQ(R, (A*S).eval() );
96  }
97  {
98  const double dat_A[] = {
99  1,2,3,
100  4,5,6 };
101  const double dat_v[] = { 1,2,3 };
102  const CMatrixFixedNumeric<double,2,3> A(dat_A);
103  const CArrayDouble<3> v(dat_v);
104  const CMatrixFixedNumeric<double,3,3> S = mrpt::math::skew_symmetric3(v);
105 
106  CMatrixFixedNumeric<double,2,3> R;
107  R.multiply_A_skew3(A,v);
108  EXPECT_TRUE(R== A*S );
109  }
110 }
111 
112 TEST(Matrices,multiply_skew3_A)
113 {
114  {
115  const double dat_A[] = {
116  1,2,
117  3,4,
118  5,6 };
119  const CMatrixDouble A(3,2,dat_A);
120  const std::vector<double> v = make_vector<3>(1.0,2.0,3.0);
121  const CMatrixDouble S = CMatrixDouble( mrpt::math::skew_symmetric3(v) );
122 
123  CMatrixDouble R;
124  R.multiply_skew3_A(v,A);
125  EXPECT_TRUE(R == S*A );
126  }
127  {
128  const double dat_A[] = {
129  1,2,
130  3,4,
131  5,6 };
132  const double dat_v[] = { 1,2,3 };
133  const CMatrixFixedNumeric<double,3,2> A(dat_A);
134  const CArrayDouble<3> v(dat_v);
135  const CMatrixFixedNumeric<double,3,3> S = mrpt::math::skew_symmetric3(v);
136 
137  CMatrixFixedNumeric<double,3,2> R;
138  R.multiply_skew3_A(v,A);
139  EXPECT_TRUE(R == S*A );
140  }
141 }
142 
143 
144 TEST(Matrices,fromMatlabStringFormat)
145 {
146  const char* mat1 = "[1 2 3;-3 -6 -5]";
147  const double vals1[] = {1,2,3,-3,-6,-5};
148 
149  const char* mat2 = " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] "; // With tabs and spaces...
150  const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000 ,7};
151 
152  const char* mat3 = "[9]";
153  const char* mat4 = "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]"; // An invalid matrix
154  const char* mat5 = "[ ]"; // Empty
155  const char* mat6 = "[ -405.200 42.232 ; 1219.600 -98.696 ]"; // M1 * M2
156 
157  const char* mat13 = "[9 8 7]";
158  const char* mat31 = "[9; 8; 7]";
159 
160  CMatrixDouble M1,M2,M3, M4, M5, M6;
161 
162  if (! M1.fromMatlabStringFormat(mat1) ||
163  (CMatrixFixedNumeric<double,2,3>(vals1)-M1).array().abs().sum() > 1e-4 )
164  GTEST_FAIL() << mat1;
165 
166  {
167  CMatrixFixedNumeric<double,2,3> M1b;
168  if (! M1b.fromMatlabStringFormat(mat1) ||
169  (CMatrixFixedNumeric<double,2,3>(vals1)-M1b).array().abs().sum() > 1e-4 )
170  GTEST_FAIL() << mat1;
171  }
172 
173  if (! M2.fromMatlabStringFormat(mat2) ||
174  M2.cols()!=2 || M2.rows()!=3 ||
175  (CMatrixFixedNumeric<double,3,2>(vals2)-M2).array().abs().sum() > 1e-4 )
176  GTEST_FAIL() << mat2;
177 
178  {
179  CMatrixFixedNumeric<double,3,2> M2b;
180  if (! M2b.fromMatlabStringFormat(mat2) ||
181  (CMatrixFixedNumeric<double,3,2>(vals2)-M2b).array().abs().sum() > 1e-4 )
182  GTEST_FAIL() << mat2;
183  }
184 
185  if (! M3.fromMatlabStringFormat(mat3) )
186  GTEST_FAIL() << mat3;
187 
188  {
189  CVectorDouble m;
190  if (! m.fromMatlabStringFormat(mat3) || m.size()!=1 ) GTEST_FAIL() << "CVectorDouble:" << mat3;
191  }
192  {
193  CArrayDouble<1> m;
194  if (! m.fromMatlabStringFormat(mat3) ) GTEST_FAIL() << "CArrayDouble<1>:" << mat3;
195  }
196 
197  {
198  CVectorDouble m;
199  if (! m.fromMatlabStringFormat(mat31) || m.size()!=3 ) GTEST_FAIL() << "CVectorDouble:" << mat31;
200  }
201  {
202  CArrayDouble<3> m;
203  if (! m.fromMatlabStringFormat(mat31) ) GTEST_FAIL() << "CArrayDouble<3>:" << mat31;
204  }
205 
206  {
207  Eigen::Matrix<double,1,3> m;
208  if (! m.fromMatlabStringFormat(mat13) ) GTEST_FAIL() << "Matrix<double,1,3>:" << mat13;
209  }
210  {
211  Eigen::Matrix<double,1,Eigen::Dynamic> m;
212  if (! m.fromMatlabStringFormat(mat13) || m.size()!=3 ) GTEST_FAIL() << "Matrix<double,1,Dynamic>:" << mat13;
213  }
214 
215  // This one MUST BE detected as WRONG:
216  if ( M4.fromMatlabStringFormat(mat4, NULL /*dont dump errors to cerr*/) )
217  GTEST_FAIL() << mat4;
218 
219  if (! M5.fromMatlabStringFormat(mat5) || size(M5,1)!=0 || size(M5,2)!=0 )
220  GTEST_FAIL() << mat5;
221 
222  if (! M6.fromMatlabStringFormat(mat6) )
223  GTEST_FAIL() << mat6;
224 
225  // Check correct values loaded:
226  CMatrixDouble RES = M1*M2;
227 
228  EXPECT_NEAR(0,(M6 - M1*M2).array().square().sum(), 1e-3);
229 }
230 
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:22
CMatrixFixedNumeric.h
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
dat_A
const double dat_A[]
Definition: matrix_ops1_unittest.cpp:31
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:657
TEST
TEST(Matrices, inv_4x4_fix)
Definition: matrix_ops2_unittest.cpp:35
multiply_skew3_A
void multiply_skew3_A(const SKEW_3VECTOR &v, const MAT_A &A)
Definition: eigen_plugins.h:573
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
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:52
multiply_A_skew3
void multiply_A_skew3(const MAT_A &A, const SKEW_3VECTOR &v)
Definition: eigen_plugins.h:569
mrpt::math::isNaN
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
Definition: math.cpp:1679
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:34
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:130
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
mrpt::utils::metaprogramming
A set of utility objects for metaprogramming with STL algorithms.
Definition: metaprogramming.h:24
mrpt::math::isFinite
bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS
Returns true if the number is non infinity.
Definition: math.cpp:1705
transpose
GLsizei GLboolean transpose
Definition: glext.h:3937
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
R
const float R
Definition: CKinematicChain.cpp:141
fromMatlabStringFormat
bool fromMatlabStringFormat(const std::string &s, std::ostream *dump_errors_here=NULL)
Read a matrix from a string in Matlab-like format, for example "[1 0 2; 0 4 -1]" The string must star...
CHECK_AND_RET_ERROR
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)
Definition: matrix_ops2_unittest.cpp:32
size
GLsizeiptr size
Definition: glext.h:3779
CMemoryStream.h


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019