19 #include <gtest/gtest.h> 28 #define CHECK_AND_RET_ERROR(_COND_, _MSG_) EXPECT_FALSE(_COND_) << _MSG_; 30 TEST(Matrices, HCHt_3x2_2x2_2x3)
32 const double dat_H[] = {0.2, -0.3, 0.1, 0.9, -0.07, 1.2};
35 const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
38 const double dat_R[] = {0.11600, -0.21500, -0.32530, -0.21500, 0.63800,
39 0.85270, -0.32530, 0.85270, 1.17272};
43 H.multiply_HCHt(C,
R);
45 EXPECT_NEAR((R_REAL -
R).array().abs().
sum(), 0, 1e-4);
48 TEST(Matrices, HCHt_scalar_1x2_2x2_2x1)
50 const double dat_H[] = {0.2, -0.3};
53 const double dat_C[] = {0.8, -0.1, -0.1, 0.8};
56 const double r = H.multiply_HCHt_scalar(C);
58 const double r2 = (H * C * H.transpose()).eval()(0, 0);
60 fabs(
r - r2) > 1e-4,
"Error in HCHt_scalar: 1x2 * 2x2 * 2x1")
63 TEST(Matrices, det_2x2_dyn)
65 const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
68 const double d = A.det();
69 const double d_R = -0.13;
74 TEST(Matrices, det_2x2_fix)
76 const double dat_A[] = {0.8, -0.3, -0.7, 0.1};
79 const double d = A.det();
80 const double d_R = -0.13;
85 TEST(Matrices, det_3x3_dyn)
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};
91 const double d = A.det();
92 const double d_R = 0.476380435871666;
97 TEST(Matrices, det_3x3_fix)
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;
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};
115 const double d = A.det();
116 const double d_R = -6.29527837425056;
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;
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};
161 const double d = A.det();
162 const double d_R = 330.498518199239;
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;
202 const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
207 const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
211 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 2x2 dyn")
216 const double dat_A[] = {1.0727710178, 0.6393375593, 0.6393375593,
221 const double dat_CHOL[] = {1.0357465992, 0.6172721781, 0.0000000000,
225 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 2x2 fix")
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};
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};
244 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 3x3 dyn")
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};
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};
263 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 3x3 fix")
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};
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};
321 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 10x10 dyn")
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};
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};
379 (CHOL - C).array().abs().
sum() > 1e-4,
"Error in Choleski, 10x10 fix")
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.
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
TEST(Matrices, HCHt_3x2_2x2_2x3)
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLdouble GLdouble GLdouble r
#define CHECK_AND_RET_ERROR(_COND_, _MSG_)