MRPT  1.9.9
matrix_ops3_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-2018, 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/CMatrixTemplateNumeric.h>
16 #include <mrpt/math/utils.h>
17 #include <mrpt/random.h>
18 #include <gtest/gtest.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::math;
22 using namespace mrpt::random;
23 using namespace std;
24 
25 #define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_;
26 
27 TEST(Matrices, HCHt_3x2_2x2_2x3)
28 {
29  const double dat_H[] = {0.2, -0.3, 0.1, 0.9, -0.07, 1.2};
30  CMatrixDouble H(3, 2, dat_H);
31 
32  const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
33  CMatrixDouble C(2, 2, dat_C);
34 
35  const double dat_R[] = {0.11600, -0.21500, -0.32530, -0.21500, 0.63800,
36  0.85270, -0.32530, 0.85270, 1.17272};
37  CMatrixDouble R_REAL(3, 3, dat_R);
38 
39  CMatrixDouble R;
40  H.multiply_HCHt(C, R);
41 
42  EXPECT_NEAR((R_REAL - R).array().abs().sum(), 0, 1e-4);
43 }
44 
45 TEST(Matrices, HCHt_scalar_1x2_2x2_2x1)
46 {
47  const double dat_H[] = {0.2, -0.3};
48  CMatrixDouble H(1, 2, dat_H);
49 
50  const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
51  CMatrixDouble C(2, 2, dat_C);
52 
53  const double r = H.multiply_HCHt_scalar(C);
54 
55  const double r2 = (H * C * H.transpose()).eval()(0, 0);
56  CHECK_AND_RET_ERROR(
57  fabs(r - r2) > 1e-4, "Error in HCHt_scalar: 1x2 * 2x2 * 2x1")
58 }
59 
60 TEST(Matrices, det_2x2_dyn)
61 {
62  const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
63  CMatrixDouble A(2, 2, dat_A);
64 
65  const double d = A.det();
66  const double d_R = -0.13;
67 
68  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 2x2 dyn")
69 }
70 
71 TEST(Matrices, det_2x2_fix)
72 {
73  const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
74  CMatrixDouble22 A(dat_A);
75 
76  const double d = A.det();
77  const double d_R = -0.13;
78 
79  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 2x2 fix")
80 }
81 
82 TEST(Matrices, det_3x3_dyn)
83 {
84  const double dat_A[] = {-3.3304e-01, -2.0585e-01, 6.2026e-05,
85  1.4631e+00, 6.0985e-01, 2.3746e+00,
86  -3.6451e-01, 4.8169e-01, -8.4419e-01};
87  CMatrixDouble A(3, 3, dat_A);
88  const double d = A.det();
89  const double d_R = 0.476380435871666;
90  // cout << "d: " << d << endl;
91  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 3x3 dyn")
92 }
93 
94 TEST(Matrices, det_3x3_fix)
95 {
96  const double dat_A[] = {-3.3304e-01, -2.0585e-01, 6.2026e-05,
97  1.4631e+00, 6.0985e-01, 2.3746e+00,
98  -3.6451e-01, 4.8169e-01, -8.4419e-01};
99  CMatrixDouble33 A(dat_A);
100  const double d = A.det();
101  const double d_R = 0.476380435871666;
102  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 3x3 fix")
103 }
104 
105 TEST(Matrices, det_4x4_dyn)
106 {
107  const double dat_A[] = {0.773931, -0.336130, 1.131764, 0.385890,
108  1.374906, -0.540629, -0.952902, 0.659769,
109  -0.387254, -1.557355, 0.139683, -2.056635,
110  -0.750078, -0.653811, 0.872027, 0.217554};
111  CMatrixDouble A(4, 4, dat_A);
112  const double d = A.det();
113  const double d_R = -6.29527837425056;
114  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 4x4 dyn")
115 }
116 
117 TEST(Matrices, det_4x4_fix)
118 {
119  const double dat_A[] = {0.773931, -0.336130, 1.131764, 0.385890,
120  1.374906, -0.540629, -0.952902, 0.659769,
121  -0.387254, -1.557355, 0.139683, -2.056635,
122  -0.750078, -0.653811, 0.872027, 0.217554};
123  CMatrixDouble44 A(dat_A);
124  const double d = A.det();
125  const double d_R = -6.29527837425056;
126  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 4x4 fix")
127 }
128 
129 TEST(Matrices, det_10x10_dyn)
130 {
131  const double dat_A[] = {
132  1.2305462976, -0.2944257811, 0.8176140437, -0.0487601371,
133  0.4418235581, -0.0088466980, -1.4100223408, -0.6219629815,
134  1.1089237266, -0.6450262619, -2.0862614547, 0.2699762709,
135  -0.0705918517, 1.1763963161, -0.3461819597, -1.3013222580,
136  -0.3310621595, -0.2595069675, -0.5188213591, 1.2261476224,
137  -1.1334297957, 2.1452881319, 1.7856021357, 0.5406722888,
138  0.5497545623, 0.4282217402, -1.6175210256, -0.3522824764,
139  0.2773929603, 0.8507134453, 0.4046854117, -2.1638696195,
140  1.0044939778, 0.9755939720, 0.9640788301, 0.5641138097,
141  0.7382236207, -0.4422212587, 0.8507041571, 1.3764399072,
142  0.3446492224, 1.1681336612, -1.3440052449, 1.0120691406,
143  -0.0430604384, 0.4823901171, 0.0881769800, 0.3984805283,
144  -1.9988153178, 0.9509748328, 0.3202853059, 1.9688559025,
145  0.4020581289, -1.5558616735, -0.8753527614, 0.1207830427,
146  0.0457715031, -0.1557123759, -0.3161307172, -0.0759276933,
147  -0.0417386037, 1.2079564736, -2.5839030155, -0.7648863647,
148  1.1541464803, 0.2127569446, -1.4882083860, -0.7630836781,
149  0.8550884427, -0.8440402465, -0.4903597050, -0.1457982930,
150  0.5893448560, -0.2353784687, 0.3474655757, 2.5874616045,
151  0.6608448038, -1.0105315509, -1.5276853710, -0.1400026815,
152  -1.7630264416, 2.4048579514, -0.3111046623, 0.7463774799,
153  -0.2800404492, -1.4175124130, -0.5708536580, -1.2085107661,
154  0.8169107561, -1.1659481510, -0.1406355512, 2.3507381980,
155  2.6346742737, -1.1028788167, -0.0533115044, 0.3752684649,
156  -1.3799576309, -0.7274190037, 1.1188847602, -0.6624231096};
157  CMatrixDouble A(10, 10, dat_A);
158  const double d = A.det();
159  const double d_R = 330.498518199239;
160  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 10x10 dyn")
161 }
162 
163 TEST(Matrices, det_10x10_fix)
164 {
165  const double dat_A[] = {
166  1.2305462976, -0.2944257811, 0.8176140437, -0.0487601371,
167  0.4418235581, -0.0088466980, -1.4100223408, -0.6219629815,
168  1.1089237266, -0.6450262619, -2.0862614547, 0.2699762709,
169  -0.0705918517, 1.1763963161, -0.3461819597, -1.3013222580,
170  -0.3310621595, -0.2595069675, -0.5188213591, 1.2261476224,
171  -1.1334297957, 2.1452881319, 1.7856021357, 0.5406722888,
172  0.5497545623, 0.4282217402, -1.6175210256, -0.3522824764,
173  0.2773929603, 0.8507134453, 0.4046854117, -2.1638696195,
174  1.0044939778, 0.9755939720, 0.9640788301, 0.5641138097,
175  0.7382236207, -0.4422212587, 0.8507041571, 1.3764399072,
176  0.3446492224, 1.1681336612, -1.3440052449, 1.0120691406,
177  -0.0430604384, 0.4823901171, 0.0881769800, 0.3984805283,
178  -1.9988153178, 0.9509748328, 0.3202853059, 1.9688559025,
179  0.4020581289, -1.5558616735, -0.8753527614, 0.1207830427,
180  0.0457715031, -0.1557123759, -0.3161307172, -0.0759276933,
181  -0.0417386037, 1.2079564736, -2.5839030155, -0.7648863647,
182  1.1541464803, 0.2127569446, -1.4882083860, -0.7630836781,
183  0.8550884427, -0.8440402465, -0.4903597050, -0.1457982930,
184  0.5893448560, -0.2353784687, 0.3474655757, 2.5874616045,
185  0.6608448038, -1.0105315509, -1.5276853710, -0.1400026815,
186  -1.7630264416, 2.4048579514, -0.3111046623, 0.7463774799,
187  -0.2800404492, -1.4175124130, -0.5708536580, -1.2085107661,
188  0.8169107561, -1.1659481510, -0.1406355512, 2.3507381980,
189  2.6346742737, -1.1028788167, -0.0533115044, 0.3752684649,
190  -1.3799576309, -0.7274190037, 1.1188847602, -0.6624231096};
191  CMatrixFixedNumeric<double, 10, 10> A(dat_A);
192  const double d = A.det();
193  const double d_R = 330.498518199239;
194  CHECK_AND_RET_ERROR(fabs(d - d_R) > 1e-4, "Error in Determinant: 10x10 fix")
195 }
196 
197 TEST(Matrices, chol_2x2_dyn)
198 {
199  const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
200  0.8262219720};
201  CMatrixDouble A(2, 2, dat_A);
202  CMatrixDouble C;
203  A.chol(C);
204  const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
205  0.6672308672};
206  CMatrixDouble CHOL(2, 2, dat_CHOL);
207  CHECK_AND_RET_ERROR(
208  (CHOL - C).array().abs().sum() > 1e-4, "Error in Choleski, 2x2 dyn")
209 }
210 
211 TEST(Matrices, chol_2x2_fix)
212 {
213  const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
214  0.8262219720};
215  CMatrixDouble22 A(dat_A);
216  CMatrixDouble22 C;
217  A.chol(C);
218  const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
219  0.6672308672};
220  CMatrixDouble CHOL(2, 2, dat_CHOL);
221  CHECK_AND_RET_ERROR(
222  (CHOL - C).array().abs().sum() > 1e-4, "Error in Choleski, 2x2 fix")
223 }
224 
225 TEST(Matrices, chol_3x3_dyn)
226 {
227  const double dat_A[] = {
228  0.515479426556448, 0.832723636299236, 0.249691538245735,
229  0.832723636299236, 1.401081397506934, 0.385539356127255,
230  0.249691538245735, 0.385539356127255, 0.128633962591437};
231  CMatrixDouble A(3, 3, dat_A);
232  CMatrixDouble C;
233  A.chol(C);
234 
235  const double dat_CHOL[] = {
236  0.717968959326549, 1.159832365288224, 0.347774837619643,
237  0.000000000000000, 0.236368952988455, -0.075395504153773,
238  0.000000000000000, 0.000000000000000, 0.044745311077990};
239  CMatrixDouble CHOL(3, 3, dat_CHOL);
240  CHECK_AND_RET_ERROR(
241  (CHOL - C).array().abs().sum() > 1e-4, "Error in Choleski, 3x3 dyn")
242 }
243 
244 TEST(Matrices, chol_3x3_fix)
245 {
246  const double dat_A[] = {
247  0.515479426556448, 0.832723636299236, 0.249691538245735,
248  0.832723636299236, 1.401081397506934, 0.385539356127255,
249  0.249691538245735, 0.385539356127255, 0.128633962591437};
250  CMatrixDouble33 A(dat_A);
251  CMatrixDouble33 C;
252  A.chol(C);
253 
254  const double dat_CHOL[] = {
255  0.717968959326549, 1.159832365288224, 0.347774837619643,
256  0.000000000000000, 0.236368952988455, -0.075395504153773,
257  0.000000000000000, 0.000000000000000, 0.044745311077990};
258  CMatrixDouble33 CHOL(dat_CHOL);
259  CHECK_AND_RET_ERROR(
260  (CHOL - C).array().abs().sum() > 1e-4, "Error in Choleski, 3x3 fix")
261 }
262 
263 TEST(Matrices, chol_10x10_dyn)
264 {
265  const double dat_A[] = {
266  2.8955668335, 2.3041932983, 1.9002381085, 1.7993158652, 1.8456197228,
267  2.9632296740, 1.9368565578, 2.1988923358, 2.0547605617, 2.5655678993,
268  2.3041932983, 3.8406914364, 2.1811218706, 3.2312564555, 2.4736403918,
269  3.4703311380, 1.4874417483, 3.1073538218, 2.1353324397, 2.9541115932,
270  1.9002381085, 2.1811218706, 2.4942067597, 1.6851007198, 1.4585872052,
271  2.3015952197, 1.0955231591, 2.2979627790, 1.3918738834, 2.1854562572,
272  1.7993158652, 3.2312564555, 1.6851007198, 3.1226161015, 1.6779632687,
273  2.7195826381, 1.2397348013, 2.3757864319, 1.6291224768, 2.4463194915,
274  1.8456197228, 2.4736403918, 1.4585872052, 1.6779632687, 2.8123267839,
275  2.5860688816, 1.4131630919, 2.1914803135, 1.5542420639, 2.7170092067,
276  2.9632296740, 3.4703311380, 2.3015952197, 2.7195826381, 2.5860688816,
277  4.1669180394, 2.1145239023, 3.3214801332, 2.6694845663, 3.0742063088,
278  1.9368565578, 1.4874417483, 1.0955231591, 1.2397348013, 1.4131630919,
279  2.1145239023, 1.8928811570, 1.7097998455, 1.7205860530, 1.8710847505,
280  2.1988923358, 3.1073538218, 2.2979627790, 2.3757864319, 2.1914803135,
281  3.3214801332, 1.7097998455, 3.4592638415, 2.1518695071, 2.8907499694,
282  2.0547605617, 2.1353324397, 1.3918738834, 1.6291224768, 1.5542420639,
283  2.6694845663, 1.7205860530, 2.1518695071, 2.1110960664, 1.6731209980,
284  2.5655678993, 2.9541115932, 2.1854562572, 2.4463194915, 2.7170092067,
285  3.0742063088, 1.8710847505, 2.8907499694, 1.6731209980, 3.9093678727};
286  CMatrixDouble A(10, 10, dat_A);
287  CMatrixDouble C;
288  A.chol(C);
289 
290  const double dat_CHOL[] = {
291  1.7016365163, 1.3541042851, 1.1167121124, 1.0574031810,
292  1.0846145491, 1.7413999087, 1.1382316607, 1.2922221137,
293  1.2075202560, 1.5077061845, 0.0000000000, 1.4167191047,
294  0.4722017314, 1.2701334167, 0.7093566960, 0.7851196867,
295  -0.0380051491, 0.9582353452, 0.3530862859, 0.6441080558,
296  0.0000000000, 0.0000000000, 1.0120209201, -0.0943393725,
297  -0.0865342379, -0.0136183214, -0.1557357390, 0.3976620401,
298  -0.1218419159, 0.1952860421, 0.0000000000, 0.0000000000,
299  0.0000000000, 0.6183654266, -0.6113744707, -0.1944977093,
300  0.1127886805, -0.2752173394, -0.1741275611, 0.0847171764,
301  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
302  0.8668818973, 0.0234194680, 0.3011475111, -0.0272963639,
303  -0.1417917925, 0.8000162775, 0.0000000000, 0.0000000000,
304  0.0000000000, 0.0000000000, 0.0000000000, 0.6924364129,
305  0.2527445784, 0.3919505633, 0.3715689962, -0.0817608778,
306  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
307  0.0000000000, 0.0000000000, 0.6358623279, 0.4364121485,
308  0.4859857603, -0.0313828244, 0.0000000000, 0.0000000000,
309  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
310  0.0000000000, 0.5408375843, -0.1995475524, 0.6258606925,
311  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
312  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
313  0.2213262214, -0.2367037013, 0.0000000000, 0.0000000000,
314  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
315  0.0000000000, 0.0000000000, 0.0000000000, 0.2838575216};
316  CMatrixDouble CHOL(10, 10, dat_CHOL);
317  CHECK_AND_RET_ERROR(
318  (CHOL - C).array().abs().sum() > 1e-4, "Error in Choleski, 10x10 dyn")
319 }
320 
321 TEST(Matrices, chol_10x10_fix)
322 {
323  const double dat_A[] = {
324  2.8955668335, 2.3041932983, 1.9002381085, 1.7993158652, 1.8456197228,
325  2.9632296740, 1.9368565578, 2.1988923358, 2.0547605617, 2.5655678993,
326  2.3041932983, 3.8406914364, 2.1811218706, 3.2312564555, 2.4736403918,
327  3.4703311380, 1.4874417483, 3.1073538218, 2.1353324397, 2.9541115932,
328  1.9002381085, 2.1811218706, 2.4942067597, 1.6851007198, 1.4585872052,
329  2.3015952197, 1.0955231591, 2.2979627790, 1.3918738834, 2.1854562572,
330  1.7993158652, 3.2312564555, 1.6851007198, 3.1226161015, 1.6779632687,
331  2.7195826381, 1.2397348013, 2.3757864319, 1.6291224768, 2.4463194915,
332  1.8456197228, 2.4736403918, 1.4585872052, 1.6779632687, 2.8123267839,
333  2.5860688816, 1.4131630919, 2.1914803135, 1.5542420639, 2.7170092067,
334  2.9632296740, 3.4703311380, 2.3015952197, 2.7195826381, 2.5860688816,
335  4.1669180394, 2.1145239023, 3.3214801332, 2.6694845663, 3.0742063088,
336  1.9368565578, 1.4874417483, 1.0955231591, 1.2397348013, 1.4131630919,
337  2.1145239023, 1.8928811570, 1.7097998455, 1.7205860530, 1.8710847505,
338  2.1988923358, 3.1073538218, 2.2979627790, 2.3757864319, 2.1914803135,
339  3.3214801332, 1.7097998455, 3.4592638415, 2.1518695071, 2.8907499694,
340  2.0547605617, 2.1353324397, 1.3918738834, 1.6291224768, 1.5542420639,
341  2.6694845663, 1.7205860530, 2.1518695071, 2.1110960664, 1.6731209980,
342  2.5655678993, 2.9541115932, 2.1854562572, 2.4463194915, 2.7170092067,
343  3.0742063088, 1.8710847505, 2.8907499694, 1.6731209980, 3.9093678727};
344  CMatrixFixedNumeric<double, 10, 10> A(dat_A);
345  CMatrixDouble C;
346  A.chol(C);
347 
348  const double dat_CHOL[] = {
349  1.7016365163, 1.3541042851, 1.1167121124, 1.0574031810,
350  1.0846145491, 1.7413999087, 1.1382316607, 1.2922221137,
351  1.2075202560, 1.5077061845, 0.0000000000, 1.4167191047,
352  0.4722017314, 1.2701334167, 0.7093566960, 0.7851196867,
353  -0.0380051491, 0.9582353452, 0.3530862859, 0.6441080558,
354  0.0000000000, 0.0000000000, 1.0120209201, -0.0943393725,
355  -0.0865342379, -0.0136183214, -0.1557357390, 0.3976620401,
356  -0.1218419159, 0.1952860421, 0.0000000000, 0.0000000000,
357  0.0000000000, 0.6183654266, -0.6113744707, -0.1944977093,
358  0.1127886805, -0.2752173394, -0.1741275611, 0.0847171764,
359  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
360  0.8668818973, 0.0234194680, 0.3011475111, -0.0272963639,
361  -0.1417917925, 0.8000162775, 0.0000000000, 0.0000000000,
362  0.0000000000, 0.0000000000, 0.0000000000, 0.6924364129,
363  0.2527445784, 0.3919505633, 0.3715689962, -0.0817608778,
364  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
365  0.0000000000, 0.0000000000, 0.6358623279, 0.4364121485,
366  0.4859857603, -0.0313828244, 0.0000000000, 0.0000000000,
367  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
368  0.0000000000, 0.5408375843, -0.1995475524, 0.6258606925,
369  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
370  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
371  0.2213262214, -0.2367037013, 0.0000000000, 0.0000000000,
372  0.0000000000, 0.0000000000, 0.0000000000, 0.0000000000,
373  0.0000000000, 0.0000000000, 0.0000000000, 0.2838575216};
374  CMatrixDouble CHOL(10, 10, dat_CHOL);
375  CHECK_AND_RET_ERROR(
376  (CHOL - C).array().abs().sum() > 1e-4, "Error in Choleski, 10x10 fix")
377 }
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:17
CMatrixFixedNumeric.h
dat_A
const double dat_A[]
Definition: matrix_ops1_unittest.cpp:28
std
STL namespace.
mrpt::math::CMatrixFixedNumeric
A numeric matrix of compile-time fixed size.
Definition: CMatrixFixedNumeric.h:40
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
TEST
TEST(Matrices, HCHt_3x2_2x2_2x3)
Definition: matrix_ops3_unittest.cpp:27
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:209
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
R
const float R
Definition: CKinematicChain.cpp:138
CMatrixTemplateNumeric.h
CHECK_AND_RET_ERROR
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)
Definition: matrix_ops3_unittest.cpp:25


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020