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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019