MRPT  2.0.1
matrix_ops3_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>
16 #include <mrpt/math/CMatrixFixed.h>
17 #include <mrpt/math/ops_matrices.h>
18 #include <mrpt/math/utils.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, HCHt_3x2_2x2_2x3)
30 {
31  const double dat_H[] = {0.2, -0.3, 0.1, 0.9, -0.07, 1.2};
32  const CMatrixFixed<double, 3, 2> H(dat_H);
33 
34  const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
35  const CMatrixFixed<double, 2, 2> C(dat_C);
36 
37  const double dat_R[] = {0.11600, -0.21500, -0.32530, -0.21500, 0.63800,
38  0.85270, -0.32530, 0.85270, 1.17272};
39  const CMatrixFixed<double, 3, 3> R_REAL(dat_R);
40 
41  const auto R = mrpt::math::multiply_HCHt(H, C);
42 
43  EXPECT_NEAR((R_REAL - R).sum_abs(), 0, 1e-4) << "R:\n"
44  << R << "\nR_REAL:\n"
45  << R_REAL << "\n";
46 }
47 
48 TEST(Matrices, HCHt_scalar_1x2_2x2_2x1)
49 {
50  const double dat_H[] = {0.2, -0.3};
51  CMatrixDouble H(1, 2, dat_H);
52 
53  const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
54  CMatrixDouble C(2, 2, dat_C);
55 
56  const double r = mrpt::math::multiply_HCHt_scalar(H, C);
57 
58  const double r2 =
59  (H.asEigen() * C.asEigen() * H.asEigen().transpose()).eval()(0, 0);
61  fabs(r - r2) > 1e-4, "Error in HCHt_scalar: 1x2 * 2x2 * 2x1")
62 }
63 
64 TEST(Matrices, det_2x2_dyn)
65 {
66  const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
67  CMatrixDouble A(2, 2, dat_A);
68 
69  const double d = A.det();
70  const double d_R = -0.13;
71 
72  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 2x2 dyn")
73 }
74 
75 TEST(Matrices, det_2x2_fix)
76 {
77  const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
79 
80  const double d = A.det();
81  const double d_R = -0.13;
82 
83  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 2x2 fix")
84 }
85 
86 TEST(Matrices, det_3x3_dyn)
87 {
88  const double dat_A[] = {-3.3304e-01, -2.0585e-01, 6.2026e-05,
89  1.4631e+00, 6.0985e-01, 2.3746e+00,
90  -3.6451e-01, 4.8169e-01, -8.4419e-01};
91  CMatrixDouble A(3, 3, dat_A);
92  const double d = A.det();
93  const double d_R = 0.476380435871666;
94  // cout << "d: " << d << endl;
95  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 3x3 dyn")
96 }
97 
98 TEST(Matrices, det_3x3_fix)
99 {
100  const double dat_A[] = {-3.3304e-01, -2.0585e-01, 6.2026e-05,
101  1.4631e+00, 6.0985e-01, 2.3746e+00,
102  -3.6451e-01, 4.8169e-01, -8.4419e-01};
104  const double d = A.det();
105  const double d_R = 0.476380435871666;
106  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 3x3 fix")
107 }
108 
109 TEST(Matrices, det_4x4_dyn)
110 {
111  const double dat_A[] = {0.773931, -0.336130, 1.131764, 0.385890,
112  1.374906, -0.540629, -0.952902, 0.659769,
113  -0.387254, -1.557355, 0.139683, -2.056635,
114  -0.750078, -0.653811, 0.872027, 0.217554};
115  CMatrixDouble A(4, 4, dat_A);
116  const double d = A.det();
117  const double d_R = -6.29527837425056;
118  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 4x4 dyn")
119 }
120 
121 TEST(Matrices, det_4x4_fix)
122 {
123  const double dat_A[] = {0.773931, -0.336130, 1.131764, 0.385890,
124  1.374906, -0.540629, -0.952902, 0.659769,
125  -0.387254, -1.557355, 0.139683, -2.056635,
126  -0.750078, -0.653811, 0.872027, 0.217554};
128  const double d = A.det();
129  const double d_R = -6.29527837425056;
130  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 4x4 fix")
131 }
132 
133 TEST(Matrices, det_10x10_dyn)
134 {
135  const double dat_A[] = {
136  1.2305462976, -0.2944257811, 0.8176140437, -0.0487601371,
137  0.4418235581, -0.0088466980, -1.4100223408, -0.6219629815,
138  1.1089237266, -0.6450262619, -2.0862614547, 0.2699762709,
139  -0.0705918517, 1.1763963161, -0.3461819597, -1.3013222580,
140  -0.3310621595, -0.2595069675, -0.5188213591, 1.2261476224,
141  -1.1334297957, 2.1452881319, 1.7856021357, 0.5406722888,
142  0.5497545623, 0.4282217402, -1.6175210256, -0.3522824764,
143  0.2773929603, 0.8507134453, 0.4046854117, -2.1638696195,
144  1.0044939778, 0.9755939720, 0.9640788301, 0.5641138097,
145  0.7382236207, -0.4422212587, 0.8507041571, 1.3764399072,
146  0.3446492224, 1.1681336612, -1.3440052449, 1.0120691406,
147  -0.0430604384, 0.4823901171, 0.0881769800, 0.3984805283,
148  -1.9988153178, 0.9509748328, 0.3202853059, 1.9688559025,
149  0.4020581289, -1.5558616735, -0.8753527614, 0.1207830427,
150  0.0457715031, -0.1557123759, -0.3161307172, -0.0759276933,
151  -0.0417386037, 1.2079564736, -2.5839030155, -0.7648863647,
152  1.1541464803, 0.2127569446, -1.4882083860, -0.7630836781,
153  0.8550884427, -0.8440402465, -0.4903597050, -0.1457982930,
154  0.5893448560, -0.2353784687, 0.3474655757, 2.5874616045,
155  0.6608448038, -1.0105315509, -1.5276853710, -0.1400026815,
156  -1.7630264416, 2.4048579514, -0.3111046623, 0.7463774799,
157  -0.2800404492, -1.4175124130, -0.5708536580, -1.2085107661,
158  0.8169107561, -1.1659481510, -0.1406355512, 2.3507381980,
159  2.6346742737, -1.1028788167, -0.0533115044, 0.3752684649,
160  -1.3799576309, -0.7274190037, 1.1188847602, -0.6624231096};
161  CMatrixDouble A(10, 10, dat_A);
162  const double d = A.det();
163  const double d_R = 330.498518199239;
164  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 10x10 dyn")
165 }
166 
167 TEST(Matrices, chol_2x2_dyn)
168 {
169  const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
170  0.8262219720};
171  CMatrixDouble A(2, 2, dat_A);
172  CMatrixDouble C;
173  A.chol(C);
174  const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
175  0.6672308672};
176  CMatrixDouble CHOL(2, 2, dat_CHOL);
178  (CHOL - C).sum_abs() > 1e-4, "Error in Choleski, 2x2 dyn")
179 }
180 
181 TEST(Matrices, chol_2x2_fix)
182 {
183  const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
184  0.8262219720};
186  CMatrixDouble22 C;
187  A.chol(C);
188  const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
189  0.6672308672};
190  CMatrixDouble CHOL(2, 2, dat_CHOL);
192  (CHOL - CMatrixDouble(C)).sum_abs() > 1e-4,
193  "Error in Choleski, 2x2 fix")
194 }
195 
196 TEST(Matrices, chol_3x3_dyn)
197 {
198  const double dat_A[] = {
199  0.515479426556448, 0.832723636299236, 0.249691538245735,
200  0.832723636299236, 1.401081397506934, 0.385539356127255,
201  0.249691538245735, 0.385539356127255, 0.128633962591437};
202  CMatrixDouble A(3, 3, dat_A);
203  CMatrixDouble C;
204  A.chol(C);
205 
206  const double dat_CHOL[] = {
207  0.717968959326549, 1.159832365288224, 0.347774837619643,
208  0.000000000000000, 0.236368952988455, -0.075395504153773,
209  0.000000000000000, 0.000000000000000, 0.044745311077990};
210  CMatrixDouble CHOL(3, 3, dat_CHOL);
212  (CHOL - C).sum_abs() > 1e-4, "Error in Choleski, 3x3 dyn")
213 }
214 
215 TEST(Matrices, chol_3x3_fix)
216 {
217  const double dat_A[] = {
218  0.515479426556448, 0.832723636299236, 0.249691538245735,
219  0.832723636299236, 1.401081397506934, 0.385539356127255,
220  0.249691538245735, 0.385539356127255, 0.128633962591437};
222  CMatrixDouble33 C;
223  A.chol(C);
224 
225  const double dat_CHOL[] = {
226  0.717968959326549, 1.159832365288224, 0.347774837619643,
227  0.000000000000000, 0.236368952988455, -0.075395504153773,
228  0.000000000000000, 0.000000000000000, 0.044745311077990};
229  CMatrixDouble33 CHOL(dat_CHOL);
231  (CHOL - C).sum_abs() > 1e-4, "Error in Choleski, 3x3 fix")
232 }
233 
234 TEST(Matrices, chol_10x10_dyn)
235 {
236  const double dat_A[] = {
237  2.8955668335, 2.3041932983, 1.9002381085, 1.7993158652, 1.8456197228,
238  2.9632296740, 1.9368565578, 2.1988923358, 2.0547605617, 2.5655678993,
239  2.3041932983, 3.8406914364, 2.1811218706, 3.2312564555, 2.4736403918,
240  3.4703311380, 1.4874417483, 3.1073538218, 2.1353324397, 2.9541115932,
241  1.9002381085, 2.1811218706, 2.4942067597, 1.6851007198, 1.4585872052,
242  2.3015952197, 1.0955231591, 2.2979627790, 1.3918738834, 2.1854562572,
243  1.7993158652, 3.2312564555, 1.6851007198, 3.1226161015, 1.6779632687,
244  2.7195826381, 1.2397348013, 2.3757864319, 1.6291224768, 2.4463194915,
245  1.8456197228, 2.4736403918, 1.4585872052, 1.6779632687, 2.8123267839,
246  2.5860688816, 1.4131630919, 2.1914803135, 1.5542420639, 2.7170092067,
247  2.9632296740, 3.4703311380, 2.3015952197, 2.7195826381, 2.5860688816,
248  4.1669180394, 2.1145239023, 3.3214801332, 2.6694845663, 3.0742063088,
249  1.9368565578, 1.4874417483, 1.0955231591, 1.2397348013, 1.4131630919,
250  2.1145239023, 1.8928811570, 1.7097998455, 1.7205860530, 1.8710847505,
251  2.1988923358, 3.1073538218, 2.2979627790, 2.3757864319, 2.1914803135,
252  3.3214801332, 1.7097998455, 3.4592638415, 2.1518695071, 2.8907499694,
253  2.0547605617, 2.1353324397, 1.3918738834, 1.6291224768, 1.5542420639,
254  2.6694845663, 1.7205860530, 2.1518695071, 2.1110960664, 1.6731209980,
255  2.5655678993, 2.9541115932, 2.1854562572, 2.4463194915, 2.7170092067,
256  3.0742063088, 1.8710847505, 2.8907499694, 1.6731209980, 3.9093678727};
257  CMatrixDouble A(10, 10, dat_A);
258  CMatrixDouble C;
259  A.chol(C);
260 
261  const double dat_CHOL[] = {
262  1.7016365163, 1.3541042851, 1.1167121124, 1.0574031810,
263  1.0846145491, 1.7413999087, 1.1382316607, 1.2922221137,
264  1.2075202560, 1.5077061845, 0.0000000000, 1.4167191047,
265  0.4722017314, 1.2701334167, 0.7093566960, 0.7851196867,
266  -0.0380051491, 0.9582353452, 0.3530862859, 0.6441080558,
267  0.0000000000, 0.0000000000, 1.0120209201, -0.0943393725,
268  -0.0865342379, -0.0136183214, -0.1557357390, 0.3976620401,
269  -0.1218419159, 0.1952860421, 0.0000000000, 0.0000000000,
270  0.0000000000, 0.6183654266, -0.6113744707, -0.1944977093,
271  0.1127886805, -0.2752173394, -0.1741275611, 0.0847171764,
272  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
273  0.8668818973, 0.0234194680, 0.3011475111, -0.0272963639,
274  -0.1417917925, 0.8000162775, 0.0000000000, 0.0000000000,
275  0.0000000000, 0.0000000000, 0.0000000000, 0.6924364129,
276  0.2527445784, 0.3919505633, 0.3715689962, -0.0817608778,
277  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
278  0.0000000000, 0.0000000000, 0.6358623279, 0.4364121485,
279  0.4859857603, -0.0313828244, 0.0000000000, 0.0000000000,
280  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
281  0.0000000000, 0.5408375843, -0.1995475524, 0.6258606925,
282  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
283  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
284  0.2213262214, -0.2367037013, 0.0000000000, 0.0000000000,
285  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
286  0.0000000000, 0.0000000000, 0.0000000000, 0.2838575216};
287  CMatrixDouble CHOL(10, 10, dat_CHOL);
289  (CHOL - C).sum_abs() > 1e-4, "Error in Choleski, 10x10 dyn")
290 }
A namespace of pseudo-random numbers generators of diferent distributions.
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
const double dat_A[]
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H*C*H^t (H: row vector, C: symmetric matrix)
Definition: ops_matrices.h:63
STL namespace.
This base provides a set of functions for maths stuff.
TEST(Matrices, HCHt_3x2_2x2_2x3)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
const float R
EXPECT_NEAR(out.cam_params.rightCameraPose.x, 0.1194, 0.005)
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
Definition: ops_matrices.h:28



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